王 婷， 陶永明， 閻 巖

(1. 大連海洋大學(xué) 應(yīng)用技術(shù)學(xué)院， 遼寧 大連 116300； 2. 東北財經(jīng)大學(xué) 管理科學(xué)與工程學(xué)院， 遼寧 大連 116025)

機(jī)器人路徑規(guī)劃問題一直是研究的熱點(diǎn)，在移動機(jī)器人[1]、水面艦艇[2]、水下機(jī)器人[3]以及物流運(yùn)輸[4]等領(lǐng)域有著許多研究成果.傳統(tǒng)的地面機(jī)器人路徑規(guī)劃側(cè)重于分析障礙物或者危險區(qū)域來保證機(jī)器人的安全，同時使得路徑最短.在一些新的應(yīng)用場景中，如輕量級旋翼無人機(jī)在風(fēng)場中或者水下機(jī)器人在流場中運(yùn)行，機(jī)器人的行為明顯受到空間內(nèi)有向外力的影響，這種有向外力會對機(jī)器人的行為起到促進(jìn)或阻礙的作用.因此機(jī)器人在路徑規(guī)劃時需要考慮外力存在的情況，合理利用外力對機(jī)器人的某項(xiàng)指標(biāo)進(jìn)行優(yōu)化.

Petres等[8]提出了一種基于快速行進(jìn)算法的連續(xù)路徑規(guī)劃方法，方法定義了各向異性的代價函數(shù)，其中環(huán)境中的有向外力場作為優(yōu)化問題的約束條件.該方法是有向外力場作用下機(jī)器人基于網(wǎng)格點(diǎn)的路徑規(guī)劃方法的突破，但仍存在很大的問題，主要在于它只適用于線性代價函數(shù)，導(dǎo)致在過強(qiáng)的外力場中無法規(guī)劃出可行的路徑.Lolla等[9]提出了海洋環(huán)境中在流場影響下的水下機(jī)器人路徑規(guī)劃問題，突破了路徑規(guī)劃問題基于格點(diǎn)搜索的限制，并且在理論上保證了在強(qiáng)海流中路徑的可行性.但是由于在計算機(jī)中數(shù)值計算還是采用離散化的方式，所以受到數(shù)值方法的約束，在實(shí)際強(qiáng)海流中路徑的可行性并沒有得到保證.

本文有以下兩點(diǎn)貢獻(xiàn)：首先是通過重新定義問題，將海流的情況推廣到有向外力的作用情況，使得方法有更廣泛的意義；另外是改進(jìn)了數(shù)值方法，使得在強(qiáng)外力作用下，規(guī)劃出路徑的可行程度更高.

1 有向力作用下的機(jī)器人路徑規(guī)劃

1.1 路徑規(guī)劃問題描述

(1)

(2)

該模型不考慮機(jī)器人具體的動力學(xué)，在機(jī)器人的運(yùn)動空間遠(yuǎn)遠(yuǎn)大于機(jī)器人尺寸的應(yīng)用場景中，這種規(guī)約是完全合理的.在機(jī)器人本體更高層面上做規(guī)劃有著非常重要的現(xiàn)實(shí)意義，本文也將在這個層面展開對問題的闡述.

1.2 Level Set方法

(3)

式中，H為速度函數(shù).

經(jīng)過一段時間的演化后，曲線可能發(fā)生拓?fù)浣Y(jié)構(gòu)變化(例如斷裂或者合并等).曲線演化一般會涉及到曲線的參數(shù)化，而曲線參數(shù)化最大的缺點(diǎn)就是不易于計算曲率、法向矢量等曲線幾何參數(shù)，且難以處理曲線的拓?fù)浣Y(jié)構(gòu)變化.

圖1 曲線按法線方向的演化示意圖Fig.1 Schematic evolution of curvealong normal direction

