CN102736626B - 基于视觉的移动小车位姿镇定控制方法 - Google Patents

Abstract

本发明公开了一种基于视觉的移动小车位姿镇定控制方法，充分考虑小车的运动学模型和动力学模型以及摄像机模型，在起始位姿和期望位姿处通过摄像机分别获取初始图像和期望图像，并在运动过程中实时获取当前图像；然后利用拍摄到的图像间的对极几何关系和三线性约束关系，基于Epipolar geometry和1D trifocal tensor采用三步切换控制策略设计三个独立有序的运动学控制器；最后将运动学控制器的输出作为动力学控制器的输入利用反演方法设计一种动态切换控制律，使小车沿着最短路径快速镇定到期望位姿。本发明解决传统视觉伺服方法在位姿镇定控制时未考虑小车动力学特性以及伺服速度慢问题，更切合实际，能够使小车更快地镇定到期望位姿。

Description

基于视觉的移动小车位姿镇定控制方法

技术领域

本发明涉及智能机器人技术领域，具体地说是指一种基于视觉的移动小车位姿镇定控制方法。

技术背景

近年来移动小车在工业、民用、科学探索等多种行业和领域得到广泛应用。随着计算机技术、微电子技术及人工智能等技术的发展，移动小车在未知或时变环境下自主工作的可靠性、安全性和智能化水平不断提高。视觉传感器由于具有成本低、信息丰富、可靠性高等特点被广泛应用于移动小车控制系统，而位姿镇定是移动小车运动控制最基本问题，研究移动小车视觉伺服镇定问题具有重要理论与应用价值。移动小车是一种受非完整约束限制的系统，任何连续时不变的状态反馈控制方法都很难有效地解决其镇定问题；另外视觉信息中包含大量的数据，要从中获得有用的信息，需要复杂的算法、耗费大量的运算时间。因此，基于视觉的移动小车位姿镇定是一个十分具有挑战性的课题，吸引众多学者的研究兴趣。

Lopez-Nicolas等人在2010年IEEE Transactions on Systems,Man,and Cybernetics,Part B:Cybernetics,40(4)期刊上利用当前图像和目标图像间的单应性关系设计一种切换控制律，使小车沿着一些最佳的路线到达期望的位姿。Becerra等人在2011年IEEEInternational Conference on Robotics and Automation会议上利用极线几何对移动小车的位姿进行动态估计，获得其位置和方向。虽然这些方法能够有效地解决基于视觉的位姿镇定问题，但是还存在以下主要不足：(1)设计视觉控制器时都只考虑小车的运动学模型，把小车当作一个理想的点来看待，未考虑最能反映小车本质特性的动力学模型；(2)伺服速度慢，视觉控制算法在设计时没有和控制理论相结合，在实际应用中很难获得满意的性能。

发明内容

本发明的目的在于提供一种基于视觉的移动小车位姿镇定控制方法，以解决上述方法在基于视觉的位姿镇定控制时未考虑小车动力学模型和伺服速度慢问题。本发明方法利用Epipolar geometry，1D trifocal tensor及反演方法设计一种动态切换控制律，使小车沿着最短路径快速镇定到期望位姿。

一种基于视觉的移动小车位姿镇定控制方法，包含以下步骤：

1.1.构建基于视觉的移动小车运动学模型和动力学模型，摄像机模型；

1.2.根据步骤1.1中的摄像机模型，建立摄像机在起始位姿、运动过程中所处的位姿和期望位姿处拍摄到的图像间的几何约束关系；

图像间的几何约束关系包括：两幅图像间对极几何关系和三幅图像间三线性约束关系，分别用Epipolar geometry模型和1D trifocal tensor模型表示，其中C₁=(x₁,z₁,θ₁)、C₂=(x₂,z₂,θ₂)和C_d=(0,0,0)分别为摄像机的初始位姿、当前位姿和期望位姿，e_1d，e_2d，e_d1和e₂₁为相应图像上的极点在图像平面x轴方向上的坐标，T₁和T₂为1阶张量；

