CN109885096A - 一种基于Lyapunov-MPC技术的自主水下机器人路径跟踪闭环控制方法 - Google Patents

Abstract

一种基于Lyapunov‑MPC技术的自主水下机器人路径跟踪闭环控制方法，本发明涉及自主水下机器人路径跟踪闭环控制方法。本发明的目的是为了解决现有MPC方法分析路径跟踪稳定性差的问题。过程为：一、测量初始时刻AUV的状态测量值，设置AUV的期望路径；二、根据当前AUV的状态测量值和AUV的期望路径得到AUV的路径跟踪误差；三、使二获得的路径跟踪误差收敛，得到AUV的控制输入，AUV的控制输入包括AUV的力矩和力；四、判断AUV是否走完跟踪路径，若走完跟踪路径，得到AUV的控制输入；若没有走完跟踪路径，重新执行二到四，直至AUV走完跟踪路径。本发明用于自主水下机器人路径跟踪领域。

Description

一种基于Lyapunov-MPC技术的自主水下机器人路径跟踪闭环 控制方法

技术领域

本发明涉及自主水下机器人路径跟踪闭环控制方法。

背景技术

自主水下机器人(Autonomous underwater vehicle,AUV)是新一代水下机器人，具有活动范围大、机动性好、安全、智能等优点，成为完成各种水下任务的重要工具。路径跟踪问题是AUV运动控制的一类基础问题，即令AUV保持在期望路径上。由于任务需求的复杂化，将会进一步提高对AUV控制精度的要求，因此需要精确的控制方法来满足复杂的控制要求。

模型预测控制(Model Predictive Control,MPC)是结合最优控制理论与实际过程控制的特点与需求发展出来的一种控制方法。MPC利用控制过程的当前状态作为最优控制问题的初始状态，解得的最优控制序列只实施第一个控制作用，因此其本质上模型预测控制是求解一个开环最优控制问题。但是由于其仍然缺少稳定性相关的分析，所以还是难以称之为一个完备的控制理论，这也是大多数MPC控制器所存在的通病。

由于需要考虑以非线性、不确定性和约束为特征的复杂过程动力学，早在上世纪末就已经有学者开展了MPC可行性、稳定性、约束满足度和不确定性等方面的研究。Bemporad等人就已经提出了终端约束条件下的MPC方案，再利用最优控制的稳定性理论分析了其闭环稳定性^[1](Bemporad A,Morari M.Robust model predictive control:Asurvey[J].Robustness in Identification&Control,1999,245(1):207-226.)。YaohuiLu等人又完善了Min-Max鲁棒MPC控制框架，考虑了最坏情况下的控制问题，这种方法认为不确定性是有界的，但是其计算代价较为昂贵且容易产生比较保守的控制策略^[2](Lu Y,Arkun Y.Quasi-Min-Max MPC algorithms for LPV systems☆[J].Automatica,2000,36(4):527-540.)。另外就是基于Lyapunov的NMPC控制策略，这种控制策略提供了一个先验的(即在控制实现或可行性测试之前)一个稳定域，并将解约束到可行域中，保证闭环稳定性^[3](Miyata M,Sakuma F,Yoshimura A,et al.Enhanced stability regions for modelpredictive control of nonlinear process systems[J].Aiche Journal,2010,54(6):1487-1498.)。

综上，导致现有MPC方法分析路径跟踪稳定性差。

发明内容

本发明的目的是为了解决现有MPC方法分析路径跟踪稳定性差的问题，而提出一种基于Lyapunov-MPC技术的自主水下机器人路径跟踪闭环控制方法。

一种基于Lyapunov-MPC技术的自主水下机器人路径跟踪闭环控制方法具体过程为：

步骤一、测量初始时刻AUV的状态测量值，设置AUV的期望路径p(σ)；

步骤二、测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；

步骤三、使步骤二获得的路径跟踪误差e_p(t)收敛，得到AUV的控制输入，AUV的控制输入包括AUV的力矩和力；

步骤四、判断AUV是否走完跟踪路径，若走完跟踪路径，得到AUV的控制输入；若没有走完跟踪路径，重新执行步骤二到步骤四，直至AUV走完跟踪路径。

本发明的有益效果为：

