CN106556395A - 一种基于四元数的单目视觉***的导航方法 - Google Patents

Abstract

本发明公开一种基于四元数的单目视觉***的导航方法，包括：步骤1、构建环境地图：步骤2、保存地图数据和轨迹点坐标信息；步骤3、加载地图和轨迹点坐标根据偏航角θ计算转角β，步骤4、根据步骤3获得的角度β和转角的方向，发送命令到车辆底层控制。采用本发明的技术方案，实现Linux***下的ROS平台上的基于四元数的单目视觉***的导航，降低了研究成本。

Description

一种基于四元数的单目视觉***的导航方法

技术领域

本发明属于低速无人巡逻车自主导航领域，涉及一种单纯利用单目摄像头室内室外定位和导航的方法。

背景技术

我国正在研发的无人驾驶智能车正在如火如荼的进行当中，吸引了无数高校、科研机构研究人员及汽车厂商的广泛关注。低速巡逻智能车巡逻的工作环境是动物园，在动物园中植被繁盛，经测试部分巡逻路段接收GPS信号较弱，无法根据GPS导航车辆行进。因此，利用传感器获取外界信息进行地图构建和导航尤为重要。近年来SLAM问题在驾驶智能车领域一直受到研究人员的广泛关注。它能利用机器人自身携带的传感器建立递增的环境地图，然后通过已构建地图实现自身定位，无需任何外部参考***(如:GPS)和其它传感器，具有潜在的经济价值和广泛的应用前景。

对于无人驾驶智能车的研究中，地图构建和定位是车辆辅助驾驶***及无人驾驶智能车领域的关键技术。目前对于SLAM的各种算法已经发展的越来越成熟，主要是利用一种或多种传感器信息使得机器人能够自主建图定位。其求解方法分为基于卡尔曼滤波的方法、基于粒子滤波方法和基于图优化的方法。基于图优化的方法由于其在大规模环境中的优良建图性能，成为了目前国内外研究的主要方法。但是目前对于这方面的研究只限于定位和建图，对于导航功能而言基本是联合其他传感器信息的导航，如超声波传感器、激光测距仪、雷达、立体视觉等，对于多传感器的使用增加了经济成本。

发明内容

针对现有技术存在的上述问题，本发明提出了一种在Linux***下的ROS平台上的基于四元数的单目视觉***的导航方法。首先，需要实现基于关键帧的地图构建，关键在于保存建图过程中的相机中心的轨迹坐标信息；其次对轨迹点坐标信息按照一定的规则进行过密点筛选；然后重新加载地图和轨迹点坐标数据；最后按照本发明导航部分算法计算当前帧坐标点到下一位置相机中心点坐标的行驶角度和路线，完成导航功能。

为实现上述目的，本发明采用如下的技术方案：

一种基于四元数的单目视觉***的导航方法，包括以下步骤：

步骤1、构建环境地图，包括以下步骤：

1)进行摄像头标定；

2)将摄像头固定到车辆正前方；

3)将输入图像从RGB颜色空间转换到灰度颜色空间；

4)对图像进行特征提取和匹配；

5)地图初始化；

6)闭环检测和重定位；

7)获取当前相机位姿，计算四元数转换到180度到180度范围的欧拉角ψ、θ、所述ψ、θ、分别为绕Z轴、Y轴、X轴的旋转角度，偏航角θ即为当前帧的角度；

步骤2、保存地图数据和轨迹点坐标信息；

步骤3、加载地图和轨迹点坐标根据偏航角θ计算转角β，

步骤4、根据步骤3获得的角度β和转角的方向，发送命令到车辆底层控制。

作为优选，ψ、θ和范围为-90度到90度，对应求解公式为：

当θ范围为-180度到+180度时，对应求解部分公式为：

作为优选，步骤3计算转角β的过程为：

设(dx,dy)、(gx,gy)分别为当前点和要到达点坐标，当前帧角度为θ，设当前点和目标点连线与x轴夹角为范围为0到90度；β为转角，底层车身控制从左到右为0到180度；γ为中间决策层判断角，不同目标点与当前点位置关系有不同的值。当前位置和目标点位置相对位置关系分为下列四种不同情况时：

