(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210774656.3 (22)申请日 2022.07.01 (71)申请人 上海术理智能科技有限公司 地址 201401 上海市奉贤区程普路37 7号4 幢B区10层 (72)发明人 杜昊　唐顿　王薇　 (74)专利代理 机构 南京行高知识产权代理有限 公司 32404 专利代理师 肖念 (51)Int.Cl. B25J 9/16(2006.01) A61H 1/00(2006.01) (54)发明名称 基于MINIMAL-SNAP的被动运动轨迹生成方 法 (57)摘要 本发明公开了一种基于MINIMAL ‑SNAP的被 动运动轨迹生成方法， 进行初始条件设置， 分配 一个稀疏轨迹点序列； 设置分段最大允许数目， 进行稀疏轨迹点序列分段， 并配置分配时间函 数； 确定目标约束矩阵； 确定过程约束矩阵， 包括 配置导数约束， 配置连续性约束； 回调求解段内 轨迹参数矩阵， 保存末端端点状态值； 遍历循环 处理两点之间的所有 段， 然后再处理轨迹序列的 所有点， 将生成的序列发往轨迹执行机构进行执 行生成轨迹。 本发明通过给定一个稀 疏粗糙的轨 迹点序列， 自主完成轨迹平滑， 动力学平滑， 且在 兼顾执行速度的情况下完成轨迹点高效输出， 弥 补了低成本运动控制器无法生成复杂轨迹的缺 陷， 可以为患者 提供良好的被动运动康复体验。 权利要求书2页 说明书6页 附图5页 CN 115157244 A 2022.10.11 CN 115157244 A 1.一种基于 MINIMAL‑SNAP的被动运动轨 迹生成方法， 其特 征在于， 包括 步骤： 步骤1， 进行初始条件设置， 分配一个稀疏轨迹点序列， 包括设置初始位置， 初始速度， 初始加速度； 步骤2， 设置轨迹分段最大允许数目， 进行稀疏轨迹点序列分段， 并配置分配时间函数， 配置约束加速度， 配置约束速度； 步骤3， 确定目标约束矩阵； 确定过程约束矩阵， 包括配置导数约束， 配置连续性约束； 回调求解段内轨 迹参数矩阵， 保存末端 端点状态值， 发送计算结果； 步骤4， 遍历循环处理两点之间的所有段， 然后再处理轨迹序列的所有点， 将每次生成 的序列发送往轨 迹执行机构进行 执行， 生成轨 迹。 2.根据权利 要求1所述的基于MINIMAL ‑SNAP的被动运动 轨迹生成方法， 其特征在于， 步 骤1中， 预先分配好稀疏轨迹点序列， 采用基于可扩展标记 性语言的数据序列 表示方法存储 分配的稀疏轨 迹点信息， 以时间或空间顺序存 储相应的稀疏轨 迹点信息 。 3.根据权利 要求1所述的基于MINIMAL ‑SNAP的被动运动 轨迹生成方法， 其特征在于， 步 骤2中， 每两个离散的点为一个组合， 再将两点轨迹分段， 段内轨迹序列的空间信息和运动 学信息多 项式表达为： X(t)＝c5t5+c54t4+c3t3+c2t2+c1t+c0 对该多项式求导可求速度， 加速度。 4.根据权利 要求3所述的基于MINIMAL ‑SNAP的被动运动 轨迹生成方法， 其特征在于， 步 骤2中， 每完成两个点， 时间参数重置并重新开始计时， 在每个Tn时刻结束计时都会被重置 为0， 则每段时间依次为Tn‑Tn‑i。 5.根据权利 要求4所述的基于MINIMAL ‑SNAP的被动运动 轨迹生成方法， 其特征在于， 步 骤2中， 采用梯形加减速负责时间矩阵的分配， 时间分配函数为： 其中， 分配距离dist， 加减速距离d。 6.根据权利 要求5所述的基于MINIMAL ‑SNAP的被动运动 轨迹生成方法， 其特征在于， 步 骤3中， 获取每个段的持续时间， 将每一段设定为目标约束矩阵M的一列， 并使M的导数表达 式满足X(t)的表示形式， 则目标约束矩阵满足下面的公式： 7.根据权利 要求6所述的基于MINIMAL ‑SNAP的被动运动 轨迹生成方法， 其特征在于， 步 骤3中， 为保证轨迹点连续运动平滑， 对轨迹矩阵进行导数约束和连续性约束； 在上一段的 末尾位置和下一段起始位置都能经 过Pi， 则在此处施加导数约束， 导数约束为：权　利　要　求　书 1/2 页 2 CN 115157244 A 2保证生成的轨迹矩阵在加速度， 加加速度， 加加加速度连续， 则至少保证 各个导数在Pi， 连续性约束为： 8.根据权利 要求7所述的基于MINIMAL ‑SNAP的被动运动 轨迹生成方法， 其特征在于， 步 骤3中， 在经过轨迹点分段处理和导数约束和连续性约束之后， 每次得到的矩阵M是一个带 状非奇异矩阵， b是一个指 定的始末 状态约束， 构建一个线性方程组Mc＝b， 求解其中参数矩 阵c； 轮询执行回调函数， 每次回调执行完毕之后根据回调来取得参数矩阵， 获得的参数矩 阵结合时间分配函数来合成相应的轨迹点的位置、 速度、 加速度、 加加速度信息； 然后向执 行器发送计算结果， 每次都保存一份上次末端状态值， 每次发布保持两点连续向执行器发 布。权　利　要　求　书 2/2 页 3 CN 115157244 A 3