CN113618743B - 一种针对多源干扰的无人机机械臂末端位姿控制方法 - Google Patents

Abstract

本发明涉及一种针对多源干扰的无人机机械臂末端位姿控制方法，目标是实现特种无人机系统在多源干扰的情况下末端高精度控制。首先，建立特种无人机机械臂末端位姿控制系统多源干扰模型，其次针对系统标称模型设计鲁棒H∞控制器；然后针对多源干扰，设计干扰观测器，对外部干扰和模型不确定性进行估计和补偿；最后，通过合理的选取鲁棒H∞控制器和干扰观测器的参数，保证了机械臂末端位姿的高精度控制。本发明基于复合分层抗干扰控制架构，实现了特种无人机系统在多源干扰下机械臂末端的高精度位姿控制，适用于应急救援、狭小空间内非合作目标捕获等特种任务。

Description

一种针对多源干扰的无人机机械臂末端位姿控制方法

技术领域

本发明涉及航空技术领域，尤其是一种针对多源干扰的无人机机械臂末端控制方法。对一类受到多源干扰的特种无人机机械臂末端位姿控制系统，通过设计鲁棒H∞控制器和干扰观测器，实现了对多源干扰的实时估计补偿和抑制，保证了特种无人机机械臂控制系统的自主抗干扰能力。与传统的机械臂抗干扰控制方法相比，本发明方法增加了复合控制器的可裁剪性，保证了特种无人机系统在多源干扰下机械臂末端的高精度控制，可适用于应急救援、狭小空间内非合作动态目标捕获等特种任务。

背景技术

特种无人机机械臂系统是一个由空中基座和机械臂组合而成的，具备高机动、高自主、高灵活性的机器人系统，如图3所示，其高精度的末端位姿控制是完成空中投递物资、监测有害环境、应急救援等复杂任务的核心技术。与地面机器人不同，执行抓取作业的特种机器人系统没有固定基座且由于动力学复杂、系统耦合和外界干扰等原因，对特种无人机的机械臂末端位姿控制系统的自主抗干扰能力带来了非常严峻的挑战。

特种无人机机械臂控制系统所受多源干扰大致可分为三类：第一类是动力学模型不确定性干扰，特种无人机是一个高度复杂的耦合系统，无论是独立建模还是联合建模都存在一定的动力学模型参数不确定性；第二类是机械臂在夹取物体时，物体载荷带来的外部干扰；第三类是无人机抖动带来的干扰。其中，模型不确定性干扰和载荷所导致的干扰主要存在机械臂系统的动力学模型中，是影响角空间内机械臂控制精度的主要干扰。在面对多源干扰同时存在的情况下，传统的控制方法一般会将其视为集总干扰进行处理，但保守性强且控制精度较低，无法满足实际需求。这些方法都由于未能充分利用干扰的动态特性，因而精度有限，严重时会导致系统振荡甚至发散，为进一步提升在多源干扰下机械臂末端位姿控制系统的精度。近年来，相关学者提出了复合分层抗干扰控制结构，其主要思想是利用不同干扰的动态特性建立其动力学模型，设计鲁棒控制器和干扰观测器相结合的复合控制器，实现基于干扰在线估计和补偿的抗干扰控制方法。其中，干扰观测器对外界干扰进行实时的估计和补偿，同时鲁棒H∞控制器则对观测误差和模型不确定性干扰进行抑制用于保证闭环控制系统的稳定性和鲁棒性。复合分层抗干扰控制方法可实现多源干扰的同时抑制与补偿，可有效地提升了特种无人机机械臂控制系统的抗干扰能力。

现有的特种无人机机械臂抗干扰控制方法多用基础控制器，如PID控制器。但在多源干扰同时存在的情况下，该控制方法保守性强且控制精度低。

专利申请号为201910916380.6中提出了一种基于解耦控制的飞行机械臂系统，该方法是利用迭代动力学方程计算机械臂的力矩，然后通过对机械臂的力矩进行补偿以提高系统末端控制精度。但该方法存在两个问题：①当机械臂自由度较高时会存在计算量过大甚至无法进行求解的情况；②该方法并没有考虑干扰存在情况下的动力学计算，在实际使用会受到一定限制。当实际系统受到一定干扰后，机械臂系统的控制精度会大幅降低。专利申请号为201910532949.9中提出了一种飞行机械臂控制系统，但也存在类似问题：①机械臂自由度过低，仅为二自由度，大大限制了实际使用；②该方法中仅使用了简单的PID控制，并不能保证系统在存在多源干扰的情况下保证系统收敛，因此在精度和稳定性差。

