CN110057371A - 一种基于压缩感知的行星巡视器主动路径规划方法 - Google Patents

Abstract

本发明公开了一种基于压缩感知的行星巡视器主动路径规划方法，步骤如下：首先，利用SLAM方法进行常规巡视器定位与建图；利用Fisher信息熵实时审查状态估计以及特征点估计的不确定性，并建立特征主动重观测决定机制；在重观测地图特征时引入压缩感知方法，利用L1范数代替压缩感知原问题中难以求解的L0范数优化问题，对非高斯分布的噪点进行估计；接着利用Gini指标函数审查噪点估计序列的稀疏正则性，确保L1范数凸优化方法的精度；最后，将最大化下一步Gini评价参数结果为优化指标，建立完整的巡视器路径规划方法。本发明涉及导航制导与控制技术领域，主要面向行星表面漫游巡视任务段的导航制导应用。

Description

一种基于压缩感知的行星巡视器主动路径规划方法

技术领域

本发明属于导航制导与控制技术领域，具体涉及一种基于压缩感知的行星巡视器主动路径规划方法。

背景技术

行星表面漫游巡视任务中，行星车的导航定位对于探测任务的安全性、科学勘探及其他工程目标的实现都是不可或缺的重要环节。目前行星车的导航定位一般采用半自主模式，通常使用数类主/被动敏感器进行组合导航，例如美国在2004年发射的勇气号任务，其巡视器的定位系统包含了车轮码盘、惯性测量单元敏感器等便捷式定位系统，同时也搭载了视觉里程计(Visual Odometry)进行光学导航定位。后续NASA于2012年发射的好奇号火星车以及欧空局于2018年发射的ExoMars火星车也同样配置了类似的导航系统，并提升了敏感器的精度。在行星车漫游巡视阶段，目前需要科学家根据已有的全局地图选定目的地，规划控制序列并上传至行星车，这种方法能够有效保证行星车的安全，但由于这种方式为半自主模式，因此在进行远星体的探测任务中，地球—行星二者之间的星间距离会导致通讯时延、数据传输带宽限制、信号衰减等，从而对探测任务的效率与回报造成很大影响。

此外，在实际任务过程中，很多情况下行星车并没有周围环境的准确先验信息，这种情况下，地面人员需要等待行星车先收集周边地貌信息，并在绘制精确的地图后才能继续选择目标点，让行星车根据指令再进行目标探测，这便极大的延长了任务周期，严重影响了探测的效率。为了提升行星漫游巡视的任务回报，行星车需要具备自主的探测能力，即完全不需要地面测控人员的干预，通过对自身周边地貌的感知便做到自主选定行驶目标，并同时进行自主定位与建图、调姿与避障。

发明内容

针对于上述现有技术的不足，本发明的目的在于提供一种基于压缩感知的行星巡视器主动路径规划方法，以解决现有技术中行星巡视器在进行自主路径规划时，因地貌纹理特征贫瘠导致的导航与制图精度低、规划路径规划目标不明确的问题。

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

本发明的一种基于压缩感知的行星巡视器主动路径规划方法，包括步骤如下：

1)利用机器人定位与建图方法，实时获取行星巡视器的相对位姿及周边地图估计，并记录行星巡视器状态方差以及与状态和地图特征点的协方差，作为状态估计不确定性的参照；

2)利用Fisher信息熵量化实时检测步骤1)中的状态估计不确定性，包括行星巡视器自身状态的不确定性和与地图特征点相关的不确定性；

3)设定特征点重观测触发条件，当状态估计不确定性满足特征点重观测时，执行行星巡视器对地图已编目特征点重观测任务，并根据与目标点的探测距离及周边障碍折衷选择重观测目标点；

4)将重观测路径规划问题转换为压缩感知问题，并应用L1范数凸优化方法寻求替代解，完成路径规划的控制量序列求解以及状态量与地图特征点位置的估计；

5)采用Gini指标评估步骤4)输出结果的稀疏正则特性，若状态量与地图特征点位置估计满足稀疏正则特性则执行步骤4)中控制量序列，实现行星巡视器的主动路径规划；若状态量及特征点位置估计量不满足稀疏正则特征则返回步骤3)，重新选择重观测目标点并执行步骤4)到步骤5)；若所有备选目标点均不满足要求则放弃路径规划任务，等待下一步指示。

