CN106020199A - 基于优化人工势场法的血凝仪动态避障路径规划方法 - Google Patents

Abstract

本发明公开了一种基于优化人工势场法的血凝仪动态避障路径规划方法，该方法包括：采用人工势场法对血凝仪机械臂的移动进行实时避障路径规划；实时检测血凝仪机械臂所到达的位置点是否为局部稳定点；若是局部稳定点，则将引力场、吸引力、斥力场、斥力分别进行更新，从而打破稳定状态，使机械臂沿正确路径运行，并与目标保持相同的运动趋势。本发明适用于动态环境中的路径规划。

Description

基于优化人工势场法的血凝仪动态避障路径规划方法

技术领域

本发明涉及血凝仪路径规划方法，尤其涉及一种基于优化人工势场法的血凝仪动态避障路径规划方法。

背景技术

20世纪以来，随着人类文明得到的飞速发展，促进了人类在生命科学和生物工程领域的深入研究，尤其在医学方面，如何有效快速的对疾病进行检测和治疗，是当今人类发展进程中的重要研究课题，因此研究先进的医学检验仪器对促进检验医学的发展有着重要意义，而血凝仪[王雪峰，王宏利，血液凝固分析仪[J]，现代医学仪器与应用.2000.12(3)：7-10]大大方便了医护人员的工作任务。

机器人的运动规划传统上被看作是一个优化问题，其中机器人的位置在一个参数空间表示。通过尝试在参数空间中搜索满足预定义的成本函数(如机器人与目标位置的距离)的参数来寻找解决问题的方案。该方法的局限时在处理涉及不可预料的障碍的动态环境时，产生一个新的方案的计算代价太高。作为机械臂与移动机器人的一个更实际的无碰撞运动的实时规划算法，人工势场法[姜德，谢勇.人工势场法规划方法[J].西南大学学报：科学版，2013，29(5):128-132]将动态感知到的反馈结合到机器人的控制中，从而克服了扩充低级运动控制器的反应能力局限。

人工势场法是实现机器人路径规划的一种重要算法，它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全。

但是人工势场法存在两个问题：(1)存在局部稳定点(或局部最小点、局部最优点)问题，是指移动机器人机械臂由于一些因素导致机械臂受到的引力和斥力平衡，达到稳定，此时，机器人会产生死锁现象，但这时机械臂并没有达到目标点。(2)当目标附近有障碍物时，移动机器人将永远也到达不了目的地，具体是由于在以前的许多研究中，目标和障碍物都离的很远，当机器人逼近目标时，障碍物的斥力变的很小，甚至可以忽略，机器人将只受到吸引力的作用而直达目标，但在许多实际环境中，往往至少有一个障碍物与目标点离的很近，在这种情况下，当移动机器人逼近目标的同时，它也将向障碍物靠近，如果利用以前对引力场函数和斥力场函数的定义，斥力将比引力大的多，这样目标点将不是整个势场的局部稳定点，因此移动机器人将不可能到达目标。

另外，在过去的研究中，很多路径规划都是将人工势场法应用于静态环境中，即目标和障碍物为静止不动的，然而，在实际应用中，机器人所处的环境大多是动态的，且不仅障碍物是运动的，甚至目标也是运动的。

发明内容

发明目的：本发明针对现有技术存在的问题，提供一种基于优化人工势场法的血凝仪动态避障路径规划方法，该方法尤其适用于动态环境中的路径规划。

技术方案：本发明所述的基于优化人工势场法的血凝仪动态避障路径规划方法包括：

采用人工势场法对血凝仪机械臂的移动进行实时避障路径规划；

实时检测血凝仪机械臂所到达的位置点是否为局部稳定点；

若是局部稳定点，则将引力场、吸引力、斥力场、斥力分别进行更新，从而打破稳定状态，使机械臂沿正确路径运行，并与目标保持相同的运动趋势；其中，更新后的引力场、吸引力、斥力场、斥力分别为：

引力场为：U_att(q)＝0.5ξ_q||q-q_g||²+0.5ξ_v||v-v_g||²+0.5ξ_a||a-a_g||²

吸引力为：

斥力场为：

斥力为：

