CN111923047A - 一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法 - Google Patents

Abstract

本发明涉及一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法，针对质心偏移及基座浮动影响机械臂末端控制精度，首先构建含有质心偏移及基座扰动的飞行机械臂动力学方程；其次，根据质心偏移对带臂旋翼无人机的动力学影响，通过配置多自由度机械臂不同关节间接控制无人机；针对基座浮动引起的扰动，设计基座浮动干扰观测器估计其幅值；针对基座浮动干扰观测误差，设计抗饱和控制器进行抑制；设计抗干扰控制器，完成多源干扰条件下的飞行机械臂抗干扰姿态控制。本发明能够显著提高带臂旋翼无人机姿态控制精度，可应用于安防、工业、军事等领域，满足带臂旋翼无人机在执行物体抓取、运输、危险物排除等特种作业过程中的高精度姿态控制需求。

Description

一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制 方法

技术领域

本发明涉及一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法，适用于重量小于25千克或者任意维度的尺寸不大于10米、搭载多自由度机械臂且需要实现高精度控制的旋翼无人机姿态控制系统，属于飞行器姿态控制领域。

背景技术

近年无人机技术已广泛应用在遥感、侦察、植保、物流、巡检等领域，然而随着任务复杂度不断提升，任务需求难度也越来越广，对无人机的机动性能提出了更高要求，同时无人机需要搭载功能更加多样性的传感器以增强自身感知能力。此外，随着无人机任务作业种类的增加，也对无人机与环境的交互提出了更高要求，需要搭载作业能力更强的执行器以提高无人机与环境的交互能力；传感器和交互作业等需求都使得多旋翼无人机需要搭载越来越灵活的执行部件，从作业需求和控制技术实施难度来说，搭载的执行部件通常为多自由度机械臂，机械臂末端安装有不同类型的末端执行器。配有多自由度机械臂的多旋翼无人机作业时，通常分为抵近飞行、悬停作业、返回降落三个部分，其中，当机械臂关节运动时会造成飞行器在作业周期内的质心偏移，而在悬停作业阶段，由于无人机空间位置的浮动会造成机械臂基座的浮动，对末端执行器的精度造成影响，甚至影响作业过程。

此外，旋翼无人机姿态控制系统的稳定性和性能一直是飞行器姿态控制中的关键技术。旋翼无人机通过电机接收电调的电压信号，驱动螺旋桨提供升力和反桨扭矩，因而对无刷电机的控制精度直接决定了旋翼无人机飞行品质。但是电机由于加工精度和物理属性的限制，其转速的大小收到了严格限制，而且越靠近极限转速其非线性属性越强。另一方面，由于无人机控制精度的问题，无人机的不稳定飞行会造成挂载机械臂基座的空间位置出现一定程度变化，表现为机械臂基座的抖动，这种抖动会通过多自由度机械臂的关节进行传递，影响机械臂末端执行器的位置精度，甚至导致作业失败。因此，为了更精确的完成飞行器姿态控制，在飞行器控制算法设计的过程中必须克服上述两类主要干扰的影响。

专利申请号为CN201720515134.6中提出了一种带有柔性抓取器的飞行机械臂，借助多旋翼飞行器快速移动的特点，提高柔性抓取器抓取任务的成功率，但存在两个问题：(1)该专利并未考虑多自由度机械臂运动时造成的质心偏移干扰，会对飞行器的动力学稳定性和精度造成影响；(2)并未考虑基座浮动对抓取器的误差传递，在实际使用会受到极大限制。专利申请号为CN201721229966.8中提出了一种面向抓取作业型旋翼飞行机械臂系统，也存在类似问题：(1)并没有考虑质心偏移形成的干扰；(2)方法中没有考虑基座浮动对末端执行器造成的影响，并不能保证作业精度，因此在精度和快速性等指标上会弱于本专利所提出的方法。

发明内容

