為簡化手術機器人的操作流程、縮短手術準備時間，本文提出一種新的快速手眼標定方法——一次注冊法，并在分析該方法誤差產生原因的基礎上，又提出一種實用的手術機器人末端工具的參數優化方法。一次注冊法利用空間固定的雙目攝像機識別固定在機器人末端工具上的標記物，并由機器人關節參數可確定標記物的空間位姿，從而建立攝像機坐標系與機器人基坐標系的聯系，完成手眼標定；但考慮到機器人末端工具的加工、裝配誤差，本文建立以機器人末端坐標系與末端工具坐標系之間的轉換矩陣為變量的誤差方程，運用數值優化方法實現了對機器人末端工具參數的優化。實驗結果表明：一次注冊法與現有方法相比可以顯著地提高手術機器人的手眼標定效率，參數優化方法可以顯著地提高一次注冊法的絕對定位精度，使其滿足臨床手術的要求。
引用本文： 張立龍, 曹彤, 劉達. 基于機器人手眼標定的末端工具參數優化. 生物醫學工程學雜志, 2017, 34(2): 271-277. doi: 10.7507/1001-5515.201607037 復制
引言
近年來，隨著機器人技術的發展以及患者對微創技術的強烈需求，手術機器人在臨床手術中得到越來越廣泛的應用[1-3]。為使手術機器人能更好地應用于手術中，機器人的操作需要更簡潔、安全，定位精度更高[4]。在腦外科手術中，對于功能性神經病變，如帕金森病等，要求定位精度在 1 mm 左右；而對于一般器質性神經病變，如腦組織活檢、腦出血等，要求定位精度在 2～3 mm[5]。像已經商業化的 Path Finder 系統，定位精度的平均值為 2.7 mm[6]。
現有手術機器人系統大多采用視覺作為其位姿反饋與目標檢測的重要手段。為了讓機器人能準確到達實際的解剖位置，就需要進行機器人手眼標定。而在實際手術中，機器人手眼標定過程復雜、耗時且對機器人定位精度影響很大，由此可知機器人視覺系統對手術機器人的臨床應用具有很大影響。機器人的視覺系統分為眼固定式和眼在手上兩種方式。在工業領域，眼在手上的機器人應用較廣，該方式機器人系統完成手眼標定多利用特殊歐氏群、對偶四元數或二者相結合的方法，都需要進行多次的平移和旋轉，而且攝像機會擠占醫生手術操作空間，不利于手術安全操作，因此不適合應用于醫療手術領域[7-13]。對于眼固定式機器人，攝像機可以遠離手術區域，視野范圍更廣，但該方式機器人系統的現有手眼標定方法都存在缺陷。例如，仿射法要求移動四次機械臂，得到四個不同點坐標，建立仿射矩陣完成標定[14]；微型裝配機器人通過 3 個空間點在機械手基坐標系中的坐標，得到手眼系統之間的轉換矩陣[15]；三維視覺標定方法雖然只需要標定一次，但需要放置靶標，擠占手空間，而且定位誤差過大（4.5 mm）不適合應用于臨床[16]。以上標定方法有的標定過程太繁瑣，有的定位精度太差，都不適合在臨床手術中使用。
由于現有手眼標定方法存在的缺陷，本文以優傲機器人（Universal Robot，UR）機器人系統為研究平臺，旨在得到一種新的快速手眼標定方法，通過將光學靶標安裝在機器人末端的方式，可以快速建立機器人基坐標系與攝像機坐標系的聯系，并利用現代優化方法優化手術機器人末端工具運動學參數，減小參數偏差，提高手術機器人絕對定位精度，以實現該方法在臨床手術中的應用。
1 系統簡介
本文采用 UR 機械臂搭建手術機器人系統來進行研究。該系統由雙目攝像機、UR 機械臂和計算機組成（見圖 1），主要用于腦外科手術。通過將戴有光學標記點的患者的電子計算機斷層掃描（computed tomography，CT）數據導入電腦進行三維重建，醫生制定手術計劃；通過雙目攝像機對戴有光學標記點的患者進行注冊，建立醫學圖像空間與攝像機空間的聯系；通過雙目攝像機對末端工具中光學標記點的識別實現對機械臂的注冊，建立攝像機空間與機器人空間的聯系。最終，實現從醫學圖像空間到機器人空間的映射，從而控制機器人完成對病灶點的定位。
圖1
手術機器人系統及末端工具
Figure1.
Surgical robot system and end-tool
2 一次注冊
2.1 數學原理
手術機器人手眼關系如圖 2 所示，貼有光學標記點的末端工具安裝在機器人末端，攝像機在空間固定，設末端工具坐標系為M，機器人基坐標系為B，攝像機坐標系為C。根據機器人正運動學，可以求解當前任意位姿下，機器人末端坐標系E 與基坐標系B 的轉換關系[17]：
| $_B^E \! T = \,_B^1 \! T \cdot \, _1^2 \! T \cdot\, _2^3 \! T \cdots \, {}_5^E \! T = \left[ {\begin{array}{*{20}{c}}{_B^ER} & {_B^EP}\\0 & 1\end{array}} \right]$ |
可知當機器人末端工具安裝完畢后，末端工具坐標系M 與機器人末端坐標系E 間的轉換矩陣 和 是唯一確定且恒定不變的。當攝像機識別上述末端工具中的光學標記點后，光學標記點在攝像機坐標系的坐標為Si （i=1，2，3），其在末端工具坐標系中坐標為Xi （i=1，2，3），由機器人正運動學可知
| $S = \, _C^M \! T \cdot X$ |
可求出攝像機坐標系C 與末端工具坐標系M 之間的轉換矩陣為
| $_C^MT = S \cdot {X^{ - 1}}$ |
綜合基坐標系與機器人末端坐標系轉換矩陣 和機器人坐標系與末端工具坐標系的轉換矩陣 ，可以推導出機器人基坐標系與攝像機坐標系的轉換矩陣表述為
| $_C^BT = \, _C^M \! T \cdot \, _M^E \! T \cdot \, _E^B \! T$ |
從而完成機器人手眼標定，再利用患者的 CT 圖像空間與攝像機空間的映射關系，實現從圖像空間向機器人空間的映射，進而可以控制機器人沿著醫生在圖像空間規劃好的手術路徑到達病灶點，完成手術定位。
圖2
手術機器人手眼關系
Figure2.
Hand-eye relation of the surgical robot
2.2 一次注冊法與仿射注冊法的標定效率
機器人手眼標定方法——一次注冊法，只需要將機械臂末端移動到攝像機視覺范圍內，就可以完成機器人的手眼標定；而現在廣泛使用的仿射方法完成機器人的手眼標定至少需要移動四次機械臂，分別對攝像機視野內四個不同的點完成注冊，并且要求四個點不在同一平面內。由此可知一次注冊法相比仿射法可以極大地縮短手眼標定時間，提高標定效率。
為了具體比較兩種注冊方法的工作效率，讓 3 位操作者分別使用仿射法和一次注冊法進行手眼標定，每一種方法重復操作 5 次，記錄操作時間，耗時對比如表 1 所示。
表1
兩種方法用時對比
Table1.
Comparison of time spent with the two methods
通過以上實驗結果的對比可以知道，相對于傳統仿射法，一次注冊法標定效率可以提高 3～4 倍，簡化了操作過程，顯著減少了操作時間，體現了一次注冊法的優越性。
3 參數優化
UR 機器臂有很高的重復定位精度，但是由于機器人末端工具的制造、安裝誤差，使末端工具坐標系與機器人末端坐標系的實際轉換矩陣與理論轉換矩陣產生偏差。如果一次注冊法使用該理論轉換矩陣，就會造成空間映射不準確，從而導致機器人的絕對定位精度下降，不能達到臨床手術的要求。因此為提高該方法的絕對定位精度，需要對末端工具的參數進行優化。
3.1 誤差方程
本文研究內容以 UR 機器臂為研究平臺，UR 機器臂具有 6 個轉動關節，為標定機器人末端坐標系E 與機器人末端工具坐標系M 之間的轉換矩陣，排除機器人 D-H（Denavit-Hartenberg）參數對其影響，將機器人的基坐標系移動至機器人末端坐標系處，虛擬放置一個參考坐標系V，并只通過轉動第 6 關節來改變參考坐標系與機器人末端坐標系的轉換關系。當機器人的第 6 關節角為 0° 時，參考坐標系V 與機器人末端坐標系E 重合，當且僅當機器人第 6 關節旋轉時，參考坐標系V 保持不變，機器人末端坐標系E 繞第 6 關節Z 軸旋轉。當第 6 關節轉動θ 角時，推導出機器人末端坐標系與參考坐標系之間的轉換關系
末端工具坐標系與機器人末端坐標系的轉換關系用R 表示，機器人末端工具中光學標記點在末端工具坐標系的坐標為X，末端工具中光學標記點在參考坐標系的坐標為P，描述如下：
| $P = \, _V^E \! T \cdot \, _E^R\! T \cdot X = ERX$ |
初始位置（機器人第 6 關節角度為θ0）末端工具中 3 個標記點在參考坐標系下坐標為X1、X2、X3，攝像機識別末端工具中的光學標記點后，可知標記點在攝像機坐標系下坐標為S1、S2、S3。機器人第 6 關節轉動 60° 后，選取標記點坐標X ′1 和S ′1。基于仿射坐標系的空間映射原理，由X1、X2、X3、X ′1 和S1、S2、S3、S ′1，可以建立攝像機坐標系和參考坐標系的轉換關系 。由以上轉換關系可以推導出末端工具中角點在攝像機坐標系的坐標S ′，描述如下：
| $S' = \,_C^V \! T \cdot P$ |
為了便于實際運用，我們對機器人運動學模型的表達式做一下轉換，并賦予新的含義。將式（6）變換如下：
| $S' = \, _C^V \! T \cdot E \cdot R \cdot X = f\left( D \right)$ |
這里D 表示參考坐標系與末端工具坐標系的轉換矩陣。通過旋轉第 6 關節，采集末端工具中光學標記點的坐標S ′。
設 ，在不同位姿下 的位置坐標為 ，由式（2）可得：
| ${b_{ij}} = {f_j}\left( {{t_i}} \right)\left( {i = 1,2,3, \cdot \! \cdot \! \cdot ,n;j = 1,2,3} \right)$ |
即
| $\begin{array}{*{20}{c}}{{b_{11}} = {f_1}\left( {{t_1}} \right)} & {{b_{12}} = {f_2}\left( {{t_1}} \right)} & {{b_{13}} = {f_3}\left( {{t_1}} \right)}\\{{b_{21}} = {f_1}\left( {{t_2}} \right)} & {{b_{22}} = {f_2}\left( {{t_2}} \right)} & {{b_{23}} = {f_3}\left( {{t_2}} \right)}\\{} & \vdots & {}\\{{b_{n1}} = {f_1}\left( {{t_n}} \right)} & {{b_{n2}} = {f_2}\left( {{t_n}} \right)} & {{b_{n3}} = {f_3}\left( {{t_n}} \right)}\end{array}$ |
根據式（9）可設想，在不考慮其他因素影響的情況下，當D 絕對精確，那么S ′ 的位置坐標也應該絕對準確，即有
| $\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{b_{ij}} - {S_{ij}}} \right|} } = 0$ |
實際上，參數矩陣D 不可能絕對精確，即
| $\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{b_{ij}} - {S_{ij}}} \right|} } > 0$ |
將式（8）代入式（10）有
| $\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{f_j}\left( {{t_i}} \right) - {S_{ij}}} \right|} } = 0$ |
對 做如下分解
| ${t_i} = t\left( {{\theta _0} + \Delta {\theta _i}} \right) \cdot {t_0}$ |
其中， 為末端工具坐標系與機器人末端坐標系的轉換矩陣， 為參考坐標系與機器人末端坐標系隨第 6 關節轉動 后的轉換矩陣。將式（13）代入式（12）得到如下方程
| $\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{f_j}\left( {t\left( {{\theta _0} + \Delta {\theta _i}} \right) \cdot {t_0}} \right) - {S_{ij}}} \right|} } = 0$ |
采集第 6 關節轉動量一次則確定一個 值，以 為常量，將 作為變量，令 ， ，則式（14）可變換成：
| $F\left( {{D_0}} \right) = \sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{f_j}\left( {{K_i} \cdot {D_0}} \right) - {S_{ij}}} \right|} } = 0$ |
對于定位精度要求非常高的手術機器人，機器人參數誤差可以用如下公式表式
| $\varepsilon = \frac{1}{{3n}}\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{f_j}\left( {{K_i} \cdot {D_0}} \right) - {S_{ij}}} \right|} } = \frac{1}{{3n}}F\left( {{D_0}} \right)$ |
由式（15）和式（16）可知，若使機器人參數誤差ε最小，只需要求解出式（15）的解 。即使不能求解出實際解，而得到近似解，也可得到更加符合實際的運動學模型。依照得到的結果[18]，調整機器人的運動學參數，可提高機器人的絕對定位精度，進而滿足臨床手術要求。
3.2 目標函數
對方程（15）的求解比較困難，但可知求解方程（15）的實質就是在滿足一定條件下求解等式（15）的最優解。根據此思路，我們考察的目標是機器人的絕對定位精度，以在攝像機坐標系下末端工具中點坐標Sij 為真實參考點，由式（15）和式（16）建立目標函數
| $\left\{ \!\!\! {\begin{array}{*{20}{l}}{\min \varepsilon \!=\! {F_0}\left( x \right) \!=\! \frac{1}{{3n}}\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{f_j}\left( {{K_i} \cdot x} \right) \!-\! {S_{ij}}} \right|} } }\\\! {s.t\;\;\;\left| {x - {t_0}} \right| \leqslant \delta }\end{array}} \right.$ |
其中， 為末端工具坐標系與機器人末端坐標系的理論轉換矩陣；δ 為允許的偏差范圍±2。由此思路建立的目標函數綜合反映了機器人末端工具理論參數與實際參數的偏離程度。當目標函數取得最小時，得到的運動學參數可使機器人的絕對定位精度最高。
至此，將求解運動學模型的問題轉化為在滿足規定條件下求解變量最值的問題。
3.3 仿真驗證
為了驗證該方法的可行性，采用 MATLAB 軟件仿真的方式對該方法進行驗證。因為在 MATLAB 中不能利用攝像機直接識別末端工具中點的坐標，所以先由 MATLAB 隨機生成末端工具坐標系和機器人末端坐標系的實際轉換矩陣 和參考坐標系與攝像機坐標系的轉換矩陣 。由參考坐標系與機器人末端坐標系的關系可知 ，末端工具中標記點末端工具坐標系的坐標MX ，可推知末端工具的標記點坐標 ，由此替代攝像機獲取末端工具的標記點坐標。
我們選取初始位姿時末端工具中 3 個標記點坐標，然后將機械臂的第 6 關節旋轉一定角度后，任意選取其中一個點，通過 4 個不在同一平面內的點建立仿射坐標系。為保證所選的 4 個點包圍的空間盡量大，又必須使末端工具始終在攝像機視野范圍內，所以將機械臂的第 6 關節旋轉 60°。機器人第 6 關節從起始位角度到終端角度旋轉幅度為 60°，第 6 關節每旋轉 5° 獲取一次末端工具光學標記點坐標代入目標函數式（13），可利用 MATLAB 中的 Optimization Toolbox 工具進行求解。MATLAB 中提供了相應的函數 fmincon，該函數提供了三種優化算法：有效集算法（Active set）、序列二次規劃算法（Succesive Quadratic Programming，SQP）和內點算法（Interior point），利用三種優化算法得到仿真優化結果見表 2。
表2
三種優化方法的迭代結果
Table2.
Iteration results of three optimization methods
由表 2 可知，利用 SQP 序列二次規劃算法誤差最小，獲得的結果與我們先前設定的轉換矩陣 最接近且變化微小，這正好符合機器人末端工具在生產、裝配過程中會產生微小誤差的實際情況。故以這一思路來優化機器人末端坐標系與末端工具坐標系轉換關系，在數學方法上是可行的。
3.4 驗證實驗
實驗通過采集數據、利用優化程序得出優化結果，然后利用優化結果，使用手術機器人系統分別對頭顱模型內設置的多個靶點定位并測定機器人的絕對定位精度，驗證優化結果的可信度。
實驗按照一定規則采集末端工具中光學標記點在攝像機下的實際坐標，在末端工具距離攝像機 560～650 mm 范圍內，每隔 10 mm 按照機器臂第 6 關節從 0～60° 范圍間隔 5° 采集一組數據；然后根據同一距離下采集的數據，利用上述仿真方法，可以得到 10 組優化矩陣。為驗證優化效果，在 CT 掃描過的頭顱模型內設置 5 個靶點，對每個靶點隨機設置 10 條穿刺路徑（見圖 3）。利用優化的參數，控制手術機器人按照設定的路徑對相應的靶點實施定位，并分別測量機器人的絕對定位精度。對比在使用不同優化結果時得到的手術機器人的絕對定位精度，得到最優矩陣為
圖3
手術規劃與注冊姿態
Figure3.
Surgical planning and registration posture
分別利用優化前、后兩組參數以及現有的仿射法實施定位，記錄手術機器人的絕對定位精度，記錄結果見表 3，定位效果對比見圖 4。
表3
三種方法實驗結果對比
Table3.
Comparison of three experimental results
圖4
機器人定位結果對比
Figure4.
Comparison of location results
實驗結果表明：參數優化后機器人的絕對定位精度相比參數優化前的定位精度提高了一倍以上，優于已經臨床應用的仿射法的定位精度，可以滿足臨床手術對定位精度的要求。并且，通過不同的穿刺路徑到達同一靶點的誤差變化微小，在不同靶點處的誤差變化不大，說明優化結果在手術操作空間內有很好的一致性。由此可以證明上述推導出的優化方法是切實可行的，可以有效地提高一次注冊法的絕對定位精度。
4 結論
一次注冊方法相比傳統方法可以快速完成機器人的手眼標定，明顯提高手術效率，考慮到末端工具加工、裝配水平的限制以及手術機器人對定位精度的較高要求，又通過推導和建立機器人末端工具運動學參數的誤差方程，提出了一種切實可行的末端工具參數的優化方法，設計了參數優化模型的目標函數，實現了對機器人末端工具運動學參數的優化。通過仿真方法驗證了優化理論和方法的可行性，并在實際試驗中，通過對理論值的優化得到更加符合實際的運動學參數，顯著地提高了機器人的絕對定位精度，這對一次注冊方法進入臨床應用具有實際意義。
引言
近年來，隨著機器人技術的發展以及患者對微創技術的強烈需求，手術機器人在臨床手術中得到越來越廣泛的應用[1-3]。為使手術機器人能更好地應用于手術中，機器人的操作需要更簡潔、安全，定位精度更高[4]。在腦外科手術中，對于功能性神經病變，如帕金森病等，要求定位精度在 1 mm 左右；而對于一般器質性神經病變，如腦組織活檢、腦出血等，要求定位精度在 2～3 mm[5]。像已經商業化的 Path Finder 系統，定位精度的平均值為 2.7 mm[6]。
現有手術機器人系統大多采用視覺作為其位姿反饋與目標檢測的重要手段。為了讓機器人能準確到達實際的解剖位置，就需要進行機器人手眼標定。而在實際手術中，機器人手眼標定過程復雜、耗時且對機器人定位精度影響很大，由此可知機器人視覺系統對手術機器人的臨床應用具有很大影響。機器人的視覺系統分為眼固定式和眼在手上兩種方式。在工業領域，眼在手上的機器人應用較廣，該方式機器人系統完成手眼標定多利用特殊歐氏群、對偶四元數或二者相結合的方法，都需要進行多次的平移和旋轉，而且攝像機會擠占醫生手術操作空間，不利于手術安全操作，因此不適合應用于醫療手術領域[7-13]。對于眼固定式機器人，攝像機可以遠離手術區域，視野范圍更廣，但該方式機器人系統的現有手眼標定方法都存在缺陷。例如，仿射法要求移動四次機械臂，得到四個不同點坐標，建立仿射矩陣完成標定[14]；微型裝配機器人通過 3 個空間點在機械手基坐標系中的坐標，得到手眼系統之間的轉換矩陣[15]；三維視覺標定方法雖然只需要標定一次，但需要放置靶標，擠占手空間，而且定位誤差過大（4.5 mm）不適合應用于臨床[16]。以上標定方法有的標定過程太繁瑣，有的定位精度太差，都不適合在臨床手術中使用。
由于現有手眼標定方法存在的缺陷，本文以優傲機器人（Universal Robot，UR）機器人系統為研究平臺，旨在得到一種新的快速手眼標定方法，通過將光學靶標安裝在機器人末端的方式，可以快速建立機器人基坐標系與攝像機坐標系的聯系，并利用現代優化方法優化手術機器人末端工具運動學參數，減小參數偏差，提高手術機器人絕對定位精度，以實現該方法在臨床手術中的應用。
1 系統簡介
本文采用 UR 機械臂搭建手術機器人系統來進行研究。該系統由雙目攝像機、UR 機械臂和計算機組成（見圖 1），主要用于腦外科手術。通過將戴有光學標記點的患者的電子計算機斷層掃描（computed tomography，CT）數據導入電腦進行三維重建，醫生制定手術計劃；通過雙目攝像機對戴有光學標記點的患者進行注冊，建立醫學圖像空間與攝像機空間的聯系；通過雙目攝像機對末端工具中光學標記點的識別實現對機械臂的注冊，建立攝像機空間與機器人空間的聯系。最終，實現從醫學圖像空間到機器人空間的映射，從而控制機器人完成對病灶點的定位。
圖1
手術機器人系統及末端工具
Figure1.
Surgical robot system and end-tool
2 一次注冊
2.1 數學原理
手術機器人手眼關系如圖 2 所示，貼有光學標記點的末端工具安裝在機器人末端，攝像機在空間固定，設末端工具坐標系為M，機器人基坐標系為B，攝像機坐標系為C。根據機器人正運動學，可以求解當前任意位姿下，機器人末端坐標系E 與基坐標系B 的轉換關系[17]：
| $_B^E \! T = \,_B^1 \! T \cdot \, _1^2 \! T \cdot\, _2^3 \! T \cdots \, {}_5^E \! T = \left[ {\begin{array}{*{20}{c}}{_B^ER} & {_B^EP}\\0 & 1\end{array}} \right]$ |
可知當機器人末端工具安裝完畢后，末端工具坐標系M 與機器人末端坐標系E 間的轉換矩陣 和 是唯一確定且恒定不變的。當攝像機識別上述末端工具中的光學標記點后，光學標記點在攝像機坐標系的坐標為Si （i=1，2，3），其在末端工具坐標系中坐標為Xi （i=1，2，3），由機器人正運動學可知
| $S = \, _C^M \! T \cdot X$ |
可求出攝像機坐標系C 與末端工具坐標系M 之間的轉換矩陣為
| $_C^MT = S \cdot {X^{ - 1}}$ |
綜合基坐標系與機器人末端坐標系轉換矩陣 和機器人坐標系與末端工具坐標系的轉換矩陣 ，可以推導出機器人基坐標系與攝像機坐標系的轉換矩陣表述為
| $_C^BT = \, _C^M \! T \cdot \, _M^E \! T \cdot \, _E^B \! T$ |
從而完成機器人手眼標定，再利用患者的 CT 圖像空間與攝像機空間的映射關系，實現從圖像空間向機器人空間的映射，進而可以控制機器人沿著醫生在圖像空間規劃好的手術路徑到達病灶點，完成手術定位。
圖2
手術機器人手眼關系
Figure2.
Hand-eye relation of the surgical robot
2.2 一次注冊法與仿射注冊法的標定效率
機器人手眼標定方法——一次注冊法，只需要將機械臂末端移動到攝像機視覺范圍內，就可以完成機器人的手眼標定；而現在廣泛使用的仿射方法完成機器人的手眼標定至少需要移動四次機械臂，分別對攝像機視野內四個不同的點完成注冊，并且要求四個點不在同一平面內。由此可知一次注冊法相比仿射法可以極大地縮短手眼標定時間，提高標定效率。
為了具體比較兩種注冊方法的工作效率，讓 3 位操作者分別使用仿射法和一次注冊法進行手眼標定，每一種方法重復操作 5 次，記錄操作時間，耗時對比如表 1 所示。
表1
兩種方法用時對比
Table1.
Comparison of time spent with the two methods
通過以上實驗結果的對比可以知道，相對于傳統仿射法，一次注冊法標定效率可以提高 3～4 倍，簡化了操作過程，顯著減少了操作時間，體現了一次注冊法的優越性。
3 參數優化
UR 機器臂有很高的重復定位精度，但是由于機器人末端工具的制造、安裝誤差，使末端工具坐標系與機器人末端坐標系的實際轉換矩陣與理論轉換矩陣產生偏差。如果一次注冊法使用該理論轉換矩陣，就會造成空間映射不準確，從而導致機器人的絕對定位精度下降，不能達到臨床手術的要求。因此為提高該方法的絕對定位精度，需要對末端工具的參數進行優化。
3.1 誤差方程
本文研究內容以 UR 機器臂為研究平臺，UR 機器臂具有 6 個轉動關節，為標定機器人末端坐標系E 與機器人末端工具坐標系M 之間的轉換矩陣，排除機器人 D-H（Denavit-Hartenberg）參數對其影響，將機器人的基坐標系移動至機器人末端坐標系處，虛擬放置一個參考坐標系V，并只通過轉動第 6 關節來改變參考坐標系與機器人末端坐標系的轉換關系。當機器人的第 6 關節角為 0° 時，參考坐標系V 與機器人末端坐標系E 重合，當且僅當機器人第 6 關節旋轉時，參考坐標系V 保持不變，機器人末端坐標系E 繞第 6 關節Z 軸旋轉。當第 6 關節轉動θ 角時，推導出機器人末端坐標系與參考坐標系之間的轉換關系
末端工具坐標系與機器人末端坐標系的轉換關系用R 表示，機器人末端工具中光學標記點在末端工具坐標系的坐標為X，末端工具中光學標記點在參考坐標系的坐標為P，描述如下：
| $P = \, _V^E \! T \cdot \, _E^R\! T \cdot X = ERX$ |
初始位置（機器人第 6 關節角度為θ0）末端工具中 3 個標記點在參考坐標系下坐標為X1、X2、X3，攝像機識別末端工具中的光學標記點后，可知標記點在攝像機坐標系下坐標為S1、S2、S3。機器人第 6 關節轉動 60° 后，選取標記點坐標X ′1 和S ′1。基于仿射坐標系的空間映射原理，由X1、X2、X3、X ′1 和S1、S2、S3、S ′1，可以建立攝像機坐標系和參考坐標系的轉換關系 。由以上轉換關系可以推導出末端工具中角點在攝像機坐標系的坐標S ′，描述如下：
| $S' = \,_C^V \! T \cdot P$ |
為了便于實際運用，我們對機器人運動學模型的表達式做一下轉換，并賦予新的含義。將式（6）變換如下：
| $S' = \, _C^V \! T \cdot E \cdot R \cdot X = f\left( D \right)$ |
這里D 表示參考坐標系與末端工具坐標系的轉換矩陣。通過旋轉第 6 關節，采集末端工具中光學標記點的坐標S ′。
設 ，在不同位姿下 的位置坐標為 ，由式（2）可得：
| ${b_{ij}} = {f_j}\left( {{t_i}} \right)\left( {i = 1,2,3, \cdot \! \cdot \! \cdot ,n;j = 1,2,3} \right)$ |
即
| $\begin{array}{*{20}{c}}{{b_{11}} = {f_1}\left( {{t_1}} \right)} & {{b_{12}} = {f_2}\left( {{t_1}} \right)} & {{b_{13}} = {f_3}\left( {{t_1}} \right)}\\{{b_{21}} = {f_1}\left( {{t_2}} \right)} & {{b_{22}} = {f_2}\left( {{t_2}} \right)} & {{b_{23}} = {f_3}\left( {{t_2}} \right)}\\{} & \vdots & {}\\{{b_{n1}} = {f_1}\left( {{t_n}} \right)} & {{b_{n2}} = {f_2}\left( {{t_n}} \right)} & {{b_{n3}} = {f_3}\left( {{t_n}} \right)}\end{array}$ |
根據式（9）可設想，在不考慮其他因素影響的情況下，當D 絕對精確，那么S ′ 的位置坐標也應該絕對準確，即有
| $\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{b_{ij}} - {S_{ij}}} \right|} } = 0$ |
實際上，參數矩陣D 不可能絕對精確，即
| $\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{b_{ij}} - {S_{ij}}} \right|} } > 0$ |
將式（8）代入式（10）有
| $\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{f_j}\left( {{t_i}} \right) - {S_{ij}}} \right|} } = 0$ |
對 做如下分解
| ${t_i} = t\left( {{\theta _0} + \Delta {\theta _i}} \right) \cdot {t_0}$ |
其中， 為末端工具坐標系與機器人末端坐標系的轉換矩陣， 為參考坐標系與機器人末端坐標系隨第 6 關節轉動 后的轉換矩陣。將式（13）代入式（12）得到如下方程
| $\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{f_j}\left( {t\left( {{\theta _0} + \Delta {\theta _i}} \right) \cdot {t_0}} \right) - {S_{ij}}} \right|} } = 0$ |
采集第 6 關節轉動量一次則確定一個 值，以 為常量，將 作為變量，令 ， ，則式（14）可變換成：
| $F\left( {{D_0}} \right) = \sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{f_j}\left( {{K_i} \cdot {D_0}} \right) - {S_{ij}}} \right|} } = 0$ |
對于定位精度要求非常高的手術機器人，機器人參數誤差可以用如下公式表式
| $\varepsilon = \frac{1}{{3n}}\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{f_j}\left( {{K_i} \cdot {D_0}} \right) - {S_{ij}}} \right|} } = \frac{1}{{3n}}F\left( {{D_0}} \right)$ |
由式（15）和式（16）可知，若使機器人參數誤差ε最小，只需要求解出式（15）的解 。即使不能求解出實際解，而得到近似解，也可得到更加符合實際的運動學模型。依照得到的結果[18]，調整機器人的運動學參數，可提高機器人的絕對定位精度，進而滿足臨床手術要求。
3.2 目標函數
對方程（15）的求解比較困難，但可知求解方程（15）的實質就是在滿足一定條件下求解等式（15）的最優解。根據此思路，我們考察的目標是機器人的絕對定位精度，以在攝像機坐標系下末端工具中點坐標Sij 為真實參考點，由式（15）和式（16）建立目標函數
| $\left\{ \!\!\! {\begin{array}{*{20}{l}}{\min \varepsilon \!=\! {F_0}\left( x \right) \!=\! \frac{1}{{3n}}\sum\limits_{i = 1}^n {\sum\limits_{j = 1}^3 {\left| {{f_j}\left( {{K_i} \cdot x} \right) \!-\! {S_{ij}}} \right|} } }\\\! {s.t\;\;\;\left| {x - {t_0}} \right| \leqslant \delta }\end{array}} \right.$ |
其中， 為末端工具坐標系與機器人末端坐標系的理論轉換矩陣；δ 為允許的偏差范圍±2。由此思路建立的目標函數綜合反映了機器人末端工具理論參數與實際參數的偏離程度。當目標函數取得最小時，得到的運動學參數可使機器人的絕對定位精度最高。
至此，將求解運動學模型的問題轉化為在滿足規定條件下求解變量最值的問題。
3.3 仿真驗證
為了驗證該方法的可行性，采用 MATLAB 軟件仿真的方式對該方法進行驗證。因為在 MATLAB 中不能利用攝像機直接識別末端工具中點的坐標，所以先由 MATLAB 隨機生成末端工具坐標系和機器人末端坐標系的實際轉換矩陣 和參考坐標系與攝像機坐標系的轉換矩陣 。由參考坐標系與機器人末端坐標系的關系可知 ，末端工具中標記點末端工具坐標系的坐標MX ，可推知末端工具的標記點坐標 ，由此替代攝像機獲取末端工具的標記點坐標。
我們選取初始位姿時末端工具中 3 個標記點坐標，然后將機械臂的第 6 關節旋轉一定角度后，任意選取其中一個點，通過 4 個不在同一平面內的點建立仿射坐標系。為保證所選的 4 個點包圍的空間盡量大，又必須使末端工具始終在攝像機視野范圍內，所以將機械臂的第 6 關節旋轉 60°。機器人第 6 關節從起始位角度到終端角度旋轉幅度為 60°，第 6 關節每旋轉 5° 獲取一次末端工具光學標記點坐標代入目標函數式（13），可利用 MATLAB 中的 Optimization Toolbox 工具進行求解。MATLAB 中提供了相應的函數 fmincon，該函數提供了三種優化算法：有效集算法（Active set）、序列二次規劃算法（Succesive Quadratic Programming，SQP）和內點算法（Interior point），利用三種優化算法得到仿真優化結果見表 2。
表2
三種優化方法的迭代結果
Table2.
Iteration results of three optimization methods
由表 2 可知，利用 SQP 序列二次規劃算法誤差最小，獲得的結果與我們先前設定的轉換矩陣 最接近且變化微小，這正好符合機器人末端工具在生產、裝配過程中會產生微小誤差的實際情況。故以這一思路來優化機器人末端坐標系與末端工具坐標系轉換關系，在數學方法上是可行的。
3.4 驗證實驗
實驗通過采集數據、利用優化程序得出優化結果，然后利用優化結果，使用手術機器人系統分別對頭顱模型內設置的多個靶點定位并測定機器人的絕對定位精度，驗證優化結果的可信度。
實驗按照一定規則采集末端工具中光學標記點在攝像機下的實際坐標，在末端工具距離攝像機 560～650 mm 范圍內，每隔 10 mm 按照機器臂第 6 關節從 0～60° 范圍間隔 5° 采集一組數據；然后根據同一距離下采集的數據，利用上述仿真方法，可以得到 10 組優化矩陣。為驗證優化效果，在 CT 掃描過的頭顱模型內設置 5 個靶點，對每個靶點隨機設置 10 條穿刺路徑（見圖 3）。利用優化的參數，控制手術機器人按照設定的路徑對相應的靶點實施定位，并分別測量機器人的絕對定位精度。對比在使用不同優化結果時得到的手術機器人的絕對定位精度，得到最優矩陣為
圖3
手術規劃與注冊姿態
Figure3.
Surgical planning and registration posture
分別利用優化前、后兩組參數以及現有的仿射法實施定位，記錄手術機器人的絕對定位精度，記錄結果見表 3，定位效果對比見圖 4。
表3
三種方法實驗結果對比
Table3.
Comparison of three experimental results
圖4
機器人定位結果對比
Figure4.
Comparison of location results
實驗結果表明：參數優化后機器人的絕對定位精度相比參數優化前的定位精度提高了一倍以上，優于已經臨床應用的仿射法的定位精度，可以滿足臨床手術對定位精度的要求。并且，通過不同的穿刺路徑到達同一靶點的誤差變化微小，在不同靶點處的誤差變化不大，說明優化結果在手術操作空間內有很好的一致性。由此可以證明上述推導出的優化方法是切實可行的，可以有效地提高一次注冊法的絕對定位精度。
4 結論
一次注冊方法相比傳統方法可以快速完成機器人的手眼標定，明顯提高手術效率，考慮到末端工具加工、裝配水平的限制以及手術機器人對定位精度的較高要求，又通過推導和建立機器人末端工具運動學參數的誤差方程，提出了一種切實可行的末端工具參數的優化方法，設計了參數優化模型的目標函數，實現了對機器人末端工具運動學參數的優化。通過仿真方法驗證了優化理論和方法的可行性，并在實際試驗中，通過對理論值的優化得到更加符合實際的運動學參數，顯著地提高了機器人的絕對定位精度，這對一次注冊方法進入臨床應用具有實際意義。
