CN104808590B - 一种基于关键帧策略的移动机器人视觉伺服控制方法 - Google Patents

Abstract

本发明公开了一种基于关键帧策略的移动机器人视觉伺服控制方法，包含以下步骤：1）根据给定条件规划机器人移动路径；2）将视觉导航任务中的物理轨迹跟踪和目标可见下的姿态调整转化到图像特征空间中的轨迹；3）基于当前图像帧与关键帧之间的单应性关系定义图像特征，以图像特征隐式表达了机器人的运动情况；4）基于机器人运动学模型和相机模型建立视觉控制系统的相互关系模型，设计预测控制器跟踪轨迹。本发明不要求预先对机器人在给定轨迹上进行训练，也不要求在运动过程中目标持续可见，同时在一定程度上保证了机器人的导航精度。

Description

一种基于关键帧策略的移动机器人视觉伺服控制方法

技术领域

本发明涉及一种机器人领域的视觉导航方法，特别是涉及一种基于关键帧策略的移动机器人视觉伺服控制方法。

背景技术

移动机器人视觉导航技术是以视觉系统作为反馈信息控制移动机器人运动的方法，是移动机器人研究的重要部分。视觉导航是目前移动机器人研究中的热点和难点，在机器人执行大环境或复杂环境下任务时起到重要的作用。

移动机器人视觉导航技术可以分为基于位置的方法和基于图像的方法。其中，基于位置的方法将视觉系统作为位置传感器使用，使用路标或视觉里程计等方式对机器人进行定位，并依此控制机器人运动，这种方法控制器设计简单，适用于大范围的运动，但是导航精度差，具有误差累积效应。基于图像的方法使用视觉系统反馈的图像作为反馈信息，控制机器人运动到拍摄到与目标图像一致的场景处，这种方法导航精度高，但是需要对系统在给定轨迹上预先进行训练，而且要求目标图像在整个运动过程中都可见。

发明内容

本发明为了克服现有技术的不足，提供了一种基于关键帧策略的移动机器人视觉伺服控制方法，该方法在保证导航精度的同时，不要求预先对机器人在给定轨迹上进行训练，同时也不要求在运动过程中目标持续可见，只需要目标位置和参考轨迹，实现成本较低。

本发明的目的是通过以下技术方案来实现的：一种基于关键帧策略的移动机器人视觉伺服控制方法，用于移动机器人在局部已知环境下的导航，所述的移动机器人为履带式机器人，自带单目摄像头可实时采集图像信息。该方法将导航任务转化到基于图像单应性定义的图像特征空间中，设计预测控制器在图像特征空间中进行轨迹跟踪控制，包括以下步骤：

(1)将机器人放置在初始位置上，给定期望位置坐标，利用已知的机器人起始、终止位置结合机器人自身的非完整约束规划出机器人在二维平面下的移动路径；

(2)根据步骤(1)得到的机器人移动路径，通过利用单应性关系定义图像特征，并从单应性矩阵中选取合适的元素作为系统状态量，进而将该路径转化到图像特征空间里，生成一组图像空间特征即系统状态；

(3)机器人开始移动后，由摄像头获取图像信息，首先把第一帧作为关键帧，之后基于当前图像帧与关键帧之间的单应性关系定义图像特征，以图像特征隐式表达机器人的运动情况。其中关键帧在机器人移动过程中需要进行切换，而切换需满足一定的条件：a.目标物体对于当前帧不可见，但是对于下一帧可见；b.切换的关键帧与上一可见帧之间应存在一定的时间间隔；c.当系统状态有接近奇异的趋势；d.关键帧切换的时间间隔不应太长。且在每一次关键帧切换之后，需要重新对路径进行规划，然后通过步骤(2)转化为图像空间轨迹；

