CN104317300B - 一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法 - Google Patents

Abstract

一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法，步骤如下：给定期望跟踪值；制导误差计算：计算期望位置与实际位置之间的距离误差，角度误差；动力学方程纵横向分解，控制器设计只取其横向状态量；求解离散化系统方程：对由以上步骤得到的平流层飞艇横侧向连续系统进行线性化处理，并且也将误差导数和进行线性化处理。然后将飞艇横向状态量和误差当成扩展状态量，并且对扩展连续状态空间方程离进行离散化处理；预测系统未来动态：根据由组合惯导等传感器测量得到的当前状态量预测未来某一段时间的状态量或输出量；构造模型预测控制目标函数：由预测状态量构造目标函数，并用标准QP算法进行求解得到系统输入量。

Description

一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法

技术领域

本发明提供一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法，它为欠驱动平流层飞艇提供一种考虑执行机构饱和问题的跟踪平面路径的新控制方法，属于自动控制技术领域。

背景技术

平流层飞艇是依靠空气浮力驻空，在远离地表的平流层全天候全天时连续工作的浮空器，其具有飞行高度适中，执行任务时间长，生存能力强，搭载有效载荷大等优点，且在通信，监控，交通管理等领域具有广阔的军事和民用前景。平流层飞艇是一种非常复杂的非线性系统，往往对其进行建模时都会出现一定的不确定性。而且，飞艇在平流层飞行时总会有一定的外部干扰。这些问题都会导致系统不稳定。而且飞艇是一种运动较为缓慢的系统，在实际运动过程中执行机构存在最大限位，滚转角速度，偏航角速度等不能过大。这些约束限制了飞艇路径跟踪过程中的输入量的大小。

为解决这些问题，本发明“一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法”，提出了基于动力学线性模型的平面路径跟踪控制方法。该方法综合了基于视线制导的路径跟踪算法和模型预测控制理论。根据本发明所提出的方法和理论设计的控制器，可以很好的解决外界的干扰和建模不确定度对平流层飞艇系统稳定性的影响，为平流层飞艇的路径跟踪控制的工程实现提供了有效的设计手段。

发明内容

(1)目的：本发明的目的在于提供一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法，控制工程师可以在实际设计中按照该方法理论的步骤并结合实际系统参数实现平流层飞艇的考虑执行机构饱和和外界扰动问题的路径跟踪控制。

(2)技术方案：本发明“一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法”，其主要内容及程序是：先由给定期望跟踪路径进行制导导航计算，生成跟踪距离误差和偏航角度误差；将平流层飞艇动力学方程按照纵横向分解，并最终得到横向动力学方程进行控制器设计计算，得到控制量。然后将平流层飞艇和误差项作为状态量，并对其方程在参考点位置进行线性化处理；将连续系统模型进行离散化处理；由当前状态量和输出量预测未来状态量和输出量；构造模型预测控制目标函数，并计算目标函数中具体参数；利用标准QP算法对目标函数进行求解，得到当前系统的输入量。实际应用中，飞艇的位置、姿态、速度等状态量由组合惯导等传感器测量得到，将由该方法计算得到的控制量传输至舵机和推进螺旋桨等执行装置即可实现平流层飞艇平面路径跟踪功能。

本发明“一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法”，其具体步骤如下：

步骤一给定期望跟踪值：给定期望平面路径；给定期望俯仰角θ_c、期望滚转角φ_c；给定期望速度υ_c。

步骤二制导误差计算：计算期望位置与实际位置之间的距离误差e，角度误差

步骤三动力学方程纵横向分解：将动力学方程按照纵横向进行分解，控制器设计只取其横向状态量。

步骤四求解离散化系统方程：对由以上步骤得到的平流层飞艇横侧向连续系统进行线性化处理，并且也将误差导数和进行线性化处理。然后将飞艇横向状态量和误差当成扩展状态量，并且对扩展连续状态空间方程离进行离散化处理。

步骤五预测系统未来动态：根据由组合惯导等传感器测量得到的当前状态量预测未来某一段时间的状态量或输出量。

