CN111185907A - 一种作业型飞行机器人抓取后的位姿稳定控制方法 - Google Patents

Abstract

本发明涉及一种作业型飞行机器人抓取后的位姿稳定控制方法，首先，利用牛顿‑欧拉方程法对四旋翼无人机搭载机械臂系统建模；然后，在位置控制器中考虑重心偏移系统参数，在滑模控制中补偿重心偏移系统参数，使四旋翼无人机平台按目标轨迹飞行；接着，解算出四旋翼无人机平台按期望轨迹飞行所需的期望翻滚角、期望俯仰角和升力；在姿态控制器中考虑重心偏移控制参数和惯性张量不为常数，在姿态控制器中加入自适应去估计重心偏移控制参数和惯性张量，并加入CMAC神经网络在线逼近惯性张量的真实值，解算出所需的输入力矩，进而联立解算出四个旋翼的的转速。本发明能解决姿态控制律中时变参数未知以及姿态控制稳定不足和重心偏移对位置稳定不足问题。

Description

一种作业型飞行机器人抓取后的位姿稳定控制方法

技术领域

本发明涉及作业型飞行机器人飞行抓取领域，特别是一种作业型飞行机器人抓取后的位姿稳定控制方法。

背景技术

四旋翼无人机是利用无线电遥操作和自备程序控制装置的不载人飞机，或者由车载计算机完全地或间歇地自主操作。四旋翼无人机已经是成熟的飞行平台，可以在该飞行平台上搭载不同组件扩展飞行平台在不同领域的应用。如今，快递日均亿件的时代已经到来，四旋翼无人机被成功的应用在物流货运。在农业、高空维修和清洁等方面都有四旋翼无人机应用的潜能。其中，这些应用不乏需要在四旋翼无人机平台上搭载机械臂，将二者结合起来就是空中智能机器人，如此高端的设备可使工业获得很大的便利。在救灾过程中，四旋翼无人机不再只是探测，而是救援的一环。

上述的这些应用都有一个飞行抓取的动作。想要凭借指令飞行抓取，就还有一些技术难点需要攻克。稳定抓取，是控制工程对飞行抓取首要解决的问题。以往影响四旋翼无人机稳定的主要因素是气流，而机械臂的加入，系统的重心位置发生改变，特别是在机械臂末端有负载时会严重影响无人机平台稳定。

对于飞行抓取的重心偏移问题，已经有学者提出了移动电池盒在短时间补偿偏移的方法；为了减小重心偏移，机械手的几何参数的选择经过优化，使机械臂质量最小化等方法；发明了一种带有重心调节机构的作业型飞行机器人以调节重心位置。对作业型飞行机器人系统会造成额外的设计负担。

发明内容

有鉴于此，本发明的目的是提供一种作业型飞行机器人抓取后的位姿稳定控制方法，克服现有中飞行抓取后重心位置发生改变导致位姿不平稳的问题。

本发明采用以下方案实现：一种作业型飞行机器人抓取后的位姿稳定控制方法，提供一四旋翼无人机搭载机械臂系统，包括以下步骤：

步骤S1：利用牛顿-欧拉方程法对四旋翼无人机搭载机械臂系统建模，不忽略重心偏移造成的干扰，惯性张量不为常数；

步骤S2：在步骤S1建立模型的基础上，基于滑模控制设计位置控制器用以进行位置稳定跟踪，用以补偿重心偏移系统参数，使四旋翼无人机按期望轨迹d_T飞行；

步骤S3：进行姿态解耦：解算出四旋翼无人机按期望轨迹d_T飞行所需的期望翻滚角

期望俯仰角θ_d和升力u₁；

步骤S4：在步骤S1建立的模型的基础上，基于滑模控制设计姿态控制器用以进行姿态稳定跟踪，并考虑重心偏移控制参数和惯性张量不为常数，姿态控制器中加入自适应去估计重心偏移控制参数和惯性张量，并加入CMAC神经网络在线逼近惯性张量的真实值，用以提高姿态控制精度，并解算出所需的输入力矩u_i，i＝2,3,4，u₂为翻滚角子系统的输入力矩，u₃为俯仰角子系统的输入力矩，u₄为偏航角子系统的输入力矩；

步骤S5：通过升力u₁、翻滚力矩u₂、俯仰力矩u₃、偏航力矩u₄联立解算出四个旋翼的转速ω_i，i＝1，2,3,4；所述四旋翼无人机搭载机械臂系统通过解算出四个旋翼的转速控制无人机飞行，用以实现位姿稳定控制。

进一步地，所述步骤S1具体包括以下内容：

对四旋翼飞行机械臂系统建模，根据力平衡和力矩平衡得到：

其中，F为系统所受的外力；M为系统所受的外力矩；m_s为系统总质量；r'为作业型飞行机器人重心在飞行平台坐标系内的位置；r₀为飞行平台在世界坐标系的位置；B为系统的推动力；Ω是无人机平台在世界坐标系的角速度矢量；I为系统的惯性张量；