当运动方向为E到F时，即当(dx>gx&&dy<gy)时：

若0＝<θ<＝90即L1方向时，则左转β＝|θ|+90-α，若-180＝<θ<-90即L4方向时，则右转β＝|θ|-90+α，若-90＝<θ<0即L2和L3方向时，则判断γ＝90-α-|θ|，若γ>0即L2方向则左转β＝γ，否则即L3方向则右转β＝-γ；

当运动方向为F′到E′时，即当(dx<gx&&dy>gy)时：

若0＝<θ<＝90即L1方向时，则右转β＝90-|θ|+α，若-180＝<θ<＝-90即L4方向时，则左转β＝180-|θ|+90-α，若90<θ<＝180即L2和L3方向时，则判断γ＝90-α-(180-|θ|)，若γ>0即L3方向时则左转β＝γ否则即L2方向则右转β＝-γ；

当运动方向P为到Q时，即当(dx<gx&&dy<gy)时：

若-90＝<θ<＝0即L1方向时，则右转β＝90+|θ|-α，若90＝<θ<＝180即L4方向时，则左转β＝|θ|-90+α，若0<θ<90即L2和L3方向时，则判断γ＝90-α-|θ|，若γ>0即L2方向时则右转β＝γ否则即L3方向则左转β＝-γ；

当运动方向为Q′到P′时，即当(dx>gx&&dy>gy)时：

若-90＝<θ<＝0即L1方向时，则左转β＝90-|θ|+α，若90＝<θ<＝180即L4方向时，则右转β＝180-|θ|+90-α，若-180＝<θ<-90即L2和L3方向时，则判断γ＝90+α-|θ|，若γ>0即L2方向时则左转β＝γ否则即L3方向则右转β＝-γ；

本发明由于采用以上技术方案，其具有以下优点：1、本发明利用单目视觉SLAM***在建图过程中计算相机中心位姿坐标作为行驶轨迹点坐标。2、本发明提出一种利用建图后的单目轨迹点坐标和重定位后的相机中心位姿采用四元数与欧拉角的转换，计算出车辆转角数据进行导航，该方法未使用任何其它外部传感器，单纯利用摄像头完成导航，降低了研究成本，有潜在的应用价值。

附图说明

图1为本发明所涉及方法流程图；

图2为张正友标定法使用的棋盘图；

图3为FAST角点检测示意图；

图4为基于词袋模型的闭环检测流程图；

图5为偏航角计算流程图；

图6为每一帧对应的欧拉角折线图；

图7为转角方向计算示意图；

图8为未加密地图下的导航轨迹结果图；

图9为图8轨迹偏差分析图；

图10为加密地图下的导航轨迹结果图；

图11为图10轨迹偏差分析图；

图12为图像建图和导航下的GPS轨迹对比图；

表1为不同四元数转欧拉角取值结果；

表2为转角决策表。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

本发明所述方法的流程图如图1所示，包括以下步骤：

步骤1，地图构建

步骤1.1，摄像机标定

相机标定方法采用张正友相机标定法：如图2棋盘格式为7乘7边长为0.02m的黑白相间的方格。将相机对准7乘7的平面棋盘，手持棋盘以各种方向得到三十多张棋盘图像，即可计算出相机的内参数和畸变参数。

基本原理如下：

其中，k为相机的内参数矩阵，[X Y 1]^T为平面棋盘上点的齐次坐标，[u v 1]^T位棋盘上点投影到图像平面上对应点的齐次坐标，[r₁ r₂ r₃]和t分别是摄像机坐标***下的旋转和平移向量，k[r₁ r₂ t]为单应性矩阵H。根据矩阵求解方法，当图像数目大于等于3时k可以求出唯一解。

步骤1.2，将摄像头固定在车辆正前方，保证地图构建时和导航时位置和方向均保持不变。

步骤1.3，将输入图像从RGB颜色空间转换到灰度颜色空间，转换公式：Gray＝R*0.299+G*0.587+B*0.114；

步骤1.4，对图像进行特征提取和匹配

