CN112947533A - 复杂小天体表面移动序列轨迹规划方法 - Google Patents

Abstract

本发明涉及一种复杂小天体表面移动序列轨迹规划方法，属于深空探测技术领域。本发明的目的是为了解决现有技术规划的运动路径存在安全隐患的问题，针对现有技术中的小天体表面弹跳移动探测器的路径规划和轨迹优化方法，没有考虑存在参数不确定性和误差时弹跳探测器与地形障碍的碰撞概率，导致影响探测器运动的安全性和准确性，提出一种复杂小天体表面移动序列轨迹规划方法，该方法在轨迹规划时考虑初始状态误差和动力学模型的参数不确定性，得到连续优化弹跳轨迹，然后基于规划的轨迹并考虑探测器位置不确定性的影响，计算探测器与地形障碍的碰撞概率，通过优化运动路径点降低碰撞概率，从而实现障碍规避以及探测器的安全和精确移动。

Description

复杂小天体表面移动序列轨迹规划方法

技术领域

本发明涉及一种复杂小天体表面移动序列轨迹规划方法，属于深空探测技术领域。

背景技术

随着深空探测任务的不断开展，近地小天体已经成为各国航天机构和组织的重点研究对象。小天体的探测活动具有丰富的任务回报，但简单的接触即走或原位探测方式能获得的小天体的表面样本和信息仍然有限。为了增加探测任务的回报，需要采用类似大行星表面漫游车的移动探测方式。然而，受小天体表面附近复杂引力环境以及不确定性表面地形条件影响，传统的轮式移动漫游车不适用于小天体表面的移动探测任务，需要发展新的移动探测方式。弹跳移动是目前最受关注的一种小天体表面移动探测方式。这种方式的优点是可借助表面弱引力的特点在较小的能量消耗下实现较远距离的移动，同时由于采用跳跃的运动方式，探测器可以跳过一定大小的障碍物，进而缩短运动路线长度。考虑以上优点，弹跳移动探测有望成为未来小天体表面移动探测的主要方式。国内外学者也已经开展了不少相关的研究，主要围绕弹跳探测器的动力学与控制问题，以及探测器的导航制导与控制、运动路径规划和弹跳轨迹优化问题。因为小天体表面地形复杂且具有一定不确定性，探测器的运动路径对其安全性和运动高效性有重要影响。因此，弹跳移动探测器的路径规划和轨迹优化问题已成为目前小天体表面移动探测的重要研究方向。

在已发展的小天体表面弹跳移动探测器的轨迹规划方法中，在先技术[1](参见B.Hockman,M.Pavone.Stochastic motionplanning for hopping rovers on smallsolar system bodies[C].18th International Symposium on Robotics Research,2017.)，采用打靶法和一种策略梯度方法，研究了存在模型不确定性和误差的情况下单次弹跳的最优轨迹，同时，将连续路径点规划问题转换为马尔可夫决策过程，并应用一种样本高效的强化学习算法求解近似最优控制策略。虽然该方法采用了复杂动力学模型，但是它是一种基于数据和样本的学习方法，对数据量和计算时间要求高。

在先技术[2](参见Jianxun Jiang,Xiangyuan Zeng,Davide Guzzetti,YuyangYou.Path planning for asteroid hopping rovers with pre-trained deepreinforcement learning architectures[J].Acta Astronautica,2020,171:265–279.)，利用深度强化学习方法，研究了复杂地形下小天体表面弹跳探测器的路径规划问题。将小天体表面的三维地形转换成水平矩阵，作为强化学习算法的输入地形数据，并提出了一种具有良好收敛性和稳定性的深度强化学习结构。该方法可用于搜索给定三维地形下的最优运动路线，但是，它没有对弹跳运动的路径点进行规划和优化设计，同时该方法需要提前的离线学习和训练。

发明内容

本发明的目的是为了解决现有技术规划的运动路径存在安全隐患的问题，针对现有技术中的小天体表面弹跳移动探测器的路径规划和轨迹优化方法，没有考虑存在参数不确定性和误差时弹跳探测器与地形障碍的碰撞概率，导致影响探测器运动的安全性和准确性，提出一种复杂小天体表面移动序列轨迹规划方法，该方法在轨迹规划时考虑初始状态误差和动力学模型的参数不确定性，得到连续优化弹跳轨迹，然后基于规划的轨迹并考虑探测器位置不确定性的影响，计算探测器与地形障碍的碰撞概率，通过优化运动路径点降低碰撞概率，从而实现障碍规避以及探测器的安全和精确移动。

本发明是通过下述技术方案实现的。

本发明公开的复杂小天体表面移动序列轨迹规划方法，实现方法如下：针对探测器在小天体表面弹跳移动时的轨迹规划问题，建立探测器的轨道动力学方程。考虑小天体表面复杂地形条件，基于最小平方中值方法对三维地形进行表征，确定可行路径点区域，并得到具有特征化地形的当地近似平面。根据得到的特征化地形平面，采用基于弹跳移动特性改进的A*路径搜索算法得到距离最优的运动路线，然后根据弹跳移动特点和多次等分弹跳策略确定可行连续路径点。在小天体表面坐标系下，考虑动力学模型参数不确定性和初始状态误差的影响，给出探测器在两相邻路径点间单次弹跳轨迹的性能指标，优化得到最优弹跳轨迹，进而得到序列路径点的连续弹跳轨迹。基于规划的连续弹跳轨迹，对生成的每个单次弹跳最优轨迹，计算探测器与地形障碍的碰撞概率即探测器与所有可能碰撞的障碍的碰撞概率之和，如果该碰撞概率超过给定的阈值，则按照路径点分解置换策略对局部路径点进行重规划，得到考虑碰撞概率的优化序列路径点，从而得到优化的连续弹跳移动轨迹。既能改善弹跳移动的位置精度，又能提高探测器在复杂地形移动的安全性。