本发明在弥补一般MPC方法不能显式分析稳定性的不足，提出一种基于控制Lyapunuov函数(Control Lyapunov Function,CLF)的MPC(LMPC)路径跟踪控制器,

本发明针对AUV路径跟踪问题，考虑AUV模型不确定与外界干扰对跟踪性能产生的影响，提出基于控制Lyapunov函数的MPC控制器，来实现AUV路径跟踪控制问题。首先，将AUV路径跟踪问题转化为期望值跟踪问题，得到跟踪的期望点与期望艏向，以适应MPC控制框架。接着，基于CLF理论设计了一种LMPC控制器。利用BSC设计辅助控制律，作为滚动时域优化问题的一个约束条件，使滚动时域优化问题的解保持在预设的吸引域中，从理论上保证了整个系统的闭环稳定性，并利用Lyapunov稳定性理论给出了相应的证明。最后，利用滚动优化方法在保证稳定性能的同时获得较好的跟踪性能。提高了外界干扰和参数摄动条件下AUV路径跟踪的稳定性和鲁棒性。

图1-2给出了AUV对“8”字形路径的跟踪效果和误差图，其中图1中虚线为参考路径，实线为LMPC的跟踪轨迹，点线为反步法控制器的跟踪轨迹，图2表示路径跟踪误差，图3、图4表示AUV的推力与力矩输入。从图1中可以看出，在严格的外界干扰以及参数摄动环境下，LMPC控制器仍能够保持较高的控制精度和稳定性，而BSC控制器则出现较大的跟踪误差。从图3、图4中可以看出，在整个跟踪控制过程中，控制输入平缓且能够保持在控制输入约束范围中。

附图说明

图1为本发明大地坐标系与随体坐标系示意图；

图2为路径跟踪误差图；

图3为控制输入力矩示意图；

图4为控制输入推力示意图。

具体实施方式

具体实施方式一：本实施方式一种基于Lyapunov-MPC技术的自主水下机器人路径跟踪闭环控制方法具体过程为：

步骤一、测量初始时刻AUV的状态测量值，设置AUV的期望路径p(σ)；

步骤二、测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；

步骤三、使步骤二获得的路径跟踪误差e_p(t)收敛，得到AUV的控制输入，AUV的控制输入包括AUV的力矩和力；

步骤四、判断AUV是否走完跟踪路径，若走完跟踪路径，得到AUV的控制输入；若没有走完跟踪路径，重新执行步骤二到步骤四，直至AUV走完跟踪路径。

具体实施方式二：本实施方式与具体实施方式一不同的是，所述步骤二中测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；具体过程为：

设置AUV水平面运动数学模型：

其中η＝[x′ y ψ]^T为大地坐标系下的坐标和姿态角，x′、y为大地坐标系下AUV的位置，ψ为大地坐标系下AUV艏向，上角标T表示求转置，为大地坐标系与随体坐标系之间的关系，为水平面坐标变换矩阵；M＝M_RB+M_A为惯性矩阵；M_RB为刚体惯性阵，M_A为附加质量阵，v＝[u′ v′ r]^T为随体坐标系下的速度向量，包括运动的速率和角速度；u′为随体坐标系下AUV的纵向，v′为随体坐标系下AUV的横向，r为随体坐标系下AUV的转艏速率，为随体坐标系下的加速度，C(v)＝C_RB(v)+C_A(v)为科氏力和向心力的矩阵；C_RB(v)为科氏力的矩阵；C_A(v)为向心力的矩阵；D(v)为流体阻尼力和力矩对AUV产生的作用；τ_thr为随体坐标系下作用于机器人上的推力和力矩，即机器人的控制输入；τ_env为环境对AUV作用的干扰力，例如风、浪、流等；

为了便于AUV控制器设计，将上述AUV水平面运动数学模型简化为如下形式：

其中x为AUV的状态量(AUV的大地坐标系下的坐标和姿态角η＝[x′ y ψ]^T和随体坐标系下的速度向量v＝[u′ v′ r]^T)，u为输入量(控制AUV的力和力矩)，f为足够连续可微的非线性函数；

设置一条规则曲线Ρ

其中p(σ)为期望路径，σ为路径参数；为路径参数的上界；

曲线Ρ定义在映射上；

根据公式(3)，定义路径跟踪误差为：

e_p(t)＝x(t)-p(σ(t)) (4)

