CN110780676A - 一种受扰小型无人直升机的复合主动抗干扰轨迹跟踪控制方法 - Google Patents

Abstract

本发明公开了一种受扰小型无人直升机的复合主动抗干扰轨迹跟踪控制方法。该发明基于非线性干扰观测器和块状反步控制技术，首先，建立无人直升机系统带有集总干扰的6自由度刚体模型，并将其分解为三个耦合相对较小的子系统；其次，分别针对三个子系统，设计非线性干扰观测器估计集总干扰；第三，将块状反步控制技术和干扰估计量相结合，设计前馈‑反馈复合抗干扰轨迹跟踪控制器；最后，通过选取合适的观测器和反馈控制器增益，可使无人直升机精确跟踪期望的位置和偏航角。本发明形式简单，降低了现有跟踪控制技术的复杂度，能实现复杂干扰环境下小型无人直升机的精确轨迹跟踪，同时有效提升了系统的抗干扰性能。

Description

一种受扰小型无人直升机的复合主动抗干扰轨迹跟踪控制 方法

技术领域

本发明涉及一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法，属于无人飞行器飞行控制的技术领域。

背景技术

在各种各样的无人飞行器中，无人直升机具有垂直起降，悬停，超低空和低速飞行，能在狭小、复杂和未知的环境中飞行等突出优势。由于这些显著的优势，无人直升机在军用和民用领域均具有广泛的应用前景，并且逐渐吸引了众多研究者的关注。然而，无人直升机系统是一个典型的强耦合、多输入多输出、静不稳定的非线性系统，同时还在飞行时还容易受到模型参数不确定、未建模动态等内部干扰和复杂外部环境干扰的影响，这给其飞行控制设计带来了很大的困难，其设计难度具体表现为：

1)无人直升机系统的高度非线性。由于其空气动力学特性的复杂性，无人直升机系统具有很强的非线性，这给建立一个能精确描述系统特性且适于控制设计的系统模型带来了很大困难。

2)无人直升机系统具有强耦合性。直升机具有四个直接控制量，它们对直升机的飞行方向、速度、姿态等六个自由度都产生控制作用，且四个控制量之间也有明显的耦合作用，这给控制设计带来了挑战。

3)无人直升机系统严重地受到干扰的影响。无人直升机系统受到的干扰包括，内部干扰：飞行条件变化、质量、转动惯量变化等导致的系统参数不确定问题，系统内部未建模动态引起的内部不确定问题；外部干扰：未知的阵风、气流影响，地面效应影响等。这些干扰因素都使得设计一个可靠的、高性能的飞行控制方法非常具有挑战性。

由于上述无人直升机飞行控制问题的困难性，传统的线性控制策略(比如PID控制、线性二次型调节器等)往往难以获得令人满意的控制性能，因此，一些非线性控制方法被应用于无人直升机飞行控制中，比如自适应控制，H控制，模型预测控制等。但是上述这些控制方法对于受扰无人直升机系统的控制效果并不令人满意。

文献(S.E.Ferik,A.H.Syed,H.M.Omar,M.A.Deriche,Nonlinear forward pathtracking controller for helicopter with slung load[J].Aerospace Science andTechnology,2017,69:602-608)提出了基于反步法的直升机轨迹跟踪控制方案，但是它属于反馈控制的范畴，是一种基于鲁棒思想设计的控制方法，以牺牲系统标称控制性能为前提来抑制干扰，而未对干扰实行直接和快速的抑制。此外，该文对无人直升机的每个一阶子系统均进行了反步递归设计，所设计的控制器结构较为复杂。文献(J.Hu,J.Huang,Z.Gao,H.Gu,Position tracking control of a helicopter in ground effect usingnonlinear disturbance observer-based incremental backstepping approach[J].Aerospace Science and Technology,2018,81:167-17.)设计了一种基于非线性干扰观测器的增量式反步位置跟踪控制器，由干扰观测器提供的干扰估计量可以主动补偿干扰的影响，提高了系统的抗干扰性能，使得系统输出在常值干扰或慢变干扰影响时，能无静差地跟踪参考信号。然而，由于所使用的干扰观测器存在局限性，难以对快变干扰(比如正弦干扰和高阶多项式干扰等)进行精确估计，使得所设计的控制器在系统受到快变干扰时，最多只能使闭环跟踪误差收敛到一个有界的范围内，甚至发散。

