面向口腔種植的機(jī)器人多姿態(tài)軌跡平滑規(guī)劃

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

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

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

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

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

單位四元數(shù)具有提供平滑插值、避免萬向節(jié)鎖死等優(yōu)點(diǎn)，被廣泛應(yīng)用于機(jī)器人末端的姿態(tài)規(guī)劃中.目前廣泛應(yīng)用的姿態(tài)插補(bǔ)算法主要有Shoemake[16]提出的球面線性插值(spherical linear interpolation，SLERP)和球面立體插值(spherical and quadrangle，SQUAD).SLERP算法計(jì)算簡單，可以高效地實(shí)現(xiàn)兩姿態(tài)間的平滑插值.

當(dāng)SLERP算法對多姿態(tài)進(jìn)行插值時(shí)，軌跡在分段連接點(diǎn)處存在尖點(diǎn)不光滑，這會(huì)給機(jī)器人關(guān)節(jié)帶來沖擊和損害，因此SLERP算法不適合用于多姿態(tài)軌跡插值.為了實(shí)現(xiàn)多姿態(tài)間的平滑插值，Shoemake結(jié)合SLERP算法和Bézier曲線提出球面立體插值SQUAD算法.該方法實(shí)現(xiàn)多姿態(tài)軌跡曲線的連續(xù)，但是仍難以滿足機(jī)器人多姿態(tài)軌跡高階平滑插值的需求，被認(rèn)為是一個(gè)較為成熟的姿態(tài)插補(bǔ)算法.

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

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

1 系統(tǒng)搭建

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

表1 LBR Med 7 R800機(jī)器人性能參數(shù)Tab.1 Performance parameters of LBR Med 7 R800 robot

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

表2 Polaris Vega XT系統(tǒng)技術(shù)參數(shù)Tab.2 Technical parameters of Polaris Vega XT system

圖1 種植牙手術(shù)機(jī)器人系統(tǒng)Fig.1 Dental implant surgery robot system

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

圖2 手術(shù)機(jī)器人系統(tǒng)坐標(biāo)系轉(zhuǎn)換關(guān)系Fig.2 Coordinate system transformation relationship of surgical robot system

2 多姿態(tài)軌跡平滑規(guī)劃方法

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

2.1 基于旋轉(zhuǎn)向量的四元數(shù)插值原理

選用旋轉(zhuǎn)向量和四元數(shù)作為主要姿態(tài)插補(bǔ)和表示方法，并且對該方法的可行性做理論解釋.單位四元數(shù)的一般數(shù)學(xué)表達(dá)方式為

式中：n為旋轉(zhuǎn)軸，是單位向量；θ為對應(yīng)的旋轉(zhuǎn)角度；z為純四元數(shù)，z=[0,n].

若是單位四元數(shù)q對應(yīng)的旋轉(zhuǎn)向量為P=θn，根據(jù)式(1)，數(shù)學(xué)表達(dá)式為

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

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

2.2 三次準(zhǔn)均勻B樣條曲線插值

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

B樣條曲線定義：

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

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

由式(6)和(7)求得導(dǎo)數(shù)曲線的節(jié)點(diǎn)矢量和控制點(diǎn)，從而導(dǎo)數(shù)曲線方程為

導(dǎo)數(shù)曲線首末節(jié)點(diǎn)的重復(fù)度仍為次數(shù)加1，故導(dǎo)數(shù)曲線仍過首末控制點(diǎn)，則B樣條曲線首末端點(diǎn)處的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)為

2.3 平滑過渡曲線構(gòu)造

將 單位 四 元數(shù) 從 S3空間 映 射為R3笛 卡 爾 空間中的三維向量，得到姿態(tài)的旋轉(zhuǎn)向量表示方法，對旋轉(zhuǎn)向量進(jìn)行三維曲線插值.采用線性插值與三次準(zhǔn)均勻B樣條曲線平滑拼接的方式構(gòu)造插補(bǔ)曲線，如圖3所示.三維空間中曲線上的各點(diǎn)即插補(bǔ)得到的旋轉(zhuǎn)向量，再經(jīng)式(3)計(jì)算后映射回四元數(shù)空間，實(shí)現(xiàn)姿態(tài)曲線插補(bǔ).

