(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211166794.X (22)申请日 2022.09.23 (71)申请人 北京航空航天大 学杭州创新研究院 地址 310051 浙江省杭州市滨江区长河街 道创慧街18号 (72)发明人 余翔　王萌　郭雷　郭克信　 陈泽帅　 (74)专利代理 机构 北京科迪生专利代理有限责 任公司 1 1251 专利代理师 江亚平 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种飞行机 械臂末端抗干 扰指向控制方法 (57)摘要 本发明提供一种飞行机械臂末端抗干扰指 向控制方法， 以解决无人机基座浮动对飞行机械 臂执行任务时的运动学干扰影 响， 首先根据齐次 变换原理构建飞行机械臂执行器末端位姿在惯 性坐标系下的运动学方程， 然后对 无人机基座浮 动对飞行机械臂执行器末端造成的运动学干扰 进行量化分析， 其次设计以无人机和机械臂状态 为输入的神经网络以预测无人机基座的运动信 息， 并通过齐次变换将机械臂在惯 性系的参考轨 迹转换至机械臂 基座坐标系下轨迹， 最后设计模 型预测控制器规划机械臂执行器末端的期望加 速度， 并将其转换到机械臂每个舵机的角度。 本 发明能够显著改善飞行机械臂在无人机基座浮 动情况下的执行器末端精度， 可用于高空雕塑检 测等高精度特种作业任务。 权利要求书3页 说明书8页 附图2页 CN 115431271 A 2022.12.06 CN 115431271 A 1.一种飞行机 械臂末端抗干扰指向控制方法， 其特 征在于， 包括以下步骤： 第一步、 根据齐次变换原 理构建飞行机械臂执行器末端位姿在惯性坐标系下的运动学 方程； 对无 人机基座 浮动对飞行机 械臂执行器末端造成的运动学干扰进行量 化分析； 第二步， 通过齐次变换将机械臂在惯性系的参考轨迹重新设计为无人机坐标系下运动 轨迹， 并设计以无 人机和机 械臂状态为输入的神经网络以预测无 人机基座的运动状态； 第三步、 设计模型预测控制器规划机械臂执行器末端的期望加速度， 并将其转换到机 械臂每个舵机的角度； 完成在无人机浮动基座干扰下飞行机械臂执行器末端的抗干扰指向 控制并实现高精度抓取任务。 2.根据权利要求1所述的一种飞行机械臂末端抗干扰指向控制方法， 其特征在于， 所述 第一步具体步骤如下： 根据齐次变换原理建立飞行机 械臂执行器末端位姿的运动学模型， 表示如下： 式中， Pe表示飞行机械臂 执行器末端在惯性系下的位置， Re是飞行机械臂 执行器末端在 惯性系下的旋 转矩阵； Pb表示无人机基 座质心在惯性系的位置， Rb是无人机质心在惯性系下 的旋转矩阵； 表示机械臂执行器末端在无人机坐标系下的位置， 表示机械臂执行器 末端在无 人机坐标系下的旋转矩阵； Rb的计算公式如下： 式中， 分别表示飞行机 械臂在惯性系下的滚转角， 俯仰角和姿态角； 根据上面建立的运动学方程对其进行定性分析从而量化无人机基座浮动对机械臂执 行器末端所造成的运动学干扰。 3.根据权利要求2所述的一种飞行机械臂末端抗干扰指向控制方法， 其特征在于， 所述 第二步具体步骤如下： 通过齐次变换原理重新设计机械臂运动轨迹， 并设计神经网络预测无人机基座的运动 状态： 将飞行机械臂执行器末端的运动状态在包括旋转和平移的刚体的变换运动下进行表 示， 形式如下： 其中， 表示飞行机械臂执行器末端在惯性坐标系下的齐 次变换矩阵， 是无人机基 座质心在惯性 坐标系下的齐次变换矩阵， 是飞行机 械臂执行器末端在无 人机坐标系的齐 次变换矩阵， I表示1 ×1的单位矩阵； 通过无人机基座的接下来的运动状态 利用齐次变换原 理将机械臂在惯性系 的轨迹转换到自身基座坐标系下， 即无人机坐标系， 以抵消无人机基座的浮动干扰， 其中， t 为当前控制时刻， i 为前向预测的步长；权　利　要　求　书 1/3 页 2 CN 115431271 A 2齐次转换 形式如下： 此时， 飞行机械臂在惯性系下的轨迹追踪问题转换至机械臂在无人机坐标系下轨迹追 踪问题， 通过充分利用机 械臂本身高精度的优点， 保证飞行机 械臂执行器末端的高精度； 无人机基座下一时刻的运动状态通过 下式的非线性 函数来表示： 其中， x(t)表示无人机基座当前的运动状态， ,u(t)表示无人机基座当前的控制输入， 表示无人机基座受到的集合干扰， 包括与自身状态x(t)有关的模型 不确定性和与机 械臂角度q(t)， 角速度 角加速度 有关的动力学耦合干扰两 部分； 建立将无人机和机械臂状态作为输入的神经网络来预测无人机下一步的运动状态， 神 经网络形式如下： η(t+1)＝c( ‑aη(t)+g(Winκ(t)+W η(t) )) fout＝Π(Winκ(t)+Woutη(t+1)) 其中， η(t)表示神经网络的中间神经元状态值； κ(t)表示神经网络的输入， 包括无人机 基座和机械臂的运动状态； W,Win和Wout分别表示神经网络中间神经元， 输入神经元和输出神 经元之间的连接权值； c和a为神经网路中的学习参数； g和Π分别代表神经网络中的函数映 射关系。 4.根据权利要求3所述的一种飞行机械臂末端抗干扰指向控制方法， 其特征在于， 所述 第三步具体步骤如下： 结合神经网络输出的无人机运动状态和运动学齐次变换关系， 并考虑无人机和机械臂 的碰撞避免和机械臂执行器的物理限制， 针对飞行机械臂执行器末端的执行器末端抗干扰 指向问题， 将机械臂执行器末端的运动轨迹通过雅各比矩阵映射到各个关节的轨迹； 模型 预测控制器在 满足多种约束条件下调节机械臂的控制量， 在实现控制目标的同时达到执行 器控制输入最优； 对飞行机 械臂执行器末端 进行运动学状态空间建模， 形式如下： 其中， 表示机械臂执行器末端在j时刻的3维位置和3维速度； hj+1表示机 械臂执行器末端在j+1时刻的3维位置和3维速度； uj表示机械臂执行器末端在j时刻的加速 度； yj+1表示状态空间在j+1时刻的输出； 系统矩阵A、 输入矩阵B和输出矩阵C的取值如下： 式中， σ t表示采样时间间隔， I3表示3×3的单位矩阵， 03表示3×3的零矩阵； 进一步设计 模型预测控制器代价函数， 同时考虑无人机机架与机械臂碰撞避免、 机械臂执行器末端加 速度约束等多种约束， 形式如下：权　利　要　求　书 2/3 页 3 CN 115431271 A 3