室內(nèi)移動(dòng)機(jī)器人路徑規(guī)劃算法

高 翔，郭衛(wèi)東

（北京航空航天大學(xué) 機(jī)械工程及自動(dòng)化學(xué)院，北京 100191）

0 引言

路徑規(guī)劃是指在具有障礙物的環(huán)境中，按照一定的評(píng)價(jià)標(biāo)準(zhǔn)，尋找一條從起始位姿到目標(biāo)位姿的最優(yōu)路徑［1］。根據(jù)對(duì)環(huán)境信息的掌握程度，路徑規(guī)劃技術(shù)包括環(huán)境已知的全局路徑規(guī)劃和環(huán)境未知的局部路徑規(guī)劃［2］。本文采用最優(yōu)化方法對(duì)路徑規(guī)劃問題進(jìn)行建模，采用帶有懲罰函數(shù)的改進(jìn)粒子群算法對(duì)路徑規(guī)劃問題的最優(yōu)化模型進(jìn)行求解，并在MATLAB中通過仿真試驗(yàn)驗(yàn)證算法的有效性。

1 路徑規(guī)劃建模

1.1 機(jī)器人與障礙物的數(shù)學(xué)表示

將機(jī)器人Ar視為二維環(huán)境下的自由移動(dòng)剛體，其最小外接圓定義為機(jī)器人圓Cr，半徑為Rr，圓心為Or（xr，yr）。以O(shè)r為原點(diǎn)，機(jī)器人前進(jìn)方向?yàn)閄軸正方向，建立機(jī)器人坐標(biāo)系XrYrOr，如圖1所示。

其中：g（s）和h（s）均為由機(jī)器人聲納安裝位置決定的函數(shù)；N為非負(fù)整數(shù)集。

對(duì)ΦR的點(diǎn)，將其變換到世界坐標(biāo)系XgYgOg中得到點(diǎn)集Φg。本算法中，用點(diǎn)集Φg替代障礙物集Obtacle作為障礙物的數(shù)學(xué)表示。

路徑示意圖如圖3所示。曲線PsPg表示機(jī)器人的真實(shí)路徑。機(jī)器人的起始位姿為Ps（xs，ys，θs），目標(biāo)位姿為Pg（xg，yg，θg），其中θs與θg為在起點(diǎn)處與終點(diǎn)處世界坐標(biāo)系與機(jī)器人坐標(biāo)系間的夾角。取Ps和Pg間的（s－2）個(gè)中間位姿Pj（xj，yj，θj），則P＝｛Ps，P1，…，Pg｝即作為一條候選路徑的數(shù)學(xué)表示。

1.2 路徑規(guī)劃的評(píng)價(jià)函數(shù)

對(duì)于路徑規(guī)劃，考慮3個(gè)評(píng)價(jià)指標(biāo)：路徑長(zhǎng)度、路徑平滑度以及路徑安全度。

第1條指標(biāo)是路徑的長(zhǎng)度。路徑越短則路徑越優(yōu)［3］，其評(píng)價(jià)公式為：

第2條指標(biāo)是路徑平滑度。機(jī)器人的轉(zhuǎn)動(dòng)與機(jī)器人的運(yùn)動(dòng)時(shí)間有直接的關(guān)系，如果路徑中存在角度較大的轉(zhuǎn)動(dòng)，機(jī)器人必須減慢速度甚至停止，這樣走完路徑所耗費(fèi)的時(shí)間就會(huì)增加很多，因此將路徑平滑度作為另外一個(gè)優(yōu)化指標(biāo)。路徑平滑度定義如下：

其中：Ij（0≤Ij≤π）為向量Pj－1Pj與PjPj＋1間的夾角；k1和k2為調(diào)節(jié)兩部分影響強(qiáng)度的因子。

第3條指標(biāo)為路徑安全度。路徑上各位姿與障礙物的距離越遠(yuǎn)，則該路徑越安全。如前所述，本文中用點(diǎn)集Φg來表示障礙物。對(duì)于路徑中的每個(gè)位姿Pj（xj，yj，θj），將位姿與障礙物間的距離定義為機(jī)器人中心與所有影響障礙物點(diǎn)垂直于SG連線方向的距離的最小值（如圖4所示），對(duì)位姿影響的障礙物點(diǎn)定義為沿SG連線方向上與位姿中心的距離小于某個(gè)固定值Sf的那些障礙物點(diǎn)。則路徑安全度的評(píng)價(jià)公式如下：

其中：k為位姿Pj的影響障礙物點(diǎn)的個(gè)數(shù)；dsj為位姿Pj中心與影響障礙物點(diǎn)沿垂直于SG連線方向的距離。

設(shè)聲納所能測(cè)量的最大距離為L(zhǎng)m，將以機(jī)器人坐標(biāo)系原點(diǎn)為圓心、Lm為半徑的圓定義為機(jī)器人感知圓Cs，將路徑中位于感知圓Cs內(nèi)的各位姿與障礙物的距離表示的和作為路徑安全度的評(píng)價(jià)函數(shù)：

其中：q為位于感知圓Cs內(nèi)部的位姿Pj個(gè)數(shù)。

圖1 機(jī)器人及其坐標(biāo)系