式中，ξ_a、ξ_v、ξ_q为比例系数，q、v、a分别指机械臂的位置、速度和加速度，q_g、v_g、a_g分别指目标的位置、速度和加速度，q_obs、v_obs、a_obs分别指障碍物的位置、速度和加速度，||q-q_g||是机械臂与目标之间的相对距离，||v-v_g||是机械臂与目标之间的相对速度，||a-a_g||是机械臂与目标之间的相对加速度，为哈密顿算子，η₁、η₂、η₃、η₄、η₅、η₆为比例系数，ρ₀指障碍物的排斥力的最大范围，ρ(q)＝||q-q_obs||是机械臂与障碍物的最小距离，x_goal为目标的位置，(x-x_goal)为机械臂与目标之间的距离，v_ro＝(v-v_obs)^Te_ro为机械臂与障碍物的相对速度在二者连线上的分量，a_ro＝(a-a_obs)^Te_ro为机械臂与障碍物的相对加速在二者连线上的分量，e_ro为从机械臂指向障碍物的单位向量。

进一步的，所述实时检测血凝仪机械臂所到达的位置点是否为局部稳定点，具体采用以下方法实现：

根据机械臂的移动步长以及斥力的作用范围设定局部稳定点的影响区域圆半径为R_l，根据机械臂的移动速度设定检测时间为t；

每隔t时间检测一次机械臂，获取机械臂上一次的位置坐标(x_before,y_before)和当前位置坐标(x_current,y_current)；

计算位置坐标(x_before,y_before)和位置坐标(x_current,y_current)之间的距离；

若计算得到的距离小于R_l，则认为机械臂当前的位置点为局部稳定点。

有益效果：本发明与现有技术相比，其显著优点是：本发明中研究的障碍物以动态障碍物为主，在这种人工势场的作用下，移动机械臂所受的虚拟力不仅与运动之间相对位置有关，而且与相对速度，甚至相对加速度都有关，因此将引力场和斥力场都进行改进更新，使得机器人时刻趋向于保持与目标相同的运动趋势，同时保持与障碍物相悖的运动趋势，还可以解决局部稳定点问题。另外，采用本发明，机械臂运动更加平滑，速度更加稳定，提高精度的同时降低成本。

附图说明

图1是本发明的一个实施例的系统框图；

图2是机械臂进入局部稳定点的区域图；

图3是计算吸引力过程各向量之间的关系；

图4是本发明排斥力过程中各向量之间的关系。

具体实施方式

如图1所示，本实施例的基于优化人工势场法的血凝仪动态避障路径规划方法包括以下步骤：

S1、采用人工势场法对血凝仪机械臂的移动进行实时避障路径规划。

传统势场的函数为：

引力场为：

斥力场为：

引力为：F^att(q)＝-ζ(q-q_g)

斥力为：

其中，ρ_g(q)＝||q-q_g||指机械臂到目标的距离，q、q_g分别指机械臂和目标的位置，ζ、η为比例系数，ρ₀指障碍物的排斥力的最大范围，ρ(q)指障碍物与机械臂的最小距离，

S2、实时检测血凝仪机械臂所到达的位置点是否为局部稳定点。

尽管人工势场法中将移动机械臂可看成是一个可移动的质点，但当机械臂陷入局部稳定点问题时，机械臂并不会停在一个固定的点上，更多的情况是，机械臂停留在某一个很小的区域内徘徊或者打转，机械臂可以原点打转，也可以是以某一个半径做圆周运动，如图2所示。

因此，采用的检测方法具体为：(1)根据机械臂的移动步长以及斥力的作用范围设定局部稳定点的影响区域圆半径为R_l，根据机械臂的移动速度设定检测时间为t；(2)每隔t时间检测一次机械臂，获取机械臂上一次的位置坐标(x_before,y_before)和当前位置坐标(x_current,y_current)；(3)计算位置坐标(x_before,y_before)和位置坐标(x_current,y_current)之间的距离；(4)若计算得到的距离小于R_l，则认为机械臂当前的位置点为局部稳定点。

S3、若是局部稳定点，则将引力场、吸引力、斥力场、斥力分别进行更新，从而打破稳定状态，使机械臂沿正确路径运行，并与目标保持相同的运动趋势。

(1)更新后的引力场为：

U_att(q)＝0.5ξ_q||q-q_g||²+0.5ξ_v||v-v_g||²+0.5ξ_a||a-a_g||²，

