CN102880062B - 基于非线性模型预测的智能小车2.5维视觉伺服控制方法 - Google Patents

Abstract

本发明公开一种基于非线性模型预测的智能小车2.5维视觉伺服控制方法，首先在当前位姿和期望位姿处通过摄像机分别获取参考目标的当前图像和期望图像；然后从获取的图像中提取参考目标的特征点以及小车的姿态信息，通过坐标变换将二维图像信号与三维姿态信号进行有机结合，建立2.5维视觉误差模型；最后针对2.5维视觉误差模型，利用非线性模型预测控制方法设计一种多级视觉预测控制器。本发明解决现有2.5维视觉伺服控制方法不能处理运动执行系统存在的速度和力矩约束以及摄像机的可见性约束问题，能够确保参考目标在伺服过程中始终保持可见，大大提高视觉伺服系统的可靠性和安全性。

Description

基于非线性模型预测的智能小车2.5维视觉伺服控制方法

技术领域

本发明涉及智能机器人技术领域，具体地说是指一种基于非线性模型预测的智能小车2.5维视觉伺服控制方法。

技术背景

智能小车视觉伺服是指利用视觉传感器获取的视觉信息对小车位置和方向进行精确控制。视觉伺服对于提高小车的智能化水平与工作能力具有非常重要的意义，可以提高小车对外界环境的学习和适应能力。按照反馈信息类型的差别，视觉伺服可以分为三维视觉伺服、二维视觉伺服以及2.5维视觉伺服，其中2.5维视觉伺服是一种将二维信息与三维信息有机结合的混合伺服方法，可以在一定程度上解决三维和二维视觉伺服存在的鲁棒性、奇异性、局部极小等问题。因此，智能小车2.5维视觉伺服越来越受到广泛关注。

近年来，国内外许多研究学者对2.5维视觉伺服控制方法做了大量的研究。FangYongchun等人在2005年IEEE Transactions on Systems,Man,and Cybernetics,Part B:Cybernetics,35(5)期刊上设计了一种基于单应矩阵的2.5维视觉伺服控制策略，这种方法在深度信息未知的情况下仍然能够使移动机器人渐近运动到由一副参考图像定义的期望位姿。Wang Chaoli在2011年IEEE International Conference on Robotics andAutomation会议上采用两阶段技术设计了一种鲁棒视觉控制器，在缺乏深度信息和精确的视觉参数情况下实现了对移动机器人图像位置和三维姿态的控制。针对系统模型参数的不确定性及外界干扰，Yang Fang等人在2011年Acta Automatica Sinica，37(7)期刊上采用自适应控制和滑模控制技术来设计视觉控制器。

以上方法对于实现智能小车2.5维视觉伺服控制具有重要的借鉴作用，但并未考虑运动执行系统存在的速度和力矩约束及摄像机的可见性约束，在伺服过程中参考目标可能偏离于摄像机的视野之外，从而致使伺服失败。

发明内容

本发明的目的在于提供一种基于非线性模型预测的智能小车2.5维视觉伺服控制方法，以解决现有2.5维视觉伺服控制方法在设计视觉控制器时未考虑运动执行系统存在的控制约束和摄像机的可见性约束问题。本发明方法利用非线性模型预测控制设计一种多级视觉预测控制器，控制小车从初始位姿运动到期望位姿同时使参考目标始终处于摄像机视野之内。

本发明方法采用的技术方案如下：

1)在当前位姿和期望位姿处通过摄像机分别获取参考目标的当前图像和期望图像；

2)从获取的当前图像和期望图像中提取参考目标的特征点及小车的姿态信号，建立2.5维视觉误差模型；

设目标特征点在当前图像中的位置为p=[p_x，p_y]^T，在期望图像中的位置为p_d=[p_xd，p_yd]^T，θ和θ_d分别为智能小车的当前方向角和期望方向角，通过坐标变换将二维图像信号与三维姿态信号进行有机结合，构造一种2.5维视觉误差信号，其中s₁，s_1d，s₂，s_2d为经过坐标变换的二维图像信号，θ_ed为三维姿态误差信号；

3)根据步骤2)中的2.5维视觉误差模型，利用非线性模型预测控制方法设计一种多级视觉预测控制器，使2.5维视觉误差信号渐近收敛，进而完成小车视觉伺服任务。

