[發(fā)明專利]一種平面冗余度機器人避障及避奇異的路徑規(guī)劃方法

在審

• 申請?zhí)枺?/td>: 201610052247.7
• 申請日: 2016-01-26
• 公開（公告）號: CN105700527A
• 公開（公告）日: 2016-06-22
• 發(fā)明（設(shè)計）人: 劉玉斌;戴騫;張赫;趙杰;李戈
• 申請（專利權(quán)）人: 哈爾濱工業(yè)大學(xué)
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 哈爾濱市松花江專利商標(biāo)事務(wù)所 23109
• 代理人: 楊立超

1.一種平面冗余度機器人避障及避奇異的路徑規(guī)劃方法，其特征在于它按以下步驟 進行：

所述方法涉及兩個同時進行的過程：

過程一：避障的路徑規(guī)劃步驟如下：

步驟一：建立各桿件坐標(biāo)系，并將各障礙物坐標(biāo)投影到各桿件坐標(biāo)系中；

步驟二：對桿件是否為安全桿件進行判斷；

步驟二.一：如桿件為安全桿件，則剔除此桿件，不作任何處理；

步驟二.二：如桿件不是安全桿件，則轉(zhuǎn)至步驟三；

步驟三：求出連桿和障礙物間的實時最小距離以及各桿件上標(biāo)志點的坐標(biāo)；

步驟四：對實時最小距離是否處于安全距離進行判斷；

步驟四.一：如實時最小距離大于或等于安全距離，則轉(zhuǎn)至步驟八；

步驟四.二：如實時最小距離小于安全距離，則轉(zhuǎn)至步驟五；

步驟五：根據(jù)實時最小距離處于危險距離或極度危險距離，分別啟動避障策略或啟動 避障增益策略；

步驟六：計算機器人雅可比矩陣和標(biāo)志點處雅可比矩陣；

步驟七：逆運動學(xué)求解，積分得到關(guān)節(jié)的運動角度；

步驟八：不啟動避障策略，繼續(xù)執(zhí)行機器人末端運動；

步驟九：判斷機器人末端是否到達(dá)終點；

步驟九.一：機器人末端到達(dá)終點，結(jié)束

過程二：避奇異的路徑規(guī)劃步驟如下：

步驟A：判斷最小奇異值是否低于奇異值下限；

步驟A.一：最小奇異值大于或等于奇異值下限，轉(zhuǎn)至步驟B；

步驟A.二：最小奇異值小于奇異值下限，轉(zhuǎn)至步驟C；

步驟B：不啟動避奇異策略，繼續(xù)執(zhí)行機器人末端運動，再轉(zhuǎn)至步驟九；

步驟C：啟動避奇異策略，再轉(zhuǎn)至步驟七。

2.如權(quán)利要求1所述的一種平面冗余度機器人避障及避奇異的路徑規(guī)劃方法，其特 征在于步驟一的具體過程為：

所述連桿是由桿件1、桿件2和桿件3構(gòu)成；

已知障礙物中心在坐標(biāo)系{0}中的位置(x_t0,y_t0)，連桿截面為邊長為a的正方形，障礙 物半徑為R，根據(jù)坐標(biāo)系{1}相對于坐標(biāo)系{0}的旋轉(zhuǎn)矩陣，得出障礙物中心在坐標(biāo)系{1} 中的位置(x_t1,y_t1)，即

xt1yt10=cosθ1-sinθ10sinθ1cosθ10001xt0yt00]]>

同理得出

xt2yt20=cosθ2-sinθ20sinθ2cosθ20001xt0yt00,xt3yt30=cosθ3-sinθ30sinθ3cosθ30001xt0yt00.]]>