轮式移动机器人视觉伺服轨迹跟踪并发深度辨识的制作方法

本发明属于计算机视觉与移动机器人的技术领域，特别是涉及一种移动机器人同时视觉伺服轨迹跟踪并发自适应深度辨识方法，能够跟踪期望时变轨迹并将其场景深度信息识别出来。

背景技术：

轮式移动机器人往往工作在危险的环境中，由于工作环境中的不确定因素，许多科研工作者研发了各种不同的解决方法来提高系统的自主控制能力。近来，由于图像处理技术的提高和控制算法理论的发展，许多科研人员采用基于视觉传感器的自主控制技术，进而来实现系统的自主导航与控制。

对于移动机器人系统而言，引入视觉传感器可大大增强其智能性，灵活性和环境感知能力，利用实时图像反馈来控制移动机器人的运动，即视觉伺服技术，能够广泛运用于各种领域，如智能交通和环境勘探，由于这些原因，这项技术受到格外关注并且成为机器人领域的研究热点。对于视觉传感器，由于是根据透视投影模型成像，其深度信息的缺失是主要缺陷，因此，对于单目摄像机视觉系统，难以完整恢复出外部三维场景信息和移动机器人自身运动信息；另外，由于移动机器人存在非完整运动约束的特点，使得位姿控制器的设计非常具有挑战性，因此，深度信息的缺失和非完整约束的限制，使移动机器人视觉控制任务变得异常艰巨；然而，现有方法大多在原有视觉伺服控制器的基础上为未知的场景信息设计补偿模块。在这种意义上，视觉伺服任务完成后依然无法得到场景模型，由于工作空间信息无法完全获得，因此限制了机器人系统的进一步应用与推广。综上所述，如何在视觉伺服控制的同时进行深度信息辨识，是机器人控制领域内一个困难但非常有价值的问题。

技术实现要素：

一种轮式移动机器人视觉伺服轨迹跟踪并发深度辨识方法。本发明针对非完整运动约束下的轮式移动机器人设计了一种视觉跟踪控制方法，并能在视觉伺服轨迹跟踪的同时辨识出场景的深度信息。首先，对共面特征点录制一段图像视频来代表待跟踪的期望轨迹。然后，通过比较静态参考图像中的特征点，当前实时图像中的特征点和预先录制的图像视频，结合投影几何关系，建立欧式坐标系下的单应矩阵。然后通过分解所得到的单应性矩阵，设计了运动控制律和深度辨识参数的自适应更新律。另外，通过扩展并发学习方法，用历史和当前系统数据去建立一个自适应更新率来恢复未知的深度信息。最后通过lyapunov方法和扩展芭芭拉定理证明了系统跟踪误差和深度辨识误差同时收敛，系统全局稳定，能够有效的辨识出将场景的深度信息。

一种轮式移动机器人同时视觉伺服并发自适应深度辨识方法，其特征在于括以下步骤：

1、一种轮式移动机器人同时视觉轨迹跟踪并发自适应深度辨识方法，其特征在于包括以下步骤：

第1，定义系统坐标系，包括：

第1.1，建立系统模型

定义轮式移动机器人的坐标系如下：以表示摄像机相对于静止特征点的参考坐标系，以表示轮式移动机器人当前位姿坐标系，以表示对应于轮式移动机器人期望位姿的直角坐标系，由共面特征点pi确定的平面为参考平面π，定义平面π的单位法向量为n^*，3-d欧氏坐标pi在下分别用pi(t)，pi^d(t)，来表示：

假设各个坐标系的原点到特征点沿光轴方向的距离恒为正，从到的旋转矩阵为从到的平移向量为其中^ct*(t)为在中表示，同样表示期望从到的时变期望旋转矩阵，从到的期望平移向量为其中^dt*(t)在中表示，^ct*(t)和^dt*(t)的定义如下：

和定义如下：

定义如下：

另外表示从到的旋转矩阵的右手旋转角度，θd表示到的旋转矩阵的右手旋转角度，可以看出：

其中代表wmr的期望角速度在中的表示，从到π的沿π的单位法向量的距离设为则

其中表示π的单位法向量。

第1.2，欧式重建

首先重建特征点和的归一化欧式坐标pi在和下的表示：

为了得到欧氏坐标，每个特征点由下的vi(t)，下的vdi(t)，下的示的投影像素坐标，他们是(即实际时变图像点)，(即期望轨迹图像点)，(即恒定参考图像点)的元素，特征点的归一化欧氏坐标通过针孔镜头模型与图像点建立如下关系：

