CN113190043A - 基于ros平台的六旋翼无人机mpc控制方法 - Google Patents

Abstract

本发明公开的一种基于ROS平台的六旋翼无人机MPC控制方法，属于无人机智能控制领域。本发明建立了六旋翼无人机的动力学模型，通过将模型线性化处理，解决了六旋翼无人机模型高度非线性、强耦合的问题，大大简化了控制目标；采用模型预测控制方法设计出了无人机单级控制器，控制结构采用闭环控制方法，实现对无人机位置和姿态的控制，突破了传统无人机需要级联两级控制器的桎梏。具有自主性强，跟踪精准，稳定性高，成本低等优点，可应用于无人机电力线路巡检等领域。

Description

基于ROS平台的六旋翼无人机MPC控制方法

技术领域

本发明属于无人机智能控制领域，尤其涉及一种基于ROS平台的六旋翼无人机MPC控制方法。具体涉及一种基于模型预测控制(model prediction control，MPC)和机器人操作系统(robot operating system，ROS))进行六旋翼无人机轨迹跟踪控制的方法。

背景技术

随着电力基础设施的完善和普及，为了确保电力系统的稳定运行，有必要对电力线路进行巡检。而电力线路所在位置往往是崇山峻岭，环境恶劣，威胁电力工作人员的生命健康。因此，采用无人机进行安全有效的电力巡检至关重要。与多旋翼系统相比，固定翼飞机更适合监视以及大规模的测绘任务，因为它们具有更长的续航能力和更高的速度，而对于需要靠近结构飞行以获得详细信息的检查任务，多旋翼无人机更合适。

但是目前传统的多旋翼无人机电力巡检需要人工遥控、依赖操作人员的技术和经验，缺乏自主性，人工控制精度低且成本高。这在一定程度上影响了电力巡检的有效性，存在漏检等问题。除此之外为解决无人机位置量和姿态角强耦合的问题，传统的无人机控制方法均是位置控制器和姿态控制器级联，两级控制器设计复杂度高，占用硬件资源。可见，可用于多旋翼无人机实现精准稳定的自主轨迹跟踪的控制方法有待进一步开发。

发明内容

本发明公开的一种基于ROS平台的六旋翼无人机MPC控制方法，尤其能解决传统无人机电力巡检依赖人工遥控、控制精度低、巡检有效性差以及无人机飞行稳定性差等问题，能实现电力线路的自主精准巡检。

本发明通过以下的技术方案实现以上发明目的。

本发明公开的一种基于MPC的六旋翼无人机轨迹跟踪控制方法，包括如下步骤：

步骤一：建立动力学模型。

根据Newton-Euler方法建立六旋翼无人机的非线性动力学模型，其表达式如下：

式(1)中x、y和z分别表示无人机的三维坐标，

θ和ψ分别表示无人机的横滚角、俯仰角和偏航角，ω_i(i＝1,2,…,6)表示六旋翼无人机六个旋翼的转速，b是无人机的升力系数，m是无人机的质量，g是重力加速度，I_x、I_y和I_z分别是无人机x轴、y轴、z轴的转动惯量。M_x、M_y和M_z分别是无人机的横滚力矩、俯仰力矩和偏航力矩，其表达式如下所示

式(2)中d是无人机的阻力系数，l是无人机力臂长度，ω_i(i＝1,2,…,6)是无人机六个旋翼的转速。由式(1)可知，六旋翼无人机动力学模型存在高度非线性，状态量之间以及状态与控制输入之间强耦合的问题，增加了其控制难度，因此传统无人机控制方法往往是分别设计位置和姿态控制器，二者级联以达到解耦的目的。由于无人机动力学模型为二阶系统，模型预测控制无法直接应用到无人机系统上去，因此需要降维处理，选取状态量q(t)为无人机位置量和姿态角以及线速度和角速度，即

无人机的升力

以及三个坐标轴上的力矩M_x、M_y和M_z作为控制输入量u(t)，得到六旋翼简化动力学模型：

式(3)中f:

步骤二：求取参考轨迹。

规划无人机电力巡检路线，基于步骤一所建立的动力学模型，根据规划的路线采用数值计算求解出无人机飞行过程中的参考轨迹q_r(t)以及参考控制输入u_r(t)：

步骤三：模型线性化。

将步骤一建立的六旋翼简化数学模型在参考点处进行泰勒展开，进行线性化处理，得到无人机线性误差模型。

式(5)中e(t)＝q(t)-q_r(t)，

