面向口腔種植的機器人多姿態軌跡平滑規劃

仲重亮，劉云峰，朱偉東，朱赴東

(1.浙江大學 機械工程學院，浙江 杭州 310027；2.浙江工業大學 機械工程學院，浙江 杭州 310023；3.浙江大學醫學院附屬口腔醫院，浙江 杭州 310006)

種植牙技術是近幾十年發展起來的一種治療牙齒脫落的有效手術方法，長期使用效果較好，因此近些年來得到越來越多患者的選擇[1].傳統種植牙技術門檻較高，手術操作流程復雜，醫生學習周期長，學習成本較高，對手術操作者提出 很高的要求，非常不利于手術的推廣.在傳統的 種植牙手術過程中，術前醫生會制定嚴謹的手術 方案，通過手持種植牙鉆進行骨鉆孔，這難免引入人為誤差，很難按照術前規劃路徑進行鉆孔植入，醫生的主觀因素對手術精度和手術成功率影響較大[2-3].近些年來人工智能、CAD、CAM、3D打印等現代工程技術逐漸在醫療領域得到廣泛應用，現代醫學逐步邁向智能醫療、精準醫療的時代[4-5]，手術機器人作為一種特殊的醫療器械受到越來越多的關注，因此研究出具有良好使用效果的種植牙手術機器人系統具有重要意義.

種植牙手術機器人系統相比較傳統種植牙技術優勢明顯，手術過程更為客觀，手術精度高，穩定性好，可以嚴格按照術前設計路徑進行植入[6-7]，并且大大降低了人力成本、經濟成本、手術安全成本和醫生的手術操作難度，有利于種植牙手術的推廣.機器人末端執行器的運動規劃作為口腔種植機器人系統的重要技術之一，對技術要求也越來越嚴格.為了確保機器人工作時的精度和效率，提高使用壽命，需要采用連續的軌跡規劃策略.機器人軌跡規劃一般包括位置規劃和姿態規劃[8]，基于現代計算幾何算法的理論基礎，機器人位置軌跡規劃算法現在已經較為成熟，常用方法有多項式插值法、B樣條插值法[9]、NURBS曲線[10]等.姿態軌跡規劃相對復雜，姿態的描述屬于 SO(3)三維旋轉群空間，在歐氏空間下的曲線插值理論不再適用于姿態軌跡插值，因此本研究將圍繞手術機器人的姿態軌跡規劃進行展開.

機器人姿態常見的表示方法包括旋轉矩陣、歐拉角、旋轉向量、四元數等.旋轉矩陣采用9個元素來描述姿態旋轉，因為內存消耗較大并且計算復雜，所以很少直接用于實時性較高的姿態軌跡規劃；歐拉角表示方便，僅使用3個元素，按照既定的旋轉順序繞坐標軸進行旋轉，缺點在于存在萬向節鎖死問題即導致連續插補較為困難[11]；旋轉向量即旋轉角度與單位轉軸相乘的三維向量，簡單直觀、容易理解、插補效果較好，但有如下缺點：1)當旋轉角度為0或 π時，旋轉軸無法確定[12]；2)無法直接對多次連續的旋轉進行合成.單位四元數計算效率高，便于實現旋轉的合成和分解，能夠提供平滑插值，目前在機器人姿態描述中得到廣泛應用[13-14].四元數屬于 S O(3)三維旋轉群空間的四維向量，因此難以在三維空間中直觀理解，并且在三維空間中一些連續性較高的插值算法也無法直接應用于四元數.四元數樣條曲線的構造則需要解非線性方程，無法求出解析解[15]，針對四元數的特點，相關學者圍繞四元數多姿態平滑插值做出大量的研究工作.

單位四元數具有提供平滑插值、避免萬向節鎖死等優點，被廣泛應用于機器人末端的姿態規劃中.目前廣泛應用的姿態插補算法主要有Shoemake[16]提出的球面線性插值(spherical linear interpolation，SLERP)和球面立體插值(spherical and quadrangle，SQUAD).SLERP算法計算簡單，可以高效地實現兩姿態間的平滑插值.

當SLERP算法對多姿態進行插值時，軌跡在分段連接點處存在尖點不光滑，這會給機器人關節帶來沖擊和損害，因此SLERP算法不適合用于多姿態軌跡插值.為了實現多姿態間的平滑插值，Shoemake結合SLERP算法和Bézier曲線提出球面立體插值SQUAD算法.該方法實現多姿態軌跡曲線的連續，但是仍難以滿足機器人多姿態軌跡高階平滑插值的需求，被認為是一個較為成熟的姿態插補算法.

