一种基于四元数的单目视觉系统的导航方法技术方案

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

【技术实现步骤摘要】

本专利技术属于低速无人巡逻车自主导航领域，涉及一种单纯利用单目摄像头室内室外定位和导航的方法。
技术介绍
我国正在研发的无人驾驶智能车正在如火如荼的进行当中，吸引了无数高校、科研机构研究人员及汽车厂商的广泛关注。低速巡逻智能车巡逻的工作环境是动物园，在动物园中植被繁盛，经测试部分巡逻路段接收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为相机的内参数矩阵，[XY1]T为平面棋盘上点的齐次坐标，[uv1]T位棋盘上点投影到图像平面上对应点的齐次坐标，[r1r2r3]和t分别是摄像机坐标系统下的旋转和平移向量，k[r1r2t]为单应性矩阵H。根据矩阵求解方法，当图像数目大于等于3时k可以求出唯一解。步骤1.2，将摄像头固定在车辆正前方，保证地图构建时和导航时位置和方向均保持不变。步骤1.3，将输入图像从RGB颜色空间转换到灰度颜色空间，转换公式：Gray＝R*0.299+G*0.587+B*0.114；步骤1.4，对图像进行特征提取和匹配特征提取和匹配均采用ORB(OrientedBrief)方法，首先采用FAST-12算法来检测特征点如图3所示：I(x)为圆周上任一点的灰度值，I(p)为圆心的灰度，εd为灰度值差的阈值，如果大于阈值则认本文档来自技高网...

【技术保护点】
一种基于四元数的单目视觉系统的导航方法，其特征在于，步骤包括：步骤1、构建环境地图，包括以下步骤：1)进行摄像头标定；2)将摄像头固定到车辆正前方；3)将输入图像从RGB颜色空间转换到灰度颜色空间；4)对图像进行特征提取和匹配；5)地图初始化；6)闭环检测和重定位；7)获取当前相机位姿，计算四元数转换到180度到180度范围的欧拉角ψ、θ、所述ψ、θ、分别为绕Z轴、Y轴、X轴的旋转角度，偏航角θ即为当前帧的角度；步骤2、保存地图数据和轨迹点坐标信息；步骤3、加载地图和轨迹点坐标根据偏航角θ计算转角β，步骤4、根据步骤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方向时，则右转β＝|θ|...

【专利技术属性】
技术研发人员：鲍泓，仝小露，娄海涛，刘元盛，员娇娇，权潇，
申请(专利权)人：北京联合大学，
类型：发明
国别省市：北京;11