(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210651217.3 (22)申请日 2022.06.09 (71)申请人 鹏城实验室 地址 518000 广东省深圳市南 山区兴科一 街2号 申请人 广东工业大 学 (72)发明人 鲁仁全　温震霆　孟伟　钟天达　 任鸿儒　 (74)专利代理 机构 深圳市世纪恒程知识产权代 理事务所 4 4287 专利代理师 郭子氚 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 空中机械臂的控制方法、 装置、 设备及存储 介质 (57)摘要 本发明属于无人机技术领域， 公开了一种空 中机械臂的控制方法、 装置、 设备及存储介质。 空 中机械臂包括： 通过连杆依次连接的五个旋转关 节， 空中机械臂与飞行器的底座通过连杆连接； 该方法包括： 获取飞行器对应的初始位姿信息和 各个旋转关节之间的相对位姿信息； 获取期望位 姿信息； 根据初始位姿信息、 相对位姿信息和期 望位姿信息进行运动状态规划， 得到机械臂位姿 期望和飞行器位置期望； 根据飞行器位置期望控 制飞行器达到期望状态； 根据机械臂位姿期望控 制旋转关节对应的驱动电机达到目标值。 通过上 述方式， 将飞行器作为机械臂基座关节， 针对飞 行器和机械臂进行联合运动规划， 实现了机械臂 末端操作器拥有六个自由度， 提升了空中机械臂 的稳定性。 权利要求书2页 说明书11页 附图3页 CN 115026818 A 2022.09.09 CN 115026818 A 1.一种空中机械臂的控制方法， 其特征在于， 所述空中机械臂包括： 通过连杆依次连接 的五个旋转关节， 所述空中机 械臂与飞行器的底座 通过连杆连接； 所述空中机 械臂的控制方法包括： 获取所述飞行器对应的初始位姿信息和各个所述旋转关节之间的相对位姿信息； 获取期望位姿信息； 根据所述初始位姿信息、 所述相对位姿信息和所述期望位姿信息进行运动状态规划， 得到机械臂位姿期望和飞行器位置期望； 根据所述飞行器位置期望控制所述飞行器达 到期望状态； 根据所述机 械臂位姿期望控制所述旋转关节对应的驱动电机 达到目标值。 2.如权利要求1所述的空中机械臂的控制方法， 其特征在于， 所述根据 所述初始位姿信 息、 所述相对位姿信息和所述期望位姿信息进行运动状态规划， 得到机械臂位姿期望和飞 行器位置期望， 包括： 根据所述初始位姿信息和所述相对位姿信息确定各个所述旋转关节相对于地球坐标 系的起始位姿信息； 根据所述初始位姿信 息、 所述起始位姿信 息和所述期望位姿信 息确定所述飞行器和各 个所述旋转关节在各个时间步长的预测位置节点； 基于所述预测位置节点确定 机械臂位姿期望和飞行器位置期望 。 3.如权利要求2所述的空中机械臂的控制方法， 其特征在于， 所述根据 所述初始位姿信 息、 所述起始位姿信息和所述期望位姿信息确定所述 飞行器和各个所述旋转关节在各个时 间步长的预测位置节点， 包括： 识别环境障碍物处于地球坐标系下的位置信息； 根据所述位置信息、 所述初始位姿信息、 所述起始位姿信息和所述期望位姿信息构建 目标环境； 在所述目标环境中随机采样所述飞行器和各个所述旋转关节对应的第一状态点； 根据所述初始位姿信 息、 所述起始位姿信 息和所述第 一状态点确定预设时间步长对应 的第一预测位置节点； 根据所述第 一预测位置节点随机采样第 二状态点， 并根据 所述第一预测位置节点和所 述第二状态点确定下一预设时间步长对应的第二预测位置节点； 根据所述第 二预测位置节点继续进行位置节点预测， 直到预测得到的最终预测位置节 点与所述期望位姿信息匹配， 此时得到所述 飞行器和各个所述旋转关节在各个时间步长的 预测位置节点。 4.如权利要求3所述的空中机械臂的控制方法， 其特征在于， 所述根据 所述初始位姿信 息、 所述起始位姿信息和所述状态点确定预设时间步长对应的第一预测 位置节点之后， 所 述方法还 包括： 根据所述第 一预测位置节点确定所述空中机械臂是否与 所述环境障碍物发生碰撞， 若 是， 则返回所述在所述目标环境中随机采样所述 飞行器和各个所述旋转关节对应的第一状 态点的步骤。 5.如权利要求3所述的空中机械臂的控制方法， 其特征在于， 所述根据 所述第二预测位 置节点继续进 行位置节点预测， 直到预测得到的最终预测位置节点与所述期望位姿信息匹权　利　要　求　书 1/2 页 2 CN 115026818 A 2配之后， 所述方法还 包括： 根据预设模型预测在各个时间步长内所述飞行器和各个所述旋转关节由前一预测位 置节点运动至后一预测位置节点的若干个目标位置节点； 根据所述目标位置节点对所述飞行器和各个所述旋转关节在各个时间步长的预测位 置节点进行补充更新。 6.如权利要求1 ‑5中任一项所述的空中机械臂的控制方法， 其特征在于， 所述根据 所述 机械臂位姿期望控制所述旋转关节对应的驱动电机 达到目标值， 包括： 获取预设加速度； 根据所述机械臂位姿期望按照所述预设加速度控制所述旋转关节对应的驱动电机达 到目标值。 7.如权利要求1 ‑5中任一项所述的空中机械臂的控制方法， 其特征在于， 所述获取所述 飞行器对应的初始位姿信息和各个所述旋转关节之间的相对位姿信息， 包括： 实时获取所述飞行器对应的三轴位置和航向角， 基于所述三轴位置和所述航向角解算 所述飞行器对应的初始位姿信息； 实时获取各个所述旋转关节对应的角度， 基于DH参数模型根据 所述角度解算各个所述 旋转关节之间的相对位姿信息 。 8.一种空中机械臂的控制装置， 其特征在于， 所述空中机械臂包括： 通过连杆依次连接 的五个旋转关节， 所述空中机 械臂与飞行器的底座 通过连杆连接； 所述空中机 械臂的控制装置包括： 初始位姿获取模块， 用于获取所述飞行器对应的初始位姿信 息和各个所述旋转关节之 间的相对位姿信息； 期望位姿获取模块， 用于获取期望位姿信息； 运动规划模块， 用于根据所述初始位姿信息、 所述相对位姿信息和所述期望位姿信息 进行运动状态规划， 得到 机械臂位姿期望和飞行器位置期望； 飞行控制模块， 用于根据所述飞行器位置期望控制所述飞行器达 到期望状态； 电机控制模块， 用于根据所述机械臂位姿期望控制所述旋转关节对应的驱动电机达到 目标值。 9.一种空中机械臂 的控制设备， 其特征在于， 所述设备包括： 存储器、 处理器及存储在 所述存储器上并可在所述处理器上运行的空中机械臂的控制程序， 所述空中机械臂的控制 程序配置为实现如权利要求1至7中任一项所述的空中机 械臂的控制方法。 10.一种存储介质， 其特征在于， 所述存储介质上存储有空中机械臂的控制程序， 所述 空中机械臂的控制程序被处理器执行时实现如权利要求1至7任一项所述的空中机械臂的 控制方法。权　利　要　求　书 2/2 页 3 CN 115026818 A 3