面向口腔种植的机器人多姿态轨迹平滑规划

仲重亮，刘云峰，朱伟东，朱赴东

(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算法，更适用于机器人多姿态的轨迹规划.