表示为Ω进行一次微分，

表示为对r₀的二次微分。

进一步地，所述步骤S2具体包括以下步骤：

步骤S21：在四旋翼无人机搭载机械臂系统中输入在惯性坐标的期望轨迹d_T＝[x_Ty_T z_T]^T；对期望轨迹求二阶微分，求解出期望加速度

步骤S22：利用卡尔曼滤波融合超宽带定位技术测量四旋翼无人机平台在世界坐标系内的位置，获取当前四旋翼无人机平台位置信息d＝[x y z]^T；

步骤S23：令跟踪误差向量为δ；实时测量四旋翼无人机平台的与期望轨迹的误差；

δ＝d_T-d,δ∈R³ (2)

步骤S24：用一个一阶滑模函数s，定义状态空间中的时变曲面：

λ₁＞0,λ₂＞0,λ₃＞0

步骤S25：姿态解耦考虑重心偏移系统参数h，用以提高四旋翼无人机搭载机械臂系统建模精度，h＝[h₁,h₂,h₃]^T，h₁表示重心偏移系统参数在x轴分量，h₂表示重心偏移系统参数在y轴分量，h₃表示重心偏移系统参数在z轴分量；h有界，存在正标量参数ρ，有|h|≤ρ；

步骤S26：定义一个饱和函数，饱和函数表示为：

其中，存在正标量参数ε和β，有0＜ε＜1，β＞0；

步骤S27：综上，位置控制器为：

其中，鲁棒项v为：

其中，η为中间变量表示为

η＝[η₁，η₂，η₃]^T。

进一步地，所述步骤S3的具体内容为：

结合式(6)，令偏航角为ψ_d，其中-π＜ψ_d＜π，从而输出所需要翻滚角

俯仰角θ_d；

其中，g为重力加速度；结合式(6)、(8)，求得升力u₁；

进一步地，所述步骤S4具体包括以下步骤：

步骤S41：令四旋翼无人机搭载机械臂系统中四旋翼无人机的三种姿态中的翻滚角子系统为第1个子系统，俯仰角子系统为第2个子系统，偏航角为第3个子系统；q为绕y轴俯仰的角速度，p为绕x轴翻滚的角速度，r为绕z轴偏航的角速度；翻滚角状态方程如下：

u₂为翻滚子系统的输入力矩，I_x、I_y和I_z是分别关于x，y和z轴的转动惯量；c₁为重心偏移控制参数；

步骤S42：通过卡尔曼滤波融合三轴惯性测量传感器，获得无人机平台当前的翻滚角

俯仰角θ，偏航角ψ；

步骤S43：定义翻滚角跟踪角度误差e₁：实时测量四旋翼无人机与期望翻滚角

的误差：

步骤S44：通过卡尔曼滤波融合三轴惯性测量传感器实时测得Ω＝[p q r]^T，其中，p为翻滚角速度，q为俯仰角速度，r为偏航角速度；

步骤S45：定义翻滚角跟踪角速度误差χ₁：实时测量四旋翼无人机与期望翻滚角速度p_d的误差：

χ₁＝p_d-p (12)

其中，

步骤S46：对期望翻滚角

求二阶微分得

步骤S47：惯性张量I＝[I_x,I_y,I_z]^T，其中

表示为作业型飞行机器人初始状态时的惯性张量；γ_x，γ_y，γ_z分别为作业时惯性张量与初始惯性张量在x、y和z轴的偏差；且

|γ_x|＜κ，|γ_y|＜κ，|γ_z|＜κ，κ为正标量参数；

步骤S48：在翻滚角子系统下，重心偏移控制参数的估计值

作业时惯性张量与初始惯性张量的偏差估计

为式(13)至(16)，用以使李雅普诺夫函数正定，一阶微分李雅普诺夫半负定；

其中，t为时间，m_s为系统总质量，ξ₁为正标量参数，

为翻滚角速度一阶微分；

步骤S49：通过俯仰角子系统和偏航角子系统，估计出另外两组惯性张量和重心偏移系统参数，表示如下：

其中，

χ₂＝p_d-p，χ₂为俯仰角速度误差；χ₃＝r_d-r，χ₃为偏航角速度误差；在三个子系统中估计出三组惯性张量

为作业型飞行机器人没有抓取目标物在初始状态时的惯性张量；

步骤S410：把式(18)(19)(20)作为CMAC神经网络的输入，网络的高斯基函数为：

其中，h(x)＝[h(x_i)]^T，h(y)＝[h(y_i)]^T，h(z)＝[h(z_i)]^T，h(x_i)为惯性张量在x轴分量的i个高斯基函数输入；a_x,i，a_y,i，a_z,i为惯性张量的i个高斯基函数输入的中心值；b_x,i，b_y,i，b_z,i为惯性张量的i个高斯基函数输入惯性张量的宽度；

步骤S411：CMAC神经网络的权值估计值为：

其中，

为

的权值，

为

的权值，

为

的权值；