本发明公开的复杂小天体表面移动序列轨迹规划方法，包括如下步骤：

步骤一、针对探测器在小天体表面弹跳移动时的轨迹规划问题，建立探测器的轨道动力学方程。

首先定义描述探测器弹跳运动的参考坐标系。小天体固连坐标系O_f-X_fY_fZ_f的原点定义在目标小天体中心，Z_f轴为目标天体的最大转动惯量轴，X_f轴为目标天体的最小转动惯量轴，Y_f轴满足右手坐标系。在研究小天体表面移动探测器的轨迹规划问题时，需要获取小天体表面的地形信息和探测器相对表面的运动状态，因此定义小天体表面坐标系O_s-X_sY_sZ_s。将当地近似平面作为小天体表面坐标系的X_sY_s平面，该平面通过表面地形表征方法确定。弹跳移动起点作为坐标系原点O_s，弹跳移动起点指向目标点的方向矢量在当地近似平面的投影为X_s轴正向，Z_s轴正向为当地近似平面的外法向方向，Y_s轴满足右手坐标系。

在小天体固连坐标系下，探测器的轨道动力学方程为：

其中，r_f为探测器在小天体固连坐标系下的位置矢量，v_f为探测器在小天体固连坐标系下的速度矢量，ω为小天体的自旋角速度矢量，g为目标小天体的引力加速度矢量。

小天体固连坐标系和小天体表面坐标系下探测器位置与速度的坐标转换关系为：

其中：r_s为探测器在小天体表面坐标系下的位置矢量；v_s为探测器在小天体表面坐标系下的速度矢量；

为从小天体表面坐标系到小天体固连坐标系的坐标转换矩阵；l为小天体固连坐标系原点到小天体表面坐标系原点的位置矢量。

由式(1)(2)可得小天体表面坐标系下探测器的轨道动力学方程为：

步骤二、考虑小天体表面复杂地形条件，基于最小平方中值方法对三维地形进行表征，确定可行路径点区域，并得到具有特征化地形的当地近似平面。

小天体表面分布着不同大小的陨石坑、崎岖山地、以及岩石块，这种复杂的地形条件对探测器的表面移动构成了一定的威胁。根据弹跳移动区域的地形三维高程数据，将表面地形表示为在平面上分布的凸起区与凹陷区。通过最小平方中值方法对地形进行表征，并根据弹跳移动特性进行分类。

地形表征算法主要包括两部分任务：拟合当地近似平面即确定平面方程；识别不同地形。算法流程如下：

1)随机选取N个表面地形样本点中三个不共线的点(x_a,x_b,x_c)，确定平面的方程为n·X+d＝0；其中，n表示平面法向量，n＝(x_b-x_a)×(x_c-x_a)，d＝-n·x_a。

2)利用1)中得到的平面，计算每个样本点x_i相对该平面的残值r_i＝(n·x_i+d)²，选取其中值。

3)重复步骤1)和2)m次，m＝ln(1-P)/ln(1-(1-o)³)；其中P为期望的概率，o为平面外点占总点数的百分比的统计数；得到m个中值。

4)从步骤3)得到的m个中值中选取最小值r_med，其对应的平面记为最小平方中值平面(n_best,d_best)，并将其视为当地近似平面。

5)计算每个样本点相对当地近似平面的残值，若满足

其中

则该样本点位于当地近似平面外，否则，该样本点位于当地近似平面内。

6)对位于当地近似平面的样本点，利用最小二乘法确定当地近似平面方程，该平面即为小天体表面坐标系的X_sY_s平面。在平面以上的为凸起地形障碍，在平面以下的为凹陷地形障碍。

在当地近似平面上，用最小外接圆表示不同地形障碍区域。根据地形障碍区域的平面尺寸即最小外接圆的直径D，考虑探测器的弹跳能力即单次弹跳的最远距离d_max，将地形障碍区域分为两类。如果D＞d_max，则为不可跳跃区域，否则为可跳跃区域。同时，地形障碍区域以外的其它区域为可行路径点区域，进而得到具有特征化地形的当地近似平面。

步骤三、根据步骤二得到的特征化地形平面，采用基于弹跳移动特性改进的A*路径搜索算法得到距离最优的运动路线。然后根据弹跳移动特点和多次等分弹跳策略确定可行连续路径点。

首先，将得到的特征化地形二维平面网格化，网格单元大小根据平面尺寸和计算量大小确定。然后采用A*搜索算法进行最优移动路线规划，并根据弹跳运动特性对最优路线上短距离内路线出现多次转向的情况进行改进。路线改进的基本原则为：在保证路线不经过不可跳跃区域的情况下，根据弹跳探测器可朝任意方向移动的特点将局部路线转向次数减到最小，同时保证路线长度不变或缩短路线长度，得到最优运动路线。

