任工昌，郭維鵬，劉 朋

（陜西科技大學(xué)機(jī)電工程學(xué)院，陜西西安 710021）

0 引言

移動(dòng)機(jī)器人自主避障是指從初始位置運(yùn)動(dòng)到目標(biāo)位置的過程中，機(jī)器人在外部條件約束下能安全地避開各種障礙物，最終抵達(dá)目標(biāo)位置［1］。常見的避障算法有神經(jīng)網(wǎng)絡(luò)算法［2］、模糊邏輯算法［3］、人工勢(shì)場(chǎng)法［4-6］和A*算法［7］等。

人工勢(shì)場(chǎng)法是Khatib［8］在研究機(jī)械臂時(shí)構(gòu)建的一種虛擬勢(shì)場(chǎng)法，其原理是將機(jī)器人放置于一個(gè)由障礙物與目標(biāo)點(diǎn)共同作用的勢(shì)場(chǎng)中，目標(biāo)點(diǎn)引力勢(shì)場(chǎng)對(duì)其有吸引作用，而障礙物斥力勢(shì)場(chǎng)對(duì)其有排斥作用，兩個(gè)勢(shì)場(chǎng)的共同作用使機(jī)器人進(jìn)行移動(dòng)。

人工勢(shì)場(chǎng)法因其具有數(shù)學(xué)分析簡(jiǎn)單、軌跡光滑等特點(diǎn)被廣泛應(yīng)用于移動(dòng)機(jī)器人避障與路徑規(guī)劃中［9］。但經(jīng)典人工勢(shì)場(chǎng)法尚存在一些不足［10-12］，而且在實(shí)際應(yīng)用過程中，動(dòng)態(tài)障礙物也是機(jī)器人在運(yùn)動(dòng)時(shí)要考慮避開的對(duì)象，使得經(jīng)典人工勢(shì)場(chǎng)法無法滿足避障要求，因此需要對(duì)該方法進(jìn)行一定改進(jìn)。對(duì)于動(dòng)態(tài)避障問題，Ge 等［13］針對(duì)目標(biāo)點(diǎn)和障礙物位置都發(fā)生變化的問題，提出一種新的勢(shì)能方法，從而定義新的勢(shì)能函數(shù)解決該問題；韓永等［14］通過引用速度勢(shì)場(chǎng)的概念，改進(jìn)傳統(tǒng)勢(shì)場(chǎng)函數(shù)，有效解決了動(dòng)態(tài)避障問題；翟紅生等［15］在傳統(tǒng)勢(shì)力場(chǎng)基礎(chǔ)上引入相對(duì)速度場(chǎng)，并結(jié)合量子粒子群算法，實(shí)現(xiàn)機(jī)器人的動(dòng)態(tài)路徑規(guī)劃。

本文一方面對(duì)經(jīng)典勢(shì)場(chǎng)函數(shù)進(jìn)行改進(jìn)，以解決目標(biāo)點(diǎn)不可達(dá)問題，并通過調(diào)整引力在坐標(biāo)軸上的分量，從而改變合力方向，解決局部極小值問題；另一方面考慮障礙物運(yùn)動(dòng)速度，將其與機(jī)器人相對(duì)速度納入勢(shì)場(chǎng)函數(shù)，從而重新建立斥力勢(shì)場(chǎng)，實(shí)現(xiàn)動(dòng)態(tài)避障。

1 經(jīng)典人工勢(shì)場(chǎng)法

人工勢(shì)場(chǎng)法的核心思想是受物理學(xué)中自然勢(shì)場(chǎng)啟發(fā)而產(chǎn)生的，如電勢(shì)場(chǎng)與重力勢(shì)場(chǎng)［16］。將這種作用機(jī)制的數(shù)學(xué)函數(shù)應(yīng)用在移動(dòng)機(jī)器人環(huán)境模型中，通過給定初始點(diǎn)、目標(biāo)點(diǎn)和各障礙物位置坐標(biāo)，根據(jù)三者之間的相對(duì)位置關(guān)系建立人工勢(shì)場(chǎng)以模擬自然勢(shì)場(chǎng)，這就是經(jīng)典人工勢(shì)場(chǎng)法。