发明内容

本发明技术解决的问题是：针对特种无人机机械臂控制系统在受到动力学模型不确定性干扰和外部干扰影响时机械臂末端控制精度和鲁棒性下降，以及现有抗干扰姿态控制系统设计方法的有效性较低的问题，为解决上述技术问题，本发明提出了针对多源干扰的特种无人机机械臂末端位姿控制系统设计方法，这一设计方法的主要思想是将鲁棒H∞控制器和干扰观测器构建复合抗干扰控制器，其中干扰观测器对主要干扰进行实时的估计和补偿，鲁棒H∞控制器对其余干扰进行抑制，共同实现机械臂控制系统在模型不确定性干扰和外部干扰同时存在情况下的末端位姿高精度控制，提供一种针对多源干扰的特种无人机机械臂末端位姿控制系统设计方法以克服现有控制设计方法的局限性。其主要思路是将干扰观测器和鲁棒H∞控制器构建复合分层抗干扰控制器，通过选取反馈增益和观测器增益使得系统的性能得到预设的指标要求。这一设计方法的特点在于：一方面干扰观测器设计时充分利用了干扰的动力学特性，对干扰进行了实时的估计和补偿；另一方面，鲁棒H∞控制器根据反馈信息可以对干扰观测器的观测误差和动力学模型不确定性干扰进行抑制，保证了闭环控制系统的干扰抑制效果，实现了特种无人机机械臂末端位姿控制系统在多源干扰情况下仍能保持末端高精度跟踪轨迹。

本发明的技术解决方案为：一种针对多源干扰的无人机机械臂末端位姿控制方法，其特征在于：控制器具有复合结构，首先构建基于机械臂标称模型的鲁棒H∞控制器，然后根据机械臂所受到的载荷干扰建立干扰观测器，最后在机械臂的任务空间中针对空中基座抖动干扰建立干扰观测器对抖动干扰进行实时估计和补偿。其中，基于机械臂标称模型建立的鲁棒H∞控制器用以保证机械臂末端位姿控制系统在无外界干扰情况下的精确性和稳定性；基于特种无人机机械臂系统多源干扰模型建立的干扰观测器则利用了多源干扰的部分动态特性信息，对机械臂系统所受到的外部干扰和模型不确定性干扰进行实时的估计和补偿，保证系统在多源干扰作用下末端位姿的精确性。上述各单元的具体设计步骤如下：

第一步，建立含有基座抖动和模型不确定性的机械臂耦合动力学方程。

针对特种无人机机械臂系统的模型不确定性，并同时考虑基座抖动等外部干扰建立整体的机械臂系统动力学模型，表示如下：

其中，状态向量q，

分别表示机械臂各个关节的转动角度，转动角速度和转动角加速度，τ代表系统的控制输入力矩，H₀是系统正定的惯性矩阵标称部分，

为包含了科氏力和向心力矩阵的标称部分，G₀(q)表示重力矩阵的标称部分，ΔH(q)，

ΔG(q)分别表示系统惯性矩阵，科氏力矩阵和重力矩阵的不确定部分，在实际的抓取作业中，未知目标的质量体积等参数都不会超出系统的最大载荷，因此模型不确定性干扰d₁为有界干扰：d₀表示系统所受到的载荷干扰，该外界载荷干扰的动力学模型可表示为：

其中，W为干扰子系统的状态变量；σ表示噪声；

为干扰子系统的状态矩阵，m为干扰频率；N为干扰子系统的输出矩阵。

第二步，根据机械臂系统模型的标称部分，设计鲁棒H∞控制器，保证特种无人机机械臂末端在无外界干扰的情况下跟踪轨迹的精确性和稳定性。控制律具体的表达式为：

其中，

表示系统的误差状态向量，e_q＝q-q_d表示机械臂系统关节角的轨迹跟踪误差，K为待求的反馈增益；联立机械臂系统动力学模型的公式(1)，得关节角误差动力学方程为：

令

方程可重写成状态空间形式为：

状态矩阵

I表示单位矩阵。

定义机械臂系统的性能输出为Z＝Cx为选取的性能矩阵，其中x为定义的系统状态向量，当选取的参数r，P₁和

满足下列不等式时，就可以通过选取反馈增益

保证机械臂系统满足给定的性能指标

sym(M)＝M+M^T。

第三步，根据载荷振动干扰模型和系统的性能指标，设计干扰观测器对外界干扰进行实时估计与补偿。首先，写出状态与干扰的增广系统模型，给出干扰观测器结构；然后，结合第二步中的鲁棒H∞控制器参数，选取干扰观测器的参数。