1.3.根据步骤1.1中的移动小车运动学模型和步骤1.2中的图像间的几何约束关系，采用切换控制策略设计数个独立有序的运动学控制器；

1.4.根据步骤1.1中的移动小车动力学模型和步骤1.3中设计的运动学控制器，利用反演方法设计三个独立有序的动力学控制器，完成整个视觉控制器的设计。

步骤1.3中根据步骤1.2中图像间的几何约束关系采用三步控制策略设计切换的运动学控制器，为动力学控制器提供期望速度：

小车在原地旋转，期望线速度υ_d1为零，利用Epipolar geometry设计角速度控制器，其输出作为动力学控制器的输入，为控制小车旋转直至摄像机光轴指向期望位置提供所需的期望角速度ω_d1；

当摄像机光轴指向期望位置时，利用1D trifocal tensor和Epipolar geometry分别设计线速度控制器和角速度控制器，其输出作为动力学控制器的输入，为控制小车做直线运动到达期望位置提供所需的期望线速度υ_d2和期望角速度ω_d2；

当小车到达期望位置时，期望线速度υ_d3为零，利用Epipolar geometry设计角速度控制器，其输出作为动力学控制器的输入，为控制小车旋转到期望方向提供所需的期望角速度ω_d3。

步骤1.4中利用反演方法设计的动力学控制器将步骤1.3中设计的运动学控制器所提供的期望速度作为输入，用其输出力矩来控制小车的速度，使其趋向于期望速度，从而控制小车沿着最短路径镇定到期望位姿：

首先，驱动力矩为零，设计转动力矩控制器控制小车的角速度，使其趋向于期望的角速度ω_d1，从而控制小车旋转使摄像机光轴方向与基线C₁C_d重合；

其次，当摄像机光轴方向与基线C₁C_d重合时，设计驱动力矩控制器和转动力矩控制器分别控制小车的线速度和角速度，使其趋向于期望的线速度υ_d2和角速度ω_d2，从而控制小车沿着基线C₁C_d到达期望位置；

最后，当小车到达期望位置时，驱动力矩为零，设计转动力矩控制器控制小车的角速度，使其趋向于期望的角速度ω_d3，从而控制小车旋转使其方向与期望的方向一致。

本发明一种基于视觉的移动小车位姿镇定控制方法，与现有技术相比，具有以下明显的优势和有益效果：

本发明设计过程明确简单，鲁棒性好，伺服速度快。本发明解决了现有视觉伺服方法在位姿镇定时没有考虑机器人本质特性以及镇定过程时间长的问题，大大提高视觉位姿镇定的速度和精度。

附图说明

图1是本发明中的一个带有单目摄像机的四轮差动驱动移动小车模型示意图；

图2是本发明中的三视角间的对极几何关系示意图；

图3是本发明中的摄像机相对位置关系示意图；

图4是本发明中的移动小车三步运动示意图；

图5是本发明中的基于视觉的移动小车位姿镇定控制方法流程图；

图6～13是本发明中的基于视觉的移动小车位姿镇定仿真结果图。

具体实施方式

为了更好地理解本发明的技术方案，以一个带有单目摄像机的四轮差动驱动移动小车为研究对象对本发明的实施方式做进一步的介绍。

1.1.构建基于视觉的移动小车运动学模型和动力学模型，摄像机模型

如图1所示为四轮移动小车模型，前两轮从动，后两轮差动驱动，单目摄像机固定安装在小车中心处且其光轴指向小车运动方向。O-XYZ为世界坐标系，C-x_cy_cz_c为摄像机坐标系，原点C与小车质心o重合。该小车的位姿由向量q=[x,z,θ]^T表示，(x,z)为小车质心o的坐标，θ为车身方向与Z轴夹角，L为后两轮中心的距离，r为轮子半径，υ和ω分别为小车的线速度和角速度。根据图1所定义的坐标系，在不考虑轮子侧滑等因素时，移动小车的运动学模型可表示为：

