王 婷， 陶永明， 閻 巖

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

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

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

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

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

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

(1)

(2)

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

1.2 Level Set方法

(3)

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

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

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

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

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

(4)

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

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

(5)

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

(6)

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

(7)

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

(8)

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

(9)

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

(10)

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

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

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

3.1 前向零水平集演化

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

(11)

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

(12)

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

(13)

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

HΔt≤Δh

(14)

3.2 后向路徑追蹤

(15)

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

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

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

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

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

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

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

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

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

5 結(jié) 論