其中e_p为路径跟踪误差，x(t)为AUV的状态量，p(σ(t))为期望路径；

路径跟踪的控制目标就是保证路径跟踪误差收敛，即lime_p(t)＝0；

由于跟踪问题在每个时刻都需要一个期望值作为跟踪目标，因此在已知参考路径的条件下，需要生成参考点作为控制目标。

考虑当前AUV的位姿状态[x,y,ψ]，以及路径p:x＝α₁(σ),y＝α₂(σ)，寻找曲线上离AUV当前状态最近的点为路径参考点，即可以归纳为如下形式：

其中α₁(σ)、α₂(σ)为参数方程；

通过求解上述优化问题式(5)，得到当前时刻的期望点的参数值σ(t)，即当前时刻的期望点的位置(x(σ(t)),y(σ(t)))；

而路径跟踪问题不仅仅需要期望点的位置(x(σ(t)),y(σ(t)))，还需要期望的航向角ψ(σ(t))来保证AUV沿期望路径的运动方向始终满足前向移动条件；

期望航向角通过下式计算得到：

其中atan2是求方位角的函数，atan2(x,y)返回原点到点(x,y)的方位角，定义域范围为ψ∈(-π,π]；为参数方程；

路径跟踪误差e_p(t)包含AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离，当前的航向与期望的航向角ψ(σ(t))的差值。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述步骤三中使步骤二获得的路径跟踪误差e_p(t)收敛，得到AUV的控制输入，AUV的控制输入包括AUV的力矩和力；具体过程为：

在标准的MPC框架中，由于预测时域有限，不能通过最优控制中解得全局最优性保证控制系统的闭环稳定性。本发明通过引入辅助约束条件，重新构造AUV路径跟踪问题的LMPC控制框架：

其中J为性能函数；N为预测时域，e_p为跟踪误差，u(·)为控制输入，f为AUV水平面运动数学模型式(1)，h(·)为引入的基于Lyapunov方法的辅助控制律，V(·)为AUV控制系统对应的CLF；x(0)为AUV当前的状态，X为定义域(AUV状态量的范围)，u(0)为当前的控制输入，U为定义域(控制AUV的推力和力矩，推力为0-100N，力矩为-200-200N)；Q、R为对应的加权矩阵，为x(t)一阶导数；CLF为控制Lyapunov函数；

对于引入的辅助控制律h(·)及AUV控制系统对应的Lyapunov函数，一定有

其中σ(x)为曲线参数；

该约束引入使MPC能够继承基于Lyapunov方法的辅助控制律h(·)的稳定性特性，此外，由于在线优化的存在，MPC会解算出性能最优的控制输出。

利用反步设计方法(Back-stepping Controller,BSC)可以得到式(7)中所需的基于Lyapunov方法的辅助控制律h(·)，将辅助控制律的表达形式带入式(7)得到优化问题；求解约束优化问题(7)得到控制AUV的力矩和力。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述利用反步设计方法(Back-stepping Controller,BSC)可以得到式(7)中所需的基于Lyapunov方法的辅助控制律h(·)；具体过程为：

考虑式(1)中的AUV水平面运动数学模型，控制要求为找到基于Lyapunov方法的辅助控制律h(·)使跟踪误差收敛到0；

为了利用反步法，首先作变量替换：

其中s为辅助镇定向量，z₁、z₂、η_e、η_d为变量；

结合式(1)得到新的被控系统(AUV水平面运动数学模型)

其中为变量，τ为状态反馈控制律，为s的一阶导；

首先选取第一个Lyapunov函数V₁

对V₁求导，再联合式(10)，整理可得

根据水平面坐标变换矩阵R(ψ)的正交性质，可以设计辅助镇定向量s

其中K₁为辅助控制律增益系数，为三阶正定对称阵；此时，变为

再选择第二个Lyapunov函数V₂

对V₂求导，再联合式(10)，整理可得

可以设计状态反馈控制律

其中K₂为辅助控制律增益系数，代入到式(16)中，可得

由于K₁和K₂均为正定的放大系数，所以有根据Lyapunov定理，基于状态反馈控制律(17)的闭环系统在平衡点全局渐近稳定；

将式(9)带入式(17)中的τ中，可以得到辅助控制律的表达形式。

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是，所述求解约束优化问题(7)得到控制AUV的力矩和力；具体过程为：