$\left\{\begin{array}{c}\stackrel{\·}{x}=-\mathrm{\υ}\sin \mathrm{\θ}\\ \stackrel{\·}{z}=\mathrm{\υ}\cos \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=\mathrm{\ω}\end{array}\right.---\left(1\right)$

移动小车非完整约束条件为：

$\stackrel{\·}{x}\cos \mathrm{\θ}+\stackrel{\·}{z}\sin \mathrm{\θ}=0---\left(2\right)$

采用拉格朗日建模方法，移动小车的动力学模型可表示为：

$M\left(q\right)\stackrel{\·\·}{q}+V_m(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=N\left(q\right)\mathrm{\τ}+A^T\left(q\right)\mathrm{\λ}---\left(3\right)$

其中M是正定惯性矩阵，是向心力和哥式力矩阵，G(q)是重力向量矩阵，N(q)是输入转换矩阵，τ是小车转矩，λ是Lagrange乘子。

在不考虑阻力矩时，式(3)中的变量定义如下：

$M\left(q\right)=\left[\begin{array}{ccc}m& 0& 0\\ 0& m& 0\\ 0& 0& I\end{array}\right],$ $V_m\left(q\right)=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right],$ $G\left(q\right)=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right],$

$N\left(q\right)=\frac{1}{r}\left[\begin{array}{cc}-\sin \mathrm{\θ}& -\sin \mathrm{\θ}\\ \cos \mathrm{\θ}& \cos \mathrm{\θ}\\ L& -L\end{array}\right],$ A(q)=[cosθ sinθ 0]，τ=[τ₁,τ₂]^T

其中m为小车质量，I为小车相对于质心o的转动惯量，τ₁,τ₂分别为后两轮左右轮的输出力矩。式(3)变为：

$M\left(q\right)\stackrel{\·\·}{q}=N\left(q\right)\mathrm{\τ}+A^T\left(q\right)\mathrm{\λ}---\left(4\right)$

对式(1)进行微分后代入式(4)，并考虑式(2)，则有：

$\left[\begin{array}{cc}m& 0\\ 0& I\end{array}\right]\left[\begin{array}{c}\stackrel{\·}{\mathrm{\υ}}\\ \stackrel{\·}{\mathrm{\ω}}\end{array}\right]=\frac{1}{r}\left[\begin{array}{cc}1& 1\\ L& -L\end{array}\right]\left[\begin{array}{c}{\mathrm{\τ}}_l\\ {\mathrm{\τ}}_r\end{array}\right]---\left(5\right)$

定义 分别为小车后两轮的驱动力矩和转动力矩，式(5)变为：

$\left[\begin{array}{cc}m& 0\\ 0& I\end{array}\right]\left[\begin{array}{c}\stackrel{\·}{\mathrm{\υ}}\\ \stackrel{\·}{\mathrm{\ω}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\τ}}_d\\ {\mathrm{\τ}}_t\end{array}\right]---\left(6\right)$

则动力学方程可表示为：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\υ}}=\frac{{\mathrm{\τ}}_d}{m}\\ \stackrel{\·}{\mathrm{\ω}}=\frac{{\mathrm{\τ}}_t}{I}\end{array}\right.---\left(7\right)$

摄像机模型可由其内部标定矩阵K来表示：

$K=\left[\begin{array}{ccc}{\mathrm{\α}}_x& s& x_0\\ 0& {\mathrm{\α}}_y& y_0\\ 0& 0& 1\end{array}\right]---\left(8\right)$

其中α_x和α_y是摄像机以像素为单位的焦距，s是斜交参数，(x₀,y₀)是摄像机主点像素坐标。

1.2.根据步骤1.1中的摄像机模型，建立摄像机在起始位姿、运动过程中所处的位姿和期望位姿处拍摄到的图像间的几何约束关系

1.2.1Epipolar geometry模型

