李辰豪 吳啟來(lái) 郭晟男

（南京理工大學(xué) 江蘇省南京市 210094）

1 引言

目前，國(guó)內(nèi)外研究人員針對(duì)地面機(jī)器人的研究已經(jīng)取得了較大的成果，其發(fā)展也日益成熟。除此之外，在空中無(wú)人機(jī)和水中無(wú)人艦艇的開(kāi)發(fā)與應(yīng)用方面，很多學(xué)者都開(kāi)展了較為豐富的研究。但是上述的機(jī)器人都局限于某一種特定工作環(huán)境，如，地面、空中、水面，因而針對(duì)這些機(jī)器人的研究也具有單一性。在現(xiàn)實(shí)世界中存在著多種環(huán)境域，當(dāng)任務(wù)涉及到多種環(huán)境域時(shí)，這就要求機(jī)器人同時(shí)具備在多種環(huán)境域下的工作能力。路徑規(guī)劃是移動(dòng)機(jī)器人運(yùn)動(dòng)與仿真領(lǐng)域的熱點(diǎn)研究問(wèn)題，國(guó)內(nèi)外很多學(xué)者都在這個(gè)研究方向上進(jìn)行了大量的研究與探索，但路徑規(guī)劃的算法仍有不足與可改進(jìn)之處。

傳統(tǒng)A*算法以路徑最短作為規(guī)劃目標(biāo)，以此目標(biāo)規(guī)劃的路徑并不適用一些特定目標(biāo)的任務(wù)。文獻(xiàn)中提到了機(jī)器人陸空兩棲的決策切換和規(guī)劃，文獻(xiàn)提出了水陸兩棲的路徑規(guī)劃，均不能解決水陸空三棲環(huán)境下的機(jī)器人規(guī)劃問(wèn)題。

在此啟發(fā)下，我們提出一種適合水陸空三棲環(huán)境的基于改進(jìn)三維A*算法的全局規(guī)劃算法，該算法以機(jī)器人的耗能和時(shí)間的綜合代價(jià)作為規(guī)劃目標(biāo)。

2 基于狀態(tài)切換和代價(jià)分析的改進(jìn)三維A*算法

2.1 傳統(tǒng)A*算法

A*算法是一種在全局靜態(tài)地圖中對(duì)最短路徑進(jìn)行求解和規(guī)劃的最有效的直接搜索方法，也是解決許多其他問(wèn)題時(shí)常用的啟發(fā)式算法，有著較好的性能和準(zhǔn)確度。公式表示為：

傳統(tǒng)的A*算法沒(méi)有將機(jī)器人運(yùn)動(dòng)時(shí)不同運(yùn)動(dòng)狀態(tài)之間相互切換的耗能代價(jià)以及在不同環(huán)境運(yùn)動(dòng)的時(shí)間、耗能代價(jià)考慮進(jìn)去。只把所求解的路徑長(zhǎng)度作為規(guī)劃目標(biāo)，然而，在實(shí)際中，多棲機(jī)器人的路徑規(guī)劃、運(yùn)動(dòng)要考慮諸如耗能、時(shí)間等因素，以達(dá)到任務(wù)對(duì)機(jī)器人運(yùn)動(dòng)的時(shí)間和耗能要求。而本文提出的改進(jìn)算法將對(duì)這一問(wèn)題進(jìn)行優(yōu)化解決。

2.2 搜索鄰域簡(jiǎn)介

在二維環(huán)境下，A*算法只需拓展當(dāng)前節(jié)點(diǎn)的8個(gè)子節(jié)點(diǎn)，而三維環(huán)境下，需拓展26個(gè)子節(jié)點(diǎn)。

2.3 三棲機(jī)器人狀態(tài)切換的代價(jià)分析

三棲機(jī)器人的水、陸、空自主切換控制狀態(tài)機(jī)的實(shí)現(xiàn)如圖1所示。

圖1：機(jī)器人自主切換控制狀態(tài)機(jī)

由于機(jī)器人在水陸空各個(gè)環(huán)境下的狀態(tài)不相同，這使得機(jī)器人在環(huán)境變化時(shí)進(jìn)行的狀態(tài)切換會(huì)產(chǎn)生比較多的能量損耗。表1是機(jī)器人不同狀態(tài)切換的能量損耗程度對(duì)應(yīng)表，其對(duì)應(yīng)的F切換損耗屬性評(píng)價(jià)分別為{0,1,2,3,4,5}。

表1：狀態(tài)切換能量損耗評(píng)價(jià)表

從當(dāng)前工作狀態(tài)到待選節(jié)點(diǎn)工作狀態(tài)切換時(shí)會(huì)產(chǎn)生狀態(tài)切換能耗，在待選工作狀態(tài)向目標(biāo)點(diǎn)的工作狀態(tài)轉(zhuǎn)換時(shí)也會(huì)產(chǎn)生狀態(tài)切換能耗。

