CN115556111A - 基于变惯性参数建模的飞行机械臂耦合扰动控制方法 - Google Patents

Abstract

一种基于变惯性参数建模的飞行机械臂耦合扰动控制方法，属于空中作业无人***控制技术领域。本发明针对现有旋翼飞行机械臂抗扰动控制中的反馈补偿存在延时，影响***稳定性的问题。包括：建立动态变化质心在机体坐标系下的表达式和动态变化惯量在机体坐标系下的表达式；利用多刚体质点系的动量与动量矩定理对旋翼飞行机械臂***进行动力学建模，得到***动力学模型；基于质心变化与惯量变化对***动力学模型进行处理，建立质心变化和惯量变化对旋翼飞行机械臂***的耦合扰动的映射关系，得到基于变惯性参数的耦合扰动模型；将耦合扰动模型计算获得的耦合扰动估计值作为前馈补偿量输入位置环和姿态环实现耦合扰动控制。本发明可以抑制耦合扰动。

Description

基于变惯性参数建模的飞行机械臂耦合扰动控制方法

技术领域

本发明涉及基于变惯性参数建模的飞行机械臂耦合扰动控制方法，属于空中作业无人***控制技术领域。

背景技术

近年来，旋翼飞行机械臂作为一种新型的空中机器人***，因其在三维空间灵活的机动性和在其工作空间强大的操纵性引起了研究人员较为广泛的研究和注意。旋翼飞行机械臂可以将机械臂的操纵能力有效拓展至三维空间，同时具备无人机的灵活机动性，使得一些高空特种无人作业任务成为可能，具有非常有价值的应用潜力和广阔的应用前景。

耦合扰动问题是摆在旋翼飞行机械臂稳定精确控制面前最大的挑战之一，其主要是因为无人机与机械臂之间存在相对运动导致***质心和惯量发生快速变化而产生的，特别是在一些需要无人机或机械臂在大范围快速运动的实际动态场景中，耦合扰动影响会异常强烈和明显，甚至所产生的耦合扰动力矩数值上可能达到无人机螺旋桨能产生的用于做姿态机动总力矩的1/3左右。对于搭载多自由度机械臂的旋翼飞行机械臂，其作为典型的强耦合、欠驱动和非线性动态***，无人机与机械臂之间的耦合扰动会直接通过它们之间的刚性连接而传递，造成***姿态的偏差，而姿态偏差又会因该***的欠驱动特性进一步影响到其位置控制的偏差，最终导致旋翼飞行机械臂的无人机基座和机械臂末端执行器控制精度均受到严重影响。是故，强烈的耦合扰动会严重影响旋翼飞行机械臂执行空中作业任务的准确性，更严重的情况可能威胁***的稳定性，甚至出现***失控造成事故的严重后果。所以，旋翼飞行机械臂精确稳定地执行空中作业任务的前提便是无人机基座能够在强烈的耦合扰动影响下仍然保持稳定和高精度的位姿控制，在此条件下，机械臂末端执行器准确稳定地达到期望位姿状态才成为可能。因此，研究和分析应对强烈耦合扰动的旋翼飞行机械臂***的精确补偿与稳定控制显得尤为重要。

在现有相关研究中，如专利申请号为CN201910916380的专利中提出了一种基于解耦控制的飞行机械臂***，该发明在处理耦合扰动时采用传统机械臂迭代动力学方程来计算驱动关节的力矩，并将其视为机械臂运动对飞行器造成的外力干扰；这种处理方式只能适用于传统与固定基座固连的机械臂，对于旋翼飞行机械臂内以浮动的无人机为基座的机械臂是不准确的，因为该耦合扰动不仅仅只与机械臂的状态有关，而是与无人机和机械臂的状态都耦合相关的。专利申请号为CN202110994016的专利中提出了一种抗干扰和饱和特性的飞行机械臂位姿控制方法，专利申请号为CN202010801707的专利中提出了一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法，专利申请号为CN201810094313的专利中提出了一种基于动态重心补偿的旋翼飞行机械臂***及算法，这些方法在处理旋翼飞行机械臂的耦合扰动时均存在一个共性的问题，就是都只考虑了机械臂运动所带来的质心偏移，并未考虑***惯量也会因机械臂运动而动态变化的情况，所以这些方法对耦合扰动的实际抑制效果会相对打折扣。另外，专利申请号为CN201810271456的专利中提出了一种旋翼飞行机械臂自适应RBFNNs测噪自抗扰控制方法，该发明利用自适应RBFNNs对耦合扰动进行估计和反馈补偿；这类根据***受扰后的状态偏差进行估计和反馈补偿的方法是需要一定的响应时间才能估计和抑制扰动的，而对于旋翼飞行机械臂来说这段响应时间可能是致命的，因为瞬时产生的强烈的耦合扰动可能使得***还未等到准确的反馈补偿量被估计出来就已经受到严重影响，所以该类反馈补偿方法在实际***应用时难免受到一定的限制。

