一种自治飞艇平面路径跟踪控制方法

摘要

一种自治飞艇平面路径跟踪控制方法，步骤如下：(一)给定期望跟踪值：给定期望平面路径；给定期望俯仰角θ

说明书

技术领域

本发明提供一种自治飞艇平面路径跟踪控制方法，它为全驱动自治飞艇提供一种跟踪任意参数化平面路径的新控制方法，属于自动控制技术领域。

背景技术

自治飞艇是一类非线性力学系统，其典型飞行状态包括起飞、巡航飞行、降落等。对于自治飞艇的巡航飞行，目前的飞行控制方法均基于动力学线性化模型进行设计，只能在平衡点附近有效。当存在模型不准确、外界干扰等因素时不能保证系统的稳定性。此外，目前的路径跟踪控制方法均存在控制奇异点，即当对象处于某些特定位置时会引起控制失效。

为解决这些问题，本发明“一种自治飞艇平面路径跟踪控制方法”，提出了基于动力学非线性模型的平面路径跟踪控制方法。该方法综合了基于导航的路径跟踪算法和轨迹线性化理论。由该方法控制的闭环系统是渐近稳定的，并且不存在控制奇异点，这就为自治飞艇的巡航飞行工程实现提供了有效的设计手段。

发明内容

(1)目的：本发明的目的在于提供一种自治飞艇平面路径跟踪控制方法，控制工程师可以按照该方法并结合实际参数实现自治飞艇的巡航飞行。

(2)技术方案：本发明“一种自治飞艇平面路径跟踪控制方法”，其主要内容及程序是：先由给定期望跟踪值进行导航计算，生成期望角度；然后进行姿态运动学控制计算得到期望角速度；将动力学方程按照纵横向分解，并分别对纵向动力学和横向动力学进行控制计算，最终得到控制量。实际应用中，飞艇的位置、姿态、速度等状态量由组合惯导等传感器测量得到，将由该方法计算得到的控制量传输至舵机和推进螺旋桨等执行装置即可实现自治飞艇平面路径跟踪功能。

本发明“一种自治飞艇平面路径跟踪控制方法”，其具体步骤如下：

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

步骤二 导航计算：计算消除期望位置与实际位置之间的误差所需的期望偏航角ψ_c。

步骤三 姿态运动学控制计算：计算消除期望姿态与实际姿态之间的误差所需的期望角速度ω_c。

步骤四 动力学纵横向分解：将动力学方程和期望速度值按照纵横向进行分解。

步骤五 纵向动力学控制计算：计算消除期望纵向速度与实际纵向速度之间的误差所需的控制量μ_lon。

步骤六 横向动力学控制计算：计算消除期望横向速度与实际横向速度之间的误差所需的控制量μ_lat。

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

其中，在步骤二中所述的计算消除期望位置与实际位置之间的误差所需的期望偏航角ψ_c，其计算方法如下：

1)计算期望路径参考点的方向角见图3所示，

2)计算飞艇当前位置与期望路径参考点的误差

p＝[x，y]^T为自治飞艇的当前位置。

3)计算期望偏航角ψ_c＝ψ_p+arctan2(-e，Δ)，见图3所示，Δ＞0为控制参数。

其中，在步骤三中所述的计算消除期望姿态与实际姿态之间的误差所需的期望角速度ω_c＝[p_c，q_c，r_c]^T，其计算方法如下：

1)伪逆计算

利用姿态运动学方程：

计算姿态运动学的伪逆：

${\omega }_n=R_{\gamma }^{-1}\left({\gamma }_c\right){\stackrel{·}{\gamma }}_c---\left(1\right)$

其中γ＝[φ，θ，ψ]^T为自治飞艇的姿态角(滚转角、俯仰角、偏航角)，ω＝[p，q，r]^T为自治飞艇的角速度，由伪微分器求得，σ_γ＞0为伪微分器的带宽。

2)误差稳定控制计算

定义a_12k＝-2ξ_1kω_1k，k＝1，2，3，其中阻尼ξ_1k，频率ω_1k根据期望闭环系统的响应指标选取；选取定义姿态误差γ_e＝γ_c-γ；计算误差稳定控制：

