一種冗余機(jī)械臂運(yùn)動(dòng)障礙物避障算法
【專利摘要】本發(fā)明為一種冗余機(jī)械臂運(yùn)動(dòng)障礙物避障算法。該算法利用冗余機(jī)械臂和運(yùn)動(dòng)障礙物的相對(duì)位置關(guān)系得到障礙物相對(duì)于機(jī)械臂各桿件的預(yù)測(cè)碰撞點(diǎn)；再利用障礙物的運(yùn)動(dòng)速度求的最短預(yù)測(cè)距離，并根據(jù)該距離值與安全閾值的關(guān)系得到各桿件的躲避速度；將躲避速度利用雅可比轉(zhuǎn)置矩陣轉(zhuǎn)換為機(jī)械臂各關(guān)節(jié)對(duì)應(yīng)的角速度變量，最后引入到梯度投影法中，得到各關(guān)節(jié)的角速度，實(shí)現(xiàn)對(duì)運(yùn)動(dòng)障礙物的躲避。使用本發(fā)明提出的算法能夠在保證對(duì)機(jī)械臂末端軌跡準(zhǔn)確追蹤的同時(shí)，實(shí)現(xiàn)對(duì)運(yùn)動(dòng)障礙物的躲避，對(duì)于提高工業(yè)機(jī)械臂的安全性具有重要意義。
【專利說(shuō)明】一種冗余機(jī)械臂運(yùn)動(dòng)障礙物避障算法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明提出了一種工業(yè)機(jī)械臂的規(guī)劃方法，具體涉及到一種冗余機(jī)械臂運(yùn)動(dòng)障礙 物避障算法。

【背景技術(shù)】
[0002] 冗余機(jī)械臂是指具有運(yùn)動(dòng)冗余自由度的機(jī)械臂。與普通機(jī)械臂相比，冗余機(jī)械臂 具有更加靈活的運(yùn)動(dòng)特性，可以在完成末端軌跡跟蹤任務(wù)的同時(shí)完成避障和防止關(guān)節(jié)角度 超限等附加任務(wù)。冗余機(jī)械臂的避障問(wèn)題一直是工業(yè)機(jī)器人領(lǐng)域的研究熱點(diǎn)之一，眾多學(xué) 者提出了人工勢(shì)場(chǎng)法、神經(jīng)網(wǎng)絡(luò)法、最小距離法等方法來(lái)解決冗余機(jī)械臂的避障問(wèn)題，這些 方法僅能解決冗余機(jī)械臂對(duì)靜止障礙物的避障問(wèn)題，但在實(shí)際情況中，運(yùn)動(dòng)障礙物對(duì)機(jī)械 臂造成的威脅更大，而之前的方法卻不能解決這一問(wèn)題。


【發(fā)明內(nèi)容】

