[發明專利]基于正態分布和雅可比矩陣的同心管機器人運動規劃方法

有效

• 申請號: 202110374981.6
• 申請日: 2021-04-08
• 公開（公告）號: CN113282077B
• 公開（公告）日: 2022-06-17
• 發明（設計）人: 馮子俊;李永強;馮宇;馮遠靜;劉揚
• 申請（專利權）人: 浙江工業大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 杭州斯可睿專利事務所有限公司 33241
• 代理人: 王利強

1.一種基于正態分布和雅可比矩陣的同心管機器人運動規劃方法，其特征在于，所述方法包括以下步驟：

1)對于幾何參數和力學參數已知的同心管機器人，預計算其正運動學的解，繪制機器人的工作空間，建立驅動輸入與末端位姿一一對應的數據表

2)通過安裝在同心管機器人末端的內窺鏡系統生成手術解剖結構的點云，并將相互距離小于同心管截面直徑d的點用一個最小球體包裹住，設此方法生成的障礙物球體共有N個，且第i個障礙物的半徑為R_i；

3)使用基于正態分布的RRT*算法進行同心管機器人的路徑規劃：從數據表中選取出位于病灶點附近且不在障礙物內的位置x_target，并根據其對應的輸入，利用正運動學模型繪制出機器人在工作空間中的整體形狀Shape，該形狀對應的點集為S0，因為同心管的建模滿足剛度主宰的假設，故若S0中的任意一點都不在障礙物內，則同心管在手術環境中的最優工作路徑即為S0；若存在一段路徑被第i個障礙物截斷，則需要對該段進行新的路徑規劃；

4)根據步驟3)實現了障礙物段的最優路徑規劃，得到路徑S2，進而得到同心管機器人在工作空間中的最佳路徑S＝S0-S1+S2；

5)為了使機器人能夠沿著路徑S到達病灶點，使用基于雅可比矩陣J的逆運動學逐步計算出路徑上每一個點x所對應的機器人驅動輸入量q；

6)使用步驟5)計算得到的輸入量作為當前步的積分初值，更快速的計算出了每個路徑點對應的輸入量q。

2.如權利要求1所述的基于正態分布和雅可比矩陣的同心管機器人運動規劃方法，其特征在于，所述步驟3)的過程如下：

3.1)首先，根據S1設定所需規劃的起始位置x_init，目標位置x_goal，迭代次數n，步長p以及碰撞風險評估指標：dis(x_i0,x₁)≥R_i+λr₀，其中r₀為同心管機器人的截面半徑，λ為安全性常數，越大代表安全性要求越高，x_i0為第i個障礙物的中心位置坐標，x₁為工作空間中任意節點的位置坐標；

3.2)基于正態隨機分布在自由空間中產生一個點x_rand，且正態分布的均值μ接近于目標位置x_goal，標準差σ＝10；

3.3)在搜索樹T中找到與x_rand最近的節點x_nearest，并沿著x_nearest指向x_rand的方向，以步長p產生一個新節點x_new；

3.4)以x_new為中心，r_i為搜索半徑，在搜索樹T中找到所有潛在的父節點x_pp，以更新x_new，先對找到的所有潛在的父節點x_pp進行碰撞風險評估，剔除不符合風險指標的節點，最終組成潛在的父節點集合E_pp；

3.5)先不考慮碰撞檢測，將E_pp中的每一個點分別作為父節點與x_new相連，計算從x_init到x_new的新路徑長度，若該路徑長度小于原路徑長度，則進行碰撞檢測；若檢測失敗，則考慮下一個潛在的父節點；若檢測通過，則刪除掉搜索樹T中多余的邊，并將該節點作為新的父節點；

3.6)遍歷所有潛在的父節點，得到更新完成后的樹T；

3.7)步驟3.2)～3.6)為基于正態分布RRT*搜索最優路徑的一次迭代，當達到最大迭代次數n后，找到一條漸近最優的路徑。