高 翔，郭衛(wèi)東

（北京航空航天大學 機械工程及自動化學院，北京 100191）

0 引言

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

1 路徑規(guī)劃建模

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

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

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

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

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

1.2 路徑規(guī)劃的評價函數(shù)

對于路徑規(guī)劃，考慮3個評價指標：路徑長度、路徑平滑度以及路徑安全度。

第1條指標是路徑的長度。路徑越短則路徑越優(yōu)［3］，其評價公式為：

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

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

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

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

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

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

圖1 機器人及其坐標系

圖2 聲納與障礙物交點示意圖

圖3 路徑示意圖

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

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

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

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

（1）運動學約束：機器人在有限時間內(nèi)只能移動有限距離和角度，因此其速度應該在有限區(qū)間內(nèi)，即：

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

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

（3）避障約束：路徑規(guī)劃時，應保證每個障礙物點都位于機器人位姿外接圓之外，即：

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

2 基于粒子群算法的求解

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

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

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

2.2 約束處理機制

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

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

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

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

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

4 結(jié)論及展望

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

圖4 路徑安全度評價

圖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)境中移動機器人導航控制理論與方法［M］.北京：科學出版社，2009.

［3］張純剛，席裕庚.全局環(huán)境未知時基于滾動窗口的機器人路徑規(guī)劃［J］.中國科學，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］張國英，吳藝娟.基于多級懲罰函數(shù)的粒子群約束優(yōu)化算法［J］.北京石油化工學院學報，2008（12）：30-32.