发明内容

针对现有旋翼飞行机械臂抗扰动控制中的反馈补偿存在延时，影响***稳定性的问题，本发明提供一种基于变惯性参数建模的飞行机械臂耦合扰动控制方法。

本发明的一种基于变惯性参数建模的飞行机械臂耦合扰动控制方法，包括，

步骤一：对旋翼飞行机械臂***的质心变化，建立动态变化质心在机体坐标系下的表达式；同时，对旋翼飞行机械臂***的惯量变化，根据平行轴移定理，建立动态变化惯量在机体坐标系下的表达式；

步骤二：利用多刚体质点系的动量与动量矩定理对旋翼飞行机械臂***进行动力学建模，得到***动力学模型；

步骤三：基于质心变化与惯量变化对***动力学模型进行处理，建立质心变化和惯量变化对旋翼飞行机械臂***的耦合扰动的映射关系，得到基于变惯性参数的耦合扰动模型；

步骤四：将耦合扰动模型计算获得的耦合扰动估计值作为前馈补偿量输入位置环和姿态环，实现耦合扰动控制。

根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，步骤一中，质心在机体坐标系下的表达式为：

式中^Br_oc表示旋翼飞行机械臂***在机体坐标系下的质心向量，

表示旋翼飞行机械臂***质心变化的速度，m_s表示旋翼飞行机械臂***的总质量；旋翼飞行机械臂***包括无人机与机械臂；m_i表示机械臂第i个连杆的质量，n表示机械臂连杆的总数量，i＝1,2…n，^Bp_ci表示机械臂第i个连杆在机体坐标系下的质心位置，^Bv_ci表示机械臂第i个连杆在机体坐标系下的质心变化速度。

根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，步骤一中，惯量在机体坐标系下的表达式为：

式中

表示机械臂相对于无人机基座与机械臂的连接点在机体坐标系下的转动惯量矩阵，

表示

的导数，^BR_i表示机械臂第i个连杆坐标系到机体坐标系的旋转矩阵，

表示机械臂第i个连杆在第i个连杆坐标系的惯量矩阵，I_3×3表示3x3的单位矩阵，^Bω_i表示第i个连杆在机体坐标系下的角速度，Skew(^Bω_i)表示^Bω_i的反对称矩阵，ⁱR_B表示^BR_i的转置；^Bp_ci,^Bv_ci,^Bω_i的获得方法为：

式中q表示机械臂的关节角度，

表示机械臂的角速度向量，^BJ_ci(q)表示机械臂第i个连杆质心相对于机体坐标系的雅可比矩阵，ⁱr_ci表示机械臂第i个连杆质心在第i个连杆坐标系的位置向量，^BT_i(q)表示从第i个连杆坐标系到机体坐标系的转换矩阵。

根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，步骤二中得到***动力学模型包括得到无人机机体坐标系到惯性系的转换矩阵^IR_B：

首先进行参考坐标系定义，以∑_I表示惯性系，以∑_B表示机体坐标系，∑_I和∑_B中均以横轴指向机头方向，竖轴指向地，∑_B中原点O位于无人机质心处，即无人机基座与机械臂的连接点；∑_{i(i＝1,2,,,n)}分别表示n自由度的机械臂各连杆坐标系，根据craig法则改进的DH参数法建立；

假设p₀点为旋翼飞行机械臂***中任一质点，p₀点相对于惯性系∑_I的绝对位置向量为r_p0，无人机相对于惯性系∑_I的绝对位置向量为r_o，p₀点相对于无人机的质心位置为

则得到以下关系式：

式中

表示机体坐标系下p₀点相对于o点的位置矢量，

为

的导数，^Bω_b表示无人机机体实际角速度向量，

表示

的导数，

为

在机体坐标系的表示，×代表叉乘运算；则得到^IR_B为：

式中φ表示无人机俯仰角，θ表示无人机滚转角，ψ表示无人机偏航角。

根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，步骤二中得到***动力学模型还包括得到旋翼飞行机械臂***的动量和动量矩：