利用勢(shì)場(chǎng)函數(shù)U建立人工勢(shì)場(chǎng)，在平面空間中，機(jī)器人初始位置坐標(biāo)為q=（x，y）T，目標(biāo)位置坐標(biāo)為qgoal=（xg，yg）T，此處的引力勢(shì)場(chǎng)會(huì)對(duì)機(jī)器人產(chǎn)生一個(gè)引力，方向由機(jī)器人指向目標(biāo)點(diǎn)；障礙物qobs=（xo，yo）T處的斥力勢(shì)場(chǎng)會(huì)對(duì)機(jī)器人產(chǎn)生一個(gè)斥力，方向由障礙物指向機(jī)器人。q處的勢(shì)場(chǎng)函數(shù)U（q）可表示為：

其中，引力勢(shì)場(chǎng)函數(shù)公式為：

則引力為：

式中，λ為非負(fù)常數(shù)，是引力勢(shì)場(chǎng)的增益系數(shù)，實(shí)驗(yàn)時(shí)可根據(jù)具體情況改變?chǔ)酥?，以增加或減少引力勢(shì)場(chǎng)作用強(qiáng)度。ρ（q，qgoal）表示機(jī)器人當(dāng)前位置q點(diǎn)與要到達(dá)目標(biāo)點(diǎn)qgoal之間的距離。

斥力勢(shì)場(chǎng)函數(shù)公式為：

式中，μ為非負(fù)常數(shù)，表示斥力勢(shì)場(chǎng)增益系數(shù)，其作用與引力勢(shì)場(chǎng)中的λ類似，用來調(diào)整斥力勢(shì)場(chǎng)大??；ρ0為障礙物作用距離閾值，其作用是考慮目標(biāo)點(diǎn)附近障礙物對(duì)機(jī)器人的影響；ρ（q，qobs）表示機(jī)器人所在位置q點(diǎn)與閾值內(nèi)障礙物qobs之間的距離。

2 改進(jìn)人工勢(shì)場(chǎng)法

2.1 目標(biāo)不可達(dá)問題及其改進(jìn)

由勢(shì)場(chǎng)函數(shù)可以看出，當(dāng)目標(biāo)點(diǎn)附近存在障礙物時(shí)，移動(dòng)機(jī)器人與目標(biāo)點(diǎn)位置越接近，其所受引力越小，但斥力受目標(biāo)點(diǎn)附近障礙物影響而逐漸增大，則移動(dòng)機(jī)器人受斥力作用還會(huì)運(yùn)動(dòng)一段距離，即出現(xiàn)目標(biāo)點(diǎn)不可達(dá)的情況。因此，可將機(jī)器人與目標(biāo)位置之間的相對(duì)距離加入到斥力勢(shì)場(chǎng)函數(shù)中，減小目標(biāo)點(diǎn)附近障礙物的斥力勢(shì)場(chǎng)。斥力勢(shì)場(chǎng)函數(shù)公式如下：

由斥力場(chǎng)可得斥力Frep（q）的分量為：

其中，參數(shù)n為正實(shí)數(shù)，F(xiàn)rep1（q）方向從障礙物指向機(jī)器人，對(duì)機(jī)器人有排斥作用；Frep2（q）方向從機(jī)器人指向目標(biāo)位置，對(duì)機(jī)器人有吸引作用。由式（3）與式（6）可知，若機(jī)器人到達(dá)目標(biāo)位置，此時(shí)引力為零，同時(shí)靠近目標(biāo)位置的障礙物產(chǎn)生的斥力勢(shì)場(chǎng)也為零，符合避障要求。

2.2 局部極小值問題與改進(jìn)

若存在障礙物分布在移動(dòng)機(jī)器人與目標(biāo)點(diǎn)連線上的情況，移動(dòng)機(jī)器人在向目標(biāo)位置運(yùn)動(dòng)時(shí)合力方向始終不變，則運(yùn)動(dòng)方向也始終不變，從而無法避開障礙物。為解決局部極小值問題，可對(duì)引力進(jìn)行改進(jìn)，若作用在機(jī)器人上的引力與斥力共線時(shí)，可調(diào)整引力在坐標(biāo)軸上的分量，從而改變合力方向，解決局部極小值問題。

當(dāng)斥力與引力共線時(shí)，改進(jìn)后的引力在坐標(biāo)軸x、y 上的分力如下：