2.4 多目標(biāo)啟發(fā)式函數(shù)設(shè)計(jì)

當(dāng)三棲機(jī)器人在現(xiàn)實(shí)環(huán)境下運(yùn)動(dòng)時(shí)，運(yùn)動(dòng)的能量消耗不可忽視。因此在路徑規(guī)劃時(shí)需要把機(jī)器人在不同環(huán)境下運(yùn)動(dòng)的耗能分析和運(yùn)動(dòng)時(shí)間都作為優(yōu)化目標(biāo)考慮進(jìn)去。本文選擇將運(yùn)動(dòng)耗能分析和運(yùn)動(dòng)時(shí)間同時(shí)作為三棲機(jī)器人最優(yōu)路徑規(guī)劃的優(yōu)化目標(biāo)。又由于三棲機(jī)器人在不同環(huán)境下的運(yùn)動(dòng)耗能和運(yùn)動(dòng)時(shí)間不完全獨(dú)立，彼此之間有一定的依賴(lài)關(guān)系，所以使用為每個(gè)優(yōu)化目標(biāo)賦予一定權(quán)值的方式對(duì)于路徑規(guī)劃算法的多目標(biāo)啟發(fā)式函數(shù)進(jìn)行描述。運(yùn)動(dòng)過(guò)程的耗能代價(jià)分為地面運(yùn)動(dòng)代價(jià)、空中飛行代價(jià)以及水面運(yùn)動(dòng)代價(jià)。

由于運(yùn)動(dòng)耗能和運(yùn)動(dòng)時(shí)間這兩個(gè)優(yōu)化目標(biāo)的量綱不同，很難進(jìn)行統(tǒng)一的計(jì)算與比較，因此本文對(duì)不同環(huán)境下運(yùn)動(dòng)耗能代價(jià)進(jìn)行統(tǒng)一量綱處理。以地面運(yùn)動(dòng)耗能代價(jià)e作為基準(zhǔn)（保證統(tǒng)一量綱處理后的計(jì)算比較耗能代價(jià)的值大于等于1），采用各環(huán)境下的運(yùn)動(dòng)耗能代價(jià)與e的比值作為各環(huán)境下的計(jì)算比較耗能代價(jià)。

式中E、E、E分別表示地面運(yùn)動(dòng)、空中飛行與水面運(yùn)動(dòng)的統(tǒng)一量綱處理后的計(jì)算比較耗能代價(jià)，e、e、e表示各環(huán)境下實(shí)際的單位距離耗能代價(jià)。

在地圖的三維柵格中，根據(jù)柵格存儲(chǔ)的環(huán)境屬性不同（水、陸、空），規(guī)劃出一條路徑之后，即可獲得在地面運(yùn)動(dòng)的距離s、在空中飛行的距離s以及在水面運(yùn)動(dòng)的距離s，分別除以三棲機(jī)器人在地面運(yùn)動(dòng)的速度v、在空中飛行的距離v以及在水面運(yùn)動(dòng)的速度v。便可得到不同環(huán)境下的運(yùn)動(dòng)時(shí)間t、t、t。同理，對(duì)其按照空中運(yùn)動(dòng)的時(shí)間t進(jìn)行統(tǒng)一量綱處理，得到計(jì)算比較運(yùn)動(dòng)時(shí)間T、T、T。

本文為了與原A*算法的代價(jià)函數(shù)相區(qū)分，將代價(jià)函數(shù)設(shè)為g'(n)，節(jié)點(diǎn)q的代價(jià)函數(shù)值g'(b)為其父節(jié)點(diǎn)q的代價(jià)函數(shù)值g'(a)與q到q的運(yùn)動(dòng)代價(jià)、狀態(tài)切換對(duì)應(yīng)的代價(jià)之和，因此可以得到節(jié)點(diǎn)q的代價(jià)函數(shù)值g'(b)表達(dá)式如下：

cost(q,q)表示從節(jié)點(diǎn)q到節(jié)點(diǎn)q所需要耗費(fèi)代價(jià)，結(jié)合2.4的分析，cost(q,q)值的計(jì)算表達(dá)式如下：

其中α與β分別為不同環(huán)境下運(yùn)動(dòng)過(guò)程耗能和運(yùn)動(dòng)時(shí)間的加權(quán)系數(shù)，且滿足α+β=1，q.env，q.env分別表示節(jié)點(diǎn)q,q的環(huán)境屬性（land、air、water）。