根据多刚体质点系的动量和动量矩定义，得到旋翼飞行机械臂***的动量P和动量矩L：

式中m_b表示无人机质量，m_man表示机械臂质量，

表示p₀点所在位置的质量微分量；

对上式变形，得到：

式中^Br_omc表示机体坐标系下的机械臂质心向量，r_oc表示旋翼飞行机械臂***质心向量，^BI_b表示无人机转动惯量矩阵，

表示机体坐标系下机械臂相对于o点的转动惯量矩阵；

机体坐标系下的机械臂质心向量^Br_omc和机体坐标系下的旋翼飞行机械臂***质心向量^Br_oc之间的关系为：

根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，对旋翼飞行机械臂***的动量P和动量矩L求导得到：

按照动量与动量矩定理，得到：

式中^IF_ext表示惯性系中作用在质点系上的合外力，

表示^IF_ext相对于o点产生的合外力矩；则对于旋翼飞行机械臂***，得到：

式中F_l表示无人机螺旋桨产生的升力，e₃表示竖直方向单位向量，e₃＝[0 0 1]^T；g表示重力加速度，τ表示无人机螺旋桨产生的力矩。

根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，结合所有关系式推导得到***动力学模型：

式中P_b＝[x,y,z]表示无人机在惯性坐标系下的位置向量，其中x,y,z为无人机在惯性坐标系下的横轴、纵轴、竖轴坐标；

表示无人机在惯性坐标系下的速度向量；Φ_b为无人机机体角速度：Φ_b＝[φ,θ,ψ]；

T(Φ_b)表示无人机机体角速度到姿态角速度的旋转矩阵；

^Br_o表示机体坐标系下机械臂与无人机基座连接点的位置，所述机械臂安装在无人机质心位置；

^Bω_b＝[p q r]^T，其中p、q、r分别为无人机的滚转角速度、俯仰角速度和偏航角速度。

根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，所述耦合扰动模型的具体形式如下：

式中F_dis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力向量，^Bτ_dis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力矩向量。

根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，步骤四中，位置环和姿态环的基础控制器均采用级联PID控制算法；

级联PID位置内环控制器的表达式为：

u_{pos_basic}＝[u_x u_y u_z]^T＝K_{p_v}(v_{b_d}-v_b)+K_{i_v}∫(v_{b_d}-v_b)dt+K_{d_v}(d(v_{b_d}-v_b)/dt)，

式中u_{pos_basic}表示由级联PID位置内环控制器生成的三个方向上的基础控制量，其中u_x、u_y、u_z分别为横轴、纵轴和竖轴的基础控制量；v_{b_d}为期望速度向量，v_b为实际速度向量；

K_{p_v}表示位置内环控制器比例参数矩阵，K_{i_v}表示位置内环控制器积分参数矩阵，K_{d_v}表示位置内环控制器微分参数矩阵；

级联PID姿态内环控制器的表达式为：

式中u_{att_basic}表示由级联PID姿态内环控制器生成的三个方向上的基础控制量，其中τ_φ表示俯仰角方向的基础控制量，τ_θ表示滚转角方向的基础控制量，τ_ψ表示偏航角方向的基础控制量，^Bω_{b_d}为期望角速度向量；K_{p_ω}表示姿态内环控制器比例参数矩阵，K_{i_ω}表示姿态内环控制器积分参数矩阵，K_{d_ω}表示姿态内环控制器微分参数矩阵。

根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，由级联PID位置内环控制器和级联PID姿态内环控制器得到整体控制器的最终形式为：

式中u_pos表示位置内环最终控制量，u_att表示姿态内环最终控制量。

本发明的有益效果：本发明方法具体应用在搭载多自由度机械臂的旋翼飞行机械臂***中，可对旋翼飞行机械臂内存在的耦合扰动影响进行精确的模型描述与前馈补偿，实现旋翼飞行机械臂在强烈的耦合扰动影响下的精确补偿与稳定控制。

本发明方法主要以旋翼飞行器为基座搭载多自由度机械臂的新型空中机器人***为研究对象，研究该***面临的强烈的耦合扰动问题。从耦合扰动产生机理层面展开深入分析，综合考虑了该***由于机械臂运动所带来的***质心与惯量的动态变化，并以变惯性参数的方式来描述机械臂运动与***质心和惯量变化的映射关系。在此基础上，基于多刚体的动量与动量矩定理，进一步推导了精确的***动力学模型和基于变惯性参数的耦合扰动模型，该模型充分利用了无人机与机械臂的状态信息，能准确反应无人机与机械臂之间耦合扰动的静态和动态关系，并实现在不借助外部传感器的情况下对耦合扰动的准确估计。