进一步地，所述步骤1)中行星巡视器状态方差与协方差记录方法如下：

仅保存行星巡视器相对位置以及相对姿态的估计方差阵P_x，即其中P_rr为巡视器位置的方差阵，P_θθ为巡视器姿态方差阵对于地图特征点则仅保存与自身相关的位置估计的方差阵即其中，为第i个特征点位置估计的方差阵。

进一步地，在进行步骤2)及步骤3)时，使用定长时间窗口内的状态信息熵增量来演化不确定性，其中状态信息熵定义为：

其中，k为离散采样时间，X_r(k)为k时刻的巡视器状态量，u_k为k时刻巡视器控制量，步骤3)中地图特征重观测的触发必须同时满足以下两个条件：

都有ΔI(X_r(k+1),X_r(k))＞0

其中，W_t为预选的时间窗口，在此取W_t＝20s；k,s均为离散时间点；Δt为采样时间间隔；ε为预设的响应阈值。

进一步地，所述步骤3)中重观测目标点位置的选择准则如下：

将观测特征点较为密集的几处位置记录为备选重观测位置，再选择重观测目标点时，以如下判据作为选择指标：

其中，α为权重参数，0＜α＜1，此处设定为α＝0.75，α值根据任务灵活设定；d_s＝p_r-p_s为行星巡视器当前位置p_r到某目标点位置p_s的欧式距离，为距离所有周边障碍的平均距离，其中p_h(i)为第i个障碍的位置。

进一步地，所述步骤4)中对地图特征点位置处理方式如下：

41)对特征点的观测方程进行重新表达，其表达式如下：

其中，为新观测到的地图特征点；为观测量中在地图中已编目的特征点；为地图特征点位置集合；h_k+1(·)为观测方程；υ_k+1为传统高斯分布的噪声；为非常规高斯分布的观测噪声，即待估计量；

42)针对每一个匹配特征点建立观测残差方程，表达式如下：

其中，为第k+1步特征点地图位置；为其预估值；g_k为逆观测方程；z_k,z_k+1|k分别为H_k，F_k+1分别为观测方程，行星巡视器动力学方程的雅克比矩阵；u_k+1为当前拟执行的控制量，为系统方程的过程噪声估计量，在此也认为其同样掺杂了非高斯分布的噪声，将待估计的系统噪声与观测噪声整理为向量形式，即其中，e_k为待估计噪声向量，则观测残差方程描述为：

其中，

43)考虑的维度较大，非高斯分布的噪声野点较为稀疏，故利用压缩感知原理来估计非高斯分布的噪声项，即建立如下优化方程：

其中：为高斯分布下的系统与观测噪声方差阵；Q_k为系统噪声的方差阵，U_k+1为观测噪声的方差阵在无非高斯噪声干扰下应符合卡方分布，τ是与卡方检验表与状态维数相关的参数，根据e_k维数查询卡方检验对照表确定；此处与压缩感知原L0范数求解有所区别，采用L1范数进行替代，即||·||₁，并利用凸优化求解器进行高效求解。

进一步地，所述步骤5)中采用Gini指标来判断估计结果的稀疏正则特性具体如下：

其中，e_k[i]为e_k中按照从大至小顺序排列的第i个元素，N为e_k的维数，以此指标为稀疏正则参照，将行星巡视器控制序列规划描述为如下形式：

s.t.

u_k+1≤u_max

Gini(e_k+1)＞Gini_min

其中，Υ_hazard为障碍区域；u_max为最大许用控制量；Gini_min为稀疏正则指标阈值，当评估结果小于阈值时，系统自动回到步骤3)，重新选择目标特征点；若所有备选目标均不满足要求，则认为规划失败。

本发明的有益效果：

1、本发明引入压缩感知方法来估计系统动力学与观测噪声，能够实现对非高斯分布下噪声的有效估计，从而提高巡视器在周边地貌纹理贫瘠下的定位精度；