式中，ξ_a、ξ_v、ξ_q为比例系数，q、v、a分别指机械臂的位置、速度和加速度，q_g、v_g、a_g分别指目标的位置、速度和加速度，||q-q_g||是机械臂与目标之间的相对距离，v-v_g||是机械臂与目标之间的相对速度，||a-a_g||是机械臂与目标之间的相对加速度。由引力场函数可知，相对位置、相对速度和相对加速减小时，引力场数值也随之减小。

由更新后的引力场可以得到吸引力为：令F_att(q,v,a)＝F_att(q)+F_att(v)+F_att(a)，则F_att(q)＝ξ_q||q-q_g||e_qrg，F_att(v)＝ξ_v||v-v_g||e_vrg，F_att(a)＝ξ_a||a-a_g||e_arg，其中，e_qrg是从机械臂位置指向目标位置的单位向量，e_vrg,e_arg也是单位向量，其方向分别为目标点与机械臂的相对速度和相对加速度方向。图3展示了计算力F_att的过程中，各个向量的关系。从图3中可以看出，F_attq作用于机械臂使其跟踪目标点的位置坐标；F_attv作用于机械臂使其趋向于保持与目标点相同的速度，F_atta作用于机器人使其趋向于保持与目标点相同的加速度。

(2)更新后斥力场为：

