一种基于地形轮廓匹配的地外天体巡视器定位方法

1.本发明涉及一种基于地形轮廓匹配的地外天体巡视器定位方法，尤其涉及一种适用于在任务区域坑洼、纹理缺失且传感器条件受限的情况下，对巡视器进行位姿估计和全局定位的方法，属于空间机器人技术领域。


背景技术：

2.随着深空探测技术的不断发展，各类轮式巡视器已被送至月面、火面，执行探测、图像采集和土壤样本采集任务。传统巡视器控制通过天地大回路的方式进行，传感器信息下传回地面站进行专家决策，再将控制指令上传。由于天地传输延时，该控制方式操作繁琐、耗时。于是近年来，巡视车自主巡视算法成为研究热点，其中，精准的定位是路径规划和避障的前提，是巡视器最重要的模块之一。然而，月面、火面巡视器的定位面临以下难题：(1)月面、火面地表不平坦，导致轮式里程计误差大，且坑洼地段易导致巡视器颠簸，视觉定位方式失准；(2)月表环境特征缺失、纹理单一，导致相对定位方法误差大；(3)受功率和载荷限制，巡视器配备的传感器种类和数量有限，多数小型巡视器只配备彩色相机，给定位系统鲁棒性带来挑战。在以上条件约束下，定位系统要保证定位系统精度和鲁棒性需要借助地图的先验信息以进行全局定位。地外天体任务在初期往往会利用环绕卫星绘制任务区域的数字高程图，高程图作为地图的先验信息可为巡视器的全局定位提供参考。
3.目前对月面巡视器自主定位技术的研究大致分为两类，以里程计为核心的相对定位方法和以模式识别为核心的绝对定位方法。视觉里程计是slam(simultaneous localization and mapping，即时定位与地图构建)问题的子问题，其通过立体视觉算法或深度传感器直接取得环境中三维点信息，利用图像中与之匹配的2d点的运动估计出相机的帧间运动，各帧运动累计得出相机相对初始时刻的位姿。有研究提出月面环境下的巡视器视觉定位方案，该方案利用视觉、惯性里程计对巡视器进行相对定位，重点解决了月面低纹理条件下定位鲁棒性差的问题。然而，里程计是相对定位方法，在月面探索场景下，巡视器通常需要行驶数千米的距离，这使得累计误差无法被忽视，需要进行全局定位来矫正里程计的累计误差。
4.全局定位即给出巡视器相对全局坐标系的位姿，由于巡视器本地只能进行相对定位，需要全局的定位信息就需要额外方式给出全局信息。全局定位方法分为两类，一种是通过卫星无线电进行，通过绕月卫星与巡视器进行无线电通讯，达到了米级的定位精度。但该方法依赖卫星与巡视器的稳定通讯，对巡视器的行进路线有严格约束，无法在任务中实现即时、随地的全局定位；另一种是将已知地图的全局特征作为先验知识，巡视器本地观测与已知特征作匹配。有研究通过巡视器搭载的星敏感器观测太阳方位，得出巡视器的全局定位，方位角估计精确，但位置精度在千米级，无法达到巡视器导航需求。此外，也有研究将卫星地图中的陨石坑作为地标，通过视觉、激光雷达等传感器还原的陨石坑形状后与地图中的陨石坑作匹配，从而得出巡视器的全局定位，方位误差均达到导航避障的标准。但是该方案对巡视器搭载传感器种类要求高，不满足巡视器轻量化设计需求。


技术实现要素：