${\omega }_e=-K_{\gamma ,I}\int {\gamma }_e\mathrm{dt}-K_{\gamma ,P}{\gamma }_e---\left(2\right)$

3)由(1)和(2)所得结果计算期望角速度：

ω_c＝ω_n+ω_e

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

1)动力学模型纵横向分解

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

$M\stackrel{·}{\eta }=F\left(\eta \right)+A\left(\eta \right)+G\left(\eta \right)+\mu ---\left(3\right)$

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

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

$M_{\mathrm{lon}}{\stackrel{·}{\eta }}_{\mathrm{lon}}=F_{\mathrm{lon}}+A_{\mathrm{lon}}+G_{\mathrm{lon}}+{\mu }_{\mathrm{lon}}---\left(4\right)$

和横向动力学方程：

$M_{\mathrm{lat}}{\stackrel{·}{\eta }}_{\mathrm{lat}}=F_{\mathrm{lat}}+A_{\mathrm{lat}}+G_{\mathrm{lat}}+{\mu }_{\mathrm{lat}}---\left(5\right)$

其中纵向状态η_lon＝[u，w，q]^T，横向状态η_lat＝[v，p，r]^T，下标为lon和lat的各项分别表示纵向运动项和横向运动项。

2)期望速度纵横向分解

将期望速度υ_c，ω_c按照纵横向分解为纵向期望速度η_lon，c＝[u_c，w_c，q_c]^T和横向期望速度η_lat，c＝[v_c，p_c，r_c]^T。

其中，在步骤五中所述的计算消除期望纵向速度与实际纵向速度之间的误差所需的控制量μ_lon，其计算方法如下：

1)伪逆计算

利用(4)式计算纵向动力学的伪逆：

${\mu }_{\mathrm{lon},n}=M_{\mathrm{lon}}{\stackrel{·}{\eta }}_{\mathrm{lon},c}-F_{\mathrm{lon}}\left({\eta }_{\mathrm{lon},c}\right)-A_{\mathrm{lon}}\left({\eta }_{\mathrm{lon},c}\right)-G_{\mathrm{lon}}\left({\eta }_{\mathrm{lon},c}\right)---\left(6\right)$

其中由伪微分器求得，σ_lon＞0为伪微分器的带宽。

2)误差稳定控制计算

计算定义k＝1，2，3，其中阻尼ξ_2k，频率ω_2k根据期望闭环系统的响应指标选取；选取定义纵向状态误差η_lon，e＝η_lon，c-η_lon；计算误差稳定控制：

μ_lon，e＝-K_lon，I∫η_lon，edt-K_lon，Pη_lon，e    (7)

3)由(6)和(7)所得结果计算纵向动力学控制量：

μ_lon＝μ_lon，n+μ_lon，e

其中，在步骤六中所述的计算消除期望横向速度与实际横向速度之间的误差所需的控制量μ_lat。其计算方法如下：

1)伪逆计算

利用(5)式计算横向动力学的伪逆：

${\mu }_{\mathrm{lat},n}=M_{\mathrm{lat}}{\stackrel{·}{\eta }}_{\mathrm{lat},c}-F_{\mathrm{lat}}\left({\eta }_{\mathrm{lat},c}\right)-A_{\mathrm{lat}}\left({\eta }_{\mathrm{lat},c}\right)-G_{\mathrm{lat}}\left({\eta }_{\mathrm{lat},c}\right)---\left(8\right)$

其中由伪微分器求得，σ_lat＞0为伪微分器的带宽。

2)误差稳定控制计算

计算定义a_32k＝-2ξ_3kω_3k，k＝1，2，3，其中阻尼ξ_3k，频率ω_3k根据期望闭环系统的响应指标选取；选取定义横向状态误差η_lat，e＝η_lat，c-η_lat；计算误差稳定控制：

μ_lat，e＝-K_lat，I∫η_lat，edt-K_lat，Pη_lat，e    (9)

3)由(8)和(9)所得结果计算横向动力学控制量：