(4)将步骤(2)得到的图像空间轨迹的系统状态作为设定值，步骤(3)中实时得到的当前系统状态作为测量值，共同作为控制器的输入信号，再由控制器输出信号驱动机器人运动，控制器采用模型预测控制，其中相互关系模型通过如下方式建立：基于机器人运动学模型和相机模型，经过系统状态对时间的微分最终得出系统状态关于速度的图像雅可比矩阵即作为相互关系模型；

(5)机器人接收驱动信号进行移动，同时利用摄像头获取实时图像，进而得到系统状态，结合规划的图像空间特征，判断是否达到目标位置，若没有则继续上述步骤(4)，需要切换关键帧则进行步骤(3)；若达到目标，则方法结束。

步骤(1)中所述在二维平面下机器人移动轨迹满足的约束根据公式(1)和(2)进行，公式(1)和(2)分别为：

ρ(y,z)＝0

式中(y,z)表示机器人在二维坐标系(坐标轴为y轴和z轴)下的坐标，ρ表示移动轨迹，表示机器人的偏转角度。

步骤(1)中在控制过程中的机器人几何中心的起始、终止坐标分别表示为公式(5)，公式(5)为：

其中表示机器人几何中心的初始坐标，表示机器人几何中心的终止位置坐标，表示相机中心的终止位置坐标， $k=\left[\begin{array}{c}{k}_1\\ k_2\\ k_3\end{array}\right]=\left[\begin{array}{c}{y}_{{\mathrm{ICR}}_r}-y_{{\mathrm{ICR}}_l}\\ y_{{\mathrm{ICR}}_r}+y_{\mathrm{IC}{R}_l}\\ Z_{\mathrm{ICR}}\end{array}\right]$ 为待辨识参数，z_ICR分别表示机器人三个旋转瞬心的坐标。

步骤(2)中所述的单应性图像特征由公式(6)定义，公式(6)为：

$H=K(R-\frac{{\mathrm{tn}}^T}{d})K^{-1}$

其中，H即为单应性矩阵，(R,t)表示了机器人的运动状态，n为相机坐标系下地面的法向量，d为相机坐标系原点与地面之间的距离,K为相机内部参数矩阵

步骤(2)中单应性图像特征的获取以及系统状态变量的定义分别由公式(10)和(11)决定，公式(10)和(11)分别为：

${\left[\begin{array}{cccc}{s}_1& s_2& s_3& s_4\end{array}\right]}^T=\frac{1}{h_{22}}\·{\left[\begin{array}{cccc}{h}_{11}& h_{12}& h_{21}& h_{33}\end{array}\right]}^T$

其中f_u、f_v分别表示焦距在像素坐标轴上对应的像素点数目,β是相机光轴与地面的夹角，是机器人期望位姿下的偏转角度。

步骤(4)中所述的机器人运动学模型根据公式(12)确定，公式(12)为：

$v_y=\frac{v_r-v_l}{y_{{\mathrm{ICR}}_r}-y_{\mathrm{IC}{R}_l}}{Z}_{\mathrm{ICR}}$

$v_z=\frac{v_r+v_l}{2}-\frac{v_r-v_l}{y_{{\mathrm{ICR}}_r}-y_{{\mathrm{ICR}}_l}}\frac{y_{{\mathrm{ICR}}_r}+y_{{\mathrm{ICR}}_l}}{2}$

$\mathrm{\ω}=\frac{v_r-v_l}{y_{{\mathrm{ICR}}_r}-y_{{\mathrm{ICR}}_l}}$

其中v_y、v_z、ω分别表示相机的侧向、前向线速度和角速度，v_l、v_r分别为左右两条履带的速度，z_ICR分别表示三个旋转瞬心的坐标。

步骤(4)中新定义了一种位姿参数，根据公式(13)进行，公式(13)为：

其中，s为系统状态变量，为相机中心的期望位置坐标，其余参数定义参照上文。

步骤(4)中所述的相互关系模型由公式(14)和(15)确定，公式(14)和(15)分别为：

$\stackrel{.}{s}=J\left(s\right)\·u$

