作业型飞行机器人控制方法设计及仿真分析

卢利中 丁 伟 刘 旭 关潇卓 王景平

（国网吉林省电力有限公司吉林供电公司，吉林 吉林 132012）

目前，作业型飞行机器人一般由多旋翼飞行机器人、多关节机械臂和末端执行机构组成，具有结构紧凑、工作空间大以及运动灵活等特点[1-2]。飞行机器人完成抓取作业的作业流程可以分为4 个阶段，即巡航飞行、目标接近、悬停作业和负载返航[3]。在完成悬停作业的过程中，由于存在多种不确定性的影响因素（例如多旋翼飞行机器人本身动力学模型具有高度非线性、时变性和不确定性，机械臂运动过程中存在惯性参数变化（例如重心变化）、风场扰动等因素[4]），因此会导致多旋翼飞行机器人的位姿稳定性较差，使机械臂的基座位姿不稳定，从而降低机械臂末端位姿的控制精度，容易导致抓取作业失败[5-6]。针对该问题，该文设计了一种飞行机器人控制方法，可以提高机械臂末端的位姿控制精度。

1 模型搭建

该飞行机器人控制方法包括以下5 个步骤：1）控制飞行机器人飞行至参考悬停点，并启动机械臂完成抓取作业，以抓取空中物体。2）在完成抓取的作业过程中，获取多旋翼飞行平台的第一运动学信息，并获取机械臂的第二运动学信息。3）根据第一运动学信息和第二运动学信息计算机械臂基座受到的扰动力和扰动力矩。4）根据扰动力和扰动力矩计算六自由度位姿补偿装置须付出的补偿力。5）控制六自由度位姿补偿装置输出补偿力，以补偿机械臂基座的位姿扰动。

其中，第一个步骤又包括以下内容：控制飞行机器人进行巡航飞行，以搜索空中物体。在搜索到空中物体后，控制飞行机器人接近空中物体。当空中物体进入机械臂的作业范围内时，进入悬停作业状态并启动机械臂完成抓取作业，以抓取空中物体。通过分阶段飞行控制，可以根据不同阶段的特点采用不同的导航方式，有利于提高抓取空中物体的效率和成功率。当空中物体进入机械臂的作业范围内时，进入悬停作业状态，在开始进行位姿稳定控制的同时进行抓取作业，有利于提高机械臂成功抓取空中物体的可靠性。

第三个步骤包括以下内容：根据第一运动学信息计算多旋翼飞行平台引起的第一扰动力和第一扰动力矩，根据第二运动学信息计算机械臂运动引起的第二扰动力和第二扰动力矩。第一运动学信息包括多旋翼飞行平台的第一位姿加速度，第一位姿加速度包括多旋翼飞行平台3 个轴向的平移加速度和3 个轴向的角加速度。

根据第一运动学信息计算多旋翼飞行平台引起的第一扰动力和第一扰动力矩[7]，如公式（1）所示。

式中：F1为第一扰动力；M1为第一扰动力矩；muav为多旋翼飞行平台的质量；Iuav为多旋翼飞行平台的惯性张量；auav为多旋翼飞行平台的平移加速度；ζx、ζy和ζz分别为多旋翼飞行平台的3 个轴向的平移加速度；αuav为多旋翼飞行平台的角加速度；ζφ、ζθ和ζφ分别为多旋翼飞行平台的3 个轴向的角加速度。

第二运动学信息包括机械臂各关节的转动角度，根据第二运动学信息计算机械臂运动引起的第二扰动力和第二扰动力矩。首先，根据机械臂各关节的转动角度计算机械臂各关节相对机械臂基座的质心位置向量。根据公式（2）计算机械臂相对机械臂基座的总质心位置矢量[8]。

其次，根据公式（3）计算第二扰动力和第二扰动力矩[9]。

六自由度位姿补偿装置包括与多旋翼飞行平台固定连接的固定台和6 根连接在固定台和机械臂基座之间的伸缩连杆，伸缩连杆的两端分别通过万向铰链与固定台和机械臂基座连接；第二运动学信息包括机械臂基座的第二位姿加速度。

第四个步骤包括以下内容：获取各伸缩连杆的杆长；获取机械臂基座受到的广义重力；获取机械臂基座相对多旋翼飞行平台的相对角速度；根据第二位姿加速度、杆长、第一扰动力、第一扰动力矩、第二扰动力、第二扰动力矩、广义重力和相对角速度，采用基于Newton-Euler 方程的动力学模型计算各伸缩连杆的驱动力[10]。

通过该方式控制各伸缩连杆的驱动力，可以实现对机械臂基座位姿的动态补偿，从而有利于降低机械臂末端跟踪误差，达到稳定作业的目的，能够提高空中机械臂的作业准确性、稳定性和环境适应性。

