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

技术特征：

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

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

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

步骤s3：进行姿态解耦：解算出四旋翼无人机按期望轨迹dt飞行所需的期望翻滚角期望俯仰角θd和升力u1；

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

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

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

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

其中，f为系统所受的外力；m为系统所受的外力矩；ms为系统总质量；r'为作业型飞行机器人重心在飞行平台坐标系内的位置；r0为飞行平台在世界坐标系的位置；b为系统的推动力；ω是无人机平台在世界坐标系的角速度矢量；i为系统的惯性张量；表示为ω进行一次微分，表示为对r0的二次微分。

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

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

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

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

δ＝dt-d,δ∈r³(2)

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

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

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

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

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

其中，鲁棒项v为：

其中，η为中间变量表示为η＝[η1，η2，η3]^t。

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

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

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

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

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

u2为翻滚子系统的输入力矩，ix、iy和iz是分别关于x，y和z轴的转动惯量；c1为重心偏移控制参数；

步骤s42：通过卡尔曼滤波融合三轴惯性测量传感器，获得无人机平台当前的翻滚角俯仰角θ，偏航角ψ；

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

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

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

χ1＝pd-p(12)

其中，

步骤s46：对期望翻滚角求二阶微分得

步骤s47：惯性张量i＝[ix,iy,iz]^t，其中表示为作业型飞行机器人初始状态时的惯性张量；γx，γy，γz分别为作业时惯性张量与初始惯性张量在x、y和z轴的偏差；且|γx|＜κ，|γy|＜κ，|γz|＜κ，κ为正标量参数；

步骤s48：在翻滚角子系统下，重心偏移控制参数的估计值作业时惯性张量与初始惯性张量的偏差估计为式(13)至(16)，用以使李雅普诺夫函数正定，一阶微分李雅普诺夫半负定；

其中，t为时间，ms为系统总质量，ξ1为正标量参数，为翻滚角速度一阶微分；

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

其中，χ2＝pd-p，χ2为俯仰角速度误差；χ3＝rd-r，χ3为偏航角速度误差；在三个子系统中估计出三组惯性张量为作业型飞行机器人没有抓取目标物在初始状态时的惯性张量；

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

其中，h(x)＝[h(xi)]^t，h(y)＝[h(yi)]^t，h(z)＝[h(zi)]^t，h(xi)为惯性张量在x轴分量的i个高斯基函数输入；ax,i，ay,i，az,i为惯性张量的i个高斯基函数输入的中心值；bx,i，by,i，bz,i为惯性张量的i个高斯基函数输入惯性张量的宽度；

步骤s411：cmac神经网络的权值估计值为：

其中，为的权值，为的权值，为的权值；为第i个子系统估计的第i组权值函数，i＝1,2,3，分别为k1、k2、k3为正标量参数；

步骤s412：综上，力矩u2为：

其中，存在正标量参数μ1，使得外部扰动|δ1|＜μ1；为权值函数累计误差的上限，即ο1≥ι1≥0，ο1和ι1都为正标量参数，α＝min(k1,k2)；

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

u3为翻滚角子系统的输入力矩，ix、iy和iz是分别关于x，y和z轴的转动惯量；c2为重心偏移控制参数；

步骤s414：定义跟踪误差e2：实时测量四旋翼无人机与期望俯仰角θd的误差：

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

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

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

步骤s417：综上，翻滚力矩u3：

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

u4为翻滚角子系统的输入力矩，ix、iy和iz是分别关于x，y和z轴的转动惯量；c3为重心偏移控制参数；

步骤s419：定义跟踪误差e3：实时测量四旋翼无人机与期望偏航角ψd的误差：

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

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

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

步骤s422：综上，偏航力矩u4：

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

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

u1＝c1(ω1²+ω2²+ω3²+ω4²)

u2＝c1(-ω2²+ω4²),u3＝c1(-ω1²+ω3²)

u4＝c2(ω1²-ω2²+ω3²-ω4²)

(37)

其中，u1为通过升力、u2为翻滚力矩、u3为俯仰力矩、u4为偏航力矩；正标量参数c1、c2定义为升力系数和扭矩系数；进而解算出四个旋翼的转速ωι，i＝1，2,3,4。

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

技术研发人员：陈彦杰;占巍巍;黄益斌;梁嘉诚;赖宁斌;何炳蔚;林立雄
受保护的技术使用者：福州大学
技术研发日：2020.01.13
技术公布日：2020.05.22