为第i个子系统估计的第i组权值函数，i＝1,2,3，分别为

k₁、k₂、k₃为正标量参数，；

步骤S412：综上，力矩u₂为：

其中，存在正标量参数μ₁，使得外部扰动|Δ₁|＜μ₁；

为权值函数累计误差的上限，即

ο₁≥ι₁≥0，ο₁和ι₁都为正标量参数，α＝min(k₁,k₂)；

步骤S413：俯仰角状态方程如下：

u₃为翻滚角子系统的输入力矩，I_x、I_y和I_z是分别关于x，y和z轴的转动惯量；c₂为重心偏移控制参数；

步骤S414：定义跟踪误差e₂：实时测量四旋翼无人机与期望俯仰角θ_d的误差：

步骤S415：定义俯仰角速度的期望轨迹为：

步骤S416：在俯仰角子系统下的重心偏移控制参数的估计值

为：

其中，ξ₂为正标量参数；

步骤S417：综上，翻滚力矩u₃：

步骤S418：偏航角状态方程如下：

u₄为翻滚角子系统的输入力矩，I_x、I_y和I_z是分别关于x，y和z轴的转动惯量；c₃为重心偏移控制参数；

步骤S419：定义跟踪误差e₃：实时测量四旋翼无人机与期望偏航角ψ_d的误差：

步骤S420：定义偏航角速度的期望轨迹为：

步骤S421：在偏航角子系统下的重心偏移控制参数的估计值

为：

其中，ξ₃为正标量参数。

步骤S422：综上，偏航力矩u₄：

k_i,i＝3,4,5,6为正标量参数。

进一步地，所述步骤S5的具体内容为：所述四个旋翼的角速度与输入u_i,i＝1,2,3,4的关系为：

u₁＝C₁(ω₁ ²+ω₂ ²+ω₃ ²+ω₄ ²)

u₂＝C₁(-ω₂ ²+ω₄ ²),u₃＝C₁(-ω₁ ²+ω₃ ²)

u₄＝C₂(ω₁ ²-ω₂ ²+ω₃ ²-ω₄ ²)

(37)

其中，u₁为通过升力、u₂为翻滚力矩、u₃为俯仰力矩、u₄为偏航力矩；正标量参数C₁、C₂定义为升力系数和扭矩系数；进而解算出四个旋翼的转速ω_ι，i＝1，2,3,4。

与现有技术相比，本发明具有以下有益效果：

(1)本发明在姿态解耦时考虑重心偏移系统参数并设计位置控制器补偿重心偏移系统参数，引入一个中间变量η表示补偿重心偏移系统参数的状态量。在位置控制设计中引用滑模控制，将传统滑模控制的符号函数改用饱和函数和鲁棒项，以减少在滑模面上振动的频率。以此解决重心偏移对位置控制稳定不足问题。

(2)在姿态控制器中考虑重心偏移控制参数和惯性张量不为常数，姿态控制器中加入自适应去估计重心偏移控制参数和惯性张量，并加入CMAC神经网络在线逼近惯性张量的真实值。以此解决姿态控制律中时变参数未知的问题以及姿态控制稳定不足的问题。

附图说明

图1为本发明实施例的位置控制器流程结构示意图。

图2为本发明实施例的姿态控制器流程结构示意图。

图3为本发明实施例的位置控制器中X轴分量的控制效果示意图。

图4为本发明实施例的位置控制器中Y轴分量的控制效果示意图。

图5为本发明实施例的位置控制器中Z轴分量的控制效果示意图。

图6为本发明实施例的姿态控制器中对翻滚角

的控制效果示意图。

图7为本发明实施例的姿态控制器中对俯仰角θ的控制效果示意图。

图8为本发明实施例的姿态控制器中对偏航角ψ的控制效果示意图。

图9为本发明实施例的四旋翼无人机搭载机械臂系统示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步说明。

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

如图1、2所示，本实施例提供一种作业型飞行机器人抓取后的位姿稳定控制方法，作业型飞行机器人是在四旋翼飞行机器人平台上搭载机械臂，提供一四旋翼无人机搭载机械臂系统，如图9所示，包括以下步骤：

步骤S1：利用牛顿-欧拉方程法对四旋翼无人机搭载机械臂系统建模，有别以往的系统建模，在建模时不再忽略重心偏移造成的干扰，惯性张量不为常数；

步骤S2：在步骤S1建立的模型的基础上，基于滑模控制设计位置控制器用以进行位置稳定跟踪，用以补偿重心偏移系统参数，使四旋翼无人机可以准确的按期望轨迹d_T飞行；

步骤S3：进行姿态解耦：解算出四旋翼无人机按期望轨迹d_T飞行所需的期望翻滚角

期望俯仰角θ_d和升力u₁；