另外，基于该精确的耦合扰动模型结合基础控制器的前馈补偿方法，相较于其他反馈补偿方法，本发明方法可及时准确地补偿由耦合扰动带给***的干扰影响，有效避免了反馈补偿方法需要一定响应时间的延迟且很难做到准确估计的弊端。因此，本发明方法可以有效抑制强烈的耦合扰动对***的影响，显著提高旋翼飞行机械臂***的抗扰能力、控制精度及稳定性。

附图说明

图1是本发明所述基于变惯性参数建模的飞行机械臂耦合扰动控制方法的流程图；

图2是旋翼飞行机械臂***建模过程参考坐标系定义示意图；图中1表示无人机的第一个电机，2表示无人机的第二个电机，3表示无人机的第三个电机，4表示无人机的第四个电机，5表示无人机的第五个电机，六表示无人机的第六个电机；X_B、Y_B、Z_B表示机体坐标系∑_B的横轴、纵轴和竖轴；X_I、Y_I、Z_I表示惯性系∑_I的横轴、纵轴和竖轴；X_i、Y_i、Z_i表示机械臂第i个连杆坐标系∑_i的横轴、纵轴和竖轴；

图3是基于本发明方法进行前馈补偿的飞行机械臂耦合扰动控制的控制框图；图中x_d、y_d、z_d分别为x、y、z轴期望位置信号，ψ_d为偏航角期望信号，φ_d为滚转角期望信号，θ_d为俯仰角期望信号，u_m为得到的升力方向控制量。

具体实施方式

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

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面结合附图和具体实施例对本发明作进一步说明，但不作为本发明的限定。

具体实施方式一、结合图1至图3所示，本发明提供了一种基于变惯性参数建模的飞行机械臂耦合扰动控制方法，整体技术方案由耦合扰动产生机理分析、变惯性参数设计、***动力学建模、耦合扰动模型建立及结合控制器进行前馈补偿这五个部分组成。对于搭载多自由度机械臂的旋翼飞行机械臂***，无人机基座与机械臂之间所产生的耦合扰动将直接影响***位置与姿态控制表现，甚至威胁到***飞行的安全。本实施方式首先从耦合扰动产生机理层面对耦合扰动进行了深入分析，考虑质心和惯量的动态变化，利用变惯性参数对该***及其耦合扰动进行了精确建模，所推到的耦合扰动模型充分利用了无人机与机械臂的状态信息，能准确反应无人机与机械臂之间耦合扰动的静态和动态关系，可实现在不借助外部传感器的情况下对耦合扰动的准确估计。利用耦合扰动模型在控制器中进行前馈补偿，可实现旋翼飞行机械臂在强烈的耦合扰动影响下的精确补偿与控制。

本实施方式对耦合扰动产生机理进行深入分析。旋翼飞行机械臂***中无人机与机械臂之间的耦合扰动，不同于传统机械臂对于固定基座施加的扰动力和力矩，其是与无人机状态和机械臂状态紧密相关的一种强烈的耦合影响。在旋翼飞行机械臂***中，当无人机与机械臂之间存在相对运动时，该耦合扰动便会产生，且随着两者之间相对运动越剧烈越快速，这种耦合扰动影响越强烈，带给整个***的干扰影响越显著。这是因为机械臂的运动会引起旋翼飞行机械臂***的质心和惯量发生变化，且机械臂运动范围越大越快速，所引起的质心和惯量变化越大越剧烈，从而产生越强烈的耦合扰动力和力矩作用于***。

具体实现步骤包括：

步骤一：变惯性参数设计：对旋翼飞行机械臂***的质心变化，建立动态变化质心在机体坐标系下的表达式；同时，对旋翼飞行机械臂***的惯量变化，根据平行轴移定理，建立动态变化惯量在机体坐标系下的表达式；

步骤二：考虑旋翼飞行机械臂是由旋翼无人机搭载一个n自由度的机械臂构成的整体***，这时不再将旋翼飞行机械臂***统一作为单个刚体，而是看作为三维空间的多个刚体组成的***。利用多刚体质点系的动量与动量矩定理对旋翼飞行机械臂***进行动力学建模，得到***动力学模型；

步骤三：基于质心变化与惯量变化对***动力学模型进行处理，建立质心变化和惯量变化对旋翼飞行机械臂***的耦合扰动的映射关系，得到基于变惯性参数的耦合扰动模型；

