CN113703320A - 一种抗干扰和饱和特性的飞行机械臂位姿控制方法 - Google Patents

Abstract

本发明涉及一种抗干扰和饱和特性的飞行机械臂位姿控制方法，针对环境外力及机械臂运动特性影响飞行机械臂控制精度问题，首先构建含有环境外力及机械臂运动特性的飞行机械臂耦合动力学方程，其次，针对机械臂与无人机间的耦合作用设计对应估计器，再次，针对外力干扰，设计外力干扰观测器；然后，针对耦合作用估计误差和外力观测误差，设计抗饱和控制器进行抑制；最后通过求取抗饱和控制器、耦合估计器与外力观测器控制增益，设计复合分层抗干扰控制器，完成多源干扰下的飞行机械臂抗干扰位姿控制；本方法能够显著提高采用直流电机执行机构的多旋翼飞行机械臂位姿控制精度，可用于飞行机械臂等多旋翼无人系统的高精度位姿控制。

Description

一种抗干扰和饱和特性的飞行机械臂位姿控制方法

技术领域

本发明涉及一种抗干扰和饱和特性的飞行机械臂位姿控制方法，适用于搭载多自由度机械臂且需要实现高精度位姿控制的多旋翼飞行无人系统，属于特种作业无人机控制领域。

背景技术

近年来随着多旋翼无人机的应用不断丰富，任务复杂度日益提升，要求多旋翼无人机搭载功能性更强的多自由度机械臂作为特种任务载荷。此外，随着多旋翼无人机系统的任务和场景逐渐复杂，对机械臂末端执行器位姿控制提出了更高要求，需要结构更轻、更加灵活的机械臂以实现高精度物体抓取；从续航时间和无人机稳定性要求来说，搭载机械臂和末端执行器的质量体积均有严格限制，因此上述机械臂和末端夹爪通常采用轻型高强度的多自由度机械结构设计；同时，机械臂自由度的增加使得无人机与机械臂间的耦合作用变得复杂，还导致整个飞行机械臂系统的质心和惯量发生变化，严重影响飞行机械臂系统位姿控制精度。

此外，针对多源干扰条件下的飞行机械臂抗干扰高精度控制一直是特种作业无人机研制中的关键技术。无刷直流电机是无人机姿态控制系统中最重要的执行部件之一，无人机位置控制的精度决定着机械臂末端的位姿精度。常规的共轴多旋翼无人机为典型的欠驱动系统，其位置控制精度易受到环境外力干扰影响。对于以飞行机械臂为代表的特种作业无人机，一方面，真实环境中存在着阵风、障碍物碰撞、物体抓取时的交互等外界环境外力干扰，这类外力干扰在建模过程中可看作集总干扰予以处理。另一方面，机械臂运动产生的耦合作用通过与无人机的刚性连接影响无人机，该耦合力矩易在机械臂与无人机本体间来回传递，引起无人机姿态控制的偏差。同时，考虑无人机系统的欠驱动特性，姿态误差又会进一步影响无人机位置控制，并导致飞行机械臂末端抓取精度下降，进而导致任务失败。因此，实现飞行机械臂位姿高精度控制，在该类特种作业无人机设计的过程中必须克服上述两类主要干扰的影响。最后，对于环境外力和机械臂耦合作用施加在无人机上的外力认为是无人机受到的集总外力，当集总外力干扰与期望运动的方向不同时，采用不同的抗干扰策略可对外力变化做到快速响应和节省控制能量的效果。

专利申请号为201721229966中提出了一种面向抓取作业型旋翼飞行机械臂系统，但存在两个问题：(1)该文并没有考虑机械臂运动产生的耦合作用对无人机位置控制带来的外力干扰，会对无人机的姿态和位置控制精度造成影响；(2)并没有考虑集总外力干扰与期望运动轨迹的关系，在实际使用会受到一定限制。专利申请号为CN201811366685.6中提出了一种旋翼飞行双机械臂目标定位抓取系统及方法，其中提出的方法同样存在类似问题：(1)未考虑机械臂耦合作用和外界环境对无人机影响；(2)并未充分考虑方法中涉及的针对无人机受到集总外力干扰与期望轨迹的关系，因此在精度和稳定性上会逊色于本文方法。

发明内容

本发明的技术解决问题是：克服现有不足，提供一种抗干扰和饱和特性的飞行机械臂位姿控制方法，利用该方法能提升以旋翼无人机为平台的空中机械臂系统的自身稳定性和环境适应性，能够实现飞行机械臂系统的高精度位姿控制。