求解约束优化问题(7)，得到预测时域内的最优控制序列：

其中为第一个时刻的最优控制输入，为二个时刻的最优控制输入，为第N个时刻的最优控制输入，N为MPC预测时域；

将最优控制序列的第一个控制量作为当前时刻控制AUV的力矩和力。

其它步骤及参数与具体实施方式一至四之一相同。

MPC：MPC是一类独特的控制方法。它的当前控制动作是在每一个采样瞬间通过求解一个有限时域开环最优控制问题而获得。过程的当前状态作为最优控制问题的初始状态，解得的最优控制序列只实施第一个控制作用。虽然目前已经发展出了各种MPC的形式，在预测模型、优化方法、约束处理上都有许多不同，但是这些方法的核心原理仍然是相同的，即围绕滚动时域、预测函数、代价函数优化等几个方面来展开。

CLF：CLF概念的产生是源自八十年代的研究成果，随后又有一些知名的科研人员对其进行了深入的研究。目前，CLF已经涉及多个方面的应用，在稳定性的分析理论中，它涉及输入，输出等状态稳定性和渐近稳定性，也可以与许多别的控制理论相结合，例如鲁棒控制、预测控制、最优控制等，成为这些控制器分析稳定性的利器。

AUV运动数学模型：通常使用两种坐标系描述AUV在空间中的运动：大地坐标系O_E-X_EY_EZ_E和随体坐标系O_b-X_bY_bZ_b ^[4](Fossen T I.Marine Control Systems:Guidance,Navigation,and Control of Ships,Rigs and Underwater Vehicles[M].2002.)。因为本发明考虑的是AUV的水平面的控制问题，假设AUV的俯仰角和横倾角很小，则大地坐标系与随体坐标系之间的关系可以用下式表述

其中

是从非惯性的随体坐标系O_b-X_bY_bZ_b向大地坐标系O_E-X_EY_EZ_E转换的矩阵，ψ代表沿大地坐标系的z轴旋转一个角度。此外η＝[x y ψ]^T代表大地坐标系下的AUV位置和艏向，而v＝[u v r]^T则代表随体坐标系下AUV的纵向、横向以及转艏速率。矩阵R(ψ)为正交矩阵，具有正交矩阵的所有性质。

AUV的六自由度动力学方程采用AUV六自由度动力学模型的矢量形式：

其中M＝M_RB+M_A是惯性矩阵，包括附加质量阵M_A和刚体惯性阵M_RB,该矩阵的存在解释了整个流体-机器人系统的动能大于机器人刚体动能的原因。C(v)＝C_RB(v)+C_A(v)是描述刚体运动C_RB(v)以及流体作用C_A(v)产生的科氏力和向心力的矩阵。D(v)代表流体阻尼力和力矩对AUV产生的作用。这种力和力矩有许多种来源，例如潜在阻尼，表面摩擦，波浪漂移阻尼等等，它们可以使系统能量耗散，使得系统状态对于有界输入是有界的。g(η)代表的是AUV的重力和浮力带来的恢复力和恢复力矩。τ_thr是随体坐标系下作用于机器人上的推力和力矩，即机器人的控制输入，它可以有许多形式，可以由推进器和舵的联合作用构成，也可以用多个推进器构成。τ_env描述了环境对AUV作用的干扰力，例如风、浪、流等。

联立AUV的动力学和运动学方程，可得AUV三自由度运动数学模型：

控制Lyapunov函数理论

首先给出一些预备定义：

定义1：对于函数V:X→R，则V有如下定义：

正定，若V(0)＝0，当x≠0时，V(x)＞0；

严真，若||x||→∞时，V(x)→∞。

定义2：若函数满足，γ(0)＝0，连续且单调上升，则称函数γ为K类函数，若同时满足严真条件，则称函数γ为K_∞类函数。

定义3：若函数满足，对于每个t，β(·,t)为K类函数。对于每个s，β(s,·)严格单调减，且limβ(s,t)→0，则称函数β为KL类函数。

接下来给出CLF的定义：

考虑连续非线性系统：

其中为状态量，为输入量，f为足够连续可微的非线性函数。

定义4：当函数V(x)满足以下条件时，称函数V(x)为系统(23)的一个CLF：

