专利名称：适用于自主移动车辆的环境感知的单目视觉导航方法
技术领域：
本发明涉及一种适用于自主移动车辆的环境感知的单目视觉导航方法。
背景技术：
自主移动车辆的一个关键技术是对周围环境的感知。主要包括周围未知环境的三维坐标恢复以及障碍的识别，从而保证导航控制系统正常工作，并使车辆能安全行走和到达指定目标点完成科学探测任务。为了增强自主车辆的环境感知能力，提高其自主规划能力，从而实现自主导航，国内多家单位都对多目立体视觉技术进行了广泛研究，但是由于计算机视觉理论在自主移动车辆应用方面的研究还不成熟，算法的精度和匹配的难题未得到根本解决，在工程应用上产生瓶颈。
较之双目立体视觉，利用单个相机根据图像表象特征进行导航的方法也研究颇多，取得了一些实际场景下的应用。但是由于单目三维测量从理论上来讲是不完备的，缺少一个输入条件，所以所见到的单目用于车体导航的算法多是在半结构化或已知场景中的应用，还未见到关于单目测量在未知环境的自主漫游车上导航的相关报告。
中国专利公开号CN1569558，
公开日2005年1月26日，发明创造名称为基于图像表现特征的移动机器人视觉导航方法，该申请案公开了一种通过单幅摄像头获得的机器人所处位置的场景图像来完成导航的方法，该方法必须提前获得已知的构建好的场景地图。中国专利公开号CN1873656，
公开日2006年12月6日，发明创造名称为机器人视觉导航中的自然目标检测方法，该申请案公开了一种对图像中的自然目标完成提取来进行导航的方法，该方法需要在离线状态下车行走之前，建立自然目标的模型才可以进行。
发明的内容本发明的技术解决问题是克服现有技术的不足，提供一种适用于自主移动车辆的环境感知的单目视觉导航方法，该方法将单目测量方法应用到未知环境中，通过实时修正跟踪路径，保证达到目标点的精度，并通过自主避障行走方式，保证车体的安全行驶。
本发明的技术解决方案是适用于自主移动车辆的环境感知的单目视觉导航方法，其特征在于包括下列步骤(1)测量相机畸变的内参数，测量后将相机安装到自主移动车辆上，确定像平面坐标系到世界坐标系的转换关系；(2)记录当前车体姿态，在当前车体可视范围内采集图像，并将采集的图像回传至图形操作界面，根据目标位置及图像情况选择车体行走方式，然后对采集得到的图像进行畸变校正，使校正后的图像满足小孔成像原理，根据畸变校正后的图像选择行走路径以及路径上的路径点，将车体当前位置和选择的路径点根据步骤(1)所述的像平面坐标系到世界坐标系的转换关系换算到世界坐标系下，将换算结果发给车体，并将选择的路径以车体宽度的形式显示在图形操作界面上；(3)车体沿着拟定路径行走，当步骤(2)中车体选择路径点跟踪行走方式时，实时修正路径点在世界坐标系下的坐标值，直至到达当前可视范围内最后一个路径点；当步骤(2)中车体选择自主避障行走方式时，将相机俯仰到指定角度对当前场景拍照，对场景中障碍物进行区域分割，识别障碍区域，估算障碍高度，规划绕开障碍的路径，车体按照规划的路径行走，直至到达当前可视范围内最后一个路径点；(4)当车体到达当前可视范围内最后一个路径点时，从步骤(2)开始循环执行，直至车体到达最终目标位置。
所述步骤(3)中车体行走过程中还定期采集图像，将图像回传至图形操作界面后对图像进行畸变校正，并将根据当前车体位置、姿态将车体修正或规划的路径点显示在图形操作界面上，当车体没有避开障碍物时，对车体发送紧急停车指令。
本发明与现有技术相比有益效果为(1)本发明采用单目视觉导航方法与双目立体视觉相比，简化计算，满足实时性要求，稳定性好。
(2)本发明将单目测量方法应用到未知环境中与现有的已知场景地图下的单目导航方法比较，适用于非结构化的未知场景，有效提高跟踪的精度。
(3)本发明由于采用基于量化图形的遥操作方法，在遥端显示规划的轨迹和标准的场景网格信息于图像上，方便操作，更好的保证了安全性。
图1为本发明方法流程图；图2为本发明图像畸变校正的示意图；图3为本发明单目投影坐标转换的示意图；图4为本发明单目投影坐标实时修正的示意图；图5为本发明自主进行障碍高度识别的原理图；图6为本发明自主避障的效果图；图7为本发明量化图形显示的效果图。
具体实施例方式
如图1所示，为本发明方法流程图，具体步骤如下(1)测量相机畸变的内参数，测量后将相机安装到自主移动车辆上，确定像平面坐标系到世界坐标系的转换关系；(2)记录当前车体姿态，在当前车体可视范围内采集图像，并将采集的图像回传至图形操作界面，根据目标位置及图像情况选择车体行走方式，然后对采集得到的图像进行畸变校正，使校正后的图像满足小孔成像原理，根据畸变校正后的图像选择行走路径以及路径上的路径点，将车体当前位置和选择的路径点根据步骤(1)所述的像平面坐标系到世界坐标系的转换关系换算到世界坐标系下，将换算结果发给车体，并将选择的路径以车体宽度的形式显示在图形操作界面上；如图2所示，o-Xlc-Ylc-Zlc代表相机坐标系，u-v-f代表像平面坐标系，P为实际点，P′为P在像平面内的对应点，所拍摄的图像满足小孔成像的原理，可以推导出如下满足像素坐标与相机光心坐标的公式。
udxf=ylcxlcvdyf=zlcxlc]]>设xlc＝t，得到成像公式的参数方程xlcylczlc=1u·dxfv·dyf*t]]>(式1)其中u、v分别为像素的横坐标和纵坐标，dx为CCD靶面u方向上的一个像素的实际长度，dy为CCD靶面v方向上的一个像素的实际长度。
这是单目投影的基本原理，在小孔模型下，将图像坐标系投影到需要计算的平面上，得到像素点在该平面下的三维坐标。但在实际的应用中，由于镜头的光学畸变等因素，成像很难满足小孔成像。所以在进行上述公式的转换之前，必须对图像进行主要的一些畸变参数校正，使其尽量满足小孔成像原理，再套用上述公式进行计算。本发明主要考虑镜头的三种畸变径向畸变、偏心畸变和像平面畸变。公式如下
x＝(x-x0) y＝(y-y0)r2＝x2+y2Δxr=K1x‾r2+K2x‾r4+K3x‾r6+···Δyr=K1y‾r2+K2y‾r4+K3y‾r6+···]]>Δxd=p1(r2+2x‾2)+2p2x‾×y‾Δyd=p2(r2+2y‾2)+2p1x‾×y‾]]>Δxm=b1x‾+b2y‾Δym=0]]>x′=x+Δxr+Δxd+Δxmy′=y+Δyr+Δyd+Δym]]>其中ΔxrΔyr为径向畸变，ΔxdΔyd为偏心畸变，ΔxmΔym为像平面畸变。
在实验室的精密已知环境下经过光学方法可以准确地标定出以上的变形系数，即采用专用的摄影测量的光学标定系统，在实际应用时对拍摄得到的图像经过以上的变形校正处理，得到新的图像则认为满足小孔成像的原理。
通过式1可建立像平面坐标系和相机坐标系之间的关系，为了导航控制的实际应用，需要将相机坐标系下的坐标转换到车体坐标系下或者地面的某个导航坐标系下，在此称为世界坐标系，故此应明确描述探测车位姿的各个坐标系及其相互转换关系。
如图3所示，为描述探测器的位姿及坐标转换关系定义如下坐标系其中OlcXlcYlcZlc为相机坐标系，原点位于相机光心，Xlc沿相机光轴向前，Zlc垂直于Xlc向上，Ylc与Xlc、Zlc成右手系；OcXcYcZc相机支架坐标系，原点位于相机支架中心，Yc沿支架向左，Xc垂直Yc且位于XlcYlc所在平面内，Zc与Xc、Yc成右手系；OlXlYlZl为桅杆坐标系，原点位于桅杆与车体的连接处，Xl为车体纵对称轴向前，Yl垂直Xl向左，Zl与Xl、Yl成右手系；ObXbYbZb为车体坐标系，原点位于车体几何中心，Xb为车体前向，Yb指向车体左侧；
OrXrYrZr为世界坐标系，原点为车体质心垂直向下与当地水平坐标系的交点，坐标系方向同车体坐标系。
同时参数定义如下α相机俯仰角；β相机偏航角；SABC为A系原点到B系原点在C方向的平移量； 相机俯仰角和偏航角都为零的情况下，相机坐标系到车体坐标系的旋转矩阵。
步骤一，完成相机坐标系向相机支架坐标系的转换关系xcyczc=(r11r12r13r14r15r16r17r18r19*xlcylczlc+SlxSlySlz)]]>[SlxSlySlz]相机坐标原点到相机支架坐标系的平移量。在相机俯仰角和偏航角都为零的情况下，相机支架坐标系和车体坐标系是完全平行的。
步骤二，完成相机支架坐标系到桅杆坐标系的转换xlylzl=cosαcosβ-sinβsinαcosβcosαsinβcosβsinαsinβ-sinα0cosα*xcyczc+SclxSclySclz]]>步骤三，完成桅杆坐标系到车体坐标系的转换xbybzb=xlylzl+SlbxSlbySlbz]]>步骤四，完成车体坐标系到世界坐标系的转换按照此时世界坐标系的定义，车体坐标系到世界坐标系的转换就是一个简单的平移，如下所示
xryrzr=xbybzb+00H]]>步骤五，综合以上四步，逐步迭代换算得到世界坐标系和相机坐标系的对应关系，得到下发路径点时的总体公式xryrzr=cosαcosβ-sinβsinαcosβcosαsinβcosβsinαsinβ-sinα0cosα*(r11r12r13r14r15r16r17r18r19*xlcylczlc+SlxSlySlz)+Slbx+SclxSlby+SclyH+Slbz+Sclz]]>(式2)步骤六，联立式(1)和式(2)就可以解算得到像平面坐标系到世界坐标系的转换关系式。由于二维的图像平面坐标解算三维空间的坐标缺少一个量，提出zr＝0的假设，才可以将三维确切的坐标值解算出来。但是这样假设会与实际的情况不相符合会产生误差，误差在进一步的运动中进行补偿。
(3)如图4所示，为车体选择路径点跟踪方式行走路径点实时修正示意图，由于真实的地形与假设的平面并不相符，在初始的一幅图像中指定探测器运动的一系列轨迹点在其后的运动中都会发生变化，为了保证轨迹跟踪的精度，根据车体当前的姿态变化实时地修正这些轨迹点在世界坐标系下的坐标，同时根据定位信息获得行走过的距离，当修正的轨迹点出现在车体的后方，表明车体已经走过该点，改为跟踪下一个修正的目标点。修正的频率越高，轨迹点跟踪的精度越高。
具体的实施步骤图4中给出五个示意点，五个示意点为步骤(2)中选择的路径点，根据此刻zr＝0的世界坐标平面下发初始路径点坐标。在车体行进的过程中，由于车体姿态和位置的变化，一成不变地使用初始位置的世界坐标平面会造成误差越来越大。如图所示，五个初始点在相机坐标系下的五条射线方程是确定的，其与假设平面的交点即为该像素点对应的三维坐标点。当假设平面为当前车体位置处的地形切线平面时，如果切线足够短，三维点的坐标是可以逐渐逼近真实值的。所以根据车体相对于初始姿态和位置的改变，修正与车体实时的位置姿态相关联的zr＝0的平面方程，利用实时测得的车体位置和姿态信息进行修正，最终使路径点在世界坐标系下的坐标值更接近真实的地表情况。当交点坐标位于当前车体位置的后方，则认为该路径点已走过，改为跟踪下一个路径点，直至当前可视范围内的最后一个路径点。
路径点的坐标修正公式如下xr′yr′zr′=cosΔpsinΔrsinΔpcosΔrsinΔp0cosΔr-sinΔr-sinΔpsinΔrcosΔpcosΔrcosΔp*(cosαcosβ-sinβsinαcosβcosαsinβcosβsinαsinβ-sinα0cosα*(r11r12r13r14r15r16r17r18r19*xlcylczlc+SlxSlySlz)+Slbx+SclxSlby+SclyH+Slbz+Sclz-Sx′Sy′Sz′)]]>(式3)其中，Δr当前车体相对初始位置的滚转角；Δp当前车体相对初始位置的俯仰角；Sx′，Sy′，Sz′为车体的当前位置在路径点下发初始位置世界坐标系下的坐标值，由导航定位信息提供。
经过行进过程中车体位置和姿态的实时修正，初始的目标点位置更接近真实值，提高了下一个路径点的跟踪精度。
当对当前路况判断车体选择自主避障行进方式时，具体步骤如下步骤一，将相机俯仰指定角度对当前场景进行拍照。
步骤二，根据场景中的障碍物灰度的特性，采用最小误差分割方法进行障碍物的区域分割。
最小误差分割方法的原理是基于图像的灰度信息，设图像的灰度函数为f(i)，i为灰度值，在8bits的灰度图像中为0～255。计算两类灰度分布的样本总数、分布均值和方差
p0(t)=Σi=mintf(i)]]>p1(t)=Σi=tmaxf(i)]]>μ0(t)=Σi=mintf(i)*ip0(t)]]>μ1(t)=Σi=tmaxf(i)*ip1(t)]]>σ0(t)=Σi=mint[i-μ0(t)]2f(i)p0(t)]]>σ1(t)=Σi=tmax[i-μ1(t)]2f(i)p1(t)]]>min、max为一幅图像中出现的灰度最小值和灰度最大值，t为进行分割的灰度阈值。然后定义最小误差函数为H(t)＝1+2*[p0(t)lnσ0(t)+p1(t)lnσ1(t)-2*[p0(t)ln p0(t)+p1(t)ln p1(t)]]循环计算所有t的H(t)值，从中找出最小的H(t)值所对应的t值，然后利用t值对图像进行二值化处理。二值化后的图像首先去除提取的孤立点，即区域面积小于一特定阈值的点集，再经过基本的形态学运算腐蚀和膨胀算法消除空洞，程序中所使用的结构元素为B=111111111.]]>步骤三，对得到的连贯的区域，即去除孤立点和空洞后的区域进行标识和面积判断，大于阈值面积的区域作为有效的障碍物，阈值面积的区域可以根据内场中的石块大小和图像特性进行设定，取为2000个像素点。大致估算出障碍物的高度信息，高于最大越障高度的石块作为危险区域，需要进行避障。
如图5所示，障碍物高度计算的基本原理图，假设障碍物的前端面具有最大高度且与水平地面大致垂直，修改式3中的Δr和Δp，改为车体当前姿态相对大地水平面的滚动和俯仰角(根据地形虽起伏不平，但大致平坦的特性)，假设大地水平面的方程，将世界坐标系补偿相对水平面的俯仰和滚动角后得到，根据障碍物的最下沿和最上沿计算其在水平面上截取的长度为Obstacle，最下沿和最上沿对应的像素坐标之间的张角为δ，根据相机当前拍摄时的姿态可以得到最下沿和最上沿对应的视线与水平面的夹角αlow和αhigh，则障碍物的高度计算公式为
障碍的高度＝Obstacle×tanαhigh本假设对水平地面上的垂直高度物进行了试验验证，证明该方法原理成立并可以达到较高精度。在实际应用中，由于地形的起伏和障碍物的不规则形状会带来测量的不准确性，但是通过对高度裕量的考虑，可以实现大障碍的安全规避。
步骤四，对需要避过的大障碍提取其在像平面坐标系下的四个坐标极值，由这四个坐标极值生成包络矩形框，修改式3中的Δr和Δp，改为车体当前姿态相对大地水平面的滚动和俯仰角，其他转换相同，这四个点形成了一个在水平面上的危险区域范围，根据危险区域范围和相机可视的范围进行路径规划，规划出一条既可避过危险区域，又可达到目标点的安全路径，路径规划方法可以为目前应用较多的人工势场法、模糊逻辑算法、神经网络法、遗传算法和栅格法等，规划路径后车体沿规划路径行走，直至当前可视范围内最后一个路径点。
(3)车体行走过程中还定期采集图像，将图像回传至图形操作界面后对图像进行畸变校正，并将根据当前车体位置、姿态将车体修正或规划的路径点显示在图形操作界面上，当车体没有避开障碍物时，对车体发送紧急停车指令。
如图6所示，为本发明障碍识别的效果图，其中白色框为图像处理后识别的障碍包络矩形，左下角的1表示障碍物的标识为图像中第一块危险障碍。其余白色的网格线为车体当前位置水平面的横向0.5m和纵向1m的等距线。需要说明的是，这里是将图像回传至遥端，即图形操作界面进行显示，实际车载端的处理是不能显示的，但所得的数据与这里相同。
在遥端的操作过程中，为了更直观地保证车体的安全性，提出了基于量化图形的遥操作构想，并做了技术实现，在遥端的图形操作界面上显示根据车体当前位置姿态信息计算的下发轨迹和标准的场景网格信息，并将车体下一步可运动达到的范围显示出来。
方法实现主要是将坐标的转换关系求反解，得到世界坐标系下的点在像平面坐标系下的位置并通过图形界面编程显示在图像上，其理论公式如下
xlcylczlc=r11r12r13r14r15r16r17r18r19T*]]>(cosαcosβcosαsinβ-sinα-sinβcosβ0sinαcosβsinαsinβcosα*((xryrzr-00H)-SlbxSlbySlbz-SclxSclySclz)-SlxSlySlz)]]>图像回传后，世界坐标系下等宽间距的网格，遥操作下发的路径和下一步车体连续运动的可达路径点范围都可以按照投影关系的反解公式计算出其像平面坐标显示在图像上，使车即将行走的轨迹结果一目了然，方便操作员的直观判断，具有重要的实用价值。
如图6所示，为内场实验的效果示意图，图中三条曲线的中间一条表示规划的路径，两边的两条表示车体的宽度范围，从图中可以很直观地判断下发路径的正确性和行走的安全性，同时还可保证下一个路径点位于车体可连续运动到达的范围，然后再进行路径下发。
(4)当车体到达当前可视范围内最后一个路径点时，从步骤(2)开始循环执行，直至车体到达最终目标位置。
权利要求
1.适用于自主移动车辆的环境感知的单目视觉导航方法，其特征在于包括下列步骤(1)测量相机畸变的内参数，测量后将相机安装到自主移动车辆上，确定像平面坐标系到世界坐标系的转换关系；(2)记录当前车体姿态，在当前车体可视范围内采集图像，并将采集的图像回传至图形操作界面，根据目标位置及图像情况选择车体行走方式，然后对采集得到的图像进行畸变校正，使校正后的图像满足小孔成像原理，根据畸变校正后的图像选择行走路径以及路径上的路径点，将车体当前位置和选择的路径点根据步骤(1)所述的像平面坐标系到世界坐标系的转换关系换算到世界坐标系下，将换算结果发给车体，并将选择的路径以车体宽度的形式显示在图形操作界面上；(3)车体沿着选择路径行走，当步骤(2)中车体选择路径点跟踪行走方式时，实时修正路径点在世界坐标系下的坐标值，直至到达当前可视范围内最后一个路径点；当步骤(2)中车体选择自主避障行走方式时，将相机俯仰到指定角度对当前场景拍照，对场景中障碍物进行区域分割，识别障碍区域，估算障碍高度，规划绕开障碍的路径，车体按照规划的路径行走，直至到达当前可视范围内最后一个路径点；(4)当车体到达当前可视范围内最后一个路径点时，从步骤(2)开始循环执行，直至车体到达最终目标位置。
2.根据权力要求1所述的适用于自主移动车辆的环境感知的单目视觉导航方法，其特征在于所述步骤(3)中车体行走过程中还定期采集图像，将图像回传至图形操作界面后对图像进行畸变校正，并将根据当前车体位置、姿态将车体修正或规划的路径点显示在图形操作界面上，当车体没有避开障碍物时，对车体发送紧急停车指令。
3.根据权力要求1或2所述的适用于自主移动车辆的环境感知的单目视觉导航方法，其特征在于所述步骤(1)中的像平面坐标系到世界坐标系的转换关系通过相机坐标系到相机支架坐标系转换、相机支架坐标系到桅杆坐标系转换、桅杆坐标系到车体坐标系转换、车体坐标系到世界坐标系转换四步后，将四步综合结合成像公式参数方程得到像平面坐标系到世界坐标系的转换关系。
4.根据权力要求1或2所述的适用于自主移动车辆的环境感知的单目视觉导航方法，其特征在于所述步骤(2)中的将行走的路径以车体宽度的形式显示在图像上采用基于量化图形的遥操作方法，将像平面坐标系到世界坐标系的转换关系求反解，得到世界坐标系下的点在像平面坐标系下的位置。
5.根据权力要求1或2所述的适用于自主移动车辆的环境感知的单目视觉导航方法，其特征在于所述步骤(3)中实时修正路径点在世界坐标系下的坐标值，根据实时测得的车体位置、姿态变化实时修正，修正公式为xr′yr′zr′=cosΔpsinΔrsinΔpcosΔrsinΔp0cosΔr-sinΔr-sinΔpsinΔrcosΔpcosΔrcosΔp*(cosαcosβ-sinβsinαcosβcosαsinβcosβsinαsinβ-sinα0cosα(rl1rl2rl3rl4rl5rl6rl7rl8rl9*xlcylczlc+SlxSlySlz)+Slbx+SclxSlby+SclyH+Slbz+Sclz-Sx′Sy′Sz′)]]>其中，α相机俯仰角；β相机偏航角；[StxStyStz]相机坐标原点到相机支架坐标系的平移量；下标lc、l、b、c分别代表相机坐标系、桅杆坐标系、车体坐标系、相机支架坐标系；SABC为A系原点到B系原点的在C方向的平移量；rl1rl2rl3rl4rl5rl6rl7rl8rl9:]]>相机俯仰角和偏航角都为零的情况下，相机坐标系到车体坐标系的旋转矩阵；Δr当前车体相对初始位置的滚转角；Δp当前车体相对初始位置的俯仰角；Sx′，Sy′，Sz′为车体的当前位置在路径点下发初始位置世界坐标系下的坐标值；H车体坐标系到世界坐标系的平移量。
6.根据权力要求1或2所述的适用于自主移动车辆的环境感知的单目视觉导航方法，其特征在于所述步骤(3)中的对障碍物进行区域分割采用最小误差分割方法，首先计算灰度分布的样本总数、分布均值、方差，然后根据计算的样本总数、分布均值、方差计算灰度阈值的最小误差函数，找出所有灰度阈值的最小误差函数中最低值对应的阈值，最后利用得到的阈值对图像进行二值化处理，并采用图像的腐蚀膨胀运算去除提取的孤立点、消除空洞。
7.根据权力要求1或2所述的适用于自主移动车辆的环境感知的单目视觉导航方法，其特征在于所述步骤(3)中的估算障碍高度，假设障碍物的前端面具有最大高度且与地面大致垂直，然后根据假设后得到的三角关系计算障碍高度。
全文摘要
适用于自主移动车辆的环境感知的单目视觉导航方法，首先测量相机畸变内参数，将相机安装到自主移动车辆上，确定像平面坐标系到世界坐标系的转换关系；然后记录当前车体姿态，在当前车体可视范围内采集图像，将图像回传至图像操作界面，确定车体的行走方式对采集图像进行畸变校正，选择路径以及路径上的路径点，将车体当前位置和选择的路径点根据坐标转换关系换算到世界坐标系下，将结果发给车体，并将拟定路径以车体宽度的形式显示在图形操作界面上；车体根据自主避障或路径点跟踪方式进行行走，直至到达当前可视范围内最后一个路径点，后从采集图像开始循环，直至到达最终的目标点。本发明将单目测量方法应用到未知环境中的视觉导航。
文档编号G01C21/26GK101067557SQ20071012290
公开日2007年11月7日 申请日期2007年7月3日 优先权日2007年7月3日
发明者毛晓艳, 张晋, 陈建新 申请人:北京控制工程研究所