張小明，于紀言

(南京理工大學 機械工程學院， 南京 210094)

在充滿危險性的戰(zhàn)場環(huán)境中，為了避免人員傷亡，各大科研機構對戰(zhàn)場環(huán)境偵察無人車展開深入研究。文獻[1]采用雙目視覺傳感器進行環(huán)境探測，基于虛擬力場法進行航跡規(guī)劃，能有效避開大型障礙物，但對細小交叉線纜的避障效果并不理想，且受霧霾和沙塵等因素影響較大；文獻[2]提出一種基于MPC的自主避障算法，解決了車輛在復雜路況下出現(xiàn)穩(wěn)定性和避障行為沖突的問題，但其模型過于復雜，且當車速較高時避障效果下降明顯；文獻[3]提出一種基于機器人速度的模糊控制算法，實現(xiàn)對動態(tài)和靜態(tài)障礙物精確的避障效果，但其動態(tài)環(huán)境下的數(shù)學模型十分復雜，且適用的避障環(huán)境中障礙物規(guī)律分布，不適用于環(huán)境復雜的地形環(huán)境；文獻[4]提出一種具有速度反饋的無人艇模糊避障控制算法[5]，將超聲波傳感器和多路激光傳感器的數(shù)據(jù)進行加權融合，并在模糊控制的基礎上根據(jù)環(huán)境情況自動調(diào)整航速，但其突出的問題是模糊控制規(guī)則過于復雜，三層結構的控制系統(tǒng)在實際復雜的使用環(huán)境中較難維護。

本文設計的避障算法應用背景是在未知戰(zhàn)場環(huán)境中由操作員在避障之前給出要偵察環(huán)境的4個頂點坐標和目標點坐標，以此確定待避障環(huán)境范圍大小和目標點位置，無人車依靠單線激光測距傳感器探測未知環(huán)境中障礙物的位置后建立模擬地形圖并將各障礙物標定其中，無人車主要完成兩個任務：(1)完成對給定范圍未知環(huán)境的偵察，確定其中各障礙物的位置和狀態(tài)信息，并在模擬地形圖中標定；(2)在對給定范圍未知環(huán)境偵察的同時基于避障算法規(guī)劃出一條至目標點的無障礙最佳通行路徑，以對目標點實現(xiàn)必要的物資補給。本文基于人工勢場法[6-7]設計環(huán)境偵察無人車避障算法，無人車每側均布兩個單線激光傳感器，形成以無人車為中心、激光探測距離為半徑的全覆蓋探測區(qū)域，并針對人工勢場法存在的局部最小點問題，提出“米”字坐標試走改進方案；針對目標點不可到達問題，提出目標點附近重新定義斥力場函數(shù)改進方案，使無人車在保證避障路線最優(yōu)情況下控制系統(tǒng)簡單可靠。

1 障礙物檢測

本文設計的未知環(huán)境探測系統(tǒng)由8個單線激光測距傳感器構成，小車每側均布兩個，每個激光測距傳感器有90°的掃描范圍，相較于多線激光雷達，最大程度的降低了成本。

1.1 激光測距傳感器

本文采用單線激光測距傳感器探測環(huán)境中障礙物的位置，其在無人車上的布置如圖1所示。

圖1 單線激光測距傳感器分布示意圖

