(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211002683.5 (22)申请日 2022.08.19 (71)申请人 北京空间飞行器总体设计 部 地址 100094 北京市海淀区友谊路104 号 (72)发明人 潘冬　梁常春　辛鹏飞　张晓东　 王瑞　胡成威　高升　王友渔　 谭启蒙　许哲　邱庆林　顾明　 禹霁阳　邹大力　吴志红　周永辉　 沈莹　 (74)专利代理 机构 中国航天科技专利中心 11009 专利代理师 张欢 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 适应复杂任务的空间站机械臂多模式路径 规划系统和方法 (57)摘要 本发明公开了一种适应复杂任务的空间站 机械臂多模式路径规划系统和方法， 系统包括： 指令处理模块、 规划算法模块和基础计算模块； 指令处理模块接收由操作台输入的控制指令， 对 指令格式和数据信息进行解析， 解析后的数据信 息输出给规划算法模块作为计算输入； 规划算法 模块实现多种运动模式规划算法计算； 接收指令 处理模块输出的数据参数， 选择相应的运动模 式， 输出期望关节角度和期望角速度序列数据给 基础计算模块； 基础计算模块实现基础计算功 能， 实现末端 位姿/速度与关节角度/速度之间转 换， 将检测无碰撞的期望关节角度和期望角速度 序列数据输出给关节控制器。 本发 明解决复杂条 件约束下的安全快速路径规划问题。 权利要求书3页 说明书10页 附图4页 CN 115284293 A 2022.11.04 CN 115284293 A 1.一种适应复杂任务的空间站机械臂多模式路径规划系统， 其特征在于， 包括： 指令处 理模块、 规划算法模块和基础计算模块； 指令处理模块： 接收由操作台输入的控制 指令， 对指令格 式和数据信 息进行解析， 解析 后的数据信息输出给规划 算法模块作为计算输入； 所述数据信息包括预编程指令序号、 目 标点期望位姿、 关节期望角度、 关节速度/加速度设置、 末端速度/加速度设置、 视觉伺服目 标点位姿； 规划算法模块： 实现多种运动模式规划算法计算； 接收指令处理模块输出的数据参数， 选择相应的运动模式， 输出期望关节角度和期望角速度序列数据给基础计算模块； 所述运 动模式包括预编程运动模式、 单关节 位置运动模式、 多关节联动运动模式、 末端直线运动模 式、 末端手柄运动模式、 视 觉伺服运动模式； 基础计算模块： 实现基础计算功能， 实现末端位姿/速度与关节角度/速度之间转换， 将 检测无碰撞的期望 关节角度和期望角速度序列数据输出给关节控制器； 所述基础计算包括 正运动学计算、 逆运动学计算、 构建速度连续变化 位移曲线、 碰撞检测计算。 2.根据权利要求1所述的一种适应复杂任务的空间站机械臂多模式路径规划系统， 其 特征在于， 所述正运动学计算， 根据空间机械臂D H参数和关节角度， 计算得到机械臂末端的 位姿。 3.根据权利要求1所述的一种适应复杂任务的空间站机械臂多模式路径规划系统， 其 特征在于， 所述逆运动学计算， 根据空间机械臂D H参数和末端速度， 计算得到机械臂各关节 速度。 4.根据权利要求1所述的一种适应复杂任务的空间站机械臂多模式路径规划系统， 其 特征在于， 所述构建速度连续变化的位移曲线， 包括： 按照梯形法规划机械臂运行路径， 根据起点时刻、 终点时刻、 加速时间、 最大速度以及 终点对应的位移， 得到 速度连续变化的位移曲线的以时间为自变量的各 段的函数表达式。 5.根据权利要求1所述的一种适应复杂任务的空间站机械臂多模式路径规划系统， 其 特征在于， 所述碰撞检测计算， 包括： 地 面算法和在轨算法； 在轨算法： 采用简化几何模型的动态碰撞检测， 将空间站舱体和机械臂本体简化为多 个外包络的圆柱体几何模型， 根据关节实际角度遍历计算各体间距离实现碰撞检测， 输出 是否发生碰撞标识、 碰撞部件和部件间距离； 地面算法： 采用散点扫描静态空间干涉方法， 将空间站舱体和机械臂本体通过散点扫 描法建立外包络模型， 根据关节实际角度遍历计算所有离散点间距离实现碰撞检测， 输出 是否发生碰撞标识、 具体碰撞点和部件上 各点间距离 。 6.根据权利要求1所述的一种适应复杂任务的空间站机械臂多模式路径规划系统， 其 特征在于， 所述预编程运动模式， 中央控制器CPU中预存储 所有关节期 望角度和期 望角速度 信息， 中央控制器CPU接收到操作平台发出的预编程控制指令后， CPU读取相应地址的预存 数据按控制周期输出发送至各个关节控制器， 控制各关节的输出轨 迹； 预编程运动模式下， 在一个控制周期内， 下发一次预存 储的关节期望角度和角速度。 7.根据权利要求1所述的一种适应复杂任务的空间站机械臂多模式路径规划系统， 其 特征在于， 所述单关节位置模式， 包括： 选定单个关节后， 进行单个关节的位置规划， 并将关节规划角度和角速度指令输出给权　利　要　求　书 1/3 页 2 CN 115284293 A 2基础计算模块， 经验证无碰撞后再 下发至关节控制器， 直至运行到位； 所述单个关节 的位置规划采用先加速、 再匀速和后减速的梯形关节速度规划， 得到期 望的关节角速度； 根据基础算法模块中的构建速度连续变化的位移曲线方法， 计算得到关 节角度命令序列， 直至运行到位， 规划结束。 8.根据权利要求1所述的一种适应复杂任务的空间站机械臂多模式路径规划系统， 其 特征在于， 所述多关节联动运动模式， 包括： 选定各关节目标角后， 进行关节空间的位置规划， 并将角速度和角度命令序列输出给 基础计算模块， 经验证无碰撞后再下发至关节控制器， 直至运行到位； 各关节同时规划运 动， 先计算各关节需转动角度， 得到其中最大关节转动 偏差， 利用最大关节角度偏差和最大 设定速度计算运动时间， 根据运动时间重新计算其余的关节最大运动速度， 确保各关节运 动同步。 9.根据权利要求1所述的一种适应复杂任务的空间站机械臂多模式路径规划系统， 其 特征在于， 所述末端直线运动模式， 包括： 末端线速度规划： 根据机械臂初始末端位姿和目标末端位姿间直线距离长度、 最大速 度和加速时间， 计算得到各时刻的末端位置和速度； 角速度规划： 根据初始姿态与终止姿态的偏差、 最大角速度和加速时间， 计算得到各时 刻的期望姿态和角速度。 10.根据权利要求1所述的一种适应复杂任务的空间站机械臂多模式路径规划系统， 其 特征在于， 所述末端手柄运动模式， 包括： 根据任务需要， 通过手柄给出当前坐标系下期望方向的末端移动速度； 经过坐标系变 换， 得出在空间站基坐标系下的末端移动速度， 利用逆运动学计算， 得到各关节运动角 速 度； 直到手柄输入速度指令为 零， 运动结束。 11.根据权利要求1所述的一种适应复杂任务的空间站机械臂多模式路径规划系统， 其 特征在于， 所述视 觉伺服运动模式， 包括： 通过机械臂腕部相机实时测量的目标位姿信 息， 将目标的位姿信 息提供给规划算法模 块， 通过视 觉伺服算法使机 械臂自主运动到目标点， 实现对空间运动目标物的跟踪； 所述视觉伺服算法， 包括： (a)计算目标点坐标系W相对于末端坐标系T在末端坐标系T下的位姿坐标差Doe并计算 末端与目标点之间的距离dv； (b)计算下一步位移量Send＝vel*dt*Doe/dv， 其中， vel为设定的机械臂末端运动速度， dt为运动控制周期， dv为末端与目标点之间距离； (c)根据末端位移量Send， 由运动学逆解计算始末端对应的两组关节角度值θnow和 θnext， 其中θnow为当前关节角度值， θnext为下一个周期开始时的关节角度值； (d)判断此时末端与目标点之间的距离dv是否在给定的误差 范围内， 以及此时末端姿态 欧拉角与目标物姿态差ΔEul是否在给定的误差范围内， 若在， 则跟踪 结束； 否则， 返回到步 骤(a)继续跟踪， 直到满足条件为止 。 12.使用如权利要求1～11任一所述的适应复杂任务的空间站机械臂多模式路径规划 系统的空间站机 械臂多模式路径规划方法， 其特 征在于， 包括： 接收由操作台输入的控制指令， 对指令格式和数据信息进行解析； 所述数据信息包括权　利　要　求　书 2/3 页 3 CN 115284293 A 3