有向外力場作用下機器人路徑規劃方法*

王 婷， 陶永明， 閻 巖

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

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

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

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

1 有向力作用下的機器人路徑規劃

1.1 路徑規劃問題描述

(1)

(2)

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

1.2 Level Set方法

(3)

式中，H為速度函數.

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

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

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

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

(4)

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

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

(5)

在微分幾何中，曲線或曲面上的內單位法線和梯度關系為

(6)

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

(7)

2 基于Level Set的路徑規劃方法

(8)

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

(9)

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

(10)

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

3 算法數值實現

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

3.1 前向零水平集演化

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

(11)

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

(12)

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

(13)

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

HΔt≤Δh

(14)

3.2 后向路徑追蹤

(15)

4 仿真結果與分析

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

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

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

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

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

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

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

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

5 結 論