A(t)和B(t)分别是函数f(q(t),u(t))相对于q(t)和u(t)的Jacobian矩阵。由式(5)可知，线性化处理解决了六旋翼无人机高度非线性和强耦合的问题，控制难度大大降低，由此便可以使用单机控制器直接控制。设置采样周期为T，将式(5)离散化处理得到：

步骤四：模型预测。

首先基于步骤三建立的无人机线性误差模型进行预测，取预测状态变量

代入式(6)中得到

y(k|k)＝CX(k|k) (8)

式(7)中各矩阵定义如下所示：

设置预测时域为N_p，控制时域为N_c，递推X(k+1k)至X(k+N_p|k)，由于y(k+ik)＝CX(k+ik),i＝1,2,…,N_p，写成紧凑形式为

Y(k)＝Ψ(k)X(k|k)+Θ(k)U(k) (10)

式(10)中各矩阵定义如下：

步骤五：滚动优化。

基于步骤四建立的预测模型(10)进行滚动优化，定义优化目标如下所示

式(13)中J₀是代价函数，

是参考状态序列，

和

分别是控制输入序列和稳态控制输入序列，Q≥0和R≥0分别是对状态误差和控制输入误差的惩罚函数，P是终端状态误差惩罚函数。ε(k)＝[ε₁(k) ε₂(k)]^T是松弛因子，可以避免出现不可行解。代价函数加入了对控制输入变化率的惩罚，使得无人机控制过程更加平稳，避免出现不必要的振荡。

步骤六：反馈校正。

将步骤五所述的代价函数转化为以下形式

式(30)中κ(k)＝[ΔU(k) ε(k)]^T，权重矩阵H(k)和f(k)定义如下：

在每个控制周期内求解上述优化问题，将优化结果通过MAVLink协议传递给PX4飞控，PX4控制Gazebo模拟物理环境中的六旋翼无人机实现轨迹跟踪和姿态优化，同时Gazebo中的无人机实时传递位置和姿态信息给PX4，再通过MAVLink协议回传给ROS系统，实现状态量反馈，然后在ROS系统里搭建的无人机控制器再根据误差进行优化控制。

有益效果

1、本发明公开的基于ROS平台的六旋翼无人机的MPC控制方法，针对六旋翼无人机电力线路巡检任务，设计了自主巡线轨迹跟踪算法。

2、本发明公开的基于ROS平台的六旋翼无人机的MPC控制方法，所设计的控制算法，解决了六旋翼无人机高度非线性、强耦合的问题，大大简化了控制目标。

3、本发明公开的基于ROS平台的六旋翼无人机的MPC控制方法，采用模型预测控制方法设计出了无人机单级控制器，实现对无人机位置和姿态的控制，突破了传统无人机需要级联两级控制器的桎梏。

附图说明

图1为本发明基于ROS平台的六旋翼无人机MPC控制方法的流程图；

图2为本发明模型预处理的流程图；

具体实施方式

下面将结合附图和实施例对本发明加以详细说明。同时也叙述了本发明技术方案解决的技术问题及有益效果，需要指出的是，所描述的实施例仅旨在便于对本发明的理解，而对其不起任何限定作用。

参照图1，本实施例公开的一种基于ROS平台的六旋翼无人机MPC控制方法，具体包括如下步骤：

步骤一：模型预处理。参照图2模型与处理流程图

1)根据Newton-Euler方法建立六旋翼无人机的非线性动力学模型，其表达式如下：

式(1)中x、y和z分别表示无人机的三维坐标，

θ和ψ分别表示无人机的横滚角、俯仰角和偏航角，ω_i(i＝1,2,...,6)表示六旋翼无人机六个旋翼的转速，b是无人机的升力系数，m是无人机的质量，g是重力加速度，I_x、I_y和I_z分别是无人机x轴、y轴、z轴的转动惯量。以上无人机的物理结构信息在Gazebo(三维物理仿真平台)中均已知。M_x、M_y和M_z分别是无人机的横滚力矩、俯仰力矩和偏航力矩，其表达式如下所示

式(2)中d是无人机的阻力系数，l是无人机力臂长度，ω_i(i＝1,2,...,6)是无人机六个旋翼的转速。由式(1)可知，六旋翼无人机动力学模型存在高度非线性，状态量之间以及状态与控制输入之间强耦合的问题，增加了其控制难度，因此传统无人机控制方法往往是分别设计位置控制器和姿态控制器，二者级联以达到解耦的目的。由于无人机动力学模型为二阶系统，模型预测控制无法直接应用到无人机系统上去，因此需要降维处理，选取状态量q(t)为无人机位置量和姿态角以及线速度和角速度，即

无人机的升力