為了進一步提高多姿態軌跡曲線的高階平滑性，Kim等[17]通過類比歐氏空間中的樣條曲線，提出 S O(3)空間中高階可導的四元數樣條曲線，保留樣條曲線的大部分幾何性質.四元數屬于非歐氏空間，在構造四元數樣條曲線時運算繁瑣，只能求解非線性方程來得到曲線控制點的近似解，因此四元數樣條曲線并不適合直接應用于機器人多姿態軌跡規劃中.謝文雅[18]提出一種SLERP線性插值與四元數Bézier曲線相拼接的方式構造姿態插補曲線；Niu等[19]采用SLERP線性插值與四元數B樣條曲線平滑拼接的形式來構造姿態軌跡曲線，使得姿態插補可以滿足C2連續.謝文雅等[18-19]提出的算法均在四元數空間進行計算，但是運算繁瑣并且未準確通過中間關鍵姿態，姿態插值的誤差較大，不適合用于對插值精度較高的場合；Pu等[20]基于對數四元數，提出一種從笛卡爾空間樣條曲線映射到四元數空間的方法，從而實現四元數多姿態平滑插值；Legnani等[21]結合SLERP算法與多項式，提出一種計算負擔較小的姿態軌跡規劃算法.

本研究開發一套自動化程度較高的種植牙手術機器人系統，介紹相關硬件設備及技術指標，對系統各空間坐標系轉換進行分析.針對現有四元數多姿態插值方法存在的高階連續性差.求解復雜等問題，基于四元數與旋轉向量的關系，重點提出一種C2連續的多姿態平滑插值算法.通過將單位四元數映射到三維空間中進行插值計算，大大減少計算難度，最后通過實驗與其他已知姿態插值算法對比分析，驗證所提算法的可行性和有效性.

1 系統搭建

為了開發口腔種植機器人系統，對于種植手術的基本流程和需求進行闡述和分析，盡管市面上或研究中出現的種植系統不同，所遵循的種植牙手術基本原則是一致的.廣義的種植牙手術包含有術前設計、種植位點制孔、種植體植入、種植牙修復以及其他相關手術等流程.本研究所開發的種植牙手術機器人系統重點在于種植位點制孔及種植體植入.種植牙手術的核心要求是將種植體準確、安全、穩定地放置在術前設計的種植位點之上，并且在術后愈合中能夠形成較為良好的骨結合，因此準確性和穩定性是影響手術成功的關鍵因素.根據口腔種植手術的需求，本研究基于光學定位技術開發一套自動化程度較高的種植牙手術機器人系統，該系統主要分為硬件與軟件2個部分，硬件部分主要有主控計算機、機器人本體、機器人控制柜、末端執行器、視覺導航設備以及相關標志器.本系統選用的機器人為KUKA公司研發的LBR Med 7 R800醫療機器人，該機器人采用S-R-S (spherical-rotational-spherical)運動學結構，具有7個自由度，重量輕便，工作空間可以滿足口腔種植手術的要求，具有精確、靈活、安全及靈敏等優點，主要性能參數如表1所示.

表1 LBR Med 7 R800機器人性能參數Tab.1 Performance parameters of LBR Med 7 R800 robot

視覺導航設備選用加拿大NDI公司開發的Polaris Vega XT系統，該系統原理為近紅外雙目系統通過識別標志器上4個近紅外反光球的球心點，構建標志器的工具坐標系，從而實現對器械的跟蹤和定位.該定位系統具有較高的定位精度和跟蹤速度，并且視場范圍較大，主要的性能參數如表2所示，種植牙手術機器人系統的整體搭建如圖1所示.

表2 Polaris Vega XT系統技術參數Tab.2 Technical parameters of Polaris Vega XT system

圖1 種植牙手術機器人系統Fig.1 Dental implant surgery robot system