步骤S4：在步骤S1建立的模型的基础上，基于反演控制设计姿态控制器用以进行姿态稳定跟踪，并考虑重心偏移控制参数和惯性张量不为常数，在姿态控制器中加入自适应去估计重心偏移控制参数和惯性张量，并加入CMAC神经网络在线逼近惯性张量的真实值，用以提高姿态控制精度，并解算出所需的输入力矩u_i，i＝2,3,4，u₂为翻滚角子系统的输入力矩，u₃为俯仰角子系统的输入力矩，u₄为偏航角子系统的输入力矩；

步骤S5：通过升力u₁、翻滚力矩u₂、俯仰力矩u₃、偏航力矩u₄联立解算出四个旋翼的转速ω_i，i＝1，2,3,4；所述四旋翼无人机搭载机械臂系统通过解算出四个旋翼的转速控制无人机飞行，用以实现位姿稳定控制。

在本实施例中，所述步骤S1具体包括以下内容：

对四旋翼飞行机械臂系统建模，根据力平衡和力矩平衡得到：

其中，F为系统所受的外力；M为系统所受的外力矩；m_s为系统总质量；r'为作业型飞行机器人重心在飞行平台坐标系内的位置；r₀为飞行平台在世界坐标系的位置；B为系统的推动力；Ω是无人机平台在世界坐标系的角速度矢量；I为系统的惯性张量；

表示为Ω进行一次微分，

表示为对r₀的二次微分。

在本实施例中，所述步骤S2具体包括以下步骤：

步骤S21：在四旋翼无人机搭载机械臂系统中输入在惯性坐标的期望轨迹d_T＝[x_Ty_T z_T]^T；对期望轨迹求二阶微分，求解出期望加速度

步骤S22：利用卡尔曼滤波融合超宽带定位技术测量四旋翼无人机平台在世界坐标系内的位置，获取当前四旋翼无人机平台位置信息d＝[x y z]^T；

步骤S23：令跟踪误差向量为δ；实时测量四旋翼无人机平台的与期望轨迹的误差；

δ＝d_T-d,δ∈R³ (2)

步骤S24：用一个一阶滑模函数s，定义状态空间中的时变曲面：

λ₁＞0,λ₂＞0,λ₃＞0

步骤S25：姿态解耦考虑重心偏移系统参数h，用以提高四旋翼无人机搭载机械臂系统建模精度，h＝[h₁,h₂,h₃]^T，h₁表示重心偏移系统参数在x轴分量，h₂表示重心偏移系统参数在y轴分量，h₃表示重心偏移系统参数在z轴分量；h有界，存在正标量参数ρ，有|h|≤ρ；

步骤S26：定义一个饱和函数，饱和函数表示为：

其中，存在正标量参数ε和β，有0＜ε＜1，β＞0；

步骤S27：综上，位置控制律为：

其中，鲁棒项v为：

其中，η为中间变量表示为

η＝[η₁，η₂，η₃]^T；对于期望轨迹d_T跟踪问题等价于当时间t大于零时轨迹必须进入滑模面，但因为重心偏移系统参数h会把轨迹拽离滑模面，通过加入饱和函数和鲁棒项v使轨迹一直待在滑模面上。

设计李雅普诺夫函数

下面证明本实施例设计的位置控制器可以在有限时间内到达滑模面，且随着时间的推移系统的位置误差δ趋于零。

将式(c)带入式(b)，式(b)可以表示为：

当

时，

当

时，

对于希望d跟踪d_T的问题等价于当t＞0时轨迹必须进入滑模面，一旦进入滑模面就不会离开。由李雅普诺夫直接方法可知，该方法能在有限的时间内到达滑模面。实际上，s等于零是一个线性微分方程，使其误差δ以指数收敛的速度逼近于零，同时滑模函数的上下界可以通过ε和ρ来调节。

上述控制器的控制效果如图3-图5，分别为位置的x轴y轴z轴分量的跟踪轨迹效果。

在本实施例中，所述步骤S3的具体内容为：

结合式(6)，令偏航角ψ_d为已知值，其中-π＜ψ_d＜π，从而输出所需要翻滚角

俯仰角θ_d；

其中，g为重力加速度；结合式(6)、(8)，求得升力u₁；

在本实施例中，所述步骤S4具体包括以下步骤：

步骤S41：令四旋翼无人机搭载机械臂系统中四旋翼无人机的三种姿态中的翻滚角子系统为第1个子系统，俯仰角子系统为第2个子系统，偏航角为第3个子系统；q为绕y轴俯仰的角速度，p为绕x轴翻滚的角速度，r为绕z轴偏航的角速度；翻滚角状态方程如下：

u₂为翻滚子系统的输入力矩，I_x、I_y和I_z是分别关于x，y和z轴的转动惯量；c₁为重心偏移控制参数；

步骤S42：通过卡尔曼滤波融合三轴惯性测量传感器，获得无人机平台当前的翻滚角

俯仰角θ，偏航角ψ；

步骤S43：定义翻滚角跟踪角度误差e₁：实时测量四旋翼无人机与期望翻滚角

的误差：

