基于測地線的移動機器人軌跡規劃方法

劉鵬程，張連東，宋雪萍

(大連交通大學機械工程學院，遼寧大連 116028)

0 前言

測地線是歐幾里得幾何中“直線”概念在黎曼幾何中的推廣，測地線在黎曼空間中的切矢量方向不改變，是局部最短線[1]。測地線理論在數學、物理學、地質學以及工程技術學領域得到了深入的探究，并于20世紀80年代應用到機器人領域。SHIN和MCKAY[2]在Bendix PACS機器人手臂的關節空間上規劃出一條時間最短的測地線路徑，實驗證明：產生的路徑比笛卡爾坐標系下的直線以及聯合插值路徑消耗的時間要短。BLAKE等[3]在移動機器人手臂上安裝一個實時輪廓跟蹤器，通過掃描障礙的輪廓，估計出最小路徑長度，尋找平滑的最短測地線來繞過障礙物到達目標位置。KIMMEL和SETHIAN[4]在離散化多面體的曲面流形上利用快速推進法獲得最短測地線曲線。張連東、王德倫[5]用MATLAB軟件解出測地線方程，獲得了平面2R機器人在黎曼空間中運動的測地線最短曲線。ORTIZ等[6]在二維環境中，應用測地線規劃路徑，減少了機器人移動過程中改變方向的次數，產生歐氏空間中的最優化路徑。JENA等[7]在位置空間和姿態空間中分別構建黎曼度量，根據不同的初始條件來生成軌跡，依靠3R SCARA機器人的實驗數據驗證結果是有效的。WU等[8]在三維曲面上非平坦區域，用測地線能量函數的梯度下降法計算出了非線性測地線方程，保證了移動機器人測地線的局部最短路徑。HU等[9]結合勢場與測地線之間的關系，利用勢場構造曲面上的測地線解決了機器人易陷入局部極小值的問題。WU等[10]在原始測地線的基礎上，提出了測地線重規劃理論，改變了原始測地線易產生邊界跟隨的缺點，產生的路徑符合長度最短的特征，并且算法在MATLAB上實現仿真。

本文作者提出一種通過求解測地線微分方程直接得到移動機器人輪子轉角變量的測地線軌跡的方法。以移動機器人的輪子轉角為變量，構建機器人軌跡弧長平方的黎曼度量，通過求解該黎曼度量下測地線的微分方程，直接得到機器人輪子轉角和角速度表示的移動機器人路徑最優的運動軌跡。根據測地線的幾何性質，沿著測地線切線方向的速度變化率為0，機器人在沿測地線路徑的運動中速度平穩。路徑是以軌跡弧長為參考，不受時間約束。求解測地線方程得到測地線的方法，為測地線理論在移動機器人軌跡規劃上的實際應用奠定基礎。

1 黎曼流形與度量的概念

對于m維歐氏空間Rm來說，設M為Hausdorff空間，有任意一個點x∈M在x的鄰域U同胚于Rm的一個開集，則稱M是m維流形[1]。假設在流形M上存在對稱的二階協變張量場G，其中(U;ui)是流形的局部坐標圖，那么G可以表示為式(1)。

G=gijdui?duj

(1)

如果對于任意的X∈Tp(M)都存在G(XX)≥0,并且X=0時G(XX)=0,可以得出G是非退化且正定的。G被稱為流形M上的度量張量，因為G是正定的，所以得到M是黎曼流形。黎曼度量的二次微分形式可以表示為式(2)，其中ds通常表示弧長元素，gij為黎曼度量矩陣的系數。對于ui=ui(t)在t0≤t≤t1范圍內的弧長s可以表示為式(3)，其中gij=gji。

ds2=gijduiduj

(2)

(3)

2 構建度量張量求解Christoffel符號

以兩輪差分驅動機器人為例，如圖1所示差分驅動機器人依靠調節左右輪子獨立的軸向運動來實現轉彎功能。XI和YI共同組成平面上的全局坐標系，XR和YR構成機器人自身坐標系。為了方便研究機器人在全局坐標系下的運動，通常選取機器人本體上的一個點代替機器人表示其在全局坐標系中的位置。以點P為機器人參考點，點P一般選取兩輪差分驅動機器人的兩輪軸線中間位置，則機器人的運動學方程表示為

圖1 差分驅動機器人運動學模型簡圖

(4)

其中：r為左右輪子半徑；θ為機器人自身坐標系與全局坐標系的夾角；θ1和θ2分別為左、右輪子轉角；b為兩輪軸向距離。

假設M是二維黎曼流形，在二維黎曼流形上以軌跡弧長微分的平方作為黎曼度量。機器人路徑長度的黎曼度量表示為

ds2=dx2+dy2

(5)