為了集成手術機器人系統，提高種植手術的準確性和精度，需要引入各坐標系以及明確各坐標系間的轉換關系，手術機器人系統坐標系轉換關系如圖2所示.該系統主要包含6個坐標系：機器人基坐標系CR、機器人法蘭坐標系CF、實際工具坐標系CD、NDI光學定位設備坐標系CN、光學定位標志器坐標系CM、病人口腔坐標系CP.在 各個坐標系之間的轉換關系中，機器人基坐標系與法蘭坐標系之間的變換矩陣通過機器人正運動學，由機器人當前位置實時獲??；標志器坐標系和光學定位設備坐標系之間的變換矩陣通過NDI Polaris Vega XT光學定位設備實時反饋給控制系統；病人口腔坐標系和標志器坐標系之間的變換矩陣臨床上通過CT三維影像數據和智能術前規劃軟件獲取.本研究機器人系統坐標系集成還需獲取法蘭坐標系和實際工具（種植牙鉆）坐標系的轉換矩陣，光學定位設備坐標系和機器人基坐標系之間的轉換矩陣.其中，前者可通過機器人工具坐標系標定得到，后者可通過手眼標定原理獲取.

圖2 手術機器人系統坐標系轉換關系Fig.2 Coordinate system transformation relationship of surgical robot system

2 多姿態軌跡平滑規劃方法

種植牙手術對機器人末端姿態的精度及平滑性有著極高的要求，基于四元數與旋轉向量的轉換關系，提出一種 C2連續的多姿態平滑過渡算法.通過在三維空間中對旋轉向量使用線性插值與B樣條曲線平滑拼接的方式來構造姿態插補曲線，為了克服旋轉向量的使用缺點，將 R3中的旋轉向量轉換為 S3中的單位四元數，實現對單位四元數的多姿態平滑插值計算，并且曲線的連續性保持不變.

2.1 基于旋轉向量的四元數插值原理

選用旋轉向量和四元數作為主要姿態插補和表示方法，并且對該方法的可行性做理論解釋.單位四元數的一般數學表達方式為

式中：n為旋轉軸，是單位向量；θ為對應的旋轉角度；z為純四元數，z=[0,n].

若是單位四元數q對應的旋轉向量為P=θn，根據式(1)，數學表達式為

由式(2)不難看出單位四元數的對數運算與對應旋轉向量之間的關系，即前者的虛部向量是后者的一半.當對樣條曲線取指數運算后，高階連續性保持不變，因此可以在三維空間中對旋轉向量進行插值運算，旋轉向量對應的四元數將保持三維空間中所具有的連續性，由此實現在三維空間中對四元數進行高階連續的姿態插值運算.基于四元數與旋轉向量之間的關系及優缺點，決定采用在三維空間中對旋轉向量進行平滑插值，再映射回四元數空間，使用單位四元數進行姿態表示的方法.在四元數qA～qB進行姿態插值計算中，將四元數轉換為旋轉向量PA、PB，利用三維空間中的相關插值理論在2個旋轉向量之間進行插值計算，得到三維空間中的一系列插補點：

根據式 (3)將 R3中的插補點轉換為S3中的單位四元數q=[a,v]，完成對單位四元數的姿態插值計算，且曲線的連續性保持不變.由于單位四元數屬于 S3空間，無法在歐氏空間對單位四元數姿態軌跡進行直觀分析，在實驗中將利用Hopf映射[22]將單位四元數映射到三維空間中的單位球面上，實現單位四元數插值曲線的可視化.

2.2 三次準均勻B樣條曲線插值

均勻B樣條曲線的特點是節點矢量均勻分布，B樣條基函數在各節點區間內具有統一的表達式，使得計算處理起來簡單方便，計算效率較高.缺點是未保留Bézier曲線的端點幾何性質，即均勻B樣條曲線不再經過控制多邊形的首末控制點.為了獲得更好的端點性質，采用三次準均勻B樣條曲線用來構造平滑過渡曲線.k次準均勻B樣條曲線的節點矢量中，兩端節點的重復度為k+1， 內節點呈均勻分布,若是控制點個數為n(n≥4)， 則節點數為n+k+1.三次準均勻B樣條曲線次數k=3，則節點矢量中首末節點和中間節點為

B樣條曲線定義：

式中：di為n個 控制點，i=(0,1,···,n-1)；Fi,k(u)為B樣條基函數；i為 序號；k為次數.

控制點與基函數一一對應，B樣條基函數可由de Boor-Cox遞推定義，曲線構造需要求解B樣條曲線在端點處的一階導數和二階導數.由于只關注B樣條曲線端點處的導數計算，根據B樣條曲線導數的基本性質求解，對于k次B樣條曲線，導數曲線為k-1次B樣條，并且節點矢量可以通過原曲線節點矢量去掉首末節點獲取[23]，節點矢量對應關系如式(6).導數曲線控制點根據原曲線控制頂點及節點矢量進行求解，具體求解方法如式(7).