圖3 平滑過渡曲線構(gòu)造Fig.3 Construction of smooth transition curve

點(diǎn)A、B、C為姿態(tài)四元數(shù)Q0、Q1、Q2在 三 維空間中的映射點(diǎn)，其中AB和BC段采用線性插值，M、N分別為AB、BC中點(diǎn).為了實(shí)現(xiàn)AB和BC間的平滑過渡，得到二階連續(xù)的插補(bǔ)曲線，分別 在MB和BN段增加過渡點(diǎn)D和E，以D和E為 端點(diǎn)，根據(jù)一階導(dǎo)數(shù)和二階導(dǎo)數(shù)的連續(xù)性構(gòu)造三次準(zhǔn)均勻B樣條曲線，保證整個(gè)曲線的C2連續(xù).為了提高插值曲線的精度，要求構(gòu)造的曲線嚴(yán)格通過每一個(gè)給定的型值點(diǎn).

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

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

式中：PB、PD、PE分別為點(diǎn)B、D、E處坐標(biāo).

關(guān)于過渡點(diǎn)D和E的選取，為了減小最大平滑誤差，過渡點(diǎn)可對稱選取，然后根據(jù)式(13)中過渡系數(shù)m進(jìn)行位置調(diào)節(jié),實(shí)驗(yàn)取過渡系數(shù)m=1.則根據(jù)積累弦長參數(shù)化法，取uB=0.5.

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

再根據(jù)過渡點(diǎn)D、E處一階導(dǎo)數(shù)和二階導(dǎo)數(shù)的連續(xù)性可列方程為

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

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

3 實(shí)驗(yàn)分析與驗(yàn)證

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

表3 關(guān)鍵姿態(tài)信息Tab.3 Information of key orientations

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

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

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

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

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

為了比較不同姿態(tài)插值算法的實(shí)時(shí)性能，在Windows 10操作系統(tǒng)上使用Matlab計(jì)算平臺(tái)進(jìn)行測試，用3種姿態(tài)插值算法對給定不同數(shù)量的隨機(jī)關(guān)鍵姿態(tài)進(jìn)行插值計(jì)算,取插補(bǔ)步長 Δu=0.001，分別統(tǒng)計(jì)不同方法所需的時(shí)間t，對該實(shí)驗(yàn)重復(fù)測試100次計(jì)算平均值，統(tǒng)計(jì)結(jié)果如表4所示.

表4 不同算法的計(jì)算時(shí)間Tab.4 Calculation time of different algorithms

SLERP算法計(jì)算簡單，在相同數(shù)量的關(guān)鍵姿態(tài)進(jìn)行插值時(shí)，計(jì)算時(shí)間最短；SQUAD算法在計(jì)算控制點(diǎn)的過程中，涉及四元數(shù)的指數(shù)和對數(shù)運(yùn)算，故計(jì)算時(shí)間結(jié)果最長；所提算法在三維空間中構(gòu)造曲線，計(jì)算相對簡單，故實(shí)時(shí)性能相比SQUAD算法有較大提升.

在算法的插值精度方面，由上節(jié)的理論分析可知，3種算法通過每一個(gè)給定的關(guān)鍵姿態(tài).在對相同的隨機(jī)關(guān)鍵姿態(tài)進(jìn)行插值時(shí)，表5比較不同插值方法所需總角位移 θd的統(tǒng)計(jì)結(jié)果.所提算法與SQUAD算法相對于SLERP算法走過一條更長的路徑，但是2種算法均平滑地插值了所有給定的關(guān)鍵姿態(tài).與SQUAD算法相比，所提算法的總角位移更小，應(yīng)用時(shí)的運(yùn)動(dòng)效率會(huì)更高.

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

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

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

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

4 結(jié)論

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