梯度下降法在双目视觉导航中的一个运用

2018-08-31广东工业大学袁文波陈庆瑜高洪兵王钦若

电子世界 2018年16期

广东工业大学袁文波陈庆瑜高洪兵王钦若

1 引言

本文将讨论的是一种基于双目视觉且路线固定的视觉导航方式，如无人搬运机器人，无人快递小车的导航。其导航特点是路线已知且相对固定，机器只需沿着设定好的路线前进就能达到目的地。

2 无人快递车导航原理概述

要实现自动导航，那么这就要求机器必须知道自己的实时位姿与正确位姿的相对偏差。由于快递小车的运行路线固定，我们可以事先人为控制让安装有双目摄像头的小车沿着正确的道路行走一遍，双目摄像头相对小车位置固定，并对前进路线环境进行拍摄，我们每一帧将有两张照片，左摄像头及右摄像头所拍摄的照片，我们对两张照片进行特征点匹配，本文选用的是SIFT特征点匹配方式，并将匹配上的特征点的特征向量及其在小车坐标系（一般选小车的中心点或者小车的某个顶点作为小车坐标系的原点，小车的正前基线、水平基线、竖直方向基线分别与小车坐标系的三个坐标轴平行）的坐标按照片先后顺序进行存储。双目视觉确定匹配上的特征点在小车坐标系的坐标方法已经很成熟在这里将不再赘述。当小车在正常运作时，不断拍摄前方环境，并在左右帧照片中进行特征点匹配，将匹配上的特征点在与之前保存的特征点匹配，选取匹配度最高的一帧作为此次调整的正确位姿。并根据匹配上的特征点的坐标计算出小车实时位姿与正确位姿的偏差，从而达到导航的目的。本文将要详细阐述的是在小车正常运行时如何检测小车实时位姿与正确位姿的偏差。

图1 SIFT特征点匹配效果

3 实时位姿与正确位姿偏差计算

图2 实时位姿与正确位姿偏差示意图

当小车正常工作时，进行左右两张照片特征点的匹配，及计算出匹配点相对小车坐标系的坐标。将匹配上的特征点与系统里事先扫描路线预存的特征点以帧为跨度进行匹配，选取匹配点数最高的一帧，接下来就是要根据这些匹配点在其各自坐标系的坐标求出这两个坐标系的变换矩阵，有了变换矩阵也就知道了小车实际位姿与正确位姿的偏差。有了这个偏差便能对小车位姿进行调控。其实这就转化成了一个数学问题，已知同一组点在两个三维坐标系的坐标求两三维坐标系之间的变换矩阵。首先我们来观察一下，两个空间三维直角坐标系能够通过怎样的变换使得两个坐标系重合，很明显我们可以先进行平移使得两个坐标系原点重合，重合后分别先后绕三个坐标轴进行三次旋转，一定能使两坐标系重合。以下四个变换矩阵分别对应以上的四个步骤，平移，绕X轴旋转，绕Y轴旋转，绕Z轴旋转。

最终变换矩阵R计算如下：

随机选取五个点，在坐标坐标系1的坐标构成矩阵Y1则Y1矩阵如下,每列代表一个点的坐标x,y,z，添1为了计算方便。

则这五点在第二个坐标系的坐标计算公式如下：

实际情况是我们有了五个点在两个坐标系的坐标，要求坐标系的变换矩阵,即求那六个参数，如果这些坐标都是精确的，那就是一个解方程的问题，但实际运用时测量都是有误差的，本文介绍一种在利用梯度下降的方法在坐标有误差的情况下求出精度符合要求的变换矩阵。我们假设xt=50，yt=-80zt=200，xr=3.1416/2，yr=3.1416/3，zr=3.1416/5带入各变换矩阵，利用公式(7)求得该五点在坐标系二的坐标如下：

再以xt,yt,zt,xr,yr,zr为未知数通过公式(9)计算，

则在坐标系二的五点的坐标为包含xt,yt,zt,xr,yr,zr的表达式，再通过公式（10）计算Loss：

此时Loss为包含xt，yt，zt，xr，yr，zr的表达式，将Loss分别对xt,yt,zt,xr,yr,zr求偏导，利用梯度下降法，不断迭代，使Loss下降到一个可以接受的范围，则便可得到各个参数较为准确的值。之后再在Y2中加入随机的正态分布误差来模仿实际测量带来的误差，用同样的方法得到各个参数较为准确的值。如下两矩阵分别是以u=0，σ=1，u=0，σ=2，随机生成的干扰数据。