由式(6)和(7)求得導數曲線的節點矢量和控制點，從而導數曲線方程為

導數曲線首末節點的重復度仍為次數加1，故導數曲線仍過首末控制點，則B樣條曲線首末端點處的一階導數和二階導數為

2.3 平滑過渡曲線構造

將 單位 四 元數 從 S3空間 映 射為R3笛 卡 爾 空間中的三維向量，得到姿態的旋轉向量表示方法，對旋轉向量進行三維曲線插值.采用線性插值與三次準均勻B樣條曲線平滑拼接的方式構造插補曲線，如圖3所示.三維空間中曲線上的各點即插補得到的旋轉向量，再經式(3)計算后映射回四元數空間，實現姿態曲線插補.

圖3 平滑過渡曲線構造Fig.3 Construction of smooth transition curve

點A、B、C為姿態四元數Q0、Q1、Q2在 三 維空間中的映射點，其中AB和BC段采用線性插值，M、N分別為AB、BC中點.為了實現AB和BC間的平滑過渡，得到二階連續的插補曲線，分別 在MB和BN段增加過渡點D和E，以D和E為 端點，根據一階導數和二階導數的連續性構造三次準均勻B樣條曲線，保證整個曲線的C2連續.為了提高插值曲線的精度，要求構造的曲線嚴格通過每一個給定的型值點.

為了得到 C2連續的插補曲線，要求B樣條曲線 嚴格經過型值點A、B、C，并且在過渡點D和E處保持一階和二階的連續性，由此可以列出7個方程，計算出7個控制點，記為d0、d1、d2、d3、d4、d5、d6.根據式(4)計算三次準均勻B樣條曲線的節點矢量為

由式(5)在點B、D、E處得方程為

式中：PB、PD、PE分別為點B、D、E處坐標.

關于過渡點D和E的選取，為了減小最大平滑誤差，過渡點可對稱選取，然后根據式(13)中過渡系數m進行位置調節,實驗取過渡系數m=1.則根據積累弦長參數化法，取uB=0.5.

式中：l0為線段BD和BE的長度，l1為線段BM和BN長度的較小值，l0、l1均為中間變量.

再根據過渡點D、E處一階導數和二階導數的連續性可列方程為

由式(9)和 (10)，帶入式(14)得：

由式(12)和(15)共7個方程，求解出7個控制點，得到一條 C2連續的平滑過渡曲線.

3 實驗分析與驗證

為了驗證所提算法構造的姿態過渡曲線的平滑性，現以一組具體的關鍵姿態數據為例，如表3所示，其中，q為姿態四元數，P為旋轉向量，θ為旋轉角度，n為轉軸矢量.將所提姿態插值方法與經典的四元數插值算法SLERP和SQUAD的應用效果進行對比分析，比較不同方法構造的姿態插值曲線、旋轉角度、角速度、角加速度和旋轉軸矢量等方面的異同，驗證算法的連續性.對3種姿態插值算法的計算時間進行對比分析，驗證算法的實時性效果；對相同的隨機關鍵姿態進行插值，比較不同方法所需的總角位移，驗證算法的運動效率.

表3 關鍵姿態信息Tab.3 Information of key orientations

利用Hopf映射將單位四元數從 S3空間映射到 S2三維球面上，方便對不同方法得到的構造曲線進行直觀分析.利用SLERP插值方法在關鍵姿態AB與BC之間進行球面線性插值，如圖4(a)所示，利用SQUAD生成的插值曲線如圖4(b)所示取過渡系數m=1來構造姿態平滑過渡曲線，如圖4(c)所示.通過比較圖4，SLERP方法構造的姿態軌跡在B點出現一個明顯的尖點，表明SLERP方法為 C0連續，不適合多姿態的插值計算.采用所提方法和SQUAD方法得到的構造曲線明顯更加平滑，但對SQUAD方法連續性的確定需要進一步的分析和討論.

圖4 不同算法的四元數多姿態插值曲線Fig.4 Quaternion multi-orientation interpolation curves for different algorithms

