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

有效

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

一種多關節診療機器人柔順力控制模式下的重力補償方法，屬于診療機器人力控領域，為了解決現有的重力補償方法對診療機器人重力補償精度不足的問題。本發明實時記錄機器人多個姿態下的姿態矩陣以及傳感器的測量值，構造線性方程組，通過最小二乘法求解診療工具的參數向量；在診療工具處于初始位置時以及按照大地坐標系調整診療工具的姿態，使診療工具的重力只剩沿六維力與力矩傳感器坐標的y軸方向時，分別記錄傳感器的測量值；在傳感器坐標系下，利用降維解析法計算診療工具重心的位置；計算出診療工具的重力分量與力矩分量補償值，實現對診療工具的重力補償。有益效果為實現對多種診療工具在多姿態工作模式下的高精度重力補償。

技術領域

本發明屬于診療機器人力控領域。

背景技術

診療機器人在重力環境下結合六維力與力矩傳感器通過人手接觸控制柔順運動時，進入控制閉環的力為診療機器人末端與人手之間的接觸力，換言之，要實現人手力對機器人的控制，首先就要精確檢測出人手力的大小和方向；實際上，安裝于診療機器人末端的六維力傳感器測量到的力，不僅包含機器人末端與人手間的接觸力，還包含安裝于力傳感器上診療工具的重力；但是傳感器無法自主區分人手力和診療工具的重力，診療工具的重力屬于干擾力，因此有必要進行精確的重力補償；如果不進行重力補償，系統的控制精度必定大大受其影響；其次，根據診療機器人的實際需求，機器人末端需要在超聲陣列探頭、診療光纖和治療光纖這三種診療工具中來回切換，而這三種診療工具的質量特性各異，重心也隨自身姿態的變化而變化；現有的重力補償方法無法滿足診療機器人多模式、多姿態和高精度的工作需求，每次對末端工具進行更換或者重新安裝后，都需要對其進行精密測量來測定其質量特性，操作繁瑣，不適用于診療機器人的工作環境。

發明內容

本發明的目的是為了解決現有的重力補償方法對診療機器人重力補償精度不足的問題，提出了一種多關節診療機器人柔順力控制模式下的重力補償方法。

本發明所述的一種多關節診療機器人柔順力控制模式下的重力補償方法通過以下步驟實現的：

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

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

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

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

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

本發明的有益效果是：該重力補償方法能夠實時提供重力補償，來抵消多關節診療機器人末端診療工具自身重力的影響，使得補償后的多關節診療機器人在任何末端位姿下不受外力作用時，多關節診療機器人在基坐標系下沿各方向的力和力矩都趨于零，因此該重力補償方法重力補償算法精度較高。

附圖說明

圖1為為具體實施方式所述的一種多關節診療機器人柔順力控制模式下的重力補償方法流程圖；

圖2為具體實施方式一中多關節診療機器人的實物圖；

圖3為具體實施方式二中基于坐標系變換的重力計算示意圖；

圖4為具體實施方式七中對計算出的診療工具重力分量與力矩分量補償值進行驗證的流程圖。

具體實施方式