水平集方法是一種用于界面跟蹤和形狀建模的數(shù)值方法，其最大的優(yōu)點(diǎn)是可以在笛卡爾網(wǎng)格中對演化的曲線(面)進(jìn)行數(shù)值計算，而不必對曲線(面)進(jìn)行計算；水平集方法一個重要的理論前提是隱函數(shù)的概念，在曲線演化理論中引入水平集的目的就是為曲線提供一種隱式表達(dá)方式，從而避免參數(shù)化這種顯式表達(dá)所帶來的一系列問題.其主要思想是，將移動變形的曲面嵌入到高一維的函數(shù)中，即將曲線的拓?fù)浣Y(jié)構(gòu)變化表示成一個連續(xù)變化的曲面與一個固定的平面(如高度為零的平面)的交線變化，曲面本身可以不發(fā)生拓?fù)浣Y(jié)構(gòu)的變化，從而使得復(fù)雜的曲線運(yùn)動過程變?yōu)楦呔S函數(shù)的演化過程.

高維曲面φ選取應(yīng)該是越簡單越好，如果該曲面和某一簡單的函數(shù)之間為一一對應(yīng)的關(guān)系，則選取這種簡單的函數(shù)來替代“高維曲面”.在水平集問題中，φ通常被選取為“符號距離函數(shù)”.假設(shè)要跟蹤曲線(即零水平集)為?ω，距離函數(shù)d(X)為曲線外圍或內(nèi)部某點(diǎn)到曲線的最短距離，在計算機(jī)計算過程中，?ω本身是由一系列的離散點(diǎn)組成的，所以d(X)的數(shù)學(xué)表述為d(X)=min|X-Xi|，其中，Xi為組成?ω的所有點(diǎn).符號距離函數(shù)定義為

(4)

對于?ω上的所有點(diǎn)Xi均有φ(Xi)=0，選取這種“符號距離函數(shù)”作為“高維曲面”存在諸多好處.首先它是光滑曲線，可以避免等高線上出現(xiàn)很陡的梯度，便于后面的計算使用.φ(X)是和時間相關(guān)的，所以可被寫作φ(X，t)，又因?yàn)槠浒忾]曲線C，所以也可寫作φ(C(t)，t)，簡寫為φ.

對于t時刻，演化得到的曲線C(t)和零水平集φ(C(t)，t)=0相等，將其對時間求導(dǎo)可得

(5)

在微分幾何中，曲線或曲面上的內(nèi)單位法線和梯度關(guān)系為

(6)

由此可見梯度和法線方向一致，將式(5)、(6)聯(lián)立可得

(7)

2 基于Level Set的路徑規(guī)劃方法

(8)

假設(shè)零水平集在初始時刻為一條極小的封閉曲線，即該曲線所包含的面積趨近于0，但可以保證在曲線上每一點(diǎn)上的法向存在，則式(8)初始條件為

(9)

由于T(y)時刻的零水平集是由t=0時刻演化所得，所以如果將T(y)時刻位置看作與y重合，則可以得到上一時刻的位置，按此思路可以逆推出t=0時刻的位置就是出發(fā)點(diǎn).反向路徑逆推過程滿足

(10)

將所有這些逆推得到的點(diǎn)相連，即可得到路徑γ，可以發(fā)現(xiàn)T(y)剛好是從起點(diǎn)到終點(diǎn)所需的時間.

3 算法數(shù)值實(shí)現(xiàn)

算法總共分為兩部分，分別稱為前向零水平集演化和后向路徑追蹤.

3.1 前向零水平集演化

定義四個差分算子，分別為x方向和y方向上的向前差分和向后差分，對應(yīng)著連續(xù)x和y方向上的偏導(dǎo)數(shù)，即

(11)

根據(jù)式(11)定義如下兩個梯度，即

(12)

將式(11)、(12)代入式(10)可得

(13)

通過式(13)的演化機(jī)制，水平集函數(shù)可以不斷向前演化.由于數(shù)值計算中使用了有限差分法，因此需要注意時間步長Δt的選擇，在給定空間網(wǎng)格間距Δh的條件下，需滿足

HΔt≤Δh

(14)

3.2 后向路徑追蹤

(15)

4 仿真結(jié)果與分析