$J=\left[\begin{array}{cc}\frac{1}{k_1}({-k}_3{n}_x{\mathrm{\λ}}_3\sin \mathrm{\β}-\frac{k_2}{2}{n}_x\sin \mathrm{\β}+n_x{\mathrm{\λ}}_1{\mathrm{\λ}}_3\sin \mathrm{\β}+{\mathrm{\λ}}_2{\mathrm{\λ}}_3{\cos }^2\mathrm{\β})& \frac{1}{2}{n}_x\sin \mathrm{\β}\\ -\frac{f_u}{k_1{f}_v}{{\mathrm{\λ}}_2}^2\sin \mathrm{\β}& 0\\ \frac{f_v}{k_1{f}_u}(k_3{n}_x-\frac{k_2}{2}{n}_x{\mathrm{\λ}}_3+{{\mathrm{\λ}}_2}^2\sin \mathrm{\β}+n_x{\mathrm{\λ}}_3{\mathrm{\λ}}_4)& \frac{f_v{n}_x}{{2f}_u}{\mathrm{\λ}}_3\\ \frac{1}{k_1}({-k}_3{n}_z{\mathrm{\λ}}_3\cos \mathrm{\β}-\frac{k_2}{2}{n}_z\cos \mathrm{\β}+n_z{\mathrm{\λ}}_1{\mathrm{\λ}}_3\cos \mathrm{\β}+{\mathrm{\λ}}_2{\mathrm{\λ}}_3{\sin }^2\mathrm{\β})& \frac{1}{2}{n}_z\cos \mathrm{\β}\end{array}\right]$

其中 $u=\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]=\left[\begin{array}{c}{v}_l-v_r\\ v_l+v_r\end{array}\right]$ 作为被控变量， $k=\left[\begin{array}{c}{k}_1\\ k_2\\ k_3\end{array}\right]=\left[\begin{array}{c}{y}_{{\mathrm{ICR}}_r}-y_{{\mathrm{ICR}}_l}\\ y_{{\mathrm{ICR}}_r}+y_{\mathrm{IC}{R}_l}\\ Z_{\mathrm{ICR}}\end{array}\right]$ 为待辨识参数。

步骤(4)中所述的根据线性化相互关系模型得出的代价函数由公式(17)定义，公式(17)为：

$J\left(k\right)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}\{s_e^T(k+j|k){\mathrm{Qs}}_e(k+j|k)+{\mathrm{\Δu}}^T(k+j-1|k)\mathrm{R\Δu}(k+j-1|k)\}$

其中Q,R分别是加权矩阵，s_e＝s-s_r为当前系统状态变量与过去生成的参考轨迹状态变量的偏差，N是预测时域。

本发明的有益效果是，本发明基于关键帧策略的改进图像视觉导航方法，在传统的基于图像视觉伺服控制方式的基础上予以改进，在利用图像信息克服了低精度问题的同时，保留了大范围运动特性，而且又针对基于图像要求的目标持续可见性提出新的方法，通过切换关键帧的方式在一定程度上克服了这样的视野约束。另外，该方法只需要目标位置和参考轨迹，实现成本较低。

附图说明

图1是本发明的流程图；

图2是机器人物理配置图；

图3是关键帧策略示意图；

图4是关键帧切换时的轨迹变换策略示意图；

图5是机器人运动学模型；

图6是实施例中机器人移动轨迹图；

图7是实施例中机器人与期望位置的距离变化示意图。

具体实施方式

所谓关键帧策略，即在机器人运动过程中选择某一时刻相机获取的目标图像为基准(关键帧)，在下一次切换关键帧之前的一段时间内，系统是根据当前帧与这一阶段选择的关键帧来获取单应性特征，进而得到系统状态。这样一种策略叫做关键帧策略，而这种策略中选择为基准的图像帧则为关键帧。

以下结合具体实施方式并对照附图对本发明加以详细说明。