步骤四：结合控制器进行前馈补偿。在通过精确的建模步骤得到了基于变惯性参数的耦合扰动模型后，只需在基础控制器设计过程中加入耦合扰动模型所计算出来的耦合扰动估计值作为前馈补偿，即可实现旋翼飞行机械臂在强烈的耦合扰动影响下的精确补偿与控制。所述基础控制器可以是任意线性或非线性控制器，只要其在没有扰动情况下的闭环***是稳定的即可。将耦合扰动模型计算获得的耦合扰动估计值作为前馈补偿量输入位置环和姿态环，实现耦合扰动控制。

进一步，步骤一中，质心在机体坐标系下的表达式为：

式中^Br_oc表示旋翼飞行机械臂***在机体坐标系下的质心向量，

表示旋翼飞行机械臂***质心变化的速度，m_s表示旋翼飞行机械臂***的总质量；旋翼飞行机械臂***包括无人机与机械臂；m_i表示机械臂第i个连杆的质量，n表示机械臂连杆的总数量，i＝1,2…n，^Bp_ci表示机械臂第i个连杆在机体坐标系下的质心位置，^Bv_ci表示机械臂第i个连杆在机体坐标系下的质心变化速度。

步骤一中，惯量在机体坐标系下的表达式为：

式中

表示机械臂相对于无人机基座与机械臂的连接点在机体坐标系下的转动惯量矩阵，

表示

的导数，^BR_i表示机械臂第i个连杆坐标系到机体坐标系的旋转矩阵，

表示机械臂第i个连杆在第i个连杆坐标系的惯量矩阵，I_3×3表示3x3的单位矩阵，^Bω_i表示第i个连杆在机体坐标系下的角速度，Skew(^Bω_i)表示^Bω_i的反对称矩阵，ⁱR_B表示^BR_i的转置；

上面两个表达式中，除了^Bp_ci,^Bv_ci,^Bω_i，其余变量均为机械臂固有惯性参数或可通过机械臂DH参数模型获取。^Bp_ci,^Bv_ci,^Bω_i的获得方法为：

式中q表示机械臂的关节角度，

表示机械臂的角速度向量，^BJ_ci(q)表示机械臂第i个连杆质心相对于机体坐标系的雅可比矩阵，ⁱr_ci表示机械臂第i个连杆质心在第i个连杆坐标系的位置向量；^BT_i(q)表示从第i个连杆坐标系到机体坐标系的转换矩阵，可通过DH参数模型获得。

再进一步，步骤二中得到***动力学模型包括得到无人机机体坐标系到惯性系的转换矩阵^IR_B：

结合图2所示，首先进行参考坐标系定义，以∑_I表示惯性系，以∑_B表示机体坐标系，∑_I和∑_B中均以横轴指向机头方向，竖轴指向地，∑_B中原点O位于无人机质心处，即无人机基座与机械臂的连接点；∑_{i(i＝1,2,,,n)}分别表示n自由度的机械臂各连杆坐标系，根据craig法则改进的DH参数法建立；

假设p₀点为旋翼飞行机械臂***中任一质点，p₀点相对于惯性系∑_I的绝对位置向量为

无人机相对于惯性系∑_I的绝对位置向量为r_o，p₀点相对于无人机的质心位置为

则得到以下关系式：

式中

表示机体坐标系下p₀点相对于o点的位置矢量，

为

的导数，^Bω_b表示无人机机体实际角速度向量，

表示

的导数，

为

在机体坐标系的表示，×代表叉乘运算；则得到^IR_B为：

式中φ表示无人机俯仰角，θ表示无人机滚转角，ψ表示无人机偏航角。

步骤二中得到***动力学模型还包括得到旋翼飞行机械臂***的动量和动量矩：

根据多刚体质点系的动量和动量矩定义，得到旋翼飞行机械臂***的动量P和动量矩L：

式中m_b表示无人机质量，m_man表示机械臂质量，

表示p₀点所在位置的质量微分量；

对上式变形，得到：

式中^Br_omc表示机体坐标系下的机械臂质心向量，r_oc表示旋翼飞行机械臂***质心向量，^BI_b表示无人机转动惯量矩阵，

表示机体坐标系下机械臂相对于o点的转动惯量矩阵；

机体坐标系下的机械臂质心向量^Br_omc和机体坐标系下的旋翼飞行机械臂***质心向量^Br_oc之间的关系为：