Epipolar geometry描述的是两幅图像间的对极几何关系，其可由一基础矩阵来表示并可利用两幅图像间的对应点来确定，已经被广泛应用于机器视觉和机器人学。如图2所示，C_d是摄像机的期望位姿，C₁和C₂分别为摄像机相对于C_d的初始位姿和当前位姿。线段C₁C_d(C₁C₂，C₂C_d)与相应的图像平面的交点称作极点，e_1d（e_2d，e_d1，e₂₁）为极点在x方向上的坐标，在图2中用黑线加粗表示。双下标表示相对应的图像，比如，e₂₁表示的是在图像2上相对于图像1计算得到的极点。由于小车在平面上运动，所有极点的y方向坐标为零。

设C₁=(x₁,z₁,θ₁)，C₂=(x₂,z₂,θ₂)，C_d=(0,0,0)，根据图2所示的几何关系，极点的x方向坐标可以表示为摄像机位置和参数α_x的函数：

$e_{1d}={\mathrm{\α}}_x\frac{x_1\cos {\mathrm{\θ}}_1+z_1\sin {\mathrm{\θ}}_1}{z_1\cos {\mathrm{\θ}}_1-x_1\sin {\mathrm{\θ}}_1}---\left(9\right)$

$e_{2d}={\mathrm{\α}}_x\frac{x_2\cos {\mathrm{\θ}}_2+z_2\sin {\mathrm{\θ}}_2}{z_2\cos {\mathrm{\θ}}_2-x_2\sin {\mathrm{\θ}}_2}---\left(10\right)$

$e_{21}={\mathrm{\α}}_x\frac{(x_1-x_2)\cos {\mathrm{\θ}}_2+(z_1-z_2)\sin {\mathrm{\θ}}_2}{(z_1-z_2)\cos {\mathrm{\θ}}_2-(x_1-x_2)\sin {\mathrm{\θ}}_2}---\left(11\right)$

$e_{d1}={\mathrm{\α}}_x\frac{x_1}{z_1}---\left(12\right)$

1.2.21D Trifocal tensor模型

Trifocal tensor描述的是三幅图像之间的三线性约束关系，不依赖于所观察到的场景，只取决于三幅图像间的相对位置和摄像机的内部标定参数，其可通过三幅图像间的相应特征估计得到。

如图3所示，(t_x1，t_z1)和(t_x2，t_z2)分别为C_d相对于C₁和C₂的坐标，根据三幅图像间的对应关系，一阶张量的数学表达式可表示为：

$T_1=\left[\begin{array}{cc}{T}_{111}& T_{112}\\ T_{121}& T_{122}\end{array}\right]=\left[\begin{array}{cc}{t}_{z1}\sin {\mathrm{\θ}}_2-t_{z2}\sin {\mathrm{\θ}}_1& -t_{z1}\cos {\mathrm{\θ}}_2+t_{z2}\cos {\mathrm{\θ}}_1\\ t_{z1}\cos {\mathrm{\θ}}_2+t_{x2}\sin {\mathrm{\θ}}_1& t_{z1}\sin {\mathrm{\θ}}_2-t_{x2}\cos {\mathrm{\θ}}_1\end{array}\right]---\left(13\right)$

$T_2=\left[\begin{array}{cc}{T}_{211}& T_{212}\\ T_{221}& T_{222}\end{array}\right]=\left[\begin{array}{cc}-t_{x1}\sin {\mathrm{\θ}}_2-t_{z2}\cos {\mathrm{\θ}}_1& t_{x1}\cos {\mathrm{\θ}}_2-t_{z2}\sin {\mathrm{\θ}}_1\\ -t_{x1}\cos {\mathrm{\θ}}_2+t_{x2}\cos {\mathrm{\θ}}_1& -t_{x1}\sin {\mathrm{\θ}}_2+t_{x2}\sin {\mathrm{\θ}}_1\end{array}\right]---\left(14\right)$

其中

t_xi=-x_i cosθ_i-z_i sinθ_i,

                                (15)

t_zi=x_i sinθ_i-z_i cosθ_i,(i＝1,2)

