(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210781222.6 (22)申请日 2022.07.04 (71)申请人 北京航空航天大 学 地址 100191 北京市海淀区学院路37号 (72)发明人 于靖军　吴昊然　裴旭　潘杰　 李国鑫　 (74)专利代理 机构 北京挺立专利事务所(普通 合伙) 11265 专利代理师 彭豆 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种连续体 机器人逆运动学求 解方法 (57)摘要 一种连续体机器人逆运动学求解方法， 涉及 连续体机器人运动控制领域， 解决现有逆运动学 方法存在着求解过程复杂， 计算效率低， 甚至存 在奇异位形等问题， 本发明通过位姿插值： 蛇形 臂关键节 点预处理， 蛇形臂关键节 点逆向迭代和 正向迭代计算 以及判断误差值等步骤实现逆运 动最终求解。 本发明方法生成的位姿更加自然， 连续体机器人的各个关节运动分布均匀。 相较于 雅可比矩阵的迭代式算法， 本方法运动分布均 匀， 无奇异点， 不存在奇异位姿 等问题。 将本发明 所述的方法应用到连续体机器人的实时控制当 中， 可以提高运动控制的精度且实时性高， 实现 对连续体 机器人的实时控制。 权利要求书2页 说明书6页 附图5页 CN 114888815 A 2022.08.12 CN 114888815 A 1.一种连续体机器人逆运动学求 解方法， 其特 征是： 该方法由以下步骤实现： 步骤一、 位姿插值； 根据蛇形臂的初始位姿和目标位姿 进行插值， 获得蛇形臂路径上的中间目标点； 步骤二、 蛇形臂关键节点预处 理； 根据蛇形臂的当前位姿和中间目标点的位置， 对蛇形臂的关键节点的位置进行调整； 步骤三、 蛇形臂关键节点逆向迭代计算； 从蛇形臂 的末端开始， 根据上一个关键节点的位姿和下一个关键节点的坐标， 逆向迭 代求解蛇形臂的构型参数； 步骤四、 蛇形臂关键节点 正向迭代计算； 步骤五、 判断误差； 根据蛇形臂的当前位姿和中间目标点的位置， 判断蛇形臂末端距离 中间目标点是否小 于阈值， 若否， 则返回执 行步骤三， 若是， 则执 行步骤六； 步骤六、 根据获得的蛇形臂的构型参数， 蛇形臂运动到下一个目标点； 步骤七、 判断蛇形臂是否 到达路径终点， 若是， 则结束； 若否， 返回执 行步骤二。 2.根据权利要求1所述的一种连续体机器人逆运动学求解方法， 其特征在于： 步骤二 中， 对蛇形臂的关键节点的位置进行调整的具体过程 为： 首先， 设定连续体机器人中间目标点的坐标为pend， 连续体机器人当前末端点的坐标为 pini， 将连续体机器人当前末端点移动到中间目标点； 则连续体机器人末端点移动的距离 为： dinv＝pend‑pini 然后， 移动其他关键节点， 并作当前末端点到中间目标点的连线， 其他关键节点的移动 方向和当前末端点移动的方向一致； 若连续体机器人共有n个关键节点， 则第i个关键节点 的移动距离为： 式中， lm为第m段连续体机器人的长度。 3.根据权利要求1所述的一种连续体机器人逆运动学求解方法， 其特征在于： 步骤三中 蛇形臂关键节点逆向迭代计算的过程 为： 步骤一、 根据第i+1个关键节点的位姿和第i个关键节点的坐标， 求解第i段连续体机器 人的构型参数； 步骤二、 根据第i+1个关键节点的坐标和i段连续体机器人的构型参数， 求解第i个关键 节点的坐标； 步骤三、 返回执 行步骤一， 直到求得第一段 连续体机器人的构型参数。 4.根据权利要求1所述的一种连续体机器人逆运动学求解方法， 其特征在于： 步骤四 中， 通过正向迭代减小和中间目标点的误差， 设定连续体机器人第i个关键节点的坐标为 pi， 连续体机器人第i段的长度为 l， 具体过程 为： 首先， 固定连续体机器人的末端关键节点， 将连续体机器人的第一个关键节点移动到 初始位置； 然后， 移动其他关键节点， 更新后的第i个关键节点位于第i个关键节点和第i ‑1个关键权　利　要　求　书 1/2 页 2 CN 114888815 A 2节点的连线上， 移动距离取决于关键节点所处的相对位置， 如下式所示： 其中： 式中， Pi第i个关键节点的坐标， Pi+1第i+1个关键节点的坐标， P1为第一个关键节点的坐 标， P1'为初始位置坐标， li为第i段连续体机器人的长度。权　利　要　求　书 2/2 页 3 CN 114888815 A 3