在最优运动路线上确定可行路径点序列，路径点选择网格中点或顶点。路径点序列确定方法如下：

1)确定路径点：起点和终点。

2)转向路径点：路线上所有位于圆形地形障碍区域外的转向点。

3)越障路径点：在最优路线通过可跳跃障碍区域时，选取位于区域外的两个路径点；所述两个路径点分别为通过障碍区域前和通过障碍区域后最优路线上距离该障碍区域最近的一个路径点。

4)路径点序列：将以上路径点，按起点到终点的顺序组成路径点序列。

若步骤1)～3)确定的路径点中存在相邻两路径点间的距离大于d_max，则增加等分中间路径点。此中间路径点的选取按照多次等分弹跳策略。

步骤四、在小天体表面坐标系下，考虑动力学模型参数不确定性和初始状态误差的影响，给出探测器在两相邻路径点间单次弹跳轨迹的性能指标，优化得到最优弹跳轨迹，进而得到序列路径点的连续弹跳轨迹。

将步骤三得到的在平面地形上规划的序列路径点投射到其三维地形图上，得到对应的三维路径点。对于三维地形上两相邻路径点的单次弹跳运动，在小天体表面坐标系下探测器的轨道动力学方程可表示为参数的函数形式的状态方程：

其中x(t)表示t时刻的状态矢量，x₀＝[r₀,v₀]表示初始状态矢量。

设各初始状态误差和参数不确定性符合均值为零的高斯分布，则可根据其协方差信息和通过敏感性分析方法得到的敏感度矩阵计算终端状态x(t_f)的线性协方差矩阵：

其中，φ(t_f,0)为从初始时刻0到终端时刻t_f的状态转移矩阵，α_i表示任一参数，α_i＝ω,g，

为敏感度矩阵，k为表示参数的总数。

给出性能指标如下：

minJ＝c·E(v₀)+trace(C) (9)

其中c为权重系数，E(v₀)表示获得初始速度的能耗，

表示终端位置误差的协方差矩阵。

通过优化性能指标J，生成综合考虑能量消耗和终点位置误差的单次弹跳最优轨迹。对于多个路径点的弹跳轨迹规划，探测器从起始点获得初始速度后进入轨道运动状态并朝下个相邻路径点运动，到达目标路径点时停在表面，下次弹跳从该点获得初始速度并朝该点下个相邻路径点运动。由此，序列路径点的连续弹跳轨迹可由多个单次弹跳最优轨迹组成。

步骤五、基于步骤四规划的连续弹跳轨迹，对生成的每个单次弹跳最优轨迹，考虑探测器的位置不确定性的影响，计算探测器与地形障碍的碰撞概率即探测器与所有可能碰撞的障碍的碰撞概率之和，如果该碰撞概率超过给定的阈值，则按照路径点分解置换策略对局部路径点进行重规划，得到考虑碰撞概率的优化序列路径点，进而得到优化的连续弹跳移动轨迹。既能改善弹跳移动的位置精度，又能提高探测器在复杂地形移动的安全性。

综合考虑小天体表面地形障碍的位置、大小与可能的误差，将表面的地形障碍如石块、山地等建模为半椭球形障碍区。以椭球的球心r_c＝[x_c,y_c,z_c]和三个半轴长度a,b,c对障碍的位置和大小进行描述。则探测器与任一障碍的碰撞概率表示为：

其中，r_c为障碍区中心位置，

为探测器的名义位置，V＝2πabc/3表示障碍区的体积。计算碰撞概率时，近似将C看作常数矩阵。

探测器与多个障碍的碰撞概率为探测器与每个障碍碰撞概率的加和：

其中m为障碍数量。

如果探测器与地形障碍的碰撞概率P_ct为1，则表示探测器的运动轨迹穿过障碍区域，即探测器与地形障碍碰撞。同时，探测器距离障碍越近，碰撞概率越大。因此，考虑探测器位置和地形障碍的不确定性，为了保证探测器的安全，探测器的轨迹需要和地形障碍保持一定距离。当单次弹跳轨迹上的碰撞概率P_ct大于给定阈值时，在地形平面上对其路径点进行分解置换，并且对序列路径点进行重规划。基于碰撞概率的路径点优化策略如下：

1)将碰撞概率P_ct大于给定阈值时距离弹跳轨迹最近的障碍所对应的圆形区域进行拓展，即增加圆的半径R。此时探测器的名义位置

与其距离最近的障碍的最大碰撞概率为：

若考虑将最大碰撞概率

降低为原来的λ倍，λ∈(0,1)，并保证降低后P_ct小于给定阈值，则此时探测器的名义位置

与其距离最近的障碍的最大碰撞概率为：

因此，可将圆形区域的半径近似增大

2)增大后的圆形区域为新的障碍区，用新的越障路径点置换新障碍区内的原路径点；当两个新越障路径点间的距离大于d_max时，不能采用跳跃的方式通过障碍，此时采用路径点分解策略得到一个或多个分解中间路径点，分解中间路径点个数由分解后的碰撞概率决定。