其中是已知不变的摄像机内参数标定矩阵，坐标系之间的旋转矩阵和平移向量可由归一化欧氏坐标表示如下：

其中h(t)，表示欧氏单应矩阵，^ct*h(t)，为含有比例因子的平移向量：

然后通过欧几里得重建技术分解h和hd，得到^ct*h(t)，^dt*h(t)，θ(t)，θd(t).

第3，构建自适应控制器

根据系统的开环动态方程，为配有摄像机的移动机器人系统设计控制器和自适应更新律，控制的目的是确保坐标系跟踪上的时变轨迹，用e(t)＝[e1，e2，e3]^t表示平移和旋转跟踪误差，具体定义如下：

其中^ct*h1(t)，^ct*h2(t)，^dt*h1(t)和^dt*h2在(12)中定义，θ(t)，θd(t)在(13)中定义，另外辅助变量定义如下：

开环误差方程为：

深度估计误差定义如下：

其中是深度辨识，当趋向于零时未知的深度信息将会有效的识别出来，轮式移动机器人设计的线速度和角速度如下所示：

根据并发学习方法，为深度辨识设计自适应更新律，其形式如下：

其中为更新增益，为正常数，tk∈[0，t]是初始时间和当前时间之间的时间点。投影函数proj(χ)定义为：

其中是正值的下界，因而有可以得到下式：

根据之前的推导，闭环误差系统如下：

当下式成立：

至此，完成了移动机器人同时视觉伺服轨迹跟踪与并发自适应深度辨识。

本发明的优点和有益效果

本发明提出了一种轮式移动机器人同时视觉伺服与自适应深度辨识方法。本发明主要做出了以下几方面工作：1.成功辨识视野的深度信息，通过视觉系统得到对外部环境的良好感知；2.用建立欧式坐标系下的单应矩阵，之后通过分解所得单应性矩阵，将机器人有效驱动到期望位姿；3.因误差同时收敛，组合控制器和深度辨识模块解决系统全局稳定问题。

附图说明：

图1为定义坐标系关系；

图2为仿真结果：轮式移动机器人的期望的当前运动路径；

图3为仿真结果：特征点的图像轨迹[虚线：所需轨迹；实线：当前轨迹]；

图4为仿真结果：系统误差收敛到零[虚线：期望值；实线：真值]；

图5为仿真结果：轮式移动机器人的角速度和线速度；

图6表示实验结果：由参数自适应更新律得到的变化[实线：的值；虚线：d^*的真实值]。

具体实施方式：

实施例1

1、一种轮式移动机器人同时视觉轨迹跟踪并发自适应深度辨识方法，其特征在于包括以下步骤：

第1，定义系统坐标系，包括：

定义轮式移动机器人的坐标系如下：以表示摄像机相对于静止特征点的参考坐标系，以表示轮式移动机器人当前位姿坐标系，以表示对应于轮式移动机器人期望位姿的直角坐标系，由共面特征点pi确定的平面为参考平面π，定义平面π的单位法向量为n^*，3-d欧氏坐标pi在下分别用来表示：

假设各个坐标系的原点到特征点沿光轴方向的距离恒为正，从到的旋转矩阵为从到的平移向量为其中^ct*(t)为在中表示，同样表示期望从到的时变期望旋转矩阵，从到的期望平移向量为其中^dt*(t)在中表示，^ct*(t)和^dt*(t)的定义如下：

和定义如下：

定义如下：

另外表示从到的旋转矩阵的右手旋转角度，θd表示到的旋转矩阵的右手旋转角度，可以看出：

其中代表wmr的期望角速度在中的表示，从到π的沿π的单位法向量的距离设为则

其中表示π的单位法向量。

第1.2，欧式重建

首先重建特征点和的归一化欧式坐标pi在和下的表示：

为了得到欧氏坐标，每个特征点由下的下的下的示的投影像素坐标，他们是(即实际时变图像点)，(即期望轨迹图像点)，(即恒定参考图像点)的元素，特征点的归一化欧氏坐标通过针孔镜头模型与图像点建立如下关系：

其中是已知不变的摄像机内参数标定矩阵，坐标系之间的旋转矩阵和平移向量可由归一化欧氏坐标表示如下：

其中h(t)，表示欧氏单应矩阵，为含有比例因子的平移向量：

