CN108638068B - 一种携带冗余度机械臂的飞行机器人控制系统设计方法 - Google Patents

Abstract

本发明公开了一种携带冗余度机械臂的飞行机器人控制系统设计方法，针对六旋翼飞行器自适应稳定飞行控制器和冗余度机械臂控制器两方面的设计，主要包括下述步骤：根据飞行实时运行数据，对携带冗余度机械臂的飞行机器人进行动力学建模，然后根据建模方程，设计位置、高度和姿态控制器，并且采用基于李雅普诺夫稳定性原理的后推方法，进行轨迹跟踪处理，然后再进行自适应处理，即可完成飞行器自适应稳定飞行控制器的设计；对于冗余度机械臂运动规划控制器，采用机械臂运动规划方案设计和二次规划设计。本发明方法的自适应后推设计方法，可快速、准确、实时跟踪飞行器的目标轨迹，并提高了系统的鲁棒性和稳定性。

Description

一种携带冗余度机械臂的飞行机器人控制系统设计方法

技术领域

本发明属于飞行机器人控制系统设计领域，特别涉及一种基于自适应后推设计的携带冗余度机械臂的飞行机器人控制系统设计方法。

背景技术

近年来世界各国都在大力发展多旋翼飞行器，无论是在军事方面还是在民用方面都得到了广泛的应用。相比于传统的飞行器而言，多旋翼飞行器具有体积较小、成本较低、机动性强、可定点悬停、可垂直起降，各种姿态的飞行等优势，可高效益地在军事上用于侦察和监视等和在民用方面用于电力巡线等。随着无人机技术的迅猛发展，雷达、激光、声纳等仪器设备都可装备到无人机上，用于完成特定的任务，而随着机器人科学和机械臂技术的进步，具备冗余度机械臂的机器人具有可协调执行机械臂复杂任务、躲避关节极限、躲避奇异点，并且具有一定的容错特性，被广泛应在在各行各业，如家用服务型机器人，可以帮助人们完成一些琐碎的工作；或者工业用机械手等，有效地提高了生产精度和生产速率。由于实际应用环境复杂性和任务条件的多样化的影响，基于PID闭环控制算法以及相应的改进控制算法进行设计的传统的无人机控制器逐渐难以满足人们的期望。因此，我们需要设计性能更加良好的控制器，而基于自适应后推方法的设计方法是非线性系统控制器设计最常用的方法之一，是将系统李雅普诺夫函数的选取与控制器的设计相结合的一种回归设计方法，可以有效地减少系统的稳态误差和加快调节速度，从而使得被控量快速跟踪目标值，实现轨迹跟踪控制，同时，自适应控制的引用能有效地消除模型的参数不确定性所带来的影响，极大提高系统的鲁棒性。

发明内容

本发明的目的在于克服现有技术中的缺点与不足，提供一种携带冗余度机械臂的飞行机器人控制系统设计方法，包括飞行器自适应稳定飞行控制器和冗余度机械臂控制器两方面的设计，采用基于李雅普诺夫稳定性原理的后推方法，计算飞行器电机控制量，能够快速、准确、实时跟踪飞行器的目标轨迹，并提高了系统的鲁棒性和稳定性。

为实现以上目的，本发明采取如下技术方案：

一种携带冗余度机械臂的飞行机器人控制系统设计方法，是针对六旋翼飞行器自适应稳定飞行控制器和冗余度机械臂控制器两方面的设计，该方法包括如下步骤：

S1、根据飞行机器人上所搭载的姿态传感器以及相应的高度与位置传感器所获取到的飞行机器人自身的飞行实时运行数据，对携带冗余度机械臂的飞行机器人进行动力学建模；

S2、根据步骤S1建立的飞行机器人动力学方程，设计水平位置控制器、高度控制器、以及姿态控制器，从系统最低阶次微分方程开始引入虚拟控制量，采用基于李雅普诺夫稳定性原理的后推方法，设计满足系统稳定要求和误差收敛要求的虚拟控制，从而确定各个控制器的虚拟控制量；

S3、根据步骤S2获得的各个控制器的控制量，并通过选取相应的李雅普诺夫函数，求解扰动自适应率，然后对飞行机器人质量进行在线估计，采用基于李雅普诺夫稳定性原理的后推方法，确定质量自适应率，即完成飞行机器人自适应稳定飞行控制器的设计；

S4、根据冗余度机械臂控制器的设计需求，设计冗余度机械臂运动规划控制器，具体包括机械臂运动规划方案设计和二次规划设计，通过冗余度机械臂逆运动学实现冗余度机械臂的运动规划方案设计，再根据不同的设计目的和指标要求，将冗余度机械臂逆运动学问题转换为受约束的时变凸二次规划问题，并将求解结果作为机械臂的关节角状态，控制飞行机器人完成相应的控制任务。

作为优选的技术方案，步骤S1中，对携带冗余度机械臂的飞行机器人进行动力学建模，所述飞行机器人采用六旋翼飞行器，其动力学方程如下：

且

其中

为对应x_i的导数，x₁、x₃、x₅分别表示飞行机器人在世界坐标系中的位置分量即坐标x、y、z；x₂、x₄、x₆分别为飞行机器人在世界坐标系中的速度分量即坐标v_x、v_y、v_z；x₇、x₉、x₁₁分别表示横滚角φ、俯仰角θ、以及偏航角ψ；x₈、x₁₀、x₁₂分别为横滚角角速度

俯仰角角速度

以及偏航角角速度

m为飞行机器人的质量；g表示重力加速度；D_x、D_y、D_z分别表示X、Y、Z轴系的扰动；U₁表示高度控制器的控制量，U₂、U₃、U₄分别表示姿态控制器的横滚角φ、俯仰角θ、以及偏航角ψ的控制量；

其中I_x、I_y、I_z分别为飞行机器人在X轴、Y轴、Z轴的转动惯量，标量b表示六旋翼飞行器机臂臂长；u_x，u_y分别表示位置控制器的X轴和Y轴的二阶虚拟控制量；

作为优选的技术方案，步骤S2，具体包括下述步骤：

S21、设计水平位置控制器，确定相应的虚拟控制量，并将虚拟控制量转换电机控制量；

