CN113650015B - 一种月面采样机械臂动态任务规划方法 - Google Patents

Abstract

一种月面采样机械臂动态任务规划方法，步骤如下：1)任务规划：①着陆区确定；②建立环境模型；③确定障碍空间；④确定机械臂的自由空间；⑤确定采样区；⑥确定采样点；⑦决定采样作业过程；2)采样策略规划：①初始化过程；②表取采样过程；③放样过程；④制定指令计划；3)运动控制规划：①建立运动学模型；②路径规划算法的范围；③确定任务的起始、目标点；④确定路径规划算法；⑤明确动态指令的参数。本发明的优点：实现对采样任务过程的动态灵活调整；解决月面表取采样任务中的动态时序约束、时变空间约束和采样机械臂运动轨迹规划问题，实现对月面采样任务遥控指令计划的快速生成。三个层级相互配合以满足月面表取采样任务动态规划需求。

Description

一种月面采样机械臂动态任务规划方法

技术领域

本发明属于控制系统或其部件的检验技术领域，涉及一种月面采样机械臂动态任务规划方法，国际专利分类号为：G05B 23/00。

背景技术

月面采样机械臂作为月面表取采样的执行机构，承担着获取月壤样品并将其放置到样品封装容器的复杂任务。月面采样机械臂通过表取方式获取月表土壤样品的过程受到月面重力、月面复杂地形、着陆姿态和机械臂变形等多种不确定因素影响，而这些因素只有在探测器着陆月面后才能获取与辨识且难以提取预知。为保证在有限的月面采样时间内完成表取采样任务，对月面采样各类事件和机械臂动作行为的实时动态规划，是顺利完成任务的关键。

针对此问题，国内外学者从任务规划的角度开展了大量的研究。其中最典型的一类是STRIPS方法该方法建立了规范的任务规划模型，提出了利用逻辑命题推理处理复杂规划的思路，在合理限制真实任务规划环境和设置假设的条件下，实现了问题的规划求解。Blum等提出了图规划算法，期望通过简洁的搜索方法来解决一般性规划问题，但囿于图搜索算法的复杂度，该方法对复杂规划的求解效率不高。Bonet等基于启发式搜索提出了图规划算法改进算法，利用启发式算法快速搜索状态空间以提高规划求解效率。由于该方法需要定义启发式函数，启发式函数的计算复杂度会影响求解效率。

发明内容

本发明的目的在于克服现有技术中存在的任务规划灵活性不高的不足之处，而提出了一种月面采样机械臂动态任务规划方法，该方法将机械臂采样任务规划划分为任务整体规划、采样策略规划和运动控制规划等三个层级，将复杂月面采样作业任务规划问题分解为空间约束、时序推理和运动控制三个维度分别求解，三个层级相互配合以满足月面表取采样任务动态规划需求。

本发明的目的是这样实现的：

一种月面采样机械臂动态任务规划方法，该方法包以下步骤：

第一步：任务整体规划：

①着陆区的确定：输入已知着陆区条件，着陆区的确定不属于本发明的范畴，着陆区确定后，进行任务整体规划；

②建立环境模型：将着陆区环境划分为三个部分，其中，只对采样区建立环境模型，为后续的采样区分析做准备；具体的，通过相机采集月面地形的图像，通过三维地形重构技术获得三维点云环境模型；

③月面三维地形与着陆器外包络取并集确定障碍空间；

④月面采样机械臂关节运动范围内的运动空间与非障碍空间取交集确定月面采样机械臂的自由空间；

⑤月面采样机械臂运动空间与月面三维地形取交集确定月面表取采样区；

⑥由任务目标确定采样点：

⑦由自由空间和采样点决定空间约束的月面采样作业过程；

第二步：采样策略规划，月面采样作业细化为：

①初始化过程：设备加电和机械臂展开；

②表取采样过程：机械臂执行月面表取采样的过程；

③放样过程：机械臂将采集到的月表样品放置到密封容器，同时观察样品放置的情况；

④通过时间约束来考虑空间和时间的月面采样作业过程，并制定遥控指令计划；

第三步：运动控制规划：

①建立机械臂正/逆运动学模型，月面采样机械臂是安装在月面着陆器上的四自由度串联机械臂，由腰关节、肩关节、大臂、肘关节、小臂、腕关节和末端采样执行器组成；