μ_lat＝μ_lat，n+μ_lat，e

(3)优点及效果：

本发明“一种自治飞艇平面路径跟踪控制方法”，与现有技术比，其优点是：

1)该方法能够跟踪任意参数化平面路径，且能保证路径跟踪方向。

2)该方法能够保证闭环系统的渐近稳定性能，且不存在控制奇异点。

3)该方法直接基于自治飞艇的非线性模型设计，且利用轨迹线性化理论保证跟踪性能，对模型不确定、干扰等有足够的鲁棒性。

4)该方法采用级联结构，将复杂的系统降阶后分别设计，避免了全系统整体求逆困难，简化了控制计算。

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

附图说明

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

图2a为本发明自治飞艇示意图；

图2b为本发明自治飞艇的惯性坐标系；

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

图中符号说明如下：

p_p为期望飞艇飞行路径，其中为路径参数，x_p，y_p为期望惯性系位置；

p    p＝[x，y]^T为飞艇的当前位置，x，y为当前惯性系位置；

υ_c         υ_c＝[u_c，v_c，w_c]^T＝[C，0，0]^T为期望速度，C＞0为常数，u_c，v_c，w_c为期望速度沿艇体坐标系的分解量；

γ_c         γ_c＝[φ_c，θ_c，ψ_c]^T为飞艇的期望姿态角，φ_c，θ_c，ψ_c分别为期望滚转角、期望俯仰角、期望偏航角；

γ          γ＝[φ，θ，ψ]^T为飞艇的实际姿态角，φ，θ，ψ分别为滚转角、俯仰角、偏航角；

ω_n          姿态运动学伪逆控制量；

ω_e          姿态运动学误差稳定控制量；

ω_c         ω_c＝[p_c，q_c，r_c]^T为期望角速度，p_c，q_c，r_c为期望角速度沿艇体坐标系的分解量；

η_lon，c    η_lon，c＝[u_c，w_c，q_c]^T为纵向期望速度；

η_lon       η_lon＝[u，w，q]^T为飞艇的实际纵向状态；

μ_lon，n    纵向动力学伪逆控制量；

μ_lon，e    纵向动力学误差稳定控制量；

μ_lon       纵向动力学实际控制量；

η_lat，c    η_lat，c＝[v_c，p_c，r_c]^T为横向期望速度；

η_lat       η_lat＝[v，p，r]^T为飞艇的实际横向状态；

μ_lat，n    横向动力学伪逆控制量；

μ_lat，e    横向动力学误差稳定控制量；

μ_lat       横向动力学实际控制量；

O_gx_gy_gz_g    惯性坐标系；

Oxyz        艇体坐标系；

ψ_p为期望路径参考点的方向角，

[s，e]  飞艇当前位置与期望路径参考点之间的距离差；

Δ      导航控制参数。

图2a和图2b中标号说明如下：

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

具体实施方式

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

本发明“一种自治飞艇平面路径跟踪控制方法”，见图1所示，其具体步骤如下：

步骤一：给定期望跟踪值

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

2)给定期望平面路径其中为路径参数，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)计算期望路径参考点的方向角见图3所示，其中

2)计算飞艇当前位置与期望路径参考点的误差其中p＝[x，y]^T为自治飞艇的当前位置。

3)计算期望偏航角ψ_c＝ψ_p+arctan2(-e，Δ)，见图3所示，其中Δ＞0为控制参数。

4)按照更新路径参数其中γ＞0为控制参数；

5)综合得期望姿态γ_c＝[φ_c，θ_c，ψ_c]^T。

步骤三：姿态运动学控制计算

1)伪逆计算

利用姿态运动学方程：

计算姿态运动学的伪逆：

${\omega }_n=R_{\gamma }^{-1}\left({\gamma }_c\right){\stackrel{·}{\gamma }}_c---\left(10\right)$

其中γ＝[φ，θ，ψ]^T为自治飞艇的姿态角(滚转角、俯仰角、偏航角)，ω＝[p，q，r]^T为自治飞艇的角速度，由伪微分器求得，σ_γ＞0为伪微分器的带宽。