1.3.根据步骤1.1中的移动小车运动学模型和步骤1.2中的图像间的几何约束关系，采用切换控制策略设计数个独立有序的运动学控制器

运动学控制器为动力学控制器提供期望速度，控制小车沿着最短路径快速到达期望位姿。如图4所示为小车三步运动示意图。

小车在原地旋转，期望线速度υ_d1为零，利用Epipolar geometry设计角速度控制器，其输出作为动力学控制器的输入，为控制小车旋转直至摄像机光轴指向期望位置提供所需的期望角速度ω_d1。

如图4(a)所示，当C₂=C₁时，e_2d=e_1d；当摄像机指向目标时，e_2d=0，因此，将e_2d作为控制量。由于小车只做旋转运动，因此我们可知

x₂=x₁,z₂=z₁,υ=0                                      (16)

对式(10)进行求导，并将式(1)与(16)代入，我们得到：

${\stackrel{\·}{e}}_{2d}=({\mathrm{\α}}_x+\frac{1}{{\mathrm{\α}}_x}{e}_{2d}^2)\mathrm{\ω}---\left(17\right)$

利用状态反馈控制方法，我们设计如下简单的角速度比例控制器：

$\mathrm{\ω}=-k_1\frac{e_{2d}}{{\mathrm{\α}}_x+e_{2d}^2/{\mathrm{\α}}_x}---\left(18\right)$

其中k₁为正的控制增益。

将式(18)代入式(17)，可得：显然，e_2d以指数形式渐近收敛到零，此时摄像机光轴方向与基线C₁C_d重合。因此角速度控制器(18)的输出即为所需的期望角速度ω_d1。

当摄像机光轴指向期望位置时，利用1D trifocal tensor和Epipolar geometry分别设计线速度控制器和角速度控制器，其输出作为动力学控制器的输入，为控制小车做直线运动到达期望位置提供所需的期望线速度υ_d2和期望角速度ω_d2；

如图4(b)所示，当摄像机沿着基线运动时，极点的值将不会发生变化，因此Epipolargeometry不能用来设计线速度控制器，在这里我们利用1D Trifocal tensor来设计线速度控制器。设摄像机光轴方向与基线重合时所处的位姿为C′₁＝(x₁,z₁,θ′₁)，并将其作为第二步小车运动的初始位姿。根据图3及图4(b)所示的几何关系，我们有t′_x1=t_x2=0，则1阶张量表达式(13)和(14)变为：

$T_1^{\′}=\left[\begin{array}{cc}{T}_{111}^{\′}& T_{112}^{\′}\\ T_{121}^{\′}& T_{122}^{\′}\end{array}\right]=\left[\begin{array}{cc}{t}_{z1}^{\′}\sin {\mathrm{\θ}}_2-t_{z2}\sin {\mathrm{\θ}}_1^{\′}& -t_{z1}^{\′}\cos {\mathrm{\θ}}_2+t_{z2}\cos {\mathrm{\θ}}_1^{\′}\\ t_{z1}^{\′}\cos {\mathrm{\θ}}_2& t_{z1}^{\′}\sin {\mathrm{\θ}}_2\end{array}\right]---\left(19\right)$

$T_2^{\′}=\left[\begin{array}{cc}{T}_{211}^{\′}& T_{212}^{\′}\\ T_{221}^{\′}& T_{222}^{\′}\end{array}\right]=\left[\begin{array}{cc}-t_{z2}\cos {\mathrm{\θ}}_1^{\′}& -t_{z2}\sin {\mathrm{\θ}}_1^{\′}\\ 0& 0\end{array}\right]---\left(20\right)$

当小车到达期望位置时，t_z2=0，由式(20)可知：T′₂₁₁=0。因此，选择T′₂₁₁作为控制量。

考虑式(1)和(15)，对式(20)中的T′₂₁₁进行求导，可得：

${\stackrel{\·}{T}}_{211}^{\′}=\mathrm{\υ}\cos {\mathrm{\θ}}_1^{\′}---\left(21\right)$

根据状态反馈控制方法，设计如下线速度控制器：