②路径规划算法包含运动范围约束、障碍物约束和控制模式约束；

③当前构型与目标状态动态变化，当前构型与目标状态分别为任务的起始点和目标点；

④机械臂正/逆运动学模型和当前构型与目标状态动态变化决定路径规划算法；

⑤将上述控制模式、控制参数、初始状态、目标状态作为遥控指令计划中动态指令的参数。

第一步中所说的采样区和采样点，对采样区环境的分析和对采样点优选如下：

①采样区环境的分析：

a.地形计算：地形平坦度反应了月面采样点周围某一尺度范围内的起伏程度,通过主成分分析法求解月面离散点云的拟合平面法方向和在此方向上的分散度；

b.确定地形平坦度：拟合平面法方向对应的分散度；

c.确定地形坡度：拟合平面与月面地平面的夹角；

d.确定地形坡向:通过拟合平面法方向进行表征。

第二步中所说的过程初始化过程、表取采样过程和放样过程，其中：初始化过程包括设备加电事件和机械臂展开事件；表取采样过程包括机械臂触月事件和机械臂采样事件；放样过程包括机械臂放样事件和样品观察事件；在保持时序逻辑不变的情况下，根据触月、采样、放样和样品观察的实时状态，动态调整机械臂操作过程或事件的次数和参数。

第二步中所说的设备加电：完成控制单元、电路、电机设备的开机和状态设置。

第二步中所说的机械臂展开：机械臂由压紧位置运行至机械臂展开构型。

本发明的有益效果：

本发明将机械臂采样任务规划划分为任务整体规划、采样策略规划和运动控制规划等三个层级，其中任务整体规划层是表取采样任务规划系统的最高层次，主要解决采样区可采性评估与采样点选取问题，分析采样区内各采样点的可达性、平坦性、安全性、通视性等指标，实施采样点选取并初步计算采样作业的可行性；采样策略规划层是实现动态任务规划能力的关键层次，主要解决采样相关机构、装置、系统的协同性作业时序排列问题，提出基于改进Allen 模型的时态规划方法，实现对采样任务过程的动态灵活调整；运动控制规划层是表取采样任务规划系统的实施层次，主要解决采样机械臂的运动过程分解与轨迹规划问题，基于最短实时避障路径的运动规划方法，实现对操作路径的快速高效规划。三个层级相互配合以满足月面表取采样任务动态规划需求。

附图说明

本发明共有17幅附图，其中图1可做为说明书摘要的附图。

图1.为月面表取采样任务规划系统框图；

图2.采样区空间约束关系示意图；

图3.采样区分析示意图；

图4.机械臂-地形距离的计算结果示意图；

图5.月面表取采样任务网络示意图；

图6.采样策略流程示意图；

图7.月面采样机械臂简图；

图8.月面采样机械臂关节坐标系及连杆参数示意图；

图9.末端采样执行器与着陆器上表面夹角示意图；

图10.采样机械臂从初始构型展开过程的末端轨迹示意图；

图11.采样机械臂从初始构型展开过程的关节轨迹示意图；

图12.采样机械臂从展开构型到采样中间点的末端轨迹示意图；

图13.采样机械臂从展开构型到采样中间点的关节轨迹示意图；

图14.采样机械臂从采样中间点到采样点的末端轨迹示意图；

图15.采样机械臂从采样中间点到采样点的关节轨迹示意图；

图16.Allen模型定义的13种时序关系；

图17.改进的Allen模型新增的4种时序关系。

具体实施方式

下面结合附图对本发明的实施例做进一步描述。

如图1所示，在着陆区确定后，将机械臂采样任务规划划分为任务整体规划、采样策略规划和运动控制规划等三个层级，其中任务整体规划层是表取采样任务规划系统的最高层次，主要解决采样区可采性评估与采样点选取问题，需要综合考虑任务目标、测控资源、月面地形地貌及机械臂运动能力等因素，分析采样区内各采样点的可达性、平坦性、安全性、通视性等指标，实施采样点选取并初步计算采样作业的可行性；采样策略规划层是实现动态任务规划能力的关键层次，主要解决采样相关机构、装置、系统的协同性作业时序排列问题，需要对月面采样作业过程为一系列任务事件及其相互关系进行细化分析，考虑事件间的逻辑关系和时间约束关系，建立复杂任务的递归分解与任务事件的动态调整机制，生成指令计划；运动控制规划层是表取采样任务规划系统的实施层次，主要解决采样机械臂的运动过程分解与轨迹规划问题，需要考虑机械臂运动范围、障碍物及控制模式等约束条件对作业过程进行分段，进而规划每一段作业过程的机械臂运动轨迹和控制策略，生成运动控制参数，控制月面采样机械臂执行采样任务。三个层级相互配合以满足月面表取采样任务动态规划需求。

