一种基于非线性模型预测控制的uuv对运动母船的跟踪方法
【专利摘要】一种基于非线性模型预测控制的UUV对运动母船的跟踪方法，涉及欠驱动UUV对运动母船的跟踪控制方法。为了解决现有的针对线性模型进行处理的方法对UUV进行控制时存在准确性不高的问题，本发明首先建立欠驱动UUV水平面的预测模型和相应的约束条件；然后选取综合性能指标，将欠驱动UUV对运动母船的跟踪控制问题转化为约束条件下的优化问题；并利用泰勒级数展开和李导数对步骤三中约束条件下的优化问题进行处理，并将求得的解析解作为UUV跟踪系统的控制输入；在每一预测时域，用新的状态值刷新步骤四中的解析解，以此不断运行，直至UUV完成对运动母船跟踪作业。本发明适用于欠驱动UUV对运动母船的跟踪控制。
【专利说明】
一种基于非线性模型预测控制的uuv对运动母船的跟踪方法
技术领域
[0001 ]本发明涉及欠驱动UUV对运动母船的跟踪控制方法。
【背景技术】
[0002] 无人水下航行器(Unmanned Underwater Vehicle,UUV)具有活动范围大、潜水深、 机动性好、智能化、运行和维护费用低等优点，作为人类在海洋活动中，特别是深海活动中 的重要替代者和执行者，已被广泛应用于科学考察，深海作业等领域，在作业完成时，需要 对其进行回收，尤其在采取运动母船背驼搭载回收过程中，需要UUV对运动母船进行准确跟 足示。
[0003] 目前，国内外学者针对UUV跟踪控制问题做了大量的研究，相应提出了很多控制方 法，如滑模控制，反步控制等等。滑模控制器不仅具有的对系统模型参数不确定及外界扰动 的鲁棒性，而且极大地提高了系统状态在平衡点附近的收敛速度。反步法计算简单，实时性 好，响应快，对传感器要求低，在工程应用中得到了广泛的应用，但该方法中存在虚拟控制 量的高阶导数问题。大多都未能够充分考虑UUV在约束条件下的跟踪控制问题，且在快速性 上有一定的局限，而模型预测控制(Model predictive control，MPC)可以很好地解决此类 问题，因此，提出将非线性模型预测控制应用到UUV对运动母船跟踪中具有重要的意义。
[0004] 2014年5月Proceedings of the Institution of Mechanical Engineers Part M: Journal of Engineering for the Maritime Environment第228卷中的 "Model predictive control of a hybrid autonomous underwater vehicle with experimental verification"提出将模型预测控制用于AUV悬停状态时深度控制，但该方 法只是针对线性模型进行处理，若将该方法迀移到UUV对运动母船跟踪控制中，在对模型进 行线性化时将影响控制的准确性，同时该方法为在线计算，将影响控制的快速性。
【发明内容】
[0005] 本发明是为了解决现有的针对线性模型进行处理的方法对UUV进行控制时存在准 确性不高的问题。
[0006] -种基于非线性模型预测控制的UUV对运动母船的跟踪方法，包括如下步骤：
[0007] 步骤一:建立欠驱动UUV水平面的预测模型；
[0008] 步骤二:根据实际，建立UUV水平面的预测模型相应的约束条件；
[0009] 步骤三:选取综合性能指标，将欠驱动UUV对运动母船的跟踪控制问题转化为约束 条件下的优化问题；
[0010] 步骤四：利用泰勒级数展开和李导数对步骤三中约束条件下的优化问题进行处 理，并将求得的解析解作为UUV跟踪系统的控制输入；
[0011] 步骤五:在每一预测时域，用新的状态值刷新步骤四中的解析解，以此不断运行， 直至UUV完成对运动母船跟踪作业。
[0012] 本发明具有以下效果：
[0013] 本发明提供了一种基于非线性模型预测控制的UUV对运动母船的跟踪方法，该方 法将UUV跟踪运动母船控制问题转换为一定约束条件下的优化问题，并利用泰勒级数展开 和李导数对优化问题进行处理，最终求得每一预测时域中控制输入的解析解，可以很好的 解决针对UUV非线性系统对运动母船的快速跟踪控制。该方法获得的解析解具备离线优化 的特点，不仅可以减小海流不确定性因素的累积作用，同时可以增强跟踪控制系统的快速 性。
【附图说明】
[0014] 图1为UUV的水平面建模图；
[0015] 图2为基于非线性模型预测控制的UUV对运动母船跟踪控制框图；
[0016] 图3为基于非线性模型预测控制的UUV对运动母船跟踪控制流程图；
[0017] 图4为UUV跟踪正弦运动母船时轨迹；
[0018] 图5为UUV跟踪正弦运动母船时纵向速度和横向速度;其中，图5a为UUV跟踪正弦运 动母船时纵向速度曲线图，图5b为UUV跟踪正弦运动母船时横向速度曲线图；
[0019] 图6为UUV跟踪正弦运动母船时艏向角及其加速度;其中，图6a为UUV艏向角的加速 度曲线图，图6b为UUV的艏向曲线图；
[0020] 图7为UUV跟踪正弦运动母船时纵向推力和转艏力矩;其中，图7a为UUV执行机构的 推力曲线图，图7b为UUV执行机构的转艏力矩曲线图；
[0021] 图8为UUV跟踪正弦运动母船时轨迹误差；其中，图8a为UUV在跟踪母船时X轴的坐 标误差曲线图，图8b为UUV在跟踪母船时y轴坐标误差曲线图；
[0022] 图9为UUV跟踪圆运动母船时轨迹；
[0023] 图10为UUV跟踪圆运动母船时纵向速度和横向速度;其中，图10a为UUV跟踪圆运动 母船时纵向速度曲线图，图l〇b为UUV跟踪圆运动母船时横向速度曲线图；
[0024]图11为UUV跟踪圆运动母船时艏向角及其加速度;其中，图11a为UUV跟踪圆运动母 船时加速度曲线图，图1 lb为UUV跟踪圆运动母船时艏向角曲线图；
[0025]图12为UUV跟踪圆运动母船时纵向推力和转艏力矩;其中，图12a为UUV跟踪圆运动 母船时纵向推力曲线图，图12b为UUV跟踪圆运动母船时转艏力矩曲线图；
[0026]图13为UUV跟踪圆运动的母船轨迹误差；其中，图13a为UUV跟踪圆运动的母船X轴 的坐标误差曲线图，图13b为UUV跟踪圆运动的母船y轴坐标误差曲线图。
【具体实施方式】
【具体实施方式】 [0027] 一:结合图1至图3说明本实施方式，
[0028] -种基于非线性模型预测控制的UUV对运动母船的跟踪方法，包括如下步骤：
[0029] 步骤一:建立欠驱动UUV水平面的预测模型；
[0030] 步骤二:根据实际，建立UUV水平面的预测模型相应的约束条件；
[0031] 步骤三:选取综合性能指标，将欠驱动UUV对运动母船的跟踪控制问题转化为约束 条件下的优化问题；
[0032] 步骤四：利用泰勒级数展开和李导数对步骤三中约束条件下的优化问题进行处 理，并将求得的解析解作为UUV跟踪系统的控制输入；
[0033]步骤五:在每一预测时域，用新的状态值刷新步骤四中的解析解，以此不断运行， 直至UUV完成对运动母船跟踪作业。
[0034]【具体实施方式】二：
[0035] 本实施方式步骤一所述的建立欠驱动UUV水平面的预测模型的具体过程如下：
[0036] 结合图1，建立了UUV的坐标系统，Ε-ξηζ是固定坐标系，0-ΧΥΖ是运动坐标系；固定 坐标系Ε-ξηζ的原点任取大地或海洋上一点，Εξ保持水平并指向地理北向，被选为UUV的主 航向；Εη指向地理东向，代表UUV的横向；Εζ指向地心，代表UUV的垂向；运动坐标系0-ΧΥΖ置 于UUV上，并随着其一起运动，其原点0置于UUV重心的位置;Οχ指向UUV前行的方向为正，绕 Οχ旋转产生横摇角速度Ρ，形成横摇角Φ ;〇y指向UUV的右舷方向为正，绕0y旋转产生纵倾角 速度q，形成纵倾角θ;〇Ζ指向UUV底部为正，绕0z旋转产生艏向角角速度r，形成艏向角Φ，根 据该坐标系统建立UUV水平面系统；
[0037] 假设UUV轨迹(Χ，7，Φ)和母船轨迹(xdjdjd)都是连续的且满足可以作多次微分运 算;UUV轨迹中的X、y为UUV在固定坐标系中的位置，Φ为UUV的艏向角，母船轨迹中的 Xd、yd为 母船在固定坐标系中的位置，加为母船在固定坐标系中的艏向角；
[0038] 定义x=[u v r X y Φ]τ为状态变量，定义y为UUV轨迹;其中，UUV的状态包括:x、 y、Φ、纵向速度u、横向速度v、艏向角角速度r;粗体X为状态变量，细体X为UUV在固定坐标系 中的位置;粗体y为UUV轨迹，细体y为UUV在固定坐标系中的位置；
[0039] 进一步令x=[u v r X y Φ]τ=[χι X2 X3 X4 X5 X6]T，得到UUV水平面预测模型：
[0040]
(6)
[0041 ]其中，X e R6，U e R2，y e R3，U为控制输入，由U1、U2共同构成，且ui = Xprop，U2 = Tr;粗 体u为控制输入，细体uSUUV纵向速度；同时为了表示方便，将[u ν r X y Φ]τ表示成[XI Χ2 Χ3 Χ4 Χ5 Χ6]Τ;
[0042] 同时给出式(6)中每项的具体形式：
[0043]
[0044]
[0045]
[0046] 其中，XprQP，Tr分别为主推进器推力和转艏力矩;cU为前进方向的水动力粘性项，d 2 横移方向的水动力粘性项，d3转艏方向的水动力粘性系数，nu为前进方向水动力附加质量， m2横移方向的水动力附加质量，m3为转艏方向的水动力附加质量。
[0047] 其它步骤及参数与【具体实施方式】一相同。
【具体实施方式】 [0048] 三：
[0049] 本实施方式步骤二所述的UUV水平面的预测模型相应的约束条件如下：
[0050] 对于UUV水平面预测模型，在某个预测时域[t，t+T]中，将满足约束：
[0051 ]
(?)
[0052] 其中τ为时间变量，且τ e [ 0，T ]; + Γ)为UUV在时域[t，t+T ]中状态变量的预测 值；+ Γ)为UUV在时域[t，t+T]中轨迹的预测值;邮+ Γ)为UUV在时域[t，t+T]中控制输 入；
[0053] 同时，在[t，t+T]预测时域中，状态变量预测值+ r)的初始值满足：
[0054]
(8)
[0055] 其中，t表示时刻，T表示预测时域的时间。
[0056] 其它步骤及参数与【具体实施方式】二相同。
[0057]【具体实施方式】四：
[0058]本实施方式步骤三具体过程如下：
[0059] 在每个预测时域中，根据控制要求选取综合性能指标为：
[0060]
[0061 ]其中，j)(i + Γ)为UUV在终端时亥Ijt+T轨迹预测值，+ Γ)为母船在终端时刻t+T 的轨迹，(i + r)为母船在时域[t，t+T]中的轨迹；
[0062] 通过最小化式（9)所给出的性能指标J，获得跟踪控制系统的最优控制输入 V
h ^并根据在步骤二中UUV水平面预测模型需要满足的约束，将UUV对运动 母船的跟踪问题转化为优化问题：
[0063]
(10)。
[0064]其它结构及参数与【具体实施方式】一至三之一相同。
【具体实施方式】 [0065] 五：
[0066]本实施方式步骤四具体过程如下：
[0067]根据模型预测控制基本原理，假设控制输入在预测时域[t，t+T]内为一个常值：
[0068]
(11)
[0069] 其中，const表示常数；
[0070]在此条件下，得到预测时域[t，t+T]中跟踪控制系统控制输入β的各阶导数皆为 零；
[0071] 利用泰勒级数展开和李导数对优化问题进行处理，根据泰勒级数定理，UUV轨迹的 预测输出iKi + 0以及母船轨迹免/ 0 0可以展开为：
[0072]
(12)
[0073] 粗体τ为以时间变量τ为自变量的函数；
[0074] 式中，所有带角标d的参数均表示母船对应参数；
[0079] 其中，Iffe)、⑴分别表示UUV轨迹预测分量&⑴和母船轨迹分量&(〇的i阶 时间导数;i = 〇,…，N;j = l，2,3; N为通过泰勒级数展开的最高阶次；
[0080] 将式(12)带入式(9)中，进一步将性能指标J整理为：
[0081]
? α3)
[0082] 其中，
为中间结果;μι为输出终端约束系 数，μ2为跟踪误差系数，W为控制输入系数；
[0083] 则式(10)等价为对式(13)关于? (6/)取最小极小值点，可得：
[0084]
C14)
[0085] 进一步将通过泰勒级数展开，j = 1，2，3，根据假设各阶导数等于零，即 if (?)二0, ill,(上面关于i的描述包括零阶导数（就是自身），而这里关于i的描述是从 一阶导数开始的，不矛盾的）；因此可以去掉中的各阶导数项，通过李导数得到 &(?)的各阶导数，j = 1，2，3;其求解结果如下所示：
[0086]
[0087]其中各系数为：
[0088] ql〇,〇 = X4
[0100] 同理可以推导出：
[0101]
[0102]其中各系数为： ;
[0115] 其中各系数为：
[0116] q3〇,〇 = h3 = X6
[0117] q3i,〇 = Lfh3 = X3
[0118] - +CA·
[0127] 选取泰勒级数展开阶次为N = 3,并取L = 2,则JK0的各阶导数表示成关于控制输 入的多项式，且相对应以Μ项的系数表示为：
[0128]
(18)
[0129] 其中各分量可以表示为：
[0130]
(1:9)
[0131] 因此预测输出，(/)可以进一步表示为：
[0132]
u ^ t j
[0133] 其中,Q(x) = [q · .,o(x)q · ·,ι(χ)…q · .,l(x)]为中间结果，为中间 结果；
[0134] 从上述可知，Λ(/}从二次方开始系数即为零，可以忽略g的二次方及以上项，1彡1 彡L，则有：
[0135]
(21)
[0136] 将(20)和(21)带入(14)得到：
[0137] ▲ V \ , ν …
…, ' w (2:2)
[0138] 其4
[0139] 于是得到非线性模型预测控制器算法控制输入的解析解：
[01401
(23)
[0141 ] 将作为UUV跟踪控制系统的控制输入。
[0142] 实施例
[0143] 根据【具体实施方式】五进行仿真实验，仿真结果如图4~图13所示；
[0144] 图4~图8分别给出了 UUV在无海流和常值海流uc = 0.5m/s，vc = 0.1m/s时跟踪正弦 运动母船的轨迹情况，图4可以看出UUV在初始位置时与母船正弦运动轨迹存在较大误差， 在非线性模型预测控制器作用下UUV快速的向母船方向运动，并逐渐减小误差，最终精准的 跟踪上正弦运动母船；图5a和图5b给出了UUV在两种情况下的纵向速度和横向速度，可以看 出在跟踪的初始时刻，纵向速度和横向速度都出现了较大变化，在使得UUV跟踪上运动母船 轨迹后，趋于平稳，并以周期性变化；图6a和图6b分别给出了UUV在两种情况下艏向角的加 速度和艏向变化；图7a和图7b给出了UUV执行机构的控制输入，即纵向推力和转艏力矩，可 以看出在跟踪的初始时刻，执行机构都出现了较大幅度的变化，在跟踪上UUV后，变化逐渐 趋于平稳；图8a和图8b给出了UUV在跟踪母船时X轴和y轴的坐标误差。图9~图13分别给出 了UUV在无海流和常值海流u c = 0.5m/s，vc = 0. lm/s时跟踪圆形运动母船的轨迹情况，同时 具有与跟踪正弦运动母船相同的规律。在实际应用中，可以通过调节控制参数进一步满足 控制要求。
【主权项】
1. 一种基于非线性模型预测控制的uuv对运动母船的跟踪方法，其特征在于包括如下 步骤： 步骤一:建立欠驱动UUV水平面的预测模型； 步骤二:建立UUV水平面的预测模型相应的约束条件； 步骤Ξ:选取综合性能指标，将欠驱动UUV对运动母船的跟踪控制问题转化为约束条件 下的优化问题； 步骤四：利用泰勒级数展开和李导数对步骤Ξ中约束条件下的优化问题进行处理，并 将求得的解析解作为UUV跟踪系统的控制输入； 步骤五:在每一预测时域，用新的状态值刷新步骤四中的解析解，W此不断运行，直至 UUV完成对运动母船跟踪作业。2. 根据权利要求1所述的一种基于非线性模型预测控制的UUV对运动母船的跟踪方法， 其特征在于步骤一所述的建立欠驱动UUV水平面的预测模型的具体过程如下： 假设UUV轨迹(x，y，il〇和母船轨迹(Xd，yd>d)都是连续的且满足作多次微分运算;UUV轨 迹中的x、y为UUV在固定坐标系中的位置，Φ为UUV的腊向角，母船轨迹中的xd、yd为母船在固 定坐标系中的位置，如为母船在固定坐标系中的腊向角； 定义x=[u V r X y Φ]τ为状态变量，定义y为UUV轨迹;其中，UUV的状态包括:x、y、iK纵 向速度U、横向速度V、腊向角角速度。粗体X为状态变量，细体X为UUV在固定坐标系中的位 置;粗体y为UUV轨迹，细体y为UUV在固定坐标系中的位置； 进一步令x=[u V r X y Φ]τ=[χι X2 X3 X4 X日Χ6]τ，得到UUV水平面预测模型：(白） 其中，义￡护，11￡护，7￡护，11为控制输入，由山、112共同构成，且山=乂。哪，112 = 1。粗体11为 控制输入，细体U为UUV纵向速度； 同时给出式(6)中每项的具体形式：其中，Xprnp，Tr分别为主推进器推力和转腊力矩;dl为前进方向的水动力粘性项，Cb横移 方向的水动力粘性项，d3转腊方向的水动力粘性系数，mi为前进方向水动力附加质量，m2横 移方向的水动力附加质量，m3为转腊方向的水动力附加质量。3. 根据权利要求2所述的一种基于非线性模型预测控制的UUV对运动母船的跟踪方法， 其特征在于步骤二所述的UUV水平面的预测模型相应的约束条件如下： 对于UUV水平面预测模型，在某个预测时域[t，t+T ]中，将满足约束：其中τ为时间变量，且τ e [0，T ] ; Χ(? +句为UUV在时域[t，t巧]中状态变量的预测值； 束片+ Γ)为UUV在时域[t，t巧忡轨迹的预测值;+ Γ)为UUV在时域[t，t巧忡控制输入； 同时，在[t，t+T]预测时域中，状态变量预测值的初始值满足： ?毛(/+ Γ)二 Λ：(〇,Γ = 0 (8) 其中，t表示时刻，Τ表示预测时域的时间。4. 根据权利要求3所述的一种基于非线性模型预测控制的UUV对运动母船的跟踪方法， 其特征在于步骤Ξ具体过程如下： 在每个预测时域中，选取综合性能指标为：其中，采〇 + r)为UUV在终端时刻t巧轨迹预测值，夫;(? + Γ)为母船在终端时刻t巧的轨 迹，九0 + r)为母船在时域[t，t巧]中的轨迹； 通过最小化式（9)所给出的性能指标J，获得跟踪控制系统的最优控制输入 Μ(? + Γ)，Γ e [0，巧，并根据在步骤二中UUV水平面预测模型需要满足的约束，将UUV对运动 母船的跟踪问题转化为优化问题：(10)。 s.t.式(7)和(8)约束5. 根据权利要求4所述的一种基于非线性模型预测控制的UUV对运动母船的跟踪方法， 其特征在于步骤四具体过程如下： 假设控制输入在预测时域[t，t+T]内为一个常值： ii{t + r) = ii{t) = const r g [0,7，] (II) 其中，const表示常数； 在此条件下，得到预测时域[t，t巧]中跟踪控制系统控制输入&的各阶导数皆为零； 利用泰勒级数展开和李导数对优化问题进行处理，UUV轨迹的预测输出+ 及母 船轨迹托片+了)展开为：(12j 粗体τ为W时间变量τ为自变量的函数； 式中，所有带角标d的参数均表示母船对应参数；其中，线to、網(〇分别表示UUV轨迹预测分量的和母船轨迹分量名如(〇的非介时间 导数;i = 0,…，N;j = l，2,3; N为通过泰勒级数展开的最高阶次； 将式(12)带入式(9)中，进一步将性能指标J整理为：其中为中间结果;μι为输出终端约束系数，μ2 为跟踪误差系数，μ3为控制输入系数； 则式(10)等价为对式(13)关于《(V；)取最小极小值点，得：(14) 进一步将^(0通过泰勒级数展开，j = 1，2，3，根据假设?的各阶导数等于零，即; 去掉名,0)中Μ(?)的各阶导数项，通过李导数得到^(〇的各阶导数，j = l，2,3; 选取泰勒级数展开阶次为N = 3,并取L = 2,则iK/)的各阶导数表示成关于控制输入 ?(ο的多项式，且相对应沒'的项的系数表示为：其中，Q(x) = [q · .,0(x)q · .,1(Χ)…q · .,L(x)]为中间结果：为中间结果； 沒的从二次方开始系数即为零，忽略技'的二次方及W上项，则有：(21) 将(20)和(21)带入(14)得到：于是得到非线性模型预测控制器算法控制输入的解析解：将作为UUV跟踪控制系统的控制输入。
【文档编号】G05D1/06GK105974930SQ201610239549
【公开日】2016年9月28日
【申请日】2016年4月18日
【发明人】张伟, 郭毅, 滕延斌, 梁志成, 孟德涛, 周佳加, 严浙平
【申请人】哈尔滨工程大学