[發明專利]多關節機構軌跡規劃方法、裝置、電子設備及存儲介質

有效

• 申請號: 202110358478.1
• 申請日: 2021-04-01
• 公開（公告）號: CN113043277B
• 公開（公告）日: 2022-06-07
• 發明（設計）人: 皮皓杰;付玲;楊召;劉延斌;于曉穎
• 申請（專利權）人: 中聯重科股份有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 北京美智年華知識產權代理事務所(普通合伙) 11846
• 代理人: 梁忠益

1.一種多關節機構軌跡規劃方法，其特征在于，包括：

根據在起始點A和終止點B的多關節機構的各關節姿態，基于關節空間軌跡規劃法確定出多關節機構末端由起始點A向終止點B運動過程中第一關節軌跡規劃數據；

根據多關節機構末端從起始點A至終止點B運動的末端速度，基于雅可比矩陣的廣義逆矩陣法確定出多關節機構的末端由起始點A向終止點B點沿直線運動的第二關節軌跡規劃數據；

根據預設的運動提前量，基于第二關節軌跡規劃數據規劃多關節機構各關節的軌跡，控制關節運動；實時判斷所述多關節機構各關節的軌跡中各關節角度值是否超出了關節角度極限值，是，則結合第一關節軌跡規劃數據，對所述多關節機構各關節的軌跡中的關節姿態進行優化；

所述預設的運動提前量為多關節機構末端移動距離提前量L或為多關節機構末端移動時間提前量T。

2.根據權利要求1所述的多關節機構軌跡規劃方法，其特征在于，

當所述預設的運動提前量為多關節機構末端移動距離提前量L，對所述多關節機構各關節的軌跡中的關節姿態進行優化，包括：

當多關節機構末端運動到時間點t₁時，規劃的第二關節軌跡規劃數據已經達到時間點t₁+T_L，得到多關節機構的關節角Θ_i(t₁+T_L)；T_L為規劃的多關節機構末端從時間點t₁起移動距離L所經歷的時間；i＝1,…,N；N為多關節機構的關節數量；

實時檢測規劃的關節角Θ_i(t₁+T_L)是否超出了對應關節的極限值；是，則結合第一關節軌跡規劃數據，基于梯度投影法優化多關節機構的關節角速度

對多關節機構角速度進行積分，確定出優化后的多關節機構末端由A點向B點沿直線運動的關節軌跡{Θ_yi(t)}，并下發至下位機控制關節運動。

3.根據權利要求2所述的多關節機構軌跡規劃方法，其特征在于，所述梯度投影法得到的多關節機構優化角速度

式中，為R^N×1的向量，J⁺為雅可比矩陣廣義逆，為R^N×3的矩陣，為多關節機構末端在笛卡爾空間中的X、Y、Z的運動速度；k為放大系數，I為單位陣，H(Θ)為優化函數，為梯度，x為多關節機構末端在X軸上的移動量，x的范圍為2L；x₁為時間點t₁對應的多關節機構末端在笛卡爾空間中X軸的坐標。

4.根據權利要求3所述的多關節機構軌跡規劃方法，其特征在于，優化函數

式中，Θ_i(t₁+T_L)為基于廣義逆矩陣法規劃得到的關節i的關節角度；θ_i(t₁+T_L)為關節空間軌跡規劃法得到的關節i在t₁+T_L時刻的關節角度；θ_imax為關節i的角度范圍最大值；θ_imin為關節i的角度范圍最小值；函數abs()返回變量的絕對值；函數max()返回兩個變量的最大值。