V(x)正定、严真且在定义域内无穷阶连续可导。

对于任意x≠0，都有

特别地，对于仿射非线性系统

其中为状态量，为输入量，f和g为足够连续可微的非线性函数，且f(0)＝0。系统(25)的CLF满足：

其中L_fV(x)表示V(x)沿函数f的导数。

Artstein-Sontag定理：系统有连续可微的状态反馈控制律u＝k(x)，使控制系统在平衡点附近全局渐近稳定的充要条件为：存在正定、严真且在定义域内无穷阶连续可导的函数V(x)满足：

(1)对任意x≠0，；

(2)对任意ε＞0，存在δ＞0，当x满足||x||＜δ且x≠0时，必然存在一个u，满足||u||＜ε，使L_fV(x)+L_gV(x)u＜0成立。

以上就是CLF的定义。可以看出，V(x)的存在使状态反馈控制律u＝k(x)存在的充分条件。因此CLF与原本的Lyapunov函数有所不同，原本的Lyapunov函数强调系统若存在一个Lyapunov函数，则系统是渐近稳定的，而当系统存在一个CLF时，系统是可以实现反馈渐近稳定，因此，CLF对于控制器的设计以及系统稳定性的分析起到重要的作用。

LMPC稳定性证明：

考虑控制系统的一个的CLF即V₂，连续可微且完全无界，根据逆李雅普诺夫稳定性理论，存在一个函数β_i(·)，满足如下不等式：

β₁(||x||)≤V₂(x)≤β₂(||x||)

结合式(7)，可以得到

通过李雅普诺夫稳定性原理，可以得到在LMPC路径跟踪算法下的闭环系统在吸引域内是全局渐近稳定的。此外，还可以通过缩小控制增益K₁和K₂的幅度来扩大吸引域。值得注意的是，尽管可以通过缩小控制增益K₁和K₂的幅度来扩大吸引域，即BSC的控制性能取决于增益矩阵，从式(28)中可以看出增益缩小会导致系统收敛更慢，所以BSC的性能与收敛之间是有一定的权衡的，而MPC框架恰好避免了这个问题，利用优化过程，即使取比较小的增益，LMPC控制器仍然可以充分利用执行器的性能，取得更好的性能指标。

采用以下实施例验证本发明的有益效果：

实施例一：

对于AUV路径跟踪问题，除了本发明算法外还有开环形式的MPC控制方案、基于反步法的控制方案等，以下简要介绍这两种方案，并与本发明算法作比较

开环形式的MPC控制方案

文献^[5](刘昌鑫，高剑，徐德民，一种欠驱动AUV模型预测路径跟踪控制方法[J].机械科学与技术,2017(11):19-23.)针对AUV的路径跟踪问题,设计了一种模型预测路径跟踪控制器。首先定义路径参数的二阶导数作为路径虚拟控制律,并将参考路径的模型扩展到AUV路径跟踪预测模型；然后采用非线性模型预测控制设计了控制律，通过对约束优化问题的滚动求解，得到满足约束的扩展控制输入。与本发明的方法对比，这种基于MPC的控制方案本质上还是一种开环的控制策略，不能从理论上保证闭环稳定性。

基于反步法的控制方案

文献^[6](王宏健,陈子印,贾鹤鸣,et al.基于滤波反步法的欠驱动AUV三维路径跟踪控制[J].自动化学报,2015,41(3).)研究了AUV的三维空间路径跟踪控制问题，针对基于虚拟向导建立的三维路径跟踪误差模型，用滤波反步法设计跟踪控制器，通过二阶滤波过程获得虚拟控制量的导数,避免了直接对虚拟控制量解析求导的复杂过程，基于李雅普诺夫稳定性理论设计鲁棒项，保证了闭环跟踪误差系统状态的渐近稳定。文献^[7](贾鹤鸣,宋文龙,周佳加.基于非线性反步法的欠驱动AUV地形跟踪控制[J].北京工业大学学报,2012,38(12):1780-1785.)为实现AUV的精确地形跟踪控制,设计了一种基于Lyapunov稳定性理论的非线性反步法控制器。基于虚拟向导的方法，结合AUV艇体的动力学特性，建立AUV垂直面地形跟踪误差方程，采用反步法设计地形跟踪控制器，利用Lyapunov稳定性理论分析了整个系统的稳定性。