change(q,q)表示從節(jié)點(diǎn)q到節(jié)點(diǎn)q狀態(tài)切換的代價(jià)，對(duì)其也按照地面運(yùn)動(dòng)的耗能進(jìn)行統(tǒng)一量綱處理，結(jié)合表1，可得出change(q,q)值的計(jì)算表達(dá)式如下：

其中κ為正常數(shù)，F(xiàn)表示節(jié)點(diǎn)q到節(jié)點(diǎn)q切換損耗屬性評(píng)價(jià)值。

本文為了與原A*算法的估價(jià)函數(shù)相區(qū)分，將代價(jià)設(shè)為h'(b)。h'(b)作為q至終點(diǎn)q的預(yù)估代價(jià)，結(jié)合前文，本文將h'(b)取為和g'(b)相同的形式，可以得到改進(jìn)后的h'(b)表達(dá)式如下：

因此優(yōu)化后的三維A*算法的代價(jià)函數(shù)計(jì)算公式為：

3 仿真環(huán)境搭建

本文采用三維柵格地圖對(duì)三棲環(huán)境進(jìn)行建模，如圖2所示。

圖2：三維柵格環(huán)境

選擇空間中一固定點(diǎn)作為坐標(biāo)原點(diǎn)， Z軸方向作為高度。在 Z 軸方向上，均勻?qū)⑷S空間n等分，形成n個(gè)平面。每個(gè)平面沿著 X 軸與 Y 軸方向分別再n和n等分，則可以用 n×n×n個(gè)柵格來(lái)表示三維空間環(huán)境信息。每個(gè)柵格包含了柵格可通行狀態(tài)和柵格空間類(lèi)型，如水面、空中柵格或地面柵格。圖3中，藍(lán)色區(qū)域?yàn)樗鏂鸥瘢凵珔^(qū)域?yàn)椴豢赏ㄐ袞鸥?，Z=1的區(qū)域?yàn)榈孛鏂鸥?，其余為空中柵格?/p>

圖3：第1組實(shí)驗(yàn)軌跡俯視圖

4 算法驗(yàn)證與分析

在MATLAB2018環(huán)境下，對(duì)改進(jìn)三維A*算法進(jìn)行路徑規(guī)劃的仿真實(shí)驗(yàn)。采用50×50×10的柵格地圖作為實(shí)驗(yàn)的仿真環(huán)境。根據(jù)參考文獻(xiàn)、文獻(xiàn)、文獻(xiàn)所提出的機(jī)器人的耗能分析并進(jìn)行仿真實(shí)驗(yàn)，設(shè)置估價(jià)函數(shù)的參數(shù)為：

e=12.5, e=9, e=1, t=1,

t=6, t=13, κ=1.7

我們?cè)O(shè)置了三組仿真實(shí)驗(yàn)，第1組為能耗、時(shí)間權(quán)重分別為0.5、0.5的改進(jìn)算法與原算法對(duì)比實(shí)驗(yàn)；第2組為權(quán)重分別為0.15、0.85的改進(jìn)算法與原算法對(duì)比實(shí)驗(yàn)；第3組實(shí)驗(yàn)為權(quán)重分別0.5、0.5和0.15、0.85的改進(jìn)算法之間的對(duì)比實(shí)驗(yàn)。

圖3，圖4為第1組實(shí)驗(yàn)仿真結(jié)果圖，將目標(biāo)點(diǎn)設(shè)置在水面中央。藍(lán)色軌跡是傳統(tǒng)A*算法，綠色軌跡是改進(jìn)A*算法。傳統(tǒng)A*算法規(guī)劃時(shí)采用了從地面運(yùn)動(dòng)狀態(tài)至飛行狀態(tài)的方式飛躍障礙物，并保持飛行狀態(tài)，在接近終點(diǎn)時(shí)，切換至水面運(yùn)動(dòng)狀態(tài)后運(yùn)動(dòng)到目標(biāo)點(diǎn)。改進(jìn)A*算法規(guī)劃的路徑選擇了維持地面運(yùn)動(dòng)狀態(tài)繞開(kāi)障礙物后，在面對(duì)水環(huán)境時(shí)，選擇了綜合代價(jià)較小的切換至飛行狀態(tài)并向目標(biāo)點(diǎn)。通過(guò)分析表2第1組實(shí)驗(yàn)數(shù)據(jù)，在當(dāng)前耗能、時(shí)間權(quán)重和環(huán)境下，改進(jìn)A* 算法規(guī)劃的路徑雖然路徑長(zhǎng)度和時(shí)間代價(jià)相對(duì)比較大，但是耗能代價(jià)明顯小，且綜合代價(jià)也更小，更利于降低機(jī)器人運(yùn)動(dòng)的成本。

圖4：第1組實(shí)驗(yàn)軌跡立體圖

表2：傳統(tǒng)算法與改進(jìn)算法對(duì)比實(shí)驗(yàn)數(shù)據(jù)