步骤3)中设计的多级视觉预测控制器至少由两个子控制器组成，一个为运动学控制器，另一个为动力学控制器，其中运动学控制器的输出作为动力学控制器的输入。

运动学控制器的控制包含以下步骤：

首先，采用欧拉近似法对2.5维视觉误差模型进行离散化处理，获得视觉误差预测模型e(k+j|k)，其中，T为采样周期，v(k)和ω(k)分别为小车在k时刻的线速度和角速度，符号(k+j|k)表示在k时刻向前预测j步得到的预测值；

其次，将小车速度ν=[v,ω]^T作为控制输入，构造一个由2.5维视觉误差预测信号和控制输入表示的二次目标函数J(k)，其中N_p为预测时域，N_c为控制时域，Q和R为正定的斜对称加权矩阵；

最后，加入可见性约束(u_min，，u_max)，(υ_min，，υ_max)和速度约束(v_min，,v_max)，其中(u,υ)为目标特征点在图像平面上的像素坐标，通过求解带有约束的二次目标函数J(k)的优化问题来获得最优控制序列v^*，取其第一项v(k|k)作为运动学控制器的输出，使得视觉误差信号渐近收敛，即lim_t→∞e=0；

动力学控制器的控制包含以下步骤：

首先，利用欧拉近似法对小车动力学模型进行离散化处理，获得小车实际速度预测模型v(k+j|k)，其中τ(k)=[τ₁(k),τ₂(k)]^T分别为小车在k时刻的驱动力矩和转动力矩；

其次，将运动学控制器的输出作为动力学控制器的参考输入，定义δ为小车实际速度与参考输入速度之间的速度误差，构造一个由预测的速度误差信号和力矩表示的二次目标函数其中为预测时域，为控制时域，和为正定的斜对称加权矩阵；

最后，加入力矩约束(τ_min,,τ_max)，最小化二次目标函数获得最优控制序列τ^*，取其第一项τ(k|k)作为动力学控制器的输出，控制小车速度，使得速度误差渐近收敛，即lim_t→∞δ=0，从而使得lim_t→∞e=0。

与现有技术相比，本发明具有以下优点及有益效果：

本发明利用非线性模型预测控制技术将2.5维视觉伺服任务转化为非线性优化问题，在设计视觉控制器时能够方便处理系统中存在的约束。本发明解决现有2.5维视觉伺服控制方法不能处理运动执行系统存在的速度和力矩约束以及摄像机的可见性约束问题，能够确保参考目标在伺服过程中始终保持可见，大大提高视觉伺服系统的可靠性和安全性。本发明实现简单，鲁棒性强，不需要精确的过程模型。

附图说明

图1是本发明中的一个两轮智能小车系统模型示意图；

图2是本发明中的智能小车2.5维视觉伺服控制方法结构示意图；

图3～8是本发明中的未考虑可见性约束时智能小车2.5维视觉伺服控制仿真结果图；其中：

图3为智能小车在XY平面上的运动轨迹仿真图；

图4为特征点在图像平面上的运动轨迹仿真图；

图5为2.5维视觉误差曲线图；

图6为智能小车实际线速度和期望线速度曲线图；

图7为智能小车实际角速度和期望角速度曲线图；

图8为智能小车驱动力矩和转动力矩曲线图；

图9～14是本发明中的考虑可见性约束时智能小车2.5维视觉伺服控制仿真结果图；其中：

图9为智能小车在XY平面上的运动轨迹仿真图；

图10为特征点在图像平面上的运动轨迹仿真图；

图11为2.5维视觉误差曲线图；

图12为智能小车实际线速度和期望线速度曲线图；

图13为智能小车实际角速度和期望角速度曲线图；

图14为智能小车驱动力矩和转动力矩曲线图。

具体实施方式

为了更好地理解本发明的技术方案，下面结合实施例和附图对本发明的实施方式做进一步的介绍。

实施例：

如图1所示为两轮智能小车系统，左右轮独立驱动，摄像机固定安装在小车上。O-XYZ、o-x_ry_r、C-x_cy_cz_c分别为世界坐标系、机器人坐标系和摄像机坐标系，小车质心o位于两驱动轮的中心，摄像机原点C与质心o的距离为l，r为轮子半径，L为轮子到质心的距离。视觉伺服的任务是：利用摄像机获取的视觉信息控制小车从初始位姿运动到期望位姿，并保证目标在运动过程中始终可见。