[0003] 本發(fā)明要解決的技術(shù)問(wèn)題是：提出一種冗余機(jī)械臂的軌跡規(guī)劃方法，在保證機(jī)械 臂末端軌跡跟蹤精度的同時(shí)，實(shí)現(xiàn)對(duì)運(yùn)動(dòng)障礙物的避障。
[0004] 為了實(shí)現(xiàn)上述目的，本發(fā)明提供的冗余機(jī)械臂運(yùn)動(dòng)障礙物避障算法，包括碰撞預(yù) 測(cè)點(diǎn)求解、桿件躲避速度求解和關(guān)節(jié)速度求解三個(gè)過(guò)程，該方法具體步驟如下：
[0005] 步驟Sl :將運(yùn)動(dòng)障礙物簡(jiǎn)化成一個(gè)質(zhì)點(diǎn)，將冗余機(jī)械臂的桿件簡(jiǎn)化成線段，并求 得線段上距離質(zhì)點(diǎn)最近的點(diǎn)作為碰撞預(yù)測(cè)點(diǎn)；
[0006] 步驟S2 :根據(jù)碰撞預(yù)測(cè)點(diǎn)與障礙物的相對(duì)位置和速度關(guān)系，求得各桿件的躲避速 度；
[0007] 步驟S3 :利用雅可比轉(zhuǎn)置矩陣將桿件的躲避速度轉(zhuǎn)換為機(jī)械臂相應(yīng)的關(guān)節(jié)角速 度變量，并引入梯度投影法中，最后得到各關(guān)節(jié)角速度，實(shí)現(xiàn)對(duì)運(yùn)動(dòng)障礙物的躲避。
[0008] 其中碰撞預(yù)測(cè)點(diǎn)求解的具體步驟如下：
[0009] 步驟Sll :求得沿桿件方向的單位向量；
[0010] 步驟S12 :利用垂直向量點(diǎn)積為0的性質(zhì)求得碰撞預(yù)測(cè)點(diǎn)距桿件指定端點(diǎn)的距離， 并用標(biāo)量表示；
[0011] 步驟S13 :如果此距離值小于0,則碰撞預(yù)測(cè)點(diǎn)為該指定端點(diǎn)，如果距離值大于桿 件的長(zhǎng)度，則碰撞預(yù)測(cè)點(diǎn)為另外一個(gè)端點(diǎn)，如果距離值為〇和桿件長(zhǎng)度之間的任意值，碰撞 預(yù)測(cè)點(diǎn)在桿件上，可以利用指定端點(diǎn)和距離值來(lái)確定其具體位置。
[0012] 其中桿件躲避速度求解的具體步驟如下：
[0013] 步驟S21 :將運(yùn)動(dòng)障礙物的速度在障礙物與碰撞預(yù)測(cè)點(diǎn)之間的連線上進(jìn)行投影， 得到最短預(yù)測(cè)距離；
[0014] 步驟S22 :當(dāng)桿件的最短預(yù)測(cè)距離大于安全閾值時(shí)，該桿件的躲避速度為0,當(dāng)桿 件的最短預(yù)測(cè)距離小于安全閾值時(shí)，桿件存在躲避速度，且最短預(yù)測(cè)距離越小，桿件躲避速 度越大
[0015] 其中關(guān)節(jié)速度求解的具體步驟如下：
[0016] 步驟S31 :利用機(jī)械臂的轉(zhuǎn)置雅可比矩陣將桿件的躲避速度轉(zhuǎn)化為相應(yīng)的關(guān)節(jié)角 速度變量；
[0017] 步驟S32 :將各關(guān)節(jié)的角速度變量引入梯度投影法中，求得關(guān)節(jié)的角速度。
[0018] 針對(duì)以往冗余機(jī)械臂避障算法的不足，本發(fā)明提出了一種能夠?qū)崿F(xiàn)對(duì)運(yùn)動(dòng)障礙物 進(jìn)行躲避的避障算法，與以往研究相比，發(fā)明提出的算法具有以下特點(diǎn)：
[0019] (1)采用雅可比轉(zhuǎn)置矩陣得到的桿件躲避速度來(lái)代替梯度投影法中單一的優(yōu)化指 標(biāo)，實(shí)現(xiàn)了冗余機(jī)械臂對(duì)多個(gè)運(yùn)動(dòng)障礙物的躲避。
[0020] (2)考慮到運(yùn)動(dòng)障礙物的速度和機(jī)械臂系統(tǒng)反應(yīng)時(shí)間，得到機(jī)械臂與障礙物的最 小預(yù)測(cè)距離，用來(lái)求解對(duì)應(yīng)桿件的躲避速度。

【專利附圖】

【附圖說(shuō)明】
[0021] 附圖1為冗余機(jī)械臂和障礙物的示意圖；
[0022] 附圖2機(jī)械臂桿件和障礙物的簡(jiǎn)化示意圖；
[0023] 附圖中：1-冗余度機(jī)械臂，2-運(yùn)動(dòng)障礙物，3-簡(jiǎn)化桿件，4-簡(jiǎn)化障礙物。

【具體實(shí)施方式】
[0024] 下面結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步說(shuō)明。
[0025] 本發(fā)明提出的冗余機(jī)械臂運(yùn)動(dòng)障礙物避障算法包括碰撞預(yù)測(cè)點(diǎn)求解、桿件躲避速 度求解和關(guān)節(jié)速度求解三個(gè)過(guò)程，下面分別對(duì)這個(gè)三個(gè)過(guò)程進(jìn)行說(shuō)明：
[0026] 附圖1為冗余機(jī)械臂和障礙物示意圖，從圖中可以看出，運(yùn)動(dòng)障礙物2在冗余機(jī)械 臂1的工作空間內(nèi)運(yùn)動(dòng)；
[0027] 附圖2為機(jī)械臂桿件和障礙物的簡(jiǎn)化示意圖，從圖中可以看出，簡(jiǎn)化桿件3為一條 線段，是冗余機(jī)械臂1上任一桿件的簡(jiǎn)化形式；簡(jiǎn)化障礙物為一質(zhì)點(diǎn)，是運(yùn)動(dòng)障礙物2的簡(jiǎn) 化形式；在對(duì)碰撞預(yù)測(cè)點(diǎn)求解時(shí)，〇點(diǎn)代表簡(jiǎn)化障礙物4的位置，A點(diǎn)和B點(diǎn)分別為簡(jiǎn)化桿 件3的端點(diǎn)，C點(diǎn)為簡(jiǎn)化桿件3上的預(yù)測(cè)碰撞點(diǎn)，即桿件上距障礙物最近的點(diǎn)；沿AB方向的 單位向量e可以用下式求出