本发明的技术解决问题是：克服现有不足，提供一种针对质心偏移及基座浮动的飞行机械臂姿态控制方法，利用该方法能够为采用无刷电机作为主要执行机构飞行机械臂系统提供抗输出饱和与抗干扰能力，通过对外部多源干扰的估计与补偿，完成对机械臂末端执行器的高精度姿态控制。

本发明的技术解决方案为：一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法，其实现步骤如下：

第一步，将质心偏移和基座浮动分别看作外部干扰，其中质心偏移看作是干扰力和干扰力矩，基座浮动则建模成作用在机械臂端的集总干扰力矩，并建立含有质心偏移干扰带臂旋翼无人机耦合动力学方程。针对多旋翼飞行机械臂常见的质心偏移及基座浮动特性，同时考虑多旋翼无人机和多自由度机械臂的动力学方程，建立带臂旋翼无人机的动力学模型，表示如下：

式中，v_b表示作为机械臂基座的旋翼无人机的平动速度，m_b和m_l分别为多旋翼无人机和机械臂的质量；F_t为多旋翼无人机的总推力，R_b为多旋翼无人机体坐标系到惯性坐标系的转换矩阵；e₃＝[0 0 1]^T，g为重力加速度，F_dis表示质心偏移导致的对基座的干扰力，ω_b为多旋翼无人机的角速度，I_b为多旋翼无人机的转动惯量，τ为多旋翼产生的力矩，τ_dis为质心偏移对基座产生的干扰力矩；

分析质心偏移，当机械臂的关节运动时，整个飞行系统的质心会发生变化，以配有二自由度机械臂的四旋翼无人机为例，如图1所示；

则质心可用如下公式表示：

r_COM(q)为整个系统的质心，r_com(q)表示在基座为参考原点下每一个关节的质心，

为每个关节的质量；

同理可得质心偏移速度：

表示第i个关节的质心在基座参考系下的线速度；

进一步可得：

解算此微分方程，令

为零表示机械臂关节近似为匀速运动，求得关节角配置q，在姿态稳定的情况下：

通过主动改变机械臂的关节配置可间接控制基座的平动位移，并设计相应的PID控制器。建立针对质心偏移与机械臂关节配置的关系，利用多自由度机械臂的不同关节配置主动控制质心偏移量从而间接控制基座，其设计的PID控制器为：

E_pos＝P_desired-P_current

其中，K_p，K_d，K_i分别为比例增益、微分增益、积分增益；T_c为PID控制器输出，E_pos为位置控制偏差，

是控制偏差变化率，P_desired，P_current分别为期望位置和当前位置。

进一步改写得到系统模型：

干扰模型如下：

联立系统模型和干扰模型可得扩维系统：

ω是构造的状态辅助变量，τ_dis通过干扰观测器估计出来并在前馈通道中进行抑制，相应的干扰观测器形式如下：

进一步有观测器

其中，z是中间变量，W和V是干扰模型的参数矩阵，x是扩维系统后的状态变量，L待求干扰观测器的增益,A、B是系统状态方程的特征矩阵，v(t)是基座浮动观测器的一个辅助状态变量，

是辅助状态变量的变化率，

是外界干扰对系统状态的影响，

是基座浮动针对集总干扰d_s的估计值，t表示时间，x表示系统的状态，u表示控制输入。

第二步，对于基座浮动引起的机械臂底座变化归总为机械臂的集总干扰力矩，可通过下式估算：

其中v(t)是基座浮动观测器的一个辅助状态变量，

是基座浮动针对集总干扰d_s(t)的估计值。

第三步，结合干扰力和干扰力矩估计器，基座浮动观测器，并考虑旋翼执行机构饱和特性，设计如下抗饱和控制器：

式中，u(t)＝sat(F_t(t))为抗饱和控制器输出，即考虑了旋翼无人机电机饱和特性后的饱和控制力矩，K为抗饱和控制器控制增益，由于考虑了电机饱和，设置电机的最大输出力矩为u_max，且u_max＞0，从而：

式中的u(t)为电机收到的控制升力指令，不会超过电机转速的最大值，F_t(t)为多旋翼的总升力。此外也能够保证飞行器系统稳定，实现存在饱和以及干扰情况下带多自由度机械臂多旋翼无人机高精度姿态控制。

