一种在线深度估计的QMM-MPC水下机器人视觉对接控制方法

一种在线深度估计的qmm-mpc水下机器人视觉对接控制方法
技术领域
1.本发明涉及水下机器人视觉对接领域，具体而言，尤其涉及一种在线深度估计的qmm-mpc水下机器人视觉对接控制方法。


背景技术：

2.在过去的几十年中，自主水下机器人(autonomous underwater vehicle，auv)在测深测绘、检查、维修、探雷、环境监测、监视和干预等方面的应用取得了突飞猛进的发展，凭借其在复杂的水下环境中大幅提高作业效率的潜力，吸引了越来越多的学术界和工业界研究团队的高度关注。auv的主要特点是在执行任务过程中自主应对变化，保持其稳定控制性能，而不需要人的操作。考虑auv自身的复杂动力学特点、任务复杂性和环境的危险性，目前复杂环境下auv的控制问题成为热点研究领域之一。因此，近年来在水下设置回收平台实现auv的自主式回收成为一个重要的研究方向。截止目前，国内外针对auv的特点和回收平台的类型已设计了多种水下回收对接系统。但无论何种对接方式在最后进行近距离对接操作时的高精度要求始终是一个研究难点。
3.考虑当前的研究工作大都依赖于视觉伺服对auv的位姿进行估计来完成对接任务，但是水下环境复杂能见度低、光线折射、吸收和散射的严重影响视觉摄像头的深度信息，针对这个问题有学者提出了一种基于图像的视觉伺服，完全依赖于特征在图像平面上的运动，但是其技术手段只是将深度信息设为恒定值，并未彻底解决深度不确定问题，可能导致水下对接任务失败；以及传统基于图像的视觉伺服，大都是将深度值确定为恒值，已有研究表明假设深度信息为恒定值，导致对接任务失败率较高；还有由于传统qmm-mpc控制器仅设计一个李雅普诺夫上界，对于六自由度auv水下视觉对接等高精度、高实时性要求的任务保守性太强，从而导致任务失败。


技术实现要素：