特征提取和匹配均采用ORB(Oriented Brief)方法，首先采用FAST-12算法来检测特征点如图3所示：

I(x)为圆周上任一点的灰度值，I(p)为圆心的灰度，ε_d为灰度值差的阈值，如果大于阈值则认为p是一个特征点。

然后采用BRIEF算法来计算一个特征点的描述子：平滑图像后在特征点周围选择一个Patch，在这个Patch内通过一种选定的方法来挑选出来256个点对。然后对于每一个点对(p,q)比较其亮度值，如果I(p)>I(q)则这个点对生成了二值串中一个的值为1，如果I(p)<I(q)，则对应在二值串中的值为-1，否则为0。所有256个点对，都进行比较之间，则得到一个256长度的二进制串，采用二进制数串来表示描述子。

步骤1.5，地图初始化

进行单目SLAM的地图初始化，移动摄像头一段距离，得到两个关键帧，通过关键帧之间的特征匹配计算Homography模型或Fundamental模型得到初始化的地图模型，Homography模型的计算方法为normalized DLT，Fundamental matrix的计算方法为normalized 8points。在视觉里程计部分，首先对两幅图像帧P_K-2、P_K-1进行特征提取和匹配；然后三角化P_K-2、P_K-1的特征，对新的图像P_K特征提取并和P_K-1特征匹配；最后通过采用PnP算法从3D到2D的匹配中估计相机位姿，通过Bundle Adjustment进行优化，可使用图优化工具g2o进行地图优化。PnP(Perspective-n-Point)问题的模型为：

R为相机的姿态，C为相机的标定矩阵，(u,v)为二维像素坐标，(x,y,z)为三维坐标即世界坐标系下的坐标。

步骤1.6，闭环检测和重定位

把当前帧转换成图像词袋，然后检索图像数据库，图像数据词袋库部分采用经典的DBoW2词袋库模型，通过计算单目摄像头中输入的图像与词袋库中模型的相似场景，为全局重定位查找关键帧如图4所示。关键帧的***标准为当新帧检测到的特征点云与参考关键帧特征点云差距小于90％时，确定当前帧为关键帧，保存至地图数据中。我们计算ORB特征和每个关键帧的地图云点的对应关系，然后对每个关键帧执行RANSAC迭代计算，用PnP算法找出相机在世界坐标系中的位置即相机位姿用四元数表示。四元数是一个标量(w)和一个3D向量(x,y,z)的组合。四元数的定义：

q＝[w x y z]^T

|q|²＝w²+x²+y²+z²＝1

步骤1.7，获取当前相机位姿

获取通过PnP+RANSAC计算出的当前相机的位姿信息，即表示三维空间旋转的四元数后。将四元数转换到欧拉角，ψ、θ、分别为绕Z轴、Y轴、X轴的旋转角度，偏航角θ即为当前帧的角度。通常定义的ψ、θ和范围为-90度到90度，对应求解公式为：

欧拉角转四元数公式为：

偏航角范围为-90到90度时不能唯一表示当前的位置信息，因此需要将偏航角范围扩展到-180度到180度。当θ范围为-180度到+180度时对应求解部分公式如下所示。仅对于求偏航角来说整个算法结合公式1、2、3其流程图如图5所示，表1为不同四元数转欧拉角取值结果，图6为旋转两圈时对应每一帧位置的欧拉角。

表1四元数转欧拉角的转换输出对比(单位：度)

当前帧四元数	真实欧拉角θ	本算法输出
			(0.0297，0.9994，0.0152，0.0046)	-180	-180
(-0.0275，-0.8640，0.0126，0.5025)	-120	-120
			(-0.0214，-0.5709，0.0086，0.8207)	-70	-70
(0.3214，0.117，0.3214，0.883)	0	0
			(0.0049，0.5010，-0.0087，0.8654)	60	60
(0.0190，0.8360，-0.1951，0.5124)	120	120
			(0.0348，0.9713，-0.2353，-0.0075)	180	180

步骤2，保存地图数据和轨迹点坐标信息