2)误差稳定控制计算

定义a_12k＝-2ξ_1kω_1k，k＝1，2，3，其中阻尼ξ_1k，频率ω_1k根据期望闭环系统的响应指标选取；选取定义姿态误差γ_e＝γ_c-γ；计算误差稳定控制：

ω_e＝-K_γ，I∫γ_edt-K_γ，Pγ_e       (11)

3)由(10)和(11)所得结果计算期望角速度：

ω_c＝ω_n+ω_e

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

1)动力学模型纵横向分解

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

$M\stackrel{·}{\eta }=F\left(\eta \right)+A\left(\eta \right)+G\left(\eta \right)+\mu ---\left(12\right)$

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

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

$M_{\mathrm{lon}}{\stackrel{·}{\eta }}_{\mathrm{lon}}=F_{\mathrm{lon}}+A_{\mathrm{lon}}+G_{\mathrm{lon}}+{\mu }_{\mathrm{lon}}---\left(13\right)$

和横向动力学方程：

$M_{\mathrm{lat}}{\stackrel{·}{\eta }}_{\mathrm{lat}}=F_{\mathrm{lat}}+A_{\mathrm{lat}}+G_{\mathrm{lat}}+{\mu }_{\mathrm{lat}}---\left(14\right)$

其中纵向状态η_lon＝[u，w，q]^T，横向状态η_lat＝[v，p，r]^T，下标为lon和lat的各项分别表示纵向运动项和横向运动项。

2)期望速度纵横向分解

将期望速度υ_c，ω_c按照纵横向分解为纵向期望速度η_lon，c＝[u_c，w_c，q_c]^T和横向期望速度η_lat，c＝[v_c，p_c，r_c]^T。

步骤五：纵向动力学控制计算

1)伪逆计算

利用(13)式计算纵向动力学的伪逆：

${\mu }_{\mathrm{lon},n}=M_{\mathrm{lon}}{\stackrel{·}{\eta }}_{\mathrm{lon},c}-F_{\mathrm{lon}}\left({\eta }_{\mathrm{lon},c}\right)-A_{\mathrm{lon}}\left({\eta }_{\mathrm{lon},c}\right)-G_{\mathrm{lon}}\left({\eta }_{\mathrm{lon},c}\right)---\left(15\right)$

其中由伪微分器求得，σ_lon＞0为伪微分器的带宽。

2)误差稳定控制计算

计算定义a_22k＝-2ξ_2kω_2k，k＝1，2，3，其中阻尼ξ_2k，频率ω_2k根据期望闭环系统的响应指标选取；选取定义纵向状态误差η_lon，e＝η_lon，c-η_lon；计算误差稳定控制：

μ_lon，e＝-K_lon，I∫η_lon，edt-K_lon，Pη_lon，e    (16)

3)由(15)和(16)所得结果计算纵向动力学控制量：

μ_lon＝μ_lon，n+μ_lon，e

步骤六：横向动力学控制计算

1)伪逆计算

利用(14)式计算横向动力学的伪逆：

${\mu }_{\mathrm{lat},n}=M_{\mathrm{lat}}{\stackrel{·}{\eta }}_{\mathrm{lat},c}-F_{\mathrm{lat}}\left({\eta }_{\mathrm{lat},c}\right)-A_{\mathrm{lat}}\left({\eta }_{\mathrm{lat},c}\right)-G_{\mathrm{lat}}\left({\eta }_{\mathrm{lat},c}\right)---\left(17\right)$

其中由伪微分器求得，σ_lat＞0为伪微分器的带宽。

2)误差稳定控制计算

计算定义a_32k＝-2ξ_3kω_3k，k＝1，2，3，其中阻尼ξ_3k，频率ω_3k根据期望闭环系统的响应指标选取；选取定义横向状态误差η_lat，e＝η_lat，c-η_lat；计算误差稳定控制：

μ_lat，e＝-K_lat，I∫η_lat，edt-K_lat，Pη_lat，e    (18)

3)由(17)和(18)所得结果计算横向动力学控制量：

μ_lat＝μ_lat，n+μ_lat，e