式（9）與式（10）是改進(jìn)后引力Fatt（q）在坐標(biāo)軸x、y上的分量，其中α、β∈（-1，1），通過改變參數(shù)α與β值可以調(diào)整引力大小與方向，合力方向也隨之改變，使機(jī)器人能夠繞過障礙物，離開局部極小值點(diǎn)。其中，θ為改進(jìn)后引力與x軸正方向的夾角，如式（11）所示。

3 引入障礙物速度斥力勢(shì)場(chǎng)函數(shù)

為實(shí)現(xiàn)移動(dòng)機(jī)器人在動(dòng)態(tài)環(huán)境中的自主避障，提高其動(dòng)態(tài)避障能力，可在斥力勢(shì)場(chǎng)函數(shù)中加入移動(dòng)機(jī)器人與障礙物的相對(duì)速度。在向目標(biāo)點(diǎn)前進(jìn)時(shí)，移動(dòng)機(jī)器人可以實(shí)時(shí)獲得距離本身最近障礙物的速度vobs，兩者相對(duì)速度在其連線上的分量vro為：

式中，nro是相對(duì)速度的單位向量，方向由機(jī)器人指向障礙物，如圖1 所示。

Fig.1 Speed relationship between robot and obstacle圖1 機(jī)器人與障礙物之間速度關(guān)系

考慮到實(shí)際運(yùn)動(dòng)過程中機(jī)器人自身大小對(duì)避障有一定影響，因此在斥力勢(shì)場(chǎng)函數(shù)中加入機(jī)器人半徑R。改進(jìn)后的斥力勢(shì)場(chǎng)函數(shù)為：

其中，η為常數(shù)，表示相對(duì)速度影響因子。通過改變參數(shù)η可調(diào)整相對(duì)速度對(duì)勢(shì)場(chǎng)的影響，當(dāng)障礙物為靜態(tài)時(shí)，η=0。

對(duì)斥力勢(shì)場(chǎng)函數(shù)Urep（q，v）求負(fù)梯度可得：

其中，F(xiàn)repq是斥力場(chǎng)關(guān)于位置q的梯度，F(xiàn)repv為斥力勢(shì)場(chǎng)關(guān)于速度v的梯度。由式（12）可知，vro相對(duì)于速度和位置的梯度為：

4 實(shí)驗(yàn)及仿真

為驗(yàn)證改進(jìn)后算法的動(dòng)態(tài)避障能力，在MATLAB 環(huán)境下對(duì)算法進(jìn)行仿真，觀察改進(jìn)算法針對(duì)局部極小值、目標(biāo)不可達(dá)以及動(dòng)態(tài)避障等問題的改進(jìn)效果，仿真參數(shù)設(shè)置如表1 所示。

4.1 目標(biāo)不可達(dá)與局部極小值問題解決后仿真結(jié)果

設(shè)置機(jī)器人初始坐標(biāo)位置（0，0），沿與x軸正方向夾角為45°的方向運(yùn)動(dòng)，目標(biāo)點(diǎn)坐標(biāo)位置（8，8），靠近目標(biāo)點(diǎn)的障礙物坐標(biāo)位置（7，7.8），圖2、圖3 分別為目標(biāo)不可達(dá)問題改進(jìn)前后仿真結(jié)果對(duì)比。

Table 1 Simulation parameter setting表1 仿真參數(shù)設(shè)置

Fig.2 Simulation results before improvement of target unreachable problem圖2 目標(biāo)不可達(dá)問題改進(jìn)前仿真結(jié)果

Fig.3 Improved simulation result of target unreachable problem圖3 目標(biāo)不可達(dá)問題改進(jìn)后仿真結(jié)果

由圖2 可以看出，當(dāng)機(jī)器人抵達(dá)目標(biāo)位置周圍時(shí)，其所受引力較小，因此機(jī)器人受斥力影響偏離目標(biāo)位置向前運(yùn)動(dòng)，不能抵達(dá)目標(biāo)位置。圖3 為改進(jìn)后的斥力勢(shì)場(chǎng)，改進(jìn)后的斥力勢(shì)場(chǎng)可減小目標(biāo)附近障礙物對(duì)機(jī)器人的影響，從而成功達(dá)到目標(biāo)點(diǎn)位置。