$\mathrm{\υ}=-k_2\frac{T_{211}^{\′}}{\cos {\mathrm{\θ}}_1^{\′}}---\left(22\right)$

其中k₂为正的控制增益。

将式(22)代入式(21)中，可得：显然，T′₂₁₁以指数形式渐近收敛到零，此时小车运动到期望位置。因此控制器(22)的输出即为所需的期望线速度υ_d2。

为了计算方向角θ′₁，我们将摄像机在完成第一步时拍摄到的当前图像储存起来，采用平面视差法可得：

${\mathrm{\θ}}_1^{\′}=\arctan \left(\frac{e_{\mathrm{cx}}}{{\mathrm{\α}}_x}\right)-\arctan \left(\frac{e_{\mathrm{tx}}}{{\mathrm{\α}}_x}\right)---\left(23\right)$

其中e_cx为当前图像上的极点，e_tx为期望图像上的极点。

在实际应用中，小车在运动过程中会受到各种噪声，漂移等因素的影响，进而影响小车的运动方向。为了使小车朝着期望位置的方向做直线运动，我们采用角速度控制器(18)来校正小车的方向误差，因此将角速度控制器(18)的输出作为所需的期望角速度ω_d2。

当小车到达期望位置时，期望线速度υ_d3为零，利用Epipolar geometry设计角速度控制器，其输出作为动力学控制器的输入，为控制小车旋转到期望方向提供所需的期望角速度ω_d3；

如图4(c)所示，当C₂=C_d时，e₂₁=e_d1，x₂=z₂=0，因此选择e₂₁作为控制量。由于小车在期望位置只做旋转运动，可知υ=0。

定义误差函数：

e=e₂₁-e_d1                                               (24)

考虑式(1)和(11)，对式(24)进行求导可得：

$\stackrel{\·}{e}=({\mathrm{\α}}_x+\frac{1}{{\mathrm{\α}}_x}{e}_{21}^2)\mathrm{\ω}---\left(25\right)$

利用状态反馈控制方法，设计如下角速度控制器：

$\mathrm{\ω}=-k_3\frac{e}{{\mathrm{\α}}_x+e_{21}^2/{\mathrm{\α}}_x}---\left(26\right)$

其中k₃为正的控制增益。

将式(26)代入式(25)中可得：显然误差e以指数形式渐近收敛到零，也就是e₂₁渐近收敛到e_d1，此时小车方向与期望方向一致，小车到达期望位姿。因此角速度控制器(26)的输出即为所需的期望角速度ω_d3。

1.4.根据步骤1.1中的移动小车动力学模型和步骤1.3中设计的运动学控制器，利用反演方法设计三个独立有序的动力学控制器。

动力学控制器将运动学控制器所提供的期望速度作为输入，用其输出力矩[τ_d,τ_t]^T来控制小车速度使其趋向于期望速度，从而控制小车镇定到期望位姿。

定义速度跟踪误差 $\mathrm{\ϵ}=\stackrel{\→}{v}-{\stackrel{\→}{v}}_d={[\mathrm{\υ}-{\mathrm{\υ}}_d,\mathrm{\ω}-{\mathrm{\ω}}_d]}^T$ 及其微分 $\stackrel{\·}{\mathrm{\ϵ}}={\stackrel{\·}{\stackrel{\→}{v}}}_d-{\stackrel{\·}{\stackrel{\→}{v}}}_d={[\stackrel{\·}{\mathrm{\υ}}-{\stackrel{\·}{\mathrm{\υ}}}_d,\stackrel{\·}{\mathrm{\ω}}-{\stackrel{\·}{\mathrm{\ω}}}_d]}^T,$ 根据反演方法，选取如下Lyapunov候选函数：

$V=\frac{1}{2}m{(\mathrm{\υ}-{\mathrm{\υ}}_d)}^2+\frac{1}{2}I{(\mathrm{\ω}-{\mathrm{\ω}}_d)}^2---\left(27\right)$