进一步的，所述的第三步动力学干扰观测器，将干扰观测器的观测误差视为一个新的状态变量，与关节角跟踪误差组成一个新的增广系统；针对干扰的特性，设计如下干扰观测器：

其中，

为估计的外界干扰，

为估计的干扰子系统的状态变量，x为上文定义的误差状态向量，

为辅助变量，L为待设计的观测器增益；干扰观测器得到干扰估计值

后，与鲁棒H∞控制器输出的标称控制量u复合，此时特种无人机机械臂系统的控制输入为：

结合动力学模型，则复合分层抗干扰控制器的误差动力学方程为：

通过给定的参数变量γ₁，γ₂，γ₃，γ₄和正定矩阵P，P₂和参数矩阵K_P和R，满足下列不等式(10)和(11)；则可以通过选取反馈增益K和干扰观测器增益L，保证机械臂系统在载荷干扰和模型不确定性干扰的情况下高精度地跟踪上轨迹高效地执行任务：

第四步，机械臂和无人机系统耦合，无人机的基座抖动干扰被合理的等效为机械臂的期望轨迹受到了干扰影响而发生了偏差，针对这一基座抖动干扰，设计干扰观测器对其进行实时的估计和补偿，以保证机械臂末端高精度跟踪轨迹，具体如下：

令

作为机械臂系统在任务空间中的状态向量，其中P_x,P_y,P_z代表末端在x轴,y轴和z轴上的位置，考虑整个特种无人机机械臂系统从轨迹规划到跟踪轨迹的动力学模型：

其中，d₂代表机械臂系统受到的基座抖动干扰，B_u为系统的输入参数矩阵，C代表系统的输出参数矩阵。这部分基座抖动干扰被合理等效到机械臂的轨迹规划中，d₃表示角空间复合控制器的跟踪误差，针对基座抖动干扰d₂，设计类似的干扰观测器对其进行实时的估计进行补偿，求解步骤与第三步中的方法相同。

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

1.本发明提出了一种针对多源干扰的特种无人机机械臂末端位姿控制系统的设计方法，该控制器具有复合结构，角空间内的标称鲁棒H∞控制器用于保证机械臂系统的跟踪性能，干扰观测器则用于对外界干扰进行实时估计和补偿，保证了闭环系统的鲁棒性能，提升了机械臂系统对多源干扰的可补偿能力。针对空中基座的抖动干扰，在机械臂的任务空间中设立了一个干扰观测器用来对对机械臂末端的轨迹进行实时的纠正和补偿，提高了特种无人机机械臂末端的跟踪精度。

2.本发明不仅能够实现特种无人机机械臂控制系统在模型不确定性，载荷振动干扰和无人机基座抖动等多源干扰同时存在情况保持末端高精度跟踪，鲁棒H∞控制器和干扰观测器设计与参数选取也十分简单，增加了特种无人机机械臂抗干扰控制系统控制器设计的灵活性。

3.本发明同时也考虑了由于系统耦合、机械臂运动带来的无人机基座抖动干扰影响，在机械臂的任务空间中建立干扰观测器对抖动干扰进行实时估计和补偿从而提高机械臂末端对参考轨迹的跟踪精度，进一步提升特种无人机机械臂系统在多源干扰作用下的跟踪精度。

附图说明

图1为所发明的针对多源干扰的特种无人机机械臂控制系统设计方法的实施流程图；

图2为所发明的针对多源干扰的特种无人机机械臂控制系统结构框图；

图3为所发明的特种无人机机械臂系统结构图。

具体实施方式

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

如图1所示，本发明提供了一种针对多源干扰的特种无人机机械臂控制方法，可实现模型不确定性干扰，载荷干扰和空中基座抖动干扰同时存在时特种无人机机械臂系统的自主抗干扰。所设计的抗干扰控制系统具有复合结构，由鲁棒H∞控制器和干扰观测器复合而成。本发明的具体设计和实施流程如下：

第一步，建立含有基座抖动和模型不确定性的机械臂耦合动力学方程。针对特种无人机机械臂系统的模型不确定性，并同时考虑基座抖动等外部干扰建立整体的机械臂系统动力学模型，表示如下：

其中，状态向量q，

分别表示机械臂各个关节的转动角度，转动角速度和转动角加速度，τ代表系统的控制输入力矩，H₀是系统正定的惯性矩阵标称部分，