第一步：任务整体规划：

月面三维地形与着陆器外包络取并集确定障碍空间；月面采样机械臂关节运动范围内的运动空间与非障碍空间取交集确定月面采样机械臂的自由空间；月面采样机械臂运动空间与月面三维地形取交集确定月面表取采样区。空间约束关系示意图如图2所示。

第二步：采样策略规划：

对于图1中的采样区分析及采样点优选进行分析，如图3所示，地形计算：地形平坦度反应了月面采样点周围某一尺度范围内(局部邻域)的起伏程度，可通过主成分分析(PCA)法求解月面离散点云的拟合平面法方向和在此方向上的分散度，即为主成分分析中的最小特征向量和对应的特征值，求解方法如下：

给定点集P＝{p₁,p₂,…,p_n}，设点p_i的最近k邻域为Nb(p_i)，

为点p_i的重心，则点p_i的协方差矩阵定义为：

其中，协方差矩阵C奇异值分解可分解成3个特征向量：v₂,v₁,v₀，其对应的特征值分别为λ₂,λ₁,λ₀(λ₂≥λ₁≥λ₀)，上述最小特征值λ₀即为地形平坦度，对应的特征向量v₀即可表示为拟合平面的法向量，即为地形坡度与地形坡向的描述向量。

采样位置可达性：采样位置可达区域是月面采样机械臂采样器可到达的月表区域，即采样器甲打开90°状态下的末端采样点可达空间与月面三维地形的交集。

采样安全性评估，即周围碰撞风险分析：采样安全性反映了采样点处机械臂与周围环境发生碰撞的风险程度。通过局部邻域地形平坦度λ₀(及坡度与坡向描述向量)、机械臂连杆与着陆器最小距离S_r、采样器与月面地形的最小距离S_t，综合评判采样安全性，即：

其中，

和

分别表示机械臂各部件相距着陆器和月面地形的距离阈值；

表示采样安全阈值。当S∈[0,1]时表示采样安全，且S越大采样安全性越高；当S＝-1 时表示采样不安全。采样安全性的计算中，局部邻域的选择是安全性评估有效的关键。局部邻域定义为采样器的最小包围盒组成的区域，如图4所示。在实际采样应用中，根据采样器跟地形的不同抵近关系和操作安全性要求，选用矩形邻域、多边形邻域和采样点矩形邻域作为局部邻域。在月面地形平坦度计算中通常选用矩形区域，在机械臂末端圆盘触月碰撞风险评估时选用多边形邻域，在采样铲采样适合度评估中选用采样点矩形邻域。

采样通视距离(路径复杂度)：是指采样中间点到地形上投影点的距离d₀与投影点到采样点的测地距离d_s之和，即为d₀+d_s，如图4所示。测地距离d_s考虑了采样点与中间点下方投影点的距离和地形起伏对机械臂路径规划的影响，特别是对于有较大地形起伏的情况，通视距离增大，反映了障碍背后点避障路径规划和实施采样的代价增大，当到达一定阈值后不适合作为采样点9。采样通视距离的计算示例结果如图4，图中圆点代表采样中间点8，中间点8下方点是采样路径代价最小的点，距离越远，通视距离越大，路径代价也就越大。

以采样过程为例，月面采样作业过程可描述为如图5所示的任务网络。

该网络中，p₁到p₁₆为状态单元，τ₁到τ₁₉为变迁单元，各个状态单元和变迁单元的含义分别如表1和表2所示。

表1任务网络中状态单元的含义

表2任务网络中变迁单元的含义