发明内容

本发明解决的技术问题是：针对小型无人直升机系统强耦合、多输入多输出、非线性强、参数快时变、受复杂外部干扰影响的特点，提出一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法。

本发明为解决上述技术问题采用以下技术方案：

本发明提出一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法，步骤包括：

步骤一、将内部干扰和直升机所受的复杂外部环境干扰当作集总干扰，建立无人直升机系统6自由度刚体模型，所述内部干扰包括未确定系统参数、未建模动态；

步骤二、将步骤一所述无人直升机系统6自由度刚体模型分解为三个子系统，包括一个二阶的高度子系统，一个二阶的偏航子系统和一个二维四阶的水平子系统；

步骤三、针对步骤二所述的三个子系统中的6个集总干扰，分别构造非线性干扰观测器来对其进行估计，获得干扰估计量；

步骤四、利用所述干扰估计量，基于块状反步控制技术设计复合抗干扰轨迹跟踪控制器，用于对所述三个子系统分别进行补偿，同时对飞行轨迹进行实时跟踪。

如前所述的一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法，进一步地：所述步骤一中简化的无人直升机6自由度刚体模型如下：

其中，P＝[x,y,z]^T和V＝[u,v,w]^T分别表示无人直升机在惯性坐标系下的位置和速度，

是用于描述直升机姿态的三个欧拉角，Ω＝[p,q,r]^T代表无人直升机在机体坐标系下的角速度，g代表重力加速度，e₃＝[0 0 1]^T；J＝diag{J_xx,J_yy,J_zz}为无人直升机在机体坐标系下的对角惯性矩阵，其中J_xx、J_yy、J_zz为转动惯量；U＝[δ_col,δ_lon,δ_lat,δ_ped]^T代表四个控制量输入，分别为主旋翼总距控制输入δ_col、纵向周期变距控制输入δ_lon、横向周期变距控制输入δ_lat、尾翼总距控制输入δ_ped；Z_w、Z_col为系统参数，d_V＝[d_u,d_v,d_w]^T和d_Ω＝[d_p,d_q,d_r]^T分别表示作用在直升机上的集总力和力矩干扰，

表示从机体坐标系到惯性坐标系的旋转矩阵，其中S_t、C_t、T_t分别表示三角函数sin(t)、cos(t)、 tan(t)；

为姿态向量变换矩阵；A和B为常系数矩阵，Y为系统的输出。

如前所述的一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法，进一步地：所述步骤二中的三个子系统为：

高度子系统：

偏航子系统：

水平子系统：

其中，P₁＝[x,y]^T，V₁＝[u,v]^T，R₁＝[r_1,3,r_2,3]^T，Ω₁＝[p,q]^T，U₁＝[δ_lon,δ_lat]^T，A₁和B₁为常系数矩阵，

r_i,j代表R(Θ)矩阵中的第i行j列元素。

如前所述的一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法，进一步地：所述步骤三中的非线性干扰观测器为如下形式：

将干扰重新记为d₁＝d_w、d₂＝d_r、d₃＝d_u、d₄＝d_v、d₅＝d_p、d₆＝d_q，并假设干扰由以下系统产生

其中，i＝1,2,…6，A_i、C_i分别为维数为m_i×m_i、r_i×m_i的系数矩阵，(Ai,Ci) 可观；

对应干扰d_i(i＝1,3,4)的非线性干扰观测器如下：

其中，i＝1,3,4，

分别为X_i,1、X_i,2、ζ_i的估计值，X_1,1＝z、X_3,1＝x、 X_4,1＝y，X_1,2＝w、X_3,2＝u、X_4,2＝v；h₁＝r_3,3(-g+Z_ww+Z_colδ_col)、[h₃,h₄]^T＝R₁(-g+Z_ww+Z_colδ_col)，α_i,1、α_i,2、α_i,3、…、为观测器增益；干扰d_i的估计值