本发明与现有技术相比的优点在于：本发明中涉及的一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法，主要面向配有多自由度机械臂的旋翼无人机姿态控制系统，同时考虑质心偏移及基座浮动，使得对于飞行系统稳定性和作业误差分析的更加全面，具有更广泛的工程适用性。此外针对质心引起的干扰力矩设计了快速收敛的干扰观测器，可快速精确地跟踪质心偏移干扰，增加了飞行机械臂控制系统的稳定性和快速响应能力，显著提高飞行机械臂的姿态控制方法的响应时间、精度与稳定度。

附图说明

图1为本发明针对质心随着机械臂关节转动发生偏转的示意图；

图2为本发明一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法流程图。

具体实施方式

以一类通用带多自由度机械臂的旋翼无人机飞行系统为例来说明系统和方法的具体实现，飞行机械臂在悬停工作阶段，对姿态控制精度和稳定度具有很高要求；

如图2所示，本发明具体实施步骤如下：

1.针对旋翼飞行机械臂常见的质心偏移及基座浮动特性，同时考虑旋翼无人机和多自由度机械臂的动力学方程，建立旋翼飞行机械臂的动力学模型，表示如下：

式中，v_b表示作为机械臂基座的旋翼无人机的平动速度，m_b和m_l分别为多旋翼无人机和机械臂的质量；F_t为多旋翼无人机的总推力，R_b为多旋翼无人机体坐标系到惯性坐标系的转换矩阵；e₃＝[0 0 1]^T，g为重力加速度，F_dis表示质心偏移导致的对基座的干扰力，ω_b为多旋翼无人机的角速度，I_b为多旋翼无人机的转动惯量，τ为多旋翼产生的力矩，τ_dis为质心偏移对基座产生的干扰力矩；

如图1所示，对飞行机械臂进行受力分析，其中COM表示系统的质心，x，y，z分别是机体坐标系的方向，F_{1，2，3，4}为每个螺旋桨提供的升力，Theta为四旋翼桨面与水平面的夹角大小，mg表示系统的重力，f_{1，2，3，4}表示每个螺旋桨升力在重力轴上的分量，R₁，R₃分别表示旋翼无人机的一号和三号电机位置。

分析质心偏移，当机械臂的关节运动时，整个飞行系统的质心会发生变化，则质心可用如下公式表示：

r_COM(q)为整个系统的质心，r_com(q)表示在基座为参考原点下每一个关节的质心，m_li为每个关节的质量；

同理可得质心偏移速度：

(q)表示第i个关节的质心在基座参考系下的线速度；

进一步可得：

解算此微分方程，令

为零表示机械臂关节近似为匀速运动，解得关节角配置q，那么在姿态稳定的情况下：

通过主动改变机械臂的关节配置可间接控制基座的平动位移；

进一步改写得到系统模型：

干扰模型：

联立系统模型和干扰可得扩维系统：

τ_dis通过干扰观测器估计并在前馈通道中进行抑制，相应的干扰观测器形式如下：

进一步有观测器：

其中，z是中间变量，W和V是干扰模型的参数矩阵，x是扩维系统后的状态变量，L是待求干扰观测器的增益，A、B是系统状态方程的特征矩阵。

2.对于基座浮动引起的机械臂底座变化归总为机械臂的集总干扰力矩，可通过下式估算：

其中v(t)是基座浮动观测器的一个辅助状态变量，

是基座浮动针对集总干扰d_s(t)的估计值。

3.结合干扰力和干扰力矩估计器，基座浮动观测器，并考虑旋翼执行机构饱和特性，进一步设计如下抗饱和控制器：

式中，u(t)&＝sat(F_t(t))为抗饱和控制器输出，即考虑了旋翼无人机电机饱和特性后的饱和控制力矩，K为抗饱和控制器控制增益，由于考虑了电机饱和，设置电机的最大输出力矩为u_max，且u_max＞0，从而：

