CN114740862A - 一种用于自动采血机器人的路径规划方法及系统 - Google Patents

Abstract

本发明公开了一种用于自动采血机器人的路径规划方法及系统，包括：采集自动采血机器人的初始位置，以及采血目标的预进针点、目标位置，构建搜索树；采集采血目标周围的潜在障碍物，通过虚拟斥力场扩展策略，获取用于扩展搜索树的第一采样点；将潜在障碍物，作为障碍物控制点，根据第一采样点与障碍物控制点的第一距离，生成用于扩展搜索树的步长；获取与第一采样点距离最近的搜索树上的第一节点，根据步长，生成第二节点；将第一节点与第二节点进行有向性连接，生成搜索树的扩展路径；基于初始位置，通过扩展路径，生成自动采血机器人的运动路径；本发明优化了采样节点的扩展方向，缩短了平均计算耗时，提高了路径规划效率。

Description

一种用于自动采血机器人的路径规划方法及系统

技术领域

本发明涉及机器人运动控制领域，具体而言，涉及一种用于自动采血机器人的路径规划方法及系统。

背景技术

随着科技发展，机器人已广泛应用于生物医学、工业生产娱乐服务等场景。手臂静脉采血是最常见的医疗护理手段之一，也是临床进行静脉采血和静脉输液的重要方法。避免因机器人路径规划准确性不高、无法确保人员安全情况下反复穿刺是大家所关切的重要问题。

目前自动采血机器人希望能全自动的完成对手臂静脉的采血工作，在视觉传感器获取初始点、目标点和潜在障碍物的工作空间位置信息后，需要自动采血机器人高效、无碰撞地完成静脉采血工作。通常采用RRT、RRT*、InformedRRT*等基于采样的路径规划算法，适合多自由度的自动采血机器人在复杂的动态环境下进行路径规划。通过对整个高维度构形空间进行采样，可确保连接得到初始位置点与目标终点的可行解，但是由于采样过程是对整个高维度构形空间进行不稳定采样，因此效率较低，在复杂的医护场景下很难实时求解并且保证解的最优性。

针对基于采样的路径规划算法盲目随机性采样扩展节点的不稳定性，为实现六自由度采血机器人准确高效完成静脉采血工作，急需一种用于自动采血机器人的路径规划方法及系统，用于避免进行盲目随机性采样扩展节点，缩短了计算时间。

发明内容

为了解决上述问题，本发明的目的是通过磁控忆阻器耦合设计出一个五维忆阻保守混沌系统及其实现电路。

为了实现上述技术目的，本申请提供了一种用于自动采血机器人的路径规划方法，包括以下步骤：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

优选地，在生成用于扩展搜索树的步长的过程中，步长的表达式为：

其中，K表示步长，K₀表示初始步长，τ_i表示控制点的权重系数τ₄＞τ₃＞τ₂＞τ₁＞0，q表示机器人设定的关键点，ρ(a_i(q))表示采样点接受范围调节参数，权重系数越大，则接受采样点范围越大。

根据步长，获取采血机器人在当前位姿的下一步位姿q_next；

式中，δ表示最大允许误差，q表示机器人设定的关键点，F(q)表示势函数，q_goal表示进行避障要到达的目标点。

优选地，在生成第二节点的过程中，判断第二节点是否在斥力场范围内，若在，则舍弃第二节点，进入下一次采样迭代；

若不在，则根据第二节点到第一节点的路径，判断路径是否分别与第一搜索树、第二搜索树、第三搜索树、第四搜索树发生碰撞，若未发生碰撞，则将路径作为扩展路径，否则舍弃第二节点；

在生成扩展路径的过程中，获取第一节点到第二节点的第一代价，以及第二节点到第一节点的第二代价，通过判断第一代价和第二代价的关系，将第一节点或第二节点作为父亲节点。

本发明还公开了一种用于自动采血机器人的路径规划系统，包括：

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

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

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

扩展路径规划模块，用于获取与第一采样点距离最近的搜索树上的第一节点，根据步长，生成第二节点，并将第一节点与第二节点进行有向性连接，生成搜索树的扩展路径，其中，有向性连接用于表示具有方向指向的连接关系；

运动路径规划模块，用于基于初始位置，通过扩展路径，生成自动采血机器人的运动路径。

本发明公开了以下技术效果：

本发明优化了采样节点的扩展方向，缩短了平均计算耗时，提高了路径规划效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明所述的路径规划方法流程示意图。

具体实施方式

下为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

如图1所示，本发明提供了一种用于自动采血机器人的路径规划方法，包括以下步骤：

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

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

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

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

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

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