在機(jī)器人與目標(biāo)點(diǎn)連線上設(shè)置障礙物，其位置為（4，4）。由圖4 可以看出，算法改進(jìn)后的機(jī)器人可成功避開障礙物并到達(dá)目標(biāo)位置，不會(huì)陷入局部極小值點(diǎn)。該算法對(duì)動(dòng)態(tài)障礙物同樣適用，如圖5、圖6 所示。從圖中可看出，機(jī)器人能夠調(diào)整運(yùn)動(dòng)方向，避免陷入局部極小值。

Fig.4 Obstacle avoidance process when the obstacle is static圖4 障礙物為靜態(tài)時(shí)避障過程

Fig.5 Obstacles and robots move towards the obstacle avoidance position圖5 障礙物與機(jī)器人相向運(yùn)動(dòng)避障位置

Fig.6 The whole process of obstacle and robot moving towards each other圖6 障礙物與機(jī)器人相向運(yùn)動(dòng)全過程

4.2 移動(dòng)機(jī)器人動(dòng)態(tài)避障仿真結(jié)果

設(shè)置初始點(diǎn)位置坐標(biāo)（-4，-4），目標(biāo)點(diǎn)位置坐標(biāo)（11.5，3.5），并在移動(dòng)機(jī)器人所在運(yùn)動(dòng)環(huán)境中設(shè)置4 個(gè)障礙物，其中兩個(gè)靜態(tài)障礙物位置坐標(biāo)分別為（-1，4.8）和（1，1.7），兩個(gè)動(dòng)態(tài)障礙物起始位置坐標(biāo)分別為（4，8）和（6，6），其運(yùn)動(dòng)方向如圖7 所示。其中，所選取障礙物位置以及運(yùn)動(dòng)軌跡都會(huì)干擾到機(jī)器人，機(jī)器人在此環(huán)境下避障仿真結(jié)果如圖7、圖8 所示。其中，圖7 是移動(dòng)機(jī)器人在遇到第一個(gè)動(dòng)態(tài)障礙物時(shí)的避障軌跡，圖8 是移動(dòng)機(jī)器人整體避障軌跡。從圖中可以看出，機(jī)器人能夠根據(jù)障礙物運(yùn)動(dòng)情況及時(shí)調(diào)整位姿，從而避開障礙物。

Fig.7 The robot avoids the first dynamic obstacle圖7 機(jī)器人躲避第一個(gè)動(dòng)態(tài)障礙物

Fig.8 The whole process of robot avoidance圖8 機(jī)器人避障全過程

機(jī)器人在避障過程中與第一個(gè)動(dòng)態(tài)障礙物之間距離變化情況如圖9 所示，由圖可知，其最小距離約為0.7m，可保證機(jī)器人與障礙物之間有一個(gè)足夠的安全距離。在整個(gè)避障過程中，移動(dòng)機(jī)器人角度變化情況如圖10 所示，可看出機(jī)器人避障時(shí)角度變化比較平穩(wěn)，最大變化僅為46°，在合理范圍內(nèi)。仿真結(jié)果表明，移動(dòng)機(jī)器人通過使用本算法能有效避開靜態(tài)及動(dòng)態(tài)障礙物。

5 結(jié)語

本文通過對(duì)經(jīng)典人工勢(shì)場(chǎng)法存在的問題進(jìn)行改進(jìn)，以解決原算法中的目標(biāo)不可達(dá)與局部極小值問題，并通過在斥力勢(shì)場(chǎng)函數(shù)中引入障礙物速度，以滿足機(jī)器人在動(dòng)態(tài)環(huán)境中的避障要求。仿真結(jié)果顯示，改進(jìn)算法對(duì)于移動(dòng)機(jī)器人在靜態(tài)、動(dòng)態(tài)環(huán)境中的避障有著良好效果，而且對(duì)機(jī)器人運(yùn)動(dòng)過程中的角度變化以及與障礙物的最小距離都能實(shí)現(xiàn)較好的控制。

Fig.9 Change of distance between robot and obstacle圖9 機(jī)器人與障礙物距離變化

Fig.10 Robot angle change圖10 機(jī)器人角度變化情況

然而，本算法主要解決經(jīng)典人工勢(shì)場(chǎng)法存在的問題，并通過改進(jìn)算法滿足機(jī)器人對(duì)動(dòng)態(tài)障礙物的避障要求，但并沒有對(duì)避障路徑進(jìn)行優(yōu)化，仍存在路徑非最短等問題，有待后續(xù)進(jìn)一步完善。