本实施方式中只保留了

中的主导项

L_man表示机械臂运动相对于o点的动量矩，这是因为可以认为机械臂质心的运动相对于o点的动量矩可主要代表机械臂整体运动对o点的动量矩，这样做是合理的。

对旋翼飞行机械臂***的动量P和动量矩L求导得到：

按照动量与动量矩定理，得到：

式中^IF_ext表示惯性系中作用在质点系上的合外力，

表示^IF_ext相对于o点产生的合外力矩；则对于旋翼飞行机械臂***，得到：

式中F_l表示无人机螺旋桨产生的升力，e₃表示竖直方向单位向量，e₃＝[0 0 1]^T；g表示重力加速度，τ表示无人机螺旋桨产生的力矩。

结合所有关系式推导得到***动力学模型：

式中P_b＝[x,y,z]表示无人机在惯性坐标系下的位置向量，其中x,y,z为无人机在惯性坐标系下的横轴、纵轴、竖轴坐标；

表示无人机在惯性坐标系下的速度向量；Φ_b为无人机机体角速度：Φ_b＝[φ,θ,ψ]；

T(Φ_b)表示无人机机体角速度到姿态角速度的旋转矩阵；

^Br_o表示机体坐标系下机械臂与无人机基座连接点的位置，所述机械臂安装在无人机质心位置；

^Bω_b＝[p q r]^T，其中p、q、r分别为无人机的滚转角速度、俯仰角速度和偏航角速度。

基于变惯性参数推导的旋翼飞行机械臂***动力学模型不同于传统旋翼飞行平台动力学模型，其含有动态变化的惯性参数及其引起的附加耦合扰动项，它们与无人机和机械臂的状态量及其导数紧密耦合在一起。从理论上讲，该模型可以很好的描述出旋翼飞行机械臂***在受耦合扰动情况下的真实动力学特性。

机械臂运动会导致旋翼飞行机械臂***的质心^Br_oc和惯量

发生变化，且机械臂运动越剧烈，所带来的***质心和惯量变化越剧烈，而这种变化和整个***间的耦合扰动之间的映射关系即为所需要建立的耦合扰动模型。将步骤二中的***动力学模型做进一步处理，可推导出基于变惯性参数的耦合扰动模型具体形式如下：

式中F_dis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力向量，^Bτ_dis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力矩向量。

耦合扰动模型中所需用到的无人机及机械臂各状态量、状态量导数及其二阶导都是可以通过旋翼飞行机械臂***自身的传感器直接测量得到或间接通过一些估计方法得到，是易于在实际***中使用的。

基于变惯性参数的耦合扰动模型是与无人机及机械臂各状态量、状态量导数及其二阶导直接相关的，其充分利用了***内部各部分的状态量，且这些状态量都是可以通过旋翼飞行机械臂***上自带的传感器直接测量或间接估计出来的，可以很好地反应无人机和机械臂之间耦合扰动的静态与动态关系。

再进一步，结合控制器进行前馈补偿。

考虑到现行大多数的实物无人机***的控制器均还是采用级联PID控制算法，为了本发明所提出方法在实际***的泛化使用，选取级联PID控制算法作为基础控制器来说明结合控制器进行前馈补偿的具体实现。

步骤四中，位置环和姿态环的基础控制器均采用级联PID控制算法；

级联PID位置内环控制器的表达式为：

u_{pos_basic}＝[u_x u_y u_z]^T＝K_{p_v}(v_{b_d}-v_b)+K_{i_v}∫(v_{b_d}-v_b)dt+K_{d_v}(d(v_{b_d}-v_b)/dt)，

式中u_{pos_basic}表示由级联PID位置内环控制器生成的三个方向上的基础控制量，其中u_x、u_y、u_z分别为横轴、纵轴和竖轴的基础控制量；v_{b_d}为期望速度向量，v_b为实际速度向量；

K_{p_v}表示位置内环控制器比例参数矩阵，K_{i_v}表示位置内环控制器积分参数矩阵，K_{d_v}表示位置内环控制器微分参数矩阵；

级联PID姿态内环控制器的表达式为：

式中u_{att_basic}表示由级联PID姿态内环控制器生成的三个方向上的基础控制量，其中τ_φ表示俯仰角方向的基础控制量，τ_θ表示滚转角方向的基础控制量，τ_ψ表示偏航角方向的基础控制量，^Bω_{b_d}为期望角速度向量；K_{p_ω}表示姿态内环控制器比例参数矩阵，K_{i_ω}表示姿态内环控制器积分参数矩阵，K_{d_ω}表示姿态内环控制器微分参数矩阵。