3)对于位于扩展的障碍地形区外的其它路径点，如果新越障路径点与其相邻的路径点之间的距离小于探测器的最小弹跳距离d_min时，省略该路径点，其余路径点保持不变，并重新规划需要的等分中间路径点。

4)综上，得到优化后的序列路径点，进而得到优化的连续弹跳移动轨迹。

通过以上步骤确定的复杂小天体表面移动序列轨迹规划方法，可提高弹跳移动探测器在存在动力学模型参数不确定性和初始状态误差时的终点位置精度，同时降低探测器与地形障碍的碰撞概率，提高移动的安全性。

有益效果：

1.本发明公开的复杂小天体表面移动序列轨迹规划方法，通过最小平方中值方法对小天体表面的复杂地形进行表征，并得到具有特征地形的当地近似平面和可行路径点区域，然后采用基于弹跳移动特性改进的A*算法，得到距离最优运动路线，进而得到可行路径点序列。

2.本发明公开的复杂小天体表面移动序列轨迹规划方法，通过考虑探测器初始状态误差和动力学模型参数不确定性，给出相邻两路径点间单次弹跳轨迹的性能指标，优化得到单次弹跳最优轨迹，从而得到序列路径点的连续弹跳轨迹。

3.本发明公开的复杂小天体表面移动序列轨迹规划方法，基于单次弹跳最优轨迹，考虑探测器的位置不确定性，计算探测器与地形障碍的碰撞概率，优化路径点并重新规划序列路径点，实现探测器的障碍规避控制，并提高弹跳移动的准确性和安全性。

附图说明

图1为本发明公开的复杂小天体表面移动序列轨迹规划方法的流程图。

图2探测器弹跳移动的参考坐标系。

图3小天体表面复杂地形三维模型图。

图4地形表征算法原理示意图

图5特征地形平面图。

图6基于弹跳特性改进的A*算法最优路线和序列路径点结果图

图7未优化序列路径点弹跳轨迹的碰撞概率

图8基于碰撞概率优化后的序列路径点和运动路线图

图9基于碰撞概率优化后序列路径点弹跳轨迹的碰撞概率

图10基于碰撞概率优化后的三维地形上的连续弹跳移动轨迹

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。

如图1所示，本实施例公开的复杂小天体表面移动序列轨迹规划方法，具体实现步骤如下：

步骤一、针对探测器在小天体表面弹跳移动时的轨迹规划问题，建立探测器的轨道动力学方程。

探测器表面移动的参考坐标系的定义如图2所示，小天体固连坐标系O_f-X_fY_fZ_f原点定义在目标小天体中心。Z_f轴为目标天体的最大转动惯量轴，X_f轴为目标天体的最小转动惯量轴，Y_f轴满足右手坐标系。小天体表面坐标系的X_sY_s平面为当地近似平面，该平面通过表面地形表征方法确定，弹跳移动起点为坐标系原点O_s，弹跳移动起点指向目标点的方向矢量在该平面的投影为X_s轴正向，Z_s轴正向为该平面的外法向方向，Y_s轴满足右手坐标系。

在小天体固连坐标系下，探测器的轨道动力学方程为：

其中：r_f为探测器在小天体固连坐标系下的位置矢量；v_f为探测器在小天体固连坐标系下的速度矢量；ω为小天体的自旋角速度矢量；g为目标小天体的引力加速度矢量。

小天体固连坐标系和小天体表面坐标系下探测器位置与速度的坐标转换关系为：

其中：r_s为探测器在小天体表面坐标系下的位置矢量；v_s为探测器在小天体表面坐标系下的速度矢量；

为从小天体表面坐标系到小天体固连坐标系的坐标转换矩阵；l为小天体固连坐标系原点到小天体表面坐标系原点的位置矢量。

由式(14)(15)可得小天体表面坐标系下探测器的轨道动力学方程为：

步骤二、考虑小天体表面复杂地形条件，基于最小平方中值方法对三维地形进行表征，确定可行路径点区域，并得到具有特征化地形的当地近似平面。

构建小天体表面复杂地形的三维模型如图3所示。根据弹跳移动区域地形数字高程数据，可以将表面地形表达为在平面上分布的凸起区与凹陷区。通过最小平方中值方法对地形进行表征，并根据弹跳移动特性进行分类。

地形表征算法主要包括两部分任务：拟合当地近似平面即确定平面方程和识别不同地形(岩石和陨石坑)，其原理示意图如图4所示。算法流程如下：

1)随机选取N个表面地形样本点中三个不共线的点(x_a,x_b,x_c)，确定平面n·X+d＝0，其中n＝(x_b-x_a)×(x_c-x_a)，d＝-n·x_a。

2)利用1)中得到的平面，计算每个样本点x_i相对该平面的残值r_i＝(n·x_i+d)²，选取其中值。

3)重复步骤1)和2)m次，m＝ln(1-P)/ln(1-(1-o)³)；其中P为期望的概率，o为平面外点占总点数的百分比的统计数；得到m个中值。

4)从步骤3)得到的m个中值中选取最小值r_med，其对应的平面记为最小平方中值平面(n_best,d_best)，并将其视为当地近似平面。

5)计算每个样本点相对当地近似平面的残值，若满足

其中

则该样本点位于当地近似平面外，否则，该样本点位于当地近似平面内。