与本发明提出的算法不同，反步法通常是通过缩小控制增益来扩大吸引域，但反步法的控制性能同样取决于增益矩阵，即增益缩小会导致系统收敛更慢，所以反步法的控制性能与稳定性能之间是有一定的权衡的，而MPC框架恰好避免了这个问题，利用优化过程，即使取比较小的增益，LMPC控制器仍然可以充分利用执行器的性能，取得更好的性能指标。

仿真准备

本发明在MATLAB R2016b仿真环境下进行仿真实验，式(22)中所需的水动力系数值如表1所示

表1仿真参数表

AUV的执行器由一个主推和垂直舵组成，这里设定执行器的最大纵向推力为100N，最大扭转力矩为200N·m，不考虑倒车情况，控制节拍为0.2秒。

预设跟踪路径为“8”字型路径，其参数方程为：

跟踪性能分析

对“8”字型路径的跟踪效果如图1所示，其中实线为LMPC控制器的跟踪效果，点线为BSC的跟踪效果，虚线为参考路径。从图中可以看出，这两种控制器都能够成功的跟踪参考路径，但显然LMPC控制器的控制性能更佳。从图2中也可以看出LMPC的跟踪误差较小，能够满足跟踪性能要求。图3、图4为LMPC控制器的推力输入和力矩输入，可以看出，控制输入能够满足执行器约束条件且变化较为平缓。

结合图1、图3、图4可以得知，在曲率较大的部分，控制器会采用保守的输入来保证跟踪的平稳，而在曲率较为平缓的部分，控制器的输出则会较为激进，这里则是利用了滚动优化过程获得更佳的控制性能而不会过于保守。

鲁棒性能分析

本发明引入稳定性约束条件的优势是能直观的考虑系统的闭环稳定性，因此为了测试闭环系统的鲁棒性能，考虑模型参数不确定与外界干扰情况。仿真实验将在十分严格的测试条件下进行：有50％的模型参数摄动，和海流干扰[20N,80N,0N]分别作用在AUV的u、v和r方向上。依然以“8”字型路径为例，结果如图1、图3、图4所示。

从图1、图3、图4中可以得知，在极端严格的测试条件下，LMPC依然能够保持跟踪的收敛，跟踪误差始终在可以接受的范围之内，而BSC则存在明显的跟踪误差，甚至偏离了设定的航向。结果说明LMPC的能够参数干扰条件和外部环境干扰下收敛，拥有较好的鲁棒性能。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (5)

1.一种基于Lyapunov-MPC技术的自主水下机器人路径跟踪闭环控制方法，其特征在于：所述方法具体过程为：

步骤一、测量初始时刻AUV的状态测量值，设置AUV的期望路径p(σ)；

步骤二、测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；

步骤三、使步骤二获得的路径跟踪误差e_p(t)收敛，得到AUV的控制输入，AUV的控制输入包括AUV的力矩和力；

步骤四、判断AUV是否走完跟踪路径，若走完跟踪路径，得到AUV的控制输入；若没有走完跟踪路径，重新执行步骤二到步骤四，直至AUV走完跟踪路径。

2.根据权利要求1所述一种基于Lyapunov-MPC技术的自主水下机器人路径跟踪闭环控制方法，其特征在于：所述步骤二中测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；具体过程为：

设置AUV水平面运动数学模型：

其中η＝[x′ y ψ]^T为大地坐标系下的坐标和姿态角，x′、y为大地坐标系下AUV的位置，ψ为大地坐标系下AUV艏向，上角标T表示求转置，为大地坐标系与随体坐标系之间的关系，R(ψ)为水平面坐标变换矩阵；M＝M_RB+M_A为惯性矩阵；M_RB为刚体惯性阵，M_A为附加质量阵，v＝[u′ v′ r]^T为随体坐标系下的速度向量，包括运动的速率和角速度；u′为随体坐标系下AUV的纵向，v′为随体坐标系下AUV的横向，r为随体坐标系下AUV的转艏速率，为随体坐标系下的加速度，C(v)＝C_RB(v)+C_A(v)为科氏力和向心力的矩阵；C_RB(v)为科氏力的矩阵；C_A(v)为向心力的矩阵；D(v)为流体阻尼力和力矩对AUV产生的作用；τ_thr为随体坐标系下作用于机器人上的推力和力矩，即机器人的控制输入；τ_env为环境对AUV作用的干扰力；