步骤六构造模型预测控制目标函数：由预测状态量构造目标函数，并用标准QP算法进行求解得到系统输入量u_k。

其中，在步骤一中所述的给定期望平面路径为一条直线y_p＝ax_p+b，x_p,y_p为飞艇期望平面位置；所述的给定期望俯仰角θ_c、期望滚转角φ_c均为零；所述的给定期望速度为υ_c＝[u_c,v_c,w_c]^T＝[V,0,0]^T，V＞0为常数，u_c,v_c,w_c为期望速度沿艇体坐标系的分解量。

其中，在步骤二中所述的计算期望位置与实际位置之间的距离误差e，角度误差其计算方法如下：

1)计算期望直线路径的方向角ψ_p＝arctan(a)，方向角误差其中ψ为飞艇偏航角，见图1所示。

2)计算飞艇当前位置与期望路径垂直距离p＝[x,y]^T为平流层飞艇的当前位置。

3)计算方向角误差导数r为飞艇偏航角速度；期望偏航角为Δ＞0为控制参数。

4)计算距离误差导数

其中，在步骤三中所述的将动力学方程和期望速度值按照纵横向进行分解，其分解方法如下：

记平流层飞艇动力学模型方程为：

其中Μ＝[m_ik]∈R^6×6(i,k＝1,2，…，6)为质量矩阵；X＝[υ^T,ω^T]^T，其中υ＝[u,v,w]^T为平流层飞艇速度沿艇体坐标系的分解量；F(X)＝[f₁,f₂，…，f₆]^T为科里奥利力和惯性力项；A(X)＝[a₁,a₂，…，a₆]^T为气动力项；G(X)＝[g₁,g₂，…，g₆]^T为重力和浮力项；μ＝[μ₁,μ₂，…，μ₆]^T为控制量。动力学模型方程(1)中各项的具体值随不同飞艇结构和参数而不同，在实际应用中根据实际情况确定。

将方程(1)按照纵横向运动分解为纵向动力学方程：

和横向动力学方程：

其中纵向状态量X_zong＝[u,w,q]^T，横向状态量X_ce＝[v,p,r]^T，下标为zong和ce的各项分别表示纵向运动项和横向运动项。在控制器设计中，取横向状态量为设计变量。如果不考虑滚转，则可以设p为零，从而不将p设为状态量。设计控制器时只考虑横向状态量，从而设前向速度u为常数。

其中，在步骤四中所述的求解离散化系统方程，其计算方法如下：

1)线性化处理

定义扩展状态变量对扩展后系统方程在参考点和u_r＝0进行线性化。线性化后得：

其中和为相对于参考点的误差。f_x,r和f_u,r为连续系统方程分别对X和u的偏导数。

2)计算离散系统方程

由于采样时间一般都比较短，我们可以采用前向差分的方法对连续系统进行离散化，设采样周期为T,设线性化后的扩展状态空间方程为：

则离散化之后的离散系统状态空间方程为：

其中A_K＝A*T+I B_K＝B*T

其中，在步骤五中所述的预测系统未来动态。其计算方法如下：

由组合惯导等传感器测量得到当前时刻的状态量X_K，通过此时的测量值预测未来某个预测时域内的状态量的值。设预测时域长度为N。则未来某一时刻状态量的预测值为：

其中，在步骤六中所述的构造模型预测控制目标函数。其计算方法如下：

在某一时刻K，定义目标函数为J_K，其表达式如下所示：

定义状态量约束和执行机构约束如下：

其中Q为状态量加权矩阵，R为输入量加权矩阵。

通过对目标函数进行求解，可以得到所以K时刻的系统输入量为的第一项，即为当前时刻的输入量。在下一个采样点重复执行步骤五和步骤六，即可求出下一时刻的输入量这样进行反复在线滚动优化，从而解决考虑外界扰动和执行机构限制情况下的路径跟踪问题。

(3)优点及效果：

本发明“一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法”，与现有技术比，其优点是：

