(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202210854385.2 (22)申请日 2022.07.20 (65)同一申请的已公布的文献号 申请公布号 CN 114932559 A (43)申请公布日 2022.08.23 (73)专利权人 季华实验室 地址 528200 广东省佛山市南海区桂城街 道环岛南路28号 (72)发明人 王豪　杨鹏　刘振　黄秀韦　 (74)专利代理 机构 佛山市海融科创知识产权代 理事务所(普通 合伙) 44377 专利代理师 许家裕 (51)Int.Cl. B25J 9/16(2006.01) B25J 13/00(2006.01)(56)对比文件 CN 112558621 A,2021.0 3.26 CN 108248 845 A,2018.07.0 6 US 2019321971 A1,2019.10.24 CN 206581905 U,2017.10.24 CN 110440934 A,2019.1 1.12 CN 111687821 A,2020.09.2 2 审查员 潘玉芬 (54)发明名称 飞行机器人控制方法、 装置、 电子设备及系 统 (57)摘要 本申请属于机器人控制技术领域， 公开了一 种飞行机器人控制方法、 装置、 电子设备及系统， 通过控制飞行机器人飞行至参考悬停点， 并启动 机械臂进行抓取作业， 以抓取空中物体； 在抓取 作业过程中， 获取多旋翼飞行平台的第一运动学 信息， 并获取机械臂的第二运动学信息； 根据第 一运动学信息和第二运动学信息计算机械臂基 座受到的扰动力和扰动力矩； 根据扰动力和扰动 力矩计算六自由度位姿补偿装置需付出的补偿 力； 控制六自由度位姿补偿装置输出补偿力以补 偿机械臂 基座的位姿扰动； 从而可提高机械臂末 端的位姿控制精度， 实现对空中物体的可靠抓 取。 权利要求书4页 说明书21页 附图4页 CN 114932559 B 2022.09.30 CN 114932559 B 1.一种飞行机器人控制方法， 用于控制飞行机器人进行空中物体抓取作业， 其特征在 于， 所述飞行机器人包括多旋翼飞行平台、 机械臂以及连接在所述多旋翼飞行平台和所述 机械臂之 间的六自由度位姿补偿装置， 所述六自由度位姿补偿装置用于调节所述机械臂的 机械臂基座的位姿； 所述飞行机器人控制方法包括 步骤： A1.控制所述飞行机器人飞行至参考悬停点， 并启动所述机械臂进行抓取作业， 以抓取 所述空中物体； A2.在抓取作业过程中， 获取所述多旋翼飞行平台的第一运动学信息， 并获取所述机械 臂的第二 运动学信息； A3.根据所述第一运动学信息和所述第二运动学信息计算所述机械臂基座受到的扰动 力和扰动力矩； A4.根据所述扰动力和所述扰动力矩计算所述六自由度位姿补偿装置需付出的补偿 力； A5.控制所述六自由度位姿补偿装置输出所述补偿力以补偿所述机械臂基座的位姿扰 动。 2.根据权利要求1所述的飞行机器人控制方法， 其特 征在于， 步骤A1包括： 控制所述飞行机器人进行巡航飞行以搜索所述空中物体； 在搜索到所述空中物体后， 控制所述飞行机器人接 近所述空中物体； 在所述空中物体进入所述机械臂 的作业范围内时， 进入悬停作业状态， 并启动所述机 械臂进行抓取作业， 以抓取 所述空中物体。 3.根据权利要求1所述的飞行机器人控制方法， 其特 征在于， 步骤A3包括： 根据所述第一运动学信息计算所述多旋翼飞行平台引起的第一扰动力和第一扰动力 矩； 根据所述第二 运动学信息计算所述机 械臂运动引起的第二扰动力和第二扰动力矩。 4.根据权利要求3所述的飞行机器人控制方法， 其特征在于， 所述第 一运动学信 息包括 所述多旋翼飞行平台的第一位姿加速度； 所述第一位姿加速度包括所述多旋翼飞行平台三 个轴向的平 移加速度和三个轴向的角加速度； 所述根据所述第一运动学信息计算所述多旋翼飞行平台引起的第一扰动力和第一扰 动力矩的步骤 包括： 根据以下公式计算所述第一扰动力和所述第一扰动力矩： 其中， 为所述第一扰动力， 为所述第一扰动力矩， 为所述多 旋翼飞行平台的 质量， 为所述多旋翼飞行平台的惯 性张量， 为所述多旋翼飞行平台的平移加速度，权　利　要　求　书 1/4 页 2 CN 114932559 B 2、 、 分别为所述多旋翼飞行平台的三个轴向的平移加速度， 为所述多旋翼飞行 平台的角加速度， 、 、 分别为所述多旋翼飞行平台的三个轴向的角加速度。 5.根据权利要求3所述的飞行机器人控制方法， 其特征在于， 所述第 二运动学信 息包括 所述机械臂各关节的转动角度； 所述根据所述第二运动学信息计算所述机械臂运动引起的第二扰动力和第二扰动力 矩的步骤 包括: 根据所述机械臂各关节的转动角度计算所述机械臂各关节相对所述机械臂基座的质 心位置向量； 根据以下公式计算所述机 械臂相对所述机 械臂基座的总质心位置 矢量： 其中， 为所述机械臂的质量，  为所述机械臂基座的质量， 为所述机械臂的 第 个关节的质量， 为所述机械臂的第 个关节相对所述机械臂基座的质心位置向量， 为所述机 械臂相对所述机 械臂基座的总质心位置 矢量； 根据以下公式计算所述第二扰动力和所述第二扰动力矩： 其中， 为所述第二扰动力， 为所述第二扰动力矩， 为所述机械臂当前抓取到 的物体的质量，   为重力加速度，   为机械臂当前抓取到的所述物体相对所述机械臂 基座的质心位置向量。 6.根据权利要求3所述的飞行机器人控制方法， 其特征在于， 所述六自由度位姿补偿装 置包括与所述多旋翼飞行平台固定连接的固定台和六根连接在所述固定台和所述机械臂 基座之间的伸缩连杆， 所述伸缩连杆的两端分别通过万向铰链与所述固定台和所述机械臂 基座连接； 所述第二 运动学信息包括所述机 械臂基座的第二 位姿加速度； 步骤A4包括： 获取各所述伸缩连 杆的杆长； 获取所述机械臂基座受到的广义重力； 获取所述机械臂基座相对所述多旋翼飞行平台的相对角速度； 根据所述第二位姿加速度、 所述杆长、 所述第一扰动力、 所述第一扰动力矩、 所述第二 扰动力、 所述第二扰动力矩、 所述广义重力和所述相对角速度， 采用基于Newton ‑Euler方程 的动力学模型， 计算各伸缩连 杆的驱动力。 7.根据权利要求6所述的飞行机器人控制方法， 其特征在于， 所述根据 所述第二位姿加 速度、 所述杆长、 所述第一扰动力、 所述第一扰动力矩、 所述第二扰动力、 所述第二扰动力 矩、 所述广义重力和所述相对角速度， 采用基于Newton ‑Euler方程的动力学模 型， 计算各伸权　利　要　求　书 2/4 页 3 CN 114932559 B 3