该改进法用于移动机器人在局部已知环境下的导航，首先给定目标在机器人期望位姿的二维平面坐标，根据给定信息结合当前信息规划出移动轨迹，将视觉导航任务中的物理轨迹跟踪和目标可见下的姿态调整转化到图像特征空间中的轨迹，并以此作为预测控制器的设定值，利用模型预测进行路径跟踪控制。相机得到的当前图像信息经过处理获得单应性图像特征，然后判断是否进行关键帧切换，再确定是否重新生成轨迹，整个控制系统的流程图如图1所示。

步骤1：将机器人放置在初始位置上，给定期望位置坐标，利用已知的机器人起始、终止位置结合机器人自身的非完整约束规划出机器人在二维平面下的移动路径；

设该移动路径在二维坐标系(坐标轴为y轴和z轴)下可表示为：

ρ(y,z)＝0 (¹)

非完整约束表示为：

表示机器人的偏转角度。

综上，这条路径应满足以下四个方程式：

ρ(y₀,z₀)＝0

ρ(y_f,z_f)＝0

将路径设为关于y，z的四次表达式，如下：

ρ(y,z)＝z+w₃y³+w₂y²+w₁y¹ ₊w₀ (4)

假设相机Pc初始坐标为(0,0,0)，最终期望坐标为根据上述式(13)的四个方程式，只要代入机器人中心Pr的初始、终止坐标即可得出规划的路径，接下来有实际情况控制过程为：

此时以机器人的起始坐标系为基准(该坐标系下相机中心Pc坐标为(0,0,0))，根据建立的机器人运动学模型，点Pr的初始、终止位姿为：

将公式(5)代入上述路径方程式中即可得出满足条件的机器人移动路径，其中k_i相关定义参见步骤4。

步骤2：根据步骤1得到的机器人移动路径，通过利用单应性关系定义图像特征，并从单应性矩阵中选取合适的元素作为系统状态量，进而将该路径转化到图像特征空间里，生成一组图像空间特征即系统状态；

如图2为机器人的物理配置图，单目相机斜向下安装在机器人的前部，光轴与地面夹角为β，基于图像单应性矩阵定义图像特征，使用公式(6)定义，公式(6)如下：

$H=K(R-\frac{{\mathrm{tn}}^T}{d})K^{-1}---\left(6\right)$

其中，H即为单应性矩阵，(R,t)表示了机器人的运动状态，n为相机坐标系下地面的法向量，d为相机坐标系原点与地面之间的距离,K为相机内部参数矩阵，通过简化可得到矩阵为

$K=\left[\begin{array}{ccc}{f}_u& 0& 0\\ 0& f_v& 0\\ 0& 0& 1\end{array}\right]---\left(7\right)$

f_u、f_v分别表示焦距在像素坐标轴上对应的像素点数目。

已知相机光轴与地面成β夹角，根据上述履带式机器人的模型可以得到相机坐标系和机器人坐标系的转换关系如下：

$R^{\mathrm{rc}}=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& -\sin \mathrm{\β}\\ 0& 1& 0\\ \sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right],t^{\mathrm{rc}}=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]---\left(8\right)$

假设相机从坐标(0,0,0)移动到了那么相机坐标系的变化量为：

根据上式，结合式(6)不难得到单应性矩阵的各个元素，令H中第i行第j列的元素为h_ij，得到：

在得到单应性矩阵之后即可获得系统状态变量，把h₂₂归一化，且由于f_u、f_v较大，所以h₃₁和h₃₂很小可忽略不计，在剩下的几个元素中，只有h₁₁、h₁₂、h₂₁和h₃₃为线性无关项，将其选作系统状态变量，定义为：

${\left[\begin{array}{cccc}{s}_1& s_2& s_3& s_4\end{array}\right]}^T=\frac{1}{h_{22}}\·{\left[\begin{array}{cccc}{h}_{11}& h_{12}& h_{21}& h_{33}\end{array}\right]}^T---\left(11\right)$