基于变惯性参数的耦合扰动模型进行前馈补偿，并结合基础控制器所生成的控制量，可以得到控制器最终形式：

由级联PID位置内环控制器和级联PID姿态内环控制器得到整体控制器的最终形式为：

式中u_pos表示位置内环最终控制量，u_att表示姿态内环最终控制量。

至此，完成了对本发明方法的详细阐述,控制***框架图如图3所示。图3中首先，被控对象为旋翼飞行机械臂***，由无人机基座搭载多自由度机械臂构成，其内部存在强烈的耦合扰动；然后，对于控制流程，基于变惯性参数的耦合扰动模型前馈补偿量部分主要负责根据无人机与机械臂的各状态量、状态量导数和采用指令滤波器等微分估计方法获取的状态量二阶导，通过耦合扰动模型计算出前馈补偿力和力矩；然后结合基础控制器生成的控制量进行前馈补偿，从而形成最终的控制量作用于旋翼飞行机械臂***，使其实现在强烈的耦合扰动影响下的精确补偿与控制。

虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其它所述实施例中。

Claims (10)

1.一种基于变惯性参数建模的飞行机械臂耦合扰动控制方法，其特征在于包括，

步骤一：对旋翼飞行机械臂***的质心变化，建立动态变化质心在机体坐标系下的表达式；同时，对旋翼飞行机械臂***的惯量变化，根据平行轴移定理，建立动态变化惯量在机体坐标系下的表达式；

步骤二：利用多刚体质点系的动量与动量矩定理对旋翼飞行机械臂***进行动力学建模，得到***动力学模型；

步骤三：基于质心变化与惯量变化对***动力学模型进行处理，建立质心变化和惯量变化对旋翼飞行机械臂***的耦合扰动的映射关系，得到基于变惯性参数的耦合扰动模型；

步骤四：将耦合扰动模型计算获得的耦合扰动估计值作为前馈补偿量输入位置环和姿态环，实现耦合扰动控制。

2.根据权利要求1所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，其特征在于，步骤一中，质心在机体坐标系下的表达式为：

式中^Br_oc表示旋翼飞行机械臂***在机体坐标系下的质心向量，

表示旋翼飞行机械臂***质心变化的速度，m_s表示旋翼飞行机械臂***的总质量；旋翼飞行机械臂***包括无人机与机械臂；m_i表示机械臂第i个连杆的质量，n表示机械臂连杆的总数量，i＝1,2…n，^Bp_ci表示机械臂第i个连杆在机体坐标系下的质心位置，^Bv_ci表示机械臂第i个连杆在机体坐标系下的质心变化速度。

3.根据权利要求2所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，其特征在于，步骤一中，惯量在机体坐标系下的表达式为：

式中

表示机械臂相对于无人机基座与机械臂的连接点在机体坐标系下的转动惯量矩阵，

表示

的导数，^BR_i表示机械臂第i个连杆坐标系到机体坐标系的旋转矩阵，

表示机械臂第i个连杆在第i个连杆坐标系的惯量矩阵，I_3×3表示3x3的单位矩阵，^Bω_i表示第i个连杆在机体坐标系下的角速度，Skew(^Bω_i)表示^Bω_i的反对称矩阵，ⁱR_B表示^BR_i的转置；^Bp_ci,^Bv_ci,^Bω_i的获得方法为：

式中q表示机械臂的关节角度，

表示机械臂的角速度向量，^BJ_ci(q)表示机械臂第i个连杆质心相对于机体坐标系的雅可比矩阵，ⁱr_ci表示机械臂第i个连杆质心在第i个连杆坐标系的位置向量，^BT_i(q)表示从第i个连杆坐标系到机体坐标系的转换矩阵。

4.根据权利要求3所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，其特征在于，步骤二中得到***动力学模型包括得到无人机机体坐标系到惯性系的转换矩阵^IR_B：

首先进行参考坐标系定义，以∑_I表示惯性系，以∑_B表示机体坐标系，∑_I和∑_B中均以横轴指向机头方向，竖轴指向地，∑_B中原点O位于无人机质心处，即无人机基座与机械臂的连接点；∑_{i(i＝1,2,,,n)}分别表示n自由度的机械臂各连杆坐标系，根据craig法则改进的DH参数法建立；

假设p₀点为旋翼飞行机械臂***中任一质点，p₀点相对于惯性系∑_I的绝对位置向量为

