(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211385533.7 (22)申请日 2022.11.07 (71)申请人 杭州柳叶刀机 器人有限公司 地址 310000 浙江省杭州市余杭区中泰街 道绿泰路5号 4幢5楼508 (72)发明人 黄志俊　刘金勇　钱坤　陈鹏　 (74)专利代理 机构 武汉天领众智专利代理事务 所(普通合伙) 42300 专利代理师 杨建军 (51)Int.Cl. G16H 20/40(2018.01) G16H 50/50(2018.01) G06F 17/16(2006.01) A61B 34/10(2016.01) A61C 8/00(2006.01) (54)发明名称 一种关于最优路径规划的骨表面随动技 术 (57)摘要 本发明公开了一种关于最优路径规划的骨 表面随动技术， 通过将牙科手术的作业过程抽象 成假体移动路径， 即由机械臂初始坐标系初始位 置移动到目标位置的过程， 将最优路径规划分成 计算位置变化和计算姿态变化两步， 具体包括建 立数学模型、 轨迹规划、 求得多个轨迹点、 获取旋 转矩阵、 将 旋转矩阵转化为相应的四元素和遍历 求解等步骤， 借由合理的数学建模， 将骨表面随 动的最优路径 规划转化为矩阵优化求解的问题， 实现了精确的牙科手术中的精确路径 规划， 同时 也具有一定的通用性， 能够转化应用于类似技术 领域， 具有很强的实用性和广阔的应用场景。 权利要求书3页 说明书6页 附图2页 CN 115424701 A 2022.12.02 CN 115424701 A 1.一种关于最优路径规划的骨表面随动技 术， 其特征在于， 包括以下步骤： 步骤一： 建立数学模型， 将机械臂轨迹规划中选择一条姿态以及位置变化最小的轨迹 使得机械臂根据图像信息定位到种植部位上方 的骨表面随动转化为机械臂的最优路径规 划问题； 步骤二： 构建相应的坐标系， 基于该坐标系对假体根尖点、 假体植入点、 作业器材顶部 根尖点、 作业器材底部植入点的位置进 行定义， 进 行将假体根尖点与假体植入点之间连线、 作业器材顶部根尖点与作业器材底部植入点之间的连线分别基于上述坐标系以向量的形 式进行表达， 分别记为 ， ； 则机械臂的移动路径 实际上转为向量 从初始位置旋转 至与向量 平行的位置的路径路径规划问题； 步骤三： 向量 从初始位置旋转至与向量 平行的位置路径规划， 包括位置变化和 姿态变化， 分别进行计算； 计算位置变化， 以根尖点/钻头基准点建立局部坐标系， 则可以求得钻头/根尖点在向 量 上的投影点p,令已知起始点 为 ， 终点为 ， 则轨迹规划如下： ； 进一步的， 取 为其范围内的不同值， 得到多个轨迹点， 此轨迹即为要求的位置变化轨 迹； 计算姿态变化， 机械臂有无穷多的解可以由机械臂初始状态坐标系移动到目标位置， 以当前机械臂位置姿态为为基座标， 按此沿着坐标系顺时针旋转， 记录旋转的欧拉角， 该角 度即为姿态变化； 步骤四： 对步骤三中的数据进行优化处理， 计算最终的向量 从初始位置旋转至与向 量 平行的位置的路径， 即机 械臂的最优路径。 2.根据权利要求1所述的一种关于最优路径规划的骨表面随动 技术， 其特征在于： 步骤 二还包括以下步骤： 1） 建立数学模型， 定义假体根尖点为 a， 假体植入点为b， 作 业器材顶部根尖点为c,作 业 器材底部植入点 为d， 则有向量： ， ， 上式中， 为由a点指向b点的向量， 为由c点指向d点的向量， 向量 与向量 的夹角为： ； 2） 轨迹规划， 以假体根尖点建立局部坐标系， 则可以求得作业器材顶部在 向量上的投 影点 为 ； 为假体根尖点在上述局部坐标系下的初始位置与最后位置连线长度；权　利　要　求　书 1/3 页 2 CN 115424701 A 2已知起始点 为 ， 终点为 ， 则轨迹规划如下： ； 3） 求得多个轨 迹点， 此轨 迹即为要求的位置变化轨 迹。 3.根据权利要求1所述的一种关于最优路径规划的骨表面随动 技术， 其特征在于： 所述 步骤三包括： 获取多组旋转矩阵， 建立坐标系， 建立机械臂初始 坐标系 （x1,y1,z1） ， 机械臂坐 标系 （x2,y2,z2） ； 植入假体过程中， 假体移动路径可以抽象成由机械臂初始坐标系初始 位置 移动到目标位置的过程， 记录当前机械臂位置姿态为 （rx1为假体移 动路径为绕x旋转的参数， ry1为假体移动路径绕着y旋转的参数,rz1为假体移动路径 为绕z 旋转的参数） ， 以此为基座标， 按 此沿着坐标系顺时针旋转n度， 直至旋转360度， 得到多组欧 拉角， 进而通过数学变化得到多组旋转矩阵： ； 其中， n取值0 到360的整数； 具体包括： 4） 将旋转矩阵转 化为相应的四元 素； 5） 遍历求 解， 引入一个参数q后， 目标机 械臂姿态得四元 数位置可以表达为： ； 多组由当前姿态旋转3 60度得到得目标姿态位置为： ； 遍历360组姿态求： ； 当求得 最小值时， 记下对应的n值， 对应到之前的欧拉角坐标系， 结合我们步骤一 中求得多个轨迹点， 机械臂变化的姿态最小， 这多个点 都选取相同的位姿信息， 即为相同的 信息， 可以保证机 械臂以最快的速度到 达相应的姿态， 得到目标移动点欧拉角为 ； 其中， 即为目标点。 4.根据权利要求2所述的一种关于最优路径规划的骨表面随动 技术， 其特征在于， 所述 步骤2） 中， 取 ， 通过对 λ赋不同值， 得到10 00个不同的轨 迹点。 5.根据权利要求1所述的一种关于最优路径规划的骨表面随动 技术， 其特征在于， 所述 步骤4） 中， 取n=1， 最终得到 360组欧拉角。 6.根据权利要求3所述的一种关于最优路径规划的骨表面随动 技术， 其特征在于， 所述 步骤4） 中， 通过将三组元素旋转矩阵 （elemental rotation matrix） 相乘即可得到旋转矩 阵。 7.根据权利要求1所述的一种关于最优路径规划的骨表面随动 技术， 其特征在于， 所述 步骤5)中， 旋转矩阵转 化为相应的四元 素包括以下步骤： a） 判断旋转矩阵是否为正交矩阵， 如果是进行步骤b） ， 如果不是， 则将其转化为正交矩权　利　要　求　书 2/3 页 3 CN 115424701 A 3