利用上述相关计算公式结合步骤1得到的机器人物理路径，便可将其转化为图像空间特征。

步骤3：机器人开始移动后，由摄像头获取图像信息，首先把第一帧作为关键帧，之后基于当前图像帧与关键帧之间的单应性关系定义图像特征，以图像特征隐式表达机器人的运动情况。

其中关键帧在机器人移动过程中需要进行切换，而切换需满足一定的条件：a.目标物体对于当前帧不可见，但是对于下一帧可见；b.切换的关键帧与上一可见帧之间应存在一定的时间间隔；c.当系统状态有接近奇异的趋势；d.关键帧切换的时间间隔不应太长。且在每一次关键帧切换之后，需要重新对路径进行规划，然后通过步骤2转化为图像空间轨迹；

步骤4：将步骤2得到的图像空间轨迹的系统状态作为设定值，步骤3中实时得到的当前系统状态作为测量值，共同作为控制器的输入信号，再由控制器输出信号驱动机器人运动，控制器采用模型预测控制。其中相互关系模型通过如下方式建立：基于机器人运动学模型和相机模型，经过系统状态对时间的微分最终得出系统状态关于速度的图像雅可比矩阵即作为相互关系模型；

a.在对机器人进行运动学建模如图5时，构造参数有：其中v_y、v_z、ω分别表示相机的侧向、前向线速度和角速度，v_l、v_r分别为左右两条履带的速度，z_ICR分别表示三个旋转瞬心的坐标。建立模型如下：

$v_y=\frac{v_r-v_l}{y_{{\mathrm{ICR}}_r}-y_{\mathrm{IC}{R}_l}}{Z}_{\mathrm{ICR}}$

$v_z=\frac{v_r+v_l}{2}-\frac{v_r-v_l}{y_{{\mathrm{ICR}}_r}-y_{{\mathrm{ICR}}_l}}\frac{y_{{\mathrm{ICR}}_r}+y_{{\mathrm{ICR}}_l}}{2}---\left(12\right)$

$\mathrm{\ω}=\frac{v_r-v_l}{y_{{\mathrm{ICR}}_r}-y_{{\mathrm{ICR}}_l}}$

另外，定义 $u=\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]=\left[\begin{array}{c}{v}_l-v_r\\ v_l+v_r\end{array}\right]$ 作为被控变量， $k=\left[\begin{array}{c}{k}_1\\ k_2\\ k_3\end{array}\right]=\left[\begin{array}{c}{y}_{{\mathrm{ICR}}_r}-y_{{\mathrm{ICR}}_l}\\ y_{{\mathrm{ICR}}_r}+y_{\mathrm{IC}{R}_l}\\ Z_{\mathrm{ICR}}\end{array}\right]$ 为待辨识参数。

b.相互关系模型的建立如上文所述，需要对系统状态求关于时间的微分，为方便推导，可以新定义一种位姿参数λ，具体公式如下：

对状态变量求关于时间的导数，可得到和速度v的关系，考虑机器人运动学模型以及世界坐标系和局部坐标系的转换，最终可以得到：

$\stackrel{.}{s}=J\left(s\right)\·u---\left(14\right)$

这里的J即为图像雅可比矩阵(相互关系)：