在有向外力作用下機(jī)器人路徑規(guī)劃問題的仿真實(shí)驗(yàn)中，配置空間規(guī)約在二維平面上，空間的兩個坐標(biāo)軸x、y都在[-1，1]之間.為進(jìn)行數(shù)值計算，x和y方向上被均分成m×n個網(wǎng)格，分別按i和j來索引.空間內(nèi)的外力速度場Vf在這些網(wǎng)格上取值，其中第(i，j)個網(wǎng)格點(diǎn)上的外力速度為Vf(i，j).在以下的仿真實(shí)驗(yàn)中，機(jī)器人的速度保持恒定，在仿真中設(shè)定為vn=0.28 m/s.

為驗(yàn)證算法的基本功能以及數(shù)值方法的精度，本文對恒定外力場中機(jī)器人的路徑進(jìn)行規(guī)劃，空間內(nèi)存在恒定的外力速度場Vf=0.2 m/s，仿真結(jié)果如圖2所示.由圖2可知，算法的基本功能已經(jīng)實(shí)現(xiàn)，并且數(shù)值算法的精度可以保證最優(yōu)路徑的生成.在外力影響下，機(jī)器人向左運(yùn)動速度減慢，向右速度加快，在曲線上的表現(xiàn)為單位時間原點(diǎn)左側(cè)的曲線被擠壓，而右側(cè)曲線被拉伸.

圖2 在恒定外力場中機(jī)器人的路徑規(guī)劃Fig.2 Path planning for robot in constantexternal force field

不同速度下機(jī)器人的路徑規(guī)化如圖3所示.圖3中空間存在著帶狀速度場，同樣結(jié)構(gòu)的外力場中，通過調(diào)整速度場的大小進(jìn)行了6組實(shí)驗(yàn).仿真結(jié)果表明，路徑規(guī)劃算法可以有效利用外力來加快自己的運(yùn)動，并且在強(qiáng)流場中算法仍能保持原有的精度.在較小的速度場中，穿過速度場的路徑與x軸的夾角較大，表示機(jī)器人可以更容易地穿越速度場；在較大的速度場中，該角度變小，表示在較大的速度場中，機(jī)器人可以移動的范圍變小，同時算法可以精準(zhǔn)地確定進(jìn)入速度場的位置，以確定在較強(qiáng)的外力作用下所規(guī)劃出的路徑仍然保持可行性.

彎曲封閉的外力場在自然界中也很常見，如大氣和海洋中存在的渦旋.圖4是模擬渦旋生成的一種彎曲外力場，模擬場為同心圓結(jié)構(gòu)，距圓心越遠(yuǎn)，速度越大.圓心處速度最小，為0 m/s，邊界處最大，為1 m/s.仿真選取了3組起始點(diǎn)和終點(diǎn)作為比較，同時以虛線給出了A*算法得到的路徑結(jié)果.結(jié)果顯示，在彎曲外力場下，路徑可以有效利用外力場，并且生成的路徑突破了傳統(tǒng)的節(jié)點(diǎn)取點(diǎn)方式，形成了連續(xù)光滑的曲線路徑.A*路徑由于必須在網(wǎng)格點(diǎn)上取點(diǎn)，因此要犧牲一些距離來取網(wǎng)格點(diǎn).從時間上來看，Level Set路徑完成后，A*路徑還沒有到達(dá)終點(diǎn)，圖4中以“×”表示與Level Set相同時間的A*所能到達(dá)的點(diǎn).

圖3 在不同速度下機(jī)器人的路徑規(guī)劃Fig.3 Path planning for robot under different velocity

圖4 在彎曲外力場中機(jī)器人的路徑規(guī)劃Fig.4 Path planning for robot in bendingexternal force field

在最外層一組仿真中，A*沒能找到合適的路徑，分析原因可知，基于格點(diǎn)的搜索對于在外力場中機(jī)器人的可通行角度有限制，而基于數(shù)值方法的Level Set方法則可以避免這一限制.

5 結(jié) 論