---

前言

今天閑的沒事，正好是10/24，CSDN有活動(dòng)，就把之前做的一些多機(jī)器人編隊(duì)避障算法（主要是純控制避障）梳理小結(jié)下( ?? ω ?? )?

---

一、基于感知的避障

1.基于感知的Epuck2避障思路(理論)

Ri為人工勢(shì)場(chǎng)的斥力項(xiàng)，假設(shè)在k時(shí)刻機(jī)器人可得到一定范圍內(nèi)的障礙物坐標(biāo)Xobs ，障礙物會(huì)對(duì)機(jī)器人 的速度產(chǎn)生一個(gè)斥力的影響R：
R的計(jì)算公式為：

其中M為障礙物個(gè)數(shù)，dm為探測(cè)距離，在此時(shí)刻避障響應(yīng)R（k）會(huì)對(duì)機(jī)器人的速度控制產(chǎn)生一個(gè)影響。

2.基于感知的Epuck2避障實(shí)現(xiàn)(現(xiàn)實(shí))

在Epuck2實(shí)機(jī)避障時(shí)，最終的控制輸出Ui是線速度和角速度。


其中position_difference和angular_difference是機(jī)器人與期望位置/角度的差值。

avoid_delta按照上述公式進(jìn)行計(jì)算。

然后當(dāng)Epuck2探測(cè)到障礙物距離小于0.1m時(shí)，計(jì)算產(chǎn)生避障項(xiàng)使linear_velocity為負(fù)值機(jī)器人遠(yuǎn)離障礙物，并且產(chǎn)生正向或反向的角速度使機(jī)器人轉(zhuǎn)向。當(dāng)與障礙物距離大于0.1m時(shí)避障項(xiàng)為0，機(jī)器人向期望位置前進(jìn)。

3.距離傳感器結(jié)合紅外傳感器修復(fù)避障Bug

前面所說的Epuck2避障思路，但最終呈現(xiàn)出的效果卻是機(jī)器人在障礙物前徘徊，無法繞過障礙物。
原因是機(jī)器人接近障礙物后，距離小于0.1m后產(chǎn)生反方向的速度，使距離拉大。當(dāng)距離大于0.1m后避障項(xiàng)為0，機(jī)器人又受到引力作用向前，進(jìn)而又使距離縮小，雖然會(huì)產(chǎn)生角速度使機(jī)器人轉(zhuǎn)向，但只要靠近障礙物距離小于0.1m即產(chǎn)生反方向速度使機(jī)器人無法前進(jìn)，原地徘徊。

解決：沒有對(duì)人工勢(shì)場(chǎng)項(xiàng)進(jìn)行更改，而是通過距離和紅外傳感器反饋的距離來細(xì)化何時(shí)添加避障項(xiàng)，進(jìn)行避障。

如圖所示為Epuck2距離傳感器(ToF)和8個(gè)紅外傳感器(Prox)的位置，通過代碼可以分別獲得他們檢測(cè)到的與障礙物的距離信息(距離傳感器的探測(cè)距離為2m，紅外傳感器的探測(cè)距離為0.05m)。

距離傳感器探測(cè)到的距離記為r，8個(gè)紅外傳感器探測(cè)到的距離依次記為p0,p1,p2…p7(依圖中標(biāo)號(hào))。

(1)避障項(xiàng)avoid_delta的計(jì)算
避障項(xiàng)的計(jì)算不變，但觸發(fā)條件改變，之前通過動(dòng)捕計(jì)算機(jī)器人中心位置與障礙物中心位置的距離(是否小于0.1)進(jìn)行觸發(fā)。改為由Epuck2前方的1個(gè)距離傳感器和4個(gè)紅外傳感器觸發(fā)，如圖所示。

程序設(shè)置當(dāng)r小于0.08m或p0，p1，p7，p6返回的探測(cè)距離中有一個(gè)小于0.04m時(shí)，表明機(jī)器人的前側(cè)方遇到障礙物，返回測(cè)得的最小距離代入公式計(jì)算 ，否則將其賦值為0

(2) 機(jī)器人線速度的計(jì)算