1)模型预测控制算法采用了滚动优化的策略，在每个采样时刻的优化解的第一个分量作用于系统，其滚动实施能顾及模型失配、时变、干扰等引起的不确定性，及时进行弥补，始终把新的优化建立在实际的基础上，使控制保持实际上的最优。

2)目标函数的优化解是在约束区域进行求解的，所以可以充分解决执行机构饱和问题，以及对飞艇状态量约束也能很好的满足。

3)该方法直接基于平流层飞艇的线性模型设计，针对不同飞艇模型控制器设计比较简单易行。

4)该方法算法结构简单，目标函数求解速度快，适合在线优化计算。

控制工程师在应用过程中可以根据实际飞艇给定任意期望巡航路径，并将由该方法计算得到的控制量直接传输至执行机构实现路径跟踪控制的功能。

附图说明

图1为本发明导航计算几何关系图；

图2为本发明所述控制方法流程框图；

图3为本发明平流层飞艇示意图；

图中符号说明如下：

Pp 期望飞艇飞行路径参考点；

Xp 飞艇的当前位置；

{E} 惯性坐标系；

X_e 惯性坐标系X轴；

Y_e 惯性坐标系Y轴；

{B} 艇体坐标系；

{SF} Serret-Frenet坐标系；

C 期望路径；

ψ 飞艇偏航角；

ψ_p 期望路径方向角；

e 飞艇与期望路径的距离误差；

Δ 导航控制参数；

T 直线切向；

N 直线法向；

K时刻系统输入；

υ 飞艇侧向速度；

Xp 飞艇质心在惯性坐标系的横坐标；

Yp 飞艇质心在惯性坐标系的纵坐标；

飞艇偏航角误差；

O_gx_gy_gz_g 惯性坐标系；

Oxyz 艇体坐标系；

p 飞艇滚转角速度；

q 飞艇俯仰角速度；

r 飞艇偏航角速度

u 飞艇前向速度；

v 飞艇侧向速度；

w 飞艇纵向速度。

图3中数字说明如下：

1：艇体坐标系；2：惯性坐标系；

具体实施方式

下面结合附图，对本发明中的各部分设计方法作进一步的说明：

本发明“一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法”，见图2所示，其具体步骤如下：

步骤一：给定期望跟踪值

1)如图3所示，以自治飞艇浮心为原点建立艇体坐标系Oxyz；以地面上任一点为原点建立惯性坐标系O_gx_gy_gz_g，其中原点O_g为地面任意一点，O_gx_g指向北，O_gy_g指向东，O_gz_g指向地心。

2)给定期望平面路径y_p＝ax_p+b，x_p,y_p为飞艇期望平面位置。

3)给定期望俯仰角θ_c、期望滚转角φ_c均为零。

4)给定期望速度υ_c＝[u_c,v_c,w_c]^T＝[C,0,0]^T(C＞0)，u_c,v_c,w_c为期望速度沿艇体坐标系的分解量。

步骤二：制导误差计算

1)计算期望路径的方向角ψ_p＝arctan(a)，方向角误差其中ψ为飞艇偏航角，见图1所示。

2)计算飞艇当前位置与期望路径垂直距离p＝[x,y]^T为平流层飞艇的当前位置。

3)计算期望偏航角为Δ＞0为控制参数。

4)计算方向角误差导数r为飞艇偏航角速度。

5)计算距离误差导数

步骤三：动力学纵横向分解

记自治飞艇动力学模型方程为：

其中Μ＝[m_ik]∈R^6×6(i,k＝1,2，…，6)为质量矩阵；X＝[υ^T,ω^T]^T，其中υ＝[u,v,w]^T为平流层飞艇速度沿艇体坐标系的分解量；F(X)＝[f₁,f₂,…，f₆]^T为科里奥利力和惯性力项；A(X)＝[a₁,a₂,…，a₆]^T为气动力项；G(X)＝[g₁,g₂,…，g₆l^T为重力和浮力项；μ＝[μ₁,μ₂,…，μ₆]^T大控制量。动力学模型方程(8)中各项的具体值随不同飞艇结构和参数而不同，在实际应用中根据实际情况确定。

将方程(8)按照纵横向运动分解为纵向动力学方程：

和横向动力学方程：

其中纵向状态X_zong＝[u,w,q]^T，横向状态X_ce＝[v,p,r]^T，下标为zong和ce的各项分别表示纵向运动项和横向运动项。在控制器设计中，取横向状态量为设计变量。如果不考虑滚转，则可以设p为零，从而不将p设为状态量。设计控制器时只考虑横向状态量，从而设前向速度u为常数。

步骤四：求解离散化系统方程

1)线性化处理