对式(27)进行求导，并考虑式(7)可得：

$\stackrel{\·}{V}=m(\mathrm{\υ}-{\mathrm{\υ}}_d)(\stackrel{\·}{\mathrm{\υ}}-{\stackrel{\·}{\mathrm{\υ}}}_d)+I(\mathrm{\ω}-{\mathrm{\ω}}_d)(\stackrel{\·}{\mathrm{\ω}}-{\stackrel{\·}{\mathrm{\ω}}}_d)$

                                                (28)

$=(\mathrm{\υ}-{\mathrm{\υ}}_d)({\mathrm{\τ}}_d-m{\stackrel{\·}{\mathrm{\υ}}}_d)+(\mathrm{\ω}-{\mathrm{\ω}}_d)({\mathrm{\τ}}_t-I{\stackrel{\·}{\mathrm{\ω}}}_d)$

选取

$\left\{\begin{array}{c}{\mathrm{\τ}}_d=-k_4(\mathrm{\υ}-{\mathrm{\υ}}_d)+m{\stackrel{\·}{\mathrm{\υ}}}_d\\ {\mathrm{\τ}}_t=-k_5(\mathrm{\ω}-{\mathrm{\ω}}_d)+I{\stackrel{\·}{\mathrm{\ω}}}_d\end{array}\right.---\left(29\right)$

其中k₄,k₅为正的常数。

则式(28)变为：

$\stackrel{\·}{V}=-k_4{(\mathrm{\υ}-{\mathrm{\υ}}_d)}^2-k_5{(\mathrm{\ω}-{\mathrm{\ω}}_d)}^2\≤0---\left(30\right)$

根据Lyapunov稳定性理论可知该系统能全局渐近稳定。

因此，根据步骤3设计的运动学控制器分别设计如下三个独立有序的动力学控制器控制小车速度使其趋向于期望速度，从而控制小车沿着最短路径镇定到期望位姿：

首先，选取

$\left\{\begin{array}{c}{\mathrm{\τ}}_d=0\\ {\mathrm{\τ}}_t=-k_5(\mathrm{\ω}-{\mathrm{\ω}}_{d1})+I{\stackrel{\·}{\mathrm{\ω}}}_{d1}\end{array}\right.---\left(31\right)$

其中控制小车的角速度ω，使其趋向于期望的角速度ω_d1，从而控制小车旋转使摄像机光轴方向与基线C₁C_d重合；

其次，选取

$\left\{\begin{array}{c}{\mathrm{\τ}}_d=-k_4(\mathrm{\υ}-{\mathrm{\υ}}_{d2})+m{\stackrel{\·}{\mathrm{\υ}}}_{d2}\\ {\mathrm{\τ}}_t=-k_5(\mathrm{\ω}-{\mathrm{\ω}}_{d2})+I{\stackrel{\·}{\mathrm{\ω}}}_{d2}\end{array}\right.---\left(32\right)$

其中 控制小车的线速度υ和角速度ω，使其趋向于期望的线速度υ_d2和角速度ω_d2，从而控制小车沿着基线C₁C_d到达期望位置；

最后，选取

$\left\{\begin{array}{c}{\mathrm{\τ}}_d=0\\ {\mathrm{\τ}}_t=-k_5(\mathrm{\ω}-{\mathrm{\ω}}_{d3})+I{\stackrel{\·}{\mathrm{\ω}}}_{d3}\end{array}\right.---\left(33\right)$

其中控制小车的角速度ω，使其趋向于期望的角速度ω_d3，从而控制小车旋转使其方向与期望的方向一致。

至此完成整个视觉控制器的设计，整个控制过程如图5所示。

对所设计的控制器的有效性进行仿真验证：设小车质量m=4kg，转动惯量I=2.5kg·m²，小车初始位姿为q₀=[1.5,-5.5,10°]^T，期望位姿为q_d=[0,0,0°]^T。构造一个虚拟的场景，并在场景中产生一些随机的三维点作为目标特征点，这些三维点通过一个虚拟摄像机映射到虚拟图像平面上，虚拟图像的大小为640×480像素。在每一次控制回路中，摄像机拍摄图片与期望图片相比较，利用匹配的点计算极点和1阶张量，控制器将速度(υ,ω)传送给虚拟的机器人，仿真结果如图6～13所示。

说明：图6为小车在XZ平面的运动轨迹，图7为三维目标点在图像像素平面上的运动轨迹，图8为小车位姿变化曲线，图9为极点e_2d和e₂₁的估计曲线，图10为1阶张量中T′₂₁₁的估计曲线，图11为小车实际线速度和期望线速度曲线，图12为实际角速度和期望角速度曲线，图13为力矩输出曲线。

由以上仿真结果图6～13可以看出，小车能够沿着最短路径快速镇定到期望位姿，因此所设计的控制器能够实现基于视觉的移动小车位姿镇定。

Claims (1)

1.一种基于视觉的移动小车位姿镇定控制方法，其特征在于：包含以下步骤：

1.1.构建基于视觉的移动小车运动学模型和动力学模型，摄像机模型；

1.2.根据步骤1.1中的摄像机模型，建立摄像机在起始位姿、运动过程中所处的位姿和期望位姿处拍摄到的图像间的几何约束关系；

所述的图像间的几何约束关系包括：两幅图像间对极几何关系和三幅图像间三线性约束关系，分别用Epipolar geometry模型和1D trifocal tensor模型表示，其中C₁=(x₁,z₁,θ₁)、C₂=(x₂,z₂,θ₂)和C_d=(0,0,0)分别为摄像机的初始位姿、当前位姿和期望位姿，e_1d，e_2d，e_d1和e₂₁为相应图像上的极点在图像平面x轴方向上的坐标，T₁和T₂为1阶张量；

1.3.根据步骤1.1中的移动小车运动学模型和步骤1.2中的图像间的几何约束关系，采用切换控制策略设计数个独立有序的运动学控制器；

步骤1.3中根据步骤1.2中图像间的几何约束关系采用三步控制策略设计切换的运动学控制器，为动力学控制器提供期望速度：

小车在原地旋转，期望线速度υ_d1为零，利用Epipolar geometry设计角速度控制器，其输出作为动力学控制器的输入，为控制小车旋转直至摄像机光轴指向期望位置提供所需的期望角速度ω_d1；

当摄像机光轴指向期望位置时，利用1D trifocal tensor和Epipolar geometry分别设计线速度控制器和角速度控制器，其输出作为动力学控制器的输入，为控制小车做直线运动到达期望位置提供所需的期望线速度υ_d2和期望角速度ω_d2；

当小车到达期望位置时，期望线速度υ_d3为零，利用Epipolar geometry设计角速度控制器，其输出作为动力学控制器的输入，为控制小车旋转到期望方向提供所需的期望角速度ω_d3；

1.4.根据步骤1.1中的移动小车动力学模型和步骤1.3中设计的运动学控制器，利用反演方法设计三个独立有序的动力学控制器，完成整个视觉控制器的设计；

步骤1.4中利用反演方法设计的动力学控制器将步骤1.3中设计的运动学控制器所提供的期望速度作为输入，用其输出力矩来控制小车的速度，使其趋向于期望速度，从而控制小车沿着最短路径镇定到期望位姿：

首先，驱动力矩为零，设计转动力矩控制器控制小车的角速度，使其趋向于期望的角速度ω_d1，从而控制小车旋转使摄像机光轴方向与基线C₁C_d重合；

其次，当摄像机光轴方向与基线C₁C_d重合时，设计驱动力矩控制器和转动力矩控制器分别控制小车的线速度和角速度，使其趋向于期望的线速度υ_d2和角速度ω_d2，从而控制小车沿着基线C₁C_d到达期望位置；

最后，当小车到达期望位置时，驱动力矩为零，设计转动力矩控制器控制小车的角速度，使其趋向于期望的角速度ω_d3，从而控制小车旋转使其方向与期望的方向一致。