[發(fā)明專利]一種超冗余機(jī)械臂分段式動(dòng)態(tài)避障方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202211381036.X
• 申請(qǐng)日: 2022-11-05
• 公開(kāi)（公告）號(hào): CN115990874A
• 公開(kāi)（公告）日: 2023-04-21
• 發(fā)明（設(shè)計(jì)）人: 曹政才;居仁杰;張東
• 申請(qǐng)（專利權(quán)）人: 北京化工大學(xué)
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16
• 代理公司: 北京思海天達(dá)知識(shí)產(chǎn)權(quán)代理有限公司 11203
• 代理人: 劉萍

1.一種超冗余機(jī)械臂分段式動(dòng)態(tài)避障方法，其特征在于，包括：

步驟1：針對(duì)場(chǎng)景中的機(jī)械臂狀態(tài)信息與觀測(cè)到的障礙物信息，建立以機(jī)械臂為中心的動(dòng)態(tài)障礙預(yù)警區(qū)域；

步驟2：當(dāng)障礙物進(jìn)入該區(qū)域監(jiān)測(cè)范圍內(nèi)，基于速度障礙策略構(gòu)建碰撞風(fēng)險(xiǎn)評(píng)估模型，實(shí)時(shí)評(píng)估機(jī)械臂與障礙物間的碰撞可能性；

步驟3：若存在碰撞風(fēng)險(xiǎn)，依據(jù)障礙物信息將機(jī)械臂劃分為若干避障區(qū)域，構(gòu)建局部避障約束、關(guān)節(jié)限位約束以及連桿長(zhǎng)度約束，求解機(jī)械臂各關(guān)節(jié)安全速度集合；

步驟4：通過(guò)前后向迭代逆運(yùn)動(dòng)學(xué)求解算法實(shí)時(shí)更新機(jī)械臂位姿，進(jìn)而實(shí)現(xiàn)動(dòng)態(tài)障礙場(chǎng)景下的超冗余機(jī)械臂分段式避障。

2.根據(jù)權(quán)利要求1所述的一種超冗余機(jī)械臂分段式動(dòng)態(tài)避障方法，其特征在于：

在步驟1中，具體包含以下步驟：

步驟1.1：從三維捕捉系統(tǒng)與機(jī)械臂關(guān)節(jié)內(nèi)部陀螺儀獲取機(jī)械臂的狀態(tài)信息：幾何構(gòu)型、機(jī)械臂臂型姿態(tài)、第i節(jié)機(jī)械臂連桿速度v_Li，與障礙物信息：幾何構(gòu)型、障礙物當(dāng)前速度v_o，其中，i＝1,2,…,n，n表示超冗余機(jī)械臂連桿數(shù)量；

步驟1.2：利用封閉式橢球體將機(jī)械臂各連桿進(jìn)行包覆，記包覆第i節(jié)機(jī)械臂連桿的封閉式橢球體為根據(jù)障礙物幾何構(gòu)型利用封閉式凸幾何體對(duì)障礙物進(jìn)行包覆，記包覆障礙物的封閉式凸幾何體為

步驟1.3：將連桿進(jìn)行簡(jiǎn)化，以障礙物質(zhì)心為中心構(gòu)建各連桿與障礙物間的封閉式閔可夫斯基和，記為：

式中，用表示閔可夫斯基和運(yùn)算，表示取區(qū)域內(nèi)任意一點(diǎn)x，表示取區(qū)域內(nèi)任意一點(diǎn)y；

步驟1.4：計(jì)算封閉式閔可夫斯基和到對(duì)應(yīng)連桿中心的最小歐式距離d_i，記機(jī)械臂連桿長(zhǎng)度為L(zhǎng)_a，以該連桿為中心設(shè)定動(dòng)態(tài)障礙預(yù)警距離為d_warn＝n*L_a，當(dāng)d_id_warn時(shí)，視為該障礙物存在潛在風(fēng)險(xiǎn)。

3.根據(jù)權(quán)利要求1所述的一種超冗余機(jī)械臂分段式動(dòng)態(tài)避障方法，其特征在于：

在步驟2中，具體包含以下步驟：

步驟2.1：將封閉式閔可夫斯基和沿基座標(biāo)系XOY與XOZ平面進(jìn)行投影為與記第i節(jié)連桿空間坐標(biāo)為P_Li，第i節(jié)連桿與障礙物的相對(duì)速度為v_rel＝v_Li-v_o，將P_Li與v_rel分別沿基座標(biāo)系XOY與XOZ平面進(jìn)行投影為^xoyP_Li、^xozP_Li與^xoyv_rel、^xozv_rel；

步驟2.2：在XOY平面內(nèi)，以^xoyP_Li為觀測(cè)中心，分別向兩側(cè)作切線，構(gòu)成障礙錐區(qū)域，記為^xoyD，在XOZ平面內(nèi)，以^xozP_Li為觀測(cè)中心，分別向兩側(cè)作切線，構(gòu)成障礙錐區(qū)域，記為^xozD，則速度障礙區(qū)域與表示為：

式中，τ為時(shí)間窗口量；

步驟2.3：當(dāng)相對(duì)速度矢量^xoyv_rel落在區(qū)域內(nèi)部或^xozv_rel落在區(qū)域內(nèi)部時(shí)，視為與在[0,τ]時(shí)間范圍內(nèi)存在碰撞風(fēng)險(xiǎn)，進(jìn)而構(gòu)建碰撞風(fēng)險(xiǎn)評(píng)估模型為：

式中，∩表示取交集，∪表示取并集。