[發(fā)明專利]一種基于二次規(guī)劃的冗余度機械臂移動障礙物躲避方法

有效

• 申請?zhí)枺?/td>: 202110596692.0
• 申請日: 2021-05-31
• 公開（公告）號: CN113276121B
• 公開（公告）日: 2022-08-09
• 發(fā)明（設計）人: 張智軍;趙黃婷;陳思遠
• 申請（專利權(quán)）人: 華南理工大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 廣州市華學知識產(chǎn)權(quán)代理有限公司 44245
• 代理人: 鐘瑞敏

1.一種基于二次規(guī)劃的冗余度機械臂移動障礙物躲避方法，其特征在于，包括如下步驟：

1)獲取移動障礙物和冗余度機械臂的運動信息，進而計算移動障礙物和冗余度機械臂脆弱點的位置和速度信息；

2)基于步驟1)計算的位置和速度信息，建立增大移動障礙物與冗余度機械臂脆弱點距離的不等式約束，并進一步建立驅(qū)使冗余度機械臂脆弱點趨向障礙物分速度反方向的不等式約束；

3)將步驟2)的兩個不等式約束進行合并，進而得到一個統(tǒng)一的移動障礙物躲避不等式約束；

4)將步驟3)的不等式約束并入到以最小化速度范數(shù)為優(yōu)化目標，以極限約束和機械臂末端姿態(tài)保持約束為約束條件的二次規(guī)劃問題中；

5)將步驟4)的二次規(guī)劃問題用遞歸神經(jīng)網(wǎng)絡求解器進行求解；

6)將步驟5)的求解結(jié)果傳遞給移動平臺和機械臂控制器，以驅(qū)動移動平臺與機械臂在完成末端任務的同時躲避移動障礙物；

步驟1)中，移動障礙物和冗余度機械臂脆弱點的位置和速度信息包括：移動障礙點與機械臂脆弱點之間的距離d，移動障礙點到機械臂脆弱點的矢量由移動障礙點的速度矢量分解的垂直于的分矢量

其中，移動障礙點是移動障礙物上距離冗余度機械臂最近的點，機械臂脆弱點為冗余度機械臂或冗余度機械臂延長線上距離移動障礙物最近的點；移動障礙點的速度矢量表示障礙點在時間點t的速度；表示預測的障礙點在時間(t+dt)的速度；dt為時間段；表示計算的障礙點在時間t的加速度；表示障礙點在時間(t-dt)的速度，R³表示是一個可在三維空間內(nèi)任意變化的矢量速度；α是預先設定的一個權(quán)重參數(shù)；

步驟2)中，增大移動障礙物與冗余度機械臂脆弱點距離的不等式約束為：

驅(qū)使冗余度機械臂脆弱點趨向障礙物分速度反方向的不等式約束為：

其中，表示關(guān)節(jié)角速度；是對進行單位化處理；J_C是機械臂脆弱點的雅可比矩陣；是對進行單位化處理；d₂是機械臂的外安全閾值；

為避免角速度的不連續(xù)致使機械臂損傷，設計先減慢機械臂脆弱點的速度，然后再強制其往反方向躲避移動障礙物，用函數(shù)S(d)表示如下：

式中，d₁是機械臂的內(nèi)安全閾值，取決于機械臂連桿的半徑；d₀是介于d₁、d₂之間的安全閾值，當機械臂脆弱點與障礙物之間的距離大于d₀時，S(d)隨著距離的縮短而逐漸減小直到-1值，這是機械臂關(guān)節(jié)角速度的緩沖期；當機械臂脆弱點與障礙物之間的距離小于d₀時，強制增大移動障礙物與機械臂脆弱點之間的距離，驅(qū)使機械臂脆弱點趨向障礙物分速度反方向；k是一個可調(diào)參數(shù)，調(diào)參數(shù)時應滿足d₁≤d₀≤d₂；

步驟3)中，統(tǒng)一的移動障礙物躲避不等式約束為：

其中，

J_N＝λJ_O+(1-λ)J_V，

式中，λ是一個權(quán)重參數(shù)，β是一個收斂系數(shù)，是的模，代表移動障礙物的速度大小。

2.根據(jù)權(quán)利要求1所述的冗余度機械臂移動障礙物躲避方法，其特征在于，步驟4)中，將不等式約束并入到二次規(guī)劃問題中，即：

其中，是基于機械臂前向運動學方程的等式約束，用于對機械臂末端的運動軌跡進行限定，J_E是機械臂末端軌跡的雅可比矩陣，是目標軌跡r_d關(guān)于機械臂關(guān)節(jié)角度的導數(shù)；是用于保持機械臂末端姿態(tài)的等式約束，J_m是姿態(tài)的方向向量的雅可比矩陣；表示機械臂關(guān)節(jié)角度和角速度的物理極限，分別表示機械臂關(guān)節(jié)在兩個不同方向的最大物理極限。

3.根據(jù)權(quán)利要求1所述的冗余度機械臂移動障礙物躲避方法，其特征在于，遞歸神經(jīng)網(wǎng)絡求解器采用線性變分不等式原對偶神經(jīng)網(wǎng)絡求解器，求解結(jié)果為機械臂的關(guān)節(jié)角速度，用于引導脆弱點遠離移動障礙物。