步骤S44：通过卡尔曼滤波融合三轴惯性测量传感器实时测得Ω＝[p q r]^T，其中，p为翻滚角速度，q为俯仰角速度，r为偏航角速度；

步骤S45：利用CMAC神经网络的逼近能力对惯性张量的不确定性局部逼近。通过在线学习不断修正各组高斯基函数的权值，都得到最近事实的惯性张量。

定义翻滚角跟踪角速度误差χ₁：实时测量四旋翼无人机与期望翻滚角速度p_d的误差：

χ₁＝p_d-p (12)

其中，

步骤S46：对期望翻滚角

求二阶微分得

步骤S47：惯性张量I＝[I_x,I_y,I_z]^T，其中

表示为作业型飞行机器人初始状态时的惯性张量；γ_x，γ_y，γ_z分别为作业时惯性张量与初始惯性张量在x、y和z轴的偏差；且

|γ_x|＜κ，|γ_y|＜κ，|γ_z|＜κ，κ为正标量参数；

步骤S48：在翻滚角子系统下，重心偏移控制参数的估计值

作业时惯性张量与初始惯性张量的偏差估计

为式(13)至(16)，用以使李雅普诺夫函数正定，一阶微分李雅普诺夫半负定；

其中，t为时间，m_s为系统总质量，ξ₁为正标量参数，

为翻滚角速度一阶微分；

步骤S49：通过俯仰角子系统和偏航角子系统，估计出另外两组惯性张量和重心偏移系统参数，表示如下

其中，

χ₂＝p_d-p，χ₂为俯仰角速度误差；χ₃＝r_d-r，χ₃为偏航角速度误差；在三个子系统中估计出三组惯性张量

为作业型飞行机器人没有抓取目标物在初始状态时的惯性张量；

步骤S410：把式(17)(18)(19)作为CMAC神经网络的输入，网络的高斯基函数为：

其中，h(x)＝[h(x_i)]^T，h(y)＝[h(y_i)]^T，h(z)＝[h(z_i)]^T，h(x_i)为惯性张量在x轴分量的i个高斯基函数输入；a_x,i，a_y,i，a_z,i为惯性张量的i个高斯基函数输入的中心值；b_x,i，b_y,i，b_z,i为惯性张量的i个高斯基函数输入惯性张量的宽度；

步骤S411：CMAC神经网络的权值估计值为：

其中，

为

的权值，

为

的权值，

为

的权值；

为第i个子系统估计的第i组权值函数，i＝1,2,3，分别为

k₁、k₂、k₃为正标量参数；

步骤S412：综上，力矩u₂为：

其中，存在正常数μ₁，使得外部扰动|Δ₁|＜μ₁；

为权值函数累计误差的上限，即

ο₁≥ι₁≥0，ο₁和ι₁都为正定常数，α＝min(k₁,k₂)；

步骤S413：俯仰角状态方程如下：

u₃为翻滚角子系统的输入力矩，I_x、I_y和I_z是分别关于x，y和z轴的转动惯量；c₂为重心偏移控制参数；

步骤S414：定义跟踪误差e₂：实时测量四旋翼无人机与期望俯仰角θ_d的误差：

步骤S415：定义俯仰角速度的期望轨迹为：

步骤S416：在俯仰角子系统下的重心偏移控制参数的估计值

为：

其中，ξ₂为正标量参数。

步骤S417：综上，翻滚力矩u₃：

步骤S418：偏航角状态方程如下：

u₄为翻滚角子系统的输入力矩，I_x、I_y和I_z是分别关于x，y和z轴的转动惯量；c₃为重心偏移控制参数；

步骤S419：定义跟踪误差e₃：实时测量四旋翼无人机与期望偏航角ψ_d的误差：

步骤S420：定义偏航角速度的期望轨迹为：

步骤S421：在偏航角子系统下的重心偏移控制参数的估计值

为：

其中，ξ₃为正标量参数。

步骤S422：综上，偏航力矩u₄：

同翻滚的输出一样，k_i,i＝3,4,5,6为正标量参数。

在本实施例中，所述步骤S5的具体内容为：所述四个旋翼的角速度与输入u_i,i＝1,2,3,4的关系为：

u₁＝C₁(ω₁ ²+ω₂ ²+ω₃ ²+ω₄ ²)

u₂＝C₁(-ω₂ ²+ω₄ ²),u₃＝C₁(-ω₁ ²+ω₃ ²)

u₄＝C₂(ω₁ ²-ω₂ ²+ω₃ ²-ω₄ ²)

(37)

其中，u₁为通过升力、u₂为翻滚力矩、u₃为俯仰力矩、u₄为偏航力矩；正标量参数C₁、C₂定义为升力系数和扭矩系数；进而解算出四个旋翼的转速ω_ι，i＝1，2,3,4。即联立升力u₁、翻滚力矩u₂、俯仰力矩u₃、偏航力矩u₄解算出四个旋翼的的转速ω_ι，i＝1，2,3,4。