定义扩展状态变量对扩展后系统方程在参考点和u_r＝0进行线性化。线性化后得：

其中和为相对于参考点的误差。f_x,r和f_u,r为连续系统方程分别对X和u的偏导数。

其中误差方程线性化后为

2)计算离散系统方程

由于采样时间一般都有比较短，我们可以采用前向差分的方法对连续系统进行离散化，设采样周期为T,线性化后的扩展状态空间方程为：

则离散化之后的离散系统状态空间方程为：

其中A_K＝A*T+I B_K＝B*T

步骤五：预测系统未来动态

由组合惯导等传感器测量得到当前时刻的状态量X_K，通过此时的测量值预测未来某个预测时域内的状态量的值。设预测时域长度为N。则未来某一时刻状态量的预测值为：

其中为在已知当前时刻状态变量的基础上根据离散系统方程计算出来的；为未知变量，即为目标函数的优化变量。

步骤六：构造模型预测控制目标函数

在某一时刻K，定义目标函数为J_K，其表达式如下所示：

定义状态量约束和执行机构约束如下：

其中Q为状态量加权矩阵，R为输入量加权矩阵。

根据方程(15)，可以将方程(16)继续推导成标准二次型形式，如下所示：

其中：

在用标准QP解法求解该二次型时，d_K只是与当前状态量有关的常值，与输入量没有关系。所以在求解时可以删去d_K。对上述标准二次型进行求解可以得到优化解为：所以K时刻的系统输入量为的第一项，即为当前时刻的输入量。在下一个采样点重复执行步骤五和步骤六，即可求出下一时刻的输入量这样进行反复在线滚动优化，即可解决考虑外界扰动和执行机构限制情况下的路径跟踪控制问题。

Claims (5)

1.一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法，其特征在于具体步骤如下：

步骤一给定期望跟踪值：给定期望平面路径；给定期望俯仰角θ_c、期望滚转角φ_c；给定期望速度υ_c；

步骤二制导误差计算：计算期望位置与实际位置之间的距离误差e，角度误差所述的制导误差计算，其计算方法如下：

1)计算期望路径的方向角Ψ_p＝arctan(a)，方向角误差其中Ψ为飞艇偏航角，a为路径参数直线斜率；

2)计算飞艇当前位置与期望路径垂直距离误差p＝[x,y]^T为平流层飞艇的当前位置，其中a路径参数直线斜率，b路径参数直线截距；

3)计算期望偏航角为Δ＞0为控制参数；

4)计算方向角误差导数r为飞艇偏航角速度；

5)计算距离误差导数其中u为飞艇前向速度，v为飞艇侧向速度；

步骤三动力学方程纵横向分解：将动力学方程按照纵横向进行分解，控制器设计只取其横向状态量；

步骤四求解离散化系统方程：对由以上步骤得到的平流层飞艇横侧向连续系统进行线性化处理，并且也将误差导数和进行线性化处理；然后将飞艇横向状态量和误差当成扩展状态量，并且对扩展连续状态空间方程进行离散化处理,所求解离散化系统方程，其计算方法如下：

1)线性化处理

定义扩展状态变量对扩展后系统方程在参考点和u_r＝0进行线性化；线性化后得：

$\stackrel{\·}{\tilde{X}}={\∫}_{x,r}\tilde{X}+{\∫}_{u,r}\tilde{u}---\left(1\right)$

其中和为相对于参考点的误差；f_x,r和f_u,r为连续系统方程分别对X和u的偏导数；