为包含了科氏力和向心力矩阵的标称部分，G₀(q)表示重力矩阵的标称部分，ΔH(q)，

ΔG(q)分别表示系统惯性矩阵，科氏力矩阵和重力矩阵的不确定部分，在实际的抓取作业中，未知目标的质量体积等参数都不会超出系统的最大载荷，因此模型不确定性干扰d₁为有界干扰；；d₀表示系统所受到的载荷干扰，该外界载荷干扰的动力学模型可表示为：

其中，W为干扰子系统的状态变量；σ表示噪声；

为干扰子系统的状态矩阵，m为干扰频率；N为干扰子系统的输出矩阵。

第二步，根据机械臂系统模型的标称部分，设计鲁棒H∞控制器，保证特种无人机机械臂末端在无外界干扰的情况下跟踪轨迹的精确性和稳定性；控制律具体的表达式为：

其中，

表示系统的误差状态向量，e_q＝q-q_d表示机械臂系统关节角的轨迹跟踪误差，K为待求的反馈增益。联立机械臂系统动力学模型的公式(1)，得关节角误差动力学方程为：

令

方程可重写成状态空间形式为：

状态矩阵

I表示单位矩阵。

定义机械臂系统的性能输出为Z＝Cx为选取的性能矩阵，其中x为定义的系统状态向量，当选取的参数r，P₁和

满足下列不等式时，就可以通过选取反馈增益

保证机械臂系统满足给定的性能指标

sym(M)＝M+M^T。

第三步，根据载荷振动干扰模型和系统的性能指标，设计干扰观测器对外界干扰进行实时估计与补偿，首先，写出状态与干扰的增广系统模型，给出干扰观测器结构；然后，结合第二步中的鲁棒H∞控制器参数，选取干扰观测器的参数；

所述的第三步动力学干扰观测器，将干扰观测器的观测误差视为一个新的状态变量，与关节角跟踪误差组成一个新的增广系统；针对干扰的特性，设计如下干扰观测器：

其中，

为估计的外界干扰，

为估计的干扰子系统的状态变量，x为上文定义的误差状态向量，

为辅助变量，L为待设计的观测器增益；干扰观测器得到干扰估计值

后，与鲁棒H∞控制器输出的标称控制量u复合，此时特种无人机机械臂系统的控制输入为：

结合动力学模型，则复合分层抗干扰控制器的误差动力学方程为：

通过给定的参数变量γ₁，γ₂，γ₃，γ₄和正定矩阵P，P₂和参数矩阵K_P和R，满足下列不等式(10)和(11)；则可以通过选取反馈增益K和干扰观测器增益L，保证机械臂系统在载荷干扰和模型不确定性干扰的情况下高精度地跟踪上轨迹高效地执行任务：

第四步，机械臂和无人机系统耦合，无人机的基座抖动干扰被合理的等效为机械臂的期望轨迹受到了干扰影响而发生了偏差，针对这一基座抖动干扰，设计干扰观测器对其进行实时的估计和补偿，以保证机械臂末端高精度跟踪轨迹，具体如下：

令

作为机械臂系统在任务空间中的状态向量，其中P_x,P_y,P_z代表末端在x轴,y轴和z轴上的位置，考虑整个特种无人机机械臂系统从轨迹规划到跟踪轨迹的动力学模型：

其中，d₂代表机械臂系统受到的基座抖动干扰，B_u为系统的输入参数矩阵，C代表系统的输出参数矩阵。这部分基座抖动干扰被合理等效到机械臂的轨迹规划中，d₃表示角空间复合控制器的跟踪误差，针对基座抖动干扰d₂，设计类似的干扰观测器对其进行实时的估计进行补偿，求解步骤与第三步中的方法相同。整体控制框架如图2所示。

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

Claims (2)

1.一种针对多源干扰的无人机机械臂末端位姿控制方法，其特征在于，步骤如下：

第一步，建立含有基座抖动和模型不确定性的机械臂耦合动力学方程；

第二步，根据机械臂系统动力学模型的标称部分，设计鲁棒H∞控制器，保证无人机机械臂末端在无外界干扰的情况下跟踪轨迹的精确性和稳定性；

第三步，根据载荷振动干扰模型和系统的性能指标，设计干扰观测器对外界干扰进行实时估计与补偿；

第四步，机械臂和无人机系统耦合无人机的基座抖动干扰被合理的等效为机械臂的期望轨迹受到了干扰影响而发生了偏差，针对这一基座抖动干扰，设计干扰观测器对其进行实时的估计和补偿，以保证机械臂末端高精度跟踪轨迹；

