[發明專利]一種多關節診療機器人柔順力控制模式下的重力補償方法

有效

• 申請號: 202110719915.8
• 申請日: 2021-06-28
• 公開（公告）號: CN113319855B
• 公開（公告）日: 2022-04-12
• 發明（設計）人: 王啟松;劉丹;鐘小聰;唐嘉澤;趙博琪;張美妍;孫金瑋
• 申請（專利權）人: 哈爾濱工業大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 哈爾濱華夏松花江知識產權代理有限公司 23213
• 代理人: 楊曉輝

1.一種多關節診療機器人柔順力控制模式下的重力補償方法，其特征在于，該重力補償方法通過以下步驟實現的：

步驟一、將六維力與力矩傳感器安裝在多關節診療機器人末端，選取多關節診療機器人的多個姿態，并實時記錄關節診療機器人多個姿態下對應的姿態矩陣以及記錄關節診療機器人多個姿態下六維力與力矩傳感器的測量值，構造線性方程組，通過最小二乘法求解診療工具的參數向量；

步驟二、在診療工具處于初始位置時，記錄六維力與力矩傳感器的測量值；

步驟三、按照大地坐標系調整診療工具的姿態，使診療工具的重力只剩沿六維力與力矩傳感器坐標的y軸方向，并記錄此時六維力與力矩傳感器的測量值；

步驟四、根據步驟二記錄的測量值與步驟三記錄的測量值，在傳感器坐標系下，利用降維解析法計算診療工具重心的位置；

步驟五、將步驟四計算出的診療工具重心的位置投影到六維力與力矩傳感器坐標系上，并結合步驟一求解出的診療工具參數向量，計算出診療工具的重力分量與力矩分量補償值，實現對診療工具的重力補償；

步驟六、對步驟五計算出的診療工具的重力分量與力矩分量補償值進行驗證；

對步驟五計算出的診療工具的重力分量與力矩分量補償值進行驗證的具體方法為：

步驟六一、對六維力與力矩傳感器進行校零；

步驟六二、安裝診療工具，記錄誤差補償值；

步驟六三、計算診療工具的重心位置，得出相對誤差；

步驟六四、計算重力補償值；

步驟六五、判斷診療工具是否測試結束，如果測試未結束，則執行步驟六六，否則執行步驟六七；

步驟六六、更換診療工具的種類；

步驟六七、驗證結束；

步驟六一中對六維力與力矩傳感器進行校零的具體方法為：

將六維力與力矩傳感器安裝在多關節診療機器人末端的法蘭上，且六維力與力矩傳感器的三個坐標軸與多關節診療機器人末端的三個坐標軸重合；

用示教器控制關節診療機器人，使六維力與力矩傳感器坐標系下的Z軸平行于基座坐標系的Z軸，并且六維力與力矩傳感器坐標系下的Z軸豎直向下，第一次點擊六維力與力矩傳感器的校零按鈕；

沿六維力與力矩傳感器坐標系下的Z軸負方向施加一個大小恒定的力，再次點擊六維力與力矩傳感器的校零按鈕，完成對六維力與力矩傳感器的校零；

步驟六二中記錄誤差補償值的具體方法為：

將診療工具固定在六維力與力矩傳感器上；

用示教器控制多關節診療機器人，使多關節診療機器人各軸旋轉角度都為0°，即初始位置，記錄六維力與力矩傳感器測得的力在Y、Z方向和力矩在X、Z方向上的分量：F_y、F_z、T_x、T_z；

用示教器控制多關節診療機器人，使多關節診療機器人出各個軸轉動90°，記錄六維力與力矩傳感器測得的力在X方向上的分量F_x和力矩在Y方向上的分量T_y；

步驟六三中計算診療工具的重心位置，得出相對誤差的具體步驟為：

步驟六三一、用示教器移動多關節診療機器人到一個任意位置，用六維力與力矩傳感器示值減去誤差值得到的力與力矩：F_x0、F_y0、F_z0、T_x0、T_y0、T_z0；

步驟六三二、用示教器按照多關節診療機器人基坐標系調整多關節診療機器人末端姿態，使診療工具重力只剩傳感器坐標的Y軸分量，用六維力與力矩傳感器示值減去誤差值得到的力與力矩：F_x00、F_y00、F_z00、T_x00、T_y00、T_z00；

步驟六三三、得出診療工具重心對于六維力與力矩傳感器坐標系的位置，即測量值：

步驟六三四、比較理論值O_GF與測量值，計算相對誤差。