其中误差方程线性化后为

$\stackrel{\·}{e}=Ucos\left({\mathrm{\Ψ}}_{LOS}\right)\stackrel{\‾}{\mathrm{\Ψ}}+vcos\left({\mathrm{\Ψ}}_{LOS}\right)---\left(2\right)$

$\stackrel{\·}{\stackrel{\‾}{\mathrm{\Ψ}}}=r$

2)计算离散系统方程

由于采样时间一般都有比较短，我们可以采用前向差分的方法对连续系统进行离散化，设采样周期为T,线性化后的扩展状态空间方程为：

$\stackrel{\·}{\tilde{X}}=A\tilde{X}+B\tilde{u}---\left(3\right)$

则离散化之后的离散系统状态空间方程为：

${\stackrel{\·}{\tilde{X}}}_{K+1}=A_K{\tilde{X}}_K+B_K{\tilde{u}}_K---\left(4\right)$

其中A_K＝A*T+I B_K＝B*T；

步骤五预测系统未来动态：根据由组合惯导等传感器测量得到的当前状态量预测未来某一段时间的状态量或输出量；

步骤六构造模型预测控制目标函数：由预测状态量构造目标函数，并用标准QP算法进行求解得到系统输入量u_k。

2.根据权利要求1所述的一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法，其特征在于：在步骤一中所述的给定期望平面路径为一条直线y_p＝ax_p+b，x_p,y_p为飞艇期望平面位置；所述的给定期望俯仰角θ_c、期望滚转角φ_c均为零；

所述的给定期望速度为υ_c＝[u_c,v_c,w_c]^T＝[V,0,0]^T，V＞0为常数，u_c,v_c,w_c为期望速度沿艇体坐标系的分解量。

3.根据权利要求1所述的一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法，其特征在于：在步骤三中所述的动力学纵横向分解，其计算方法如下：

记自治飞艇动力学模型方程为：

$M\stackrel{\·}{X}=F\left(X\right)+A\left(X\right)+G\left(X\right)+B\mathrm{\μ}---\left(5\right)$

其中Μ＝[m_ik]∈R^6×6(i,k＝1,2,…,6)为质量矩阵；X＝[υ^T,ω^T]^T，其中υ＝[u,v,w]^T为平流层飞艇速度沿艇体坐标系的分解量；F(X)＝[f₁,f₂,…,f₆]^T为科里奥利力和惯性力项；A(X)＝[a₁,a₂,…,a₆]^T为气动力项；G(X)＝[g₁,g₂,…,g₆]^T为重力和浮力项；μ＝[μ₁,μ₂，…，μ₆]^T为控制量；动力学模型方程(5)中各项的具体值随不同飞艇结构和参数而不同，在实际应用中根据实际情况确定；

将方程(5)按照纵横向运动分解为纵向动力学方程：

$M_{zong}\stackrel{\·}{X_{zong}}=F_{zong}+A_{zong}+G_{zong}+B_{zong}{\mathrm{\μ}}_{zong}---\left(6\right)$

和横向动力学方程：

$M_{ce}{\stackrel{\·}{X}}_{ce}=F_{ce}+A_{ce}+G_{ce}+B_{ce}{\mathrm{\μ}}_{ce}---\left(7\right)$

其中纵向状态X_zong＝[u,w,q]^T，横向状态X_ce＝[v,p,r]^T，下标为zong和ce的各项分别表示纵向运动项和横向运动项；在控制器设计中，取横向状态量为设计变量；如果不考虑滚转，则可以设p为零，从而不将p设为状态量；设计控制器时只考虑横向状态量，从而设前向速度u为常数。

4.根据权利要求1所述的一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法，其特征在于：在步骤五中所述的预测系统未来动态，其计算方法如下：

由组合惯导等传感器测量得到当前时刻的状态量X_K，通过此时的测量值预测未来某个预测时域内的状态量的值；设预测时域长度为N；则未来某一时刻状态量的预测值为：