S211、针对水平位置控制器的X轴控制，由X轴状态方程假设一阶系统

在水平位置层上定义一阶跟踪误差z₁＝x_1d-x₁，其中x_1d为目标X轴方向位置，x₁为实际X轴方向位置；为实现位置x₁的轨迹跟踪，需要使得一阶跟踪误差收敛至0，采用基于李雅普诺夫稳定性原理的积分型后推设计方法：引入一阶跟踪误差的积分

t表示当前时间，并定义李雅普诺夫函数

λ₁为X轴的一阶跟踪误差的收敛率，对其求导可得

表示世界坐标系下X轴目标位移变化率，根据李雅普诺夫稳定性定理，要使得X轴的一阶系统是稳定的，取X轴的一阶虚拟控制量

β₁为常量，β₁＞0；

针对水平位置控制器的X轴二阶虚拟控制系统

设

为X轴扰动的在线估计值，定义

为扰动实际值与在线估计值的误差，即

且

要使估计误差

收敛至0，采用基于李雅普诺夫稳定性原理的积分型后推设计方法，定义二阶跟踪误差和李雅普诺夫函数，所述二阶跟踪误差为

其中

所述李雅普诺夫函数为

然后再对李雅普诺夫函数求导，并消除估值误差变量影响，求得

其中u_x4为消除估值误差变量操作中与u_x相关的控制量，当控制量

μ_x为常量，μ_x＞0，使得

X轴的二阶系统是稳定的，并解得X轴的二阶虚拟控制量：

其中，

表示世界坐标系下X轴目标速度变化率；

S212、针对水平位置控制器的Y轴控制，采用与步骤S211相同的原理，由Y轴状态方程假设一阶系统

然后定义一阶跟踪误差z₃＝x_3d-x₃，其中x_3d表示目标Y轴方向位置，x₃表示实际Y轴方向位置；采用基于李雅普诺夫稳定性原理的积分型后推设计方法，求得一阶虚拟控制量

β₃为常量，β₃＞0，λ₂表示Y轴的一阶跟踪误差收敛率，η₃为一阶跟踪误差z₃在当前时间t上的积分，

为世界坐标系下Y轴目标位置变化率；对于Y轴的二阶系统

根据基于李雅普诺夫稳定性原理的积分型后推设计方法，定义二阶跟踪误差z₄＝v₃-x₄和李雅普诺夫函数

对李雅普诺夫函数进行求导运算，使得

μ_y为常量，μ_y＞0，根据李雅普诺夫稳定性定理，确定Y轴的二阶系统是稳定的，解得Y轴的二阶虚拟控制量：

其中，

为世界坐标系下Y轴目标速度变化率；

S213、在飞行控制系统中，二阶虚拟控制量u_x和u_y不能直接作为电机控制量，需要经过反解计算得到目标横滚角φ_d、目标俯仰角θ_d，并输入到姿态控制器中；由公式

经过反解运算求得：

S22、根据步骤S211中的原理，针对高度控制器，由高度Z轴状态方程假设一阶系统

定义Z轴的一阶跟踪误差z₅＝x_5d-x₅，其中，x_5d表示目标Z轴方向高度，x₅表示实际Z轴方向位置；然后采用基于李雅普诺夫稳定性原理的后推设计方法，求得Z轴的一阶虚拟控制量

β₅为常量，β₅＞0，

为世界坐标系下Y轴目标位置变化率；对于Z轴的二阶系统

针对其扰动估计误差，定义二阶跟踪误差和相应的李雅普诺夫函数，求得Z轴的二阶虚拟控制量：

其中β₆为常量，β₆＞0；

为Z轴扰动的在线估计值；

为世界坐标系下z轴目标速度变化率；z₆为Z轴的二阶跟踪误差，

且

S23、设计姿态控制器，并求解相应的控制量；所述姿态控制器用于对横滚角、俯仰角、以及偏航角的控制；

S231、针对姿态控制器，由横滚角状态方程假设一阶系统

在姿态层上定义横滚角的一阶跟踪误差z₇＝x_7d-x₇，其中x_7d为式(3)求得的目标横滚角φ_d，x₇为实际横滚角，然后采用基于李雅普诺夫稳定性原理的积分型后推设计方法，求得横滚角的一阶虚拟控制量

β₇为常量，β₇＞0，λ₇为横滚角的一阶跟踪误差收敛率，λ₇＞0，η₇表示横滚角的一阶跟踪误差z₇在时间t上的积分；针对横滚角的二阶系统

根据基于李雅普诺夫稳定性原理的积分型后推设计方法，求得横滚角的二阶虚拟控制量：

其中β₈为常量，β₈＞0，z₈为横滚角的二阶跟踪误差，

且

为目标横滚角角速度；

S232、由俯仰角状态方程假设一阶系统

定义俯仰角的一阶跟踪误差z₉＝x_9d-x₉，其中x_9d为式(3)求得的目标俯仰角θ_d，x₉为实际俯仰角，然后采用基于李雅普诺夫稳定性原理的积分型后推设计方法，求得俯仰角的一阶虚拟控制量

β₉为常量，β₉＞0，λ₉为俯仰角的一阶跟踪误差收敛率，λ₉＞0，η₉表示俯仰角的一阶跟踪误差z₉在当前时间t上的积分；针对俯仰角的二阶系统

根据基于李雅普诺夫稳定性原理的积分型后推设计方法，求得俯仰角的二阶虚拟控制量：

其中β₁₀为常量，β₁₀＞0，z₁₀表示俯仰角的二阶跟踪误差，z₁₀＝v₉-x₁₀；

S233、由偏航角状态方程假设偏航角的一阶系统

定义一阶跟踪误差z₁₁＝x_11d-x₁₁，采用基于李雅普诺夫稳定性原理的积分型后推设计方法，求得偏航角的一阶虚拟控制量

β₁₁为常量，β₁₁＞0，λ₁₁为偏航角的一阶跟踪误差收敛率，λ₁₁＞0；针对偏航角的二阶系统

根据基于李雅普诺夫稳定性原理的积分型后推设计方法，求得偏航角的二阶虚拟控制量：

其中β₁₂为常量，β₁₂＞0，z₁₂表示偏航角的二阶跟踪误差，z₁₂＝v₁₁-x₁₂；

