基于測(cè)地線的移動(dòng)機(jī)器人軌跡規(guī)劃方法

劉鵬程，張連東，宋雪萍

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

0 前言

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

本文作者提出一種通過求解測(cè)地線微分方程直接得到移動(dòng)機(jī)器人輪子轉(zhuǎn)角變量的測(cè)地線軌跡的方法。以移動(dòng)機(jī)器人的輪子轉(zhuǎn)角為變量，構(gòu)建機(jī)器人軌跡弧長(zhǎng)平方的黎曼度量，通過求解該黎曼度量下測(cè)地線的微分方程，直接得到機(jī)器人輪子轉(zhuǎn)角和角速度表示的移動(dòng)機(jī)器人路徑最優(yōu)的運(yùn)動(dòng)軌跡。根據(jù)測(cè)地線的幾何性質(zhì)，沿著測(cè)地線切線方向的速度變化率為0，機(jī)器人在沿測(cè)地線路徑的運(yùn)動(dòng)中速度平穩(wěn)。路徑是以軌跡弧長(zhǎng)為參考，不受時(shí)間約束。求解測(cè)地線方程得到測(cè)地線的方法，為測(cè)地線理論在移動(dòng)機(jī)器人軌跡規(guī)劃上的實(shí)際應(yīng)用奠定基礎(chǔ)。

1 黎曼流形與度量的概念

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

G=gijdui?duj

(1)

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

ds2=gijduiduj

(2)

(3)

2 構(gòu)建度量張量求解Christoffel符號(hào)

以兩輪差分驅(qū)動(dòng)機(jī)器人為例，如圖1所示差分驅(qū)動(dòng)機(jī)器人依靠調(diào)節(jié)左右輪子獨(dú)立的軸向運(yùn)動(dòng)來實(shí)現(xiàn)轉(zhuǎn)彎功能。XI和YI共同組成平面上的全局坐標(biāo)系，XR和YR構(gòu)成機(jī)器人自身坐標(biāo)系。為了方便研究機(jī)器人在全局坐標(biāo)系下的運(yùn)動(dòng)，通常選取機(jī)器人本體上的一個(gè)點(diǎn)代替機(jī)器人表示其在全局坐標(biāo)系中的位置。以點(diǎn)P為機(jī)器人參考點(diǎn)，點(diǎn)P一般選取兩輪差分驅(qū)動(dòng)機(jī)器人的兩輪軸線中間位置，則機(jī)器人的運(yùn)動(dòng)學(xué)方程表示為

圖1 差分驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型簡(jiǎn)圖

(4)

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

假設(shè)M是二維黎曼流形，在二維黎曼流形上以軌跡弧長(zhǎng)微分的平方作為黎曼度量。機(jī)器人路徑長(zhǎng)度的黎曼度量表示為

ds2=dx2+dy2

(5)

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

為了構(gòu)造符合機(jī)器人轉(zhuǎn)角參數(shù)的黎曼度量，同時(shí)滿足度量矩陣的正定性質(zhì)，把運(yùn)動(dòng)學(xué)方程中的x替換為dx，y替換為dy，θ1和θ2分別用dθ1和dθ2代替，將數(shù)值代入黎曼度量表達(dá)式(5)中，得到度量為

(6)

將表達(dá)式(2)簡(jiǎn)述為度量矩陣的形式如下：

(7)

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

(8)

矩陣G就是黎曼度量矩陣，因?yàn)槠湔ㄐ郧蟮闷淠婢仃嘒-1為式(9)：

(9)

其中g(shù)ij為G-1的第i行j列的元素。

而表達(dá)式：

(10)

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

SymPy是Python語言中用于代數(shù)運(yùn)算的庫，在mpmath庫的支持下可以進(jìn)行任何浮點(diǎn)運(yùn)算。SymPy作為一種交互式工具，可以在其他程序中自定義函數(shù)進(jìn)行代碼的擴(kuò)展。引用SymPy中的diffgeom函數(shù)關(guān)于流形、張量計(jì)算、第二類Christoffel符號(hào)來實(shí)現(xiàn)克氏符號(hào)分量的求解。在Python語言中習(xí)慣從零開始索引，因此在程序中把x0、x1表示為左右輪子的轉(zhuǎn)角θ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)

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