實驗計算得到SLERP、SQUAD和所提方法3種插值算法在姿態插補過程中的旋轉角度 θ，角速度 ω 和角加速度α .當u在取值區間內均勻取值時，例 如 Δu=0 .001，得到變化曲線如圖5所示.根據圖5(a)，SLERP在多姿態插補中只能保證旋轉角度的連續性，在角速度和角加速度曲線中存在斷點.分析圖5(b)，SQUAD保證角速度的連續性，但是角加速度是非連續的，表明該方法為 C1連續.2種方法都會對機器人關節產生額外的沖擊和振動，對機器人的工作性能帶來損害，而所提算法在角速度和角加速度方面都保持連續，可以驗證在多姿態插值方面優于SLERP和SQUAD算法，適用于機器人末端的多姿態軌跡規劃.

對3種插值方法的轉軸矢量進行分析，如圖6所示.所提算法和SQUAD計算得到的轉軸矢量曲線均是連續平滑的， SLERP方法的轉軸矢量軌跡在連接點處存在明顯的尖點不夠平滑，故不適合用于機器人的多姿態插值計算.

圖6 不同算法的轉軸矢量軌跡曲線Fig.6 Rotation axis trajectory curves for different algorithms

為了比較不同姿態插值算法的實時性能，在Windows 10操作系統上使用Matlab計算平臺進行測試，用3種姿態插值算法對給定不同數量的隨機關鍵姿態進行插值計算,取插補步長 Δu=0.001，分別統計不同方法所需的時間t，對該實驗重復測試100次計算平均值，統計結果如表4所示.

表4 不同算法的計算時間Tab.4 Calculation time of different algorithms

SLERP算法計算簡單，在相同數量的關鍵姿態進行插值時，計算時間最短；SQUAD算法在計算控制點的過程中，涉及四元數的指數和對數運算，故計算時間結果最長；所提算法在三維空間中構造曲線，計算相對簡單，故實時性能相比SQUAD算法有較大提升.

在算法的插值精度方面，由上節的理論分析可知，3種算法通過每一個給定的關鍵姿態.在對相同的隨機關鍵姿態進行插值時，表5比較不同插值方法所需總角位移 θd的統計結果.所提算法與SQUAD算法相對于SLERP算法走過一條更長的路徑，但是2種算法均平滑地插值了所有給定的關鍵姿態.與SQUAD算法相比，所提算法的總角位移更小，應用時的運動效率會更高.

表5 不同算法的總角位移Tab.5 Total angular displacement of different algorithms

為了進一步驗證所提多姿態平滑插值算法的合理性，實驗利用機器人平臺進行應用層面的測試.與理論仿真一樣，給定表3中的3個關鍵姿態，利用不同的四元數插值算法對姿態軌跡進行規劃并且對機器人進行逆運動學求解；針對本系統中S-R-S構型的七自由度機器人逆解優化問題，采用臂角法得到逆解的封閉解，根據關節避限和避奇異的要求，構建目標函數進行求解，實現逆運動學最優解的選取，最終通過分析機器人笛卡爾空間歐拉角的變化情況來評價算法.

通過采集機器人末端相對于機器人基坐標系的歐拉角E(αz,βy,γx)，得到圖7所示的笛卡爾空間姿態角隨時間t的變化曲線.SLERP方法的姿態角連續但不光滑，不適合用于多姿態的軌跡規劃；在機器人運動過程中，SQUAD方法和所提算法得到的姿態角均為連續且光滑的，驗證了所提多姿態插值算法的可行性.綜上實驗驗證表明，所提多姿態平滑插值算法是合理的.

圖7 不同算法的機器人笛卡爾空間歐拉角變化Fig.7 Change of euler angles in robot Cartesian space for different algorithms

4 結論

針對傳統種植牙技術門檻較高、手術操作流程復雜等缺點，本研究基于光學定位技術開發了一套種植牙手術機器人系統，介紹相關硬件設備及技術指標，并對系統空間坐標系轉換等關鍵技術進行研究.針對手術機器人末端執行器姿態運動要求較高等特點，重點提出一種基于四元數和旋轉向量的多姿態C2連續的平滑插值算法，通過在三維空間中采用線性插值與三次B樣條曲線平滑拼接的方式實現對四元數C2連續的多姿態平滑插值.實驗結果表明，所提多姿態插值算法相較于傳統四元數插值算法SLERP和SQUAD具有C2連續性，并且計算量和所需的總角位移較小，在實時性能和運動效率方面優于SQUAD算法，更適用于機器人多姿態的軌跡規劃.