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

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


背景技术：

2.月面采样机械臂作为月面表取采样的执行机构，承担着获取月壤样品并将 其放置到样品封装容器的复杂任务。月面采样机械臂通过表取方式获取月表土 壤样品的过程受到月面重力、月面复杂地形、着陆姿态和机械臂变形等多种不 确定因素影响，而这些因素只有在探测器着陆月面后才能获取与辨识且难以提 取预知。为保证在有限的月面采样时间内完成表取采样任务，对月面采样各类 事件和机械臂动作行为的实时动态规划，是顺利完成任务的关键。
3.针对此问题，国内外学者从任务规划的角度开展了大量的研究。其中最典 型的一类是strips方法该方法建立了规范的任务规划模型，提出了利用逻辑命 题推理处理复杂规划的思路，在合理限制真实任务规划环境和设置假设的条件 下，实现了问题的规划求解。blum等提出了图规划算法，期望通过简洁的搜索 方法来解决一般性规划问题，但囿于图搜索算法的复杂度，该方法对复杂规划 的求解效率不高。bonet等基于启发式搜索提出了图规划算法改进算法，利用启 发式算法快速搜索状态空间以提高规划求解效率。由于该方法需要定义启发式 函数，启发式函数的计算复杂度会影响求解效率。


技术实现要素：