圖5是第2組實(shí)驗(yàn)仿真結(jié)果圖，將目標(biāo)點(diǎn)設(shè)置在水面中央。藍(lán)色軌跡是傳統(tǒng)A*算法，綠色軌跡是改進(jìn)A*算法，在第2組實(shí)驗(yàn)中，調(diào)大時(shí)間在代價(jià)分析中的權(quán)重，讓機(jī)器人以更快的時(shí)間到達(dá)目標(biāo)，而較小考慮耗能代價(jià)。傳統(tǒng)A*算法規(guī)劃時(shí)路徑保持不變。改進(jìn)A*算法選擇了時(shí)間代價(jià)更小的路徑，直接從起點(diǎn)飛過(guò)障礙物，且中間不再進(jìn)行狀態(tài)切換，也不再水面上運(yùn)動(dòng)，直接飛達(dá)目標(biāo)點(diǎn)。通過(guò)分析表2第2組數(shù)據(jù)，在當(dāng)前耗能、時(shí)間權(quán)重和環(huán)境下，改進(jìn)A* 算法規(guī)劃的路徑雖然耗能代價(jià)相對(duì)比較大，但是運(yùn)動(dòng)時(shí)間代價(jià)明顯小，且綜合代價(jià)也更小，有利于機(jī)器人完成對(duì)運(yùn)動(dòng)時(shí)間要求比較高的導(dǎo)航任務(wù)。

圖5：第2組實(shí)驗(yàn)軌跡立體圖

圖6是第3組實(shí)驗(yàn)仿真結(jié)果圖。綠色軌跡為能耗、時(shí)間權(quán)重分別為0.5,0.5的路徑軌跡，藍(lán)色軌跡為能耗、時(shí)間權(quán)重分別為0.15,0.85的路徑軌跡。藍(lán)色軌跡時(shí)間代價(jià)權(quán)重較大，因此在面對(duì)水環(huán)境時(shí)，選擇直接切換至飛行狀態(tài)，飛達(dá)目標(biāo)點(diǎn)。而綠色軌跡降低了時(shí)間權(quán)重，因此面對(duì)水環(huán)境，并沒(méi)有直接選擇切換至飛行狀態(tài)并飛達(dá)目標(biāo)點(diǎn)這一時(shí)間代價(jià)較小的路徑，還是選擇了綜合代價(jià)較小的切換至水面狀態(tài)，經(jīng)過(guò)水面環(huán)境后，切換至陸地狀態(tài)運(yùn)動(dòng)一段時(shí)間。之后，算法比較了直接飛過(guò)障礙物和通過(guò)陸地運(yùn)動(dòng)繞過(guò)障礙的綜合代價(jià)，選擇了飛過(guò)障礙物這一綜合代價(jià)較小的方式。表3數(shù)據(jù)表明，采取不同的權(quán)重，算法規(guī)劃的目標(biāo)不同，結(jié)果也不一樣。可通過(guò)改變權(quán)重來(lái)改變規(guī)劃目標(biāo)。

圖6：第3組實(shí)驗(yàn)軌跡立體圖

表3：第3組實(shí)驗(yàn)（不同權(quán)重的改進(jìn)算法對(duì)比實(shí)驗(yàn)）數(shù)據(jù)

5 結(jié)語(yǔ)

本文研究并探討了三棲機(jī)器人的廣闊前景以及現(xiàn)有的路徑規(guī)劃方法的優(yōu)點(diǎn)以及不足之處，在現(xiàn)有學(xué)者的只針對(duì)陸空環(huán)境的規(guī)劃以及僅在突發(fā)環(huán)境下局部規(guī)劃等研究成果的基礎(chǔ)上，提出了基于運(yùn)動(dòng)狀態(tài)以及狀態(tài)切換的耗能分析和時(shí)間綜合代價(jià)分析的改進(jìn)三維A*算法，實(shí)現(xiàn)三棲機(jī)器人的全局路徑規(guī)劃。經(jīng)實(shí)驗(yàn)驗(yàn)證，改進(jìn)算法能夠降低機(jī)器人運(yùn)動(dòng)的綜合代價(jià)，且能夠通過(guò)改變權(quán)重來(lái)使機(jī)器人執(zhí)行特定要求下的導(dǎo)航任務(wù)，這對(duì)多棲機(jī)器人運(yùn)動(dòng)的研究具有一定的借鑒意義。

此外，對(duì)于26鄰域進(jìn)行搜索會(huì)導(dǎo)致算法的時(shí)間復(fù)雜度較高，可以考慮優(yōu)先對(duì)相同環(huán)境屬性或陸地屬性的柵格鄰域進(jìn)行搜索，提高算法的時(shí)間性能。