$J=\left[\begin{array}{cc}\frac{1}{k_1}({-k}_3{n}_x{\mathrm{\λ}}_3\sin \mathrm{\β}-\frac{k_2}{2}{n}_x\sin \mathrm{\β}+n_x{\mathrm{\λ}}_1{\mathrm{\λ}}_3\sin \mathrm{\β}+{\mathrm{\λ}}_2{\mathrm{\λ}}_3{\cos }^2\mathrm{\β})& \frac{1}{2}{n}_x\sin \mathrm{\β}\\ -\frac{f_u}{k_1{f}_v}{{\mathrm{\λ}}_2}^2\sin \mathrm{\β}& 0\\ \frac{f_v}{k_1{f}_u}(k_3{n}_x-\frac{k_2}{2}{n}_x{\mathrm{\λ}}_3+{{\mathrm{\λ}}_2}^2\sin \mathrm{\β}+n_x{\mathrm{\λ}}_3{\mathrm{\λ}}_4)& \frac{f_v{n}_x}{{2f}_u}{\mathrm{\λ}}_3\\ \frac{1}{k_1}({-k}_3{n}_z{\mathrm{\λ}}_3\cos \mathrm{\β}-\frac{k_2}{2}{n}_z\cos \mathrm{\β}+n_z{\mathrm{\λ}}_1{\mathrm{\λ}}_3\cos \mathrm{\β}+{\mathrm{\λ}}_2{\mathrm{\λ}}_3{\sin }^2\mathrm{\β})& \frac{1}{2}{n}_z\cos \mathrm{\β}\end{array}\right]---\left(15\right)$

c.关于预测控制器驱动方面

根据式(9)，在当前状态s_k处线性化得到线性模型离散化之后可得到：

s(k+1)＝s(k)+G·u(k) (16)

其中G＝J(s_k)·T,T为时间周期。

模型预测控制使用滚动优化的策略求取最优的控制输入，针对上述线性化模型，定义代价函数如式(17)。要求解上式最小化问题，可作为二次规划求解

$J\left(k\right)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}\{s_e^T(k+j|k){\mathrm{Qs}}_e(k+j|k)+{\mathrm{\Δu}}^T(k+j-1|k)\mathrm{R\Δu}(k+j-1|k)\}---\left(17\right)$

s.t

$\left[\begin{array}{cc}1& 1\\ 1& -1\\ -1& -1\\ -1& 1\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]\≤\left[\begin{array}{c}{2v}_{\max }\\ {2v}_{\max }\\ {-2v}_{\max }\\ {-2v}_{\max }\end{array}\right]---\left(18\right)$

其中Q,R分别是加权矩阵，s_e＝s-s_r为当前系统状态变量与过去生成的参考轨迹状态变量的偏差，v_max为履带的最大速度。

步骤5：机器人接收驱动信号进行移动，同时利用摄像头获取实时图像，进而得到系统状态，结合规划的图像空间特征，判断是否达到目标位置，若没有则继续上述步骤(4)，需要切换关键帧则进行步骤(3)；若达到目标，则方法结束。

实施例

图6和图7表示了该改进方法的运行结果，给定条件为：起始位姿为(0,0,0)，终止位姿为(150,200,0.1)。相关参数取： $y_{{\mathrm{ICR}}_l}=10\mathrm{cm},y_{{\mathrm{ICR}}_r}=-10\mathrm{cm},z_{\mathrm{ICR}}=50\mathrm{cm},$ 相机光轴与地面夹角β＝pi/5，相机中心距地面高度为d＝10cm，f_u、f_v均为500，机器人v_max＝100cm/s,采样周期T＝0.01s，控制器的预测时域为N＝20，Q＝I_4×4，R＝0.1×I_2N×2N。

机器人开始运行后，首先根据已知的起始、终止位置规划出一条满足机器人非完整约束的移动路径，并将其转换到图像特征空间，将表示为图像空间特征的系统状态作为预测控制器的设定值，再结合实际机器人当前图像帧与关键帧获得的系统状态对机器人进行驱动控制。当运行满足一定条件时，对关键帧进行切换，重新规划路径，继续之前的步骤直至达到期望位姿。具体结果见图6和图7。

图6表示的是机器人在二维平面下的移动轨迹，其中以机器人起始坐标系作为世界坐标系，z轴为机器人起始位置下的朝向，y轴为机器人起始位置下的侧向，单位均为cm；图7表示的是控制过程的机器人与期望位置的距离变化。实际运行结果为，机器人最终移动到的位置为(149.6,199.8,0.10)，非常接近期望位置(150,200,0.1)，故从结果可以看出，本发明能够很好的实现视觉导航功能，移动轨迹平滑且误差较小，导航精度高。

