一种非一致约束下多节点着陆器协同避障决策方法

1.本发明属于深空探测领域，特别涉及一种非一致约束下多节点着陆器协同避障(决策)方法。


背景技术：

2.现有的专利大多数是针对单节点着陆器的着陆避障规划，对于多节点的着陆器，除了考虑整体避障避碰外，还需要考虑节点间如何避碰避障的问题。


技术实现要素：

3.本发明提供非一致约束下多节点着陆器协同避障方法，旨在至少解决现有技术中存在的技术问题之一。本发明面向一种刚柔耦合的多节点着陆器，提出了该着陆器中多个节点如何相互协同进行避障路径规划，使着陆器具有自主避险和定点附着的能力。
4.本发明的技术方案涉及一种非一致约束下多节点着陆器协同避障方法，所述多节点着陆器包括柔性体、设置在所述柔性体内部的三个刚性节点，三个刚性节点的中心为虚拟中心，以所述虚拟中心为原点建立虚拟中心坐标系所述方法包括以下步骤：
5.s100、控制多节点着陆器下降到预设的高度，获取星表的环境地图；
6.s200、计算所述多节点着陆器虚拟中心的轨迹δv(t)和各个节点的位置pi(t)；
7.s300、基于所述多节点着陆器的构型势场、避障势场和避碰势场等柔性变换约束，确定各节点相对所述虚拟中心的实时距离ri(t)，计算各节点自身的轨迹δi(t)；
8.s400、控制各节点跟踪所述自身的轨迹δi(t)实现协同避障。
9.进一步，所述节点在所述虚拟中心坐标系坐标系的相对位置表示为：
10.{r1(t)，r2(t)，r3(t)}，
11.其中，t为时间；
12.节点i的在世界坐标系中的位置表示为：
13.pi(t)＝pv(t)+rv(t)ri(t)，
14.其中，pv(t)∈r3和rv(t)∈so3表示虚拟中心坐标系的原点在世界星体下的位置和姿态。
15.进一步，步骤s200包括：
16.s210、基于虚拟中心避障势场，采用关键航点和插值方法，计算所述虚拟中心实时航点{k1...kn}的位置pv(t)和偏航角ψv(t)的平坦输出；
17.s220、计算节点的位置pi(t)及其四阶导数，
[0018][0019]
其中，ri表示在虚拟中心坐标系下，每个节点相对于虚拟中心的实时距离，pi表示节点i在星体坐标系下的位置，偏航角ψi(t)及其四阶导数与虚拟中心的偏航角一致。
[0020]
进一步，步骤s300包括，所述多节点着陆器的构型势场为：
[0021][0022]
其中，a为节点构型参数，为节点的期望位置，r为节点的实际位置，i表示第i个所述节点。
[0023]
进一步，步骤s300包括，所述多节点着陆器的避障势场包括虚拟中心避障势场和节点避障势场。
[0024]
进一步，所述虚拟中心避障势场中由障碍物生成的向量场的整体排斥向量为：
[0025][0026]
其中，为从障碍物位置指向虚拟中心的排斥向量，其大小为与障碍物的位置和半径以及虚拟中心的朝向有关的类高斯函数，k为环境中障碍物的个数，为障碍物k在全局坐标系中的水平位置，rk表示障碍物半径，表示虚拟中心的朝向，
[0027]
其中，
[0028][0029]
其中，bk是一个与障碍物k的半径rk相关的参数，∑是一个2x2的正定矩阵，
[0030][0031]
其中，是单位矢量的垂直方向，σ1，σ2是协方差。
[0032]
进一步，所述节点避障势场为：
[0033][0034]
其中，ck是一个与障碍物k的半径rk相关的参数。
[0035]
进一步，步骤s300还包括，在虚拟中心坐标系下，所述节点的避碰势场为：
[0036][0037]
其中，表示节点i的相邻点。
[0038]
本发明还提出一种面向小行星的多节点着陆器，用于实现上述的非一致约束下多节点着陆器协同避障方法，所述多节点着陆器包括：
[0039]
柔性体；
[0040]
节点，所述节点设置在所述柔性体内部，所述节点为刚性节点，所述节点的个数为三个，所述节点的分布不在同一条直线上，每个所述节点包括节点推力器，所述节点推力器包括用于补偿小行星弱引力的朝上推力器和用于所述节点的平动、姿态调整以及驱动所述柔性体进行柔性变换的朝下推力器，所述节点上还包括宽窄视场相机、激光雷达、imu等感知设备和载荷；
[0041]
总体控制系统，用于控制所述节点和所述柔性体运动。
[0042]
本发明还提出一种计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施如上述的方法。
[0043]
与现有的技术相比，本发明具有以下的特点。
[0044]
本发明面向一种刚柔耦合的多节点着陆器，提出了该着陆器中多个节点如何相互协同进行避障路径规划，使着陆器具有自主避险和定点附着的能力；本方法赋予未来的小行星探测器，在执行着陆和样本返回任务时，具备能够稳健准确的附着在具有较高科学价值的着陆点，能够应对各种危险底线，如陡峭的坡面，狭长的山谷等。
附图说明
[0045]
图1为非一致约束下多节点着陆器协同避障方法的流程图；
[0046]
图2为非一致约束下多节点着陆器协同避障方法的示意图；
[0047]
图3为非一致约束下多节点着陆器协同避障方法中虚拟中心避障势场的示意图；
[0048]
图4为非一致约束下多节点着陆器协同避障方法中节点避障势场的示意图；
[0049]
图5为非一致约束下多节点着陆器协同避障方法中着陆器着陆场景示意图；
[0050]
图6为非一致约束下多节点着陆器协同避障方法中柔性变换过程。
具体实施方式
[0051]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0052]
以下将结合实施例和附图对本发明的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本发明的目的、方案和效果。
[0053]
需要说明的是，如无特殊说明，当某一特征被称为“固定”、“连接”在另一个特征，它可以直接固定、连接在另一个特征上，也可以间接地固定、连接在另一个特征上。本文所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。此外，除非另有定义，本文所使用的所有的技术和科学术语与本技术领域的技术人员通常理解的含义相同。本文说明书中所使用的术语只是为了描述具体的实施例，而不是为了限制本发明。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的组合。
[0054]
应当理解，尽管在本公开可能采用术语第一、第二、第三等来描述各种元件，但这些元件不应限于这些术语。这些术语仅用来将同一类型的元件彼此区分开。例如，在不脱离本公开范围的情况下，第一元件也可以被称为第二元件，类似地，第二元件也可以被称为第
一元件。本文所提供的任何以及所有实例或示例性语言(“例如”、“如”等)的使用仅意图更好地说明本发明的实施例，并且除非另外要求，否则不会对本发明的范围施加限制。此外，本文所采用的行业术语“位姿”是指某个元件相对于空间坐标系的位置和姿态。
[0055]
参照图1至图5，本发明实施例提供了一种非一致约束下多节点着陆器协同避障(决策)方法，所述多节点着陆器包括柔性体、设置在所述柔性体内部的三个刚性节点，三个刚性节点的中心为虚拟中心，以所述虚拟中心为原点建立虚拟中心坐标系所述方法包括以下步骤：
[0056]
s100、控制多节点着陆器下降到预设的高度，获取星表的环境地图；
[0057]
s200、计算所述多节点着陆器虚拟中心的轨迹δv(t)和各个节点的位置pi(t)；
[0058]
s300、基于所述多节点着陆器的构型势场、避障势场和避碰势场等柔性变换约束，确定各节点相对所述虚拟中心的实时距离ri(t)，计算各节点自身的轨迹δi(t)；
[0059]
s400、控制各节点跟踪所述自身的轨迹δi(t)实现协同避障。
[0060]
本发明面向一种刚柔耦合的多节点着陆器，提出了该着陆器中多个节点如何相互协同进行避障路径规划，使着陆器具有自主避险和定点附着的能力；本方法赋予未来的小行星探测器，在执行着陆和样本返回任务时，具备能够稳健准确的附着在具有较高科学价值的着陆点，能够应对各种危险底线，如陡峭的坡面，狭长的山谷等。
[0061]
为了说明新型探测器多节点在避障时的轨迹规划问题，请参照图5，未来的小行星着陆和样本返回任务将寻找具有较高科学价值的着陆点，如危险地形，这意味着航天器需要具有自主规避危险的能力，因此定义了防撞区。参考“防碰区”的概念设计了所示的研究场景，在距离星表仅有20米时，探测器为了到达目标着陆点，需要避开星体表面两处“防碰区”(实体障碍物或无光照条件的禁飞区)。为展示新型探测器的柔性变换过程，特意设置两“防碰区”之间留有一定的空隙可供探测器穿过。
[0062]
所述非一致约束为对多节点着陆器而言，所述多节点着陆器包括柔性体和设置在所述柔性体内部的三个刚性节点，其内部各个节点不仅要避开星体上的岩石，还要防止节点间距过近或过远而造成的柔性材料的过度挤压与拉伸，由于环境约束和着陆器自身柔性约束不一致的情况，称为非一致约束。
[0063]
参照图2，首先构建三个节点的虚拟中心，通过所述虚拟中心将多节点着陆器抽象为一个整体，为其内部多节点的表征提供了一种简单、自然的方式。三个节点在运动过程中，受到所述虚拟中心在速度和位置上的“管控”。假设探测器在距离星表很近时获得星表详尽的环境地图，在环境地图中，首先生成所述虚拟中心的轨迹δv(t)，然后将该轨迹与所有节点通信，并以节点的构型势场、避障势场、避碰势场等为柔性变换约束，使各节点解算出自身的轨迹δi(t)，最后通过控制各节点跟踪自身的轨迹δi(t)实现协同避障。
[0064]
进一步，所述节点在所述虚拟中心坐标系坐标系的相对位置表示为：
[0065]
{r1(t)，r2(t)，r3(t)}，
[0066]
其中，t为时间；
[0067]
节点i的在世界坐标系和的位置表示为：
[0068]
pi(t)＝pv(t)+rv(t)ri(t)，
[0069]
其中，pv(t)∈r3和rv(t)∈so3表示虚拟中心坐标系的原点在世界星体下的位置和姿态。
[0070]
进一步，步骤s200包括：
[0071]
s210、基于虚拟中心避障势场，采用关键航点和插值方法，计算所述虚拟中心实时航点{k1...kn}的位置pv(t)和偏航角ψv(t)的平坦输出；
[0072]
在避障策略中，最重要的环节是多节点轨迹协同规划，由于所述虚拟中心是虚拟的对象，没有相关动力学约束，因此采用关键航点和插值方法生成虚拟中心避障的轨迹，其轨迹只需要确定虚拟中心实时航点{k1...kn}的位置pv(t)和偏航角ψv(t)的平坦输出，虚拟中心实时航点{k1...kn}的位置pv(t)和偏航角ψv(t)的平坦输出可通过所述虚拟中心避障势场解算而来。
[0073]
s220、计算节点的位置pi(t)及其四阶导数，
[0074][0075]
其中，ri表示在虚拟中心坐标系下，每个节点相对于虚拟中心的实时距离，pi表示节点i在星体坐标系下的位置，偏航角ψi(t)及其四阶导数与虚拟中心的偏航角一致。
[0076]
节点是具体物理实体，具有相关动力学约束，节点轨迹必须确定的给出每个状态量，如位置、速度、加速度、欧拉角和欧拉角速率等。节点的实时位置和偏航角由虚拟中心轨迹δv(t)(虚拟中心位置pv(t)和偏航角ψv(t))和节点避碰、避障、构型势场共同解算确定。其中所设计的各种势场主要确定各节点相对虚拟中心的实时距离ri(t)，即探测器的柔性变换量。在确定好节点的位置pi(t)和偏航角ψi(t)及其四阶导数后，则其余状态量可利用微分平坦性理论求得，最后在航点之间插值即可求得各个节点的轨迹δi(t)。
[0077]
请参照图6，柔性变换指三个节点距离虚拟中心的相对位置是时变的{r1(t)，r2(t)，r3(t)}。以此实现柔性体的展开、变形等动作。
[0078]
进一步，步骤s300包括，所述多节点着陆器的构型势场为：
[0079][0080]
其中，a为节点构型参数，为节点的期望位置，r为节点的实际位置，i表示第i个所述节点。
[0081]
节点在柔性体内部只能进行小范围相对运动，为了稳定柔性体的构型，每个节点在虚拟中心坐标系下生成一个向量场，这样当节点偏离其预期位置时，一个强迫势场将作用于节点，将其推回预期位置。由于引入了虚拟中心坐标系这样就不用考虑节点在全局坐标系中的机动，只考虑它们在虚拟中心坐标系下虚拟位置。
[0082]
进一步，步骤s300包括，所述多节点着陆器的避障势场包括虚拟中心避障势场和节点避障势场。
[0083]
为了有较好的避障效果，当感知到障碍物后首先应降低整个着陆器的运动速度，其次要保证各个节点之间避障速度协调性。因此避障势场主要有两部分：首先是虚拟中心避障势场，请参照图3，为环境中每个障碍物生成势场，当虚拟中心运动速度指向该障碍物时，该势场会迫使虚拟中心的速度降低，从而使各个节点在遇到障碍物时降速缓冲；其次是
节点避障势场，请参照图4，为环境中每个障碍生成另一个势场，由于在行进过程中各个节点距与障碍物的距离不一致，在避障时各个节点的速度应相互协调，已经避开障碍物的节点“等待”还未避开障碍物的节点，从而保证节点间距合适不违反柔性连接约束。
[0084]
进一步，所述虚拟中心避障势场中由障碍物生成的向量场的整体排斥向量为：
[0085][0086]
其中，为从障碍物位置指向虚拟中心的排斥向量，其大小为与障碍物的位置和半径以及虚拟中心的朝向有关的类高斯函数，k为环境中障碍物的个数，为障碍物k在全局坐标系中的水平位置，rk表示障碍物半径，表示虚拟中心的朝向，
[0087]
其中，
[0088][0089]
其中，bk是一个与障碍物k的半径rk相关的参数，∑是一个2x2的正定矩阵，选择bk的值，使虚拟中心的速度降速。由于虚拟中心非真实存在，确定了动态类高斯函数的主要和次要轴，
[0090][0091]
其中，是单位矢量的垂直方向，σ1，σ2是协方差。
[0092]
定义一个从障碍物位置指向虚拟中心的排斥向量p，其大小为与障碍物的位置和半径以及虚拟中心的朝向有关的类高斯函数。假设环境中有k个障碍物，用表示障碍物k在全局坐标系中的水平位置，那么在水平坐标处由k个障碍物生成的向量场的整体排斥向量为
[0093]
因此，如果虚拟中心直接朝向障碍物，它将会受到一个很大的排斥力，防止虚拟中心直接撞向障碍物。
[0094]
进一步，所述节点避障势场为：
[0095][0096]
其中，ck是一个与障碍物k的半径rk相关的参数。ck的值必须选得足够大，以确保避免碰撞。
[0097]
节点避障势场与虚拟中心避障势场类似，但节点需要更强的“排斥力”，这样当节点太靠近障碍物时，排斥矢量的力量就足以将节点“推开”。
[0098]
进一步，步骤s300还包括，在虚拟中心坐标系下，所述节点的避碰势场为：
[0099][0100]
其中，表示节点i的相邻点。
[0101]
除了避障外，还需要考虑节点之间的避碰，可以类比上述避障势场，将节点j视为节点i的障碍物，从而实现节点间的相互避碰。
[0102]
本发明还提出一种面向小行星的多节点着陆器，用于实现上述的非一致约束下多
节点着陆器协同避障(决策)方法，所述多节点着陆器包括：
[0103]
柔性体；
[0104]
节点，所述节点设置在所述柔性体内部，所述节点为刚性节点，所述节点的个数为三个，所述节点的分布不在同一条直线上，每个所述节点包括节点推力器，所述节点推力器包括用于补偿小行星弱引力的朝上推力器和用于所述节点的平动、姿态调整以及驱动所述柔性体进行柔性变换的朝下推力器，所述节点上还包括宽窄视场相机、激光雷达、imu等感知设备和载荷；
[0105]
总体控制系统，用于控制所述节点和所述柔性体运动。
[0106]
本发明还提出一种计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施如上述的方法。
[0107]
应当认识到，本发明实施例中的方法步骤可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机系统通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。
[0108]
此外，可按任何合适的顺序来执行本文描述的过程的操作，除非本文另外指示或以其他方式明显地与上下文矛盾。本文描述的过程(或变型和/或其组合)可在配置有可执行指令的一个或多个计算机系统的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。
[0109]
进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、ram、rom等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本文所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还可以包括计算机本身。
[0110]
计算机程序能够应用于输入数据以执行本文所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。
[0111]
以上所述，只是本发明的较佳实施例而已，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。在本发明的保护范围内其技术方案和/或实施方式可以有各种不同的修改和变化。