4.根据上述提出现有auv水下对接任务环境复杂，深度不确定性等问题使得auv无法有效执行对接任务的技术问题，本发明提供本发明采用的技术方案是：
5.一种在线深度估计的qmm-mpc水下机器人视觉对接控制方法，包括以下步骤：
6.建立弱光工况下及深度信息不确定影响下的六自由度auv视觉伺服模型；
7.基于六自由度auv视觉伺服模型，结合auv和对接站特征点在图像平面上的运动测量数据，建立用于对水下机器人与对接站之间实时深度信息进行估计的在线深度估计器，并将实时深度估计值并入模型，得到带有在线深度估计器的auv视觉伺服模型；
8.根据控制目标，建立目标函数以及优化问题；
9.结合改进的qmm-mpc算法设计控制器，更新目标函数，建立总的lmis，求解得到水下机器人视觉对接控制系统，实现水下机器人与对接站的对接。
10.进一步地，所述建立弱光工况下及深度信息不确定影响下的六自由度auv视觉伺服模型，包括如下步骤：
11.通过世界坐标系、auv局部坐标系、摄像机坐标系和图像平面坐标系的转换关系建立相机透视模型；
12.基于相机透视模型，建立弱光工况下及深度信息不确定影响下的六自由度auv视觉伺服模型。
13.进一步地，所述带有在线深度估计器的auv视觉伺服模型表达式如下：
[0014][0015]
其中：
[0016][0017][0018]ei
(k)表示第i个特征点的图像平面坐标与期望图像平面坐标的横向差值m
ie
(k)，纵向差值n
ie
(k)；t为采样时间，v为水下机器人总速度向量，包括对各轴的线速度和角速度，给出第i个特征点在视觉伺服误差系统中的雅可比矩阵如下：
[0019][0020]
其中，f为摄像机焦距，为第i个特征点在图像平面上的期望坐标，表示第i个特征点估计的深度值，表达式如下：
[0021][0022]
其中，v1＝[u,v,w]
t
，v2＝[p,q,r]
t
分别代表auv在三坐标轴的线速度和角速度。最终可由最小二乘法求解并得到估计的深度值。
[0023]
进一步地，其特征在于，所述根据控制目标，建立目标函数以及优化问题，包括如下步骤，包括如下步骤：
[0024]
求取凸多面体，根据摄像机的分辨率可确定图像坐标m
ie
(k)和n
ie
(k)的变化范围为：将其变化边界带入雅可比矩阵l(p(k))，将l(p(k))分解为已知顶点矩阵的凸组合形式，使得任意采样时刻k，l(p(k))在顶点矩阵lr(r＝1,2,
…
,r)组成的凸多面体ω中变化，即：l(p(k))∈ω＝co{l1,l2,
…
,lr}；
[0025]
利用min-max控制策略对目标函数优化，得到优化问题。
[0026]
进一步地，所述优化问题如下:
[0027][0028]
其中：e(0|k)＝e(k)，为k+1时刻到无穷时刻的目标函数值，qe和qu分别为状态和控制输入加权矩阵。
[0029]
进一步地，所述凸多面体中不同顶点构成的视觉系统模态，为每个顶点设计李雅普诺夫函数，的具体过程如下；
[0030]
为凸面体的每个顶点设计李雅普诺夫函数如下：
[0031]vr
(e(i|k))＝e(i|k)
t
pr(k)e(i|k),r＝1,
…
,r
ꢀꢀꢀ
(25)
[0032]
其中，pr(k)为待求的对称正定矩阵，若使闭环系统渐近稳定，则vr(e(i|k))需满足：
[0033]vr
(e(i+1|k))-vr(e(i|k))≤-[e(i|k)
t
qee(i|k)+u(i|k)
tqu
u(i|k)]
ꢀꢀꢀ
(26)
[0034]
将式(26)左端从i＝1累加到∞可得：
[0035][0036]
闭环系统的渐近稳定性vr(e(∞|k))＝0，上式化简可得到针对不同顶点所构成的系统模态中的上界：
[0037][0038]
将状态反馈控制器u(i|k)＝f(k)e(i|k)代入递减性约束(26)可得：
[0039]
e(i|k)
t
{(i+l(i|k)f(k))
t
p(k)(i+l(i|k)f(k))-p(k)+qe+f(k)
tqu
f(k)}e(k+i|k)《0
ꢀꢀꢀ
(29)
[0040]
根据线性系统的性质，当各个多面体顶点满足约束(29)时，未来时刻的系统模型必定满足约束(29)，可以将(29)重写为以下形式：
[0041][0042]
并附加以下性能要求：
[0043]
v(0,k)＝e(k|k)
t
p(0|k)e(k|k)《γ
ꢀꢀꢀ
(31)
[0044]
根据(9)和(10)建立以下优化问题，并设计如下的线性矩阵不等式：
[0045][0046][0047][0048]
其中，
[0049]
根据舒尔补引理，由(13)式可得qr》0，g+g
t
》qr，矩阵满足非负定，因此；
[0050][0051]
根据(35)式，可将(33)式转化为：
[0052][0053]
将(36)左乘diag{qrg-t
,i,i,i}，右乘diag{g-1
qj,i,i,i}，可将(33)等价为以下lmi：
[0054][0055]
其中，g＝f-1
y，所述(16)为优化问题；
[0056]
优化问题(32)有解qr，r＝1,2,
…
,r和一对矩阵y，g。为求取的最终上界，定义qm＝min[qr]；可得的上界为e(1|k)
t
pm(k)e(1|k)，其中pm＝qm/γ。
[0057]
进一步地：所述更新目标函数，建立总的线性矩阵不等式组，求解得到水下机器人视觉对接控制系统的过程如下：
[0058]
根据当前时刻k，e(0|k)＝e(k)和u(0|k)＝u(k)，可得：
[0059][0060]
根据动能约束，摄像机视场约束，以及(10)和(17)的约束条件，六自由度auv视觉伺服准最大最小模型预测控制问题表示如下：
[0061][0062][0063]
进而可得如下的lmis：
[0064][0065][0066][0067]
为了使目标函数更易求解，令优化问题(18)的目标函数满足：
[0068]
e(k)
t
qee(k)+u(k)
tqu
u(k)+e(1|k)
t
pm(k)e(1|k)≤γ
ꢀꢀꢀ
(44)
[0069]
将e(1|k)＝e(k)+l(p(k))u(k)代入(41)，由舒尔补引理，可得等价条件：
[0070]
h(k)≥0
ꢀꢀꢀ
(45)
[0071]
对(40)中的递减性约束和控制输入约束应用舒尔补定理可得等价条件：
[0072]cr
(k)≥0,dr(k)≥0
ꢀꢀꢀ
(46)
[0073]
结合(44)-(46)，优化问题(39)可改写为：
[0074][0075][0076]
其中，(46)中e
min-e(k)≤b(p(k))u(k)≤e
max-e(k)为视觉可见性约束，将优化问题(39)转换为在线求解较为便捷的线性矩阵不等式描述，假定对该问题进行求解，求得最优解γ
*
，u(k)
*
，qm(k)
*
和y(k)
*
，则将auv视觉伺服控制器定义为u
mpc
(k)＝u(k)
*
，终端权值p(k)＝γqm(k)
*-1
，f(k)＝y(k)
*
g(k)
*
，相应的auv闭环视觉伺服控制系统为。
[0077]
e(k+1)＝e(k)+l(p(k))u
mpc
(k)
ꢀꢀꢀ
(49)
[0078]
本发明提供的一种在线深度估计的qmm-mpc水下机器人视觉对接控制方法，本方法通过auv和对接站特征点在图像平面上的运动数据设计在线深度控制器，并且将所估计得到的深度值作为信息并入qmm-mpc算法，同时设计多李雅普诺夫函数上界来降低传统qmm-mpc的强保守性，经过上述步骤最水下机器人视觉对接控制系统，实现水下机器人与对接站的对接。
[0079]
本发明采用基于图像的六自由度auv视觉伺服模型，使其完全依赖于特征在图像平面上的运动；
[0080]
本发明所设计的在线深度估计器只与特征提取精度相关，不受摄像机安装位置与auv运动的影响；
[0081]
本发明提出的实现水下机器人与对接站的对接，在融合深度估计信息保证精度的同时，借鉴多李雅普诺夫min-max控制策略，在求取上界的过程中，针对auv视觉伺服参变系
统模型的多个模态，分别设计李雅普诺夫函数，从而降低控制器保守性；
[0082]
本发明将auv水下对接实时优化问题转化为线性矩阵不等式的在线求解，大大降低了计算量问题，更适用于auv水下视觉对接等对实时性要求较高的工作任务。
附图说明
[0083]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0084]
图1是视觉对接系统在图像平面上的投影图；
[0085]
图2是有无深度估计器的摄像机轨迹图；
[0086]
图3(a)传统qmm-mpc算法(无深度估计器)图像平面图，(b)本技术改进的qmm-mpc算法图像平面图；
[0087]
图4(a)传统qmm-mpc算法误差变化图；(b)本技术改进的qmm-mpc算法误差变化图；
[0088]
图5(a)传统qmm-mpc算法的速度规划图；(b)本技术改进的qmm-mpc算法速度规划图。
具体实施方式
[0089]
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合，下面将参考附图并结合实施例来详细说明本发明。
[0090]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0091]
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
[0092]
除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当清楚，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员己知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任向具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。
[0093]
在本发明的描述中，需要理解的是，方位词如“前、后、上、下、左、右”、“横向、竖向、
垂直、水平”和“顶、底”等所指示的方位或位置关系通常是基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，在未作相反说明的情况下，这些方位词并不指示和暗示所指的装置或元件必须具有特定的方位或者以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制：方位词“内、外”是指相对于各部件本身的轮廓的内外。
[0094]
为了便于描述，在这里可以使用空间相对术语，如“在
……
之上”、“在
……
上方”、“在
……
上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其位器件或构造之下”。因而，示例性术语“在
……
上方”可以包括“在
……
上方”和“在
……
下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。
[0095]
此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本发明保护范围的限制。
[0096]
一种基于改进的带有在线深度估计器的qmm-mpc控制方法，包括以下步骤：
[0097]
s1：建立弱光工况下及深度信息不确定影响下的六自由度auv视觉伺服模型；
[0098]
s2:基于六自由度auv视觉伺服模型，结合auv和对接站特征点在图像平面上的运动测量数据，建立用于对水下机器人与对接站之间实时深度信息进行估计的在线深度估计器，并将实时深度估计值并入模型，得到带有在线深度估计器的auv视觉伺服模型；
[0099]
s3:根据控制目标，建立目标函数以及优化问题；
[0100]
s4:结合改进的qmm-mpc算法设计控制器，目的是降低原qmm-mpc算法的保守性；更新目标函数，建立总的lmis，求解得到水下机器人视觉对接控制系统，实现水下机器人与对接站的对接。
[0101]
图1是视觉对接系统在图像平面上的投影图；
[0102]
步骤s1、s2、s3、s4顺序进行；
[0103]
其中：改进的qmm-mpc算法(quasi-min-max model predictive control，简称qmm-mpc))也指的是多李雅普诺夫函数的准最大最小模型预测控制；
[0104]
进一步地，所述建立弱光工况下及深度信息不确定影响下的六自由度auv视觉伺服模型，包括如下步骤：
[0105]
通过世界坐标系、auv局部坐标系、摄像机坐标系和图像平面坐标系的转换关系建立相机透视模型；图2有无深度估计器的摄像机轨迹图；图中对比发现无深度估计器的算法虽然也能使得auv到达期望位姿，但摄像机轨迹很不理想，相比有深度估计器的本算法产生了很多额外的运动，大大浪费了auv能耗。
[0106]
基于相机透视模型，建立弱光工况下及深度信息不确定影响下的六自由度auv视觉伺服模型。
[0107]
进一步地，建立在线深度估计器，并将实时深度估计值并入模型，得到带有在线深度估计器的auv视觉伺服模型，包括：
[0108]
根据auv和对接站特征点在图像平面上的运动测量数据，设计在线深度估计器，可
用最小二乘法求解，实时得到深度估计值。
[0109]
采用欧拉近似离散六自由度auv视觉伺服模型得到离散时间下的六自由度auv视觉伺服模型。
[0110]
将深度估计值并入离散时间下的六自由度auv视觉伺服模型，从而得到带有在线深度估计器的auv视觉伺服模型。
[0111]
进一步地，根据控制目标建立目标函数以及优化问题；
[0112]
求取凸多面体，根据摄像机的分辨率可以确定图像坐标m
ie
(k)和n
ie
(k)的变化范围为：将其变化边界带入雅可比矩阵l(p(k))，可将l(p(k))分解为已知顶点矩阵的凸组合形式，使得任意采样时刻k，l(p(k))在顶点矩阵lr(r＝1,2,
…
,r)组成的凸多面体ω中变化，即：l(p(k))∈ω＝co{l1,l2,
…
,lr}；
[0113]
建立目标函数，得到优化问题；我们的控制目标是再尽量少的动能输入下，使得图像平面的误差在短时间趋近于零，基于控制目标并进一步分析，视觉伺服误差系统中雅可比矩阵l(p(k))中的视觉特征参数向量p(k)在当前k时刻可测量得知。而对于未来时刻的数值则不能提前预估得到，仅能保证其动能范围在凸多面体ω中。所以针对当前时刻以及未来时刻分别定义目标函数，由于未来时刻目标函数具有参数不确定性，需利用min-max控制策略对其进行优化，从而得到优化问题。
[0114]
进一步地，结合改进的qmm-mpc算法，来降低原qmm-mpc算法对系统的强保守性，为了获取最优性能指标，需求取最坏情况下的控制性能，即确定的上界，在传统qmm-mpc中，往往仅设计一个李雅普诺夫函数上界，而针对auv视觉对接系统，具有太强的保守性。本技术采用凸多面体中不同顶点构成的视觉系统模态，为每个顶点设计李雅普诺夫函数。最终求取最优的目标函数上界。
[0115]
进一步地，根据上述求取的最优目标函数上界，来更新目标函数，建立总的lmis，求解得到系统参数以及控制输入从，而得到水下机器人视觉对接控制系统，实现水下机器人与对接站的对接。
[0116]
进一步地，基于相机透视模型，建立弱光工况下及深度信息不确定影响下的六自由度auv视觉伺服模型。具体来说，包括以下步骤：
[0117]
已知世界坐标系{g}下第i个特征点的坐标pi＝(xi,yi,zi)与其在图像坐标系下的投影点pi＝(mi,ni)的坐标映射关系如下：
[0118][0119]
其中，zi表示第i个特征点的深度，mi，me分别表示相机的内参矩阵和外参矩阵，定义如下：
[0120]
[0121][0122]
其中，f代表摄像机的焦距，(m0,n0)表示图像平面中心像素坐标，每个像素点在图像物理坐标系的x轴和y轴的尺寸为dx和dy；r，t分别代表摄像机坐标系{c}到世界坐标系{g}的旋转矩阵和平移向量。旋转矩阵r由zyx欧拉角生成，可由下式描述：
[0123][0124]
其中，在zyx欧拉角中的ψ，θ和是指相机先绕z轴旋转ψ角度，在绕y轴旋转θ角度，再绕x轴旋转角度。
[0125]
设特征点在世界坐标系下的坐标为pi＝[xi,yi,zi]
t
∈3，对pi求导可得：
[0126][0127]
其中，v1＝[u,v,w]
t
，v2＝[p,q,r]
t
分别代表auv在三坐标轴的线速度和角速度；
[0128]
根据相机的透视投影，给出下式：
[0129][0130]
其中，si＝[mi,ni]为第i个特征点在图像平面上当前图像坐标，为图像平面上的期望图像坐标。对公式(6)求导，并结合(5)式可得视觉伺服模型如下：
[0131][0132]
其中，
[0133]
为相机速度与成像率之间的图像雅可比矩阵；v＝[v1,v2]
t
表示auv的速度向量，zi表示第i个特征点的深度信息；考虑视觉跟踪需要n个视觉特征点，当前图像坐标和期望图像坐标可以定义为：
[0134][0135]
根据(7)式可得auv视觉伺服模型如下：
[0136][0137]
其中，z＝[z1,
…
,zi,
…
,zn]
t
∈
2n
为每个特征点对应的深度向量，l(s,z)代表堆叠的雅可比矩阵：
[0138][0139]
进一步地，基于六自由度auv视觉伺服模型，结合auv和对接站特征点在图像平面上的运动测量数据，建立用于对水下机器人与对接站之间实时深度信息进行估计的在线深度估计器，最终得到带有在线深度估计器的auv视觉伺服模型。具体来说：
[0140]
基于auv和图像的运动数据，设计在线深度值估计器.考虑第i个特征点的视觉伺服模型(9),可知雅可比矩阵中前三列与深度相关，将式(9)重新整理可得:
[0141][0142]
其中，j
t
和j
ω
分别代表相机的平移运动和旋转运动对图像特征矢量的影响,分别表达如下:
[0143][0144]
得到紧凑的视觉伺服系统线性方程如下:
[0145]
aθ＝b
ꢀꢀꢀ
(12)
[0146]
其中，a＝j
t
v1,b是剩余光流，即，观察到的光流与相机旋转即将产生的光流差，对(12)求解可得:
[0147][0148]
其中，是深度估计值，为模型预测控制器的设计提供高度近似与真实深度值的深度估计信息；
[0149]
考虑auv视觉伺服模型(9)，进一步的定义视觉跟踪误差:
[0150][0151]
对式(14)求导可得:
[0152][0153]
考虑auv视觉伺服误差系统(15),选取t为采样时间,利用欧拉近似离散系统,可得带有在线深度估计器的auv视觉伺服模型:
[0154][0155]
其中,e(k)＝[e1(k),
…
,ei(k),
…
,en(k)]
t
＝[m
1e
(k),n
1e
(k),
…
,m
ie
(k),n
ie
(k),
…
,m
ne
(k),n
ne
(k)]
t
，表示每个特征点在图像平面上的横向和纵向误差.
[0156][0157]
其中,是所有特征点在线估计的深度值向量，第i个特征点得雅可比矩阵可表示如下:
[0158][0159]
进一步地，基于六自由度auv视觉伺服模型和控制目标，建立目标函数以及优化问题：
[0160]
由(18)式可知，雅可比矩阵是关于变量m
ie
(k)和n
ie
(k)的函数，即随着参变向量p(k)＝[m
ie
(k),n
ie
(k)]的变化而变化.根据摄像机的分辨率，可以确定图像坐标m
ie
(k)和n
ie
(k)的变化范围分别为:l(p(k))可以分解为已知顶点矩阵的凸组合形式,使得任意采样时刻k,l(p(k))在顶点矩阵lr(r＝1,2,
…
,r)组成的凸多面体ω中变化,即:
[0161]
l(p(k))∈ω＝co{l1,l2,
…
,lr}
ꢀꢀꢀ
(19)
[0162]
考虑误差系统(16)满足以下约束:
[0163][0164][0165]
考虑(18)-(21)，雅可比矩阵l(p(k))中的视觉特征参数向量p(k)在当前k时刻可测量得知.而对于未来时刻的数值不能提前预估得到,仅能保证其动能范围在凸多面体ω中，因此，分别对当前时刻以及未来时刻设计预测控制器：
[0166]
定义目标函数如下:
[0167][0168]
其中u(0|k)＝u(k)＝[v1,v2],e(0|k)＝e(k),为k+1时刻到无穷时刻的目标函数值，qe和qu分别为状态和控制输入加权矩阵。由于未来时刻目标函数具有参数不确定性,需利用min-max控制策略对其进行优化，等价于对进行优化.设定由于未来时刻不确定性,所造成最差情况下的性能为对其极小化可得到最优性能指标。从而得到优化问题如下:
[0169][0170]
其中，
[0171]
为k+1时刻到无穷时刻的控制输入，e(0|k)＝e(k)，为k+1时刻到无穷时刻的目标函数值，qe和qu分别为状态和控制输入加权矩阵。
[0172]
进一步地，为了使所述水下机器人视觉对接控制系统稳定，设计一种引入深度估计信息的改进准最大最小模型预测控制器设计，主要分为两步，具体来说如下：
[0173]
s41、在传统qmm-mpc中，仅有一个李雅普诺夫函数上界，而针对auv视觉对接系统，具有太强的保守性，会导致无法给auv规划合适的速度从而导致对接任务失败；采用凸多面体中不同顶点构成的视觉系统模态，为每个顶点设计李雅普诺夫函数，从而降低传统qmm-mpc的强保守型，具体如下(注：(i|k)表示在k时刻对k+i时刻的预测)：
[0174]
针对上述优化问题(23)，定义为状态反馈控制律：
[0175]
u(i|k)＝f(k)e(i|k)，i＝1,2,
…
,∞
ꢀꢀꢀ
(24)
[0176]
其中f(k)为反馈控制增益。
[0177]
为每个顶点设计李雅普诺夫函数vr(e(i|k))如下：
[0178]vr
(e(i|k))＝e(i|k)
t
pr(k)e(i|k),r＝1,
…
,r
ꢀꢀꢀ
(25)
[0179]
其中，pr(k)为待求的对称正定矩阵，r为顶点数量，若使闭环系统渐近稳定，则李雅普诺夫函数vr(e(i|k))需满足，
[0180][0181]
将式(26)左端从i＝1累加到∞可得：
[0182][0183]
考虑闭环系统的渐近稳定性vr(e(∞|k))＝0，上式化简可得到针对不同顶点所构成的系统模态中的上界，为k+1时刻到无穷时刻的目标函数值，qe和qu分别为状态和控制输入加权矩阵：
[0184][0185]
进一步，将状态反馈控制器u(i|k)＝f(k)e(i|k)(24)代入递减性约束(26)可得：
[0186]
e(i|k)
t
{(i+l(i|k)f(k))
t
p(k)(i+l(i|k)f(k))-p(k)+qe+f(k)
tqu
f(k)}e(k+i|k)《0
ꢀꢀꢀ
(29)
[0187]
根据线性系统的性质，当各个多面体顶点满足约束(29)时，未来时刻的系统模型必定满足约束(29)，可以将(29)重写为以下形式：
[0188][0189]
并附加以下性能要求：
[0190]
v(0,k)＝e(k|k)
t
p(0|k)e(k|k)《γ
ꢀꢀꢀ
(31)
[0191]
根据(30)和(31)建立以下优化问题，并设计如下的线性矩阵不等式：
[0192]
[0193][0194][0195]
其中，
[0196]
根据舒尔补引理，由(34)式可得qr》0，g+g
t
》qr，进一步，矩阵满足非负定，因此
[0197][0198]
根据(35)式，可将(33)式转化为：
[0199][0200]
将(36)左乘diag{qrg-t
,i,i,i}，右乘diag{g-1
qj,i,i,i}，可将(33)等价为以下lmi：
[0201][0202]
其中，g＝f-1
y，
[0203]
如果优化问题(32)有解qr，r＝1,2,
…
,r和一对矩阵y，g。为求取的最终上界，定义qm＝min[qr]，可得的上界为e(1|k)
t
pm(k)e(1|k)，其中pm＝qm/γ；
[0204]
s42、更新目标函数，建立总的线性矩阵不等式组(lmis)，具体如下：
[0205]
考虑当前时刻k，e(0|k)＝e(k)和u(0|k)＝u(k)，整理(22)式可得，
[0206][0207]
根据(20)，(21)，(30)和(38)，auv视觉伺服准最大最小模型预测控制问题表示如下：
[0208]
[0209][0210]
进而可得如下的lmis：
[0211][0212][0213][0214]
为了使目标函数更易求解，令优化问题(39)的目标函数满足：
[0215]
e(k)
t
qee(k)+u(k)
tqu
u(k)+e(1|k)
t
pm(k)e(1|k)≤γ
ꢀꢀꢀ
(44)
[0216]
将e(1|k)＝e(k)+l(p(k))u(k)代入(41)，由舒尔补引理，可得等价条件：
[0217]
h(k)≥0
ꢀꢀꢀ
(45)
[0218]
对(40)中的递减性约束和控制输入约束应用舒尔补定理可得等价条件：
[0219]cr
(k)≥0,dr(k)≥0
ꢀꢀꢀ
(46)
[0220]
结合(44)-(46)，优化问题(39)可改写为：
[0221][0222][0223]
其中，(46b)最后一个约束为视觉可见性约束。将优化问题(39)转换为在线求解较为便捷的线性矩阵不等式描述。假定该问题进行求解，求得最优解γ
*
，u(k)
*
，qm(k)
*
和y(k)
*
。则将auv视觉伺服控制器定义为u
mpc
(k)＝u(k)
*
，终端权值p(k)＝γqm(k)
*-1
，f(k)＝y(k)
*
g(k)
*
，相应的auv闭环视觉伺服控制系统为：
[0224]
e(k+1)＝e(k)+l(p(k))u
mpc
(k)
ꢀꢀꢀ
(49)
[0225]
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
[0226]
图3(a)传统qmm-mpc算法(无深度估计器)图像平面图，(b)本技术改进的qmm-mpc算法图像平面图；对比(a)、(b)可知传统qmm-mpc算法(无深度估计器)相比，本技术图像平
面上的图像轨迹更加曲折而且产生严重的回退,呈现锯齿状。
[0227]
图4(a)传统qmm-mpc算法误差变化图；(b)本技术改进的qmm-mpc算法误差变化图；对比(a)、(b)可知本技术相比传统算法无论是在收敛性，还是响应时间方面都具有更好的控制性能。
[0228]
图5(a)传统qmm-mpc算法的速度规划图；(b)本技术改进的qmm-mpc算法速度规划图。对比(a)、(b)可知，本技术相比于传统算法规划的速度值更大且满足约束，能使得auv更快的到达期望位置，大大减小了传统算法的强保守性。
[0229]
在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
[0230]
在本技术所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。
[0231]
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
[0232]
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
[0233]
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
[0234]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。