采用数据流的方式将点云数据(世界坐标系中的三维坐标、ORB特征描述子)、关键帧数据(相机位姿信息)保存为.bin格式文件，将点坐标信息包括x和z方向的数据保存到txt文件中。

步骤3，加载地图和轨迹点坐标计算转角

步骤3.1加载已经建好的地图，执行重定位功能，以此计算当前相机位姿信息。

根据基于GPS的导航算法可衍生到基于相机轨迹点的导航算法中。据此提出基于视觉SLAM的导航决策算法，如图7所示。算法分为两部分：选取预描点算法、计算转角度数β算法。

步骤3.2轨迹点加密，选取预描点对预描点进行滤波预处理：按关键帧方法得到的轨迹点数据部分地方会比较稀疏，因此需要进行加密处理后再进行下一步操作。采用均值滤波的方法对稀疏点加密：

选取预描点时，设当前点为B点，按照搜索在阈值0.35内的前后所有点的方法，搜索距离B点所有距离在0.35范围内的所有点，保存到数组M[n]＝{A，B，C，D，...}，然后对数组进行从小到大排序sort(M,M+n)选取M[n]为预描点。对于不同的地图轨迹数据有不同的预描点选取阈值，需要根据实际地图数据进行参数选取。对于闭合路线非起始点和终点位置：对于200个轨迹点的闭合路线来说：起始点标号0，终点标号199，将搜索到的点排序后最大值为199时选取标号0点为下一目标点即为预描点，要求0点到199点间距离大于搜索阈值。

步骤3.3计算转角：计算转角度数β算法：提出了两种计算转角算法。设(dx,dy)、(gx,gy)分别为当前点和要到达点坐标：

按图7前角度θ在范围±180°的坐标系下，设当前点和目标点连线与x轴夹角为范围为0到90度；β为转角，底层车身控制从左到右为0到180度；γ为中间决策层判断角，不同目标点与当前点位置关系有不同的值。表2为对应图6的转角决策表，当前位置和目标点位置相对位置关系分为表中所示的四种不同情况。

表2转角决策表

当运动方向为E到F时，即当(dx>gx&&dy<gy)时：

若0＝<θ<＝90即L1方向时，则左转β＝|θ|+90-α，若-180＝<θ<-90即L4方向时，则右转β＝|θ|-90+α，若-90＝<θ<0即L2和L3方向时，则判断γ＝90-α-|θ|，若γ>0即L2方向则左转β＝γ，否则即L3方向则右转β＝-γ；

当运动方向为F′到E′时，即当(dx<gx&&dy>gy)时：

若0＝<θ<＝90即L1方向时，则右转β＝90-|θ|+α，若-180＝<θ<＝-90即L4方向时，则左转β＝180-|θ|+90-α，若90<θ<＝180即L2和L3方向时，则判断γ＝90-α-(180-|θ|)，若γ>0即L3方向时则左转β＝γ否则即L2方向则右转β＝-γ；

当运动方向P为到Q时，即当(dx<gx&&dy<gy)时：

若-90＝<θ<＝0即L1方向时，则右转β＝90+|θ|-α，若90＝<θ<＝180即L4方向时，则左转β＝|θ|-90+α，若0<θ<90即L2和L3方向时，则判断γ＝90-α-|θ|，若γ>0即L2方向时则右转β＝γ否则即L3方向则左转β＝-γ；

当运动方向为Q′到P′时，即当(dx>gx&&dy>gy)时：

若-90＝<θ<＝0即L1方向时，则左转β＝90-|θ|+α，若90＝<θ<＝180即L4方向时，则右转β＝180-|θ|+90-α，若-180＝<θ<-90即L2和L3方向时，则判断γ＝90+α-|θ|，若γ>0即L2方向时则左转β＝γ否则即L3方向则右转β＝-γ；

步骤4，根据步骤3获得的角度β和转角的方向，发送命令到车辆底层控制。