对应干扰d_i(i＝2,5,6)的非线性干扰观测器如下：

其中，i＝2,5,6，

和

分别为X_i,1和ζ_i的估计值，X_2,1＝r、X_5,1＝p、X_6,1＝q，为观测器增益；干扰d_i的估计值

如前所述的一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法，进一步地：在步骤四中，在三个子系统中所设计的无人直升机前馈-反馈复合抗干扰轨迹跟踪控制器具体形式如下：

高度子系统：

偏航子系统：

水平子系统：

其中，

为虚拟控制器； T_m＝-g+Z_ww+Z_colδ_col；k_z、k_w、

k_r为正的控制器增益，

为正定对角矩阵增益，e_z、e_w、

e_r、

为各状态分量跟踪误差，z_d、P_1d＝[x_d,y_d]^T、为期望的位置和偏航角。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1)针对直升机具有四个直接控制量，且四个控制量之间也有明显的耦合作用的情形，通过采用非线性干扰观测器来估计匹配和不匹配时变集总干扰，并将干扰估计量用于前馈补偿，设计了前馈-反馈复合抗干扰轨迹跟踪控制器，该控制器不仅能保证无人直升机系统的精确轨迹跟踪，同时还提高了系统的抗干扰性能；

2)基于块状反步控制技术设计反馈控制器，使得所提出的控制器具有更简单的结构，从而增加了控制器的实用性和可靠性；

3)基于非线性干扰观测器对常值干扰、谐波干扰、甚至快速时变干扰，比如抛物线和高阶多项式干扰，精确估计的特性，本发明所设计的复合控制器能保证无人直升机系统在快速时变干扰下轨迹跟踪的精确性；

4)本发明所提出的前馈-反馈复合抗干扰轨迹跟踪控制设计思路具有较好的普适性，可应用到其他种类飞行器的轨迹跟踪控制设计中，可使用的场景复杂多变，具有很好的灵活性。

附图说明

图1为无人直升机结构及坐标系图；

图2为本发明设计流程图；

图3为无人直升机闭环系统框图；

图4为本发明方法的无人直升机三维轨迹跟踪曲线；

图5为本发明方法的无人直升机输出曲线图；

图6为本发明方法的无人直升机输出误差曲线图；

图7为本发明方法的无人直升机控制量曲线图；

图8为无人直升机集总干扰估计曲线图；

图9为无人直升机集总干扰估计误差曲线图。

图10为无人直升机滚转角和俯仰角曲线图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

下面结合附图对本发明的实施方式作详细设计步骤说明。

步骤一：建立简化的无人直升机6自由度刚体模型

图1为无人直升机结构及坐标系图，

为惯性坐标系，

为无人直升机机体坐标系。无人直升机6自由度刚体模型如下：

其中，P＝[x,y,z]^T和V＝[u,v,w]^T分别表示无人直升机在惯性坐标系下的位置和速度；

是用于描述直升机姿态的三个欧拉角，分别为滚转角φ、俯仰角θ、偏航角

Ω＝[p,q,r]^T代表无人直升机在机体坐标系下的角速度；g代表重力加速度，e_3,＝[0,0,1]^T， m为无人直升机的质量，J＝diag{J_xx,J_yy,J_zz}为无人直升机在机体坐标系下的对角惯性矩阵，×表示计算两个向量的向量积；表示从机体坐标系到惯性坐标系的旋转矩阵，其中S_t、C_t、T_t分别表示三角函数sin(t)、cos(t)、tan(t)；

为姿态向量变换矩阵；F和Γ分别表示在机体坐标系中描述的作用在无人直升机上的外部力和力矩，F_d和Γ_d分别表示无人直升机集总力和力矩干扰，包括内部干扰和外部干扰。内部干扰包括参数不确定和未建模动态等，外部干扰包括风扰动和地面效应等。

无人直升机外部力F可表示为：

F＝[0,0,T]^T, (2)