4.本发明的目的在于克服现有技术中存在的任务规划灵活性不高的不足之处， 而提出了一种月面采样机械臂动态任务规划方法，该方法将机械臂采样任务规 划划分为任务整体规划、采样策略规划和运动控制规划等三个层级，将复杂月 面采样作业任务规划问题分解为空间约束、时序推理和运动控制三个维度分别 求解，三个层级相互配合以满足月面表取采样任务动态规划需求。
5.本发明的目的是这样实现的：
6.一种月面采样机械臂动态任务规划方法，该方法包以下步骤：
7.第一步：任务整体规划：
8.①
着陆区的确定：输入已知着陆区条件，着陆区的确定不属于本发明的范 畴，着陆区确定后，进行任务整体规划；
9.②
建立环境模型：将着陆区环境划分为三个部分，其中，只对采样区建立 环境模型，为后续的采样区分析做准备；具体的，通过相机采集月面地形的图 像，通过三维地形重构技术获得三维点云环境模型；
10.③
月面三维地形与着陆器外包络取并集确定障碍空间；
11.④
月面采样机械臂关节运动范围内的运动空间与非障碍空间取交集确定月 面采样机械臂的自由空间；
12.⑤
月面采样机械臂运动空间与月面三维地形取交集确定月面表取采样区；
13.⑥
由任务目标确定采样点：
14.⑦
由自由空间和采样点决定空间约束的月面采样作业过程；
15.第二步：采样策略规划，月面采样作业细化为：
16.①
初始化过程：设备加电和机械臂展开；
17.②
表取采样过程：机械臂执行月面表取采样的过程；
18.③
放样过程：机械臂将采集到的月表样品放置到密封容器，同时观察样品 放置的情况；
19.④
通过时间约束来考虑空间和时间的月面采样作业过程，并制定遥控指令 计划；
20.第三步：运动控制规划：
21.①
建立机械臂正/逆运动学模型，月面采样机械臂是安装在月面着陆器上的 四自由度串联机械臂，由腰关节、肩关节、大臂、肘关节、小臂、腕关节和末 端采样执行器组成；
22.②
路径规划算法包含运动范围约束、障碍物约束和控制模式约束；
23.③
当前构型与目标状态动态变化，当前构型与目标状态分别为任务的起始 点和目标点；
24.④
机械臂正/逆运动学模型和当前构型与目标状态动态变化决定路径规划 算法；
25.⑤
将上述控制模式、控制参数、初始状态、目标状态作为遥控指令计划中 动态指令的参数。
26.第一步中所说的采样区和采样点，对采样区环境的分析和对采样点优选如 下：
27.①
采样区环境的分析：
28.a.地形计算：地形平坦度反应了月面采样点周围某一尺度范围内的起伏程 度,通过主成分分析法求解月面离散点云的拟合平面法方向和在此方向上的分 散度；
29.b.确定地形平坦度：拟合平面法方向对应的分散度；
30.c.确定地形坡度：拟合平面与月面地平面的夹角；
31.d.确定地形坡向:通过拟合平面法方向进行表征。
32.第二步中所说的过程初始化过程、表取采样过程和放样过程，其中：初始 化过程包括设备加电事件和机械臂展开事件；表取采样过程包括机械臂触月事 件和机械臂采样事件；放样过程包括机械臂放样事件和样品观察事件；在保持 时序逻辑不变的情况下，根据触月、采样、放样和样品观察的实时状态，动态 调整机械臂操作过程或事件的次数和参数。
33.第二步中所说的设备加电：完成控制单元、电路、电机设备的开机和状态 设置。
34.第二步中所说的机械臂展开：机械臂由压紧位置运行至机械臂展开构型。
35.本发明的有益效果：
36.本发明将机械臂采样任务规划划分为任务整体规划、采样策略规划和运动 控制规划等三个层级，其中任务整体规划层是表取采样任务规划系统的最高层 次，主要解决采样区可采性评估与采样点选取问题，分析采样区内各采样点的 可达性、平坦性、安全性、通视性等指标，实施采样点选取并初步计算采样作 业的可行性；采样策略规划层是实现动态任务规划能力的关键层次，主要解决 采样相关机构、装置、系统的协同性作业时序排列问题，提出基于改进allen 模型的时态规划方法，实现对采样任务过程的动态灵活调整；运动
控制规划层 是表取采样任务规划系统的实施层次，主要解决采样机械臂的运动过程分解与 轨迹规划问题，基于最短实时避障路径的运动规划方法，实现对操作路径的快 速高效规划。三个层级相互配合以满足月面表取采样任务动态规划需求。
附图说明
37.本发明共有15幅附图，其中图1可做为说明书摘要的附图。
38.图1.为月面表取采样任务规划系统框图；
39.图2.采样区空间约束关系示意图；
40.图3.采样区分析示意图；
41.图4.机械臂
‑
地形距离的计算结果示意图；
42.图5.月面表取采样任务网络示意图；
43.图6.采样策略流程示意图；
44.图7.月面采样机械臂简图；
45.图8.月面采样机械臂关节坐标系及连杆参数示意图；
46.图9.末端采样执行器与着陆器上表面夹角示意图；
47.图10.采样机械臂从初始构型展开过程的末端轨迹示意图；
48.图11.采样机械臂从初始构型展开过程的关节轨迹示意图；
49.图12.采样机械臂从展开构型到采样中间点的末端轨迹示意图；
50.图13.采样机械臂从展开构型到采样中间点的关节轨迹示意图；
51.图14.采样机械臂从采样中间点到采样点的末端轨迹示意图；
52.图15.采样机械臂从采样中间点到采样点的关节轨迹示意图；
53.图16.allen模型定义的13种时序关系；
54.图17.改进的allen模型新增的4种时序关系。
具体实施方式
55.下面结合附图对本发明的实施例做进一步描述。
56.如图1所示，在着陆区确定后，将机械臂采样任务规划划分为任务整体规 划、采样策略规划和运动控制规划等三个层级，其中任务整体规划层是表取采 样任务规划系统的最高层次，主要解决采样区可采性评估与采样点选取问题， 需要综合考虑任务目标、测控资源、月面地形地貌及机械臂运动能力等因素， 分析采样区内各采样点的可达性、平坦性、安全性、通视性等指标，实施采样 点选取并初步计算采样作业的可行性；采样策略规划层是实现动态任务规划能 力的关键层次，主要解决采样相关机构、装置、系统的协同性作业时序排列问 题，需要对月面采样作业过程为一系列任务事件及其相互关系进行细化分析， 考虑事件间的逻辑关系和时间约束关系，建立复杂任务的递归分解与任务事件 的动态调整机制，生成指令计划；运动控制规划层是表取采样任务规划系统的 实施层次，主要解决采样机械臂的运动过程分解与轨迹规划问题，需要考虑机 械臂运动范围、障碍物及控制模式等约束条件对作业过程进行分段，进而规划 每一段作业过程的机械臂运动轨迹和控制策略，生成运动控制参数，控制月面 采样机械臂执行采样任务。三个层级相互配合以满足月面表取采样任务动态规 划需求。
57.第一步：任务整体规划：
58.月面三维地形与着陆器外包络取并集确定障碍空间；月面采样机械臂关节 运动范围内的运动空间与非障碍空间取交集确定月面采样机械臂的自由空间； 月面采样机械臂运动空间与月面三维地形取交集确定月面表取采样区。空间约 束关系示意图如图2所示。
59.第二步：采样策略规划：
60.对于图1中的采样区分析及采样点优选进行分析，如图3所示，地形计算： 地形平坦度反应了月面采样点周围某一尺度范围内(局部邻域)的起伏程度， 可通过主成分分析(pca)法求解月面离散点云的拟合平面法方向和在此方向上 的分散度，即为主成分分析中的最小特征向量和对应的特征值，求解方法如下：
61.给定点集p＝{p1,p2,
…
,p
n
}，设点p
i
的最近k邻域为nb(p
i
)，为点p
i
的 重心，则点p
i
的协方差矩阵定义为：
[0062][0063]
其中，协方差矩阵c奇异值分解可分解成3个特征向量：v2,v1,v0，其对应 的特征值分别为λ2,λ1,λ0(λ2≥λ1≥λ0)，上述最小特征值λ0即为地形平坦度，对应 的特征向量v0即可表示为拟合平面的法向量，即为地形坡度与地形坡向的描述向 量。
[0064]
采样位置可达性：采样位置可达区域是月面采样机械臂采样器可到达的月 表区域，即采样器甲打开90
°
状态下的末端采样点可达空间与月面三维地形的 交集。
[0065]
采样安全性评估，即周围碰撞风险分析：采样安全性反映了采样点处机械 臂与周围环境发生碰撞的风险程度。通过局部邻域地形平坦度λ0(及坡度与坡向 描述向量)、机械臂连杆与着陆器最小距离s
r
、采样器与月面地形的最小距离s
t
， 综合评判采样安全性，即：
[0066][0067]
其中，和分别表示机械臂各部件相距着陆器和月面地形的距离阈值；表示采样安全阈值。当s∈[0,1]时表示采样安全，且s越大采样安全性越高；当s＝
‑
1 时表示采样不安全。采样安全性的计算中，局部邻域的选择是安全性评估有效 的关键。局部邻域定义为采样器的最小包围盒组成的区域，如图4所示。在实 际采样应用中，根据采样器跟地形的不同抵近关系和操作安全性要求，选用矩 形邻域、多边形邻域和采样点矩形邻域作为局部邻域。在月面地形平坦度计算 中通常选用矩形区域，在机械臂末端圆盘触月碰撞风险评估时选用多边形邻域， 在采样铲采样适合度评估中选用采样点矩形邻域。
[0068]
采样通视距离(路径复杂度)：是指采样中间点到地形上投影点的距离d0与 投影点到采样点的测地距离d
s
之和，即为d0+d
s
，如图4所示。测地距离d
s
考虑了 采样点与中间点下方投影点的距离和地形起伏对机械臂路径规划的影响，特别 是对于有较大地形起伏的
情况，通视距离增大，反映了障碍背后点避障路径规 划和实施采样的代价增大，当到达一定阈值后不适合作为采样点9。采样通视距 离的计算示例结果如图4，图中圆点代表采样中间点8，中间点8下方点是采样 路径代价最小的点，距离越远，通视距离越大，路径代价也就越大。
[0069]
以采样过程为例，月面采样作业过程可描述为如图5所示的任务网络。
[0070]
该网络中，p1到p
16
为状态单元，τ1到τ
19
为变迁单元，各个状态单元和变迁单 元的含义分别如表1和表2所示。
[0071]
表1任务网络中状态单元的含义
[0072][0073]
表2任务网络中变迁单元的含义
[0074][0075][0076]
将采样中间点p1、放样上方点p4、放样精调起始点p
10
、放样后抬升点p
13
、观 测精调起始点p
14
和观测后抬升点p
16
定义月面采样机械臂操作的节点状态单元， 节点状态单元之间的转移与月面环境的关联性较小，可以根据着陆器外包罗空 间约束预先规划获得。将触月、采样、放样、样品观察定义为动态状态单元， 从节点状态单元到动态状态单元的过程与
月面环境、机械臂变形等因素密切相 关，需要在任务实施过程中实施动态确定，即以月面地形约束下的机械臂自由 运动空间为空间约束条件、以视觉观测定位得到的机械臂当前状态为初始状态， 规划机械臂在节点状态单元与动态可变状态单元转换的关节构型或末端位姿状 态。
[0077]
以采样过程为例，月面采样作业过程可描述为如图5所示的任务网络。
[0078]
在采样策略规划层，采用递归分解机制将复杂作业任务逐级分解为“[作 业]
‑
[过程]
‑
[事件]”，从而形成整个月面采样作业的时序逻辑的准确描述方法。 具体的，月面采样作业可以被细化为过程初始化过程、表取采样过程和放样过 程。其中，初始化过程包括设备加电事件和机械臂展开事件；表取采样过程包 括机械臂触月事件和机械臂采样事件；放样过程包括机械臂放样事件和样品观 察事件。在保持时序逻辑不变的情况下，根据触月、采样、放样和样品观察的 实时状态，动态调整机械臂操作过程或事件的次数和参数。
[0079]
月面采样过程事件是具有一定持续时间的有序指令集合，其执行过程是月 面采样机械臂执行一系列有序的指令集合从初始状态转换到目标状态的过程。 如图6所示，以“[事件]
‑
[指令序列：参数]
‑
[指令：参数]”作为采样策略规 划的分支控制模式，通过设置指令序列参数和指令参数，控制事件中指令序列 或指令组合调用关系和动作过程的调整变化。一方面形成不同的有序指令集合， 实现对事件的动态调整；另一方面形成不同的动作轨迹，实现对操作过程的动 态调整。
[0080]
为了解决上述动态调整问题，提出基于改进allen模型的时态规划方法。 allen模型定义了基于时间区间的时间约束描述方法，将事件之间的时序逻辑通 过时间区间之间的时序逻辑表达，将任意两个事件之间的关系定义为13种可能 表达，如图16所示。
[0081]
以事件i1＝[t
1s
,t
1e
]和i2＝[t
2s
,t
2e
]为例，事件间时序关系的矩阵表示为：
[0082][0083]
其中，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
分别表示事件i1的起止时刻和事件i2的起止时刻。
[0084]
allen模型虽然能够比较准确地描述总数固定的一系列事件的时序逻辑，但 时序关系矩阵为定性描述，事件间的时序约束不强，且部分事件增加或删除会 导致后续事件时序逻辑的全部重新调整，难以适应采样任务快速动态调整的要 求。为此本节引入定量时序关系矩阵和虚拟事件对allen模型进行改进，适应 中间过程的灵活动态调整要求，描述时间约束关系。
[0085]
定义事件i1＝[t
1s
,t
1s
+δt1]和i2＝[t
2s
，t
2s
+δt2]间的定量时序关系矩阵表示为：
[0086][0087]
其中，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
；δt1和δt2分别为事件i1和事件i2的持续时间。
[0088]
当另一个事件i3与事件i2间的定量时序关系矩阵mr(i2,i3)已知，则事件i1与事 件i3的时序关系满足m(i1,i3)＝sgn[mr(i1,i3)]，有
[0089][0090]
其中，
ij
mr表示定量时序关系矩阵第i行第j列的元素。
[0091]
为了实现对动态过程的自适应调整要求，在事件设计中引入虚拟事件，虚 拟事件仅对有新增事件需求的时序关系进行占位处理，不影响已知事件间时序 关系。虚拟事件与已知事件序列满足原有的13种时序关系，并与新增事件间预 设4种时序关系，如图16、图17所示。新增事件加入后，无需调整原有事件序 列的时序关系，仅建立新增事件与虚拟事件间的时序逻辑，即可实现新增事件 的临时加入和实时更换。具体的，在allen模型中，n
old
个事件中加入n
new
个新 增事件则增加的时序关系数量为在改进的allen模型中，增加的 时序关系数量为n
new
，有
[0092][0093]
成立。因此，改进的allen模型在适应任务动态调整方面有明显的优势。
[0094]
以一系列事件{i1,i0，i3}为例，各个事件的持续时间分别δt1、δt0和δt3，且事件 间的定量时序关系矩阵为mr(i1，i0)、mr(i0，i3)和mr(i1，i3)。由于事件i0是虚拟事件， 则{i1,i0,i3}等价于{i1,i3}，输出的时序关系为m(i1,i3)＝sgn[mr(i1,i3)]。此时，在事件i1与事件i3之间增加事件i2仅需建立虚拟事件i0与事件i2的定量时序关系矩阵 mr(i0,i2)，则得到{i1,i2,i3}输出的时序关系
[0095][0096]
按照上述时序关系，将事件进行逐级展开为有序指令集合，如图7所示， 即生成机械臂操作控制的指令计划。令事件的指令描述为
[0097][0098]
其中，px和pl分别表示指令序列参数集和指令参数集；
(
·
)
l
i_m
表示事件i的 第m个指令，指令左上角标表示执行该指令输入的指令参数，为空则表示该指令 默认执行，例如，i
i
({px1,px
j
},{pl1})＝{l
i_1
,l
i_k
,l
i_(m+1)
}。
[0099]
将有序指令集合按照事件间的时序逻辑组合，例如i1＝{l
1_1
,l
1_4
}，i2＝{l
2_2
,l
2_3
}， i3＝{l
3_1
,l
3_3
}，则i1{m}i2{m}i3＝{l
1_1
,l
1_4
,l
2_2
,l
2_3
,l
3_1
,l
3_3
}，从而最终得到机械臂月面采样操 作控制的指令计划。
[0100]
第三步：运动控制规划
[0101]
月面采样机械臂是安装在月面着陆器上的四自由度串联机械臂，由腰关节1、 肩关节2、大臂3、肘关节4、小臂5、腕关节6和末端采样执行器7组成，如 图7所示。其中，大臂与肩关节固连，小臂与肘关节固连，末端采样执行器与 腕关节固连。月面采样机械臂通过4个关节的转动带动末端采样执行器到达期 望采样点，实现对土壤样品的铲取或挖掘。月面采样机械臂的结构设计非常紧 凑，使其在收缩状态下，大臂、小臂和末端采样执行器可处于一个平面。月面 采样机械臂关节坐标系及连杆参数定义如图8所示。基于dh (denavit
‑
hartenberg)方法的月面采样机械臂连杆参数如表3所示。
[0102]
表3月面采样机械臂连杆参数表
[0103][0104]
具体的，α
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
轴的距离。
[0105]
根据dh参数建立采样机械臂各连杆变换矩阵，进而计算月面采样机械臂关 节空间到末端笛卡尔空间的正运动学变换矩阵模型，表示为：
[0106][0107]
具体的，
[0108][0109]
其中，s
i
表示sin(θ
i
)；c
i
表示cos(θ
i
)；s
23
表示sin(θ2+θ3)；c
23
表示cos(θ2+θ3)；s
234
表 示sin(θ2+θ3+θ4)；c
234
表示cos(θ2+θ3+θ4)。
[0110]
月面采样机械臂关节空间的关节角构型θ＝[θ1,θ2,θ3,θ4]
t
能够唯一确定末端笛卡 尔空间位姿p＝[p
x
,p
y
,p
z
,φ]
t
。其中，φ表示末端采样执行器与月面着陆器上表面的 夹角，φ＝θ2+θ3+θ4，如图9所示。经求解，月面采样机械臂末端笛卡尔空间到关 节空间的逆运动学可表示为：
[0111][0112]
其中，d＝d2+d3+d4，p2＝p
z
‑
d1，a2＝a2+a3c3，a3＝a2c2+a3c
23
。
[0113]
上述逆运动学公式将得到不只一组关节角度组合。针对逆运动学存在的多 解情况，采用最小变化角度的启发式策略：
[0114][0115]
其中，表示逆运动学公式得到的第j组解中关节i的角度；θ
i
表示关节i的 参考关节角度，通常取前一运动状态的关节i角度值。
[0116]
采样任务开始后，月面采样机械臂首先执行展开动作到达采样中间点，之 后执行触月、采样、放样、样品观察等动作开展采样作业。机械臂的运动路径 规划采取了以作业过程关键点为目标的转移路径实时规划策略，从而实现机械 臂大范围转移运动趋势的固化和小范围最短实时避障路径规划，得到运动控制 参数并最终形成机械臂运动指令。
[0117]
采样任务中的关键点集χ
p
＝{x1,x2,
…
,x
n
}需满足无碰撞自由空间χ
f
的约束条件， 即具体的，令χ
e
表示机械臂末端运动空间，χ
c
表示着陆器最大外包络空 间，χ
m
表示月表最大外包络空间。机械臂运动空间内的障碍空间χ
obs
和自由空间 χ
f
可分别表示为χ
obs
＝χ
e
∩(x
c
∪x
m
)和χ
f
＝χ
e
‑
χ
obs
。
[0118]
月面采样机械臂在关键点间转移，运动规划算法采用了基于梯形速度曲线 法。转移过程中，着陆器可动部件运动及月面环境不确定因素带来约束空间的 实时变化。定义χ
obs
(t)为t时刻的障碍空间；χ
arm
(t)为t时刻的机械臂臂杆外包络空 间。则对于路径规划问题(x
init
,θ
init
,x
tar
,χ
obs
(t))，月面采样机械臂最短路径规划满足
[0119] 关节运动行程最小
[0120]
s.t.x(t)＝fk[θ(t)] 运动学约束
[0121]
x(t)∈[χ
e
‑
χ
obs
(t)] 末端自由空间约束
[0122] 臂杆碰撞规避
[0123]
θ
imin
≤θ
i
≤θ
imax
,i＝1,2,3,4 关节角度约束
[0124]
x(0)＝x
init
,x(t
end
)＝x
tar 边界条件
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0125]
其中，通过关节角度约束描述机械臂运动范围约束；x
init
和x
tar
分别表示起始 点
和目标点，且x
init
，x
tar
∈χ
p
；fk(
·
)表示机械臂从关节空间到笛卡尔空间的正运动 学方程。上述路径规划问题可通过搜索方法求解。
[0126]
在一种可能实现的方式中，a
*
算法是典型的启发式搜索方法，被广泛应用于 最优路径的求解，其核心在于估价函数的设计。在选择当前节点的下一个评估 节点时引入估价函数f(x)
[0127]
f(x)＝g(x)+h(x)
[0128]
式中，g(x)是从起始点到节点x最小代价路径的实际代价；h(x)是从节点x到 目标点路径的估计代价。
[0129]
为便于利用a*算法，定义采样机械臂的4个关节角度为一个四维数组，起始 关节角度为当前关节角度为第i步最佳关节角度为 关节角度搜索步长为第i步的后继节点关节角度为 (由关节角度搜索步长确定)，期望关节角度为
[0130]
定义估价函数其中，q
i m
为第i步机械臂关节角度中的第m个关节的关节角度，q
des m
为机械臂 期望关节角度中的第m个关节的关节角度。由此可以求出第i步后继节点关节角 度中f
i
(q)值最小的节点作为此步最优关节角度，根据步长生成第i+1步的后继 节点关节角度，重复此过程可保证每一步都可以得到最优关节角度，因此通过 该估价函数可以实现采样机械臂从起始关节角度到目标关节角度的构型空间最 优路径规划。
[0131]
为了便于分析，设定3个表，即open表、close表和后继节点表。把起始点 q
ini
放入open表，初始化f0(q)＝h0(q)，置close表为空表。
[0132]
重复下列过程，直到找到目标点为止。若open表为空表，表明空间机械臂 路径规划失败；否则，开始以下循环。
[0133]
step1选取open表中未设置过的具有最小f值的节点为最佳节点q
best_i
，并 将其放入close表，同时将其从open表中删除；
[0134]
step2判断q
best_i
是否为目标点q
des
，若是，则求解成功，循环结束；否则 进行下一步；
[0135]
step3根据设定的步长l
step
值，计算后继节点q
suc_i
；
[0136]
step4按下述流程对每个后继节点q
suc_i
进行处理：
[0137]
a.判断q
suc_i
是否在采样机械臂自由运动空间内，若是，建立从q
suc_i
返回 q
best_i
的指针；否则转至step3；
[0138]
b.计算
[0139]
c.判断q
suc_i
是否在open表中，若是转至d；否则转至e；
[0140]
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；
[0141]
e.判断q
suc_i
是否在close表中，若是，转至d；否则转至f；
[0142]
f.将q
suc_i
放入open表，并添入到q
best_i
的后继表中，转至g；
[0143]
g.计算f(q
suc_i
)，并转至a。
[0144]
当前构型与目标状态动态变化、运动控制模式、不同运动控制模式的运动 控制参数如下：
[0145]
当前构型与目标状态分别为任务的起始点和目标点。以下面的例子进行说 明采样机械臂关节角度约束和采样运动过程关键点分别如表4和表5所示。采 样机械臂模拟月面采样过程。
[0146]
表4采样机械臂关节角范围
[0147][0148]
表5采样机械臂运动过程关键点
[0149][0150]
运动控制模式包括关节空间规划模式和笛卡尔空间规划模式
[0151]
关节空间规划模式是以关节角度的函数来描述采样机械臂的路径，进行路 径规划。关节空间规划模式实现方法是，给定采样机械臂的起始节点位形和终 止点位形，对关节进行梯形速度插值，从而得到关节路径。
[0152]
例如，若规划路径起始节点的关节角θ0，规划路径目标节点的关节角θ
f
以及 最大角速度和最大角加速度可采用如下梯形速度插值：
[0153][0154]
其中，θ(t)表示规划的路径；表示路径加速段和减速段的加速度；t
c1
表 示路径
的加速段的时长，t
c2
表示路径的匀速段的时长，表示路径的匀速段的时长，t
c3
表示路径的减速段的时长，t
f
表示路径的时长， t
f
＝t1+t2+t3。
[0155]
笛卡尔空间规划模式是在给定机械臂初始构型和目标末端位姿的基础上， 对机械臂末端进行梯形速度插值，从实现机械臂从起始节点运动至到目标节点 的过程。完成笛卡尔空间的路径规划并没有完成整个路径规划的工作，最终还 要转换到关节空间的路径规划。笛卡尔空间路径规划实现方法是：首先用数学 方法在笛卡尔空间中描述期望路径，计算出末端各插值点的位姿信息，然后通 过逆运动学(公式10所示)求解得到关节的相应参数，从而得到目标轨迹。
[0156]
采样机械臂首先从初始构型展开，运动控制模式为关节空间规划模式，关 节空间规划模式的运动控制参数为：最大角速度为2
°
/s，最大角加速度为0.5
°
/s2。 运动规划结果如图10和图11所示。经规划，采样机械臂从初始构型到展开构 型的运动时长为24s。
[0157]
采样机械臂的第二段运动从展开构型到采样中间点，采用笛卡尔空间规划 模式，笛卡尔空间规划模式的运动控制参数为最大线速度和最大线加速度分别 为0.02m/s和0.002m/s2，运动规划结果如图12和图13所示，月面采样机械臂完成从 采样中间点到采样点所需的运动时长为83.085s。
[0158]
采样机械臂的第三段运动从采样中间点到采样点，运动控制模式为笛卡尔 空间规划模式，最大线速度和最大线加速度分别为0.02m/s和0.002m/s2。运动控制规 划结果如图14和图15所示，月面采样机械臂完成从采样上方点到采样点所需 的运动时长为120.905s。
[0159]
将上述控制模式、控制参数、初始状态、目标状态作为遥控指令计划中动 态指令的参数。