无人机相对于惯性系∑_I的绝对位置向量为r_o，p₀点相对于无人机的质心位置为

则得到以下关系式：

式中

表示机体坐标系下p₀点相对于o点的位置矢量，

为

的导数，^Bω_b表示无人机机体实际角速度向量，

表示

的导数，

为

在机体坐标系的表示，×代表叉乘运算；则得到^IR_B为：

式中φ表示无人机俯仰角，θ表示无人机滚转角，ψ表示无人机偏航角。

5.根据权利要求4所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，其特征在于，步骤二中得到***动力学模型还包括得到旋翼飞行机械臂***的动量和动量矩：

根据多刚体质点系的动量和动量矩定义，得到旋翼飞行机械臂***的动量P和动量矩L：

式中m_b表示无人机质量，m_man表示机械臂质量，

表示p₀点所在位置的质量微分量；

对上式变形，得到：

式中^Br_omc表示机体坐标系下的机械臂质心向量，r_oc表示旋翼飞行机械臂***质心向量，^BI_b表示无人机转动惯量矩阵，

表示机体坐标系下机械臂相对于o点的转动惯量矩阵；

机体坐标系下的机械臂质心向量^Br_omc和机体坐标系下的旋翼飞行机械臂***质心向量^Br_oc之间的关系为：

6.根据权利要求5所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，其特征在于，对旋翼飞行机械臂***的动量P和动量矩L求导得到：

按照动量与动量矩定理，得到：

式中^IF_ext表示惯性系中作用在质点系上的合外力，

表示^IF_ext相对于o点产生的合外力矩；则对于旋翼飞行机械臂***，得到：

式中F_l表示无人机螺旋桨产生的升力，e₃表示竖直方向单位向量，e₃＝[0 0 1]^T；g表示重力加速度，τ表示无人机螺旋桨产生的力矩。

7.根据权利要求6所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，其特征在于，结合所有关系式推导得到***动力学模型：

式中P_b＝[x,y,z]表示无人机在惯性坐标系下的位置向量，其中x,y,z为无人机在惯性坐标系下的横轴、纵轴、竖轴坐标；

表示无人机在惯性坐标系下的速度向量；Φ_b为无人机机体角速度：Φ_b＝[φ,θ,ψ]；

T(Φ_b)表示无人机机体角速度到姿态角速度的旋转矩阵；

^Br_o表示机体坐标系下机械臂与无人机基座连接点的位置，所述机械臂安装在无人机质心位置；

^Bω_b＝[p q r]^T，其中p、q、r分别为无人机的滚转角速度、俯仰角速度和偏航角速度。

8.根据权利要求7所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，其特征在于，所述耦合扰动模型的具体形式如下：

式中F_dis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力向量，Bτ_dis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力矩向量。

9.根据权利要求8所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，其特征在于，步骤四中，位置环和姿态环的基础控制器均采用级联PID控制算法；

级联PID位置内环控制器的表达式为：

u_{pos_basic}＝[u_x u_y u_z]^T＝K_{p_v}(v_{b_d}-v_b)+K_{i_v}∫(v_{b_d}-v_b)dt+K_{d_v}(d(v_{b_d}-v_b)/dt)，

式中u_{pos_basic}表示由级联PID位置内环控制器生成的三个方向上的基础控制量，其中u_x、u_y、u_z分别为横轴、纵轴和竖轴的基础控制量；v_{b_d}为期望速度向量，v_b为实际速度向量；

K_{p_v}表示位置内环控制器比例参数矩阵，K_{i_v}表示位置内环控制器积分参数矩阵，K_{d_v}表示位置内环控制器微分参数矩阵；

级联PID姿态内环控制器的表达式为：

式中u_{att_basic}表示由级联PID姿态内环控制器生成的三个方向上的基础控制量，其中τ_φ表示俯仰角方向的基础控制量，τ_θ表示滚转角方向的基础控制量，τ_ψ表示偏航角方向的基础控制量，^Bω_{b_d}为期望角速度向量；K_{p_ω}表示姿态内环控制器比例参数矩阵，K_{i_ω}表示姿态内环控制器积分参数矩阵，K_{d_ω}表示姿态内环控制器微分参数矩阵。

10.根据权利要求9所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法，其特征在于，

由级联PID位置内环控制器和级联PID姿态内环控制器得到整体控制器的最终形式为：

式中u_pos表示位置内环最终控制量，u_att表示姿态内环最终控制量。

Images