6)对位于当地近似平面的样本点，利用最小二乘法确定当地近似平面方程，该平面即为小天体表面坐标系的X_sY_s平面。在平面以上的为岩石和崎岖山地，在平面以下的为陨石坑。

在当地近似平面上，用最小外接圆表示不同地形障碍区域。根据地形障碍区域的平面尺寸即最小外接圆的直径D，考虑探测器的弹跳能力即单次弹跳的最远距离d_max，将地形障碍区域分为两类。如果D＞d_max，则为不可跳跃区域，否则为可跳跃区域。同时，地形障碍区域以外的其它区域为可行路径点区域，进而得到具有特征化地形的当地近似平面。

假设单次弹跳可到达的最远距离为d_max＝50m，表征后的特征地形平面如图5所示。深灰色圆形区域为岩石、山地等地形障碍，浅灰色区域为陨石坑等低洼地形，其它区域为近似平面地形。同时具有黑色边框的圆形区域是不可跳跃区域，其它圆形区域是可跳跃区域。路径规划时路线不能通过不可跳跃区域，且路径点不能位于圆形地形区域内。

步骤三、根据步骤二得到的特征化地形平面，采用基于弹跳移动特性改进的A*路径搜索算法得到距离最优的运动路线。然后根据弹跳移动特点和多次等分弹跳策略确定可行连续路径点。

首先，将得到的特征化地形平面网格化，网格单元大小为5m。然后采用A*搜索算法进行最优移动路线规划，并根据弹跳运动特性改进最优路线。起点A的平面坐标为[2.5,7.5]，终点B的平面坐标为[392.5,392.5]，该算法流程如下：

1)将起点A加入开放列表，开启列表是一个等待检查网格点的列表。

2)寻找A周围的网格中点，将其中可行或可到达的点加入开放列表，并将A设置为这些点的父节点，同时将A从开放列表移除，加入到封闭列表中。关闭列表中存放的为不需要再次检查的网格点。

3)从开放列表的点中选出一个距离最优点，该点的选取用到估价函数：

F(n)＝G(n)+H(n) (19)

其中，G(n)表示从起点A移动到相应子点的移动耗费，可沿着对角线方向移动。H(n)表示从指定点移动到终点B的预计耗费。由此得到具有最小F(n)值A的最优子点A1，将其从开放列表移除并加入封闭列表中；

4)对上一步得到的最优子点A1进行操作，获取其可到达的子点并加入到开放列表中，计算相应的G(n)和F(n)值，如果有子点已经存在开放列表中，那么检查经由A1点到达该子点的新路径的G(n)值是否会更小，如果新的G(n)值更小，则将A1设置为该点的父节点，重新计算G(n)和F(n)值，否则不做任何操作，由此可得A1的最优子点A2；

5)重复步骤4，直到终点B成为最优子点；

6)从终点B开始向着各点的父节点移动直到回到起点A，即得到传统A*算法下的最优移动路线。

7)根据弹跳运动特性对最优路线上短距离内路线出现多次转向的情况进行改进。路线改进的基本原则为：在保证路线不经过不可跳跃区域的情况下，根据弹跳探测器可朝任意方向移动的特点将局部路线转向次数减到最小，同时保持路线长度不变或缩短路线长度。

弹跳探测器的运动路径由一系列连续路径点组成，则其路径规划问题即为序列路径点的规划问题。通过路径规划算法获得最优运动路线后，需要由此最优路线确定可行路径点序列。给出如下路径点序列确定策略：

1)确定路径点：起点和终点。

2)转向路径点：路线上所有位于圆形地形区域外的转向点。

3)越障路径点：在最优路线通过可跳跃障碍区域时，选取位于区域外的两个路径点；所述两个路径点分别为通过障碍区域前和通过障碍区域后最优路线上距离该障碍区域最近的一个路径点。

4)路径点序列：将以上路径点，按起点到终点的顺序组成路径点序列。

若步骤1)～3)确定的路径点中存在相邻两路径点间的距离大于d_max，则增加等分中间路径点。此中间路径点的选取按照多次等分弹跳策略。

由传统A*算法得到的最优路线和改进的局部路线以及序列路径点如图5所示。

步骤四、在小天体表面坐标系下，考虑动力学模型参数不确定性和初始状态误差的影响，给出探测器在两相邻路径点间单次弹跳轨迹的性能指标，优化得到最优弹跳轨迹，进而得到序列路径点的连续弹跳轨迹。

将步骤三得到的在平面地形上规划的序列路径点投射到其三维地形图上得到对应的三维路径点。对于三维地形上两相邻路径点的单次弹跳运动，在小天体表面坐标系下探测器的轨道动力学方程可表示为参数的函数形式的状态方程：

其中x(t)表示t时刻的状态矢量，x₀＝[r₀,v₀]表示初始状态矢量。

设各初始状态误差和参数不确定性符合均值为零的高斯分布，则可根据其协方差信息和通过敏感性分析方法得到的敏感度矩阵计算终端状态x(t_f)的线性协方差矩阵：

其中，φ(t_f,0)为从初始时刻0到终端时刻t_f的状态转移矩阵，α_i表示任一参数，α_i＝ω,g，

为敏感度矩阵，k为表示参数的总数。

给出性能指标如下：

