(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210910189.2 (22)申请日 2022.07.29 (71)申请人 北京控制工程研究所 地址 100080 北京市海淀区北京272 9信箱 (72)发明人 肖帅　宁宇　张世俊　梁静静　 王韬　张树华　张弛　刘蕊　 刘忠汉　 (74)专利代理 机构 中国航天科技专利中心 11009 专利代理师 庞静 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 空间机械臂与航天器基座的协同自主避障 规划方法及系统 (57)摘要 空间机械臂与航天器基座的协同自主避障 规划方法及系统， 属于空间控制技术领域。 此协 同自主避障规划方法包括： 在任务空间对空间机 械臂的末端进行路径规划； 依次选取末端的运动 路径点数据， 求解末端的运动路径 点数据对应的 关节角度； 基于当前航天器基座的姿态角和空间 机械臂的关节角度进行碰撞检测； 当检测到发生 碰撞时， 通过对航天器基座的姿态进行自适应调 整， 使得空间机械臂避开障碍物继续运动， 从而 实现空间机械臂与航天器基座的协同自主避障 规划。 权利要求书3页 说明书9页 附图5页 CN 115179293 A 2022.10.14 CN 115179293 A 1.空间机 械臂与航天器 基座的协同自主 避障规划方法， 其特 征在于， 包括： (1)在任务空间对空间机械臂的末端进行路径规划， 得到末端在任务空间的运动路径 点数据； 依顺序选取运动路径点数据， 重复执 行(2)至(5)操作： (2)根据航天器基座当前姿态角、 当前空间机械臂末端运动路径点数据， 计算空间机械 臂关节角度； (3)根据航天器基座当前姿态角、 空间机械臂关节角度， 进行碰撞检测； 如发生碰撞， 转 至执行(4)； 如不发生碰撞， 当前航天器基座姿态角为目标姿态角， 空间机械臂关节角度为 目标关节角， 转至执 行(5)； (4)通过对航天器基座的姿态角进行自适应调整， 并重新求解空间机械臂关节角度， 直 至使空间机械臂与障碍物不 发生碰撞； 不碰撞时， 航 天器基座姿态角为目标姿态角， 对应的 空间机械臂关节角度为目标关节角； (5)控制航天器基座姿态调整至目标姿态角， 控制空间机械臂 的各关节运动至目标关 节角， 空间机 械臂末端到 达当前末端运动路径点。 2.根据权利要求1所述的空间机械臂与航天器基座的协同自主避障规划方法， 其特征 在于： 步骤(2)包括： (21)根据航天器 基座的姿态角， 计算空间机 械臂基坐标系在任务空间的位姿数据； (22)根据当前运动路径点数据， 计算空间机械臂末端相对于空间机械臂基坐标系的位 姿数据； (23)采用逆运动学 方法， 计算上述得到的位姿数据对应的空间机 械臂关节角度。 3.根据权利要求1所述的空间机械臂与航天器基座的协同自主避障规划方法， 其特征 在于： 步骤(4)包括： (41)保持空间机械臂末端位姿不变， 采用逆运动学方法， 分别计算在航天器基座三轴 姿态角调整 ±Δ时， 所对应的空间机械臂关节角度； 其中， 调整前航天器基座的姿态角为 [φsθsψs]， Δ为调整步长， φs为滚动角， θs为俯仰角， ψs为偏航角； (42)分别计算姿态角变化前后空间机械臂与障碍物的距 离变化量， 组成数列[Δdφ+， Δ dφ‑， Δdθ +， Δdθ‑， Δdψ+， Δdψ‑]， Δdφ+为仅航天器基座滚动角调整+Δ时的距离变 化量， Δdφ‑ 为仅航天器基 座滚动角调整 ‑Δ时的距离变 化量， Δdθ +为仅航天器基 座俯仰角调整+Δ时的 距离变化量， Δdθ‑为仅航天器基座俯仰角调整 ‑Δ时的距离变化量， Δdψ+为仅航天器基座 偏航角调整+Δ时的距离变化 量， Δdψ‑为仅航天器 基座偏航角调整 ‑Δ时的距离变化 量； (43)选取距离变化量最大值所对应的单轴角及方 向进行姿态角调整， 设i为[Δdφ+， Δ dφ‑， Δdθ +， Δdθ‑， Δdψ+， Δdψ‑]中最大值对应的序号， i的取值依次为0～5， 根据i的取值计算 滚动角变化系数dφ、 俯仰角变化系数dθ、 偏航角变化系数dψ： (44)取 在保持空间机械臂末端位姿不变的条件下， 采用逆运动学权　利　要　求　书 1/3 页 2 CN 115179293 A 2的方法， 重新计算 航天器基座姿态角对应的空间机 械臂关节角度； (45)进行碰撞检测， 如果检测到发生碰撞， 则返回步骤(44)， 重新计算； 否则， 当前得到 的航天器基座姿态角为 目标姿态角， 对应的空间机械臂关节角度为 目标关节角， 转至步骤 (5)。 4.根据权利要求3所述的空间机械臂与航天器基座的协同自主避障规划方法， 其特征 在于， 步骤(41)包括： 航天器基座滚动角调整+Δ时， 计算所对应的空间机 械臂关节角度Θφ+； 航天器基座滚动角调整 ‑Δ时， 计算所对应的空间机 械臂关节角度Θφ‑； 航天器基座俯仰角调整+Δ时， 计算所对应的空间机 械臂关节角度Θθ +； 航天器基座俯仰角调整 ‑Δ时， 计算所对应的空间机 械臂关节角度Θθ‑； 航天器基座偏航角调整+Δ时， 计算所对应的空间机 械臂关节角度Θψ+； 航天器基座偏航角调整 ‑Δ时， 计算所对应的空间机 械臂关节角度Θψ‑。 5.根据权利要求4所述的空间机械臂与航天器基座的协同自主避障规划方法， 其特征 在于， 步骤(42)中， 分别计算航天器基座滚动角、 俯仰角、 偏航角中， 上述任一个角调整 ± Δ， 空间机 械臂与障碍物的距离在调整前后的变化 量： 航天器基座滚动角调整+Δ 时， 姿态角变为[φs+Δθsψs]， 关节角为Θφ+， 计算调整后空 间机械臂与障碍物的距离与未调整时的距离之差 Δdφ+； 航天器基座滚动角调整 ‑Δ时， 姿态角变为[φs‑Δθsψs]， 关节角为Θφ‑， 计算调整后空 间机械臂与障碍物的距离与未调整时的距离之差 Δdφ‑； 航天器基座俯仰角调整+Δ时， 姿态角为[φsθs+Δψs]， 关节角为Θθ+， 计算调整后空间 机械臂与障碍物的距离与未调整时的距离之差 Δdθ +； 航天器基座俯仰角调整 ‑Δ时， 姿态角为[φsθs‑Δψs]， 关节角为Θθ‑， 计算调整后空间 机械臂与障碍物的距离与未调整时的距离之差 Δdθ‑； 航天器基座偏航角调整+Δ时， 姿态角为[φsθsψs+Δ]， 关节角为Θψ+， 计算调整后空间 机械臂与障碍物的距离与未调整时的距离之差 Δdψ+； 航天器基座偏航角调整 ‑Δ时， 姿态角为[φsθsψs‑Δ]， 关节角为Θψ‑， 计算调整后空间 机械臂与障碍物的距离与未调整时的距离之差 Δdψ‑。 6.根据权利要求3所述的空间机械臂与航天器基座的协同自主避障规划方法， 其特征 在于， 步骤(5)中， 在保持空间机械臂末端位姿不变的条件下， 设计到当前末端运动路径点 的轨迹规划， 控制空间机 械臂运动至目标关节角。 7.根据权利要求1所述的空间机械臂与航天器基座的协同自主避障规划方法， 其特征 在于， 步骤(3)中， 采用碰撞检测方法判断空间机 械臂是否与障碍物发生碰撞。 8.空间机械臂与航天器基座的协同自主避障规划系统， 其特征在于， 包括路径规划模 块、 碰撞检测模块、 控制运动模块、 角度计算模块； 路径规划模块根据任务需求确定空间机械臂末端需要到达的目标位置、 姿态， 对空间 机械臂末端的运动进 行路径规划， 得到空间机械臂末端的运动路径 点数据， 按照路径顺序， 逐个向角度计算模块发送运动路径点； 角度计算模块接收路径规划模块得到的当前运动路径点数据， 计算空间机械臂的关节 角度； 角度计算模块根据碰撞检测模块的反馈， 调整航 天器基座的姿态角， 并计算对应的空权　利　要　求　书 2/3 页 3 CN 115179293 A 3