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

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

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