激光測距傳感器由兩部分組成：激光回路部分和信號測量部分。激光回路部分由激光發(fā)射器、凸透鏡和分光鏡組成；信號測量部分由雪崩光電二極管、信號放大器、比較器和延時測量芯片TDC-GP2組成。整個激光測距傳感器的系統(tǒng)圖如圖2所示，具體工作過程如下：當系統(tǒng)對準障礙物后，激光器發(fā)出一個很強很窄的激光脈沖，這個脈沖經(jīng)過凸透鏡，將光束發(fā)散角壓縮為幾個毫弧度，使得脈沖光聚焦為平行光發(fā)射。這樣的光脈沖射到幾公里遠的地方，光斑直徑只有幾米，當測量距離小于 30 m時，光斑更小，更利于高精度測量[8]。在脈沖發(fā)射出去的同時，其中極小一部分光由分束鏡反射到探測器-光電二極管，它作為發(fā)射參考信號，用來標識激光發(fā)出的時間，即計時開始時刻t1。 參考信號到達探測信號的光電轉換器(雪崩光電二級管 APD)，將光脈沖變成電脈沖。這個電脈沖經(jīng)放大、整形濾波后送入延時測量芯片，使其開始計時。而射向目標的光脈沖由于目標的漫反射，總有一部分光從原路反射回來，于是射向目標的光脈沖在凸透鏡匯聚后射向目標點，經(jīng)目標點反射回來的散射光先通過窄帶濾波片濾除雜波，再由凸透鏡將微弱的反射光會聚到小面積的光電探測器上，經(jīng)過光電轉換器變成電脈沖，再經(jīng)過放大整形電路而進入時間測量系統(tǒng)，這是計時結束時刻t2，使其停止計時。 由此，TDC-GP2 便可以測得激光往返的時間，再將測量到的激光延時通過4線8位數(shù)據(jù)位的SPI口[9]傳輸?shù)綗o人車的控制系統(tǒng)MCU，由MCU計算出障礙物離無人車的距離及其他狀態(tài)信息。在本設計中，主控系統(tǒng)的MCU采用168 MHz的STM32F407。

圖2 測距系統(tǒng)結構示意圖

1.2 TDC-GP2與STM32的通信

TDC-GP2將測量到的t1與t2之間的時間間隔傳輸?shù)絊TM32中計算出最終的障礙物位置信息，具體是通過串行外設接口(Serial Peripheral Interface，SPI)。SPI以主從方式工作，通常有一個主設備和一個或多個從設備，需要至少4根線，它們是SDO(數(shù)據(jù)輸入)、SDI(數(shù)據(jù)輸出)、SCLK(時鐘)、CS(片選)[10]，各引腳的作用分別是：(1)SDO：主設備數(shù)據(jù)輸出，從設備數(shù)據(jù)輸入；(2)SDI：主設備數(shù)據(jù)輸入，從設備數(shù)據(jù)輸出；(3)SCLK：時鐘信號，由主設備產(chǎn)生；(4)CS：從設備使能信號，由主設備控制；

其中片選信號SSN控制芯片是否被選中，即只有片選信號為預先規(guī)定的使能信號時(高電位或低電位)，對此芯片的操作才有效。這就允許在同一總線上連接多個SPI從設備。TDC-GP2將激光延時傳輸?shù)絊TM32后，根據(jù)以下公式計算出無人車與障礙物之間的距離，即：

(1)

2 戰(zhàn)場環(huán)境偵察無人車避障算法

戰(zhàn)場環(huán)境偵察無人車主要用在局部地形偵察，實現(xiàn)在靜態(tài)環(huán)境中從起始點到目標點的無障礙最優(yōu)通行。環(huán)境中巖石、樹木和小山丘分布較多，這些障礙物均屬于靜態(tài)障礙物，符合人工勢場法的運用條件。該避障算法數(shù)學模型簡單、運行可靠，配合均布在車身的八個單線激光測距傳感器，可以保證無人車具有很高的障礙物探測精度。

2.1 經(jīng)典人工勢場法

經(jīng)典人工勢場法是美國著名科學家O.Khatib于1986年提出的，其基本原理是將目標點構建為虛擬引力場，將障礙物構建為虛擬斥力場，如圖3所示。而無人車避障系統(tǒng)運行在由引力場和斥力場共同作用的人工勢場中，通過搜索勢能減弱的方式尋找無障礙路徑，具有算法簡單、易于實現(xiàn)、實時性較高等特點。

圖3 人工勢場法原理示意圖

其中目標點對無人車的引力函數(shù)為：

(2)

式(2)中：η為正比例位置增益系數(shù)；ρ(q,qg)表示的矢量大小為無人車位置q和目標點位置qg之間的歐幾里得距離|q-qg|，矢量方向為兩個位置連線上指向目標點。

障礙物對無人車的斥力函數(shù)為：

(3)

式(3)中：k為正比例位置增益系數(shù)；ρ0為正常數(shù)，表示在無人車周圍障礙物的最大影響半徑；ρ(q,q0)表示無人車坐標點q和無人車離無人車距離最近的障礙物坐標點q0之間的歐幾里得距離|q-q0|，方向為障礙物指向無人車。