【權(quán)利要求】
1. 一種冗余機(jī)械臂運(yùn)動(dòng)障礙物避障算法，其特征在于：該方法包括步驟如下： 步驟S1 :將運(yùn)動(dòng)障礙物簡(jiǎn)化成一個(gè)質(zhì)點(diǎn)，將冗余機(jī)械臂的桿件簡(jiǎn)化成線段，并求得線 段上距離質(zhì)點(diǎn)最近的點(diǎn)作為碰撞預(yù)測(cè)點(diǎn)； 步驟S2 :根據(jù)碰撞預(yù)測(cè)點(diǎn)與障礙物的相對(duì)位置和速度關(guān)系，求得各桿件的躲避速度； 步驟S3 :利用雅可比轉(zhuǎn)置矩陣將桿件的躲避速度轉(zhuǎn)換為機(jī)械臂相應(yīng)的關(guān)節(jié)角速度變 量，并引入梯度投影法中，最后得到各關(guān)節(jié)角速度，實(shí)現(xiàn)對(duì)運(yùn)動(dòng)障礙物的躲避。
2. 按權(quán)利要求1所述的冗余機(jī)械臂運(yùn)動(dòng)障礙物避障算法，其特征在于：所述碰撞預(yù)測(cè) 點(diǎn)求解的具體步驟如下： 步驟S11 :求得沿桿件方向的單位向量； 步驟S12 :利用垂直向量點(diǎn)積為0的性質(zhì)求得碰撞預(yù)測(cè)點(diǎn)距桿件指定端點(diǎn)的距離，并用 標(biāo)量表不； 步驟S13 :如果此距離值小于0,則碰撞預(yù)測(cè)點(diǎn)為該指定端點(diǎn)，如果距離值大于桿件的 長(zhǎng)度，則碰撞預(yù)測(cè)點(diǎn)為另外一個(gè)端點(diǎn)，如果距離值為0和桿件長(zhǎng)度之間的任意值，碰撞預(yù)測(cè) 點(diǎn)在桿件上，可以利用指定端點(diǎn)和距離值來(lái)確定其具體位置。
3. 按權(quán)利要求1所述的冗余機(jī)械臂運(yùn)動(dòng)障礙物避障算法，其特征在于：所述的桿件躲 避速度求解的具體步騾如下： 步驟S21 :將運(yùn)動(dòng)障礙物的速度在障礙物與碰撞預(yù)測(cè)點(diǎn)之間的連線上進(jìn)行投影，得到 最短預(yù)測(cè)距離； 步驟S22 :當(dāng)桿件的最短預(yù)測(cè)距離大于安全閾值時(shí)，該桿件的躲避速度為0,當(dāng)桿件的 最短預(yù)測(cè)距離小于安全閾值時(shí)，桿件存在躲避速度，且最短預(yù)測(cè)距離越小，桿件躲避速度越 大。
4. 按權(quán)利要求1所述的冗余機(jī)械臂運(yùn)動(dòng)障礙物避障算法，其特征在于：所述的關(guān)節(jié)角 速度求解的具體步驟如下： 步驟S31 :利用機(jī)械臂的轉(zhuǎn)置雅可比矩陣將桿件的躲避速度轉(zhuǎn)化為相應(yīng)的關(guān)節(jié)角速度 變量； 步驟S32 :將各關(guān)節(jié)的角速度變量引入梯度投影法中，求得關(guān)節(jié)的角速度。
【文檔編號(hào)】G06F19/00GK104392081SQ201410300873
【公開(kāi)日】2015年3月4日 申請(qǐng)日期:2014年6月30日 優(yōu)先權(quán)日:2014年6月30日
【發(fā)明者】管小清, 常青 申請(qǐng)人:管小清, 常青