一種全向移動(dòng)機(jī)器人的動(dòng)態(tài)避障方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機(jī)器人領(lǐng)域，更具體地，涉及一種全向移動(dòng)機(jī)器人的動(dòng)態(tài)避障方法。
【背景技術(shù)】
[0002] 機(jī)器人能實(shí)現(xiàn)任意方向的運(yùn)動(dòng)，可廣泛應(yīng)用于軍事、工業(yè)、家用服務(wù)等領(lǐng)域。移動(dòng) 機(jī)器人在未知環(huán)境中如何有效避開障礙物是機(jī)器人實(shí)現(xiàn)其自主移動(dòng)的關(guān)鍵問題之一。在有 運(yùn)動(dòng)障礙物環(huán)境下的避障問題稱為動(dòng)態(tài)避障。與傳統(tǒng)靜態(tài)環(huán)境下避障不同，由于障礙物的 速度與位置有可能實(shí)時(shí)變化，未知環(huán)境中的動(dòng)態(tài)避障含有更多不確定因素，這對(duì)動(dòng)態(tài)避障 算法的實(shí)時(shí)性與傳感器精度都提出了更高的要求。目前對(duì)機(jī)器人避障的研究多集中在靜態(tài) 環(huán)境中的避障，或把運(yùn)動(dòng)障礙物在某個(gè)時(shí)刻考慮為是靜止的，這不利于避障的實(shí)時(shí)性要求， 尤其在遇到復(fù)雜環(huán)境中速度變化較快的障礙物時(shí)，采用這些方案易使機(jī)器人出現(xiàn)避障失敗 或多余避障的現(xiàn)象。
[0003] 移動(dòng)機(jī)器人的避障研究主要是通過設(shè)計(jì)障礙物檢測策略與避障控制算法，根據(jù)車 載傳感器反饋信息實(shí)時(shí)調(diào)整機(jī)器人的速度大小與方向進(jìn)行避障規(guī)劃，從而使機(jī)器人在行進(jìn) 過程中不與動(dòng)態(tài)及靜態(tài)障礙物發(fā)生任何的碰撞。人工勢(shì)場法在移動(dòng)機(jī)器人自主避障中得到 廣泛應(yīng)用，人工勢(shì)場法把目標(biāo)看作低勢(shì)能點(diǎn)，目標(biāo)對(duì)機(jī)器人產(chǎn)生了吸引力，環(huán)境中障礙物為 高勢(shì)能點(diǎn)，對(duì)機(jī)器人產(chǎn)生斥力，機(jī)器人在引力和斥力的共同作用下運(yùn)動(dòng)以躲避障礙物，并到 達(dá)目標(biāo)位置。由于人工勢(shì)場法僅考慮了移動(dòng)機(jī)器人與障礙物的位置信息，當(dāng)遇到復(fù)雜環(huán)境 下速度變化較快的障礙物時(shí)，人工勢(shì)場法易出現(xiàn)振蕩、多余避碰或避障失敗等現(xiàn)象。不同于 人工勢(shì)場法僅考慮位置信息，速度障礙法將碰撞規(guī)避問題轉(zhuǎn)化成規(guī)避速度障礙的問題。當(dāng) 存在多個(gè)障礙物時(shí)，計(jì)算傳感范圍內(nèi)所有障礙物對(duì)它的障礙區(qū)，然后根據(jù)速度障礙區(qū)所在 的空間位置關(guān)系，計(jì)算與速度障礙區(qū)均不相交的速度，以實(shí)現(xiàn)自主避障。當(dāng)未知環(huán)境中存在 多個(gè)動(dòng)態(tài)障礙時(shí)，傳統(tǒng)的速度障礙法所規(guī)劃的避障路徑易出現(xiàn)振蕩的現(xiàn)象，并且所規(guī)劃的 機(jī)器人速度較慢。D.B.J.Van等人提出了互惠式速度避障算法，該算法考慮了多機(jī)器人之間 的速度信息，利用外部定位系統(tǒng)提供的多個(gè)機(jī)器人的精確定位信息，在多機(jī)器人移動(dòng)的動(dòng) 態(tài)場景中表現(xiàn)出優(yōu)秀的相互避障性能?；セ菔剿俣缺苷纤惴ㄒ蕾囁@信息的對(duì)稱性，例如 在考慮兩個(gè)機(jī)器人之間的避碰時(shí)，單個(gè)機(jī)器人各自承擔(dān)一半的避障任務(wù)。另一方面，算法假 設(shè)移動(dòng)機(jī)器人能即刻達(dá)到任意所需的速度。在實(shí)際應(yīng)用中，由于存在檢測與估計(jì)誤差以及 環(huán)境變化過快等原因，所獲得的單個(gè)機(jī)器人的信息并不完全對(duì)稱，而且由于慣性機(jī)器人也 難以即刻達(dá)到所需的速度。在此情況下互惠式速度避障算法易出現(xiàn)避障不及時(shí)與多余避障 的現(xiàn)象。
[0004] 將運(yùn)動(dòng)區(qū)間內(nèi)的動(dòng)態(tài)障礙物作為移動(dòng)機(jī)器人，因此，機(jī)器人在未知?jiǎng)討B(tài)環(huán)境中的 自主避障可類比為多個(gè)移動(dòng)機(jī)器人之間的避碰。本方案基于速度避障策略設(shè)計(jì)動(dòng)態(tài)避障算 法，考慮互惠式速度避障算法的優(yōu)點(diǎn)并針對(duì)其局限性，本發(fā)明設(shè)計(jì)改進(jìn)的互惠式速度避障 算法，設(shè)置避障責(zé)任系數(shù)，并針對(duì)不同的障礙物引入風(fēng)險(xiǎn)因子，以改進(jìn)互惠式速度障礙算法 中由于測量誤差導(dǎo)致的避障不及時(shí)與多余避障的現(xiàn)象，并使規(guī)劃的避障軌跡更為平滑。在 沒有外部定位系統(tǒng)的未知?jiǎng)討B(tài)環(huán)境中，采用車載激光測距儀檢測障礙物并估計(jì)動(dòng)態(tài)障礙物 的運(yùn)動(dòng)速度，并基于所提出的避障方案實(shí)現(xiàn)機(jī)器人自主躲避運(yùn)動(dòng)區(qū)間內(nèi)的動(dòng)態(tài)及靜態(tài)障礙 物。

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