将采样中间点p₁、放样上方点p₄、放样精调起始点p₁₀、放样后抬升点p₁₃、观测精调起始点p₁₄和观测后抬升点p₁₆定义月面采样机械臂操作的节点状态单元，节点状态单元之间的转移与月面环境的关联性较小，可以根据着陆器外包罗空间约束预先规划获得。将触月、采样、放样、样品观察定义为动态状态单元，从节点状态单元到动态状态单元的过程与月面环境、机械臂变形等因素密切相关，需要在任务实施过程中实施动态确定，即以月面地形约束下的机械臂自由运动空间为空间约束条件、以视觉观测定位得到的机械臂当前状态为初始状态，规划机械臂在节点状态单元与动态可变状态单元转换的关节构型或末端位姿状态。

以采样过程为例，月面采样作业过程可描述为如图5所示的任务网络。

在采样策略规划层，采用递归分解机制将复杂作业任务逐级分解为“[作业]-[过程]-[事件]”，从而形成整个月面采样作业的时序逻辑的准确描述方法。具体的，月面采样作业可以被细化为过程初始化过程、表取采样过程和放样过程。其中，初始化过程包括设备加电事件和机械臂展开事件；表取采样过程包括机械臂触月事件和机械臂采样事件；放样过程包括机械臂放样事件和样品观察事件。在保持时序逻辑不变的情况下，根据触月、采样、放样和样品观察的实时状态，动态调整机械臂操作过程或事件的次数和参数。

月面采样过程事件是具有一定持续时间的有序指令集合，其执行过程是月面采样机械臂执行一系列有序的指令集合从初始状态转换到目标状态的过程。如图6所示，以“[事件]-[指令序列：参数]-[指令：参数]”作为采样策略规划的分支控制模式，通过设置指令序列参数和指令参数，控制事件中指令序列或指令组合调用关系和动作过程的调整变化。一方面形成不同的有序指令集合，实现对事件的动态调整；另一方面形成不同的动作轨迹，实现对操作过程的动态调整。

为了解决上述动态调整问题，提出基于改进Allen模型的时态规划方法。 Allen模型定义了基于时间区间的时间约束描述方法，将事件之间的时序逻辑通过时间区间之间的时序逻辑表达，将任意两个事件之间的关系定义为13种可能表达，如图16所示。

以事件I₁＝[t_1s,t_1e]和I₂＝[t_2s,t_2e]为例，事件间时序关系的矩阵表示为：

其中，M(I_i,I_j)表示事件间的时序关系矩阵，满足M(I_j,I_i)＝-M(I_i,I_j)^T；M_p(t_i,t_j)表示时刻的关系，当t_i＜t_j时表示时刻t_i先于时刻t_j且M_p(t_i,t_j)＝-1，当t_i＝t_j时表示时刻t_i同步于时刻t_j且M_p(t_i,t_j)＝0，当t_i＞t_j时表示时刻t_i晚于时刻t_j且M_p(t_i,t_j)＝1；t_1s、t_1e、t_2s和t_2e分别表示事件I₁的起止时刻和事件I₂的起止时刻。

Allen模型虽然能够比较准确地描述总数固定的一系列事件的时序逻辑，但时序关系矩阵为定性描述，事件间的时序约束不强，且部分事件增加或删除会导致后续事件时序逻辑的全部重新调整，难以适应采样任务快速动态调整的要求。为此本节引入定量时序关系矩阵和虚拟事件对Allen模型进行改进，适应中间过程的灵活动态调整要求，描述时间约束关系。

定义事件I₁＝[t_1s,t_1s+Δt₁]和I₂＝[t_2s，t_2s+Δt₂]间的定量时序关系矩阵表示为：

其中，MR(I_i,I_j)表示事件间的定量时序关系矩阵，与时序关系矩阵满足M(I_i,I_j)＝sgn[MR(I_i,I_j)]，且满足MR(I_j,I_i)＝-MR(I_i,I_j)^T；M_r(t_i,t_j)表示事件开始时刻的定量关系，且M_r(t_i,t_j)＝t_i-t_j；Δt₁和Δt₂分别为事件I₁和事件I₂的持续时间。

当另一个事件I₃与事件I₂间的定量时序关系矩阵MR(I₂,I₃)已知，则事件I₁与事件I₃的时序关系满足M(I₁,I₃)＝sgn[MR(I₁,I₃)]，有