S24、将求解的各二阶虚拟控制量U₁～U₄根据不同旋翼飞行机器人的结构以及电机数目进行不同的输出控制分配。

作为优选的技术方案，步骤S3中，具体包括下述步骤：

S31、针对X轴扰动，令

其中σ_x1、σ_x2、σ_x3为任意非负常量，则X轴的扰动自适应率

其中κ_x为任意非负常量，同时式(1)

选取

则式(1)选取为：

通过式(8)，可求解出σ_x1η₁+σ_x2z₁+σ_x3z₂，则最终求得如下X轴的扰动自适应率：

S32、针对Y轴扰动，采用与步骤S31相同的原理，令

其中σ_y1、σ_y2、σ_y3为任意非负常量，则Y轴的扰动自适应率

κ_y为任意非负常量；将式(2)选取为:

通过式(10)，可求解出σ_y1η₃+σ_y2z₃+σ_y3z₄，则最终求得如下的Y轴的扰动自适应率：

S33、针对Z轴扰动，将Z轴的二阶虚拟控制量

β₆＞0代入Z轴的李雅普诺夫函数的导函数

则

即当

时，可确定

恒小于0，根据李雅普诺夫稳定性原理，Z轴估计扰动量较快收敛于实际扰动量，进行准确的在线估计，故最终求得Z轴扰动自适应率：

其中，κ_z＞0，κ_z为Z轴的二阶跟踪误差收敛率；

S34、在不考虑飞行机器人系统扰动的情况下，即D_z＝0，

飞行机器人系统质量的不确定性只对高度Z轴的运动产生影响，设

为飞行机器人质量的在线估计值，定义

为质量实际值与估计值的误差，即

且

其中a为常数；为使得质量估计误差的最小化，需要使得

收敛至0，定义包含质量估计误差变量的李雅普诺夫函数

其中λ_m＞0，λ_m为飞行机器人的质量估计误差收敛率；根据李雅普诺夫稳定性定理，

小于0，即

恒为负时，可确定该二阶系统是稳定的；化简函数

可得

将

代入化简后的函数

可得

实际上

为满足李雅普诺夫稳定性要求，质量自适应率选取为：

S35、根据步骤S31～S34求解出的自适应率对不确定因数进行在线估计，更新控制器参数来调整电机输出量。

作为优选的技术方案，所述步骤S4中的冗余度机械臂运动规划方案设计和二次规划设计，具体包括下述步骤：

S41、冗余度机械臂的运动规划方案通过冗余度机械臂逆运动学实现，其中逆运动学方程描述为：

f(θ)＝r(14)

其中，r是机械臂末端的期望轨迹，f(·)为冗余度机械臂关节角度到末端轨迹的非线性映射方程；对方程两边同时求导得到冗余度机械臂在速度层上的逆运动学方程：

其中，

为实数域上的m×n维矩阵，J(θ)为冗余度机械臂的雅克比矩阵，n表示机械臂的自由度数，m表示机械臂末端轨迹的空间维数，

和

分别为冗余度机械臂关节角度和末端轨迹关于时间的导数；

S42、根据不同的设计目的和指标要求，将逆运动学问题转换为受约束的时变凸二次规划问题，具体公式为：

s.t.Ax＝b， (17)

Cx≤d， (18)

其中Ax＝b为相应完成任务所需的等式约束，Cx≤d为不等式约束，

与

为对应关节角的双端不等式约束；根据二次规划的算法，设计神经网络求解相应的二次型最优解；根据求解出的二次型最优解作为机械臂的关节角状态，控制飞行机器人完成相应的控制任务。

本发明相对于现有技术具有如下的优点和效果：

本发明针对无人飞行器的数学模型多变量、非线性、强耦合、欠驱动的特点，基于自适应后推设计方法，该方法通过从系统最低阶次微分方程开始，引入虚拟控制量的概念，一步一步设计满足要求的虚拟控制，由于引入了积分环节，可有效地减少系统的稳态误差和加快调节速度，可以使被控量快速跟踪目标值，可以实现轨迹跟踪控制，以及通过设计质量自适应控制器和XYZ轴扰动自适应控制器，可以有效消除模型参数的不确定性影响，提高系统的稳定性和鲁棒性；通过冗余度机械臂运动规划方案设计和二次规划设计的冗余度机械臂能够较好且快速跟随期望轨迹，其躲避奇异点以及精度高的抓取特点有助于完成更为复杂多变的工作，应用范围更大，开发领域更广。

附图说明

图1为本发明的携带冗余度机械臂的飞行机器人控制系统设计总体流程图。

图2为本发明的自适应稳定飞行控制系统设计框图。

图3为本发明的携带冗余度机械臂的六旋翼飞行机器人机体坐标系图与地面参考坐标系。

图4为本发明的携带冗余度机械臂的六旋翼飞行机器人结构三维视图。

图5为本发明的携带冗余度机械臂的六旋翼飞行机器人结构俯视图。

图6为本发明的冗余度机械臂结构三维视图。

图5、图6中所示为：1-电机及其螺旋桨；2-多旋翼飞行器机架；3-飞行器控制系统组件；4-防震装置；5-舵机；6-末端执行器；7-机械零固件。

具体实施方式

为了使本发明的目的、技术方案以及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，并不限于本发明。

实施例

如图1所示，一种携带冗余度机械臂的飞行机器人控制系统设计方法，具体针对飞行器自适应稳定飞行控制器和冗余度机械臂控制器两方面的设计，包括下述步骤：

S1、根据飞行机器人上所搭载的姿态传感器以及相应的高度与位置传感器所获取到的飞行机器人自身的飞行实时运行数据，对携带冗余度机械臂的飞行机器人进行动力学建模；

S2、根据步骤S1建立的飞行机器人动力学方程，设计水平位置控制器、高度控制器、以及姿态控制器，从系统最低阶次微分方程开始引入虚拟控制量，采用基于李雅普诺夫稳定性原理的后推方法，设计满足系统稳定要求的虚拟控制，从而确定各个控制器的虚拟控制量；