根据第二位姿加速度、杆长、第一扰动力、第一扰动力矩、第二扰动力、第二扰动力矩、广义重力和相对角速度，采用基于Newton-Euler 方程的动力学模型计算各伸缩连杆的驱动力。

根据各伸缩连杆的杆长计算各伸缩连杆在长度方向的方向矢量，根据公式（4）计算雅可比矩阵。

式中：JF为雅可比矩阵；e为伸缩连杆的长度方向的方向矢量；p为伸缩连杆与机械臂基座的铰接点相对机械臂基座的位置矢量。

根据公式（5）计算各伸缩连杆的驱动力。

式中：Fs为驱动力矩阵；Ib为机械臂基座的惯性张量；q为机械臂基座的第二位姿加速度；Gb为机械臂基座受到的广义重力；ωb为相对角速度；I3为3×3 阶的单位矩阵；mb为机械臂基座本身的质量。

对六自由度位姿补偿装置的第i个伸缩连杆来说，其上、下2 个铰接点在机械臂基座坐标系下的位置分别为Bpi1、Bpi2。由于下铰接点在机械臂基座上的位置是固定且已知的，因此Bpi2是已知的，上铰接点在多旋翼飞行平台上的位置是固定且已知的，可以根据各伸缩连杆的长度计算多旋翼飞行平台和机械臂基座坐标系之间的位姿转换矩阵，从而根据该位姿转换矩阵和上铰接点在多旋翼飞行平台上的位置计算得到Bpi1，进而可以根据公式（6）计算各伸缩连杆的长度方向的方向矢量。

机械臂基座的Newton-Euler 方程如公式（7）所示。

式中：ab为机械臂基座的平移加速度，包括世界坐标系在3 个轴向的平移加速度；Mb为机械臂基座受到的力矩；αb为机械臂基座相对多旋翼飞行平台的角加速度；Fbi为第i个伸缩连杆的驱动力。

机械臂基座的动力学模型如公式（8）所示。

式中：为机械臂基座的位姿变化速度（可以通过机械臂基座上的传感器测得）；Cb为机械臂基座的向心力和科氏力的系数矩阵。

在实际应用中，由于多旋翼飞行平台的转动角速度较小，因此机械臂基座的向心力和科氏力可以忽略不计，从而公式（8）可以简化为公式（9）。

2 仿真分析

当该文设计的基于模型预测控制的轨迹跟踪控制器控制机械臂完成抓取作业时，控制其飞行至参考悬停点，启动机械臂进行抓取作业，根据不同阶段的特点采用不同的导航方式，有利于提高抓取空中物体的效率和成功率。为了验证基于模型预测控制的轨迹跟踪控制器的跟踪控制效果，搭建机械臂大机动条件下旋翼平台悬停控制的仿真试验场景：总仿真时间为50 s，在整个模拟期间，机械臂关节以0.05 rad/s2的恒定加速度旋转，并且旋翼平台的预期轨迹是保持悬停。x轴悬停控制结果、y轴悬停控制结果和z轴悬停控制结果分别如图1～图3所示。

图1 x 轴悬停控制结果

图2 y 轴悬停控制结果

图3 z 轴悬停控制结果

设计和模拟末端执行器受到显著外力时旋翼平台的轨迹跟踪控制。试验场景如下：总模拟时间为50 s，在模拟时间段，1 N 的外力在x方向上施加到末端器，同时y方向受到摩擦系数为0.25 的壁面反作用摩擦力，并期望带臂无人机系统在末端器受力时沿y方向均匀移动。受力模式下无人机轨迹跟踪x轴结果、受力模式下无人机轨迹跟踪y轴结果和受力模式下无人机轨迹跟踪z轴结果分别如图4～图6所示。

图4 受力模式无人机轨迹跟踪x 轴结果

图5 受力模式无人机轨迹跟踪y 轴结果

图6 受力模式无人机轨迹跟踪z 轴结果

由图1～图6 可知，基于模型预测控制的轨迹跟踪控制器控制效果较好，与真实值间的误差较小。控制六自由度位置姿态补偿装置对机械臂基座进行补偿，能够保证机械臂基座的稳定性，提高了飞行机器人机械臂末端的控制精度，可以满足工程应用设计要求，具有一定工程意义和研究价值。

3 结语

该飞行机器人控制方法通过估算机械臂基座受到的扰动力和扰动力矩来计算对该扰动力和扰动力矩进行补偿时六自由度位姿补偿装置须付出的补偿力，然后控制六自由度位姿补偿装置对机械臂基座的位姿进行补偿，从而保证机械臂基座位姿的稳定性，在该基础上控制机械臂进行抓取作业，可以提高机械臂末端的位姿控制精度。