基于優(yōu)化人工勢場法的血凝儀動態(tài)避障路徑規(guī)劃方法
【專利摘要】本發(fā)明公開了一種基于優(yōu)化人工勢場法的血凝儀動態(tài)避障路徑規(guī)劃方法，該方法包括：采用人工勢場法對血凝儀機械臂的移動進行實時避障路徑規(guī)劃；實時檢測血凝儀機械臂所到達的位置點是否為局部穩(wěn)定點；若是局部穩(wěn)定點，則將引力場、吸引力、斥力場、斥力分別進行更新，從而打破穩(wěn)定狀態(tài)，使機械臂沿正確路徑運行，并與目標保持相同的運動趨勢。本發(fā)明適用于動態(tài)環(huán)境中的路徑規(guī)劃。
【專利說明】
基于優(yōu)化人工勢場法的血凝儀動態(tài)避障路徑規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及血凝儀路徑規(guī)劃方法，尤其設(shè)及一種基于優(yōu)化人工勢場法的血凝儀動 態(tài)避障路徑規(guī)劃方法。
【背景技術(shù)】
[0002] 20世紀W來，隨著人類文明得到的飛速發(fā)展，促進了人類在生命科學和生物工程 領(lǐng)域的深入研究，尤其在醫(yī)學方面，如何有效快速的對疾病進行檢測和治療，是當今人類發(fā) 展進程中的重要研究課題，因此研究先進的醫(yī)學檢驗儀器對促進檢驗醫(yī)學的發(fā)展有著重要 意義，而血凝儀[王雪峰，王宏利，血液凝固分析儀[J]，現(xiàn)代醫(yī)學儀器與應用.2000.12(3): 7-10]大大方便了醫(yī)護人員的工作任務(wù)。
[0003] 機器人的運動規(guī)劃傳統(tǒng)上被看作是一個優(yōu)化問題，其中機器人的位置在一個參數(shù) 空間表示。通過嘗試在參數(shù)空間中捜索滿足預定義的成本函數(shù)(如機器人與目標位置的距 離）的參數(shù)來尋找解決問題的方案。該方法的局限時在處理設(shè)及不可預料的障礙的動態(tài)環(huán) 境時，產(chǎn)生一個新的方案的計算代價太高。作為機械臂與移動機器人的一個更實際的無碰 撞運動的實時規(guī)劃算法，人工勢場法[姜德，謝勇.人工勢場法規(guī)劃方法[J].西南大學學報： 科學版，2013,29(5) :128-132]將動態(tài)感知到的反饋結(jié)合到機器人的控制中，從而克服了擴 充低級運動控制器的反應能力局限。
[0004] 人工勢場法是實現(xiàn)機器人路徑規(guī)劃的一種重要算法，它的基本思想是將機器人在 周圍環(huán)境中的運動，設(shè)計成一種抽象的人造引力場中的運動，目標點對移動機器人產(chǎn)生"引 力"，障礙物對移動機器人產(chǎn)生"斥力"，最后通過求合力來控制移動機器人的運動。應用勢 場法規(guī)劃出來的路徑一般是比較平滑并且安全。
[0005] 但是人工勢場法存在兩個問題：（1)存在局部穩(wěn)定點（或局部最小點、局部最優(yōu)點） 問題，是指移動機器人機械臂由于一些因素導致機械臂受到的引力和斥力平衡，達到穩(wěn)定， 此時，機器人會產(chǎn)生死鎖現(xiàn)象，但運時機械臂并沒有達到目標點。（2)當目標附近有障礙物 時，移動機器人將永遠也到達不了目的地，具體是由于在W前的許多研究中，目標和障礙物 都離的很遠，當機器人逼近目標時，障礙物的斥力變的很小，甚至可W忽略，機器人將只受 到吸引力的作用而直達目標，但在許多實際環(huán)境中，往往至少有一個障礙物與目標點離的 很近，在運種情況下，當移動機器人逼近目標的同時，它也將向障礙物靠近，如果利用W前 對引力場函數(shù)和斥力場函數(shù)的定義，斥力將比引力大的多，運樣目標點將不是整個勢場的 局部穩(wěn)定點，因此移動機器人將不可能到達目標。
[0006] 另外，在過去的研究中，很多路徑規(guī)劃都是將人工勢場法應用于靜態(tài)環(huán)境中，即目 標和障礙物為靜止不動的，然而，在實際應用中，機器人所處的環(huán)境大多是動態(tài)的，且不僅 障礙物是運動的，甚至目標也是運動的。