本发明在构建搜索树的过程中，构建了四个具有路径指向性的搜索树，分别为预进针点到初始位置的第一搜索树，预进针点到目标位置的第二搜索树，初始位置到预进针点的第三搜索树，目标位置到预进针点的第四搜索树；

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

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

本发明提到的引力场模型表示为：

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

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

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

本发明在扩展搜索树的过程中，还包括一种随机扩展的过程，即分别根据四个具有路径指向性的搜索树，通过随机生成采样点进行搜索树的随机扩展，具体过程为：根据第一搜索树和/或第二搜索树和/或第三搜索树和/或第四搜索树，随机生成第二采样点；

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

本发明在对搜索树进行随机扩展的过程中，需要判断第二采样点是否为障碍物控制点，若是，则根据第一采样点扩展搜索树，若否，则根据第二采样点和第一采样点扩展搜索树，或只根据第二采样点扩展搜索树。

本发明在生成用于扩展搜索树的步长的过程中，步长的表达式为：

其中，K表示步长，K₀表示初始步长，τ_i表示控制点的权重系数τ₄＞τ₃＞τ₂＞τ₁＞0，q表示机器人设定的关键点，ρ(a_i(q))表示采样点接受范围调节参数，权重系数越大，则接受采样点范围越大。

该步长为动态步长，通过采样点与障碍物控制点的距里关系，适应性的进行步长动态调整，当距离较大时，选择较大步长进行扩展，当距离较小时，选择小步长进行扩展。

根据步长，通过梯度下降方向确定，采血机器人在当前位姿的下一步位姿q_next；

式中，δ表示最大允许误差，q表示机器人设定的关键点，F(q)表示势函数，q_goal表示进行避障要到达的目标点。本发明在生成第二节点的过程中，还需要判断第二节点是否在斥力场范围内，若在，则舍弃第二节点，进入下一次采样迭代；

若不在，则根据第二节点到第一节点的路径，判断路径是否分别与第一搜索树、第二搜索树、第三搜索树、第四搜索树发生碰撞，若未发生碰撞，则将路径作为扩展路径，否则舍弃第二节点；

在生成扩展路径的过程中，获取第一节点到第二节点的第一代价，以及第二节点到第一节点的第二代价，通过判断第一代价和第二代价的关系，将第一节点或第二节点作为父亲节点。

本发明进一步公开了一种用于自动采血机器人的路径规划系统，包括：

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

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

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

扩展路径规划模块，用于获取与第一采样点距离最近的搜索树上的第一节点，根据步长，生成第二节点，并将第一节点与第二节点进行有向性连接，生成搜索树的扩展路径，其中，有向性连接用于表示具有方向指向的连接关系；

运动路径规划模块，用于基于初始位置，通过扩展路径，生成自动采血机器人的运动路径。

本发明还公开了一种计算机程序和可移动存储设备，计算机程序用于通过实现自动采血机器人的运动路径的规划方法，通过嵌入到可移动存储设备中，实现路径规划系统的系统功能。

实施例1：本发明的核心为，自动采血机器人在高维空间路径规划过程中结合人工势场思想进行同时四向、有方向性地搜索、采样节点。通过引入预进针点，在初始路径阶段，初始点与预进针点和预进针点与目标点之间四棵随机树并行扩展；通过动态步长生成新的节点。在路径优化阶段，与Informed-RRT*在超椭球子集直接采样一样，改进算法通过预进针点，构建2个独立的子集空间进行统一采样来分别优化初始点到预进针点和预进针点到目标点这两段路径。

本发明通过引入预进针点，在初始路径阶段，初始点与预进针点和预进针点与目标点之间四棵随机树并行扩展；通过动态步长生成新的节点。在路径优化阶段，与Informed-RRT*在超椭球子集直接采样一样，改进算法通过预进针点，构建2个独立的子集空间进行统一采样来分别优化初始点到预进针点和预进针点到目标点这两段路径。

本发明公开的自动采血机器人路径规划方法，其算法具体步骤为：

S1.首先获取采血机器人末端执行器的初始位置x_init作为根节点，预进针点x_pre，目标位置x_goal和周围潜在障碍物的位置、形状、尺寸信息；初始化树T₁、T₂、T₃、T₄，分别对应x_init→x_pre、x_pre→x_init、x_pre→x_goal、x_goal→x_pre并设初始路径信息列表L为空。

步骤S1.1：首先以初始点x_init、预进针点x_pre和目标点x_goal为起点构建四棵随机搜索树，初始搜索树的步长均设置为K₀，然后通过x_init与x_pre和x_pre与x_goal之间形成四棵树扩展来高效探索空间路径。

