8.2.3　带臂旋翼无人机的协调控制

8.2.3　带臂旋翼无人机的协调控制

1）系统重心估算。

因此关节臂i的重心相对于平台坐标系B｛｝的位置坐标向量为P i 为：

令平台坐标系B｛｝为系统重心参考坐标系，根据多体系统重心求解理论，操作型可悬停飞行器的重心为：

从式（8-8）可以看出，要求r G 必须先求m d。根据系统动力学模型可知：当系统悬停于固定一点，并且飞行平台的俯仰和滚转角都为零的情况下，其4个螺旋桨的升力大小与系统所受重力相等且方向相反，根据受力分析有

其中，g 是重力加速度。

综上所述，在目标抓取物体质量m d 被估测完成后，系统的重心只与主动操作臂的关节角θi 有关，即系统重心r G 为θi 的函数，可写成：

2）重心补偿位姿控制器设计。

旋翼飞行机械臂是一个高度耦合欠驱动系统，其飞行平台具有6个自由度，而输入控制量只有4个。通过将飞行平台的位置控制和姿态控制分离进行独立控制，将位置控制的输出作为姿态控制的输入，并对机械臂的规划运动所带来的扰动进行补偿，设计基于动态重心补偿的位姿控制器，从而可以实现飞行平台快速高精度的位姿控制。图8.4是重心补偿位姿控制器总体结构图，飞行平台的状态测量采用三轴惯性测量传感器经过Kalman滤波器输出，目标轨迹生成器把目标轨迹和偏航角输出给位置跟踪控制器，位置跟踪控制主体为PD控制器，其输出通过系统动力学方程计算得到姿态控制所需的姿态角，姿态稳定控制器采用反演法（backstepping）进行设计，其输出经过式（8-6）的结构解耦，输出到电机。

图8.4　重心补偿位姿控制器总体结构图