【發(fā)明內(nèi)容】

[0007] 發(fā)明目的：本發(fā)明針對現(xiàn)有技術(shù)存在的問題，提供一種基于優(yōu)化人工勢場法的血 凝儀動態(tài)避障路徑規(guī)劃方法，該方法尤其適用于動態(tài)環(huán)境中的路徑規(guī)劃。
[0008] 技術(shù)方案:本發(fā)明所述的基于優(yōu)化人工勢場法的血凝儀動態(tài)避障路徑規(guī)劃方法包 括：
[0009] 采用人工勢場法對血凝儀機械臂的移動進行實時避障路徑規(guī)劃；
[0010] 實時檢測血凝儀機械臂所到達的位置點是否為局部穩(wěn)定點；
[0011] 若是局部穩(wěn)定點，則將引力場、吸引力、斥力場、斥力分別進行更新，從而打破穩(wěn)定 狀態(tài)，使機械臂沿正確路徑運行，并與目標保持相同的運動趨勢;其中，更新后的引力場、吸 引力、斥力場、斥力分別為：
[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為比例系數(shù)，q、V、a分別指機械臂的位置、速度和加速度，Qg、Vg、ag分 別指目標的位置、速度和加速度，9。63、巾。63、曰。63分別指障礙物的位置、速度和加速度，||9-98 I是機械臂與目標之間的相對距離，I Iv-VgM是機械臂與目標之間的相對速度，I |a-ag||是 機械臂與目標之間的相對加速度，V為哈密頓算子，m、屯、ri3、ru、ri日、％為比例系數(shù)，PO指障礙 物的排斥力的最大范圍，P(q) = I I q-q0bs M是機械臂與障礙物的最小距離，Xgoal為目標的位 置，（X-Xgoal )為機械臂與目標之間的距離，Vro =( V-Vobs ) Ter。為機械臂與障礙物的相對速度 在二者連線上的分量，日"=(日-日。63)了日,。為機械臂與障礙物的相對加速在二者連線上的分 量，er。為從機械臂指向障礙物的單位向量。
[0017] 進一步的，所述實時檢測血凝儀機械臂所到達的位置點是否為局部穩(wěn)定點，具體 采用W下方法實現(xiàn)：
[0018] 根據(jù)機械臂的移動步長W及斥力的作用范圍設(shè)定局部穩(wěn)定點的影響區(qū)域圓半徑 為Ri,根據(jù)機械臂的移動速度設(shè)定檢測時間為t;
[0019]每隔t時間檢測一次機械臂，獲取機械臂上一次的位置坐標(亂6:。,6,766加6)和當前 ( Xcurrent, Jcurrent)；
[0020] 計算位置坐標(Xbefore，ybefore)和位置坐標(Xcurrent，ycurrent)之間的距離；
[0021] 若計算得到的距離小于Rl,則認為機械臂當前的位置點為局部穩(wěn)定點。
[0022] 有益效果:本發(fā)明與現(xiàn)有技術(shù)相比，其顯著優(yōu)點是:本發(fā)明中研究的障礙物W動態(tài) 障礙物為主，在運種人工勢場的作用下，移動機械臂所受的虛擬力不僅與運動之間相對位 置有關(guān)，而且與相對速度，甚至相對加速度都有關(guān)，因此將引力場和斥力場都進行改進更 新，使得機器人時刻趨向于保持與目標相同的運動趨勢，同時保持與障礙物相惇的運動趨 勢，還可W解決局部穩(wěn)定點問題。另外，采用本發(fā)明，機械臂運動更加平滑，速度更加穩(wěn)定， 提高精度的同時降低成本。
【附圖說明】
[0023] 圖1是本發(fā)明的一個實施例的系統(tǒng)框圖；
[0024] 圖2是機械臂進入局部穩(wěn)定點的區(qū)域圖；
[0025] 圖3是計算吸引力過程各向量之間的關(guān)系；
[0026] 圖4是本發(fā)明排斥力過程中各向量之間的關(guān)系。
【具體實施方式】
[0027] 如圖1所示，本實施例的基于優(yōu)化人工勢場法的血凝儀動態(tài)避障路徑規(guī)劃方法包 括W下步驟：
[00%] Sl、采用人工勢場法對血凝儀機械臂的移動進行實時避障路徑規(guī)劃。
[0029] 傳統(tǒng)勢場的函數(shù)為：
[0030] 引力場3
[0031] 斥力場3
[00創(chuàng)引力為:：
[0033] 斥力為：
[0034] 其中，Pg(q)= I k-qg||指機械臂到目標的距離，q、qg分別指機械臂和目標的位置， C、n為比例系數(shù)，PO指障礙物的排斥力的最大范圍，p(q)指障礙物與機械臂的最小距離， LUU。?！?br>T金測血凝儀機械臂所到達的位置點是否為局部穩(wěn)定點。
[0036] 盡管人工勢場法中將移動機械臂可看成是一個可移動的質(zhì)點，但當機械臂陷入局 部穩(wěn)定點問題時，機械臂并不會停在一個固定的點上，更多的情況是，機械臂停留在某一個 很小的區(qū)域內(nèi)徘徊或者打轉(zhuǎn)，機械臂可W原點打轉(zhuǎn)，也可W是W某一個半徑做圓周運動，如 圖2所示。
[0037] 因此，采用的檢測方法具體為：（1)根據(jù)機械臂的移動步長W及斥力的作用范圍設(shè) 定局部穩(wěn)定點的影響區(qū)域圓半徑為化，根據(jù)機械臂的移動速度設(shè)定檢測時間為t; (2)每隔t 時間檢測一次機械臂，獲取機械臂上一次的位置坐標Ubefore ,ybefore)和當前位置坐標 (Xcurrent, Ycurrent);( 3 )計算位置坐柄^ ( Xbefore , Ybefore )和位置坐柄^ ( Xcurrent, Ycurrent)之間的距 離；（4)若計算得到的距離小于Ri,則認為機械臂當前的位置點為局部穩(wěn)定點。
[0038] S3、若是局部穩(wěn)定點，則將引力場、吸引力、斥力場、斥力分別進行更新，從而打破 穩(wěn)定狀態(tài)，使機械臂沿正確路徑運行，并與目標保持相同的運動趨勢。
[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為比例系數(shù)，q、V、a分別指機械臂的位置、速度和加速度，qg、Vg、ag分 別指目標的位置、速度和加速度，I I q-qg M是機械臂與目標之間的相對距離，V-Vg I I是機械 臂與目標之間的相對速度，I I a-ag I I是機械臂與目標之間的相對加速度。由引力場函數(shù)可 知，相對位置、相對速度和相對加速減小時，引力場數(shù)值也隨之減小。
[0042] 由更新后的引力場可W得到吸引力為：
，令Fatt(q，v，a)=Fatt (q)+Fatt(V)+Fatt(曰），貝lJFatt(q)=寫q||q-qg||eqrg，F(xiàn)att(V) = 寫v||v_Vg||evrg，F(xiàn)att(曰）=寫a||曰一 agllearg，其中，eq巧是從機械臂位置指向目標位置的單位向量，evrg，ea巧也是單位向量，其方 向分別為目標點與機械臂的相對速度和相對加速度方向。圖3展示了計算力Fatt的過程中， 各個向量的關(guān)系。從圖3中可W看出，F(xiàn)attq作用于機械臂使其跟蹤目標點的位置坐標;Fattv作 用于機械臂使其趨向于保持與目標點相同的速度，F(xiàn)atta作用于機器人使其趨向于保持與目 標點相同的加速度。
[0043] (2)更新后斥力場為：
[0044]
[0045] 式中，Tli、ri2、ri3、TU、n日、％為比例系數(shù)，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的過程中的各種向量之間的關(guān)系。矢量Frepl的方向障礙物指 向機械臂，矢量Frep2的方向為機械臂指向目標，顯而易見Frepl對機械臂產(chǎn)生斥力，F(xiàn)rep擁機 械臂產(chǎn)生引力。
[0057] 因此，通過W上=個步驟，血凝儀的機械臂能夠穩(wěn)定運行，從初始點出發(fā)，沿途繞 過動態(tài)的障礙物，達到目標位置，完成血凝儀取樣任務(wù)。
[005引 W上所掲露的僅為本發(fā)明一種較佳實施例而已，不能W此來限定本發(fā)明之權(quán)利范 圍，因此依本發(fā)明權(quán)利要求所作的等同變化，仍屬本發(fā)明所涵蓋的范圍。
【主權(quán)項】
1. 一種基于優(yōu)化人工勢場法的血凝儀動態(tài)避障路徑規(guī)劃方法，其特征在于該方法包 括： 采用人工勢場法對血凝儀機械臂的移動進行實時避障路徑規(guī)劃； 實時檢測血凝儀機械臂所到達的位置點是否為局部穩(wěn)定點； 若機械臂到達局部穩(wěn)定點，則將引力場、吸引力、斥力場、斥力分別進行更新，從而打破 穩(wěn)定狀態(tài)，使機械臂沿正確路徑運行，并與目標保持相同的運動趨勢;其中，更新后的引力 場、吸引力、斥力場、斥力分別為：式中，、ξν、Iq為比例系數(shù)，q、V、a分別指機械臂的位置、速度和加速度，qg、Vg、ag分別指 目標的位置、速度和加速度，9。^、￥。^、3。^分別指障礙物的位置、速度和加速度，||9-98||是 機械臂與目標之間的相對距離，I |v-vg| I是機械臂與目標之間的相對速度，I |a_ag| I是機械 臂與目標之間的相對加速度，▽為哈密頓算子，111、112、113、114、115、116為比例系數(shù)，0()指障礙物的 排斥力的最大范圍，P(q) = I |q-q〇bs I I是機械臂與障礙物的最小距離，Xgcml為目標的位置， (X-XgciaI)為機械臂與目標之間的距離，^。=~1。^，&。為機械臂與障礙物的相對速度在二 者連線上的分量，&1^。=(&-&。^) 101^。為機械臂與障礙物的相對加速在二者連線上的分量，61·。 為從機械臂指向障礙物的單位向量。2. 根據(jù)權(quán)利要求1所述的基于優(yōu)化人工勢場法的血凝儀動態(tài)避障路徑規(guī)劃方法，其特 征在于:所述實時檢測血凝儀機械臂所到達的位置點是否為局部穩(wěn)定點，具體采用以下方 法實現(xiàn)： 根據(jù)機械臂的移動步長以及斥力的作用范圍設(shè)定局部穩(wěn)定點的影響區(qū)域圓半徑為Ri， 根據(jù)機械臂的移動速度設(shè)定檢測時間為t; 每隔t時間檢測一次機械臂，獲取機械臂上一次的位置坐標(1^_，％#_)和當前位置 坐標(X current，ycurrent); 計算彳!/·置坐豐不(Xbefore，ybefore )和vIiL置坐楊^ ( Xcurrent，ycurrent) 間的距 1? ; 若計算得到的距離小于Ri，則認為機械臂當前的位置點為局部穩(wěn)定點。
【文檔編號】G05D1/02GK106020199SQ201610528044
【公開日】2016年10月12日
【申請日】2016年7月6日
【發(fā)明人】王直, 付青
【申請人】江蘇科技大學