以及三个坐标轴上的力矩M_x、M_y和M_z作为控制输入量u(t)，得到六旋翼简化动力学模型：

式(3)中f:

2)：求取参考轨迹。

规划无人机电力巡检路线，基于步骤一所建立的动力学模型，根据规划的路线采用数值计算求解出无人机飞行过程中的参考轨迹q_r(t)以及参考控制输入u_r(t)：

3)：模型线性化。

将步骤一建立的六旋翼简化数学模型在参考点处进行泰勒展开，进行线性化处理，得到无人机线性误差模型。

式(5)中e(t)＝q(t)-q_r(t)，

A(t)和B(t)分别是函数f(q(t),u(t))相对于q(t)和u(t)的Jacobian矩阵。由式(5)可知，线性化处理解决了六旋翼无人机高度非线性和强耦合的问题，控制难度大大降低，由此便可以使用单级控制器直接控制。无人机控制信号发送频率不能低于2Hz，即采样周期T≤0.5s，这里取采样周期T＝0.01s，将式(5)离散化处理得到：

步骤二：模型预测。

模型预测控制对应于控制理论的历史演变，旨在应对固定时域控制的已知挑战，它是基于模型的控制策略，分为三步，第一步是模型预测，第二步是滚动优化，第三步是反馈校正。线性二次调节器(Linear Quadratic Regulator，LQR)的无限时域控制，有助于提高控制器的稳定性和可行性，但计算量会显著增加，且有限时域的模型预测控制则可以通过选择终端代价函数以及终端约束来保证闭环稳定性和可行性。而计算能力有限的无人机控制平台，需要快速的计算出控制动作，显然模型预测控制相对于LQR更适合无人机的控制。

首先基于步骤一所述的无人机线性误差模型进行预测，取预测状态变量

代入式(6)中得到

y(k|k)＝CX(k|k) (23)

式(7)中各矩阵定义如下所示：

设置预测时域为N_p，控制时域为N_c，递推X(k+1|k)至X(k+N_p|k)：

由于y(k+i|k)＝CX(k+i|k),i＝1,2,...,N_p，写成紧凑形式为

Y(k)＝Ψ(k)X(k|k)+Θ(k)U(k) (26)

式(10)中各矩阵定义如下：

步骤三：滚动优化。

基于步骤二建立的预测模型(10)进行滚动优化，定义优化目标如下所示

式(13)中J₀是代价函数，

是参考状态序列，

和

分别是控制输入序列和稳态控制输入序列，Q≥0和R≥0分别是对状态误差和控制输入误差的惩罚函数，P是终端状态误差惩罚函数。ε(k)＝[ε₁(k) ε₂(k)]^T是松弛因子，可以避免出现不可行解。代价函数加入了对控制输入变化率的惩罚，使得无人机控制过程更加平稳，避免出现不必要的振荡。

步骤四：反馈校正。

ROS是机器人开源操作系统，具有类似操作系统所提供的功能，为机器人研究和开发提供代码复用。MAVROS是ROS的一个软件包，它建立起运行ROS的计算机与支持MAVLink(Micro Air Vehicle Communication Protocol)协议的飞控板如PX4或者支持MAVLink协议的地面站之间的通讯。

将步骤三所述的代价函数转化为以下形式

式(30)中κ(k)＝[ΔU(k) ε(k)]^T，权重矩阵定义如下：

在每个控制周期内求解上述优化问题，将优化结果通过MAVLink协议传递给PX4飞控，PX4发布控制指令给Gazebo模拟物理环境中的六旋翼无人机实现轨迹跟踪和姿态优化，同时Gazebo中的无人机实时传递位置和姿态信息给PX4，再通过MAVLink协议回传给ROS系统，实现状态量反馈，然后ROS里搭建的无人机控制器再根据误差进行优化控制。

本实施例，通过一种基于ROS平台的六旋翼无人机MPC控制方法，实现了六旋翼无人机的轨迹跟踪，解决了无人机高度非线性、强耦合的问题，为基于模型的无人机控制方法提供了一套可行方案，可以为电力线路提供精准稳定的巡检，减电力事故的发生，确保电力安全。

以上所述的基于ROS平台的六旋翼无人机MPC控制方法具体描述，对发明的目的、技术方案和有益效果进行进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.基于ROS平台的六旋翼无人机MPC控制方法，其特征在于，包括如下步骤：

步骤一：建立动力学模型；

步骤二：求取参考轨迹；

步骤三：模型线性化；

步骤四：模型预测；

步骤五：滚动优化；

步骤六：反馈校正。

2.如权利要求1所述的基于ROS平台的六旋翼无人机MPC控制方法，其特征在于，步骤一的实现方法为：