S3、根据步骤S2获得的各个控制器的控制量，并通过选取相应的李雅普诺夫函数，求解扰动自适应率，然后对飞行机器人系统质量进行在线估计，采用基于李雅普诺夫稳定性原理的后推方法，确定质量自适应率，即完成飞行机器人自适应稳定飞行控制器的设计；

S4、根据冗余度机械臂控制器的设计需求，设计冗余度机械臂运动规划控制器，具体包括机械臂运动规划方案设计和二次规划设计，通过冗余度机械臂逆运动学实现冗余度机械臂的运动规划方案设计，再根据不同的设计目的和指标要求，将冗余度机械臂逆运动学问题转换为受约束的时变凸二次规划问题，并将求解结果作为机械臂的关节角状态，控制飞行机器人完成相应的控制任务。

如图2所示为本实施例飞行机器人飞行控制系统设计框图。采用自适应后推方法设计负反馈闭环回路；根据反馈回的实时姿态位置高度数据，通过递推参数估计器和控制器参数计算器的求解运算，对相应的控制器的参数进行在线估计更新，并经姿态位置高度控制器执行相应的求解运算后，输出电机控制量，最终实现飞行机器人的稳定飞行。

如图3所示为携带冗余度机械臂的六旋翼飞行机器人所在的机体坐标系与地面参考坐标系示意图。根据机体坐标系做出如下定义：

(1)、按照逆时针方向定义六旋翼飞行器六个电机分别为①号到⑥号；

(2)、X轴沿①号旋翼臂方向，通过机体重心指向飞行器前进方向；

(3)、Y轴沿②、③号旋翼臂的对称轴方向，通过机体重心指向飞行器左侧运动方向；

(4)、Z轴垂直于六旋翼平面向上，通过机体重心指向飞行器爬升方向；

(5)、俯仰角θ为机体X轴与大地水平面间所夹角度，设定向下为正；

(6)、横滚角φ为机体Z轴与过机体X轴的大地竖直平面之间的夹角，飞机右倾为正；

(7)、偏航角ψ为机体X轴在大地水平面上的投影与大地坐标系中X轴之间所夹角度，机头向左为正。

如图4所示为装载冗余度机械臂的六旋翼飞行机器人模型。其中六旋翼飞行器由图5中的防震装置4和六组由电机及其螺旋桨1、多旋翼飞行器机架2、飞行器控制系统组件3构成的旋翼所组成；冗余度机械臂部分由图6中舵机5、末端执行器6以及相应的机械零固件7以及相应的组成冗余度机械臂控制系统组件组成。

本实施例中，根据飞行机器人上所搭载的姿态传感器以及相应的高度与位置传感器所获取到的飞行机器人自身的飞行实时运行数据，对携带冗余度机械臂的飞行机器人进行动力学建模，并通过飞行机器人所搭载的自适应稳定飞行处理器的求解运算来调整电机控制量；由相关的传感器反馈机械臂每个关节的旋转角度以及姿态位置信息，将所需执行冗余度机械臂任务，将冗余度机械臂的运动任务转化为二次型，转化为运动规划求解问题，对相应的运动规划求解问题通过二次规划求解出相应的二次型最优解，得出各个机械臂关节舵机关节角度，控制机械臂完成目标任务。

根据图1的设计总图和上述说明，下面针对本发明进行详细的算法解析。

S1、根据飞行机器人上所搭载的姿态传感器以及相应的高度与位置传感器所获取到的飞行机器人自身的飞行实时运行数据，对携带冗余度机械臂的飞行机器人进行动力学建模；

在进行动力学建模时由于六旋翼的气动效应非常复杂，难以精确建模，为了简化起见，在飞行器低速飞行时忽略气动效应，整个六旋翼飞行器模型视为六自由度刚体，根据坐标变换原理和牛顿力学知识，在考虑飞行器的三个轴系的扰动D_i(i＝x,y,z)后，可得到六旋翼飞行器的动力学方程：

且

其中

为对应

的导数，x₁、x₃、x₅分别表示飞行机器人在世界坐标系中的位置分量即坐标x、y、z；x₂、x₄、x₆分别为飞行机器人在世界坐标系中的速度分量即坐标v_x、v_y、v_z；x₇、x₉、x₁₁分别表示横滚角φ、俯仰角θ、以及偏航角ψ；x₈、x₁₀、x₁₂分别为横滚角角速度

俯仰角角速度

以及偏航角角速度

m为飞行机器人的质量；g表示重力加速度；D_x、D_y、D_z分别表示X、Y、Z轴系的扰动；U₁表示高度控制器的控制量，U₂、U₃、U₄分别表示姿态控制器的横滚角φ、俯仰角θ、以及偏航角ψ的控制量；

其中I_x、I_y、I_z分别为飞行机器人在X轴、Y轴、Z轴的转动惯量，标量b表示六旋翼飞行器机臂臂长。

S2、根据步骤S1建立的飞行机器人动力学方程，设计水平位置控制器、高度控制器、以及姿态控制器，从系统最低阶次微分方程开始引入虚拟控制量，采用基于李雅普诺夫稳定性原理的后推方法，设计满足系统稳定要求的虚拟控制，从而确定各个控制器的虚拟控制量；具体包括下述步骤：

S21、设计水平位置控制器，确定相应的虚拟控制量，并将所述虚拟控制量转换电机控制量；

S211、针对水平位置控制器的X轴控制，由X轴状态方程假设一阶系统

在水平位置层上定义一阶跟踪误差z₁＝x_1d-x₁，其中x_1d为目标X轴方向位置，x₁为实际X轴方向位置；为实现位置x₁的轨迹跟踪，需要使得一阶跟踪误差收敛至0，采用基于李雅普诺夫稳定性原理的积分型后推设计方法：引入一阶跟踪误差的积分

t表示当前时间，并定义李雅普诺夫函数

λ₁为X轴的一阶跟踪误差的收敛率，对其求导可得

表示世界坐标系下x轴目标位移变化率，根据李雅普诺夫稳定性定理，要使得X轴的一阶系统是稳定的，取X轴的一阶虚拟控制量

β₁为常量，β₁＞0；

针对水平位置控制器的X轴二阶虚拟控制系统

设

为X轴扰动的在线估计值，定义

为扰动实际值与在线估计值的误差，即

且通常情况下有

要使估计误差