其中，^ijMR表示定量时序关系矩阵第i行第j列的元素。

为了实现对动态过程的自适应调整要求，在事件设计中引入虚拟事件，虚拟事件仅对有新增事件需求的时序关系进行占位处理，不影响已知事件间时序关系。虚拟事件与已知事件序列满足原有的13种时序关系，并与新增事件间预设4种时序关系，如图16、图17所示。新增事件加入后，无需调整原有事件序列的时序关系，仅建立新增事件与虚拟事件间的时序逻辑，即可实现新增事件的临时加入和实时更换。具体的，在Allen模型中，N_old个事件中加入N_new个新增事件则增加的时序关系数量为

在改进的Allen模型中，增加的时序关系数量为N_new，有

成立。因此，改进的Allen模型在适应任务动态调整方面有明显的优势。

以一系列事件{I₁,I₀，I₃}为例，各个事件的持续时间分别Δt₁、Δt₀和Δt₃，且事件间的定量时序关系矩阵为MR(I₁，I₀)、MR(I₀，I₃)和MR(I₁，I₃)。由于事件I₀是虚拟事件，则{I₁,I₀,I₃}等价于{I₁,I₃}，输出的时序关系为M(I₁,I₃)＝sgn[MR(I₁,I₃)]。此时，在事件I₁与事件I₃之间增加事件I₂仅需建立虚拟事件I₀与事件I₂的定量时序关系矩阵 MR(I₀,I₂)，则得到{I₁,I₂,I₃}输出的时序关系

按照上述时序关系，将事件进行逐级展开为有序指令集合，如图7所示，即生成机械臂操作控制的指令计划。令事件的指令描述为

其中，px和pl分别表示指令序列参数集和指令参数集；^(·)l_{i_m}表示事件i的第m个指令，指令左上角标表示执行该指令输入的指令参数，为空则表示该指令默认执行，例如，I_i({px₁,px_j},{pl₁})＝{l_{i_1},l_{i_k},l_{i_(m+1)}}。

将有序指令集合按照事件间的时序逻辑组合，例如I₁＝{l_{1_1},l_{1_4}}，I₂＝{l_{2_2},l_{2_3}}， I₃＝{l_{3_1},l_{3_3}}，则I₁{m}I₂{m}I₃＝{l_{1_1},l_{1_4},l_{2_2},l_{2_3},l_{3_1},l_{3_3}}，从而最终得到机械臂月面采样操作控制的指令计划。

第三步：运动控制规划

月面采样机械臂是安装在月面着陆器上的四自由度串联机械臂，由腰关节1、肩关节2、大臂3、肘关节4、小臂5、腕关节6和末端采样执行器7组成，如图7所示。其中，大臂与肩关节固连，小臂与肘关节固连，末端采样执行器与腕关节固连。月面采样机械臂通过4个关节的转动带动末端采样执行器到达期望采样点，实现对土壤样品的铲取或挖掘。月面采样机械臂的结构设计非常紧凑，使其在收缩状态下，大臂、小臂和末端采样执行器可处于一个平面。月面采样机械臂关节坐标系及连杆参数定义如图8所示。基于DH (Denavit-Hartenberg)方法的月面采样机械臂连杆参数如表3所示。

表3月面采样机械臂连杆参数表

具体的，α_i(rad)表示沿X_i轴，从Z_i轴旋转到Z_i+1轴的角度；a_i(m)表示沿X_i轴，从Z_i轴移动到Z_i+1轴的距离；θ_i(rad)表示沿Z_i轴，从X_i-1轴旋转到X_i轴的角度；d_i(m) 表示沿Z_i轴，从X_i-1轴移动到X_i轴的距离。

根据DH参数建立采样机械臂各连杆变换矩阵，进而计算月面采样机械臂关节空间到末端笛卡尔空间的正运动学变换矩阵模型，表示为：

具体的，

其中，s_i表示sin(θ_i)；c_i表示cos(θ_i)；s₂₃表示sin(θ₂+θ₃)；c₂₃表示cos(θ₂+θ₃)；s₂₃₄表示sin(θ₂+θ₃+θ₄)；c₂₃₄表示cos(θ₂+θ₃+θ₄)。