minJ＝c·E(v₀)+trace(C) (22)

其中c为权重系数，E(v₀)表示获得初始速度的能耗，

表示终端位置误差的协方差矩阵。

通过优化性能指标J，可生成综合考虑能量消耗和终点位置误差的单次弹跳最优轨迹。对于多个路径点的弹跳轨迹规划，探测器从起始点获得初始速度后进入轨道运动状态并朝下个相邻路径点运动，到达目标路径点时停在表面，下次弹跳从该点获得初始速度并朝该点下个相邻路径点运动。由此，序列路径点的连续弹跳轨迹可由多个单次弹跳最优轨迹组成。

步骤五、基于步骤四规划的连续弹跳轨迹，对生成的每个单次弹跳最优轨迹，考虑探测器的位置不确定性的影响，计算探测器与地形障碍的碰撞概率即探测器与所有可能碰撞的障碍的碰撞概率之和。如果该碰撞概率超过给定的阈值，则按照路径点分解置换策略对局部路径点进行重规划，得到考虑碰撞概率的优化路径点序列，进而得到优化的连续弹跳移动轨迹。既能改善弹跳移动的位置精度，又可提高探测器在复杂地形移动的安全性。

综合考虑小天体表面地形障碍的位置、大小与可能的误差，将表面的地形障碍如石块、山丘等建模为半椭球形障碍区。以椭球的球心r_c＝[x_c,y_c,z_c]和三个半轴长度a,b,c对障碍的位置和大小进行描述。则探测器与任一障碍的碰撞概率可表示为：

其中，r_c为障碍区中心位置，

为探测器的名义位置，V＝2πabc/3表示障碍区的体积。计算碰撞概率时，近似将C看作常数矩阵。

探测器与多个障碍的碰撞概率为探测器与每个障碍碰撞概率的加和：

其中m为障碍数量。

基于步骤四得到的连续弹跳轨迹，计算得到的碰撞概率结果如图7所示。

如果探测器与地形障碍的碰撞概率P_ct为1，则表示探测器的运动轨迹通过障碍区域，即探测器与地形障碍碰撞。同时，探测器距离障碍越近，碰撞概率越大。因此，考虑探测器位置和地形障碍的不确定性，为了保证探测器的安全，探测器的轨迹需要和地形障碍保持一定距离。当单次弹跳轨迹上的碰撞概率P_ct大于给定阈值时，在地形平面上对其路径点进行分解置换，并且对序列路径点进行重规划。基于碰撞概率的路径点优化策略如下：

1)将碰撞概率P_ct大于给定阈值时距离弹跳轨迹最近的障碍所对应的圆形区域进行拓展，即增加圆的半径R。此时探测器的名义位置

与其距离最近的障碍的最大碰撞概率为：

若考虑将最大碰撞概率

降低为原来的λ倍，λ∈(0,1)，并保证降低后P_ct小于给定阈值，则此时探测器的名义位置

与其距离最近的障碍的最大碰撞概率为：

因此，可将圆形区域的半径近似增大

2)增大后的圆形区域为新的障碍区，用新的越障路径点置换新障碍区内的原路径点；当两个新越障路径点间的距离大于d_max时，不能采用跳跃的方式通过障碍，此时采用路径点分解策略得到一个或多个分解中间路径点，分解中间路径点个数由分解后的碰撞概率决定。

3)对于位于扩展的障碍地形区外的其它路径点，如果新越障路径点与其相邻的路径点之间的距离小于探测器的最小弹跳距离d_min时，省略该路径点，其余路径点保持不变，并重新规划需要的等分中间路径点。

4)综上，得到优化后的序列路径点，进而得到优化的连续弹跳移动轨迹。

假设ΔR＝10m，d_min＝10m，可得到优化后的序列路径点和运动路线如图8所示，其中虚线圆为扩展后的地形障碍区域。优化后的序列路径点弹跳轨迹的碰撞概率如图9所示。对比图7和图9可以看出，优化后的碰撞概率显著降低，从而实现对地形障碍的规避，提高探测器弹跳移动的安全性。探测器在复杂地形表面的连续弹跳移动轨迹如图10所示。

通过以上步骤确定的复杂小天体表面移动序列轨迹规划方法，可提高弹跳移动探测器在存在动力学模型参数不确定性和初始状态误差时的终点位置精度，同时降低探测器与地形障碍的碰撞概率，提高移动的安全性。

以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.复杂小天体表面移动序列轨迹规划方法，其特征在于：包括如下步骤：

步骤一、针对探测器在小天体表面弹跳移动时的轨迹规划问题，建立探测器的轨道动力学方程；

步骤二、考虑小天体表面复杂地形条件，基于最小平方中值方法对三维地形进行表征，确定可行路径点区域，并得到具有特征化地形的当地近似平面；

步骤三、根据步骤二得到的特征化地形平面，采用基于弹跳移动特性改进的A*路径搜索算法得到距离最优的运动路线；然后根据弹跳移动特点和多次等分弹跳策略确定可行连续路径点；

步骤四、在小天体表面坐标系下，考虑动力学模型参数不确定性和初始状态误差的影响，给出探测器在两相邻路径点间单次弹跳轨迹的性能指标，优化得到最优弹跳轨迹，进而得到序列路径点的连续弹跳轨迹；