实施步骤如下：

1)在当前位姿和期望位姿处通过摄像机分别获取参考目标的当前图像和期望图像；

2)从获取的当前图像和期望图像中提取参考目标的特征点及小车的姿态信号，建立2.5维视觉误差模型；

设所提取的目标特征点在当前图像中的位置为p=[p_x，p_y]^T，在期望图像中的位置为p_d=[p_xd，p_yd]^T，θ和θ_d分别为移动小车的当前方向角和期望方向角，为了便于后续控制器的设计，对二维图像坐标进行如下坐标变换：

$s=\left[\begin{array}{c}{s}_1\\ s_2\end{array}\right]=\left[\begin{array}{c}\frac{p_x}{p_y}\\ -\frac{f}{p_y}\end{array}\right],$ $s_d=\left[\begin{array}{c}{s}_{1d}\\ s_{2}d\end{array}\right]=\left[\begin{array}{c}\frac{p_{\mathrm{xd}}}{p_{y}d}\\ -\frac{f}{p_{\mathrm{yd}}}\end{array}\right].---\left(1\right)$

其中f为摄像机的焦距。

将二维图像信号与三维姿态信号有机结合，构造如下2.5维视觉误差信号：

$e=\left[\begin{array}{c}{e}_1\\ e_2\\ e_3\end{array}\right]=\left[\begin{array}{c}{s}_1\\ s_2\\ \mathrm{\θ}\end{array}\right]-\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_e& -\sin {\mathrm{\θ}}_2& 0\\ \sin {\mathrm{\θ}}_e& \cos {\mathrm{\θ}}_e& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{s}_{1d}\\ s_{2d}\\ {\mathrm{\θ}}_d\end{array}\right],---\left(2\right)$

其中θ_e=θ-θ_d为三维姿态误差信号，可以利用匹配的特征点采用运动估计技术直接估计得到。

从式(2)可以看出，当误差e →0时，可以得到：

s₁→s_1d,s₂→s_2d,θ→θ_d             (3)

根据式(1)和式(3)可以得到：p_x→p_xd，p_y→p_yd，θ_e→0，也就是说当前图像与期望图像重合，小车运动到期望位姿。

因此，2.5维视觉伺服的控制目标是设计一个控制器τ=[τ₁,τ₂]^T，使得lim_t→∞e=0且lim_t→∞τ=0。

为了建立2.5维视觉误差信号(2)式的动态模型，还需进行以下步骤：

首先，建立智能小车运动学模型：根据图1所定义的坐标系，在不考虑轮子侧滑等因素时，智能小车的运动学模型可表示为：

$\stackrel{.}{q}=\left[\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\\ \stackrel{.}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}-\sin \mathrm{\θ}& 0\\ \cos \mathrm{\θ}& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right],---\left(4\right)$

其中q=[x,y,θ]^T为小车的位姿，(x,y)为小车质心o的坐标，θ为小车方向角，v、ω分别为小车的线速度和角速度。

其次，构建摄像机投影模型：如图1所示，在场景中考虑四个静态点作为参考目标的特征点，根据透视原理可以得到如下摄像机投影模型：