無人車的總勢場力可表示為：

F(q)=-▽U(q)=Fy(q)+Fc(q)

(4)

2.2 經(jīng)典人工勢場法存在的問題

在實際使用中經(jīng)典人工勢場法存在一些缺陷和問題，如目標點不可到達問題和局部最小點問題等。

2.2.1目標點不可到達問題

目標點不可到達問題是指當無人車在引力場作用下向目標點行進時，若此時在目標點距離很近的地方布置有一個障礙物，則此時無人車與目標點附近的障礙物的距離也越來越小，所受障礙物的斥力也就越來越大，引力反而越來越小，當無人車受到的斥力大于所受到的引力時，目標點將不會是整個勢場的全局最小點[11]，因此無人車將會在綜合作用力的作用下遠離目標點，這就是目標點不可到達的問題。

針對這個問題，我們需要在無人車進入離目標點很近的范圍內(nèi)時重新定義斥力場函數(shù)，如果能夠滿足在無人車向目標點逼近時斥力場趨于零，那么目標點將是整個勢場的全局最小點，從而解決目標點不可到達問題，這就驅使我們在重新定義斥力場函數(shù)時，把無人車與目標點之間的相對距離也考慮進去，從而建立一個新的斥力場函數(shù)，有：

(5)

式(5)中：ρ是無人車與障礙物之間的最近距離；(X-Xgoal)為無人車與目標點之間的距離，障礙物的影響范圍在距離ρ0之內(nèi)；η是一個位置增益系數(shù)。與之前提出的斥力場函數(shù)相比，引入了無人車與目標點之間的相對距離，保證了整個勢場僅在目標點Xgoal處全局最小。引力和斥力的勢場函數(shù)應該使得無人車所受的合力Fsum將驅使無人車遠離障礙物，向目標點靠近。根據(jù)上一章節(jié)的定義，即X=Xgoal時，斥力可以寫為：

(6)

式(6)中：

(7)

(8)

新的斥力矢量Frep1的方向為從障礙物指向無人車，新的斥力矢量Frep2的方向為從無人車指向目標點。無人車在新斥力場函數(shù)作用下所受的斥力與它的兩個斥力分量的關系如圖4所示。很顯然，當Frep1對無人車產(chǎn)生方向背離無人車的斥力時，F(xiàn)rep2相應的對無人車產(chǎn)生朝向目標點的吸引力。在式(7)、(8)中，當系數(shù)n=0時，與原斥力場函數(shù)相同，無人車在目標點附近所受到的斥力仍未關聯(lián)目標點與無人車間的距離。因此，在新的斥力場函數(shù)中，我們選擇n>0。圖4為戰(zhàn)場環(huán)境偵察無人車在新的斥力場函數(shù)下的受力分析圖。

圖4 新斥力場函數(shù)下無人車受力分析圖

2.2.2局部最小點問題

無人車在自主避障規(guī)劃路徑時，在合力的引導下避開障礙物，向總勢場強度減弱的方向運動，但在某些特殊情況下，由于障礙物的數(shù)量以及排列方式等因素使得無人車在其運動路徑中的某一點處所受到的斥力作用與引力作用大小相等，方向相反時達到受力平衡，從而失去牽引作用，此時無人車將無法通過障礙物從而無法到達目標點，這就是局部最小點問題。