然后通过欧几里得重建技术分解h和hd，得到^ct*h(t)，^dt*h(t)，θ(t)，θd(t).

第3，构建自适应控制器

根据系统的开环动态方程，为配有摄像机的移动机器人系统设计控制器和自适应更新律，控制的目的是确保坐标系跟踪上的时变轨迹，用e(t)＝[e1，e2，e3]^t表示平移和旋转跟踪误差，具体定义如下：

其中^ct*h1(t)，^ct*h2(t)，^dt*h1(t)和^dt*h2在(10)中定义，θ(t)，θd(t)在(4)中定义，另外辅助变量定义如下：

开环误差方程为：

深度估计误差定义如下：

是深度辨识，当趋向于零时未知的深度信息将会有效的识别出来，轮式移动机器人设计的线速度和角速度如下所示：

根据并发学习方法，为深度辨识设计自适应更新律，其形式如下：

其中为更新增益，为正常数，tk∈[0，t]是初始时间和当前时间之间的时间点。其中为更新增益，为正常数，tk∈[0，t]是初始时间和当前时间之间的时间点，可以看出，在自适应更新率的并发学习法中使用了n个采样周期中记录的数据，因此需要最优平滑器来估计wc(tk)，^ct*h2(tk)，vc(tk)的准确值，在这种程度来说，可以看到在参数估计中具有明显的改进。

投影函数proj(χ)定义为：

其中是正值的下界，因而有可以得到下式：

实际上，控制参数kv，kw主要对机器人的控制和更新增益γ1，γ2有影响从而改变机器人的深度识别，但由于参数相当小，这使其很容易正确调整参数，从而可以用于实际应用。根据之前的推导，闭环误差系统如下：

当下式成立：

至此，完成了移动机器人同时视觉伺服轨迹跟踪与并发自适应深度辨识。

第4，稳定性分析

定理1：控制律和参数更新律保证了轮式移动机器人的跟踪误差渐进趋于零，将移动机器人镇定并跟踪上期望轨线同时进行了深度辨识，即下式成立：

假定期望轨线的时间导数满足如下条件：

证明：

定义非负的lyapunov函数为：

对lyapunov函数求导，并将开环误差方程代入可得：

将(16)代入上式(24)，得到如下表达式：

可以看出

从(23)和(26)可知e1(t)，e3(t)，和e1(t)，从(14)中看出d^*是常数，根据所构造的^dt*h1(t)，^dt*h2(t)，θd(t)，为有界函数以及的性质，利用(11)(12)和(16)中的表达式可证明e2(t)，^ct*h1(t)，^ct*h2(t)，θd(t)，基于前面的发展，通过(13)(15)(16)可以得到vc(t)，因此所有的系统状态变量都保持有界性。另外，定义ф为所有使的点的集合：

定义m为ф的最大不变集，从(25)可知下列关系式对m中的点成立：

因此：

然后将(28)和(29)代入在(19)中的闭环动态方程，得到如下：

因此根据上文中的假设，得出在集合m中从(12)和(29)中可以证明e2＝0，由于使用投影函数(17)使分段光滑，但其对于给定初始条件是连续的，从(14)(16)和(28)可以看出：

从中得到的正边界是不变的，最大不变集m仅包含平衡点，其形式如下：

根据拉塞尔不变性原理，移动机器人跟踪误差和深度辨识误差渐进收敛至零，即

第5，系统仿真

在这部分中，本发明提供了仿真结果来验证所提出的方法的性能，为了得到单应性矩阵，随机选取4个平面特征点，然后摄像机的内参数设置如下：

加入标准差为σ＝0.15的图像噪声测试控制器的稳定性和特征高度辨识的抗干扰能力，选择控制器参数并设置为kv＝0.605，kω＝0.100，γ1＝γ2＝0.900。n选择为100，记录前100个采样周期中的数据。

仿真结束后，从图2所示的轮式移动机器人的路径，可以看出机器人成功地跟踪期望的轨迹；可以在图3看出，当前特征点图像轨迹有效追踪到期望特征点的图像轨迹；从图4可以看出，系统误差收敛到零；图5显示了轮式移动机器人的vc(t)和ωc(t)；图6表明深度估计快速高效收敛于其真实值，这意味着未知的场景深度信息是可以完全识别的；可以看出，机器人以高效率跟踪所需的轨迹路径具有小的稳态误差。