本发明的技术方案为：一种抗干扰和饱和特性的飞行机械臂位姿控制方法，其实现步骤如下：

步骤一、构建含有环境外力及机械臂运动特性的飞行机械臂耦合动力学方程，针对机械臂与无人机间的耦合作用设计对应估计器；

针对直流无刷电机中的饱和特性，并同时考虑机械臂耦合作用与外力干扰的动力学方程，建立飞行机械臂模型，表示如下：

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

进一步可以得到：

F_dis＝F_man+F_env

式中F_man表示由机械臂运动产生的耦合作用力，视为施加在无人机上的干扰外力，F_env表示飞行机械臂与外界环境交互产生的外力干扰；r_COM表示在基座为参考原点下系统的质心，m_l为机械臂关节的质量，m_b为无人机的质量，m_s为系统总质量，R_b为四旋翼旋转矩阵，e₃＝[0 0 1]，g是重力加速度，ω_b是无人机角速度；

表示系统的加速度；F_t是多旋翼无人机的总推力；

步骤二、对飞行机械臂中由于外界环境交互带来的干扰，设计环境外力干扰观测器；

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

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

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

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

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

步骤三、针对机械臂与无人机间的耦合作用估计误差和环境外力交互干扰观测误差，设计抗饱和控制器进行抑制；

结合耦合作用估计器，环境外力干扰观测器，并考虑无刷直流电机执行机构饱和特性，进一步设计如下抗饱和控制器：

式中，u(t)＝sat(T_c(t))为抗饱和控制器输出，即考虑了电机饱和特性后的饱和控制输出，K为抗饱和控制器控制增益，电机的最大输出为u_max，且u_max>0，从而得到抗饱和控制器如下：

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

步骤四、通过求取抗饱和控制器、耦合作用估计器与环境外力观测器控制增益，设计复合分层抗干扰控制器，并根据耦合作用和环境外力的矢量方向采用智能匹配、分类补偿的原则设计飞行机械臂位置控制器，完成多源干扰影响下的飞行机械臂抗干扰姿态控制。

最后，根据的F_dis与F_d＝ma_d的夹角θ判断补偿力F_com，其中，m为机器人总质量，a_d表示期望的机器人运动加速度：

本发明与现有技术相比的优点在于：

本发明中涉及的一种抗干扰和饱和特性的飞行机械臂位姿控制方法，主要考虑以多旋翼无人机为载体的空中作业平台，深入分析无人机作业平台受到机械臂运动引起的耦合干扰和环境外力干扰，明确了各干扰源对系统位置姿态控制误差的影响，具有更广泛的使用面。此外针对无人机作业平台使用了干扰观测器，能够快速精确地跟踪集总干扰，同时设计了集总干扰的补偿与期望轨迹关系，增加了飞行机械臂位姿控制系统的快速响应能力，降低了无人机抗集总干扰的能量消耗，显著提高飞行机械臂位姿控制方法的速度、精度与稳定度。

附图说明

图1为本发明一种抗干扰和饱和特性的飞行机械臂位姿控制方法的设计流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅为本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域的普通技术人员在不付出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。

根据本发明的实施例，以一类通用以多旋翼无人机为空中作业平台的特种作业飞行机械臂为例来说明系统和方法的具体实现，飞行机械臂在空中执行物体操作作业时，对位置和姿态控制精度和稳定度具有很高要求；

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

步骤一、构建含有环境外力及机械臂运动特性的飞行机械臂耦合动力学方程，针对机械臂与无人机间的耦合作用设计对应估计器；

针对直流无刷电机中的饱和特性，并同时考虑机械臂耦合作用与外力干扰的动力学方程，建立飞行机械臂模型，表示如下：

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

进一步可以得到：

F_dis＝F_man+F_env

式中F_man表示由机械臂运动产生的耦合作用力，视为施加在无人机上的干扰外力，F_env表示飞行机械臂与外界环境交互产生的外力干扰；r_COM表示在基座为参考原点下系统的质心，m_l为机械臂关节的质量，m_b为无人机的质量，m_s为系统总质量，R_b为四旋翼旋转矩阵，e₃＝[0 0 1]，g是重力加速度，ω_b是无人机角速度；

表示系统的加速度；F_t是多旋翼无人机的总推力；

步骤二、对飞行机械臂中由于外界环境交互带来的干扰，设计环境外力干扰观测器；

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

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

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

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

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

