[发明专利]三维路径规划方法、装置、设备和存储介质

权利要求书

1.一种三维路径规划方法，其特征在于，应用于包括硬针穿刺的场景，所述方法包括：

获得所述硬针的穿刺目标位置和刺入位置，根据所述刺入位置确定目标区域，以及根据所述穿刺目标位置和所述刺入位置确定初始区域；

根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点；对所述初始搜索节点进行扩展确定状态节点对应的搜索区域；

基于所述初始搜索路径和所述搜索区域规划目标搜索路径；

判断所述目标搜索路径是否到达所述目标区域，获得判断结果；在所述判断结果表明所述目标搜索路径到达所述目标区域的情况下，将所述目标搜索路径作为硬针穿刺的三维路径规划。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：

在所述判断结果表明所述目标搜索路径没有到达所述目标区域的情况下，将所述状态节点按预设方向进行扩展确定新节点对应的扩展区域；所述预设方向表征所述状态节点朝向所述目标区域的方向；

根据所述目标搜索路径和所述扩展区域规划新搜索路径；

判断所述新搜索路径是否到达所述目标区域，在所述新搜索路径到达所述目标区域的情况下，将所述新搜索路径作为硬针穿刺的三维路径规划。

3.根据权利要求1所述的方法，其特征在于，所述根据所述穿刺目标位置和所述刺入位置确定初始区域，包括：

获得初始穿刺角度和初始穿刺步长；

根据所述穿刺目标位置、所述刺入位置、所述初始穿刺角度和所述初始穿刺步长确定圆锥体；

将所述圆锥体的底部区域作为所述初始区域。

4.根据权利要求3所述的方法，其特征在于，所述根据所述穿刺目标位置和所述初始区域规划初始搜索路径，获得初始搜索节点，包括：

将所述穿刺目标位置作为扩展树的根节点；

基于所述根节点和所述初始区域通过搜素随机树RRT算法规划可行路径；

根据A星算法确定所述可行路径中的最优路径；

判断所述最优路径是否成功避开障碍物；在所述最优路径成功避开障碍物的情况下，将所述最优路径作为初始搜索路径；

将所述初始搜索路径在所述初始区域中的端点作为初始搜索节点。

5.根据权利要求1所述的方法，其特征在于，所述对所述初始搜索节点进行扩展确定状态节点对应的搜索区域，包括：

根据搜素随机树RRT算法对所述初始搜索节点进行扩展，获得多个随机状态节点；

基于所述初始搜索节点和所述多个随机状态节点中的每个随机状态节点确定多个状态节点；

根据A星算法确定所述多个状态节点中的最佳状态节点；在所述初始搜索节点和所述最佳状态节点之间的运动直线是无碰撞路径的情况下，根据所述初始搜索节点的位置信息和所述最佳状态节点的位置信息确定第一穿刺步长；

获得硬针穿刺角度的约束范围；基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围确定所述最佳状态节点处所述硬针的第一穿刺角度；

根据所述初始搜索节点、所述最佳状态节点、所述第一穿刺步长和所述第一穿刺角度确定第一圆锥体；

将所述第一圆锥体的底部区域作为所述搜索区域。

6.根据权利要求5所述的方法，其特征在于，所述方法还包括：

在所述初始搜索节点和所述最佳状态节点之间的运动直线不是无碰撞路径的情况下，获得第一状态节点；所述第一状态节点为所述多个状态节点中除所述最佳状态节点以外的任一状态节点；

根据所述第一状态节点的位置信息和所述最佳状态节点的位置信息确定所述第一状态节点的第二穿刺步长；

基于所述初始搜索节点的硬针穿刺姿态信息和所述约束范围更新所述第一状态节点，确定更新后第一状态节点处所述硬针的第二穿刺角度；

根据所述更新后第一状态节点、所述第二穿刺步长和所述第二穿刺角度确定第二圆锥体；

将所述第二圆锥体的底部区域作为所述搜索区域。