根据Newton-Euler方法建立六旋翼无人机的非线性动力学模型，其表达式如下：

式(1)中x、y和z分别表示无人机的三维坐标，

θ和ψ分别表示无人机的横滚角、俯仰角和偏航角，ω_i(i＝1,2,...,6)表示六旋翼无人机六个旋翼的转速，b是无人机的升力系数，m是无人机的质量，g是重力加速度，I_x、I_y和I_z分别是无人机x轴、y轴、z轴的转动惯量；M_x、M_y和M_z分别是无人机的横滚力矩、俯仰力矩和偏航力矩，其表达式如下所示

式(2)中d是无人机的阻力系数，l是无人机力臂长度，ω_i(i＝1,2,...,6)是无人机六个旋翼的转速；由式(1)可知，六旋翼无人机动力学模型存在高度非线性，状态量之间以及状态与控制输入之间强耦合的问题，增加了其控制难度，因此传统无人机控制方法往往是分别设计位置和姿态控制器，二者级联以达到解耦的目的；由于无人机动力学模型为二阶系统，模型预测控制无法直接应用到无人机系统上去，因此需要降维处理，选取状态量q(t)为无人机位置量和姿态角以及线速度和角速度，即

无人机的升力

以及三个坐标轴上的力矩M_x、M_y和M_z作为控制输入量u(t)，得到六旋翼简化动力学模型：

式(3)中

3.如权利要求1所述的基于ROS平台的六旋翼无人机MPC控制方法，其特征在于，步骤二的实现方法为：

规划无人机电力巡检路线，基于步骤一所建立的动力学模型，根据规划的路线采用数值计算求解出无人机飞行过程中的参考轨迹q_r(t)以及参考控制输入u_r(t)：

4.如权利要求1所述的基于ROS平台的六旋翼无人机MPC控制方法，其特征在于，步骤三的实现方法为：

将步骤一建立的六旋翼简化数学模型在参考点处进行泰勒展开，进行线性化处理，得到无人机线性误差模型；

式(5)中e(t)＝q(t)-q_r(t)，

A(t)和B(t)分别是函数f(q(t),u(t))相对于q(t)和u(t)的Jacobian矩阵；由式(5)可知，线性化处理解决了六旋翼无人机高度非线性和强耦合的问题，控制难度大大降低，由此便可以使用单机控制器直接控制；设置采样周期为T，将式(5)离散化处理得到：

5.如权利要求1所述的基于ROS平台的六旋翼无人机MPC控制方法，其特征在于，步骤四的实现方法为：

首先基于步骤三建立的无人机线性误差模型进行预测，取预测状态变量

代入式(6)中得到

y(k|k)＝CX(k|k) (8)

式(7)中各矩阵定义如下所示：

设置预测时域为N_p，控制时域为N_c，递推X(k+1|k)至X(k+N_p|k)，由于y(k+i|k)＝CX(k+i|k),i＝1,2,...,N_p，写成紧凑形式为

Y(k)＝Ψ(k)X(k|k)+Θ(k)U(k) (10)

式(10)中各矩阵定义如下：

6.如权利要求1所述的基于ROS平台的六旋翼无人机MPC控制方法，其特征在于，步骤五的实现方法为：

基于步骤四建立的预测模型(10)进行滚动优化，定义优化目标如下所示

式(13)中J₀是代价函数，

是参考状态序列，

和

分别是控制输入序列和稳态控制输入序列，Q≥0和R≥0分别是对状态误差和控制输入误差的惩罚函数，P是终端状态误差惩罚函数；ε(k)＝[ε₁(k) ε₂(k)]^T是松弛因子，可以避免出现不可行解；代价函数加入了对控制输入变化率的惩罚，使得无人机控制过程更加平稳，避免出现不必要的振荡。

7.如权利要求1所述的基于ROS平台的六旋翼无人机MPC控制方法，其特征在于，步骤六的实现方法为：

将步骤五所述的代价函数转化为以下形式

s.t.aκ(k)≤b(k)

式(30)中κ(k)＝[ΔU(k) ε(k)]^T，权重矩阵H(k)和f(k)定义如下：

在每个控制周期内求解上述优化问题，将优化结果通过MAVLink协议传递给PX4飞控，PX4控制Gazebo模拟物理环境中的六旋翼无人机实现轨迹跟踪和姿态优化，同时Gazebo中的无人机实时传递位置和姿态信息给PX4，再通过MAVLink协议回传给ROS系统，实现状态量反馈，然后在ROS系统里搭建的无人机控制器再根据误差进行优化控制。

Images