步骤五、基于步骤四规划的连续弹跳轨迹，对生成的每个单次弹跳最优轨迹，考虑探测器的位置不确定性的影响，计算探测器与地形障碍的碰撞概率即探测器与所有可能碰撞的障碍的碰撞概率之和，如果该碰撞概率超过给定的阈值，则按照路径点分解置换策略对局部路径点进行重规划，得到考虑碰撞概率的优化序列路径点，进而得到优化的连续弹跳移动轨迹；既能改善弹跳移动的位置精度，又能提高探测器在复杂地形移动的安全性。

2.如权利要求1所述的复杂小天体表面移动序列轨迹规划方法，其特征在于：步骤一的具体实现方法为：

首先定义描述探测器弹跳运动的参考坐标系；小天体固连坐标系O_f-X_fY_fZ_f的原点定义在目标小天体中心，Z_f轴为目标天体的最大转动惯量轴，X_f轴为目标天体的最小转动惯量轴，Y_f轴满足右手坐标系；在研究小天体表面移动探测器的轨迹规划问题时，需要获取小天体表面的地形信息和探测器相对表面的运动状态，因此定义小天体表面坐标系O_s-X_sY_sZ_s；将当地近似平面作为小天体表面坐标系的X_sY_s平面，该平面通过表面地形表征方法确定；弹跳移动起点作为坐标系原点O_s，弹跳移动起点指向目标点的方向矢量在当地近似平面的投影为X_s轴正向，Z_s轴正向为当地近似平面的外法向方向，Y_s轴满足右手坐标系；

在小天体固连坐标系下，探测器的轨道动力学方程为：

其中，r_f为探测器在小天体固连坐标系下的位置矢量，v_f为探测器在小天体固连坐标系下的速度矢量，ω为小天体的自旋角速度矢量，g为目标小天体的引力加速度矢量；

小天体固连坐标系和小天体表面坐标系下探测器位置与速度的坐标转换关系为：

其中：r_s为探测器在小天体表面坐标系下的位置矢量；v_s为探测器在小天体表面坐标系下的速度矢量；

为从小天体表面坐标系到小天体固连坐标系的坐标转换矩阵；l为小天体固连坐标系原点到小天体表面坐标系原点的位置矢量；

由式(1)(2)可得小天体表面坐标系下探测器的轨道动力学方程为：

3.如权利要求1所述的复杂小天体表面移动序列轨迹规划方法，其特征在于：步骤二的具体实现方法为：

小天体表面分布着不同大小的陨石坑、崎岖山地、以及岩石块，这种复杂的地形条件对探测器的表面移动构成了一定的威胁；根据弹跳移动区域的地形三维高程数据，将表面地形表示为在平面上分布的凸起区与凹陷区；通过最小平方中值方法对地形进行表征，并根据弹跳移动特性进行分类；

地形表征算法主要包括两部分任务：拟合当地近似平面即确定平面方程；识别不同地形；算法流程如下：

1)随机选取N个表面地形样本点中三个不共线的点(x_a,x_b,x_c)，确定平面的方程为n·X+d＝0；其中，n表示平面法向量，n＝(x_b-x_a)×(x_c-x_a)，d＝-n·x_a；

2)利用1)中得到的平面，计算每个样本点x_i相对该平面的残值r_i＝(n·x_i+d)²，选取其中值；

3)重复步骤1)和2)m次，m＝ln(1-P)/ln(1-(1-o)³)；其中P为期望的概率，o为平面外点占总点数的百分比的统计数；得到m个中值；

4)从步骤3)得到的m个中值中选取最小值r_med，其对应的平面记为最小平方中值平面(n_best,d_best)，并将其视为当地近似平面；

5)计算每个样本点相对当地近似平面的残值，若满足

其中

则该样本点位于当地近似平面外，否则，该样本点位于当地近似平面内；

6)对位于当地近似平面的样本点，利用最小二乘法确定当地近似平面方程，该平面即为小天体表面坐标系的X_sY_s平面；在平面以上的为凸起地形障碍，在平面以下的为凹陷地形障碍；

在当地近似平面上，用最小外接圆表示不同地形障碍区域；根据地形障碍区域的平面尺寸即最小外接圆的直径D，考虑探测器的弹跳能力即单次弹跳的最远距离d_max，将地形障碍区域分为两类；如果D＞d_max，则为不可跳跃区域，否则为可跳跃区域；同时，地形障碍区域以外的其它区域为可行路径点区域，进而得到具有特征化地形的当地近似平面。

4.如权利要求1所述的复杂小天体表面移动序列轨迹规划方法，其特征在于：步骤三的具体实现方法为：

首先，将得到的特征化地形二维平面网格化，网格单元大小根据平面尺寸和计算量大小确定；然后采用A*搜索算法进行最优移动路线规划，并根据弹跳运动特性对最优路线上短距离内路线出现多次转向的情况进行改进；路线改进的基本原则为：在保证路线不经过不可跳跃区域的情况下，根据弹跳探测器可朝任意方向移动的特点将局部路线转向次数减到最小，同时保证路线长度不变或缩短路线长度，得到最优运动路线；

在最优运动路线上确定可行路径点序列，路径点选择网格中点或顶点。