Claims (9)

1.一种基于关键帧策略的移动机器人视觉伺服控制方法，用于移动机器人在局部已知环境下的导航，所述的移动机器人为履带式机器人，自带单目摄像头可实时采集图像信息；其特征在于，该方法将导航任务转化到基于图像单应性定义的图像特征空间中，设计预测控制器在图像特征空间中进行轨迹跟踪控制，包括以下步骤：

(1)将机器人放置在初始位置上，给定期望位置坐标，利用已知的机器人起始、终止位置结合机器人自身的非完整约束规划出机器人在二维平面下的移动路径；

(2)根据步骤(1)得到的机器人移动路径，通过利用单应性关系定义图像特征，并从单应性矩阵中选取合适的元素作为系统状态量，进而将该路径转化到图像特征空间里，生成一组图像空间特征即系统状态；

(3)机器人开始移动后，由摄像头获取图像信息，首先把第一帧作为关键帧，之后基于当前图像帧与关键帧之间的单应性关系定义图像特征，以图像特征隐式表达机器人的运动情况；其中关键帧在机器人移动过程中需要进行切换，而切换需满足一定的条件：a.目标物体对于当前帧不可见，但是对于下一帧可见；b.切换的关键帧与上一可见帧之间应存在一定的时间间隔；c.当系统状态有接近奇异的趋势；d.关键帧切换的时间间隔不应太长；且在每一次关键帧切换之后，需要重新对路径进行规划，然后通过步骤(2)转化为图像空间轨迹；

(4)将步骤(2)得到的图像空间轨迹的系统状态作为设定值，步骤(3)中实时得到的当前系统状态作为测量值，共同作为控制器的输入信号，再由控制器输出信号驱动机器人运动，控制器采用模型预测控制，其中相互关系模型通过如下方式建立：基于机器人运动学模型和相机模型，经过系统状态对时间的微分最终得出系统状态关于速度的图像雅可比矩阵即作为相互关系模型；

(5)机器人接收驱动信号进行移动，同时利用摄像头获取实时图像，进而得到系统状态，结合规划的图像空间特征，判断是否达到目标位置，若没有则继续上述步骤(4)，需要切换关键帧则进行步骤(3)；若达到目标，则方法结束。

2.根据权利要求1所述的方法，其特征在于：步骤(1)中所述在二维平面下机器人移动轨迹满足的约束根据公式(1)和(2)进行，公式(1)和(2)分别为：

ρ(y,z)＝0

式中(y,z)表示机器人在二维坐标系(坐标轴为y轴和z轴)下的坐标，ρ表示移动轨迹，表示机器人的偏转角度。

3.根据权利要求1所述的方法，其特征在于：步骤(1)中在控制过程中的机器人几何中心的起始、终止坐标分别表示为公式(5)，公式(5)为：

其中表示机器人几何中心的初始坐标，表示机器人几何中心的终止位置坐标，表示相机中心的终止位置坐标，k_i为待辨识参数，i为1、2、3。

4.根据权利要求1所述的方法，其特征在于：步骤(2)中所述的单应性图像特征由公式(6)定义，公式(6)为：

$H=K(R-\frac{{\mathrm{tn}}^T}{d})K^{-1}$

其中，H即为单应性矩阵，(R,t)表示了机器人的运动状态，n为相机坐标系下地面的法向量，d为相机坐标系原点与地面之间的距离,K为相机内部参数矩阵。

5.根据权利要求1所述的方法，其特征在于：步骤(2)中单应性图像特征的获取以及系统状态变量的定义分别由公式(10)和(11)决定，

公式(10)为：

公式(11)为：

${\left[\begin{array}{cccc}{s}_1& s_2& s_3& s_4\end{array}\right]}^T=\frac{1}{h_{22}}\·{\left[\begin{array}{cccc}{h}_{11}& h_{12}& h_{21}& h_{33}\end{array}\right]}^T;$