2、本发明利用Gini指标来实时审查估计量的正确性，能够在不显著增加计算代价的前提下增强算法的估计准确性，从而保证巡视器定位与制图的实时精度。

附图说明

图1为本发明方法的流程图；

图2为随机规划的巡视器历史估计路径示意图；

图3为绕圆行驶下的巡视器历史估计路径示意图；

图4为以每步Fisher信息熵最大为规划目标的巡视器历史估计路径示意图；

图5为绕圆行驶运动为基础，辅以基于压缩感知的主动重观测方法的巡视器历史估计路径示意图；

图6a为敏感器云台转向角控制量规划结果示意图；

图6b为沿巡视器体轴方向速度控制量规划结果示意图；

图6c为巡视器航向角角速度控制量规划结果示意图；

图7a为四种规划方法中巡视器位置估计累积误差比较结果示意图；

图7b为四种规划方法中巡视器航向角估计累积误差比较结果示意图。

具体实施方式

为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。

参照图1所示，本发明的一种基于压缩感知的行星巡视器主动路径规划方法，包括步骤如下：

1)利用机器人定位与建图方法，实时获取行星巡视器的相对位姿及周边地图估计，并记录行星巡视器状态方差以及与状态和地图特征点的协方差，作为状态估计不确定性的参照；

在步骤1)中行星巡视器状态方差与协方差记录方法如下：

仅保存行星巡视器相对位置以及相对姿态的估计方差阵P_x，即其中P_rr为行星巡视器位置的方差阵，P_θθ为行星巡视器姿态方差阵对于地图特征点则仅保存与自身相关的位置估计的方差阵即其中，为第i个特征点位置估计的方差阵。

2)利用Fisher信息熵量化实时检测步骤1)中的状态估计不确定性，包括行星巡视器自身状态的不确定性和与地图特征点相关的不确定性；

3)设定特征点重观测触发条件，当状态估计不确定性满足特征点重观测时，执行行星巡视器对地图已编目特征点重观测任务，并根据与目标点的探测距离及周边障碍折衷选择重观测目标点；

在进行步骤2)及步骤3)时，使用定长时间窗口内的状态信息熵增量来演化不确定性，其中状态信息熵定义为：

其中，k为离散采样时间，X_r(k)为k时刻的巡视器状态量，u_k为k时刻巡视器控制量，步骤3)中地图特征重观测的触发必须同时满足以下两个条件：

都有ΔI(X_r(k+1),X_r(k))＞0

其中，W_t为预选的时间窗口，在此取W_t＝20s；k,s均为离散时间点；Δt为采样时间间隔；ε为预设的响应阈值。

以及，所述步骤3)中重观测目标点位置的选择准则如下：

将观测特征点较为密集的几处位置记录为备选重观测位置，再选择重观测目标点时，以如下判据作为选择指标：

其中，α为权重参数，0＜α＜1，此处设定为α＝0.75，α值根据任务灵活设定；d_s＝p_r-p_s为行星巡视器当前位置p_r到某目标点位置p_s的欧式距离，为距离所有周边障碍的平均距离，其中p_h(i)为第i个障碍的位置。

4)将重观测路径规划问题转换为压缩感知问题，并应用L1范数凸优化方法寻求替代解，完成路径规划的控制量序列求解以及状态量与地图特征点位置的估计；

在步骤4)中对地图特征点位置处理方式如下：

41)对特征点的观测方程进行重新表达，其表达式如下：

其中，为新观测到的地图特征点；为观测量中在地图中已编目的特征点；为地图特征点位置集合；h_k+1(·)为观测方程；υ_k+1为传统高斯分布的噪声；为非常规高斯分布的观测噪声，即待估计量；

42)针对每一个匹配特征点建立观测残差方程，表达式如下：

其中，为第k+1步特征点地图位置；为其预估值；g_k为逆观测方程；z_k,z_k+1|k分别为H_k，F_k+1分别为观测方程，行星巡视器动力学方程的雅克比矩阵；u_k+1为当前拟执行的控制量，为系统方程的过程噪声估计量，在此也认为其同样掺杂了非高斯分布的噪声，将待估计的系统噪声与观测噪声整理为向量形式，即其中，e_k为待估计噪声向量，则观测残差方程描述为：