收敛至0，采用基于李雅普诺夫稳定性原理的积分型后推设计方法，定义二阶跟踪误差和李雅普诺夫函数，所述二阶跟踪误差为

其中

所述李雅普诺夫函数为

然后再对李雅普诺夫函数求导，可得

为消除估值误差变量的影响，令

其中κ_x＞0，则

再令

则

再令

则

当控制量

μ_x为常量，μ_x＞0，可以使得

X轴的二阶系统是稳定的，并解得X轴的二阶虚拟控制量：

其中，

表示世界坐标系下x轴目标速度变化率；

S212、针对水平位置控制器的Y轴控制，采用与步骤S211相同的原理，由Y轴状态方程假设一阶系统

根据目标Y轴方向位置x_3d和实际Y轴方向位置x₃，在位置层上定义一阶跟踪误差z₃＝x_3d-x₃，为了实现位置x₃的轨迹跟踪，采用基于李雅普诺夫稳定性原理的积分型后推设计方法，引入一阶跟踪误差的积分

并定义李雅普诺夫函数

λ₂为Y轴的一阶跟踪误差收敛率，对该函数求导，并根据李雅普诺夫稳定性定理，要使得Y轴的一阶系统是稳定的，可取一阶虚拟控制量

β₃为常量，β₃＞0。

对于水平位置Y轴的二阶系统

设

为Y轴扰动的在线估计值，定义

为扰动实际值与估计值的误差，即

且通常情况下有

要使估计误差收敛至0，根据基于李雅普诺夫稳定性原理的积分型后推设计方法，定义二阶跟踪误差z₄＝v₃-x₄，李雅普诺夫函数

对其进行求导运算，最终同样的可以使得

μ_y为常量，μ_y＞0，根据李雅普诺夫稳定性定理，确定该二阶系统是稳定的，并最终解得：

其中，

为世界坐标系下Y轴目标速度变化率；

S213、在飞行控制系统中，虚拟控制量u_x和u_y不能直接作为电机控制量，需要经过反解计算得到目标横滚角φ_d、目标俯仰角θ_d，并输入到姿态控制器中；由公式

经过反解运算求得：

S22、根据步骤S211中的原理，针对高度控制器，由高度Z轴状态方程可假设一阶系统

根据目标Z轴方向高度值x_5d和实际Z轴方向高度值x₅，在高度层上定义一阶跟踪误差z₅＝x_5d-x₅，为了实现高度x₅的轨迹跟踪，需要使得跟踪误差收敛至0，采用基于李雅普诺夫稳定性原理的后推设计方法，可定义李雅普诺夫函数

对其求导可得

根据李雅普诺夫稳定性定理，要使得该一阶系统是稳定的，可取一阶虚拟控制量

β₅为常量，β₅＞0；

针对Z轴的二阶系统

设

为Z轴扰动的在线估计值，定义

为扰动实际值与估计值的误差，即

且通常情况下有

要使估计误差

收敛至0，根据基于李雅普诺夫稳定性原理的后推设计方法，定义二阶跟踪误差和李雅普诺夫函数，所述二阶跟踪误差为

其中

所述李雅普诺夫函数

κ_z＞0，κ_z为Z轴的二阶跟踪误差收敛率，对该函数求导可得

根据李雅普诺夫稳定性原理，可选取Z轴的二阶虚拟控制量：

其中β₆为常量，β₆＞0。

S23、根据步骤S211中相同的原理，设计姿态控制器，并求解相应的控制量；，所述姿态控制器用于对横滚角、俯仰角、以及偏航角的控制；

S231、针对姿态控制器，由横滚角状态方程假设一阶系统

根据目标横滚角x_7d和实际横滚角x₇，可在姿态层上定义一阶跟踪误差z₇＝x_7d-x₇，为了加快横滚角x₇的收敛速率，采用基于李雅普诺夫稳定性原理的积分型后推设计方法，引入一阶跟踪误差的积分

并定义李雅普诺夫函数

λ₇＞0，λ₇为横滚角的一阶跟踪误差收敛率，对其求导可得

根据李雅普诺夫稳定性定理，要使得横滚角一阶系统是稳定的，可取横滚角的一阶虚拟控制量

β₇为常量，β₇＞0。

对横滚角的二阶系统

根据基于李雅普诺夫稳定性原理的积分型后推设计方法，定义二阶跟踪误差和李雅普诺夫函数，所述二阶跟踪误差：

其中

所述李雅普诺夫函数：

对该函数求导可得

由李雅普诺夫稳定性定理，可选取横滚角的二阶虚拟控制量：

其中β₈为常量，β₈＞0，可使得

可以确定横滚角的二阶系统是稳定的；

S232、由俯仰角状态方程可假设一阶系统

根据目标俯仰角x_9d和实际俯仰角x₉，可在姿态层上定义一阶跟踪误差z₉＝x_9d-x₉，为了加快俯仰角x₉的收敛速率，采用基于李雅普诺夫稳定性原理的积分型后推设计方法，引入一阶跟踪误差的积分

并定义李雅普诺夫函数

λ₉为俯仰角的一阶跟踪误差收敛率，对该函数求导，并根据李雅普诺夫稳定性定理，要使得俯仰角的一阶系统是稳定的，可取俯仰角的一阶虚拟控制量

β₉为常量，β₉＞0；

针对俯仰角的二阶系统

根据基于李雅普诺夫稳定性原理的积分型后推设计方法，定义二阶跟踪误差和相应的李雅普诺夫函数，所述二阶跟踪误差：z₁₀＝v₉-x₁₀，并进行相应的李雅普诺夫函数求导运算后，可求得俯仰角的二阶虚拟控制量：

其中β₁₀为常量，β₁₀＞0；β₉＞0，λ₉为俯仰角的一阶跟踪误差收敛率，同样可以确定俯仰角的二阶系统是稳定的；

S232、由偏航角状态方程可假设偏航角的一阶系统

根据目标偏航角x_11d和实际偏航角x₁₁，可在姿态层上定义一阶跟踪误差z₁₁＝x_11d-x₁₁，为了加快偏航角x₁₁的收敛速率，采用基于李雅普诺夫稳定性原理的积分型后推设计方法，引入一阶跟踪误差的积分