當(dāng)機(jī)器人前側(cè)方遇到障礙物后，計(jì)算產(chǎn)生了一個(gè)負(fù)的避障項(xiàng)avoid_delta，在乘以設(shè)定的系數(shù)k_avoid，使其與期望位置產(chǎn)生的正向速度疊加，最終產(chǎn)生一個(gè)負(fù)的線速度，使機(jī)器人向遠(yuǎn)離障礙物方向移動(dòng)。
當(dāng)機(jī)器人傳感器沒有檢測(cè)到障礙物時(shí)，避障項(xiàng)avoid_delta為0，機(jī)器人根據(jù)與期望位置的距離產(chǎn)生正的引力(線速度)，使其朝著期望位置移動(dòng)。

(3) 機(jī)器人角速度的計(jì)算
為了模擬出機(jī)器人受到(目標(biāo)點(diǎn)吸引力和障礙物斥力疊加產(chǎn)生的合力)的效果，對(duì)機(jī)器人的角速度進(jìn)行以下設(shè)計(jì)。

3.1)當(dāng)障礙物位于機(jī)器人前進(jìn)方向的右側(cè)

如圖所示當(dāng)機(jī)器人前進(jìn)方向的右側(cè)出現(xiàn)障礙物時(shí)，受到目標(biāo)位置引力和障礙物位置斥力的疊加，應(yīng)產(chǎn)生向前進(jìn)方向左側(cè)的力。

我是這樣模擬給這個(gè)力的：如圖所示當(dāng)機(jī)器人前進(jìn)方向右側(cè)遇到障礙物時(shí)，機(jī)器人右側(cè)的兩個(gè)紅外傳感器能夠檢測(cè)到與障礙物的距離p0,p1，當(dāng)他們中有一個(gè)的距離小于0.04m時(shí)就產(chǎn)生這樣一個(gè)角速度：

由于檢測(cè)到障礙物時(shí)計(jì)算產(chǎn)生的 項(xiàng)為負(fù)，前面加負(fù)號(hào)產(chǎn)生一個(gè)正的角速度，使機(jī)器人逆時(shí)針旋轉(zhuǎn)，并且為了使機(jī)器人不因 項(xiàng)產(chǎn)生負(fù)的線速度使機(jī)器人后退，設(shè)置當(dāng)機(jī)器人角速度絕對(duì)值大于0時(shí)(即機(jī)器人旋轉(zhuǎn)時(shí))，機(jī)器人線速度賦值為0。

如圖所示，當(dāng)轉(zhuǎn)到機(jī)器人右側(cè)的兩個(gè)紅外傳感器檢測(cè)到與障礙物的距離都大于0.04m時(shí)， avoid_delta項(xiàng)變?yōu)?，機(jī)器人的角速度為0，機(jī)器人停止旋轉(zhuǎn)。但并沒有結(jié)束，為了使機(jī)器人朝目標(biāo)位置移動(dòng)，此時(shí)會(huì)判斷當(dāng)前機(jī)器人角度與期望航向角的差值，若差值大于設(shè)定的閾值，即產(chǎn)生角速度使差值減小。

這樣又會(huì)使機(jī)器人產(chǎn)生負(fù)的角速度，轉(zhuǎn)向障礙物，以至于機(jī)器人在原地來回偏轉(zhuǎn)，我通過位于機(jī)器人輪子旁的紅外傳感器p3來解決這個(gè)問題，當(dāng)機(jī)器人位于圖3.4所示的位置時(shí)，判斷紅外傳感器p3返回的距離信息，如果小于0.045m，把角速度設(shè)置為0，這樣當(dāng)機(jī)器人的角速度為0，當(dāng)機(jī)器人角速度為0時(shí)允許產(chǎn)生線速度，并且機(jī)器人前側(cè)的距離和紅外傳感器測(cè)得的障礙物距離大于設(shè)定值， avoid_delta項(xiàng)為0，產(chǎn)生正的線速度使機(jī)器人前進(jìn)。當(dāng)機(jī)器人前進(jìn)到如圖所示位置時(shí)。

紅外傳感器測(cè)得的距離p3大于設(shè)定值0.045m，此時(shí)再根據(jù)當(dāng)前角度與期望航向角的差值計(jì)算產(chǎn)生負(fù)的角速度，使機(jī)器人順時(shí)針偏轉(zhuǎn)繞開障礙物朝著目標(biāo)位置前進(jìn)。

3.2)當(dāng)障礙物位于機(jī)器人前進(jìn)方向的左側(cè)
基本思路相同，在機(jī)器人左側(cè)紅外傳感器檢測(cè)到p7,p6小于0.04m時(shí)，產(chǎn)生一個(gè)負(fù)的角速度使機(jī)器人順時(shí)針旋轉(zhuǎn)，繞過障礙物。

