一种巡视器地面导航规划控制方法
【专利摘要】一种巡视器地面导航规划控制方法，步骤包括：序列图像拼接、三维地形恢复、地形可通过性分析、路径规划和移动策略生成。本发明基于双目立体视觉系统对巡视器进行地面导航规划控制，能够根据巡视器探测目标的位置结合实际情况选择包含探测目标的若干对图像进行处理，减少了不必要的计算，规划效率较高；提出自动降噪和人工去噪的方式对三维地形数据进行处理，提高了环境感知的准确性，有助于对巡视器的高精度导航规划控制；提出利用数据融合的方式对多组地形数据进行拼接，增大了环境感知的范围，提高了导航规划的安全性；对于配备其他敏感器进行环境感知的巡视器，本发明提出的地形可通过性分析、路径规划和移动策略生成等导航规划控制方案仍然适用。
【专利说明】一种巡视器地面导航规划控制方法
【技术领域】
[0001]本发明涉及一种地面导航规划控制方案，适用于外星球表面或复杂未知环境中巡视器的地面导航规划控制。
【背景技术】
[0002]对于工作在行星表面或者复杂未知环境中的巡视器来说，地面导航规划控制就是在地面根据巡视器的状态，利用巡视器采集到的环境信息，对探测区域进行场景重建，由地面操作人员对巡视器的运动做出相应的规划、提出控制策略，并对控制策略进行安全性验证。
[0003]迄今为止，前苏联、美国等国家陆续向外星球发射多台巡视探测器并成功实现软着陆。
[0004]上世纪70年代前苏联先后发射了两台月球探测器，其控制模式为地面遥操作模式:巡视器上装有四部用于采集月面图像的电视摄像机，地面控制工作组以电视摄像机采集的小幅图像作为参考，完成简单的障碍识别、估计到障碍的距离、分析道路可穿越性以及巡视器运动控制等工作。由于这种控制方式完全依靠操作员对月面地形环境进行简单估计，无法实现对巡视器进行高精度导航控制。
[0005]1997年7月美国发射的探测器Sojourner成功在火星着陆。Sojourner根据地面指令执行任务，以“启动-停止”方式工作，在完成当前指令后，等待获取下一指令。地面操作员利用巡视器拍摄的图像，指定巡视器下一步运动区域内的目标位置和行驶路径。但由于火星与地球之间存在较大通讯时延，一天仅进行两次通信，因此操作员不能实时地控制巡视器。
[0006]2004年美国发射的两台火星探测器Spirit和Opportunity成功着陆。对火星车的地面导航控制模式为直接驾驶模式(directed driving mode),允许操作员指定火星车沿着由路径规划算法规划出的轨迹行驶。火星车的遥操作平台具备全景图拼接、单点三维坐标恢复、环境建模、运动仿真等功能。但同样由于受到火星与地球之间通讯时延的影响，无法实现对火星车的实时控制。
[0007]综上，现有对巡视探测器的地面导航规划控制技术存在控制精度低，实时性差，规划效率低等问题。
【发明内容】
[0008]本发明要解决的技术问题是:克服现有技术的不足，提供一种巡视器地面导航规划控制方法，该方法规划效率高，能够实现高精度、安全有效的导航规划控制，具有广泛的适用性。
[0009]如图1所示，本发明的技术解决方案具体实现步骤如下:
[0010](I)序列图像拼接
[0011]利用巡视器上配备的一对左右相机(也称为导航相机)，对巡视器周围的环境以一定的角度间隔进行环拍，得到一个导航相机图像对序列，通过在序列图像中寻找相邻两幅图像间的对应关系，将序列图像拼接成一幅宽视场的二维全景图像；
[0012](2)三维地形恢复
[0013](21)根据给定的导航规划控制的目标位置(? yG)，计算目标方向与巡视器前进方向的夹角V=atan2 (yG, xG),结合导航相机成像的角度,从步骤(1)得到的序列图像中选取包含目标的导航相机图像对；
[0014](22)利用经过相机标定得到的相机内外参数，对选取出的导航相机图像对进行图像校正，完成相机图像的极线对正，对校正后的相机图像利用图像匹配算法完成立体匹配，确定左右相机图像中各像素点之间的对应关系，从而得到视差数据；
[0015](23)利用视差数据根据双目立体视觉成像原理计算图像中各像素点在相机坐标系下的三维坐标，结合相机成像时相机与巡视器的相对关系将相机坐标系下的三维坐标转换到水平坐标系下，得到三维点云地形数据；利用滤波算法对三维点云地形数据进行自动降噪处理，再通过人工去噪的方式进行二次去噪，得到高精度三维地形数据；
[0016](24)对三维地形数据所覆盖的范围进行分析，如果单对导航相机图像恢复出的地形不能完全覆盖目标所在区域，则根据步骤(1)得到的二维全景图像对目标周围的环境进行判断，从序列图像中选择多对导航相机图像分别完成三维地形恢复，采用数据融合方法将恢复出的多个三维点云地形数据进行拼接，得到融合后的大场景三维地形；
[0017](3)地形可通过性分析
[0018](31)对步骤(2)得到的三维点云地形数据做栅格化处理，将其分割为若干尺寸为
I。的栅格，对地形数据中信息缺失的区域`进行数据插值，得到数字高程图，记为E= Izi j，其中(i，j)表示栅格所在的行数和列数，Ziij是栅格(i，j)对应的高程值；
[0019](32)对数字高程图的每个栅格(i，j)，定义一个正方形的地形块:P= Iziu Ik=^.+L；I=J-L,...,J+J，P的边长为2L+1 (以栅格的数目为单位),其中心为栅格(i, j)，L是一个正整数，它的选取应该使地形块P能够包含巡视器的外包络轮廓；利用地形块P内的高程值进行平面拟合，计算拟合平面的坡度角P、粗糙度σ和高程差ε等地形信息，基于巡视器的爬坡坡度、最大粗糙度和越障高度等性能指标，对地形信息归一化并进行加权，将加权结果作为栅格(i，j)的适宜度，记为Giij ;
[0020](33)对数字高程图中的所有栅格计算适宜度，通过设定适宜度阈值将适宜度小于设定阈值的栅格标记为“障碍单元”，对“障碍单元”栅格按照连通性进行聚类，将处于同一连通区域的“障碍单元”栅格标记为一个障碍物，从而完成障碍物提取，得到包含若干障碍物区域的适宜度地图；
[0021](4)路径规划
[0022](41)在步骤(3)得到的适宜度地图上设置起始点和目标点，考虑里程最短、时间最短、安全性最高等因素作为优化条件，设计代价函数，可通过设置代价函数中距离、时间和安全性的比例因子对各因素在路径搜索中的比重进行调整；
[0023](42)利用代价函数计算适宜度地图上起始点和目标点间栅格的路径代价，采用如D'FieldD*等基于栅格的路径搜索算法，在适宜度地图上搜索路径，结合巡视器的最小转弯半径对搜索得到的路径进行平滑处理，得到一个路径点序列；
[0024](43)结合步骤(2)得到的三维点云地形数据，地面操作人员根据需要对规划得到的路径点序列进行调整，如:修改某些路径点的位置，删除某些路径点，或者增加一些关键路径点等等，从而完成对路径规划结果的修正和优化；
[0025](5)移动策略生成
[0026](51)若选择采用整器指令的移动策略，则根据巡视器的运动性能和航向约束，通过曲线拟合的方式将步骤(4)得到的路径点序列拟合成平滑的曲线，按曲率对拟合的曲线进行逐段分解，得到以曲率、长度等信息表示的多段前后相连的弧线，从而生成由运动速度、运动曲率及运动里程组成的整器指令；
[0027](52)若选择采用直接指令的移动策略，则基于巡视器的运动学关系，建立巡视器的驱动运动学方程，并对其求解，将一条整器指令分解为包含驱动轮期望转速和转向轮期望转角的直接指令；
[0028](53)若选择采用路径点跟踪指令的移动策略，则从路径点序列中选取若干路径点的坐标作为路径点跟踪指令，令巡视器自主地控制运动速度、运动曲率和运动里程，动态地跟踪指定的路径点；
[0029](54)对于生成的移动策略，基于步骤(2)得到的三维地形数据，利用巡视器运动学模型或动力学模型对巡视器的移动进行仿真，通过计算巡视器按照生成的运动控制指令移动时的位置姿态信息、摇臂、轮系及各运动部件的关节转角和角速度信息，对移动策略进行验证，确保移动策略安全有效。
[0030]所述步骤(5)中的巡视器运动控制指令主要包括:整器指令、直接指令和路径点跟踪指令，其中，整器指令即巡视器整器的运动速度、运动曲率及运动里程等指令，表征整器的基本运动趋势，属于任务级指令，此类指令可生成单条指令或指令组；直接指令即轮系的驱动轮期望转速和转向轮期望转角指令，属于指令级指令，此类指令可以一次生成单条指令；路径点跟踪指令即巡视器所需跟踪的路径轨迹，通常可以用一组路径点的坐标描述，也属于任务级指令，此类指令可由路径规划直接得到。
[0031]本发明与现有技术相比的优点在于:
[0032]( I)本发明基于立体视觉系统对巡视器进行地面导航规划控制，能够根据巡视器探测目标的位置结合实际情况选择包含探测目标的若干对图像进行处理，减少了不必要的计算，规划效率较高；
[0033](2)本发明提出自动降噪和人工去噪的方式对三维地形数据进行处理，提高了环境感知的准确性，有助于对巡视器的高精度导航规划控制；
[0034](3)本发明提出利用数据融合的方式对多组地形数据进行拼接，增大了环境感知的范围，提高了导航规划的安全性；
[0035](4)对于配备其他敏感器进行环境感知的巡视器，本发明提出的地形可通过性分析、路径规划和移动策略生成等导航规划控制方案仍然适用。
【专利附图】
【附图说明】
[0036]图1为本发明的巡视器地面导航规划控制流程图；
[0037]图2为本发明的二维全景图像示例；
[0038]图3为本发明的数据融合后的三维地形数据示例；
[0039]图4为本发明的移动策略对应的运动轨迹示例。【具体实施方式】
[0040]以月面巡视探测器为例，对巡视器进行地面导航规划控制的具体实现如下:
[0041](1)序列图像拼接
[0042]令巡视器的导航相机从-40°到40°以20°的角度间隔，从左侧向右侧进行序列成像，得到5对相机图像。在序列图像中寻找相邻两幅图像间的对应关系，将序列图像拼接成一幅宽视场的二维全景图像，如图2所示；
[0043](2)三维恢复
[0044]给定导航规划控制的目标位置(6.7，-0.3)，根据目标所处的位置，经计算可知目标方向与巡视器前进方向的夹角约为V=atan2(-0.3，6.7)=-2.564°，而序列图像中的第三对图像是在0°时拍摄的，故可知目标处在序列图像的第3对图像之中；
[0045]利用经过相机标定得到的相机内外参数，对第3对图像进行极线校正和立体匹配，得到视差数据；
[0046]利用视差数据根据双目立体视觉成像原理计算图像中各像素点在相机坐标系下的三维坐标，结合第3对图像拍摄时相机与巡视器的相对关系将相机坐标系下的三维坐标转换到水平坐标系下，得到三维点云地形数据；利用滤波算法对三维点云地形数据进行自动降噪处理，再通过人工去噪的方式进行二次去噪，得到高精度三维地形数据；
[0047]观察图2所示的二维全景图像可知，巡视器正前方存在一个石块和两个浅坑构成的障碍，需绕过障碍物才能安全到达目标；考虑巡视器的尺寸为1.5mX 1.0m，结合三维地形数据范围分析可知，仅靠单对相机图像恢复出的地形数据无法覆盖绕行的路线，故需要进行多个地形数据的融合；从二维全景图像中可知前方右侧区域较为平坦，故选择从右侧绕行障碍物，所以选择第4对相机图像进行三维恢复，并将这两个三维点云地形数据拼接融合，融合后的三维地形结果如图3所示；
[0048]( 3 )地形可通过性分析
[0049]对步骤(2)得到的三维点云地形数据做栅格化处理，将其分割为若干尺寸为Ic=0.3m的栅格，对地形数据中信息缺失的区域进行数据插值，得到数字高程图，记为E={Zi j，其中(i，j)表示栅格所在的行数和列数，Ziij是栅格(i，j)对应的高程值；
[0050]对数字高程图的每个栅格(i，j)，定义一个正方形的地形块=P=IziuIk=Um'
为了能够包含巡视器的外包络轮廓，取P的边长为7 (以栅格的数目为单位)；利用
地形块P内的高程值进行平面拟合，计算拟合平面的坡度角P、粗糙度σ和高程差ε等地形信息，基于巡视器的爬坡坡度A最大粗糙度Τ。和越障高度Te等性能指标，对地形信息
按下式归一化并进行加权，得到栅格(i，j)的适宜度，记为Giij:
[0051]Gi；J=a !Sg+a 2rg+a 3hg
[0052]式中，minGw/drd-minQ，。Τ。)，hg=l_( ε T ε)2 ;7；取为 20。，Τ。取为
0.05m, Τε取为0.2m，a ^ a 2和a 3为加权系数，分别取为0.3、0.2和0.5 ;
[0053]对数字高程图中的所有栅格计算适宜度，设定适宜度阈值为0.3，将适宜度小于
0.3的栅格标记为“障碍单元”，对“障碍单元”栅格按照连通性进行聚类，将处于同一连通区域的“障碍单元”栅格标记为一个障碍物，从而完成障碍物提取，得到包含若干障碍物区域的适宜度地图；[0054](4)路径规划
[0055]在步骤(3)得到的适宜度地图上，选定起始点坐标(0.0，0.0)和目标点坐标(6.7，-0.3)，以里程最短作为优化条件，采用FieldD*算法搜索路径，得到一个路径点序列，如表1所示；
[0056]表1路径点序列
[0057]
【权利要求】
1.一种巡视器地面导航规划控制方法，其特征在于实现步骤如下: (1)序列图像拼接 利用巡视器上配备的一对左右相机也称为导航相机，对巡视器周围的环境以一定的角度间隔进行环拍，得到一个导航相机图像对序列，通过在序列图像中寻找相邻两幅图像间的对应关系，将序列图像拼接成一幅宽视场的二维全景图像； (2)三维地形恢复 从步骤(1)的序列图像中选取包含目标的导航相机图像对，对选取出的图像对进行三维恢复，生成三维点云地形数据并进行去噪处理；如果单对导航相机图像恢复出的地形数据不能完全覆盖目标所在区域，则选择多对导航相机图像进行三维恢复，并将多个地形数据融合拼接，得到融合后的大场景三维地形； (3)地形可通过性分析 基于步骤(2)得到的三维地形数据，结合巡视器自身的几何结构参数，以及巡视器的越障能力、爬坡能力和安全姿态等性能指标，从高度、坡度和粗糙度等方面对三维地形数据进行地形可通过性分析，计算该地形的适宜度，识别地形中障碍的位置和大小，从而得到表征巡视器在该地形上行驶安全性的适宜度地图； (4)路径规划 基于步骤(3)得到的适宜度地图，结合步骤(1)中指定的目标位置，考虑时间、里程和安全性等因素进行路径规划，生成一条从巡视器当前位置到目标位置路径，以路径点序列表示；地面操作人员根据需要人为增加、更改或删除路径点序列中的路径点，从而得到修正后的路径规划结果； (5)移动策略生成 根据步骤(4)得到的路径规划结果，考虑巡视器的几何结构参数、轮系和机构的运动参数，结合巡视器具备的运动能力，生成整器指令、直接指令或路径点跟踪指令等形式的运动控制指令，并结合三维地形数据对运动控制指令的安全性进行运动学或动力学仿真验证，确保巡视器的移动策略安全有效。
2.根据权利要求1所述的巡视器地面导航规划控制方法，其特征在于:所述步骤(2)中完成三维地形恢复的具体过程实现如下: (21)根据给定的导航规划控制的目标位置(?yG)，计算目标方向与巡视器前进方向的夹角Ψ =atan2 (yG, xG),结合导航相机成像的角度，从步骤(1)得到的序列图像中选取包含目标的导航相机图像对； (22)利用经过相机标定得到的相机内外参数，对选取出的导航相机图像对进行图像校正，完成相机图像的极线对正，对校正后的相机图像利用图像匹配算法完成立体匹配，确定左右相机图像中各像素点之间的对应关系，从而得到视差数据； (23)利用视差数据根据双目立体视觉成像原理计算图像中各像素点在相机坐标系下的三维坐标，结合相机成像 时相机与巡视器的相对关系将相机坐标系下的三维坐标转换到水平坐标系下，得到三维点云地形数据；利用滤波算法对三维点云地形数据进行自动降噪处理，再通过人工去噪的方式进行二次去噪，得到高精度三维地形数据； (24)对三维地形数据所覆盖的范围进行分析，如果单对导航相机图像恢复出的地形不能完全覆盖目标所在区域，则根据步骤(1)得到的二维全景图像对目标周围的环境进行判断，从序列图像中选择多对导航相机图像分别完成三维地形恢复，采用数据融合方法将恢复出的多个三维点云地形数据进行拼接，得到融合后的大场景三维地形。
3.根据权利要求1所述的巡视器地面导航规划控制方法，其特征在于:所述步骤(3)中的地形可通过性分析具体实现过程如下: (31)对步骤(2)得到的三维点云地形数据做栅格化处理，将其分割为若干尺寸为I。的栅格，对地形数据中信息缺失的区域进行数据插值，得到数字高程图，记为E= {Zij，其中(i, j)表示栅格所在的行数和列数，Zi, j是栅格(i，j)对应的高程值； (32)对数字高程图的每个栅格(i，j)，定义一个正方形的地形块:P=(ZiuIk=Utm，...，j+J，P的边长为2L+1 (以栅格的数目为单位)，其中心为栅格(i，j)，L是一个正整数，它的选取应该使地形块P能够包含巡视器的外包络轮廓；利用地形块P内的高程值进行平面拟合，计算拟合平面的坡度角P、粗糙度σ和高程差ε等地形信息，基于巡视器的爬坡坡度、最大粗糙度和越障高度性能指标，对地形信息归一化并进行加权，将加权结果作为栅格(i, j)的适宜度，记为Gi, j ； (33)对数字高程图中的所有栅格计算适宜度，通过设定适宜度阈值将适宜度小于设定阈值的栅格标记为“障碍单元”，对“障碍单元”栅格按照连通性进行聚类，将处于同一连通区域的“障碍单元”栅格标记为一个障碍物，从而完成障碍物提取，得到包含若干障碍物区域的适宜度地图。
4.根据权利要求1所述的巡视器地面导航规划控制方法，其特征在于:所述步骤(4)中的路径规划具体实现过程如下: (41)在步骤(3)得到的适宜度地图上设置起始点和目标点，考虑里程最短、时间最短、安全性最高因素作为优化条件，设计代价函数，通过设置代价函数中距离、时间和安全性的比例因子对各因素在路径搜索中的比重进行调整；` (42)利用代价函数计算适宜度地图上起始点和目标点间栅格的路径代价，采用基于栅格的路径搜索算法，在适宜度地图上搜索路径，结合巡视器的最小转弯半径对搜索得到的路径进行平滑处理，得到一个路径点序列； (43)结合步骤(2)得到的三维点云地形数据，地面操作人员根据需要对规划得到的路径点序列进行调整，如:修改某些路径点的位置，删除某些路径点，或者增加一些关键路径点，完成对路径规划结果的修正和优化。
5.根据权利要求1所述的巡视器地面导航规划控制方法，其特征在于:所述步骤(5)中的移动策略生成具体实现过程如下: (51)若选择采用整器指令的移动策略，则根据巡视器的运动性能和航向约束，通过曲线拟合的方式将步骤(4)得到的路径点序列拟合成平滑的曲线，按曲率对拟合的曲线进行逐段分解，得到以曲率、长度等信息表示的多段前后相连的弧线，从而生成由运动速度、运动曲率及运动里程组成的整器指令； (52)若选择采用直接指令的移动策略，则基于巡视器的运动学关系，建立巡视器的驱动运动学方程，并求解，将一条整器指令分解为包含驱动轮期望转速和转向轮期望转角的直接指令； (53)若选择采用路径点跟踪指令的移动策略，则从路径点序列中选取若干路径点的坐标作为路径点跟踪指令，令巡视器自主地控制运动速度、运动曲率和运动里程，动态地跟踪指定的路径点； (54)对于生成的移动策略，基于步骤(2)得到的三维地形数据，利用巡视器运动学模型或动力学模型对巡视器的移动进行仿真，通过计算巡视器按照生成的运动控制指令移动时的位置姿态信息、摇臂、轮系及各运动部件的关节转角和角速度信息，对移动策略进行验证，确保移动策略安全有效。
6.根据权利要求1所述的巡视器地面导航规划控制方法，其特征在于:所述步骤(5)中的巡视器运动控制指令主要包括:整器指令、直接指令和路径点跟踪指令，其中，整器指令即巡视器整器的运动速度、运动曲率及运动里程等指令，表征整器的基本运动趋势，属于任务级指令，此类指令生成单条指令或指令组；直接指令即轮系的驱动轮期望转速和转向轮期望转角指令，属于指令级指令，此类指令一次生成单条指令；路径点跟踪指令即巡视器所需跟踪的路径轨迹，通常用一组路径点的坐标描述，也属于任务级指令，此类指令可由路径规划直接得到。
【文档编号】G05D1/10GK103869820SQ201410101409
【公开日】2014年6月18日 申请日期:2014年3月18日 优先权日:2014年3月18日
【发明者】邢琰, 刘祥, 滕宝毅, 毛晓艳, 王大轶, 何健, 刘云, 何英姿, 贾永 申请人:北京控制工程研究所