$\begin{array}{c}{\tilde{X}}_{K+1}=A_K{\tilde{X}}_K+B_K{\tilde{u}}_K\\ {\tilde{X}}_{K+2}=A_K^2{\tilde{X}}_K+A_K{B}_K{\tilde{u}}_K+B_K\tilde{u}{B}_K{\tilde{u}}_{K+1}\\ .\\ .\\ .\\ {\tilde{X}}_{K+i}=A_K^i{\tilde{X}}_K+A_K^{i-1}{B}_K{\tilde{u}}_K+A_K^{i-2}{B}_K{\tilde{u}}_{K+1}+...+B_K{\tilde{u}}_{K+i-1}\\ .\\ .\\ .\\ {\tilde{X}}_{K+N}=A_K^N{\tilde{X}}_K+A_K^{N-1}{B}_K{\tilde{u}}_K+A_K^{N-2}{B}_K{\tilde{u}}_{K+1}+...+B_K{\tilde{u}}_{K+N-1}\end{array}---\left(8\right)$

其中为在已知当前时刻状态变量的基础上根据离散系统方程计算出来的；为未知变量，即为目标函数的优化变量。

5.根据权利要求1所述的一种基于模型预测控制的平流层飞艇平面路径跟踪控制方法，其特征在于：在步骤六中所述的构造模型预测控制目标函数，其计算方法如下：

在某一时刻K，定义目标函数为J_K，其表达式如下所示：

${\mathrm{minJ}}_K=\underset{j=1}{\overset{N}{\Σ}}\left({\tilde{X}}_{K+j}^{T}Q{\tilde{X}}_{K+j}+{\tilde{u}}_{K+j-1}^{T}R{\tilde{u}}_{K+j-1}\right)---\left(9\right)$

定义状态量约束和执行机构约束如下：

$-{\mathrm{\δ}}_{max}\≤{\tilde{u}}_{K+j-1}\≤{\mathrm{\δ}}_{max}$

$-{\mathrm{\Δ\δ}}_{max}\≤\mathrm{\Δ}{\tilde{u}}_{K+j-1}\≤{\mathrm{\Δ\δ}}_{max}$

$x_{min}\≤{\tilde{X}}_{K+j}\≤x_{max}$

其中Q为状态量加权矩阵，R为输入量加权矩阵；

继续推导成标准二次型形式，如下所示：

$J\left(K\right)=\frac{1}{2}{\stackrel{\‾}{u}}_K^T{H}_K{\stackrel{\‾}{u}}_K+{\∫}_K^T{\stackrel{\‾}{u}}_K+d_K---\left(10\right)$

其中：

$H_K=2\left({\stackrel{\‾}{B}}_K^T\stackrel{\‾}{Q}{\stackrel{\‾}{B}}_K+\stackrel{\‾}{R}\right)$

$\∫K=2{\stackrel{\‾}{B}}_K^T\stackrel{\‾}{Q}{\stackrel{\‾}{A}}_K{\tilde{X}}_K$

$d_K={\tilde{X}}_K^T{\stackrel{\‾}{A}}_K^T\stackrel{\‾}{Q}{\stackrel{\‾}{A}}_K{\tilde{X}}_K$

$\stackrel{\‾}{Q}=diag(Q;...;Q)$

$\stackrel{\‾}{R}=diag(R;...;R)$

${\stackrel{\‾}{u}}_K={\[{\tilde{u}}_K^T,...,{\tilde{u}}_{K+N-1}^T\]}^T$

${\stackrel{\‾}{A}}_K=\left[\begin{array}{c}{A}_K\\ A_K^2\\ .\\ .\\ .\\ A_K^N\end{array}\right]$

在用标准QP解法求解该二次型时，d_K只是与当前状态量有关的常值，与输入量没有关系；所以在求解时可以删去d_K；对上述标准二次型进行求解可以得到优化解为：所以K时刻的系统输入量为的第一项，即为当前时刻的输入量；在下一个采样点重复执行步骤五和步骤六，即可求出下一时刻的输入量这样进行反复在线滚动优化，即可解决考虑外界扰动和执行机构限制情况下的路径跟踪控制问题。