式中：ds2為弧長元素的平方值，也就是黎曼度量。

為了構造符合機器人轉角參數的黎曼度量，同時滿足度量矩陣的正定性質，把運動學方程中的x替換為dx，y替換為dy，θ1和θ2分別用dθ1和dθ2代替，將數值代入黎曼度量表達式(5)中，得到度量為

(6)

將表達式(2)簡述為度量矩陣的形式如下：

(7)

得到的黎曼度量矩陣為式(8)。

(8)

矩陣G就是黎曼度量矩陣，因為其正定性求得其逆矩陣G-1為式(9)：

(9)

其中gij為G-1的第i行j列的元素。

而表達式：

(10)

第二類克氏符號由黎曼度量唯一確定，它存在的意義在于:使得在黎曼空間的曲線上始終存在沿著曲線的切矢量在線性移動中保持不變。也就是在平行移動過程中，黎曼度量始終是不變的。克氏符號的唯一性代表了在黎曼流形上存在唯一的無撓容許聯絡[1]。

SymPy是Python語言中用于代數運算的庫，在mpmath庫的支持下可以進行任何浮點運算。SymPy作為一種交互式工具，可以在其他程序中自定義函數進行代碼的擴展。引用SymPy中的diffgeom函數關于流形、張量計算、第二類Christoffel符號來實現克氏符號分量的求解。在Python語言中習慣從零開始索引，因此在程序中把x0、x1表示為左右輪子的轉角θ1、θ2。代碼部分如下:

from sympy.diffgeom import Manifold, Patch, CoordSystem

from sympy.diffgeom import TensorProduct as TP

from sympy.diffgeom import metric_to_ Christoffel_2nd as Christoffel

from sympy import symbols

r,b = symbols("r,b")

n = 2

M = Manifold(′M′, n)

P = Patch(′P′, M)

coord =CoordSystem(′coord′, P, [′x%s′%i for i in range(n)])

x = coord.coord_functions()

dx = coord.base_oneforms()

g=[[(r**2/4)*(1+(x[0]+x[1])**2*(r**2/b**2)),(r**2/4)*(1-(x[0]+x[1])**2*(r**2/b**2))],[(r**2/4)*(1-(x[0]+x[1])**2*(r**2/b**2)),(r**2/4)*(1+(x[0]+x[1])**2*(r**2/b**2))]]

metric=0.5*sum([g[i][j]*TP(dx[i],dx[j]) for i in range(n) for j in range(n)])

C = Christoffel(metric)

利用程序獲得的克氏符號分量為式(11)

(11)

3 測地線方程的求解

對于移動機器人而言，輪子的轉角是機器人軌跡曲線的重要組成參數。在測地線理論中，假設n維空間中的曲線C：θi=θi(t)符合沿曲線切向量的協變微分是0，即曲線切方向的速度保持不變，那么這條曲線就符合測地線理論，曲線C就是所求的測地線。這時選取弧長s作為曲線C的主要參數，其dθi/ds為它的切向量分量，得到曲線切向量的協變微分：

(12)

令式(12)中的D(dθi/ds)為0，可以得到測地線方程的一般形式為

(13)

式中:i、j、k循環取遍1、2。

將測地線方程展開為如下兩個等式：

(14)

把程序求得的克氏符號分量代入式(14)中得到式(15)

(15)

(16)

下面用SciPy庫中的odeint函數實現式(16)的求解。SciPy庫包含很多科學計算以及工程上應用公式的求解，使用SciPy庫通常進行微分方程的求解、信號函數的分析等。通常為了圖形化數據，結合matplotlib生成二維或者三維的圖形，觀察輸出的圖形與想要得到的結果是否相同。為了使輸出結果更直觀，假設機器人左右輪子半徑r為單位長度1，并且機器人兩輪之間的軸向距離b也為單位長度1。引入Scipy庫中的odeint函數，將式(16)中的微分方程封裝到geodesic函數中，部分代碼如下：

from scipy.integrate import odeint

def geodesic(w,s,v):

x0, y0, x1, y1 = w

r,b=v

f=[y0,-(2*(b**2)-r**2*((x0+x1)**2))/(2*(b**2)*(x0+x1))*(y0**2)-(x0+x1)*(r**2)/(b**2)*(y0*y1)+(2*(b**2)+r**2*(x0+x1)**2)/(2*(b**2)*(x0+x1))*(y1**2),y1,(2*(b**2)+(r**2)*((x0+x1)**2))/(2*(b**2)*(x0+x1))*(y0**2)-(x0+x1)*(r**2)/(b**2)*(y0*y1)-(2*(b**2)-r**2*(x0+x1)**2)/(2*(b**2)*(x0+x1))*(y1**2)]

return f