$\left\{\begin{array}{c}u={\mathrm{fk}}_u\frac{x_c}{z_c}+u_0,\\ \mathrm{\υ}={\mathrm{fk}}_{\mathrm{\υ}}\frac{y_c}{z_c}+{\mathrm{\υ}}_0,\end{array}\right.---\left(5\right)$

其中(x_c,y_c,z_c)^T为特征点P在摄像机坐标系下的坐标，(u,υ)^T为相应的图像像素坐标，k_u、k_υ为图像坐标中单位距离的像素数量，(u₀,υ₀)^T为图像坐标中心点的位置。

根据式(5)可计算得到特征点在图像平面上的位置：

$\left[\begin{array}{c}{p}_x\\ p_y\end{array}\right]=\left[\begin{array}{c}f\frac{x_c}{z_c}\\ f\frac{y_c}{z_c}\end{array}\right]=\left[\begin{array}{c}\frac{u-u_0}{k_u}\\ \frac{\mathrm{\υ}-{\mathrm{\υ}}_0}{k_{\mathrm{\υ}}}\end{array}\right].---\left(6\right)$

最后，通过几何分析法构建参考目标的特征点在摄像机坐标系下的运动学模型：通过几何分析法对图1所示坐标系统的相对位置关系进行分析，可以得到如下方程：

$\left\{\begin{array}{c}a=x_r\cos \mathrm{\θ}-y_r\sin \mathrm{\θ}+x,\\ b=x_r\sin \mathrm{\θ}+y_r\cos \mathrm{\θ}+y\\ x_r=x_c,\\ y_r=z_c+l,\end{array}\right.---\left(7\right)$

其中(a,b)、(x_r,y_r)分别为特征点P在世界坐标系和机器人坐标系下的坐标。

对式(7)进行求导，并考虑式(4)可得特征点在摄像机坐标系下的运动学模型：

$\left\{\begin{array}{c}{\stackrel{.}{x}}_c=z_c\mathrm{\ω}+\mathrm{l\ω},\\ {\stackrel{.}{z}}_c=-v-x_c\mathrm{\ω}.\end{array}\right.---\left(8\right)$

在以上模型的基础上，对式(2)进行求导，并考虑式(1)、(6)、(8)，可得到2.5维视觉误差模型如下：

$\stackrel{.}{e}=\left[\begin{array}{c}{\stackrel{.}{e}}_1\\ {\stackrel{.}{e}}_2\\ {\stackrel{.}{e}}_3\end{array}\right]=\left[\begin{array}{c}-\mathrm{\ω}{e}_2+\mathrm{\ωl\α}\\ \mathrm{v\α}+\mathrm{\ω}{e}_1\\ \mathrm{\ω}\end{array}\right],---\left(9\right)$

其中由于小车做平面运动，因此α是个常数，这里我们假设特征点在摄像机坐标系下的高度信息y_c≠0。

3)根据步骤2)中的2.5维视觉误差模型，利用非线性模型预测控制方法设计一种多级视觉预测控制器，使2.5维视觉误差信号渐近收敛，进而完成小车视觉伺服任务。

多级视觉预测控制器至少由两个子控制器组成，一个为运动学控制器，另一个为动力学控制器，其中运动学控制器的输出作为动力学控制器的输入。

运动学控制器的设计包含以下步骤：

首先，采用欧拉近似法对2.5维视觉误差模型(9)进行离散化处理，获得如下离散时间误差模型：

$e(k+1)=\left[\begin{array}{c}{e}_1(k+1)\\ e_2(k+1)\\ e_3(k+1)\end{array}\right]=\left[\begin{array}{c}{e}_1\left(k\right)-\mathrm{T\ω}\left(k\right)e_2\left(k\right)+\mathrm{T\ω}\left(k\right)\mathrm{l\α}\\ e_2\left(k\right)+\mathrm{Tv}\left(k\right)\mathrm{\α}+\mathrm{\ω}\left(k\right)e_1\left(k\right)\\ e_3\left(k\right)+\mathrm{T\ω}\left(k\right)\end{array}\right],---\left(10\right)$

其中T是采样周期。

式(10)可以写成以下简洁形式：

e(k+1)=f(e(k),v(k)).             (11)

因此，考虑式(11)可以得到2.5维视觉误差预测模型：

e(k+j|k)=f(e(k+j-1|k),v(k+j-1|k)),j∈[1,Np].     (12)

其中符号(k+j|k)表示在k时刻向前预测j步得到的预测值。

其次，将小车速度v=[v,ω]^T作为控制输入，构造一个由2.5维视觉误差预测信号和控制输入表示的二次目标函数J(k)：

$J\left(k\right)=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{e}^T(k+j|k)\mathrm{Qe}(k+j|k)+$ (13)

$\underset{j=1}{\overset{N_c}{\mathrm{\Σ}}}{v}^T(k+j-1|k)\mathrm{Rv}(k+j-1|k),$

其中N_p为预测时域，N_c为控制时域，Q和R为正定的斜对称加权矩阵。

最后，为了确保目标特征点在伺服过程中始终处于摄像机视野范围内及小车在高速度下能够安全行驶，考虑如下可见性约束和速度约束：

可见性约束，即将特征点在图像平面上的运动轨迹限制在一个定义的窗口区域内：

$\left\{\begin{array}{c}{u}_{\min }\≤u\≤u_{\max }\\ {\mathrm{\υ}}_{\min }\≤\mathrm{\υ}\≤{\mathrm{\υ}}_{\max }.\end{array}\right.---\left(14\right)$

速度约束，即将小车速度限制在一个有界区域内：

v_max≤v≤v_min              (15)

因此，运动学预测控制器的设计转换为求解如下优化问题：

$v^{*}=\arg \underset{v}{\min }J\left(k\right),---\left(16\right)$

受限于

$\left\{\begin{array}{c}e\left(k\right|k)=e_0,\\ e(k+j|k)=f(e(k+j-1|k),v(k+j-1|k)),j\∈[1,N_p],\\ u_{\min }\≤u(k+j|k)\≤u_{\max },{\mathrm{\υ}}_{\min }\≤\mathrm{\υ}(k+j|k)\≤{\mathrm{\υ}}_{\max },j\∈[0,N_p],\\ v_{\min }\≤v(k+j|k)\≤v_{\max },j\∈[0,N_p-1]\end{array}\right.---\left(17\right)$

其中e₀为初始条件，可通过式(1)、(2)和(6)计算得到：

$e\left(k\right|k)=\left[\begin{array}{c}{e}_1\left(k\right|k)\\ e_2\left(k\right|k)\\ e_3\left(k\right|k)\end{array}\right]=$

$\left[\begin{array}{c}\frac{k_u(u\left(k\right|k)-u_0)}{k_{\mathrm{\υ}}(\mathrm{\υ}\left(k\right|k)-{\mathrm{\υ}}_0)}-s_{1d}\cos \left({\mathrm{\θ}}_e\left(k\right|k)\right)+s_{2d}\sin \left({\mathrm{\θ}}_e\left(k\right|k)\right)\\ \frac{-{\mathrm{fk}}_{\mathrm{\υ}}}{\mathrm{\υ}\left(k\right|k)-{\mathrm{\υ}}_0}-s_{1d}\sin \left({\mathrm{\θ}}_e\left(k\right|k)\right)-s_{2d}\left({\mathrm{\θ}}_e\left(k\right|k)\right)\\ {\mathrm{\θ}}_e\left(k\right|k)\end{array}\right]---\left(18\right)$

在每一次采样时间，通过求解式(16)可以获得小车速度的最优控制序列v^*={v(k|k),v(k+1|k),…,v(k+N_c|k),…,v(k+N_p-1|k)}.，从v(k+N_c+1|k)到v(k+N_p-1|k)，控制输入的值保持不变且等于v(k+N_c|k)。将最优控制序列的第一项v(k|k)作为运动学控制器的输出，并作用于小车运动学模型，使得2.5维视觉误差信号渐近收敛，即lim_t→∞e=0。

动力学控制器是针对小车动力学模型来设计的，假设外界干扰为零，由于小车质心和两轮轴线中心重合，智能小车的动力学模型可表示为：

$\left\{\begin{array}{c}\stackrel{.}{v}=\frac{{\mathrm{\τ}}_1}{m},\\ \stackrel{.}{\mathrm{\ω}}=\frac{{\mathrm{\τ}}_2}{I}.\end{array}\right.---\left(19\right)$

其中m是小车质量，I是转动惯量，τ₁、τ₂分别为小车的驱动力矩和转动力矩。

动力学控制器的设计是为了产生力矩τ=[τ₁,τ₂]^T来控制小车实际速度，使小车实际速度渐近逼近期望速度，从而使2.5维视觉误差信号渐近收敛，其设计过程包含以下步骤：

首先，采用欧拉近似法对小车动力学模型式(21)进行离散化处理，得到如下离散时间动力学模型：

$\left\{\begin{array}{c}v(k+1)=v\left(k\right)+T\frac{{\mathrm{\τ}}_1\left(k\right)}{m},\\ \mathrm{\ω}(k+1)=\mathrm{\ω}\left(k\right)+T\frac{{\mathrm{\τ}}_2\left(k\right)}{I}.\end{array}\right.---\left(20\right)$

式(20)可以写成如下简化形式：

v(k+1)=f(v(k),τ(k)).            (21)

根据式(21)可得小车实际速度预测模型：

$v(k+j|k)=f(v(k+j-1|k),\mathrm{\τ}(k+j-1|k)),j\∈[1,{\stackrel{\‾}{N}}_p].---\left(22\right)$

其次，将运动学控制器的输出v_c=[v_c,ω_c]^T作为动力学预测控制器的参考输入，定义速度误差δ=v_c-v，构造一个由预测的速度误差信号和力矩表示的二次目标函数

(23)

其中为预测时域，为控制时域，和为正定的斜对称加权矩阵。

最后，考虑力矩约束：τ_max≤τ≤τ_min，动力学控制器的设计转化为求解如下优化问题：

受限于

$\left\{\begin{array}{c}v\left(k\right|k)=v_0,\\ v(k+j|k)=f(v(k+j-1|k),v(k+j-1|k)),j\∈[1,{\stackrel{\‾}{N}}_p],\\ \mathrm{\δ}(k+j|k)=v_c\left(k\right)-v(k+j|k),j\∈[1,{\stackrel{\‾}{N}}_p],\\ {\mathrm{\τ}}_{\min }\≤\mathrm{\τ}(k+j|k)\≤{\mathrm{\τ}}_{\max },j\∈[0,{\stackrel{\‾}{N}}_p-1],\end{array}\right.---\left(25\right)$

其中v₀为小车实际速度对应于时刻k的初始值，我们设小车实际速度初始值为零，即v(0)=0。

在每次采样时刻最小化二次目标函数获得最优控制序列 ${\mathrm{\τ}}^{*}=\{\mathrm{\τ}\left(k\right|k),\mathrm{\τ}(k+1|k),\·\·\·,\mathrm{\τ}(k+{\stackrel{\‾}{N}}_c|k),\·\·\·,\mathrm{\τ}(k+{\stackrel{\‾}{N}}_p-1|k)\}\·,$ 并取其第一项τ(k|k)作为动力学控制器的输出，作用于小车动力学模型，控制小车速度，使得速度误差渐近收敛，即lim_t→∞δ=0，从而使得lim_t→∞e=0。

为了验证所设计控制器的有效性，在MATLAB平台上进行两种情形的仿真，一种是不考虑可见性约束，另一种是考虑可见性约束：

设小车质量m=4kg，转动惯量I=2.5kg·m²，小车初始位姿q₀=[1.5,-5,0]^T，期望位姿为q_d=[0,0,0]^T，使用一个虚拟摄像机来获取仿真图像，虚拟摄像机的焦距f=6mm，拍摄到的虚拟图像的大小为640×480像素，虚拟摄像机的位置到小车质心的距离为l=0.1m，参数α=5m^-1，采样时间T=100ms，预测时域控制时域加权矩阵选择如下：

$Q=\left[\begin{array}{ccc}100& 0& 0\\ 0& 100& 0\\ 0& 0& 50\end{array}\right],$ $\stackrel{\‾}{Q}=\left[\begin{array}{cc}120& 0\\ 0& 120\end{array}\right],$ $R=\stackrel{\‾}{R}=\left[\begin{array}{cc}0.1& 0\\ 0& 0.1\end{array}\right],$

可见性约束选择如下：

$\left[\begin{array}{c}{u}_{\min }=-240\mathrm{pixels}\\ {\mathrm{\υ}}_{\min }=-200\mathrm{pixels}\end{array}\right]\≤\left[\begin{array}{c}u\\ \mathrm{\υ}\end{array}\right]\≤\left[\begin{array}{c}{u}_{\max }=240\mathrm{pixels}\\ {\mathrm{\υ}}_{\max }=200\mathrm{pixels}\end{array}\right].$

在两种情形的仿真中均考虑如下控制约束：

$\left[\begin{array}{c}-3m/s\\ -1.5\mathrm{rad}/s\end{array}\right]\≤v\≤\left[\begin{array}{c}3m/s\\ 1.5\mathrm{rad}/s\end{array}\right],$ $\left[\begin{array}{c}-50N\·m\\ -50N\·m\end{array}\right]\≤\mathrm{\τ}\≤\left[\begin{array}{c}50N\·m\\ 50N\·m\end{array}\right]$

仿真结果如图3～14所示。

说明：图3～8，图9～14分别为未考虑可见性约束和考虑可见性约束时的仿真结果图，其中图3、图9为小车在XY平面上的运动轨迹，图4、图10为特征点在图像平面上的运动轨迹，图5、图11为2.5维视觉误差变化曲线，图6、图12为小车实际线速度v和期望线速度v_c变化曲线，图7、图13为小车实际角速度ω和期望角速度ω_c变化曲线，图8、图14为小车驱动力矩τ₁和转动力矩τ₂变化曲线。

从以上仿真结果图3～14可以看出，小车能够到达期望的位姿，速度和力矩的变化均在所定义的约束范围内，且2.5维视觉误差收敛到零，加入可见性约束后，特征点的运动轨迹被限制在所定义的窗口区域内，因此可以证实所设计的视觉预测控制器能够处理系统中存在的可见性约束和控制约束，使小车安全快速运动到期望位姿且使目标始终处于摄像机的视野内。

Claims (1)

1.一种基于非线性模型预测的智能小车2.5维视觉伺服控制方法，其特征在于：包含以下步骤：

1.1.在当前位姿和期望位姿处通过摄像机分别获取参考目标的当前图像和期望图像；

1.2.从获取的当前图像和期望图像中提取参考目标的特征点及小车的姿态信号，建立2.5维视觉误差模型，该模型由如下公式确定：

$e=\left[\begin{array}{c}{e}_1\\ e_2\\ e_3\end{array}\right]=\left[\begin{array}{c}{s}_1\\ s_2\\ \mathrm{\θ}\end{array}\right]-\left[\begin{array}{ccc}{\cos \mathrm{\θ}}_e& -\sin {\mathrm{\θ}}_e& 0\\ \sin {\mathrm{\θ}}_e& \cos {\mathrm{\θ}}_e& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{s}_{1d}\\ s_{2d}\\ {\mathrm{\θ}}_d\end{array}\right],$

其中，θ和θ_d分别为移动小车的当前方向角和期望方向角，s₁，s_1d，s₂，s_2d为经过坐标变换的二维图像信号，θ_e＝θ-θ_d为三维姿态误差信号；

1.3.根据步骤1.2中的2.5维视觉误差模型，利用非线性模型预测控制方法设计一种多级视觉预测控制器，使2.5维视觉误差信号渐近收敛；该多级视觉预测控制器由至少两个子控制器组成，一个为运动学控制器，另一个为动力学控制器，其中运动学控制器的输出作为动力学控制器的输入；其具体操作包括：

1.3.1.运动学控制器的设计：

首先，采用欧拉近似法对视觉误差模型进行离散化处理，获得视觉误差预测模型e(k+j|k)，其中，T为采样周期，v(k)和ω(k)分别为小车在k时刻的线速度和角速度，符号(k+j|k)表示在k时刻向前预测j步得到的预测值；

其次，将小车速度ν＝[ν,ω]^T作为控制输入，构造一个由2.5维视觉误差预测信号和控制输入表示的二次目标函数J(k)，该目标函数由如下公式确定：

$\begin{array}{c}J\left(k\right)=\underset{j=1}{\overset{N_p}{\mathrm{\Σ}}}{e}^T(k+j|k)\mathrm{Qe}(k+j|k)+\\ \underset{j=1}{\overset{N_c}{\mathrm{\Σ}}}{v}^T(k+j-1|k)\mathrm{Rv}(k+j-1|k),\end{array}$

其中N_p为预测时域，N_c为控制时域，Q和R为正定的斜对称加权矩阵；

最后，加入可见性约束(u_min，,u_max)，(υ_min,,υ_max)和速度约束(ν_min,,ν_max)，其中(u,υ)为目标特征点在图像平面上的像素坐标，通过求解带有约束的二次目标函数J(k)的优化问题来获得最优控制序列v^*，取其第一项ν(k|k)作为运动学控制器的输出，使得视觉误差信号渐近收敛，即lim_t→∞e＝0；

1.3.2.动力学控制器的设计：

首先，利用欧拉近似法对小车动力学模型进行离散化处理，获得小车实际速度预测模型v(k+j|k)，其中τ(k)＝[τ₁(k),τ₂(k)]^T分别为小车在k时刻的驱动力矩和转动力矩；

其次，将运动学控制器的输出作为动力学预测控制器的参考输入，定义δ为小车实际速度与参考输入速度之间的速度误差，构造一个由预测的速度误差信号和力矩表示的二次目标函数，该目标函数由如下公式确定：

其中为预测时域，为控制时域，和为正定的斜对称加权矩阵；

最后，加入力矩约束(τ_min,,τ_max)，最小化二次目标函数获得最优控制序列τw，取其第一项τ(k|k)作为动力学控制器的输出，控制小车速度，使得速度误差渐近收敛，即lim_t→∞δ＝0，从而使得lim_t→∞e＝0。