其中f_u、f_v分别表示焦距在像素坐标轴上对应的像素点数目,β是相机光轴与地面的夹角，是机器人期望位姿下的偏转角度，h_ij为单应性矩阵H中第i行第j列的元素。

6.根据权利要求1所述的方法，其特征在于：步骤(4)中所述的机器人运动学模型根据公式(12)确定，公式(12)为：

$v_y=\frac{v_r-v_l}{y_{{\mathrm{ICR}}_r}-y_{{\mathrm{ICR}}_l}}{z}_{ICR}$

$v_z=\frac{v_r+v_l}{2}-\frac{v_r-v_l}{y_{{\mathrm{ICR}}_r}-y_{{\mathrm{ICR}}_l}}\frac{y_{{\mathrm{ICR}}_r}+y_{{\mathrm{ICR}}_l}}{2}$

$\mathrm{\ω}=\frac{v_r-v_l}{y_{{\mathrm{ICR}}_r}-y_{{\mathrm{ICR}}_l}}$

其中v_y、v_z、ω分别表示相机的侧向、前向线速度和角速度，v_l、v_r分别为左右两条履带的速度，z_ICR分别表示三个旋转瞬心的坐标。

7.根据权利要求1所述的方法，其特征在于：步骤(4)中新定义了一种位姿参数，根据公式(13)进行，公式(13)为：

其中s为系统状态变量，为相机中心的期望位置坐标，其余参数定义参照上文。

8.根据权利要求1所述的方法，其特征在于：步骤(4)中所述的相互关系模型由公式(14)和(15)确定，公式(14)和(15)分别为：

$\stackrel{\·}{s}=J\left(s\right)\·u$

$J=\left[\begin{array}{cc}\frac{1}{k_1}(-k_3{n}_x{\mathrm{\λ}}_3\sin \mathrm{\β}-\frac{k_2}{2}{n}_x\sin \mathrm{\β}+n_x{\mathrm{\λ}}_1{\mathrm{\λ}}_3\sin \mathrm{\β}+{\mathrm{\λ}}_2{\mathrm{\λ}}_3{\cos }^2\mathrm{\β})& \frac{1}{2}{n}_x\sin \mathrm{\β}\\ -\frac{f_u}{k_1{f}_v}{{\mathrm{\λ}}_2}^2\sin \mathrm{\β}& 0\\ \frac{f_v}{k_1{f}_u}(k_3{n}_x-\frac{k_2}{2}{n}_x{\mathrm{\λ}}_3+{{\mathrm{\λ}}_2}^2\sin \mathrm{\β}+n_x{\mathrm{\λ}}_3{\mathrm{\λ}}_4)& \frac{f_v{n}_x}{2f_u}{\mathrm{\λ}}_3\\ \frac{1}{k_1}(-k_3{n}_z{\mathrm{\λ}}_3\cos \mathrm{\β}-\frac{k_2}{2}{n}_z\cos \mathrm{\β}+n_z{\mathrm{\λ}}_1{\mathrm{\λ}}_3\cos \mathrm{\β}+{\mathrm{\λ}}_2{\mathrm{\λ}}_3{\sin }^2\mathrm{\β})& \frac{1}{2}{n}_z\cos \mathrm{\β}\end{array}\right]$

其中作为被控变量，为待辨识参数。

9.根据权利要求1所述的方法，其特征在于：步骤(4)中所述的根据线性化相互关系模型得出的代价函数由公式(17)定义，公式(17)为：

$J\left(k\right)=\underset{j=1}{\overset{N}{\Σ}}\{s_e^T(k+j|k){\mathrm{Qs}}_e(k+j|k)+{\mathrm{\Δu}}^T(k+j-1|k)R\mathrm{\Δ}u(k+j-1|k)\}$

其中Q,R分别是加权矩阵，s_e＝s-s_r为当前系统状态变量与过去生成的参考轨迹状态变量的偏差，N是预测时域。