[发明专利]一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法

权利要求书

1.一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，其特征在于，包括以下步骤：

第一步、根据旋量理论与指数积公式，建立机械臂的运动学模型；

第二步、建立飞行机械臂的一体化运动学模型，计算相应的雅各比矩阵，将运动学模型分解为可控部分与不可控部分；

第三步、建立飞行机械臂的水平方向质心偏移模型，推导与质心偏移变化率对应的雅各比矩阵；

第四步、将机械臂末端轨迹跟踪精度与质心偏移量同时作为优化目标，建立并求解分层二次规划问题模型，得到满足末端目标跟踪性能且最小化质心偏移的飞行机械臂运动轨迹。

2.根据权利要求1所述的一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，其特征在于，所述第一步包括：

定义当机械臂位于初始位姿时，机械臂第i关节的运动旋量ξ_i：

其中，ω_i表示在机械臂基坐标系下关节i的单位角速度矢量,v_i表示在机械臂基坐标系下关节i的线速度矢量，r_i表示机械臂基坐标系下关节i转轴上任意一点的坐标。所述初始位姿为机械臂各个关节角为0时的位姿；

将运动旋量ξ_i转化为矩阵形式

其中，S(·)为反对称矩阵运算符；

根据指数积公式，计算得到机械臂的前向运动学齐次变换矩阵

其中，表示机械臂末端相对于机械臂基坐标系的旋转矩阵，表示机械臂末端在机械臂基坐标系下的位置，n是机械臂关节数目，q_i是关节i的关节角度，是关节i的指数映射，T₀是机械臂在初始位姿下的齐次变换矩阵，且：

计算机械臂的雅各比矩阵

其中，表示关节i在任意位姿下的运动旋量。可由ξ_i经过伴随变换运算后得到：

3.根据权利要求2所述的一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，其特征在于，所述第二步包括：

依据齐次变换原理建立飞行机械臂的一体化前向运动学模型：

其中，P_e表示机械臂执行器在惯性坐标系下的位置，R_e是机械臂末端在惯性坐标下的旋转矩阵，P_b表示无人机机体坐标系在惯性坐标系下的位置，R_b是无人机机体坐标系相对于惯性坐标系下的旋转矩阵；

将上式对时间求导，得到飞行机械臂的微分运动学方程：

其中，Ω_b和Ω_e分别代表无人机质心和飞行机械臂末端在惯性坐标系下的角速度；是飞行机械臂末端在机体标系下的角速度；是机械臂的关节角速度向量；分别是P_e，P_b，R_b，对时间的导数；

进一步得到飞行机械臂末端在惯性坐标系下的表示：

其中，代表飞行机械臂末端坐标系的线速度和角速度，代表无人机机体坐标系的线速度和角速度；

定义机械臂末端和无人机的角速度与欧拉角速度之间的转换矩阵分别为T_e与T_b，得到飞行机械臂末端位姿在惯性坐标系的表达式为：

其中，x_e＝[P_e Θ_e]^T,x_b＝[P_b Θ_b]^T；Θ_e＝[φ_e θ_e ψ_e]^T，Θ_b＝[φ_b θ_b ψ_b]^T分别是机械臂末端与无人机的欧拉角；θ_e，ψ_b分别为飞行机械臂末端和无人机的滚转角；θ_e，θ_e分别为飞行机械臂末端和无人机的俯仰角；ψ_e，ψ_b分别为飞行机械臂末端和无人机的偏航角；

由于四旋翼无人机是一个欠驱动系统，其三维位置和偏航角通道可控，而俯仰角和滚转角通道不可控，故将微分运动学方程重新排列，得到：

其中，ζ_c＝[P_b ψ_b q]^T，ζ_uc＝[φ_b θ_b]^T，J_c与J_uc是重排列后得到的可控部分雅各比矩阵与不可控部分雅各比矩阵。