[0005] 本發(fā)明為解決以上現(xiàn)有技術(shù)的缺陷，提供了一種全向移動(dòng)機(jī)器人的動(dòng)態(tài)避障方 法，該方法能夠確保移動(dòng)機(jī)器人動(dòng)態(tài)避障的實(shí)時(shí)性與運(yùn)動(dòng)軌跡的平滑性，為未知?jiǎng)討B(tài)環(huán)境 中全向移動(dòng)機(jī)器人自主躲避動(dòng)態(tài)及靜態(tài)障礙物提供一種高適用性、有一定抗干擾能力的方 案。
[0006] 為實(shí)現(xiàn)以上發(fā)明目的，采用的技術(shù)方案是：
[0007] -種全向移動(dòng)機(jī)器人的動(dòng)態(tài)避障方法，包括以下步驟：
[0008]SI.對(duì)機(jī)器人周圍障礙物的位置進(jìn)行檢測，并根據(jù)機(jī)器人與障礙物相對(duì)位置的變 化計(jì)算二者的相對(duì)速度，然后在獲知機(jī)器人的速度的基礎(chǔ)上，計(jì)算障礙物的速度；
[0009]S2.設(shè)機(jī)器人i、障礙物j的半徑分別為巧與r,，速度為\與V,，位置為？1與p,， 則障礙物j對(duì)機(jī)器人i產(chǎn)生的速度障礙空間記為VOll,T，它表示導(dǎo)致機(jī)器人i與障礙物j在 時(shí)間T內(nèi)相碰撞的相對(duì)速度的集合：
[0011] 其中DCpj-Pi,ri+rj) = {q|I IQ-(Pj-Pi)I I<ri+rj表示以（P.廠Pi)為中心，（:Ti+r) 為半徑的圓域；
[0012] S3.計(jì)算機(jī)器人i和障礙物j的避障責(zé)任系數(shù)&1、aj，其具體步驟如下：
[0013]S31.初始時(shí)刻，設(shè)每個(gè)機(jī)器人或障礙物初始速度的大小相同，按照各自從起點(diǎn)到 目標(biāo)點(diǎn)的距離從大到小進(jìn)行排序以分配責(zé)任系數(shù)，其中，距離目標(biāo)點(diǎn)最遠(yuǎn)的機(jī)器人或障礙 物的責(zé)任系數(shù)為1，距離目標(biāo)點(diǎn)最近的機(jī)器人或障礙物的責(zé)任系數(shù)為為機(jī)器人和障礙 物的數(shù)量；
[0014]S32.在行進(jìn)過程中，把機(jī)器人或障礙物到各自目標(biāo)點(diǎn)的距離除以機(jī)器人或障礙物 自身的速度大小作為需耗費(fèi)的時(shí)間值，然后根據(jù)該時(shí)間值按從大到小的順序重新排序，以 此更新每個(gè)機(jī)器人的責(zé)任系數(shù)，時(shí)間值最大的機(jī)器人的責(zé)任系數(shù)為1，時(shí)間值最小的機(jī)器人 的責(zé)任系數(shù)為n」
[0015]S4.計(jì)算障礙物j對(duì)機(jī)器人i的風(fēng)險(xiǎn)因子，具體如下：
[0016] (1)機(jī)器人i與障礙物j的相對(duì)速度在速度障礙空間的外部時(shí)，采用機(jī)器人i和障 礙物j之間的距離來表示障礙物j對(duì)機(jī)器人i的風(fēng)險(xiǎn)因子X1U:
[0018]其中I|Pl，P]|I表示機(jī)器人i和障礙物j之間的相對(duì)距離，d_、d_分別表示相對(duì) 距離的最大閾值和最小閾值；
[0019] ⑵機(jī)器人i與障礙物j的相對(duì)速度在速度障礙空間的內(nèi)部時(shí)，計(jì)算兩者的碰撞時(shí) 間Llj，并采用碰撞時(shí)間tlM和相對(duì)距離來確定風(fēng)險(xiǎn)因子的大小：
[0021] 其中t_、t_表示碰撞時(shí)間的兩個(gè)閾值；
[0022] S4.根據(jù)計(jì)算得到的風(fēng)險(xiǎn)因子Xl|]來確定機(jī)器人i與障礙物j的相對(duì)速度的改變 量Avij:
[0024]其中U為機(jī)器人i和障礙物j的相對(duì)速度（V1-Vj)到VOllj 界長度最小的向量，
[0025]S5.求取機(jī)器人i、障礙物j的速度改變量，表示如下
[0027] 其中aj-a! =M;
[0028] S6.求取機(jī)器人i和障礙物j在時(shí)間T內(nèi)不會(huì)相撞的速度集合ORCA1,jT，ORCA1ijT 是一個(gè)以Vi+AVi為起始點(diǎn)，以n為方向的半平面：
[0029] ORCAiIjT={vI (V-(VAVi)) ?n^ 0}
[0030] 若nr> 2,可通過下式構(gòu)造ORCA;T集合：
[0032] D(0,Vlmax)代表機(jī)器人的速度限制；
[0033] S7.根據(jù)構(gòu)造的ORCA1T，求解機(jī)器人i的更新速度Vladiust6d:
[0035] 更新速度Vlad]usted為集合ORCAiT中最接近期待速度Vrf的值。
[0036] 優(yōu)選地，所述步驟Sl中，使用激光測距儀對(duì)機(jī)器人與障礙物的距離進(jìn)行檢測，然 后根據(jù)檢測的數(shù)據(jù)確定障礙物相對(duì)機(jī)器人的位置，其具體過程如下：
[0037] S11.建立掃描窗對(duì)激光測距儀回傳的掃描數(shù)據(jù)進(jìn)行處理，掃描窗的半徑設(shè)為 cU_，m為數(shù)據(jù)點(diǎn)的序列號(hào)，Clni為第m個(gè)數(shù)據(jù)距離激光測距儀的距離值，并且ddset_;
[0038] S12.設(shè)相鄰兩個(gè)數(shù)據(jù)點(diǎn)對(duì)應(yīng)的距離的差值為ICU-ClniI，根據(jù)ICU-ClniI使用S121的 方法判斷數(shù)據(jù)點(diǎn)對(duì)應(yīng)的障礙物檢測情況；
[0039] S13.假設(shè)第j個(gè)障礙物表示為 <9/(A^egin，iVfd,iVfddkV//)，其中A^begm、 ~廣、分別表示第個(gè)障礙物在掃描窗內(nèi)的起始點(diǎn)、結(jié)束點(diǎn)、中間點(diǎn)的序列號(hào)； d] 為中間點(diǎn)A^llddle與激光測距儀的距離，把Clj近似為障礙物j到機(jī)器人i幾何中心的相對(duì)距 離，由此可估計(jì)障礙物j相對(duì)機(jī)器人i的位置；
[0040] 所述步驟S121的操作過程具體如下：
[0041] A.判斷Clni是否與dset_相等，若否，執(zhí)行步驟B，否則執(zhí)行步驟D;
[0042] B.判斷cU是否與dset_相等，若是，則表示檢測不到障礙物，否則執(zhí)行步驟C;
[0043] C.判斷Idni-CUl是否大于設(shè)定的閾值Clhcild，若是則表示檢測到了兩個(gè)障礙物，若 否則表示這兩個(gè)數(shù)據(jù)點(diǎn)檢測到的障礙物為同一障礙物；
[0044] D.判斷cU是否與dset_相等，若是，則表示檢測不到障礙物，否則執(zhí)行步驟F;
[0045] F.判斷dm+k是否與dSf；tmax相等，若是則執(zhí)行步驟G，若否則令k=k+1，然后執(zhí)行步 驟F;
[0046] G.判斷k是否大于設(shè)定的閾值nhc]ld，若是則表示檢測到了新的障礙物，若否則表示 檢測不到障礙物。
[0047] 其中，nhc]ld為設(shè)置的有效障礙物數(shù)據(jù)點(diǎn)數(shù)的閾值，采用動(dòng)態(tài)閾值法設(shè)置該閾值，以 此減少避障過程中由于外部干擾產(chǎn)生的障礙物誤判：障礙物距離機(jī)器人越近，Iihcild的值設(shè) 置得越大；障礙物距離機(jī)器人越遠(yuǎn)，nhc]ld的值設(shè)置得越小。
[0048] 優(yōu)選地，步驟Sl中，計(jì)算機(jī)器人與障礙物相對(duì)速度的具體過程如下：
[0049] 設(shè)在t時(shí)刻檢測到