(11)

3 測(cè)地線方程的求解

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

(12)

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

(13)

式中:i、j、k循環(huán)取遍1、2。

將測(cè)地線方程展開為如下兩個(gè)等式：

(14)

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

(15)

(16)

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

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中的的參數(shù)數(shù)值代入函數(shù)中：

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

v=[r,b]

w0=[x0,y0,x1,y1]

取終止長(zhǎng)度為2,樣本點(diǎn)為500：

stoplength = 2

numpoints = 500

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

調(diào)用odeint()函數(shù)，設(shè)置相應(yīng)的參數(shù)求解微分方程：

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

算法流程如下:

(1)定義以轉(zhuǎn)角為參數(shù)的差分驅(qū)動(dòng)機(jī)器人測(cè)地線方程，將一階微分方程定義到函數(shù)之中。

(2)根據(jù)不同的運(yùn)動(dòng)狀態(tài)定義初始條件。

(3)設(shè)置ode求解器適當(dāng)?shù)膮?shù)。

(4) 為ode求解器輸出創(chuàng)建曲線長(zhǎng)度樣本。

(5)調(diào)用ode求解器實(shí)現(xiàn)測(cè)地線方程的求解。

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

差分驅(qū)動(dòng)機(jī)器人的初始位姿與由角速度引起的機(jī)器人軌跡的變化是不相關(guān)的，也就是說差分驅(qū)動(dòng)機(jī)器人按照固定的角度與角速度運(yùn)動(dòng)，所產(chǎn)生的內(nèi)在軌跡是不變的，只不過機(jī)器人自身坐標(biāo)系相對(duì)世界坐標(biāo)系轉(zhuǎn)角發(fā)生變化。因此為了方便計(jì)算，假設(shè)機(jī)器人的初始位置在世界坐標(biāo)系的原點(diǎn)，取弧長(zhǎng)范圍是0～2,樣本點(diǎn)為500個(gè)。初始化不同角速度如下：

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

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

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

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

情況A中，圖2(a)表示在弧長(zhǎng)遞增的情況下左、右輪子角度與弧長(zhǎng)的關(guān)系，可以看出角度分別從各自的初始角度逐漸增加，并且增速基本相同，這也就意味著機(jī)器人在環(huán)境中是前進(jìn)的。由于給定的兩輪初始角速度是相等的，通過圖2(b)可知，角速度與軌跡弧長(zhǎng)關(guān)系的曲線重合。在弧長(zhǎng)遞增的情況下，由測(cè)地線微分方程得到的機(jī)器人左、右輪子的角速度軌跡是相同的且保持相對(duì)的穩(wěn)定。圖2(d)表示關(guān)節(jié)空間中的測(cè)地線軌跡，因?yàn)槌跏冀撬俣仁窍嗤?，所以軌跡是一條斜直線。

情況B中，因?yàn)樽笥逸喿硬煌慕撬俣纫约安煌钠鹗冀嵌?，圖3(a)中的軌跡形狀與圖2是不同的。左輪轉(zhuǎn)角與弧長(zhǎng)關(guān)系曲線在右輪上方，這是因?yàn)槌跏冀嵌戎g存在差異。按照?qǐng)D像曲線的走勢(shì)，可以看到隨著弧長(zhǎng)的增加，左、右輪子轉(zhuǎn)角越來越接近。對(duì)于圖3(b)，右輪的角速度比左輪大，呈現(xiàn)出來的軌跡是上凸的曲線，這符合機(jī)器人的運(yùn)動(dòng)學(xué)模型。情況C中，左右輪子的角速度差值不如情況B，可以從圖4(d)中看出曲線上凸不是很明顯，這也就進(jìn)一步證明與機(jī)器人運(yùn)動(dòng)是相符的。

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

4 結(jié)論

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

通過Python語言，針對(duì)不同的初始條件求出了相應(yīng)的測(cè)地線方程，實(shí)現(xiàn)了測(cè)地線方程ROS環(huán)境下的求解，軌跡仿真結(jié)果驗(yàn)證了測(cè)地線軌跡規(guī)劃方法的正確性，為測(cè)地線實(shí)時(shí)軌跡規(guī)劃與ROS系統(tǒng)實(shí)時(shí)控制的集成奠定了基礎(chǔ)。