较佳的，在本实施例中，以翻滚角子系统为例证明重心偏移控制参数c₁和惯性张量I估计的准确性，以及本发明设计的控制器可以使姿态跟踪误差在有限的时间内逼近于零。

设计状态观测器有：

其中，

和Y₁ ^*均为正标量参数。在该子系统下，

其中，～表示为估计误差。接下来证明估计的准确性。定义李雅普诺夫函数V₂：

李雅普诺夫函数V₂对时间的一阶导有：

其中，

将上式带入式，则有：

李雅普诺夫函数V₂正定，而一阶微分李雅普诺夫函数

只是半负定，不能证明随着时间的推移误差χ₁趋于零。引入Barbalat引理作类李雅普诺夫稳定性分析证明控制器的有效性，证明如下：

因为

可微且有上界，

连续，所以时间t趋于无穷时，

解得χ₁＝0。综上，该自适应律可以使系统达到渐进稳定，当且仅当k₁、k₂、ξ₁为正标量参数。

分析翻滚角的状态方程发现，要使

追踪期望的轨迹

可以改变系统的输入翻滚力矩u₂来控制翻滚角速度p，再通过控制p反向控制

期望翻滚角速度p_d

设计李雅普诺夫函数：

一阶微分新李雅普诺夫函数

为：

其中，

为理想权值，

的估计值，

为理想权值。h(x)，h(y)，h(z)是惯性张量分别在x，y，z轴分量的高斯基函数输出。

权值函数的自适应律为：

其中，存在正标量参数μ₁，使得外部扰动|Δ₁|＜μ₁。

为权值函数累计误差的上限，即

ο₁为正标量参数。

将式(48)(49)(50)带入式(47)，则式(47)有：

当k_i＞0(i＝1,…,6)时，满足李雅普诺夫稳定性条件。其中，α＝min(k₁,k₂)。李雅普诺夫函数V₄集中了未知重心偏移控制参数和惯性张量的误差，使得系统存在稳态误差。当时间t→∞，

较佳的，在本实施例中，上述控制器控制效果如图6-图8。

下面用具体的应用实例对操作进行详细说明，本实施例的基于考虑惯性张量不为常数和重心偏移参数的系统模型进行位置和姿态控制器设计，将分别从位置控制器和姿态控制器两个方面来证明其有效性。具体设置如下：

①模拟时长100s模拟。为了增加模拟环境的多样性，体现本发明的有效性。在该过程中，给定机械臂各关节一个运动规划：

②抓取目标物体后，给定期望轨迹使作业型飞行机器人带着目标物按期望轨迹飞行。无人机的初始位置为d＝[0 0 0]^T，期望轨迹初始位置d_T＝[0 0 0]^T。在重心不断改变的情况下，给定无人的期望轨迹d_T如下：

硬件参数如表1所示。

表1硬件参数

对于位置控制器，各部分仿真参数如下：

翻滚，俯仰，偏航，三个姿态稳定控制参数相等，均为k_ι＝3(i＝1，…,4)，μ_ι＝0.2(i＝1，2,3)，ι_ι＝0.2(i＝1，2,3)，ξ_ι＝0.8(i＝1，2,3)。仿真时长为100s。仿真补偿固定为0.01s在整个仿真中始终固定偏航角为0弧度。

图3-图5是位置控制器的控制效果。本实施例设计的位置控制器可以使作业型飞行机器人位置的各分量在小波动下追踪到目标轨迹。之后，以较小的稳态误差使作业型飞行机器人按期望轨迹运动。图6-图8是姿态控制器的控制效果。当位置控制器补偿了重心偏移系统参数提高系统模型精度时，将得到一条不平滑的期望姿态轨迹。跟踪一条不平滑的轨迹就对姿态控制器的性能有了更高的要求。可以看出本实施例设计的姿态控制器在1s内跟踪到了不平滑的期望轨迹,并且在0.01rad的稳态误差内。图3-图8证明了本实施例的有效性。

较佳的，本实施例位姿控制器分为两部分设计，一部分为位置控制器，引入滑模控制器；一部分为姿态控制器，引入自适应反演控制器。将不从外部补偿重心偏移去考虑问题。而对系统进行完善，从系统建模的角度将重心偏移引起的时变参数和惯性张量不为常数列为本实施例的研究对象，时变参数分为重心偏移系统参数和重心偏移控制参数，惯性张量视为在初始惯性张量上浮动。(1)在姿态解耦时考虑重心偏移系统参数并设计位置控制器补偿重心偏移系统参数，引入一个中间变量表示补偿重心偏移系统参数的状态量。在位置控制设计中引用滑模控制，将传统滑模控制的符号函数改用饱和函数和鲁棒项，以减少在滑模面上振动的频率。以此解决重心偏移对位置稳定不足问题。(2)姿态控制的设计，在自适应反演控制器中加入CMAC神经网络。利用反演控制实现基本的姿态控制。在此基础上，加入自适应律对重心偏移控制参数以及惯性张量进行估计，并加入CMAC神经网络在线逼近惯性张量的真实值。以此解决姿态控制律中时变参数未知的问题以及姿态控制稳定不足的问题。