将上述AUV水平面运动数学模型简化为如下形式：

其中x为AUV的状态量，u为输入量，f为足够连续可微的非线性函数；

设置一条规则曲线Ρ

其中p(σ)为期望路径，σ为路径参数；为路径参数的上界；

曲线Ρ定义在映射上；

根据公式(3)，定义路径跟踪误差为：

e_p(t)＝x(t)-p(σ(t)) (4)

其中e_p为路径跟踪误差，x(t)为AUV的状态量，p(σ(t))为期望路径；

考虑当前AUV的位姿状态[x,y,ψ]，以及路径p:x＝α₁(σ),y＝α₂(σ)，寻找曲线上离AUV当前状态最近的点为路径参考点，即归纳为如下形式：

其中α₁(σ)、α₂(σ)为参数方程；

通过求解上述优化问题式(5)，得到当前时刻的期望点的参数值σ(t)，即当前时刻的期望点的位置(x(σ(t)),y(σ(t)))；

期望航向角通过下式计算得到：

其中atan2是求方位角的函数，定义域范围为ψ∈(-π,π]；为参数方程；

路径跟踪误差e_p(t)包含AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离，当前的航向与期望的航向角ψ(σ(t))的差值。

3.根据权利要求1或2所述一种基于Lyapunov-MPC技术的自主水下机器人路径跟踪闭环控制方法，其特征在于：所述步骤三中使步骤二获得的路径跟踪误差e_p(t)收敛，得到AUV的控制输入，AUV的控制输入包括AUV的力矩和力；具体过程为：

通过引入辅助约束条件，重新构造AUV路径跟踪问题的LMPC控制框架：

其中J为性能函数；N为预测时域，e_p为跟踪误差，u(·)为控制输入，f为AUV水平面运动数学模型，h(·)为引入的基于Lyapunov方法的辅助控制律，V(·)为AUV控制系统对应的CLF；x(0)为AUV当前的状态，X为定义域，u(0)为当前的控制输入，U为定义域；Q、R为对应的加权矩阵，为x(t)一阶导数；CLF为控制Lyapunov函数；

对于引入的辅助控制律h(·)及AUV控制系统对应的Lyapunov函数，一定有

其中σ(x)为曲线参数；

利用反步设计方法得到式(7)中所需的基于Lyapunov方法的辅助控制律h(·)，将辅助控制律的表达形式带入式(7)得到优化问题；

求解约束优化问题(7)得到控制AUV的力矩和力。

4.根据权利要求3所述一种基于Lyapunov-MPC技术的自主水下机器人路径跟踪闭环控制方法，其特征在于：所述利用反步设计方法得到式(7)中所需的基于Lyapunov方法的辅助控制律h(·)；具体过程为：

首先作变量替换：

其中s为辅助镇定向量，z₁、z₂、η_e、η_d为变量；

结合式(1)得到新的被控系统：

其中为变量，τ为状态反馈控制律，为s的一阶导；

首先选取第一个Lyapunov函数V₁

对V₁求导，再联合式(10)，整理得

根据水平面坐标变换矩阵R(ψ)的正交性质，设计辅助镇定向量s

其中K₁为辅助控制律增益系数，为三阶正定对称阵；此时，变为

再选择第二个Lyapunov函数V₂

对V₂求导，再联合式(10)，整理得

设计状态反馈控制律

其中K₂为辅助控制律增益系数，代入到式(16)中，得

由于K₁和K₂均为正定的放大系数，所以有根据Lyapunov定理，基于状态反馈控制律(17)的闭环系统在平衡点全局渐近稳定；

将式(9)带入式(17)的τ中，得到辅助控制律的表达形式。

5.根据权利要求4所述一种基于Lyapunov-MPC技术的自主水下机器人路径跟踪闭环控制方法，其特征在于：所述求解约束优化问题(7)得到控制AUV的力矩和力；具体过程为：

求解约束优化问题(7)，得到预测时域内的最优控制序列：

其中为第一个时刻的最优控制输入，为二个时刻的最优控制输入，为第N个时刻的最优控制输入，N为MPC预测时域；

将最优控制序列的第一个控制量作为当前时刻控制AUV的力矩和力。