其中，T代表由主旋翼总距控制输入δ_col控制的直升机主推力，其表达式为 T＝m(-g+Z_ww+Z_colδ_col)。

直升机外部力矩可以表示为：

其中，δ_ped代表尾翼总距控制输入；a和b为纵向与横向挥舞角，分别由纵向周期变距控制输入δ_lon和横向周期变距控制输入δ_lat控制。在稳定状态，挥舞角a、b与控制量δ_lon、δ_lat的关系如下：

结合式(1)-(4)可得简化的无人直升机6自由度刚体模型为：

其中，

和d_Ω＝J^-1Γ_d＝[d_p,d_q,d_r]^T分别表示作用在直升机上的集总力和力矩干扰，Y为系统的输出，

为系统常系数矩阵，L_lat＝L_aA_lat+L_bB_lat，L_lon＝L_aA_lon+L_bB_lon， M_lat＝M_aA_lat+M_bB_lat，M_lon＝M_aA_lon+M_bB_lon，U＝[δ_col,δ_lon,δ_lat,δ_ped]^T代表四个控制量输入，分别为主旋翼总距控制输入δ_col、纵向周期变距控制输入δ_lon、横向周期变距控制输入δ_lat、尾翼总距控制输入δ_ped。Z_w、Z_col、τ_m、L_a、L_b、M_a、M_b、N_r、N_col、N_ped、L_lon、L_lat、 M_lon、M_lat为系统参数，可以通过系统辨识获得。

步骤二：将无人直升机模型分解为三个子系统

从无人直升机模型(5)中可以看出，四个控制量对直升机飞行方向、速度、姿态的控制是互相耦合的，为了能相对独立地设计四个控制量，将无人直升机系统分为三个耦合相对较小的子系统，分别为：

1)高度子系统：

2)偏航子系统：

3)水平子系统：

其中，P₁＝[x,y]^T，V₁＝[u,v]^T，R₁＝[r_1,3,r_2,3]^T，Ω₁＝[p,q]^T，U₁＝[δ_lon,δ_lat]^T，

r_i,j代表R(Θ)矩阵中的第i行j列元素。

步骤三：设计非线性干扰观测器

为简化表达，将干扰重新记为d₁＝d_w、d₂＝d_r、d₃＝d_u、d₄＝d_v、d₅＝d_p、d₆＝d_q。干扰d₃、d₄与控制量不在一个通道中，称为不匹配干扰。

假设干扰由以下系统产生

其中，i＝1,2,…6，

A_i、C_i分别为维数为m_i×m_i、r_i×m_i的系数矩阵， (A_i,C_i)可观。该式可描述多种类型干扰，包括谐波干扰、常值干扰、斜坡干扰、高阶多项式干扰等常见类型的干扰。

1)由于d₃、d₄为不匹配干扰，同时干扰d₁的估计值的导数将在控制器设计中用到，所以针对干扰d_i(i＝1,3,4)的非线性干扰观测器基于二阶子系统状态设计，其具体形式如下：

其中，i＝1,3,4，

分别为X_i,1、X_i,2、ζ_i的估计值，X_1,1＝z、X_3,1＝x、 X_4,1＝y，X_1,2＝w、X_3,2＝u、X_4,2＝v，h₁＝r_3,3(-g+Z_ww+Z_colδ_col)、[h₃,h₄]^T＝R₁(-g+Z_ww+Z_colδ_col)，α_i,1、α_i,2、α_i,3、…、

为观测器增益。则干扰d_i的估计值

2)基于一阶子系统状态，设计针对干扰d_i(i＝1,3,4)的非线性干扰观测器，其具体形式如下：

其中，i＝2,5,6，

和分别为X_i,1和ζ_i的估计值，X_2,1＝r、X_5,1＝p、X_6,1＝q，

[h₅,h₆]^T＝f₁(Ω₁)+A₁Ω₁+B₁U₁。α_i,1、α_i,2、…、

为观测器增益。则干扰d_i的估计值

以非线性干扰观测器(10)为例，定义观测误差

可得闭环观测误差系统为

其中，