月面采样机械臂关节空间的关节角构型θ＝[θ₁,θ₂,θ₃,θ₄]^T能够唯一确定末端笛卡尔空间位姿p＝[p_x,p_y,p_z,φ]^T。其中，φ表示末端采样执行器与月面着陆器上表面的夹角，φ＝θ₂+θ₃+θ₄，如图9所示。经求解，月面采样机械臂末端笛卡尔空间到关节空间的逆运动学可表示为：

其中，D＝d₂+d₃+d₄，

P₂＝p_z-d₁，

A₂＝a₂+a₃c₃，A₃＝a₂c₂+a₃c₂₃。

上述逆运动学公式将得到不只一组关节角度组合。针对逆运动学存在的多解情况，采用最小变化角度的启发式策略：

其中，

表示逆运动学公式得到的第j组解中关节i的角度；θ_i表示关节i的参考关节角度，通常取前一运动状态的关节i角度值。

采样任务开始后，月面采样机械臂首先执行展开动作到达采样中间点，之后执行触月、采样、放样、样品观察等动作开展采样作业。机械臂的运动路径规划采取了以作业过程关键点为目标的转移路径实时规划策略，从而实现机械臂大范围转移运动趋势的固化和小范围最短实时避障路径规划，得到运动控制参数并最终形成机械臂运动指令。

采样任务中的关键点集χ_p＝{x₁,x₂,…,x_n}需满足无碰撞自由空间χ_f的约束条件，即

具体的，令χ_e表示机械臂末端运动空间，χ_c表示着陆器最大外包络空间，χ_m表示月表最大外包络空间。机械臂运动空间内的障碍空间χ_obs和自由空间χ_f可分别表示为χ_obs＝χ_e∩(x_c∪x_m)和χ_f＝χ_e-χ_obs。

月面采样机械臂在关键点间转移，运动规划算法采用了基于梯形速度曲线法。转移过程中，着陆器可动部件运动及月面环境不确定因素带来约束空间的实时变化。定义χ_obs(t)为t时刻的障碍空间；χ_arm(t)为t时刻的机械臂臂杆外包络空间。则对于路径规划问题(x_init,θ_init,x_tar,χ_obs(t))，月面采样机械臂最短路径规划满足

关节运动行程最小

s.t.x(t)＝FK[θ(t)] 运动学约束

x(t)∈[χ_e-χ_obs(t)] 末端自由空间约束

臂杆碰撞规避

θ_imin≤θ_i≤θ_imax,i＝1,2,3,4 关节角度约束

x(0)＝x_init,x(t_end)＝x_tar 边界条件 (12)

其中，通过关节角度约束描述机械臂运动范围约束；x_init和x_tar分别表示起始点和目标点，且x_init，x_tar∈χ_p；FK(·)表示机械臂从关节空间到笛卡尔空间的正运动学方程。上述路径规划问题可通过搜索方法求解。

在一种可能实现的方式中，A^*算法是典型的启发式搜索方法，被广泛应用于最优路径的求解，其核心在于估价函数的设计。在选择当前节点的下一个评估节点时引入估价函数f(x)

f(x)＝g(x)+h(x)

式中，g(x)是从起始点到节点x最小代价路径的实际代价；h(x)是从节点x到目标点路径的估计代价。

为便于利用A*算法，定义采样机械臂的4个关节角度为一个四维数组，起始关节角度为

当前关节角度为

第i步最佳关节角度为

关节角度搜索步长为

第i步的后继节点关节角度为

(由关节角度搜索步长确定)，期望关节角度为

定义估价函数

其中，q_{i m}为第i步机械臂关节角度中的第m个关节的关节角度，q_{des m}为机械臂期望关节角度中的第m个关节的关节角度。由此可以求出第i步后继节点关节角度中f_i(q)值最小的节点作为此步最优关节角度，根据步长生成第i+1步的后继节点关节角度，重复此过程可保证每一步都可以得到最优关节角度，因此通过该估价函数可以实现采样机械臂从起始关节角度到目标关节角度的构型空间最优路径规划。

为了便于分析，设定3个表，即OPEN表、CLOSE表和后继节点表。把起始点q_ini放入OPEN表，初始化f₀(q)＝h₀(q)，置CLOSE表为空表。