其中，

43)考虑的维度较大，非高斯分布的噪声野点较为稀疏，故利用压缩感知原理来估计非高斯分布的噪声项，即建立如下优化方程：

其中：为高斯分布下的系统与观测噪声方差阵；Q_k为系统噪声的方差阵，U_k+1为观测噪声的方差阵在无非高斯噪声干扰下应符合卡方分布，τ是与卡方检验表与状态维数相关的参数，根据e_k维数查询卡方检验对照表确定；此处与压缩感知原L0范数求解有所区别，采用L1范数进行替代，即||·||₁，并利用凸优化求解器进行高效求解。

5)采用Gini指标评估步骤4)输出结果的稀疏正则特性，若状态量与地图特征点位置估计满足稀疏正则特性则执行步骤4)中控制量序列，实现行星巡视器的主动路径规划；若状态量及特征点位置估计量不满足稀疏正则特征则返回步骤3)，重新选择重观测目标地点并执行步骤4)到步骤5)；若所有备选目标点均不满足要求则放弃路径规划任务，等待下一步指示。

在步骤5)中采用Gini指标来判断估计结果的稀疏正则特性具体如下：

其中，e_k[i]为e_k中按照从大至小顺序排列的第i个元素，N为e_k的维数，以此指标为稀疏正则参照，将行星巡视器控制序列规划描述为如下形式：

s.t.

u_k+1≤u_max

Gini(e_k+1)＞Gini_min

其中，Υ_hazard为障碍区域；u_max为最大许用控制量；Gini_min为稀疏正则指标阈值，当评估结果小于阈值时，系统自动回到步骤3)，重新选择目标特征点；若所有备选目标均不满足要求，则认为规划失败。

图2-图7b为本发明方法的仿真示例，其中图2-图5中实体线代表实际路径，点状线代表估计路径，散点代表地图特征点。图2为随机规划的巡视器历史估计路径；图3为绕圆行驶下的巡视器历史估计路径；图4为以每步Fisher信息熵最大为规划目标的巡视器历史估计路径；图5为绕圆行驶运动为基础，辅以基于压缩感知的主动重观测方法的巡视器历史估计路径。图6a为敏感器云台角速度控制量规划结果，图6b为沿巡视器体轴方向的速度控制量规划结果，图6c为航向角角速度控制量规划结果。图7a和图7b为50次蒙特卡洛仿真统计结果，其中统计量为估计量的累积误差之和。从图7a和图7b中可以看出，基于Fisher信息熵最大的路径规划方法和基于压缩感知的主动重观测路径规划方法均比随机规划及绕圆运动的累积误差低，体现了路径规划在提高定位精度上的优势，其中本技术发明所提出的方法的位置估计精度(见图7a)和航向角估计精度(见图7b)均优于基于Fisher信息熵最大的规划方法，并且规划得出的主动重观测方案对原既定路线影响不大，体现了该方法的优势。

本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (6)

1.一种基于压缩感知的行星巡视器主动路径规划方法，其特征在于，包括步骤如下：

1)利用机器人定位与建图方法，实时获取行星巡视器的相对位姿及周边地图估计，并记录行星巡视器状态方差以及与状态和地图特征点的协方差，作为状态估计不确定性的参照；

2)利用Fisher信息熵量化实时检测步骤1)中的状态估计不确定性，包括行星巡视器自身状态的不确定性和与地图特征点相关的不确定性；

3)设定特征点重观测触发条件，当状态估计不确定性满足特征点重观测时，执行行星巡视器对地图已编目特征点重观测任务，并根据与目标点的探测距离及周边障碍折衷选择重观测目标点；

4)将重观测路径规划问题转换为压缩感知问题，并应用L1范数凸优化方法寻求替代解，完成路径规划的控制量序列求解以及状态量与地图特征点位置的估计；

5)采用Gini指标评估步骤4)输出结果的稀疏正则特性，若状态量与地图特征点位置估计满足稀疏正则特性则执行步骤4)中控制量序列，实现行星巡视器的主动路径规划；若状态量及特征点位置估计量不满足稀疏正则特征则返回步骤3)，重新选择重观测目标点并执行步骤4)到步骤5)；若所有备选目标点均不满足要求则放弃路径规划任务，等待下一步指示。