圖2 聲納與障礙物交點(diǎn)示意圖

圖3 路徑示意圖

綜上所述，路徑規(guī)劃的最終評(píng)價(jià)函數(shù)為：

其中：ω1，ω2，ω3為調(diào)節(jié)3個(gè)評(píng)價(jià)指標(biāo)影響大小的參數(shù)。

1.3 路徑規(guī)劃的約束條件

在路徑規(guī)劃過程中，還需要滿足各種約束：運(yùn)動(dòng)學(xué)約束、始末位姿約束和避障約束。

（1）運(yùn)動(dòng)學(xué)約束：機(jī)器人在有限時(shí)間內(nèi)只能移動(dòng)有限距離和角度，因此其速度應(yīng)該在有限區(qū)間內(nèi)，即：

其中：Xd，Yd，θd均為與機(jī)器人硬件相關(guān)的參數(shù)。

（2）始末位姿約束：機(jī)器人的初始位姿和目標(biāo)位姿均已知，故有以下等式約束：

（3）避障約束：路徑規(guī)劃時(shí)，應(yīng)保證每個(gè)障礙物點(diǎn)都位于機(jī)器人位姿外接圓之外，即：

綜上所述，路徑規(guī)劃問題建模如下：

2 基于粒子群算法的求解

2.1 基本粒子群優(yōu)化算法

粒子群算法（PSO）的數(shù)學(xué)表示如下［4］：設(shè)搜索空間為r維，粒子總數(shù)為u，第l個(gè)粒子的位置向量為xl（t）＝（xl1，xl2，…，xlr），瞬時(shí)速 度 向 量 為vl（t）＝（vl1，vl2，…，vlr），第l個(gè)粒子搜索到的最優(yōu)位置為pl＝（pl1，pl2，…，plr），整個(gè)粒子群搜索到的最優(yōu)位置為g＝（g1，g2，...，gr）。則每個(gè)粒子的速度和位置更新公式為：

其中：c1，c2分別為自身認(rèn)識(shí)因子和社會(huì)認(rèn)識(shí)因子，用來調(diào)整pl和g的影響強(qiáng)度，c1，c2為正數(shù)；r1l，r2l為區(qū)間［0，1］內(nèi)的隨機(jī)數(shù)；ω為速度慣性因子，表示前一時(shí)刻速度的影響強(qiáng)度。

2.2 約束處理機(jī)制

基本的粒子群算法用于求解無約束最優(yōu)化問題，因此，對(duì)于上述約束優(yōu)化問題，需要相應(yīng)的約束處理機(jī)制。本文采用文獻(xiàn)［5］提出的方法，將罰函數(shù)法與PSO算法相結(jié)合，在迭代過程中用懲罰方法構(gòu)造增廣目標(biāo)函數(shù)，將約束問題轉(zhuǎn)化為無約束優(yōu)化問題。加入懲罰項(xiàng)的增廣目標(biāo)函數(shù)如下：

其中：f（x）為初始目標(biāo)函數(shù)；γ（t）＝為懲罰函數(shù)的因子，t為粒子群算法的迭代次數(shù)；P（x）為懲罰項(xiàng)。

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

在MATLAB中使用含有一個(gè)多邊形障礙物的模擬環(huán)境來檢驗(yàn)算法的有效性。機(jī)器人的起始位姿和目標(biāo)位姿如圖5所示，規(guī)劃的結(jié)果如圖6所示。

仿真結(jié)果驗(yàn)證了本文中所提算法的有效性。采用這種算法的移動(dòng)機(jī)器人具有良好的避障能力。

4 結(jié)論及展望

本文提出了一種考慮路徑平滑度和路徑安全度的路徑規(guī)劃算法并仿真驗(yàn)證了該算法的有效性。在本文中，采用障礙物點(diǎn)來作為環(huán)境的數(shù)學(xué)表示，這是一種較為簡(jiǎn)單的方法，在復(fù)雜環(huán)境中，這可能導(dǎo)致路徑規(guī)劃算法的失效，在將來的工作中，將對(duì)障礙物的數(shù)學(xué)表示作進(jìn)一步的研究。

圖4 路徑安全度評(píng)價(jià)

圖5 帶有障礙物的仿真環(huán)境

圖6 規(guī)劃結(jié)果示意圖

［1］Roland S，Iiiah R N，Davide Scaramuzza.Introduction to Autonomous Mobile Robot［M］.USA：The MIT Press，2012.

［2］蔡自興，賀漢根.未知環(huán)境中移動(dòng)機(jī)器人導(dǎo)航控制理論與方法［M］.北京：科學(xué)出版社，2009.

［3］張純剛，席裕庚.全局環(huán)境未知時(shí)基于滾動(dòng)窗口的機(jī)器人路徑規(guī)劃［J］.中國(guó)科學(xué)，2001，31（1）：51-58.

［4］James Kennedy，Russell Eberhart.Particle swarm optimization［C］//Proceedings of IEEE International Conference on Neural Networks.［s.l.］：IEEE，1995：1942-1948.

［5］張國(guó)英，吳藝娟.基于多級(jí)懲罰函數(shù)的粒子群約束優(yōu)化算法［J］.北京石油化工學(xué)院學(xué)報(bào)，2008（12）：30-32.