重复下列过程，直到找到目标点为止。若OPEN表为空表，表明空间机械臂路径规划失败；否则，开始以下循环。

Step1选取OPEN表中未设置过的具有最小f值的节点为最佳节点q_{best_i}，并将其放入CLOSE表，同时将其从OPEN表中删除；

Step2判断q_{best_i}是否为目标点q_des，若是，则求解成功，循环结束；否则进行下一步；

Step3根据设定的步长l_step值，计算后继节点q_{suc_i}；

Step4按下述流程对每个后继节点q_{suc_i}进行处理：

a.判断q_{suc_i}是否在采样机械臂自由运动空间内，若是，建立从q_{suc_i}返回q_{best_i}的指针；否则转至Step3；

b.计算

c.判断q_{suc_i}是否在OPEN表中，若是转至d；否则转至e；

d.定义数组q_{old_i}＝q_{suc_i}，将q_{old_i}填到q_{best_i}的后继节点表中，进而比较新旧轨迹代价：若g(q_{old_i})>g(q_{suc_i})，更新q_{old_i}的父辈节点为q_{best_i}，并更新 g(q_{old_i})＝g(q_{suc_i})，进一步修正f(q_{old_i})值，同时对OPEN表重新排序；否则转至g；

e.判断q_{suc_i}是否在CLOSE表中，若是，转至d；否则转至f；

f.将q_{suc_i}放入OPEN表，并添入到q_{best_i}的后继表中，转至g；

g.计算f(q_{suc_i})，并转至a。

当前构型与目标状态动态变化、运动控制模式、不同运动控制模式的运动控制参数如下：

当前构型与目标状态分别为任务的起始点和目标点。以下面的例子进行说明采样机械臂关节角度约束和采样运动过程关键点分别如表4和表5所示。采样机械臂模拟月面采样过程。

表4采样机械臂关节角范围

表5采样机械臂运动过程关键点

运动控制模式包括关节空间规划模式和笛卡尔空间规划模式

关节空间规划模式是以关节角度的函数来描述采样机械臂的路径，进行路径规划。关节空间规划模式实现方法是，给定采样机械臂的起始节点位形和终止点位形，对关节进行梯形速度插值，从而得到关节路径。

例如，若规划路径起始节点的关节角θ₀，规划路径目标节点的关节角θ_f以及最大角速度

和最大角加速度

可采用如下梯形速度插值：

其中，θ(t)表示规划的路径；

表示路径加速段和减速段的加速度；t_c1表示路径的加速段的时长，

t_c2表示路径的匀速段的时长，

t_c3表示路径的减速段的时长，

t_f表示路径的时长，t_f＝t₁+t₂+t₃。

笛卡尔空间规划模式是在给定机械臂初始构型和目标末端位姿的基础上，对机械臂末端进行梯形速度插值，从实现机械臂从起始节点运动至到目标节点的过程。完成笛卡尔空间的路径规划并没有完成整个路径规划的工作，最终还要转换到关节空间的路径规划。笛卡尔空间路径规划实现方法是：首先用数学方法在笛卡尔空间中描述期望路径，计算出末端各插值点的位姿信息，然后通过逆运动学(公式10所示)求解得到关节的相应参数，从而得到目标轨迹。

采样机械臂首先从初始构型展开，运动控制模式为关节空间规划模式，关节空间规划模式的运动控制参数为：最大角速度为2°/s，最大角加速度为0.5°/s²。运动规划结果如图10和图11所示。经规划，采样机械臂从初始构型到展开构型的运动时长为24s。

采样机械臂的第二段运动从展开构型到采样中间点，采用笛卡尔空间规划模式，笛卡尔空间规划模式的运动控制参数为最大线速度和最大线加速度分别为0.02m/s和0.002m/s²，运动规划结果如图12和图13所示，月面采样机械臂完成从采样中间点到采样点所需的运动时长为83.085s。

采样机械臂的第三段运动从采样中间点到采样点，运动控制模式为笛卡尔空间规划模式，最大线速度和最大线加速度分别为0.02m/s和0.002m/s²。运动控制规划结果如图14和图15所示，月面采样机械臂完成从采样上方点到采样点所需的运动时长为120.905s。

将上述控制模式、控制参数、初始状态、目标状态作为遥控指令计划中动态指令的参数。

Claims (9)