5.本发明的主要目的是提供一种基于地形轮廓匹配的地外天体巡视器定位方法，基于卫星数字高程图和巡视器视觉里程计定位，在定位初值附近生成位姿搜索网络，将高程点云投影至每个位姿节点处，将得到投影轮廓与相机观测轮廓作特征匹配，搜索得到最优位姿，具有鲁棒性强，定位精度高等优点。
6.本发明的目的是通过下述技术方案实现的：
7.本发明公开的一种基于地形轮廓匹配的地外天体巡视器定位方法，由里程计相对定位和投影搜索绝对定位两个线程组成。相对定位始终运行，绝对定位择机运行。相对定位线程运行视觉里程计，接受rgb图像，通过估计帧间运动累计出巡视器相机的位姿初值，给出实时、连续的相对定位。绝对定位目的是在任务过程中修正相对定位的累计误差，或在巡视器颠簸、侧翻等情况下丢失实时定位时，找回全局定位。在巡视器运行过程中，通过高程图和巡视器方位判断视野内是否有明显的山脉轮廓作为参照以保证定位的可靠性。当巡视器视野内存在环形山时，运行绝对定位模块。绝对定位模块首先通过插值和比例缩放，将数字高程图还原为点云地图。将相对定位给出的位姿作为搜索中心，构建三维(平面坐标x，y和方向角γ)的搜索网络。根据相机内参、外参将地图点云投影至每个位姿节点，得到投影图。将投影图与相机图像进行轮廓提取操作，将各位姿的投影图与图像轮廓作特征匹配。选出匹配度最高的曲线，其节点对应的位姿即最终全局最优位姿。
8.本发明公开的一种基于地形轮廓匹配的地外天体巡视器定位方法，包括如下步骤：
9.步骤一：通过运行光流法前端，提取光流特征点，求解相机帧间运动，并将帧间运动累计给出相对起始点的巡视器位姿，用直角坐标和欧拉角的形式表示为：
10.pose＝{x,y,z,γ,β,α}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
11.其中，x,y,z为直角坐标，γ,β,α为欧拉角，对应转序为z-y-x，方向遵循右手定则。
12.步骤二：处理数字高程图，通过切片处理得到二维栅格图，该栅格图反映任务区域中环形山的分布；根据相机视场角和步骤一得到的巡视器位姿，确定相机视野内是否存在环形山作为参照；在平面地图中，巡视器六自由度位姿退化为三自由度位姿
13.pose'＝{x,y,γ}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
14.根据pose'确定平面上的一条射线，所述射线代表巡视器位置和其相机视场方向；相机视线与巡视器轴线平行，考虑相机视场角fov后，将相机视场表示为覆盖视场范围的若干条射线，第i条射线的表达式为
[0015][0016]
其中，n为射线总数，ray()为方位到射线的映射；将n条射线与栅格图中表示环形山的黑色栅格作碰撞检测，计算射线长度，即环形山障碍与巡视器的间距；当相机视野内出现环形山并距离小于设定阈值时进行步骤三，启动绝对定位线程；
[0017]
步骤三：以相对定位给出的初始定位为中心，生成三维位姿搜索网络，表示为
[0018][0019]
其中，ni,nj,nk为网络每维的搜索半径，(xi,yj,γk)为位姿节点；
[0020]
首先，方位角在初值附近采样
[0021]
γk＝γ0+δ
γ
·
(k-nk)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0022]
其中，δ
γ
为方位角搜索步长，k为搜索遍历值，γ0为方位角初值；
[0023]
对于每个方位角γk，作位置采样，分别在方位角方向和其法向交叉搜索
[0024][0025]
其中，δ为距离搜索步长；至此构造出位姿搜索网络s；
[0026]
步骤四：将数字高程图还原为三维点云，数字高程图为(u,v,d)的形式，前两维为像素，后一维为高度，通过下式将像素点映射为三维坐标
[0027][0028]
其中，λ为高程图的伸缩比例；u、v分别为横向和纵向像素，d为像素高程；
[0029]
然后，将地图三维点云投影至位姿搜索网络s的每个节点s
i,j,k
上，投影变换为
[0030]
p
uv
＝ktp
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0031]
其中，p
uv
为投影点的像素坐标，k表示相机内参，t表示相机坐标系相对世界系的齐次变换矩阵，p表示地图点云；
[0032]
得到投影散点图后，通过边缘提取算法提取出投影图的轮廓曲线。对搜索网络s的所有节点作同样操作，得到轮廓集提取出相机图像中的轮廓曲线，记作e
img
。一条轮廓e表示为
[0033][0034]
其中，(ui,vi)为轮廓中第i个节点的像素坐标,n为节点总数。
[0035]
步骤五：构建方位角金字塔，消除巡视器滚转和俯仰对匹配的影响。对位姿搜索网络进行扩维
[0036][0037]
其中，xi,yj,γk的构造方法同步骤三所述，n
l
为滚转角采样半径，α
l
是第l个滚转角的采样值，构造为
[0038]
α
l
＝α0+δ
α
·
(l-n
l
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0039]
其中，α0为滚转角初值，δ
α
为滚转角搜索步长。将s'通过步骤四的方法投影得到轮廓集和图像轮廓e
img
。因为巡视器俯仰改变曲线纵向位置，不改变曲线形状，故对于每个投影轮廓e，将其纵向平移，使其第一个节点与e
img
重合，即：
[0040][0041]
其中，vi和v
′i分别为平移前后轮廓第i个节点的纵向像素坐标，v0和分别为投影轮廓和图像轮廓首节点的纵向像素坐标。记纵向对齐后的投影轮廓为e'，经过纵向对齐
后的轮廓消除巡视器俯仰对匹配的影响。
[0042]
将e'与e
img
用动态规整算法进行相似度匹配，得出每个位姿节点的相似度网络
[0043][0044]
其中，d
i,j,k,l
为轮廓e
i,j,k,l
对应的相似度。
[0045]
用遴选最优值的方法消除滚转维度
[0046][0047]
其中，d
′
i,j,k
表示每个三自由度位姿节点的相似度。
[0048]
筛选出最优匹配度的位姿节点的索引为
[0049][0050]
最优位姿为
[0051][0052]
根据上述寻优方法提高地外天体巡视器定位的精度和鲁棒性。
[0053]
有益效果：
[0054]
1、本发明公开的一种基于地形轮廓匹配的地外天体巡视器定位方法，通过轮廓投影的方式将三维的数字高程图降维，进行二维特征的匹配，得出巡视器的全局定位，相较其它全局定位方法，定位精度高，且对传感器配置要求低。
[0055]
2、本发明公开的一种基于地形轮廓匹配的地外天体巡视器定位方法，采用“相对+绝对”的定位方法，相对定位通过视觉里程计输出实时、连续的巡视器定位；绝对定位通过山脉轮廓的模板匹配得出巡视器的绝对定位，校准巡视器的累计误差。本发明在保证定位实时性的同时，有效提高定位系统的精度和鲁棒性。
[0056]
3、本发明公开的一种基于地形轮廓匹配的地外天体巡视器定位方法，采用轮廓对齐和构造“姿态金字塔”的方式，选取了每个节点s
i,j,k
中最优的姿态角，消除滚转和俯仰角对轮廓曲线匹配的影响。
附图说明
[0057]
图1本发明公开的一种基于地形轮廓匹配的地外天体巡视器定位方法示意图。
[0058]
图2本发明实例中月面栅格地图转化过程；其中图a为数字高程图；图b为栅格地图。
[0059]
图3本发明实例中一处适合绝对定位的地点。
[0060]
图4本发明实例中转化得到的点云地图。
[0061]
图5本发明实例中相机图像轮廓和最优点投影轮廓；其中图a为相机图像轮廓，图b为投影轮廓。
[0062]
图6本发明实例中绝对定位中各节点位姿的匹配度分布。
具体实施方式
[0063]
为了更好地说明本发明的目的和优点，下面通过对于月面某任务区域内巡视器的
导航定位进行实例分析，对本发明做出详细解释。
[0064]
实施例1：
[0065]
在一次月面巡视任务中，月面着陆器释放一小型月面巡视器，该巡视器仅配备了rgb相机，相机视场角为90
×
60(度)，成像分辨率为1280
×
720(像素)，图像帧率为1fps。巡视器任务为穿越一片已知高程地图的区域，在行进过程中，巡视器要保证精准的定位。
[0066]
步骤一：已知巡视器在地图中的起始位置，通过运行光流法前端，提取光流特征点，求解相机帧间运动，并将帧间运动累计给出相机相对起始点的位姿，用直角坐标和欧拉角的形式表示为：
[0067]
pose＝{x,y,z,γ,β,α}
[0068]
其中，欧拉角γ,β,α对应转序为z-y-x，方向遵循右手定则。
[0069]
步骤二：处理数字高程图，通过切片处理得到二维栅格图，该栅格图反映了任务区域中环形山的分布，如图2所示。并根据步骤一得到的巡视器位姿和相机视场角，确定相机视野内是否存在环形山作为参照。在平面地图中，巡视器六自由度位姿退化为三自由度
[0070]
pose＝{x,y,γ}
[0071]
该位姿唯一决定了平面上的一条射线，反映到地图中即巡视器相机视场中心的射线，假设相机视线与巡视器轴线平行，考虑相机视场角fov后，将相机视场表示为覆盖视场范围的若干条射线，第i条射线的表达式为
[0072][0073]
其中，n为射线总数，ray()为方位到射线的映射。将两条射线与栅格图中表示环形山的黑色栅格作碰撞检测，计算射线长度可得环形山障碍与巡视器间距，当相机视野内出现环形山并距离小于一定阈值时启动绝对定位线程。图3展示了符合绝对定位条件的一种情况，取采样射线数为3，阈值范围为8-15m，三条视场线均与栅格图相交，且距离在范围内。
[0074]
步骤三：以相对定位给出的初始定位为中心，生成三维位姿搜索网络，可表示为
[0075][0076]
其中，ni,nj,nk为网络每维的搜索半径，首先，方位角在初值附近采样
[0077]
γk＝γ0+δ
γ
·
(k-nk)
[0078]
其中，δ
γ
为方位角搜索步长，对于每个方位角γk，作位置采样，分别在方位角方向和其法向交叉搜索
[0079][0080]
其中，δ为距离搜索步长。至此构建出位姿搜索网络p。
[0081]
步骤四：将数字高程图还原为三维点云，如图4所示。然后，将地图三维点云投影至位姿搜索网络s的每个节点s
i,j,k
上。得到投影散点图后，通过边缘提取算法提取出投影图的地形轮廓，记作同时，提取出相机图像中的山脉轮廓。
[0082]
步骤五：通过动态规整算法逐一比较投影轮廓与图像轮廓，可以计算得到两条曲线的相似度，筛选出最高匹配度的位姿节点即为全局最优位姿。以任务中的一处绝对定位
点为例，图5展示了在点处的相机图像的轮廓和最优位姿下的投影轮廓，该位姿下的投影轮廓与图像轮廓相似度最高。图6展示了该处的搜索网络中各节点匹配度分布(位置真值为8.46,-2.47)，可见算法可收敛至真值，且在真值附近，相似度分布平滑，证明了算法有效性和鲁棒性。
[0083]
在任务过程中，选出三处绝对定位的地点，定位前后的结果如表1所示。为模拟初始位置的误差，在巡视器起始位置附近设定2m的位置误差，在巡视器初始化时，运行绝对定位，将巡视器定位误差调整至1m之内。运行至中间区域时，视觉里程计累积了定位误差，绝对定位后，定位仍能调整回1m精度之内，优化比例达到80.6％。到达目标区域附近，运行绝对定位将定位优化至1.5m内，仍能保证末端目标定位精度。
[0084]
表1绝对定位点优化前后定位
[0085][0086]
以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种基于地形轮廓匹配的地外天体巡视器定位方法，其特征在于：包括如下步骤，步骤一：通过运行光流法前端，提取光流特征点，求解相机帧间运动，并将帧间运动累计给出相对起始点的巡视器位姿，用直角坐标和欧拉角的形式表示为：pose＝{x,y,z,γ,β,α}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)其中，x,y,z为直角坐标，γ,β,α为欧拉角，对应转序为z-y-x，方向遵循右手定则；步骤二：处理数字高程图，通过切片处理得到二维栅格图，该栅格图反映任务区域中环形山的分布；根据相机视场角和步骤一得到的巡视器位姿，确定相机视野内是否存在环形山作为参照；在平面地图中，巡视器六自由度位姿退化为三自由度位姿pose'＝{x,y,γ}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)根据pose'确定平面上的一条射线，所述射线代表巡视器位置和其相机视场方向；相机视线与巡视器轴线平行，考虑相机视场角fov后，将相机视场表示为覆盖视场范围的若干条射线，第i条射线的表达式为其中，n为射线总数，ray()为方位到射线的映射；将n条射线与栅格图中表示环形山的黑色栅格作碰撞检测，计算射线长度，即环形山障碍与巡视器的间距；当相机视野内出现环形山并距离小于设定阈值时进行步骤三，启动绝对定位线程；步骤三：以相对定位给出的初始定位为中心，生成三维位姿搜索网络，表示为其中，n
i
,n
j
,n
k
为网络每维的搜索半径，(x
i
,y
j
,γ
k
)为位姿节点；方位角在初值附近采样γ
k
＝γ0+δ
γ
·
(k-n
k
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)其中，δ
γ
为方位角搜索步长，k为搜索遍历值，γ0为方位角初值；对于每个方位角γ
k
，作位置采样，分别在方位角方向和其法向交叉搜索其中，δ为距离搜索步长；至此构造出位姿搜索网络s；步骤四：将数字高程图还原为三维点云，数字高程图为(u,v,d)的形式，前两维为像素，后一维为高度，通过下式将像素点映射为三维坐标其中，λ为高程图的伸缩比例；u、v分别为横向和纵向像素，d为像素高程；将地图三维点云投影至位姿搜索网络s的每个节点s
i,j,k
上，投影变换为p
uv
＝ktp
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)其中，p
uv
为投影点的像素坐标，k表示相机内参，t表示相机坐标系相对世界系的齐次变换矩阵，p表示地图点云；
得到投影散点图后，通过边缘提取算法提取出投影图的轮廓曲线；对搜索网络s的所有节点作同样操作，得到轮廓集提取出相机图像中的轮廓曲线，记作e
img
；一条轮廓e表示为其中，(u
i
,v
i
)为轮廓中第i个节点的像素坐标,n为节点总数；步骤五：构建方位角金字塔，消除巡视器滚转和俯仰对匹配的影响；对位姿搜索网络进行扩维其中，x
i
,y
j
,γ
k
的构造方法同步骤三所述，n
l
为滚转角采样半径，α
l
是第l个滚转角的采样值，构造为α
l
＝α0+δ
α
·
(l-n
l
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)其中，α0为滚转角初值，δ
α
为滚转角搜索步长；将s'通过步骤四的方法投影得到轮廓集和图像轮廓e
img
；因为巡视器俯仰改变曲线纵向位置，不改变曲线形状，故对于每个投影轮廓e，将其纵向平移，使其第一个节点与e
img
重合，即：其中，v
i
和v
′
i
分别为平移前后轮廓第i个节点的纵向像素坐标，v0和分别为投影轮廓和图像轮廓首节点的纵向像素坐标；记纵向对齐后的投影轮廓为e'，经过纵向对齐后的轮廓消除了巡视器俯仰对匹配的影响；将e'与e
img
用动态规整算法进行相似度匹配，得出每个位姿节点的相似度网络其中，d
i,j,k,l
为轮廓e
i,j,k,l
对应的相似度；用遴选最优值的方法消除滚转维度其中，d
′
i,j,k
表示每个三自由度位姿节点的相似度；筛选出最优匹配度的位姿节点的索引为最优位姿为根据上述寻优方法提高地外天体巡视器定位的精度和鲁棒性。

技术总结
本发明公开的一种基于地形轮廓匹配的地外天体巡视器定位方法，属于空间机器人技术领域。本发明由里程计相对定位和投影搜索绝对定位两个线程组成。相对定位始终运行，绝对定位择机运行。相对定位线程运行视觉里程计，接收RGB图像，通过估计帧间运动累计出巡视器相机的位姿初值。绝对定位目的是在任务过程中修正相对定位的累计误差，或在丢失实时定位时找回全局定位。在巡视器运行过程中，通过高程图和巡视器方位判断视野内是否有明显的山脉轮廓作为参照以保证定位可靠性。根据相机内参、外参将地图点云投影至每个位姿节点，得到投影图。将各位姿的投影图与图像轮廓作特征匹配，选出匹配度最高的曲线，并选出最终全局最优位姿。姿。姿。


技术研发人员：田泽川 张宏莹 胡权 张尧 姚林涵 毛岸远 赖志腾 文明 陈林 马奕然 李悦怡 彭靖卓 孙一勇
受保护的技术使用者：北京理工大学
技术研发日：2023.07.10
技术公布日：2023/10/15