通过选择合适的观测器增益α_i,1、α_i,2、α_i,3、…、

可保证观测误差系统矩阵

为 Hurwitz的，则闭环观测误差系统(12)是稳定的，干扰估计值渐近趋于实际值。

步骤四：设计前馈-反馈复合抗干扰轨迹跟踪控制器

结合图2可以看出，本发明所设计的前馈-反馈复合抗干扰轨迹跟踪控制器基于块状反步控制技术，并将步骤三中非线性干扰观测器获得的干扰估计量用于前馈补偿。在三个子系统中所设计的复合控制器的具体形式如下：

1)高度子系统：

其中，e_z＝z_d-z、

为高度和垂直速度的跟踪误差，控制增益k_z和k_w为正常数；z_d为期望的垂直位置；

2)偏航子系统：

3)水平子系统：

其中，虚拟控制器

P_1d＝[x_d,y_d]^T为期望的水平位置，e_p1＝P_1d-P₁、

为状态跟踪误差，T_m＝-g+Z_ww+Z_colδ_col，

控制增益

为正定对角矩阵。

通过选取合适的观测器增益和反馈控制器增益，使闭环跟踪误差系统渐近稳定，从而使小型无人直升机系统在所提出的前馈-反馈复合抗干扰轨迹跟踪控制器作用下，实现精确轨迹跟踪，同时具有良好的抗干扰性能。

为了验证本发明所提出的前馈-反馈复合抗干扰轨迹跟踪控制器的有效性，对无人直升机进行了数值仿真。无人直升机的期望轨迹为两个阶段，第一阶段为垂直起飞曲线，第二阶段为“8”形曲线，其具体数学表达式为：

集总干扰

和d_Ω＝J^-1Γ_d＝[d_p,d_q,d_r]^T包括两部分：外部干扰

和

分别设置为，

为周期正弦干扰，

为周期梯形波干扰，

为周期斜坡干扰；内部干扰为对模型参数随机增加±10％以内的不确定性。

图4、5、6分别为在本发明所设计的复合控制器作用下，无人直升机的三维轨迹跟踪曲线、输出曲线及输出误差曲线图。图6为本发明方法的无人直升机输出误差曲线图，其中，(a)为横向位置误差e_x，(b)为纵向位置误差e_y，(c)为垂直位置误差e_z，(d)为偏航角误差可以看出，在匹配和不匹配干扰的存在的情况下，无人直升机系统能获得很高的轨迹跟踪精度和良好的抗干扰性能。

图7为本发明方法的无人直升机控制量曲线图，其中，(a)为主旋翼总距控制输入δ_col，(b) 为纵向周期变距控制输入δ_lon，(c)为横向周期变距控制输入δ_lat，(d)为尾翼总距控制输入δ_ped；

图8为无人直升机集总干扰估计曲线图，其中，(a)为d₁，(b)为d₂，(c)为d₃，(d)为d₄， (e)为d₅，(f)为d₆；

图9为无人直升机集总干扰估计误差曲线图，其中，(a)为

(b)为

(c)为

(d) 为

(e)为(f)为

图10为无人直升机滚转角和俯仰角曲线图，其中，(a)为滚转角φ，(b)为俯仰角θ。从中可以看出，本发明所设计的非线性干扰观测器能估计多种类型的干扰，从而保证了复合控制器良好的抗干扰能力。

以上所述仅是本发明的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法，其特征在于，步骤包括：

步骤一、将内部干扰和直升机所受的复杂外部环境干扰当作集总干扰，建立无人直升机系统6自由度刚体模型，所述内部干扰包括未确定系统参数、未建模动态；

步骤二、将步骤一所述无人直升机系统6自由度刚体模型分解为三个子系统，包括一个二阶的高度子系统，一个二阶的偏航子系统和一个二维四阶的水平子系统；

步骤三、针对步骤二所述的三个子系统中的6个集总干扰，分别构造非线性干扰观测器来对其进行估计，获得干扰估计量；

步骤四、利用所述干扰估计量，基于块状反步控制技术设计复合抗干扰轨迹跟踪控制器，用于对所述三个子系统分别进行补偿，同时对飞行轨迹进行实时跟踪。