技术特征：
1.一种非一致约束下多节点着陆器协同避障方法，其特征在于，所述多节点着陆器包括柔性体、设置在所述柔性体内部的三个刚性节点，三个刚性节点的中心为虚拟中心，以所述虚拟中心为原点建立虚拟中心坐标系所述方法包括以下步骤：s100、控制多节点着陆器下降到预设的高度，获取星表的环境地图；s200、计算所述多节点着陆器虚拟中心的轨迹δ
v
(t)和各个节点的位置p
i
(t)；s300、基于所述多节点着陆器的构型势场、避障势场和避碰势场等柔性变换约束，确定各节点相对所述虚拟中心的实时距离r
i
(t)，计算各节点自身的轨迹δ
i
(t)；s400、控制各节点跟踪所述自身的轨迹δ
i
(t)实现协同避障。2.根据权利要求1所述的非一致约束下多节点着陆器协同避障方法，其特征在于，所述节点在所述虚拟中心坐标系坐标系的相对位置表示为：{r1(t)，r2(t)，r3(t)}，其中，t为时间；节点i的在世界坐标系中的位置表示为：p
i
(t)＝p
v
(t)+r
v
(t)r
i
(t)，其中，p
v
(t)∈r3和r
v
(t)∈so3表示虚拟中心坐标系的原点在世界星体下的位置和姿态。3.根据权利要求1所述的非一致约束下多节点着陆器协同避障方法，其特征在于，步骤s200包括：s210、基于虚拟中心避障势场，采用关键航点和插值方法，计算所述虚拟中心实时航点{k1...k
n
}的位置p
v
(t)和偏航角ψ
v
(t)的平坦输出；s220、计算节点的位置p
i
(t)及其四阶导数，其中，r
i
表示在虚拟中心坐标系下，每个节点相对于虚拟中心的实时距离，p
i
表示节点i在星体坐标系下的位置，偏航角ψ
i
(t)及其四阶导数与虚拟中心的偏航角一致。4.根据权利要求1所述的非一致约束下多节点着陆器协同避障方法，其特征在于，步骤s300包括，所述多节点着陆器的构型势场为：其中，a为节点构型参数，为节点的期望位置，r为节点的实际位置，i表示第i个所述节点。5.根据权利要求1所述的非一致约束下多节点着陆器协同避障方法，其特征在于，步骤s300包括，所述多节点着陆器的避障势场包括虚拟中心避障势场和节点避障势场。6.根据权利要求4所述的非一致约束下多节点着陆器协同避障方法，其特征在于，所述虚拟中心避障势场中由障碍物生成的向量场的整体排斥向量为：
其中，为从障碍物位置指向虚拟中心的排斥向量，其大小为与障碍物的位置和半径以及虚拟中心的朝向有关的类高斯函数，k为环境中障碍物的个数，为障碍物k在全局坐标系中的水平位置，r
k
表示障碍物半径，表示虚拟中心的朝向，其中，其中，b
k
是一个与障碍物k的半径r
k
相关的参数，∑是一个2x2的正定矩阵，其中，是单位矢量的垂直方向，σ1，σ2是协方差。7.根据权利要求4所述的非一致约束下多节点着陆器协同避障方法，其特征在于，所述节点避障势场为：其中，c
k
是一个与障碍物k的半径r
k
相关的参数。8.根据权利要求1所述的非一致约束下多节点着陆器协同避障方法，其特征在于，步骤s300还包括，在虚拟中心坐标系下，所述节点的避碰势场为：其中，表示节点i的相邻点。9.一种面向小行星的多节点着陆器，用于实现如权利要求1至8任一项所述的非一致约束下多节点着陆器协同避障方法，其特征在于，所述多节点着陆器包括：柔性体；节点，所述节点设置在所述柔性体内部，所述节点为刚性节点，所述节点的个数为三个，所述节点的分布不在同一条直线上，每个所述节点包括节点推力器，所述节点推力器包括用于补偿小行星弱引力的朝上推力器和用于所述节点的平动、姿态调整以及驱动所述柔性体进行柔性变换的朝下推力器，所述节点上还包括宽窄视场相机、激光雷达、imu感知设备和载荷；总体控制系统，用于控制所述节点和所述柔性体运动。10.一种计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施如权利要求1至8中任一项所述的方法。

技术总结
本发明涉及一种非一致约束下多节点着陆器协同避障方法，其中方法包括：控制多节点着陆器下降到预设的高度，获取星表的环境地图；计算所述多节点着陆器虚拟中心的轨迹和各个节点的位置；基于所述多节点着陆器的构型势场、避障势场和避碰势场等柔性变换约束，确定各节点相对所述虚拟中心的实时距离，计算各节点自身的轨迹；控制各节点跟踪所述自身的轨迹实现协同避障。本方法赋予未来的小行星探测器，在执行着陆和样本返回任务时，具备能够稳健准确的附着在具有较高科学价值的着陆点，能够应对各种危险底线，如陡峭的坡面，狭长的山谷等。谷等。谷等。


技术研发人员：梅杰 柴敬轩 马广富 吴伟仁
受保护的技术使用者：哈尔滨工业大学（深圳）
技术研发日：2022.12.29
技术公布日：2023/4/18