[发明专利]一种用于自动采血机器人的路径规划方法及系统

权利要求书

1.一种用于自动采血机器人的路径规划方法，其特征在于，包括以下步骤：

采集自动采血机器人的初始位置，以及采血目标的预进针点、目标位置，构建搜索树；

采集采血目标周围的潜在障碍物，通过虚拟斥力场扩展策略，获取用于扩展所述搜索树的第一采样点；

将所述潜在障碍物，作为障碍物控制点，根据所述第一采样点与所述障碍物控制点的第一距离，生成用于扩展所述搜索树的步长；

获取与所述第一采样点距离最近的所述搜索树上的第一节点，根据所述步长，生成第二节点；

将所述第一节点与所述第二节点进行有向性连接，生成所述搜索树的扩展路径，其中，所述有向性连接用于表示具有方向指向的连接关系；

基于所述初始位置，通过所述扩展路径，生成所述自动采血机器人的运动路径。

2.根据权利要求1所述一种用于自动采血机器人的路径规划方法，其特征在于：

在构建搜索树的过程中，构建所述预进针点到所述初始位置的第一搜索树，所述预进针点到所述目标位置的第二搜索树，所述初始位置到所述预进针点的第三搜索树，所述目标位置到所述预进针点的第四搜索树；

根据所述第一搜索树、所述第二搜索树、所述第三搜索树、所述第四搜索树，构建所述搜索树。

3.根据权利要求2所述一种用于自动采血机器人的路径规划方法，其特征在于：

在自动采血机器人进行路径规划的过程中，根据所述初始位置和所述目标位置，以及所述初始位置到所述障碍物控制点的阈值距离，构建所述自动采血机器人的引力场模型，所述引力场模型用于防止产生过大引力导致路径规划失效，其中，所述初始位置到所述目标位置的第二距离越大，所述引力越大。

4.根据权利要求3所述一种用于自动采血机器人的路径规划方法，其特征在于：

在构建所述自动采血机器人的引力场模型的过程中，所述引力场模型表示为：

其中，q＝(θ₁，θ₂，θ₃，θ₄，θ₅，θ₆)^T表示机器人的当前位姿；q_goal表示目标位姿；ζ表示引力增益；l表示阈值距离。

5.根据权利要求4所述一种用于自动采血机器人的路径规划方法，其特征在于：

在自动采血机器人进行路径规划的过程中，基于所述潜在障碍物，构建斥力场模型，所述斥力场模型用于防止所述自动采血机器人与所述潜在障碍物发生接触，其中，所述斥力场模型表示为：

式中，k_p表示斥力场系数，r表示与机器人本体最近障碍区域的距离，r₀表示障碍区域的作用距离阈值。

6.根据权利要求5所述一种用于自动采血机器人的路径规划方法，其特征在于：

在扩展所述搜索树的过程中，根据所述第一搜索树和/或所述第二搜索树和/或所述第三搜索树和/或所述第四搜索树，随机生成第二采样点；

基于所述斥力场模型和所述引力场模型，根据所述第一采样点和/或所述第二采样点，扩展所述搜索树。

7.根据权利要求6所述一种用于自动采血机器人的路径规划方法，其特征在于：

在通过第二采样点扩展搜索树的过程中，判断所述第二采样点是否为所述障碍物控制点，若是，则根据所述第一采样点扩展所述搜索树，若否，则根据所述第二采样点和所述第一采样点扩展所述搜索树，或只根据所述第二采样点扩展所述搜索树。