5.如权利要求4所述的复杂小天体表面移动序列轨迹规划方法，其特征在于：所述路径点序列确定方法如下：

1)确定路径点：起点和终点；

2)转向路径点：路线上所有位于圆形地形障碍区域外的转向点；

3)越障路径点：在最优路线通过可跳跃障碍区域时，选取位于区域外的两个路径点；所述两个路径点分别为通过障碍区域前和通过障碍区域后最优路线上距离该障碍区域最近的一个路径点；

4)路径点序列：将以上路径点，按起点到终点的顺序组成路径点序列。

6.如权利要求5所述的复杂小天体表面移动序列轨迹规划方法，其特征在于：若步骤1)～3)确定的路径点中存在相邻两路径点间的距离大于d_max，则增加等分中间路径点；此中间路径点的选取按照多次等分弹跳策略。

7.如权利要求1所述的复杂小天体表面移动序列轨迹规划方法，其特征在于：步骤四的具体实现方法为：

将步骤三得到的在平面地形上规划的序列路径点投射到其三维地形图上，得到对应的三维路径点；对于三维地形上两相邻路径点的单次弹跳运动，在小天体表面坐标下探测器的轨道动力学方程可表示为参数的函数形式的状态方程：

其中x(t)表示t时刻的状态矢量，x₀＝[r₀,v₀]表示初始状态矢量；

设各初始状态误差和参数不确定性符合均值为零的高斯分布，则可根据其协方差信息和通过敏感性分析方法得到的敏感度矩阵计算终端状态x(t_f)的线性协方差矩阵：

其中，φ(t_f,0)为从初始时刻0到终端时刻t_f的状态转移矩阵，α_i表示任一参数，α_i＝ω,g，

为敏感度矩阵，k为表示参数的总数；

给出性能指标如下：

min J＝c·E(v₀)+trace(C) (9)

其中c为权重系数，E(v₀)表示获得初始速度的能耗，C＝cov(r_tf)表示终端位置误差的协方差矩阵；

通过优化性能指标J，生成综合考虑能量消耗和终点位置误差的单次弹跳最优轨迹；对于多个路径点的弹跳轨迹规划，探测器从起始点获得初始速度后进入轨道运动状态并朝下个相邻路径点运动，到达目标路径点时停在表面，下次弹跳从该点获得初始速度并朝该点下个相邻路径点运动；由此，序列路径点的连续弹跳轨迹可由多个单次弹跳最优轨迹组成。

8.如权利要求1所述的复杂小天体表面移动序列轨迹规划方法，其特征在于：步骤五的具体实现方法为：

综合考虑小天体表面地形障碍的位置、大小与可能的误差，将表面的地形障碍如石块、山地等建模为半椭球形障碍区；以椭球的球心r_c＝[x_c,y_c,z_c]和三个半轴长度a,b,c对障碍的位置和大小进行描述；则探测器与任一障碍的碰撞概率表示为：

其中，r_c为障碍区中心位置，

为探测器的名义位置，V＝2πabc/3表示障碍区的体积；计算碰撞概率时，近似将C看作常数矩阵；

探测器与多个障碍的碰撞概率为探测器与每个障碍碰撞概率的加和：

其中m为障碍数量；

如果探测器与地形障碍的碰撞概率P_ct为1，则表示探测器的运动轨迹穿过障碍区域，即探测器与地形障碍碰撞；同时，探测器距离障碍越近，碰撞概率越大；因此，考虑探测器位置和地形障碍的不确定性，为了保证探测器的安全，探测器的轨迹需要和地形障碍保持一定距离；当单次弹跳轨迹上的碰撞概率P_ct大于给定阈值时，在地形平面上对其路径点进行分解置换，并且对序列路径点进行重规划。

9.如权利要求8所述的复杂小天体表面移动序列轨迹规划方法，其特征在于：所述基于碰撞概率的路径点优化策略如下：

1)将碰撞概率P_ct大于给定阈值时距离弹跳轨迹最近的障碍所对应的圆形区域进行拓展，即增加圆的半径R；此时探测器的名义位置

与其距离最近的障碍的最大碰撞概率为：

若考虑将最大碰撞概率

降低为原来的λ倍，λ∈(0,1)，并保证降低后P_ct小于给定阈值，则此时探测器的名义位置

与其距离最近的障碍的最大碰撞概率为：

因此，可将圆形区域的半径近似增大

2)增大后的圆形区域为新的障碍区，用新的越障路径点置换新障碍区内的原路径点；当两个新越障路径点间的距离大于d_max时，不能采用跳跃的方式通过障碍，此时采用路径点分解策略得到一个或多个分解中间路径点，分解中间路径点个数由分解后的碰撞概率决定；

3)对于位于扩展的障碍地形区外的其它路径点，如果新越障路径点与其相邻的路径点之间的距离小于探测器的最小弹跳距离d_min时，省略该路径点，其余路径点保持不变，并重新规划需要的等分中间路径点；

4)综上，得到优化后的序列路径点，进而得到优化的连续弹跳移动轨迹；

通过以上步骤确定的复杂小天体表面移动序列轨迹规划方法，可提高弹跳移动探测器在存在动力学模型参数不确定性和初始状态误差时的终点位置精度，同时降低探测器与地形障碍的碰撞概率，提高移动的安全性。

Images