針對局部最小點問題，本文提出改進方案。當檢測到無人車陷入局部穩(wěn)定區(qū)域后，算法就會從人工勢場法轉入局部穩(wěn)定區(qū)改進算法來確定無人車下一個前進方向，局部最小點改進算法采用“米”字試走法。當檢測到無人車在路徑中的某一點經(jīng)過時間間隔t內(nèi)行走的距離超過局部最小距離R，則說明無人車此時并未陷入局部最小點，此時不需“米”字試走算法介入。當無人車在某一點處被檢測到在時間間隔t內(nèi)行走距離未達到局部最小距離R時，系統(tǒng)自動由“米”字試走算法介入控制，以無人車此時的坐標點m_cNow為原始點，即“米”字中心點，標定此時水平向左的方向為0°，按順時針方向分別標定為45°,90°,135°,180°,225°,270°,315°，0°，如圖5所示。以此時無人車所處的位置m_cNow為坐標原點，建立上述的“米”字型坐標系，先以水平向左方向即0°方向為下一次的行進方向，經(jīng)過時間間隔t后再次檢測無人車行進距離是否大于R，若經(jīng)過時間間隔t后的行進距離大于R，則說明此時無人車已不處于局部最小區(qū)域，此時再次由人工勢場控制算法介入。若經(jīng)過時間間隔t后，無人車行進距離小于R，則說明無人車仍處于局部最小區(qū)域內(nèi)，此時無人車將沿0°方向退回，在原點位置以0°方向順時針遞增45°為下一次行進的方向，時間間隔t后再次檢測無人車是否走出局部最小區(qū)域，接下來的步驟與上述過程同理，直到無人車順利到達目標點。

圖5 “米”字試走法示意圖

3 驗證設計

為了進一步說明人工勢場法對戰(zhàn)場環(huán)境偵察的適用性，針對目前常用的模糊控制算法、A*算法和人工勢場法在模擬戰(zhàn)場環(huán)境下設計對比實驗，觀察避障效果。在MobotSim仿真平臺中采用0.2 m×0.1 m的移動無人車并且在小車周圍均布8個單線激光傳感器，用同樣的模擬實驗環(huán)境一次對上述3種算法進行驗證，并觀察其避障路徑，如圖6所示。

圖6 3種避障算法仿真實驗避障路線圖

圖6的模擬戰(zhàn)場實驗環(huán)境屬于靜態(tài)環(huán)境，尺寸為5 m×3 m，分別模擬了3種情景：規(guī)律分布障礙物、回轉形不規(guī)則障礙物和無規(guī)律分布障礙物。在仿真平臺中對于避障性能的第一評價指標為通行時間，第二評價指標為通行距離。小車除去加速減速的情況之外勻速行駛速度保持不變，一般情況下通行時間最短的避障路徑就是最佳避障路徑，為了進一步確保其性能最佳，以第二評價指標再次進行排序篩選。由圖6可以看出，模糊控制算法和人工勢場法均完成了避障到達目標點，而A*算法則陷入局部最小點問題困在了回轉形不規(guī)則障礙物中。3種算法在對比實驗中所得的避障性能數(shù)據(jù)如表1所示。由此認為人工勢場法更加適用于戰(zhàn)場靜態(tài)環(huán)境偵察避障。

表1 3種避障算法數(shù)據(jù)

為了進一步驗證本文提出的針對性改進辦法在實際應用中是否有效，在MobotSim仿真平臺中設計模擬實驗以及布置實地實驗環(huán)境驗證其避障性能。模擬實驗中事先根據(jù)引力場和斥力場計算出局部最小點的位置，并在該位置設置障礙物，模擬實驗結果如圖7所示。

由圖7可得，人工勢場法下無人車始終沒有走出局部最小區(qū)域；改進人工勢場法下無人車在檢測到陷入局部最小區(qū)域后按照“米”字坐標試走法成功走出局部最小區(qū)域，由于在目標點附近重新定義了引力場函數(shù)，因此無人車順利到達目標點。實地實驗環(huán)境場地按照模擬實驗1∶2搭建，障礙物布局相同，確認局部最小點的位置，分別試驗人工勢場法和改進人工勢場法，最終實驗結果得出人工勢場法經(jīng)過152 s走出局部最小區(qū)域，經(jīng)過復雜的避障路線最終到達目標點；改進人工勢場法經(jīng)過8.3 s走出局部最小區(qū)域，按照簡單直接的避障路徑最終順利達到目標點。

圖7 改進方法效果驗證路線圖

4 結論

戰(zhàn)場環(huán)境偵察無人車避障算法基于經(jīng)典人工勢場法進行針對性改進，該方法通過在目標點附近重新定義斥力場函數(shù)解決目標點不可到達問題；通過“米”字坐標試走法避免了局部最小點問題。通過對比性實驗，充分驗證了改進人工勢場法的快速性和可靠性，說明了改進人工勢場法相較于其他常用避障方法對于戰(zhàn)場環(huán)境偵察的適用性。