以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (6)

1.一种作业型飞行机器人抓取后的位姿稳定控制方法，提供一四旋翼无人机搭载机械臂系统，其特征在于：包括以下步骤：

步骤S1：利用牛顿-欧拉方程法对四旋翼无人机搭载机械臂系统建模，不忽略重心偏移造成的干扰，惯性张量不为常数；

步骤S2：在步骤S1建立的模型的基础上，基于滑模控制设计位置控制器用以进行位置稳定跟踪，用以补偿重心偏移系统参数，使四旋翼无人机按期望轨迹d_T飞行；

步骤S3：进行姿态解耦：解算出四旋翼无人机按期望轨迹d_T飞行所需的期望翻滚角

期望俯仰角θ_d和升力u₁；

步骤S4：在步骤S1建立的模型的基础上，基于滑模控制设计姿态控制器用以进行姿态稳定跟踪，并考虑重心偏移控制参数和惯性张量不为常数，姿态控制器中加入自适应去估计重心偏移控制参数和惯性张量，并加入CMAC神经网络在线逼近惯性张量的真实值，用以提高姿态控制精度，并解算出所需的输入力矩u_i，i＝2,3,4，u₂为翻滚角子系统的输入力矩，u₃为俯仰角子系统的输入力矩，u₄为偏航角子系统的输入力矩；

步骤S5：通过升力u₁、翻滚力矩u₂、俯仰力矩u₃、偏航力矩u₄联立解算出四个旋翼的转速ω_i，i＝1，2,3,4；所述四旋翼无人机搭载机械臂系统通过解算出四个旋翼的转速控制无人机飞行，用以实现位姿稳定控制。

2.根据权利要求1所述的一种作业型飞行机器人抓取后的位姿稳定控制方法，其特征在于：所述步骤S1具体包括以下内容：

对四旋翼飞行机械臂系统建模，根据力平衡和力矩平衡得到：

其中，F为系统所受的外力；M为系统所受的外力矩；m_s为系统总质量；r'为作业型飞行机器人重心在飞行平台坐标系内的位置；r₀为飞行平台在世界坐标系的位置；B为系统的推动力；Ω是无人机平台在世界坐标系的角速度矢量；I为系统的惯性张量；

表示为Ω进行一次微分，

表示为对r₀的二次微分。

3.根据权利要求1所述的一种作业型飞行机器人抓取后的位姿稳定控制方法，其特征在于：所述步骤S2具体包括以下步骤：

步骤S21：在四旋翼无人机搭载机械臂系统中输入在惯性坐标的期望轨迹d_T＝[x_T y_Tz_T]^T；对期望轨迹求二阶微分，求解出期望加速度

步骤S22：利用卡尔曼滤波融合超宽带定位技术测量四旋翼无人机平台在世界坐标系内的位置，获取当前四旋翼无人机平台位置信息d＝[x y z]^T；

步骤S23：令跟踪误差向量为δ；实时测量四旋翼无人机平台的与期望轨迹的误差；

δ＝d_T-d,δ∈R³ (2)

步骤S24：用一个一阶滑模函数s，定义状态空间中的时变曲面：

步骤S25：姿态解耦考虑重心偏移系统参数h，用以提高四旋翼无人机搭载机械臂系统建模精度，h＝[h₁,h₂,h₃]^T，h₁表示重心偏移系统参数在x轴分量，h₂表示重心偏移系统参数在y轴分量，h₃表示重心偏移系统参数在z轴分量；h有界，存在正标量参数ρ，有|h|≤ρ；

步骤S26：定义一个饱和函数，饱和函数表示为：

其中，存在正标量参数ε和β，有0＜ε＜1，β＞0；

步骤S27：综上，位置控制器为：

其中，鲁棒项v为：

其中，η为中间变量表示为

η＝[η₁，η₂，η₃]^T。

4.根据权利要求3所述的一种作业型飞行机器人抓取后的位姿稳定控制方法，其特征在于：所述步骤S3的具体内容为：

结合式(6)，令偏航角为ψ_d，其中-π＜ψ_d＜π，从而输出所需要翻滚角

俯仰角θ_d；

其中，g为重力加速度；结合式(6)、(8)，求得升力u₁；

5.根据权利要求3所述的一种作业型飞行机器人抓取后的位姿稳定控制方法，其特征在于：所述步骤S4具体包括以下步骤：

步骤S41：令四旋翼无人机搭载机械臂系统中四旋翼无人机的三种姿态中的翻滚角子系统为第1个子系统，俯仰角子系统为第2个子系统，偏航角为第3个子系统；q为绕y轴俯仰的角速度，p为绕x轴翻滚的角速度，r为绕z轴偏航的角速度；翻滚角状态方程如下：

u₂为翻滚子系统的输入力矩，I_x、I_y和I_z是分别关于x，y和z轴的转动惯量；c₁为重心偏移控制参数；