所述第一步，建立含有基座抖动和模型不确定性的机械臂耦合动力学方程，具体如下：

针对特种无人机机械臂系统的模型不确定性，并同时考虑基座抖动的外部干扰建立整体的机械臂系统动力学模型，表示如下：

其中，状态向量q,

分别表示机械臂各个关节的转动角度，转动角速度和转动角加速度，τ代表系统的控制输入力矩，H₀是系统正定的惯性矩阵标称部分，

为包含了科氏力和向心力矩阵的标称部分，G₀(q)表示重力矩阵的标称部分，ΔH(q),

ΔG(q)分别表示系统惯性矩阵，科氏力矩阵和重力矩阵的不确定部分，在实际的抓取作业中，未知目标的质量体积等参数都不会超出系统的最大载荷，因此模型不确定性干扰d₁为有界干扰；d₀表示系统所受到的载荷干扰，该载荷干扰的动力学模型表示为：

其中，W为干扰子系统的状态变量；σ表示噪声；

为干扰子系统的状态矩阵，m为干扰频率；N为干扰子系统的输出矩阵；H为噪声信号σ对干扰子系统W的输入矩阵；

所述第二步，根据机械臂系统动力学模型的标称部分，设计鲁棒H∞控制器，保证特种无人机机械臂末端在无外界干扰的情况下跟踪轨迹的精确性和稳定性；控制律具体的表达式为：

其中，

表示系统的误差状态向量，e_q＝q-q_d表示机械臂系统关节角的轨迹跟踪误差，q_d为机械臂各个关节期望的转动角度；K为待求的反馈增益；联立机械臂系统动力学模型的公式(1)，得关节角误差动力学方程为：

令

方程可重写成状态空间形式为：

状态矩阵

I表示单位矩阵；

定义机械臂系统的性能输出为Z＝Cx为选取的性能矩阵，其中x为系统的误差状态向量，C代表选取的性能参数矩阵，可以从状态向量中选取所需的状态；当选取的参数r,P₁和

满足下列不等式时，就可以通过选取反馈增益

保证机械臂系统满足给定的性能指标

sym(M)＝M+M^T；

所述第三步，根据载荷振动干扰模型和系统的性能指标，设计干扰观测器对外界干扰进行实时估计与补偿，首先，写出状态与干扰的增广系统模型，给出干扰观测器结构；然后，结合第二步中的鲁棒H∞控制器参数，选取干扰观测器的参数；具体的，所述的第三步动力学干扰观测器，将干扰观测器的观测误差视为一个新的状态变量，与关节角跟踪误差组成一个新的增广系统；针对干扰的特性，设计如下干扰观测器：

其中，

为估计的外界干扰，

为估计的干扰子系统的状态变量，x为系统的误差状态向量，

为辅助变量，L为待设计的观测器增益；干扰观测器得到干扰估计值

后，与鲁棒H∞控制器输出的标称控制量u复合，此时特种无人机机械臂系统的控制输入力矩为：

结合动力学模型，则复合分层抗干扰控制器的误差动力学方程为：

其中，

代表干扰观测器对载荷干扰子系统W的估计误差；

通过给定的参数变量γ₁,γ₂,γ₃,γ₄和正定矩阵P,P₂和参数矩阵K_P和R，满足下列不等式(10)和(11)；则可以通过选取反馈增益K和干扰观测器增益L，保证机械臂系统在载荷干扰和模型不确定性干扰的情况下高精度地跟踪上轨迹高效地执行任务：

2.根据权利要求1所述的一种针对多源干扰的无人机机械臂末端位姿控制方法，其特征在于，所述第四步，机械臂和无人机系统耦合无人机的基座抖动干扰被合理的等效为机械臂的期望轨迹受到了干扰影响而发生了偏差，针对这一基座抖动干扰，设计干扰观测器对其进行实时的估计和补偿，以保证机械臂末端高精度跟踪轨迹，具体如下：

令

作为机械臂系统在任务空间中的状态向量，其中P_x,P_y,P_z代表末端在x轴,y轴和z轴上的位置，考虑整个无人机机械臂系统从轨迹规划到跟踪轨迹的动力学模型：

其中，d₂代表机械臂系统受到的基座抖动干扰，B_u为系统的输入参数矩阵；这部分基座抖动干扰被合理等效到机械臂的轨迹规划中，d₃表示角空间复合控制器的跟踪误差，针对基座抖动干扰d₂，设计类似的干扰观测器对其进行实时的估计进行补偿，求解步骤与第三步中的方法相同。

Images