2.根据权利要求1所述的基于压缩感知的行星巡视器主动路径规划方法，其特征在于，所述步骤1)中行星巡视器状态方差与协方差记录方法如下：

仅保存行星巡视器相对位置以及相对姿态的估计方差阵P_x，即其中P_rr为巡视器位置的方差阵，P_θθ为巡视器姿态方差阵对于地图特征点则仅保存与自身相关的位置估计的方差阵即其中，为第i个特征点位置估计的方差阵。

3.根据权利要求1所述的基于压缩感知的行星巡视器主动路径规划方法，其特征在于，在进行步骤2)及步骤3)时，使用定长时间窗口内的状态信息熵增量来演化不确定性，其中状态信息熵定义为：

其中，k为离散采样时间，X_r(k)为k时刻的巡视器状态量，u_k为k时刻巡视器控制量，步骤3)中地图特征重观测的触发必须同时满足以下两个条件：

都有ΔI(X_r(k+1),X_r(k))＞0

其中，W_t为预选的时间窗口，在此取W_t＝20s；k,s均为离散时间点；Δt为采样时间间隔；ε为预设的响应阈值。

4.根据权利要求1所述的基于压缩感知的行星巡视器主动路径规划方法，其特征在于，所述步骤3)中重观测目标点位置的选择准则如下：

将观测特征点较为密集的几处位置记录为备选重观测位置，再选择重观测目标点时，以如下判据作为选择指标：

其中，α为权重参数，0＜α＜1，此处设定为α＝0.75，α值根据任务灵活设定；d_s＝p_r-p_s为行星巡视器当前位置p_r到某目标点位置p_s的欧式距离，为距离所有周边障碍的平均距离，其中p_h(i)为第i个障碍的位置。

5.根据权利要求1所述的基于压缩感知的行星巡视器主动路径规划方法，其特征在于，所述步骤4)中对地图特征点位置处理方式如下：

41)对特征点的观测方程进行重新表达，其表达式如下：

其中，为新观测到的地图特征点；为观测量中在地图中已编目的特征点；为地图特征点位置集合；h_k+1(·)为观测方程；υ_k+1为传统高斯分布的噪声；为非常规高斯分布的观测噪声，即待估计量；

42)针对每一个匹配特征点建立观测残差方程，表达式如下：

其中，l_k+1为第k+1步特征点地图位置；为其预估值；g_k为逆观测方程；z_k,z_k+1|k分别为H_k，F_k+1分别为观测方程，行星巡视器动力学方程的雅克比矩阵；u_k+1为当前拟执行的控制量，为系统方程的过程噪声估计量，在此也认为其同样掺杂了非高斯分布的噪声，将待估计的系统噪声与观测噪声整理为向量形式，即其中，e_k为待估计噪声向量，则观测残差方程描述为：

其中，

43)利用压缩感知原理来估计非高斯分布的噪声项，即建立如下优化方程：

其中：为高斯分布下的系统与观测噪声方差阵；Q_k为系统噪声的方差阵，U_k+1为观测噪声的方差阵在无非高斯噪声干扰下应符合卡方分布，τ是与卡方检验表与状态维数相关的参数，根据e_k维数查询卡方检验对照表确定；此处与压缩感知原L0范数求解有所区别，采用L1范数进行替代，即||·||₁，并利用凸优化求解器进行高效求解。

6.根据权利要求1所述的基于压缩感知的行星巡视器主动路径规划方法，其特征在于，所述步骤5)中采用Gini指标来判断估计结果的稀疏正则特性具体如下：

其中，e_k[i]为e_k中按照从大至小顺序排列的第i个元素，N为e_k的维数，以此指标为稀疏正则参照，将行星巡视器控制序列规划描述为如下形式：

s.t.

u_k+1≤u_max

Gini(e_k+1)＞Gini_min

其中，Υ_hazard为障碍区域；u_max为最大许用控制量；Gini_min为稀疏正则指标阈值，当评估结果小于阈值时，系统自动回到步骤3)，重新选择目标特征点；若所有备选目标均不满足要求，则认为规划失败。