基于优化人工势场法的血凝仪动态避障路径规划方法
【专利摘要】本发明公开了一种基于优化人工势场法的血凝仪动态避障路径规划方法，该方法包括：采用人工势场法对血凝仪机械臂的移动进行实时避障路径规划；实时检测血凝仪机械臂所到达的位置点是否为局部稳定点；若是局部稳定点，则将引力场、吸引力、斥力场、斥力分别进行更新，从而打破稳定状态，使机械臂沿正确路径运行，并与目标保持相同的运动趋势。本发明适用于动态环境中的路径规划。
【专利说明】
基于优化人工势场法的血凝仪动态避障路径规划方法
技术领域
[0001] 本发明设及血凝仪路径规划方法，尤其设及一种基于优化人工势场法的血凝仪动 态避障路径规划方法。
【背景技术】
[0002] 20世纪W来，随着人类文明得到的飞速发展，促进了人类在生命科学和生物工程 领域的深入研究，尤其在医学方面，如何有效快速的对疾病进行检测和治疗，是当今人类发 展进程中的重要研究课题，因此研究先进的医学检验仪器对促进检验医学的发展有着重要 意义，而血凝仪[王雪峰，王宏利，血液凝固分析仪[J]，现代医学仪器与应用.2000.12(3): 7-10]大大方便了医护人员的工作任务。
[0003] 机器人的运动规划传统上被看作是一个优化问题，其中机器人的位置在一个参数 空间表示。通过尝试在参数空间中捜索满足预定义的成本函数(如机器人与目标位置的距 离）的参数来寻找解决问题的方案。该方法的局限时在处理设及不可预料的障碍的动态环 境时，产生一个新的方案的计算代价太高。作为机械臂与移动机器人的一个更实际的无碰 撞运动的实时规划算法，人工势场法[姜德，谢勇.人工势场法规划方法[J].西南大学学报： 科学版，2013,29(5) :128-132]将动态感知到的反馈结合到机器人的控制中，从而克服了扩 充低级运动控制器的反应能力局限。
[0004] 人工势场法是实现机器人路径规划的一种重要算法，它的基本思想是将机器人在 周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生"引 力"，障碍物对移动机器人产生"斥力"，最后通过求合力来控制移动机器人的运动。应用势 场法规划出来的路径一般是比较平滑并且安全。
[0005] 但是人工势场法存在两个问题：（1)存在局部稳定点（或局部最小点、局部最优点） 问题，是指移动机器人机械臂由于一些因素导致机械臂受到的引力和斥力平衡，达到稳定， 此时，机器人会产生死锁现象，但运时机械臂并没有达到目标点。（2)当目标附近有障碍物 时，移动机器人将永远也到达不了目的地，具体是由于在W前的许多研究中，目标和障碍物 都离的很远，当机器人逼近目标时，障碍物的斥力变的很小，甚至可W忽略，机器人将只受 到吸引力的作用而直达目标，但在许多实际环境中，往往至少有一个障碍物与目标点离的 很近，在运种情况下，当移动机器人逼近目标的同时，它也将向障碍物靠近，如果利用W前 对引力场函数和斥力场函数的定义，斥力将比引力大的多，运样目标点将不是整个势场的 局部稳定点，因此移动机器人将不可能到达目标。
[0006] 另外，在过去的研究中，很多路径规划都是将人工势场法应用于静态环境中，即目 标和障碍物为静止不动的，然而，在实际应用中，机器人所处的环境大多是动态的，且不仅 障碍物是运动的，甚至目标也是运动的。
【发明内容】
[0007] 发明目的：本发明针对现有技术存在的问题，提供一种基于优化人工势场法的血 凝仪动态避障路径规划方法，该方法尤其适用于动态环境中的路径规划。
[0008] 技术方案:本发明所述的基于优化人工势场法的血凝仪动态避障路径规划方法包 括：
[0009] 采用人工势场法对血凝仪机械臂的移动进行实时避障路径规划；
[0010] 实时检测血凝仪机械臂所到达的位置点是否为局部稳定点；
[0011] 若是局部稳定点，则将引力场、吸引力、斥力场、斥力分别进行更新，从而打破稳定 状态，使机械臂沿正确路径运行，并与目标保持相同的运动趋势;其中，更新后的引力场、吸 引力、斥力场、斥力分别为：
[001^ 弓巧场为:Uatt(q)=0.5Cq| k-qg| |2+0.5Cv| Iv-Vgl |2+0.5Ca| |a-ag| |2
[001引吸弓巧；
[0014] 斥力场；
[001引斥力为：
[OOW 式中，Ca、Cv、Cq为比例系数，q、V、a分别指机械臂的位置、速度和加速度，Qg、Vg、ag分 别指目标的位置、速度和加速度，9。63、巾。63、曰。63分别指障碍物的位置、速度和加速度，||9-98 I是机械臂与目标之间的相对距离，I Iv-VgM是机械臂与目标之间的相对速度，I |a-ag||是 机械臂与目标之间的相对加速度，V为哈密顿算子，m、屯、ri3、ru、ri日、％为比例系数，PO指障碍 物的排斥力的最大范围，P(q) = I I q-q0bs M是机械臂与障碍物的最小距离，Xgoal为目标的位 置，（X-Xgoal )为机械臂与目标之间的距离，Vro =( V-Vobs ) Ter。为机械臂与障碍物的相对速度 在二者连线上的分量，日"=(日-日。63)了日,。为机械臂与障碍物的相对加速在二者连线上的分 量，er。为从机械臂指向障碍物的单位向量。
[0017] 进一步的，所述实时检测血凝仪机械臂所到达的位置点是否为局部稳定点，具体 采用W下方法实现：
[0018] 根据机械臂的移动步长W及斥力的作用范围设定局部稳定点的影响区域圆半径 为Ri,根据机械臂的移动速度设定检测时间为t;
[0019]每隔t时间检测一次机械臂，获取机械臂上一次的位置坐标(乱6:。,6,766加6)和当前 ( Xcurrent, Jcurrent)；
[0020] 计算位置坐标(Xbefore，ybefore)和位置坐标(Xcurrent，ycurrent)之间的距离；
[0021] 若计算得到的距离小于Rl,则认为机械臂当前的位置点为局部稳定点。
[0022] 有益效果:本发明与现有技术相比，其显著优点是:本发明中研究的障碍物W动态 障碍物为主，在运种人工势场的作用下，移动机械臂所受的虚拟力不仅与运动之间相对位 置有关，而且与相对速度，甚至相对加速度都有关，因此将引力场和斥力场都进行改进更 新，使得机器人时刻趋向于保持与目标相同的运动趋势，同时保持与障碍物相惇的运动趋 势，还可W解决局部稳定点问题。另外，采用本发明，机械臂运动更加平滑，速度更加稳定， 提高精度的同时降低成本。
【附图说明】
[0023] 图1是本发明的一个实施例的系统框图；
[0024] 图2是机械臂进入局部稳定点的区域图；
[0025] 图3是计算吸引力过程各向量之间的关系；
[0026] 图4是本发明排斥力过程中各向量之间的关系。
【具体实施方式】
[0027] 如图1所示，本实施例的基于优化人工势场法的血凝仪动态避障路径规划方法包 括W下步骤：
[00%] Sl、采用人工势场法对血凝仪机械臂的移动进行实时避障路径规划。
[0029] 传统势场的函数为：
[0030] 引力场3
[0031] 斥力场3
[00创引力为:：
[0033] 斥力为：
[0034] 其中，Pg(q)= I k-qg||指机械臂到目标的距离，q、qg分别指机械臂和目标的位置， C、n为比例系数，PO指障碍物的排斥力的最大范围，p(q)指障碍物与机械臂的最小距离， LUU。。」
T金测血凝仪机械臂所到达的位置点是否为局部稳定点。
[0036] 尽管人工势场法中将移动机械臂可看成是一个可移动的质点，但当机械臂陷入局 部稳定点问题时，机械臂并不会停在一个固定的点上，更多的情况是，机械臂停留在某一个 很小的区域内徘徊或者打转，机械臂可W原点打转，也可W是W某一个半径做圆周运动，如 图2所示。
[0037] 因此，采用的检测方法具体为：（1)根据机械臂的移动步长W及斥力的作用范围设 定局部稳定点的影响区域圆半径为化，根据机械臂的移动速度设定检测时间为t; (2)每隔t 时间检测一次机械臂，获取机械臂上一次的位置坐标Ubefore ,ybefore)和当前位置坐标 (Xcurrent, Ycurrent);( 3 )计算位置坐柄^ ( Xbefore , Ybefore )和位置坐柄^ ( Xcurrent, Ycurrent)之间的距 离；（4)若计算得到的距离小于Ri,则认为机械臂当前的位置点为局部稳定点。
[0038] S3、若是局部稳定点，则将引力场、吸引力、斥力场、斥力分别进行更新，从而打破 稳定状态，使机械臂沿正确路径运行，并与目标保持相同的运动趋势。
[0039] (I)更新后的引力场为：
[0040] Uatt(q) =0.巧q I I q-qg I 12+0.5Cv M V-Vg I 12+0.5Ca M a-ag I 12，
[0041 ]式中，U Cv、Cq为比例系数，q、V、a分别指机械臂的位置、速度和加速度，qg、Vg、ag分 别指目标的位置、速度和加速度，I I q-qg M是机械臂与目标之间的相对距离，V-Vg I I是机械 臂与目标之间的相对速度，I I a-ag I I是机械臂与目标之间的相对加速度。由引力场函数可 知，相对位置、相对速度和相对加速减小时，引力场数值也随之减小。
[0042] 由更新后的引力场可W得到吸引力为：
，令Fatt(q，v，a)=Fatt (q)+Fatt(V)+Fatt(曰），贝lJFatt(q)=写q||q-qg||eqrg，Fatt(V) = 写v||v_Vg||evrg，Fatt(曰）=写a||曰一 agllearg，其中，eq巧是从机械臂位置指向目标位置的单位向量，evrg，ea巧也是单位向量，其方 向分别为目标点与机械臂的相对速度和相对加速度方向。图3展示了计算力Fatt的过程中， 各个向量的关系。从图3中可W看出，Fattq作用于机械臂使其跟踪目标点的位置坐标;Fattv作 用于机械臂使其趋向于保持与目标点相同的速度，Fatta作用于机器人使其趋向于保持与目 标点相同的加速度。
[0043] (2)更新后斥力场为：
[0044]
[0045] 式中，Tli、ri2、ri3、TU、n日、％为比例系数，q〇bs、Vobs、a〇bs分别指障碍物的位置、速度和加 速度，PO指障碍物的排斥力的最大范围，p(q) = I k-qobs M是机械臂与障碍物的最小距离， Xgoal为目标的位置，（X-Xgoal)为机械臂与目标之间的距离，Vro= (V-Vobs)Tero为机械臂与障 碍物的相对速度在二者连线上的分量，日~=(日-日。63)了日,。为机械臂与障碍物的相对加速在二 者连线上的分量，er。为从机械臂指向障碍物的单位向量。时，机械臂背离障碍物运 动，不采取避障行为，Vr。>0时，机械臂朝向障碍物运动，需要采用避障行为。
[0046] 由斥力场得到斥力为：
[0047]
[004引 其中：
[0049]
[(K)加 ]
[0化1 ]
[0化2]
[0化3]
[0化4]
[0化5]
[0056] 图4展示了计算力Frep的过程中的各种向量之间的关系。矢量Frepl的方向障碍物指 向机械臂，矢量Frep2的方向为机械臂指向目标，显而易见Frepl对机械臂产生斥力，Frep拥机 械臂产生引力。
[0057] 因此，通过W上=个步骤，血凝仪的机械臂能够稳定运行，从初始点出发，沿途绕 过动态的障碍物，达到目标位置，完成血凝仪取样任务。
[005引 W上所掲露的仅为本发明一种较佳实施例而已，不能W此来限定本发明之权利范 围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。
【主权项】
1. 一种基于优化人工势场法的血凝仪动态避障路径规划方法，其特征在于该方法包 括： 采用人工势场法对血凝仪机械臂的移动进行实时避障路径规划； 实时检测血凝仪机械臂所到达的位置点是否为局部稳定点； 若机械臂到达局部稳定点，则将引力场、吸引力、斥力场、斥力分别进行更新，从而打破 稳定状态，使机械臂沿正确路径运行，并与目标保持相同的运动趋势;其中，更新后的引力 场、吸引力、斥力场、斥力分别为：式中，、ξν、Iq为比例系数，q、V、a分别指机械臂的位置、速度和加速度，qg、Vg、ag分别指 目标的位置、速度和加速度，9。^、￥。^、3。^分别指障碍物的位置、速度和加速度，||9-98||是 机械臂与目标之间的相对距离，I |v-vg| I是机械臂与目标之间的相对速度，I |a_ag| I是机械 臂与目标之间的相对加速度，▽为哈密顿算子，111、112、113、114、115、116为比例系数，0()指障碍物的 排斥力的最大范围，P(q) = I |q-q〇bs I I是机械臂与障碍物的最小距离，Xgcml为目标的位置， (X-XgciaI)为机械臂与目标之间的距离，^。=~1。^，&。为机械臂与障碍物的相对速度在二 者连线上的分量，&1^。=(&-&。^) 101^。为机械臂与障碍物的相对加速在二者连线上的分量，61·。 为从机械臂指向障碍物的单位向量。2. 根据权利要求1所述的基于优化人工势场法的血凝仪动态避障路径规划方法，其特 征在于:所述实时检测血凝仪机械臂所到达的位置点是否为局部稳定点，具体采用以下方 法实现： 根据机械臂的移动步长以及斥力的作用范围设定局部稳定点的影响区域圆半径为Ri， 根据机械臂的移动速度设定检测时间为t; 每隔t时间检测一次机械臂，获取机械臂上一次的位置坐标(1^_，％#_)和当前位置 坐标(X current，ycurrent); 计算彳!/·置坐丰不(Xbefore，ybefore )和vIiL置坐杨^ ( Xcurrent，ycurrent) 间的距 1? ; 若计算得到的距离小于Ri，则认为机械臂当前的位置点为局部稳定点。
【文档编号】G05D1/02GK106020199SQ201610528044
【公开日】2016年10月12日
【申请日】2016年7月6日
【发明人】王直, 付青
【申请人】江苏科技大学