并定义李雅普诺夫函数

λ₁₁为偏航角的一阶跟踪误差收敛率，对该函数求导，根据李雅普诺夫稳定性定理，要使得该一阶系统是稳定的，可取一阶虚拟控制量

β₁₁为常量，β₁₁＞0；

针对偏航角的二阶系统，

根据基于李雅普诺夫稳定性原理的积分型后推设计方法，定义二阶跟踪误差：z₁₂＝v₁₁-x₁₂，以及相应的李雅普诺夫函数，并进行相应的李雅普诺夫函数求导运算后，可求得:

其中β₁₂为常量，β₁₂＞0；β₁₁＞0，λ₁₁为偏航角的一阶跟踪误差收敛率，同样可以确定偏航角的二阶系统是稳定的；

S24、将求解的各二阶虚拟控制量U₁～U₄根据不同旋翼飞行机器人的结构以及电机数目进行不同的输出控制分配。

S3、根据步骤S2获得的各个控制器的控制量，并通过选取相应的李雅普诺夫函数，求解扰动自适应率，然后对飞行机器人系统质量进行在线估计，确定质量自适应率，即完成飞行机器人自适应稳定飞行控制器的设计；具体包括下述步骤：

S31、针对X轴扰动，令

其中σ_x1、σ_x2、σ_x3为任意非负常量，则

其中κ_x为任意非负常量，同时式(1)

结合

和

可选取李雅普诺夫函数

其中函数最后部分(*)是不需要明确计算出的，且经过试凑可使得

符合李雅普诺夫稳定性条件，X轴估计扰动量能较快收敛于实际扰动量，进行较为准确的在线估计。结合

和

可选取

则X轴的二阶虚拟控制量式(1)可选取为:

通过式(8)，可求解出σ_x1η₁+σ_x2z₁+σ_x3z₂，则最终求得如下X轴的扰动自适应率：

其中，σ_x3＞0。

S32、针对Y轴扰动，采用与步骤S31相同的原理，令

其中σ_y1、σ_y2、σ_y3为任意非负常量，由

其中κ_y为任意非负常量，同时式(2)

综合条件可选取

