[發明專利]一種繩驅蛇形機械臂路徑規劃首尾運動跟隨方法、裝置、計算機及存儲介質

在審

• 申請號: 202210092850.3
• 申請日: 2022-01-26
• 公開（公告）號: CN114603539A
• 公開（公告）日: 2022-06-10
• 發明（設計）人: 谷海宇;黃倬然;向陽;劉鵬;魏承
• 申請（專利權）人: 哈爾濱工業大學
• 主分類號: B25J9/06
• 分類號: B25J9/06;B25J9/16
• 代理公司: 哈爾濱市陽光惠遠知識產權代理有限公司 23211
• 代理人: 張宏威

1.一種繩驅蛇形機械臂路徑規劃首尾運動跟隨方法，所述繩驅蛇形機械臂包括n個錐節和n個關節，所述n大于3，其特征在于，所述規劃運動跟隨方法包括：

根據繩驅蛇形機械臂臂體末端點的空間位置末端點下一時刻的空間位置及末端關節的空間位置獲取下一時刻臂體末端關節的空間位置和角度

根據下一時刻臂體末端關節的空間位置和關節跟隨運動規劃條件獲取第i個關節的下一時刻位置及角度，i＝n-1,n-2,……,2，依次類推，直至獲得第2關節下一時刻的空間位置和角度

根據第2個關節的空間位置及角度獲取第1個關節的空間位置r₁^t+Δt及角度

根據獲取的所有關節的角度完成繩驅蛇形機械臂路徑規劃運動跟隨。

2.根據權利要求1所述的一種繩驅蛇形機械臂路徑規劃首尾運動跟隨方法，其特征在于，所述獲取下一時刻臂體末端關節的空間位置和角度具體為：

臂體末端的第n個關節沿上一時刻t該關節的軸向方向運動，所述運動的時間步長為Δt，該關節在下一時刻t+Δt的空間位置到末端距離為第n個錐節的長度l_n；則根據求解方程：

獲得臂體末端的第n個關節的空間位置

臂體末端的第n個關節的角度根據求解方程獲得：

為臂體末端在慣性系下的x、y、z方向的坐標。

3.根據權利要求2所述的一種繩驅蛇形機械臂路徑規劃首尾運動跟隨方法，其特征在于，所述運動跟隨方法還包括判斷臂體末端的第n個關節的角度是否在臂體末端的關節的機構運動空間內：若所述關節角度在機構運動空間內，則空間位置為和角度為下一時刻臂體末端的第n個關節的空間位置和角度；否則，則判定繩驅蛇形機械臂在進行平移運動，并獲取當前繩驅蛇形機械臂末端第n個關節的空間位置和角度。

4.根據權利要求3所述的一種繩驅蛇形機械臂路徑規劃首尾運動跟隨方法，其特征在于，所述獲取當前繩驅蛇形機械臂末端第n個關節的空間位置和角度是根據求解方程獲得：

5.根據權利要求1所述的一種繩驅蛇形機械臂路徑規劃首尾運動跟隨方法，其特征在于，所述根據下一時刻臂體末端關節的空間位置和關節跟隨運動規劃條件獲取第i個關節的下一時刻位置及角度，i＝n-1,n-2,……,2，依次類推，直至獲得第2關節下一時刻的空間位置和角度具體為：

獲取第i個關節的下一時刻位置根據求解方式：

所述i＝n-1,n-2,……,2；l_i為第i個錐節的長度；

獲取第i個關節的下一時刻角度根據求解方式：

其中，為第i個關節在慣性系下的x、y、z方向的坐標；

6.根據權利要求5所述的一種繩驅蛇形機械臂路徑規劃首尾運動跟隨方法，其特征在于，所述運動跟隨方法還包括根據臂體的第i個關節的角度進行第i個關節是否在機構運動空間的判斷：若所述關節角度在機構運動空間內，則空間位置為和角度為下一時刻臂體的第i個關節的空間位置和角度；否則，則判定繩驅蛇形機械臂在進行平移運動，并獲取當前臂體第i個關節的空間位置和角度。

7.根據權利要求1所述的一種繩驅蛇形機械臂路徑規劃首尾運動跟隨方法，其特征在于，所述獲取第1個關節的空間位置r₁^t+Δt及角度根據求解方程：

獲得第1關節的空間位置r₁^t+Δt，其中，l₁為第1個錐節的長度；

然后再求解方程

獲得第1個關節下一時刻的角度其中，為第1個關節在在慣性系下的x、y、z方向的坐標。