步骤三、针对机械臂与无人机间的耦合作用估计误差和环境外力交互干扰观测误差，设计抗饱和控制器进行抑制；

结合耦合作用估计器，环境外力干扰观测器，并考虑无刷直流电机执行机构饱和特性，进一步设计如下抗饱和控制器：

式中，u(t)＝sat(T_c(t))为抗饱和控制器输出，即考虑了电机饱和特性后的饱和控制输出，K为抗饱和控制器控制增益，电机的最大输出为u_max，且u_max>0，从而得到抗饱和控制器如下：

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

步骤四、通过求取抗饱和控制器、耦合作用估计器与环境外力观测器控制增益，设计复合分层抗干扰控制器，并根据耦合作用和环境外力的矢量方向采用智能匹配、分类补偿的原则设计飞行机械臂位置控制器，完成多源干扰影响下的飞行机械臂抗干扰姿态控制。

根据的F_dis与F_d＝ma_d的夹角θ判断补偿力F_com，其中，m为机器人总质量，a_d表示期望的机器人运动加速度：

此外该控制策略也能够保证飞行机械臂控制系统快速跟踪、保持稳定，实现存在集总干扰情况下对飞行机械臂高精度位姿控制。

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

Claims (5)

1.一种抗干扰和饱和特性的飞行机械臂位姿控制方法，其特征在于，包括以下步骤：

步骤一、构建含有环境外力及机械臂运动特性的飞行机械臂耦合动力学方程，针对机械臂与无人机间的耦合作用设计对应估计器；；

步骤二、对飞行机械臂中由于外界环境交互带来的干扰，设计环境外力干扰观测器；

步骤三、针对机械臂与无人机间的耦合作用估计误差和环境外力交互干扰观测误差，设计抗饱和控制器进行抑制；

步骤四、通过求取抗饱和控制器、耦合作用估计器与环境外力观测器控制增益，设计复合分层抗干扰控制器，并根据耦合作用和环境外力的矢量方向采用智能匹配、分类补偿的原则设计飞行机械臂位置控制器，完成多源干扰影响下的飞行机械臂抗干扰姿态控制。

2.根据权利要求1所述的一种抗干扰和饱和特性的飞行机械臂位姿控制方法，其特征在于，所述步骤一、构建含有环境外力及机械臂运动特性的飞行机械臂耦合动力学方程，具体步骤如下：

针对直流无刷电机中的饱和特性，并同时考虑机械臂耦合作用与外力干扰的动力学方程，建立飞行机械臂模型，表示如下：

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

进一步得到：

F_dis＝F_man+F_env

式中F_man表示由机械臂运动产生的耦合作用力，视为施加在无人机上的干扰外力，F_env表示飞行机械臂与外界环境交互产生的外力干扰；r_COM表示在基座为参考原点下系统的质心，上面两点·表示二阶导数，m_l为机械臂关节的质量，m_b为无人机的质量，m_s为系统总质量，R_b为四旋翼旋转矩阵，e₃＝[0 0 1]，g是重力加速度，ω_b是无人机角速度；

表示系统的加速度；F_t是多旋翼无人机的总推力。

3.根据权利要求1所述的一种抗干扰和饱和特性的飞行机械臂位姿控制方法，其特征在于，所述步骤二、对飞行机械臂中由于外界环境交互带来的干扰，设计环境外力干扰观测器；

对于与外界环境交互而引起的外力干扰由下述模型表征：

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

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

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

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

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

4.根据权利要求1所述的一种抗干扰和饱和特性的飞行机械臂位姿控制方法，其特征在于，所述步骤三、针对机械臂与无人机间的耦合作用估计误差和环境外力交互干扰观测误差，设计抗饱和控制器进行抑制：

结合耦合作用估计器，环境外力干扰观测器，并考虑无刷直流电机执行机构饱和特性，进一步设计如下抗饱和控制器：

式中，u(t)＝sat(T_c(t))为抗饱和控制器输出，即考虑了电机饱和特性后的饱和控制输出，sat函数为饱和函数，K为抗饱和控制器控制增益，电机的最大输出为u_max，且u_max>0，从而得到抗饱和控制器如下：

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

5.根据权利要求1所述的一种抗干扰和饱和特性的飞行机械臂位姿控制方法，其特征在于，步骤四进一步包括：

最后，根据的F_dis与F_d＝m*a_d的夹角θ判断补偿力F_com，其中，m为机器人总质量，a_d表示期望的机器人运动加速度：

Images