3.3)其他情況
除了這兩種情況，還有可能是恰好障礙物位于機(jī)器人前進(jìn)方向的正中間(只有距離傳感器測(cè)得的距離r小于設(shè)定值)，或者是遇到墻壁障礙物(前側(cè)所有障礙物檢測(cè)的距離都小于設(shè)定值)。此時(shí)不論機(jī)器人朝那個(gè)方向旋轉(zhuǎn)都可以。

4.問題

雖然解決了機(jī)器人的避障問題，但理論方面并沒有改進(jìn)，只是在實(shí)機(jī)實(shí)驗(yàn)的過程中，遇到問題解決問題，相當(dāng)于人為的把所有可預(yù)見的情況，設(shè)置了相應(yīng)的機(jī)器人應(yīng)對(duì)辦法，好處是直接，壞處是泛化性差，可能有一些沒有預(yù)見到的復(fù)雜情況，只用不斷測(cè)試完善才能確保機(jī)器人的避障效果。并且根據(jù)機(jī)器人與障礙物距離計(jì)算產(chǎn)生的避障項(xiàng)作用不大不能很好體現(xiàn)出其優(yōu)勢(shì)。

5.代碼邏輯圖

距離傳感器(ToF)測(cè)得的距離為r，紅外傳感器順時(shí)針旋轉(zhuǎn)依次測(cè)得距離依次為p1,p2,p3….p7，代碼邏輯如下：

二、基于人工勢(shì)場(chǎng)力的避障

1.基于人工勢(shì)場(chǎng)的Epuck2避障思路(理論)

前一個(gè)方法雖然通過人工勢(shì)場(chǎng)公式給機(jī)器人加了引力斥力，但力是根據(jù)機(jī)器人與目標(biāo)點(diǎn)/障礙物間的距離作用于機(jī)器人的線速度和角速度。由于通過距離遠(yuǎn)近來改變力的大小而影響速度，故沒辦法控制力的方向。為能夠?qū)崿F(xiàn)當(dāng)機(jī)器人陷入局部最優(yōu)時(shí)給機(jī)器人速度方向垂直的力，引入方向向量設(shè)計(jì)如下：

2.基于人工勢(shì)場(chǎng)力的Epuck2避障實(shí)現(xiàn)(現(xiàn)實(shí))

以機(jī)器人前方方向左側(cè)遇到障礙物進(jìn)行受力分析，并說明代碼實(shí)現(xiàn)思路：

三、兩種避障方法的對(duì)比分析

首先是兩種避障方法的路徑結(jié)果對(duì)比，設(shè)置相同的起始位置(0.229,0.367)，目標(biāo)位置設(shè)為(2,0.367)，在前進(jìn)方向設(shè)置障礙物如圖所示。

分別采用兩種不同的避障策略，記錄各策略三次避障的路徑結(jié)果如下：首先采用基于傳感器感知的避障方法，路徑結(jié)果記錄如下。

(1) 基于傳感器感知的避障



(2) 基于人工勢(shì)場(chǎng)加速度方向垂直力的避障方法

---

總結(jié)

這兩種方法都存在較大的問題，也簡(jiǎn)單的嘗試了將這兩種方法應(yīng)用于多機(jī)器人編隊(duì)中，但結(jié)果遠(yuǎn)遠(yuǎn)低于預(yù)期，兩種方法存在相同的缺陷，在編隊(duì)行進(jìn)過程中一旦遇到障礙物，編隊(duì)就離散后面也很難繼續(xù)保持編隊(duì)前進(jìn)。

目前也看了一些其他的避障方法，發(fā)現(xiàn)做出很好實(shí)機(jī)效果的多機(jī)編隊(duì)，絕大多數(shù)采用基于軌跡引導(dǎo)的避障方法(代表EGO-Swarm多無人機(jī)編隊(duì))，這種方法具有很大的優(yōu)勢(shì)，本文采用的兩種避障方法遇到障礙物不斷試探著去通過，而基于軌跡引導(dǎo)的避障具有預(yù)見性，在遇到障礙物前就已經(jīng)開始規(guī)劃進(jìn)而調(diào)整整個(gè)編隊(duì)中每個(gè)個(gè)體的控制策略，確保在避障的同時(shí)維持較好的編隊(duì)效果。

寫好多字了，就這樣吧，還有一個(gè)基于二次規(guī)劃的多機(jī)編隊(duì)避障，下次再寫o(￣▽￣)ブ