步骤S1.2：引力场存在于自动采血机器人末端执行器与静脉穿刺位置之间，离目标位置越远，引力越大，产生的引力牵引机器人末端执行器到达目标位置。在笛卡尔空间中规划多自由度机器人路径时，需要不断进行逆运动学求解，常常导致奇异解的出现。为了避免此问题，在多自由度机器人的关节空间内建立引力场U_att(q)，q为采血机器人末端执行器目前位置。

式中：q＝(θ₁，θ₂，θ₃，θ₄，θ₅，θ₆)^T为机器人末端执行器当前位姿；q_goal为目标位姿；ζ为引力增益；l为设定与障碍物的阈值距离，预防产生过大引力导致路径规划失效。

步骤S1.3.为潜在障碍物建立一个虚拟的“斥力场公式”：

障碍物虚拟“斥力场公式”中k_p为斥力场系数，与自动采血机器人末端执行器运行空间大小正相关，r₀与潜在障碍物数量关系成正相关。为保留Informed-RRT*随机扩展性，四棵树分别随机数采样点扩展时，定义常数q_hold，当随机数temp小于q_hold时，使用虚拟斥力场扩展策略，否则仍沿着随机方向进行多方向扩展。

S2.迭代开始，随机产生的变量temp与自定义常量q_hold比较，选择不同的拓展点策略生成采样点x_rand。

S3.在树T₁、T₂、T₃、T₄中搜索距离x_rand最近邻节点x_nearest进行扩展。

S4.以动态步长

及方向搜索得到x_new，通过连接x_nearest生成新的节点。初始搜索树的步长均设置为K₀，为提升算法的搜索效率，在路径搜索的过程中引入动态步长策略。采血机器人末端执行器下一步的位姿q_next，通过梯度下降方向确定，有

K为探索步长，δ为最大允许误差。算法根据x_rand与障碍物控制点的距离自适应修改步长K.当障碍物较远时，采用大步长提高搜索效率；当障碍物较近时，缩小步长，保障算法精度，如：

K₀为初始步长；τ_i为控制点的权重系数，τ₄＞τ₃＞τ₂＞τ₁＞0。

S5.判断x_new是否在斥力场范围内，若在，则舍弃该点，进入下一次采样迭代；若不在，对节点x_new和树枝x_new--x_nearest进行碰撞检测，若未发生碰撞，则将x_new加入到对应的树T中，否则舍弃x_new，并转至步骤S2继续搜索进行节点采样。

S6.在x_new邻近区域内执行重连策略，如果从x_new到达x_nearest的代价比x_nearest之前的代价更小，则将x_new作为x_nearest的父节点。记录更新路径信息列表L。

S7.直到分别判断四棵搜索树x_new是否达到目标点x_goal或满足约束条件，若满足|x_new-x_goal|≤d(d为设置的x_new和x_goal距离阈值)，则算法结束搜索；否则算法继续搜索。

S8.重复步骤S2到步骤S7，直到达到所设置的迭代次数为止。最后，起始四棵搜索树的根节点到目标连接搜索树的根节点之间经过的全部节点为规划的路径。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

在本发明的描述中，需要理解的是，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (10)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

在生成用于扩展所述搜索树的步长的过程中，所述步长的表达式为：

其中，K表示步长，K₀表示初始步长，τ_i表示控制点的权重系数τ₄＞τ₃＞τ₂＞τ₁＞0，q表示机器人设定的关键点，ρ(a_i(q))表示采样点接受范围调节参数，权重系数越大，则接受采样点范围越大。

根据所述步长，获取采血机器人在当前位姿的下一步位姿q_next；

式中，δ表示最大允许误差，q表示机器人设定的关键点，F(q)表示势函数，q_goal表示进行避障要到达的目标点。

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

在生成第二节点的过程中，判断所述第二节点是否在斥力场范围内，若在，则舍弃所述第二节点，进入下一次采样迭代；

若不在，则根据所述第二节点到所述第一节点的路径，判断所述路径是否分别与所述第一搜索树、所述第二搜索树、所述第三搜索树、所述第四搜索树发生碰撞，若未发生碰撞，则将所述路径作为所述扩展路径，否则舍弃所述第二节点；

在生成所述扩展路径的过程中，获取所述第一节点到所述第二节点的第一代价，以及所述第二节点到所述第一节点的第二代价，通过判断所述第一代价和所述第二代价的关系，将所述第一节点或所述第二节点作为父亲节点。

10.一种用于自动采血机器人的路径规划系统，其特征在于，包括：

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

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

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

扩展路径规划模块，用于获取与所述第一采样点距离最近的所述搜索树上的第一节点，根据所述步长，生成第二节点，并将所述第一节点与所述第二节点进行有向性连接，生成所述搜索树的扩展路径，其中，所述有向性连接用于表示具有方向指向的连接关系；

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

Images