2.根据权利要求1所述的一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法，其特征在于：所述步骤一中简化的无人直升机6自由度刚体模型如下：

其中，P＝[x,y,z]^T和V＝[u,v,w]^T分别表示无人直升机在惯性坐标系下的位置和速度，

是用于描述直升机姿态的三个欧拉角，Ω＝[p,q,r]^T代表无人直升机在机体坐标系下的角速度，g代表重力加速度，e₃＝[0 0 1]^T；J＝diag{J_xx,J_yy,J_zz}为无人直升机在机体坐标系下的对角惯性矩阵，其中J_xx、J_yy、J_zz为转动惯量；U＝[δ_col,δ_lon,δ_lat,δ_ped]^T代表四个控制量输入，分别为主旋翼总距控制输入δ_col、纵向周期变距控制输入δ_lon、横向周期变距控制输入δ_lat、尾翼总距控制输入δ_ped；Z_w、Z_col为系统参数，d_V＝[d_u,d_v,d_w]^T和d_Ω＝[d_p,d_q,d_r]^T分别表示作用在直升机上的集总力和力矩干扰，

表示从机体坐标系到惯性坐标系的旋转矩阵，其中S_t、C_t、T_t分别表示三角函数sin(t)、cos(t)、tan(t)；为姿态向量变换矩阵；A和B为常系数矩阵，Y为系统的输出。

3.根据权利要求1所述的一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法，其特征在于：所述步骤二中的三个子系统为：

高度子系统：

偏航子系统：

水平子系统：

其中，P₁＝[x,y]^T，V₁＝[u,v]^T，R₁＝[r_1,3,r_2,3]^T，Ω₁＝[p,q]^T，U₁＝[δ_lon,δ_lat]^T，

A₁和B₁为常系数矩阵，

r_i,j代表R(Θ)矩阵中的第i行j列元素。

4.根据权利要求1所述的一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法，其特征在于：所述步骤三中的非线性干扰观测器为如下形式：

将干扰重新记为d₁＝d_w、d₂＝d_r、d₃＝d_u、d₄＝d_v、d₅＝d_p、d₆＝d_q，并假设干扰由以下系统产生

其中，i＝1,2,…6，

A_i、C_i分别为维数为m_i×m_i、r_i×m_i的系数矩阵，(A_i,C_i)可观；

对应干扰d_i(i＝1,3,4)的非线性干扰观测器如下：

其中，i＝1,3,4，

分别为X_i,1、X_i,2、ζ_i的估计值，X_1,1＝z、X_3,1＝x、X_4,1＝y，X_1,2＝w、X_3,2＝u、X_4,2＝v；h₁＝r_3,3(-g+Z_ww+Z_colδ_col)、[h₃,h₄]^T＝R₁(-g+Z_ww+Z_colδ_col)，α_i,1、α_i,2、α_i,3、…、

为观测器增益；干扰d_i的估计值

对应干扰d_i(i＝2,5,6)的非线性干扰观测器如下：

其中，i＝2,5,6，

和

分别为X_i,1和ζ_i的估计值，X_2,1＝r、X_5,1＝p、X_6,1＝q，

[h₅,h₆]^T＝f₁(Ω₁)+A₁Ω₁+B₁U₁；α_i,1、α_i,2、…、为观测器增益；干扰d_i的估计值

5.根据权利要求1所述的一种受扰小型无人直升机的复合主动复合抗干扰轨迹跟踪控制方法，其特征在于：在步骤四中，在三个子系统中所设计的无人直升机前馈-反馈复合抗干扰轨迹跟踪控制器具体形式如下：

高度子系统：

偏航子系统：

水平子系统：

其中，

为虚拟控制器；

T_m＝-g+Z_ww+Z_colδ_col；k_z、k_w、

k_r为正的控制器增益，

为正定对角矩阵增益，e_z、e_w、

e_r、

为各状态分量跟踪误差，z_d、P_1d＝[x_d,y_d]^T、

为期望的位置和偏航角。

Images