步骤S42：通过卡尔曼滤波融合三轴惯性测量传感器，获得无人机平台当前的翻滚角

俯仰角θ，偏航角ψ；

步骤S43：定义翻滚角跟踪角度误差e₁：实时测量四旋翼无人机与期望翻滚角

的误差：

步骤S44：通过卡尔曼滤波融合三轴惯性测量传感器实时测得Ω＝[p q r]^T，其中，p为翻滚角速度，q为俯仰角速度，r为偏航角速度；

步骤S45：定义翻滚角跟踪角速度误差χ₁：实时测量四旋翼无人机与期望翻滚角速度p_d的误差：

χ₁＝p_d-p (12)

其中，

步骤S46：对期望翻滚角

求二阶微分得

步骤S47：惯性张量I＝[I_x,I_y,I_z]^T，其中

表示为作业型飞行机器人初始状态时的惯性张量；γ_x，γ_y，γ_z分别为作业时惯性张量与初始惯性张量在x、y和z轴的偏差；且

|γ_x|＜κ，|γ_y|＜κ，|γ_z|＜κ，κ为正标量参数；

步骤S48：在翻滚角子系统下，重心偏移控制参数的估计值

作业时惯性张量与初始惯性张量的偏差估计

为式(13)至(16)，用以使李雅普诺夫函数正定，一阶微分李雅普诺夫半负定；

其中，t为时间，m_s为系统总质量，ξ₁为正标量参数，

为翻滚角速度一阶微分；

步骤S49：通过俯仰角子系统和偏航角子系统，估计出另外两组惯性张量和重心偏移系统参数，表示如下：

其中，

χ₂＝p_d-p，χ₂为俯仰角速度误差；χ₃＝r_d-r，χ₃为偏航角速度误差；在三个子系统中估计出三组惯性张量

为作业型飞行机器人没有抓取目标物在初始状态时的惯性张量；

步骤S410：把式(17)(18)(19)作为CMAC神经网络的输入，网络的高斯基函数为：

其中，h(x)＝[h(x_i)]^T，h(y)＝[h(y_i)]^T，h(z)＝[h(z_i)]^T，h(x_i)为惯性张量在x轴分量的i个高斯基函数输入；a_x,i，a_y,i，a_z,i为惯性张量的i个高斯基函数输入的中心值；b_x,i，b_y,i，b_z,i为惯性张量的i个高斯基函数输入惯性张量的宽度；

步骤S411：CMAC神经网络的权值估计值为：

其中，

为

的权值，

为

的权值，

为

的权值；

为第i个子系统估计的第i组权值函数，i＝1,2,3，分别为

k₁、k₂、k₃为正标量参数；

步骤S412：综上，力矩u₂为：

其中，存在正标量参数μ₁，使得外部扰动|Δ₁|＜μ₁；

为权值函数累计误差的上限，即

ο₁≥ι₁≥0，ο₁和ι₁都为正标量参数，α＝min(k₁,k₂)；

步骤S413：俯仰角状态方程如下：

u₃为翻滚角子系统的输入力矩，I_x、I_y和I_z是分别关于x，y和z轴的转动惯量；c₂为重心偏移控制参数；

步骤S414：定义跟踪误差e₂：实时测量四旋翼无人机与期望俯仰角θ_d的误差：

步骤S415：定义俯仰角速度的期望轨迹为：

步骤S416：在俯仰角子系统下的重心偏移控制参数的估计值

为：

其中，ξ₂为正标量参数；

步骤S417：综上，翻滚力矩u₃：

步骤S418：偏航角状态方程如下：

u₄为翻滚角子系统的输入力矩，I_x、I_y和I_z是分别关于x，y和z轴的转动惯量；c₃为重心偏移控制参数；

步骤S419：定义跟踪误差e₃：实时测量四旋翼无人机与期望偏航角ψ_d的误差：

步骤S420：定义偏航角速度的期望轨迹为：

步骤S421：在偏航角子系统下的重心偏移控制参数的估计值

为：

其中，ξ₃为正标量参数。

步骤S422：综上，偏航力矩u₄：

k_i,i＝3,4,5,6为正标量参数。

6.根据权利要求3所述的一种作业型飞行机器人抓取后的位姿稳定控制方法，其特征在于：所述步骤S5的具体内容为：所述四个旋翼的角速度与输入u_i,i＝1,2,3,4的关系为：

u₁＝C₁(ω₁ ²+ω₂ ²+ω₃ ²+ω₄ ²)

u₂＝C₁(-ω₂ ²+ω₄ ²),u₃＝C₁(-ω₁ ²+ω₃ ²)

u₄＝C₂(ω₁ ²-ω₂ ²+ω₃ ²-ω₄ ²)

(37)

其中，u₁为通过升力、u₂为翻滚力矩、u₃为俯仰力矩、u₄为偏航力矩；正标量参数C₁、C₂定义为升力系数和扭矩系数；进而解算出四个旋翼的转速ω_ι，i＝1，2,3,4。

Images