按照之前定義好的的半徑與輪子軸向距離，將情況A中的的參數數值代入函數中：

r=1,b=1,x0=1.795,y0=1,x1=0.295,y1=1

v=[r,b]

w0=[x0,y0,x1,y1]

取終止長度為2,樣本點為500：

stoplength = 2

numpoints = 500

s = [stoplength * float(i) / (numpoints - 1)for i in range(numpoints)]

調用odeint()函數，設置相應的參數求解微分方程：

wsol=odeint(geodesic,w0,s,args=(v,),atol=1.0e-10, rtol=1.0e-8)

算法流程如下:

(1)定義以轉角為參數的差分驅動機器人測地線方程，將一階微分方程定義到函數之中。

(2)根據不同的運動狀態定義初始條件。

(3)設置ode求解器適當的參數。

(4) 為ode求解器輸出創建曲線長度樣本。

(5)調用ode求解器實現測地線方程的求解。

(6)用matplotlib輸出曲線圖像。

差分驅動機器人的初始位姿與由角速度引起的機器人軌跡的變化是不相關的，也就是說差分驅動機器人按照固定的角度與角速度運動，所產生的內在軌跡是不變的，只不過機器人自身坐標系相對世界坐標系轉角發生變化。因此為了方便計算，假設機器人的初始位置在世界坐標系的原點，取弧長范圍是0～2,樣本點為500個。初始化不同角速度如下：

將A、B、C所列出的不同角速度與角加速度分別代入公式(16)中，得到不同初始條件的圖像，如圖2—圖4所示。

圖2 初始條件A中輪子轉角以及其角速度的軌跡

圖3 初始條件B中輪子轉角以及其角速度的軌跡

圖4 初始條件C中輪子轉角以及其角速度的軌跡

情況A中，圖2(a)表示在弧長遞增的情況下左、右輪子角度與弧長的關系，可以看出角度分別從各自的初始角度逐漸增加，并且增速基本相同，這也就意味著機器人在環境中是前進的。由于給定的兩輪初始角速度是相等的，通過圖2(b)可知，角速度與軌跡弧長關系的曲線重合。在弧長遞增的情況下，由測地線微分方程得到的機器人左、右輪子的角速度軌跡是相同的且保持相對的穩定。圖2(d)表示關節空間中的測地線軌跡，因為初始角速度是相同的，所以軌跡是一條斜直線。

情況B中，因為左右輪子不同的角速度以及不同的起始角度，圖3(a)中的軌跡形狀與圖2是不同的。左輪轉角與弧長關系曲線在右輪上方，這是因為初始角度之間存在差異。按照圖像曲線的走勢，可以看到隨著弧長的增加，左、右輪子轉角越來越接近。對于圖3(b)，右輪的角速度比左輪大，呈現出來的軌跡是上凸的曲線，這符合機器人的運動學模型。情況C中，左右輪子的角速度差值不如情況B，可以從圖4(d)中看出曲線上凸不是很明顯，這也就進一步證明與機器人運動是相符的。

根據測地線理論，由測地線方程求解出來的測地線在歐氏空間是最短線。針對3種情況中生成的軌跡，如圖2(c)、圖3(c)、圖4(c)所示，生成的軌跡為直線，也就是歐氏空間中的最短線。并且根據機器人運動方程把初始轉角代入其中，得到的初始機器人自身坐標系與世界坐標系的夾角θ與歐氏空間中直線與x軸的夾角是一致的。由此得到，由測地線方程求解直接生成的測地線是局部的最短線。

4 結論

首先建立了兩輪差分移動機器人的數學模型，在此數學模型的基礎上，構建了移動機器人的運動軌跡弧長平方的運動學黎曼度量。該黎曼度量下的測地線方程是機器人運動的最短路徑。在給定的黎曼度量下，測地線由初始條件唯一確定。傳統的移動機器人軌跡規劃主要是利用各種優化算法得到機器人的最優軌跡，然后轉化為機器人電機的控制變量，控制機器人沿著規劃好的軌跡運動。而基于測地線的軌跡規劃方法是根據不同的優化目標得到的黎曼度量，直接得到關節(移動機器人輪子電機)空間內的測地線最優軌跡，對機器人進行控制。另外，傳統的移動移動機器人控制變量是基于時間參考的，當機器人遇到未知障礙受阻后，原來的規劃可能會失效。而測地線軌跡規劃是基于路徑弧長為參考的，當障礙物被移除后，機器人可以按照原有的非時間參考的軌跡規劃繼續運動。

通過Python語言，針對不同的初始條件求出了相應的測地線方程，實現了測地線方程ROS環境下的求解，軌跡仿真結果驗證了測地線軌跡規劃方法的正確性，為測地線實時軌跡規劃與ROS系統實時控制的集成奠定了基礎。