$U_{rep}\left(q\right)=\left\{\begin{array}{cc}0.5{\mathrm{\η}}_1{(\frac{1}{\mathrm{\ρ}\left(q\right)}-\frac{1}{{\mathrm{\ρ}}_0})}^2{(x-x_{goal})}^n+{\mathrm{\η}}_2{v}_{ro}+{\mathrm{\η}}_3{a}_{ro}& \mathrm{\ρ}\left(q\right)\≤{\mathrm{\ρ}}_0\\ 0& \mathrm{\ρ}\left(q\right)>{\mathrm{\ρ}}_0\end{array}\right.$

式中，η₁、η₂、η₃、η₄、η₅、η₆为比例系数，q_obs、v_obs、a_obs分别指障碍物的位置、速度和加速度，ρ₀指障碍物的排斥力的最大范围，ρ(q)＝||q-qo_bs||是机械臂与障碍物的最小距离，x_goal为目标的位置，(x-x_goal)为机械臂与目标之间的距离，v_ro＝(v-v_obs)^Te_ro为机械臂与障碍物的相对速度在二者连线上的分量，a_ro＝(a-a_obs)^Te_ro为机械臂与障碍物的相对加速在二者连线上的分量，e_ro为从机械臂指向障碍物的单位向量。v_ro≤0时，机械臂背离障碍物运动，不采取避障行为，v_ro＞0时，机械臂朝向障碍物运动，需要采用避障行为。

由斥力场得到斥力为：

$F_{rep}\left(q\right)=-\▿\[U_{rep}\left(q\right)\]=\left\{\begin{array}{cc}{F}_{rep1}+F_{rep2}& \mathrm{\ρ}\left(q\right)\≤{\mathrm{\ρ}}_0\\ 0& \mathrm{\ρ}\left(q\right)>{\mathrm{\ρ}}_0\end{array}\right.$

由F_rep＝F_repq+F_repa+F_repv可知，F_rep1＝F_repq1+F_repa1+F_repv1，F_rep2＝F_repq2+F_repa2+F_repv2。其中：

$F_{repq1}=-\▿q\[0.5{\mathrm{\η}}_1{(\frac{1}{\mathrm{\ρ}\left(q\right)}-\frac{1}{{\mathrm{\ρ}}_0})}^2{(x-x_{goal})}^n+{\mathrm{\η}}_2{v}_{ro}+{\mathrm{\η}}_3{a}_{ro}\]$

$F_{repv1}=-\▿v\[0.5{\mathrm{\η}}_1{(\frac{1}{\mathrm{\ρ}\left(q\right)}-\frac{1}{{\mathrm{\ρ}}_0})}^2{(x-x_{goal})}^n+{\mathrm{\η}}_2{v}_{ro}+{\mathrm{\η}}_3{a}_{ro}\]$

$F_{repa1}=-\▿a\[0.5{\mathrm{\η}}_1{(\frac{1}{\mathrm{\ρ}\left(q\right)}-\frac{1}{{\mathrm{\ρ}}_0})}^2{(x-x_{goal})}^n+{\mathrm{\η}}_2{v}_{ro}+{\mathrm{\η}}_3{a}_{ro}\]$

因此可得出同理可得出：

$F_{rep2}=\[{\mathrm{\η}}_4{(\frac{1}{\mathrm{\ρ}\left(q\right)}-\frac{1}{{\mathrm{\ρ}}_0})}^2{(x-x_{goal})}^{n-1}\]e_{ro}+{\mathrm{\η}}_5{e}_{ro}+{\mathrm{\η}}_6{e}_{ro}$

因此更新后的斥力为：

$\begin{array}{c}{F}_{rep}=\[{\mathrm{\η}}_1(\frac{1}{\mathrm{\ρ}\left(q\right)}-\frac{1}{{\mathrm{\ρ}}_0})\frac{1}{{\mathrm{\ρ}}^2\left(q\right)}{(x-x_{goal})}^n\]e_{ro}+{\mathrm{\η}}_2{e}_{ro}+{\mathrm{\η}}_3{e}_{ro}\\ +\[{\mathrm{\η}}_4{(\frac{1}{\mathrm{\ρ}\left(q\right)}-\frac{1}{{\mathrm{\ρ}}_0})}^2{(x-x_{goal})}^{n-1}\]e_{ro}+{\mathrm{\η}}_5{e}_{ro}+{\mathrm{\η}}_6{e}_{ro}\end{array}$

图4展示了计算力F_rep的过程中的各种向量之间的关系。矢量F_rep1的方向障碍物指向机械臂，矢量F_rep2的方向为机械臂指向目标，显而易见F_rep1对机械臂产生斥力，F_rep2对机械臂产生引力。

因此，通过以上三个步骤，血凝仪的机械臂能够稳定运行，从初始点出发，沿途绕过动态的障碍物，达到目标位置，完成血凝仪取样任务。

以上所揭露的仅为本发明一种较佳实施例而已，不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。

Claims (2)

1.一种基于优化人工势场法的血凝仪动态避障路径规划方法，其特征在于该方法包括：

采用人工势场法对血凝仪机械臂的移动进行实时避障路径规划；

实时检测血凝仪机械臂所到达的位置点是否为局部稳定点；

若机械臂到达局部稳定点，则将引力场、吸引力、斥力场、斥力分别进行更新，从而打破稳定状态，使机械臂沿正确路径运行，并与目标保持相同的运动趋势；其中，更新后的引力场、吸引力、斥力场、斥力分别为：

引力场为：U_att(q)＝0.5ξ_q||q-q_g||²+0.5ξ_v||v-v_g||²+0.5ξ_a||a-a_g||²

吸引力为：

斥力场为：

斥力为：

式中，ξ_a、ξ_v、ξ_q为比例系数，q、v、a分别指机械臂的位置、速度和加速度，q_g、v_g、a_g分别指目标的位置、速度和加速度，q_obs、v_obs、a_obs分别指障碍物的位置、速度和加速度，||q-q_g||是机械臂与目标之间的相对距离，||v-v_g||是机械臂与目标之间的相对速度，||a-a_g||是机械臂与目标之间的相对加速度，为哈密顿算子，η₁、η₂、η₃、η₄、η₅、η₆为比例系数，ρ₀指障碍物的排斥力的最大范围，ρ(q)＝||q-q_obs||是机械臂与障碍物的最小距离，x_goal为目标的位置，(x-x_goal)为机械臂与目标之间的距离，v_ro＝(v-v_obs)^Te_ro为机械臂与障碍物的相对速度在二者连线上的分量，a_ro＝(a-a_obs)^Te_ro为机械臂与障碍物的相对加速在二者连线上的分量，e_ro为从机械臂指向障碍物的单位向量。

2.根据权利要求1所述的基于优化人工势场法的血凝仪动态避障路径规划方法，其特征在于：所述实时检测血凝仪机械臂所到达的位置点是否为局部稳定点，具体采用以下方法实现：

根据机械臂的移动步长以及斥力的作用范围设定局部稳定点的影响区域圆半径为R_l，根据机械臂的移动速度设定检测时间为t；

每隔t时间检测一次机械臂，获取机械臂上一次的位置坐标(x_before,y_before)和当前位置坐标(x_current,y_current)；

计算位置坐标(x_before,y_before)和位置坐标(x_current,y_current)之间的距离；

若计算得到的距离小于R_l，则认为机械臂当前的位置点为局部稳定点。