[發明專利]一種手術機器人的手眼標定方法在審
| 申請號： | 202111114945.2 | 申請日： | 2021-09-23 |
| 公開（公告）號： | CN113843792A | 公開（公告）日： | 2021-12-28 |
| 發明（設計）人： | 汪陽;房鶴;萬安;崔小飛;祝勝 | 申請（專利權）人： | 四川鋒準機器人科技有限公司 |
| 主分類號： | B25J9/16 | 分類號： | B25J9/16 |
| 代理公司： | 北京市盛峰律師事務所 11337 | 代理人： | 席小東 |
| 地址： | 641300 四川省資陽市雁*** | 國省代碼： | 四川;51 |
| 權利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關鍵詞： | 一種 手術 機器人 手眼 標定 方法 | ||
1.一種手術機器人的手眼標定方法，其特征在于，包括以下步驟：
步驟S1，在機械臂末端固定安裝工具示蹤器；其中，所述工具示蹤器包括剛性支架以及固定安裝于所述剛性支架的4個不對稱布置的定位球；4個定位球分別稱為：1號定位球、2號定位球、3號定位球和4號定位球；
步驟S2，控制機械臂末端移動，進而帶動所述工具示蹤器同步移動，使所述工具示蹤器位于光學定位儀的測量視野中央時停止移動，設此時機械臂末端移動至點O1，此時機械臂末端TCP坐標系記為Tcp1＝(x1,y1,z1,o1)，其中，x1,y1,z1,o1分別表示Tcp1坐標系的x軸、y軸、z軸和坐標原點；此時的工具示蹤器坐標系記為Tool1；此時，采用光學定位儀測量工具示蹤器的1號定位球在光學定位儀坐標系下的坐標，記為
步驟S3，保持機械臂末端姿態不變，控制機械臂末端沿Tcp1坐標系的x1軸正方向移動設定長度到達點O2，此時機械臂末端TCP坐標系記為Tcp2＝(x2,y2,z2,o2)，其中，x2,y2,z2,o2分別表示Tcp2坐標系的x軸、y軸、z軸和坐標原點；此時的工具示蹤器坐標系記為Tool2；此時，采用光學定位儀測量工具示蹤器的1號定位球在光學定位儀坐標系下的坐標，記為
步驟S4，保持機械臂末端姿態不變，控制機械臂末端沿Tcp2坐標系的y2軸正方向移動設定長度到達點O3，此時機械臂末端TCP坐標系記為Tcp3＝(x3,y3,z3,o3)，其中，x3,y3,z3,o3分別表示Tcp3坐標系的x軸、y軸、z軸和坐標原點；此時的工具示蹤器坐標系記為Tool3；此時，采用光學定位儀測量工具示蹤器的1號定位球在光學定位儀坐標系下的坐標，記為采用光學定位儀測量工具示蹤器的2號定位球、3號定位球和4號定位球在光學定位儀坐標系下的坐標，分別記為和
步驟S5，保持TCP3坐標系的原點固定不動，控制機械臂末端以原點O3為旋轉中心點，分別旋轉到6個不同的姿態，在每個姿態，采用光學定位儀測量工具示蹤器的1號定位球在光學定位儀坐標系下的坐標，分別記為：和
其中：
代表旋轉到第1個姿態時的坐標；
代表旋轉到第2個姿態時的坐標；
代表旋轉到第3個姿態時的坐標；
代表旋轉到第4個姿態時的坐標；
代表旋轉到第5個姿態時的坐標；
代表旋轉到第6個姿態時的坐標；
步驟S6，計算Tcp3坐標系到光學定位儀坐標系的轉換矩陣Tcp3TL；
步驟S6.1，計算得到Tcp3坐標系到光學定位儀坐標系的旋轉矩陣Tcp3RL；
步驟S6.1.1，采用下式，獲得Tcp3坐標系的x3軸在光學定位儀坐標系下的方向向量
其中：
和分別為方向向量在x軸、y軸和z軸方向的向量分量；
具體的，由于機械臂末端從點O1移動到點O2，再從點O2移動到點O3時，機械臂末端姿態始終不變，所以，坐標系Tcp1，Tcp2和Tcp3的x軸、y軸和z軸方向分別平行，因此，坐標系Tcp1的x1軸在光學定位儀坐標系下的方向向量等于坐標系Tcp3的x3軸在光學定位儀坐標系下的方向向量
步驟S6.1.2，采用下式，獲得Tcp3坐標系的y3軸在光學定位儀坐標系下的初始方向向量
具體的，由于機械臂末端從點O1移動到點O2，再從點O2移動到點O3時，機械臂末端姿態始終不變，所以，坐標系Tcp1，Tcp2和Tcp3的x軸、y軸和z軸方向分別平行，因此，坐標系Tcp2的y2軸在光學定位儀坐標系下的方向向量等于坐標系Tcp3的y3軸在光學定位儀坐標系下的初始方向向量
步驟S6.1.3，采用下式，獲得Tcp3坐標系的z3軸在光學定位儀坐標系下的方向向量
其中：
和分別為方向向量在x軸、y軸和z軸方向的向量分量；
步驟S6.1.4，采用下式，獲得Tcp3坐標系的y3軸在光學定位儀坐標系下的修正后的方向向量
其中：和分別為方向向量在x軸、y軸和z軸方向的向量分量；
步驟S6.1.5，建立矩陣采用下式對其進行歸一化處理，得到Tcp3坐標系到光學定位儀坐標系的旋轉矩陣Tcp3RL：
其中：
和分別為方向向量在x軸、y軸和z軸方向的歸一化后的向量分量；
和分別為方向向量在x軸、y軸和z軸方向的歸一化后的向量分量；
和分別為方向向量在x軸、y軸和z軸方向的歸一化后的向量分量；
步驟S6.2，計算和組成的球的球心，即是Tcp3坐標系的原點O3在光學定位儀坐標系下的坐標
步驟S6.3，由此得到Tcp3坐標系到光學定位儀坐標系的轉換矩陣Tcp3TL：
步驟S7，根據步驟S4得到的1號定位球、2號定位球、3號定位球和4號定位球在光學定位儀坐標系下的坐標，計算得到Tool3坐標系到光學定位儀坐標系的轉換矩陣Tool3TL；
步驟S8，采用下式，得到Tool3坐標系到Tcp3坐標系的轉換矩陣Tool3TTcp3：
由此實現手眼標定。
該專利技術資料僅供研究查看技術是否侵權等信息，商用須獲得專利權人授權。該專利全部權利屬于四川鋒準機器人科技有限公司，未經四川鋒準機器人科技有限公司許可，擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作，請聯系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/202111114945.2/1.html，轉載請聲明來源鉆瓜專利網。