结论：图8即为原始相机轨迹信息和自主导航轨迹信息，其中建图轨迹为没有经过加密处理的数据。分析发现，导航线和地图线基本重合。具体分析图8的导航轨迹偏差数据如图9所示，最大偏差为0.029约为0.1m，偏差非常小。图10为经过加密处理的建图轨迹和对应的三圈导航轨迹，图11为轨迹偏差数据，轨迹偏差最大值为0.017，可视为路线基本重合。图12为在单目视觉建图过程中同时采集的GPS数据和在使用上述方法下的单目视觉导航过程中采集的GPS数据轨迹图，两条轨迹最大偏差处约为0.0013km，此处偏差较大是由于转弯处的拐角度数较大，自动导航时车辆转弯在拐角度数小的地方能够完全重合。

综上所述，本发明单纯利用单目摄像头，不需要任何其它外部传感器，在较小误差范围内实现了基于单目视觉SLAM的导航***，降低了研究和应用成本，具有潜在的应用价值。

Claims (3)

1.一种基于四元数的单目视觉***的导航方法，其特征在于，步骤包括：

步骤1、构建环境地图，包括以下步骤：

1)进行摄像头标定；

2)将摄像头固定到车辆正前方；

3)将输入图像从RGB颜色空间转换到灰度颜色空间；

4)对图像进行特征提取和匹配；

5)地图初始化；

6)闭环检测和重定位；

7)获取当前相机位姿，计算四元数转换到180度到180度范围的欧拉角ψ、θ、所述ψ、θ、分别为绕Z轴、Y轴、X轴的旋转角度，偏航角θ即为当前帧的角度；

步骤2、保存地图数据和轨迹点坐标信息；

步骤3、加载地图和轨迹点坐标根据偏航角θ计算转角β，

步骤4、根据步骤3获得的角度β和转角的方向，发送命令到车辆底层控制。

2.如权利要求1所述的基于四元数的单目视觉***的导航方法，其特征在于，ψ、θ和范围为-90度到90度，对应求解公式为：

当θ范围为-180度到+180度时，对应求解部分公式为：

3.如权利要求2所述的基于四元数的单目视觉***的导航方法，其特征在于，步骤3计算转角β的过程为：

设(dx,dy)、(gx,gy)分别为当前点和要到达点坐标，设当前点和目标点连线与x轴夹角为范围为0到90度；β为转角，底层车身控制从左到右为0到180度；γ为中间决策层判断角，不同目标点与当前点位置关系有不同的值。当前位置和目标点位置相对位置关系分为下列四种不同情况时：

当运动方向为E到F时，即当(dx>gx&&dy<gy)时：

若0＝<θ<＝90即L1方向时，则左转β＝|θ|+90-α，若-180＝<θ<-90即L4方向时，则右转β＝|θ|-90+α，若-90＝<θ<0即L2和L3方向时，则判断γ＝90-α-|θ|，若γ>0即L2方向则左转β＝γ，否则即L3方向则右转β＝-γ；

当运动方向为F′到E′时，即当(dx<gx&&dy>gy)时：

若0＝<θ<＝90即L1方向时，则右转β＝90-|θ|+α，若-180＝<θ<＝-90即L4方向时，则左转β＝180-|θ|+90-α，若90<θ<＝180即L2和L3方向时，则判断γ＝90-α-(180-|θ|)，若γ>0即L3方向时则左转β＝γ否则即L2方向则右转β＝-γ；

当运动方向P为到Q时，即当(dx<gx&&dy<gy)时：

若-90＝<θ<＝0即L1方向时，则右转β＝90+|θ|-α，若90＝<θ<＝180即L4方向时，则左转β＝|θ|-90+α，若0<θ<90即L2和L3方向时，则判断γ＝90-α-|θ|，若γ>0即L2方向时则右转β＝γ否则即L3方向则左转β＝-γ；

当运动方向为Q′到P′时，即当(dx>gx&&dy>gy)时：

若-90＝<θ<＝0即L1方向时，则左转β＝90-|θ|+α，若90＝<θ<＝180即L4方向时，则右转β＝180-|θ|+90-α，若-180＝<θ<-90即L2和L3方向时，则判断γ＝90+α-|θ|，若γ>0即L2方向时则左转β＝γ否则即L3方向则右转β＝-γ。