其中函数最后部分(*是不需要明确计算出的，且经过试凑可使得

符合李雅普诺夫稳定性条件，Y轴估计扰动量能较快收敛于实际扰动量，进行较为准确的在线估计。结合条件可选取

则Y轴的二阶虚拟控制量式(2)最终可选取为:

通过式(10)，可求解出σ_y1η₃+σ_y2z₃+σ_y3z₄，则最终求得如下Y轴的扰动自适应率：

其中，σ_y3＞0。

S33、针对Z轴扰动,将Z轴的二阶虚拟控制量

β₆＞0代入Z轴的李雅普诺夫函数的导函数

则

即当

时，可确定

恒小于0，根据李雅普诺夫稳定性原理，Z轴估计扰动量能较快收敛于实际扰动量，进行较为准确的在线估计，故最终求得Z轴扰动自适应率：

其中，κ_z＞0，κ_z为Z轴的二阶跟踪误差收敛率。

S34、在不考虑飞行机器人系统扰动的情况下，即D_z＝0，

系统质量的不确定性只对高度Z轴的运动产生影响，设

为飞行机器人质量的在线估计值，定义

为质量实际值与估计值的误差，即

且

其中a为常数；为使得质量估计误差的最小化，需要使得

收敛至0，定义包含质量估计误差变量的李雅普诺夫函数

其中λ_m＞0，λ_m为飞行机器人质量估计误差收敛率；根据李雅普诺夫稳定性定理，

小于0，即

恒为负时，可确定系统质量的二阶系统是稳定的；化简函数

可得

将

代入化简后的函数

可得

实际上

为满足李雅普诺夫稳定性要求，质量自适应率选取为：

根据上述求解出的自适应率对不确定因数进行在线估计，更新控制器参数来调整电机输出量。

S4、根据冗余度机械臂控制器的设计需求，设计冗余度机械臂运动规划控制器，具体包括机械臂运动规划方案设计和二次规划设计，通过冗余度机械臂逆运动学实现冗余度机械臂的运动规划方案设计，再根据不同的设计目的和指标要求，将冗余度机械臂逆运动学问题转换为受约束的时变凸二次规划问题，并将求解结果作为机械臂的关节角状态，控制飞行机器人完成相应的控制任务；具体包括下述步骤：

S41、冗余度机械臂的运动规划方案通过冗余度机械臂逆运动学实现，其中逆运动学方程描述为：

f(θ)＝r (14)

其中，r是机械臂末端的期望轨迹，f(·)为冗余度机械臂关节角度到末端轨迹的非线性映射方程；对方程两边同时求导得到冗余度机械臂在速度层上的逆运动学方程：

其中，

为实数域上的m×n维矩阵，J(θ)为冗余度机械臂的雅克比矩阵，n表示机械臂的自由度数，m表示机械臂末端轨迹的空间维数，

和

分别为冗余度机械臂关节角度和末端轨迹关于时间的导数；

S42、根据不同的设计目的和指标要求，将逆运动学问题转换为受约束的时变凸二次规划问题，具体公式为：

s.t.Ax＝b， (17)

Cx≤d (18)

其中Ax＝b为相应完成任务所需的等式约束，Cx≤d为不等式约束，

与

为对应关节角的双端不等式约束；根据二次规划的算法，设计神经网络求解相应的二次型最优解；根据求解出的二次型最优解作为机械臂的关节角状态，控制飞行机器人完成相应的控制任务。

根据上述步骤可以完成本发明的。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以权利要求所述为准。

Claims (5)

1.一种携带冗余度机械臂的飞行机器人控制系统设计方法，是针对六旋翼飞行器自适应稳定飞行控制器和冗余度机械臂控制器两方面的设计,其特征在于，该方法包括如下步骤：

S1、根据飞行机器人上所搭载的姿态传感器以及相应的高度与位置传感器所获取到的飞行机器人自身的飞行实时运行数据，对携带冗余度机械臂的飞行机器人进行动力学建模；

S2、根据步骤S1建立的飞行机器人动力学方程，设计水平位置控制器、高度控制器、以及姿态控制器，从系统最低阶次微分方程开始引入虚拟控制量，采用基于李雅普诺夫稳定性原理的后推方法，设计满足系统稳定要求和误差收敛要求的虚拟控制，从而确定各个控制器的虚拟控制量；

S3、根据步骤S2获得的各个控制器的控制量，并通过选取相应的李雅普诺夫函数，求解扰动自适应率，然后对飞行机器人质量进行在线估计，采用基于李雅普诺夫稳定性原理的后推方法，确定质量自适应率，即完成飞行机器人自适应稳定飞行控制器的设计；

S4、根据冗余度机械臂控制器的设计需求，设计冗余度机械臂运动规划控制器，具体包括机械臂运动规划方案设计和二次规划设计，通过冗余度机械臂逆运动学实现冗余度机械臂的运动规划方案设计，再根据不同的设计目的和指标要求，将冗余度机械臂逆运动学问题转换为受约束的时变凸二次规划问题，并将求解结果作为机械臂的关节角状态，控制飞行机器人完成相应的控制任务。

2.根据权利要求1所述的携带冗余度机械臂的飞行机器人控制系统设计方法，其特征在于，步骤S1中，对携带冗余度机械臂的飞行机器人进行动力学建模，所述飞行机器人采用六旋翼飞行器，其动力学方程如下：

且

其中

为对应x_i的导数，x₁、x₃、x₅分别表示飞行机器人在世界坐标系中的位置分量即坐标x、y、z；x₂、x₄、x₆分别为飞行机器人在世界坐标系中的速度分量即坐标v_x、v_y、v_z；x₇、x₉、x₁₁分别表示横滚角φ、俯仰角θ、以及偏航角ψ；x₈、x₁₀、x₁₂分别为横滚角角速度

俯仰角角速度

以及偏航角角速度

m为飞行机器人的质量；g表示重力加速度；D_x、D_y、D_z分别表示X、Y、Z轴系的扰动；U₁表示高度控制器的控制量，U₂、U₃、U₄分别表示姿态控制器的横滚角φ、俯仰角θ、以及偏航角ψ的控制量；

其中I_x、I_y、I_z分别为飞行机器人在X轴、Y轴、Z轴的转动惯量，标量b表示六旋翼飞行器机臂臂长；u_x，u_y分别表示位置控制器的X轴和Y轴的二阶虚拟控制量。

3.根据权利要求1所述的携带冗余度机械臂的飞行机器人控制系统设计方法，其特征在于，步骤S2，具体包括下述步骤：

S21、设计水平位置控制器，确定相应的虚拟控制量，并将虚拟控制量转换电机控制量；

S211、针对水平位置控制器的X轴控制，由X轴状态方程假设一阶系统

在水平位置层上定义一阶跟踪误差z₁＝x_1d-x₁，其中x_1d为目标X轴方向位置，x₁为实际X轴方向位置；为实现位置x₁的轨迹跟踪，需要使得一阶跟踪误差收敛至0，采用基于李雅普诺夫稳定性原理的积分型后推设计方法：引入一阶跟踪误差的积分

t表示当前时间，并定义李雅普诺夫函数

λ₁为X轴的一阶跟踪误差的收敛率，对其求导可得

表示世界坐标系下X轴目标位移变化率，根据李雅普诺夫稳定性原理，要使得X轴的一阶系统是稳定的，取X轴的一阶虚拟控制量

β₁为常量，β₁＞0；

针对水平位置控制器的X轴二阶虚拟控制系统

x₂为飞行机器人在世界坐标系中的速度分量即坐标v_x，设

为X轴扰动的在线估计值，定义

为扰动实际值与在线估计值的误差，即

且

要使估计误差

收敛至0，采用基于李雅普诺夫稳定性原理的积分型后推设计方法，定义二阶跟踪误差和李雅普诺夫函数，所述二阶跟踪误差为

其中

所述李雅普诺夫函数为

然后再对李雅普诺夫函数求导，并消除估值误差变量影响，求得

其中u_x4为消除估值误差变量操作中与u_x相关的控制量，当控制量

μ_x为常量，μ_x＞0，使得

X轴的二阶系统是稳定的，并解得X轴的二阶虚拟控制量：

其中，

表示世界坐标系下X轴目标速度变化率；

S212、针对水平位置控制器的Y轴控制，采用与步骤S211相同的原理，由Y轴状态方程假设一阶系统

然后定义一阶跟踪误差z₃＝x_3d-x₃，其中x_3d表示目标Y轴方向位置，x₃表示实际Y轴方向位置；采用基于李雅普诺夫稳定性原理的积分型后推设计方法，求得一阶虚拟控制量

β₃为常量，β₃＞0，λ₂表示Y轴的一阶跟踪误差收敛率，η₃为一阶跟踪误差z₃在当前时间t上的积分，

为世界坐标系下Y轴目标位置变化率；对于Y轴的二阶系统

根据基于李雅普诺夫稳定性原理的积分型后推设计方法，定义二阶跟踪误差z₄＝v₃-x₄和李雅普诺夫函数

对李雅普诺夫函数进行求导运算，使得

μ_y为常量，μ_y＞0，根据李雅普诺夫稳定性原理，确定Y轴的二阶系统是稳定的，解得Y轴的二阶虚拟控制量：

其中，

为世界坐标系下Y轴目标速度变化率；

S213、在飞行控制系统中，二阶虚拟控制量u_x和u_y不能直接作为电机控制量，需要经过反解计算得到目标横滚角φ_d、目标俯仰角θ_d，并输入到姿态控制器中；由公式

经过反解运算求得：

S22、根据步骤S211中的原理，针对高度控制器，由高度Z轴状态方程假设一阶系统

定义Z轴的一阶跟踪误差z₅＝x_5d-x₅，其中，x_5d表示目标Z轴方向高度，x₅表示实际Z轴方向位置；然后采用基于李雅普诺夫稳定性原理的后推设计方法，求得Z轴的一阶虚拟控制量

β₅为常量，β₅＞0，

为世界坐标系下Y轴目标位置变化率；对于Z轴的二阶系统

针对其扰动估计误差，定义二阶跟踪误差和相应的李雅普诺夫函数，求得Z轴的二阶虚拟控制量：

其中β₆为常量，β₆＞0；

为Z轴扰动的在线估计值；

为世界坐标系下z轴目标速度变化率；z₆为Z轴的二阶跟踪误差，

且

S23、设计姿态控制器，并求解相应的控制量；所述姿态控制器用于对横滚角、俯仰角、以及偏航角的控制；

S231、针对姿态控制器，由横滚角状态方程假设一阶系统

在姿态层上定义横滚角的一阶跟踪误差z₇＝x_7d-x₇，其中x_7d为式(3)求得的目标横滚角φ_d，x₇为实际横滚角，然后采用基于李雅普诺夫稳定性原理的积分型后推设计方法，求得横滚角的一阶虚拟控制量

β₇为常量，β₇＞0，λ₇为横滚角的一阶跟踪误差收敛率，λ₇＞0，η₇表示横滚角的一阶跟踪误差z₇在时间t上的积分；针对横滚角的二阶系统

根据基于李雅普诺夫稳定性原理的积分型后推设计方法，求得横滚角的二阶虚拟控制量：

其中β₈为常量，β₈＞0，z₈为横滚角的二阶跟踪误差，

且

为目标横滚角角速度；

S232、由俯仰角状态方程假设一阶系统

定义俯仰角的一阶跟踪误差z₉＝x_9d-x₉，其中x_9d为式(3)求得的目标俯仰角θ_d，x₉为实际俯仰角，然后采用基于李雅普诺夫稳定性原理的积分型后推设计方法，求得俯仰角的一阶虚拟控制量

β₉为常量，β₉＞0，λ₉为俯仰角的一阶跟踪误差收敛率，λ₉＞0，η₉表示俯仰角的一阶跟踪误差z₉在当前时间t上的积分；针对俯仰角的二阶系统

根据基于李雅普诺夫稳定性原理的积分型后推设计方法，求得俯仰角的二阶虚拟控制量：

其中β₁₀为常量，β₁₀＞0，z₁₀表示俯仰角的二阶跟踪误差，z₁₀＝v₉-x₁₀；

S233、由偏航角状态方程假设偏航角的一阶系统

定义一阶跟踪误差z₁₁＝x_11d-x₁₁，采用基于李雅普诺夫稳定性原理的积分型后推设计方法，求得偏航角的一阶虚拟控制量

β₁₁为常量，β₁₁＞0，λ₁₁为偏航角的一阶跟踪误差收敛率，λ₁₁＞0；针对偏航角的二阶系统

根据基于李雅普诺夫稳定性原理的积分型后推设计方法，求得偏航角的二阶虚拟控制量：

其中β₁₂为常量，β₁₂＞0，z₁₂表示偏航角的二阶跟踪误差，z₁₂＝v₁₁-x₁₂；

S24、将求解的各二阶虚拟控制量U₁～U₄根据不同旋翼飞行机器人的结构以及电机数目进行不同的输出控制分配。

4.根据权利要求3所述的携带冗余度机械臂的飞行机器人控制系统设计方法，其特征在于，步骤S3中，具体包括下述步骤：

S31、针对X轴扰动，令

其中σ_x1、σ_x2、σ_x3为任意非负常量，则X轴的扰动自适应率

其中κ_x为任意非负常量，同时式(1)

选取

则式(1)选取为：

通过式(8)，可求解出σ_x1η₁+σ_x2z₁+σ_x3z₂，则最终求得如下X轴的扰动自适应率：

S32、针对Y轴扰动，采用与步骤S31相同的原理，令

其中σ_y1、σ_y2、σ_y3为任意非负常量，则Y轴的扰动自适应率

κ_y为任意非负常量；将式(2)选取为:

通过式(10)，可求解出σ_y1η₃+σ_y2z₃+σ_y3z₄，则最终求得如下的Y轴的扰动自适应率：

S33、针对Z轴扰动，将Z轴的二阶虚拟控制量

β₆＞0代入Z轴的李雅普诺夫函数的导函数

则

即当

时，可确定

恒小于0，根据李雅普诺夫稳定性原理，Z轴估计扰动量较快收敛于实际扰动量，进行准确的在线估计，故最终求得Z轴扰动自适应率：

其中，κ_z＞0，κ_z为Z轴的二阶跟踪误差收敛率；

S34、在不考虑飞行机器人系统扰动的情况下，即D_z＝0，

飞行机器人系统质量的不确定性只对高度Z轴的运动产生影响，设

为飞行机器人质量的在线估计值，定义

为质量实际值与估计值的误差，即

且

其中a为常数；为使得质量估计误差的最小化，需要使得

收敛至0，定义包含质量估计误差变量的李雅普诺夫函数

其中λ_m＞0，λ_m为飞行机器人的质量估计误差收敛率；根据李雅普诺夫稳定性原理，

小于0，即

恒为负时，可确定该二阶系统是稳定的；化简函数

可得

将

代入化简后的函数

可得

实际上

为满足李雅普诺夫稳定性要求，质量自适应率选取为：

S35、根据步骤S31～S34求解出的自适应率对不确定因数进行在线估计，更新控制器参数来调整电机输出量。

5.根据权利要求1所述的携带冗余度机械臂的飞行机器人控制系统设计方法，其特征在于，所述步骤S4中的冗余度机械臂运动规划方案设计和二次规划设计，具体包括下述步骤：

S41、冗余度机械臂的运动规划方案通过冗余度机械臂逆运动学实现，其中逆运动学方程描述为：

f(θ)＝r (14)

其中，r是机械臂末端的期望轨迹，f(·)为冗余度机械臂关节角度到末端轨迹的非线性映射方程；对方程两边同时求导得到冗余度机械臂在速度层上的逆运动学方程：

其中，

为实数域上的m×n维矩阵，J(θ)为冗余度机械臂的雅克比矩阵，n表示机械臂的自由度数，m表示机械臂末端轨迹的空间维数，

和

分别为冗余度机械臂关节角度和末端轨迹关于时间的导数；

S42、根据不同的设计目的和指标要求，将逆运动学问题转换为受约束的时变凸二次规划问题；根据二次规划的算法，设计神经网络求解相应的二次型最优解；根据求解出的二次型最优解作为机械臂的关节角状态，控制飞行机器人完成相应的控制任务。

Images