式中的u(t)为电机收到的控制升力指令，不会超过其转速的最大值，此外也能够保证飞行器系统稳定，实现存在饱和以及干扰情况下带多自由度机械臂旋翼无人机高精度姿态控制。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (5)

1.一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法，其特征在于，包括以下步骤：

第一步，将质心偏移和基座浮动分别看作外部集总干扰，将旋翼无人机看作机械臂的可浮动基座，其中由机械臂关节运动引起的质心偏移看作是可建模的干扰力和干扰力矩，而基座浮动则视为对机械臂的干扰，建立含有质心偏移干扰带臂旋翼无人机动力学方程；

第二步，针对可建模的质心偏移所引起的干扰，设计质心偏移PID控制器，利用多自由度机械臂的不同关节配置主动改变质心偏移量从而间接控制基座移动，再次，针对基座浮动对机械臂带来的干扰，设计基座浮动干扰观测器进行量测；

第三步，针对质心偏移和基座浮动的干扰观测误差，设计抗饱和控制器进行质心偏移控制和抑制基座浮动对机械臂的干扰；

第四步，最后基于第二步中的质心偏移控制器、第二步中的基座浮动观测器与第三步中的抗饱和控制器的控制增益，设计复合抗干扰控制器，完成多源干扰条件下的带臂旋翼无人机抗干扰姿态控制系统设计。

2.根据权利要求1所述的针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法，其特征在：所述第一步中，建立含有质心偏移干扰带臂旋翼无人机耦合动力学方程如下：

式中，v_b表示作为机械臂基座的旋翼无人机的平动速度，m_b和m_l分别为多旋翼无人机和机械臂；F_t为多旋翼无人机的总推力，R_b为多旋翼无人机体坐标系到惯性坐标系的转换矩阵；e₃＝[0 0 1]^T，g为重力加速度，F_dis表示质心偏移导致的对基座的干扰力，ω_b为多旋翼无人机的角速度，I_b为多旋翼无人机的转动惯量，τ为多旋翼产生的力矩，τ_dis为质心偏移对基座产生的干扰力矩。

3.根据权利要求1所述的针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法，其特征在：所述第二步中，质心偏移PID控制器为：

E_pos＝P_desired-P_current

其中，K_p，K_d，K_i分别为比例增益、微分增益、积分增益；T_c为PID控制器输出，E_pos为位置控制偏差，

是控制偏差变化率，P_desired，P_current分别为期望位置和当前位置；

设计的基座浮动干扰观测器具体实现如下：

其中v(t)是基座浮动观测器的一个辅助状态变量，

是辅助状态变量的变化率，

是外界干扰对系统状态的影响，

是基座浮动针对集总干扰d_s(t)的估计值，L是待求干扰观测器的增益，A、B是系统状态方程的特征矩阵，t表示时间，x(t)表示系统的状态，u(t)表示控制输入，W和V是干扰模型的参数矩阵。

4.根据权利要求1所述的针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法，其特征在：所述第三步，针对质心偏移估计和基座浮动干扰观测误差，设计抗饱和控制器进行主动偏移控制和抑制基座浮动对机械臂的干扰具体实现如下：

电机的最大输出力矩为u_max，且u_max＞0，从而得到抗饱和控制器如下：

式中的u(t)为电机收到的控制升力指令，不会超过电机转速的最大值，F_t(t)为多旋翼的总升力。

5.根据权利要求1所述的针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法，其特征在：所述第四步，复合抗干扰控制如下：

r_COM表示在基座为参考原点下系统的质心，m_l为机械臂关节的质量，m_b为无人机的质量，m_s为系统总质量，R_b为四旋翼旋转矩阵，e₃＝[0 0 1]，g是重力加速度，ω_b是无人机角速度；

表示系统的加速度；F_t是多旋翼无人机的总推力；F_dis表示质心偏移导致的对基座的干扰力；u_c(t)是抗饱和控制器的输出，F_dis(t)是可控扰动，

为干扰估计值，α为待优化的超参数，u(t)是无人机干扰观测器、可控扰动与抗饱和控制器复合后的控制输入。

Images