1.一种月面采样机械臂动态任务规划方法，其特征在于：该方法包以下步骤：

第一步：任务整体规划：着陆区的确定：输入已知着陆区条件，着陆区确定后，进行任务整体规划；建立环境模型：将着陆区环境划分为三个部分，月面三维地形与着陆器外包络取并集确定障碍空间；月面采样机械臂关节运动范围内的运动空间与非障碍空间取交集确定月面采样机械臂的自由空间；月面采样机械臂运动空间与月面三维地形取交集确定月面表取采样区；其中，只对采样区建立环境模型，为后续的采样区分析做准备；具体的，通过相机采集月面地形的图像，通过三维地形重构技术获得三维点云环境模型；由任务目标确定采样点：由自由空间和采样点决定空间约束的月面采样作业过程；

第二步：采样策略规划，月面采样作业过程细化为：①初始化过程：设备加电和机械臂展开；②表取采样过程：机械臂执行月面表取采样的过程；③放样过程：机械臂将采集到的月表样品放置到密封容器，同时观察样品放置的情况；④通过时间约束来考虑空间和时间的月面采样作业过程，并制定遥控指令计划；

第三步：运动控制规划：①建立机械臂正/逆运动学模型，月面采样机械臂是安装在月面着陆器上的四自由度串联机械臂；②路径规划算法包含运动范围约束、障碍物约束和控制模式约束；③当前构型与目标状态动态变化，当前构型与目标状态分别为任务的起始点和目标点；④机械臂正/逆运动学模型和当前构型与目标状态动态变化决定路径规划算法；⑤将控制模式、控制参数、初始状态、目标状态作为遥控指令计划中动态指令的参数；第一步中所述的采样区和采样点，对采样区环境的分析如下：a.地形计算：地形平坦度反应了月面采样点周围某一尺度范围内的起伏程度,通过主成分分析法求解月面离散点云的拟合平面法方向和在此方向上的分散度；b.确定地形平坦度：拟合平面法方向对应的分散度；c.确定地形坡度：拟合平面与月面地平面的夹角；d.确定地形坡向:通过拟合平面法方向进行表征；

计算地形平坦度的方法为：

给定点集P＝{p1,p2,…,pn}，设点pi的最近k邻域为Nb(pi)，

为点pi的重心，则点pi的协方差矩阵定义为：

其中，协方差矩阵C奇异值分解可分解成3个特征向量：v2,v1,v0，其对应的特征值分别为λ2,λ1,λ0，λ2≥λ1≥λ0，最小特征值λ0即为地形平坦度，对应的特征向量v0即可表示为拟合平面的法向量，即为地形坡度与地形坡向的描述向量。

2.根据权利要求1所述的一种月面采样机械臂动态任务规划方法，其特征在于：第二步中所述的初始化过程包括设备加电事件和机械臂展开事件。

3.根据权利要求1所述的一种月面采样机械臂动态任务规划方法，其特征在于：第二步中所述的表取采样过程包括机械臂触月事件和机械臂采样事件。

4.根据权利要求1所述的一种月面采样机械臂动态任务规划方法，其特征在于：第二步中所述的放样过程包括机械臂放样事件和样品观察事件。

5.根据权利要求1所述的一种月面采样机械臂动态任务规划方法，其特征在于：所述的初始化过程、表取采样过程和放样过程在保持时序逻辑不变的情况下，根据触月、采样、放样和样品观察的实时状态，动态调整机械臂操作过程或事件的次数和参数。

6.根据权利要求1所述的一种月面采样机械臂动态任务规划方法，其特征在于：第二步中所述的设备加电：完成控制单元、电路、电机设备的开机和状态设置。

7.根据权利要求1所述的一种月面采样机械臂动态任务规划方法，其特征在于：第二步中所述的机械臂展开：机械臂由压紧位置运行至机械臂展开构型。

8.根据权利要求1所述的一种月面采样机械臂动态任务规划方法，其特征在于：所述的四自由度串联机械臂由腰关节、肩关节、大臂、肘关节、小臂、腕关节和末端采样执行器组成。

9.根据权利要求1所述的一种月面采样机械臂动态任务规划方法，其特征在于：所述的四自由度串联机械臂是由关节角度约束机械臂运动范围。

Images