基于遍歷測度理論的機器人空間搜索無碰撞軌跡規(guī)劃

周 浩，唐 倩*，李代楊，李志航，撒利軍

（1.重慶大學 機械傳動國家重點實驗室，重慶 400044；2.重慶長安汽車股份有限公司，重慶 400023）

0 引言

在機器人的應用領域中，有許多應用需要高效控制策略用于空間搜尋任務，比如監(jiān)控領域、目標定位等領域[1]。機器人直接對空間進行逐點搜尋效率較低，并且考慮到機器人的供電續(xù)航問題，逐點空間搜尋的策略明顯不符合實際。此外，機器人在空間搜尋的過程中還要避免與環(huán)境的障礙發(fā)生碰撞。所以本文針對機器人在搜索空間無碰撞的運動軌跡的規(guī)劃算法進行研究。

基于模型的空間無碰撞軌跡規(guī)劃方法由于其可靠性高得到了廣泛關注與研究，目前常見的算法是人工勢場法和快速隨機樹算法。早期KHATIB[2]通過虛擬力建立了環(huán)境的人工勢場，采用梯度下降的方法得到機器人的運動軌跡。最近，張強等[3]基于劉建華提出的軌跡規(guī)劃算法[4]提出了改進人工勢場的蟻群算法，從而實現(xiàn)了移動機器人的最優(yōu)路徑規(guī)劃。此外，隨機樹擴展方法在高維空間的高效率規(guī)劃而廣泛應用于機器人軌跡規(guī)劃領域[5]，該算法主要包括隨機路徑規(guī)劃法（RRT[6]）及其改進算法RRT-Connect[7]。雖然上述算法都能用于移動機器人運動規(guī)劃，但是上述算法只能實現(xiàn)初始位置到終點位置的無碰撞路徑規(guī)劃，不能實現(xiàn)空間搜尋的軌跡。隨著測度理論的不斷發(fā)展，Mathew[8]提出了遍歷性的度量標準，并通過最小化哈密頓函數(shù)得到遍歷控制的最優(yōu)控制量的范圍，Todd D.Murphey[9]基于遍歷度量提出了遍歷迭代控制算法（Ergodic iterative-Sequential Action Control,EISAC），并利用該方法對無人機的目標軌跡追蹤進行了研究。

本文針對機器人空間搜尋無碰撞軌跡規(guī)劃問題，嘗試建立空間搜尋無碰撞軌跡的目標函數(shù)。并通過求解建立的優(yōu)化目標函數(shù)得到機器人的控制策略，從而得到機器人空間搜尋的無碰撞軌跡。

1 空間搜尋策略的數(shù)學模型

針對機器人的空間搜尋無碰撞軌跡規(guī)劃問題，本文擬基于遍歷測度理論對空間搜尋無碰撞軌跡進行建模。

1.1 搜尋策略模型

機器人空間搜尋無碰撞軌跡通過遍歷測度理論建立目標函數(shù)。該標函數(shù)需要滿足系統(tǒng)狀態(tài)方程和控制量的約束，通過約束的優(yōu)化目標函數(shù)可以得到最優(yōu)控制策略。空間搜尋可避障軌跡規(guī)劃的數(shù)學模型如式(1)所示。

其中ε為目標函數(shù)，μ*為最優(yōu)控制，x為規(guī)劃系統(tǒng)的狀態(tài)，f為系統(tǒng)狀態(tài)方程。

1.2 搜尋軌跡度量標準

遍歷測度理論表明，搜索空間區(qū)域的軌跡分布與輸入空間信息的密度成正比（如圖1所示），規(guī)劃軌跡在空間的分布可以通過式(2)求解：

圖1 遍歷軌跡與輸入信息關系圖

其中：δ表示狄拉克函數(shù)，s為空間[0,L1]×[0,L1]×…×[0,L1]上的點，v為空間的維度，xv(t)表示t時刻軌跡位于空間的位置，因子1/T是為了保證搜索軌跡分布滿足概率和為1的性質(zhì)。

本文利用傅里葉系數(shù)量化空間信息分布與均勻時間的軌跡分布，空間信息分布Φ(s)的傅里葉系數(shù)φk以及空間均勻時間的軌跡分布C(s)的傅里葉系數(shù)Ck可以通過式(3)與式(4)得到：

對于空間信息和軌跡均勻時間分布計算中的傅里葉基函數(shù)Fk可以通過式(5)求得：

其中hk為歸一化系數(shù)，k=0,1,2…，k越大越能反映空間信息分布，從圖2中可以得到：隨著的增加，在空間中的信息分布越精確，但是k值過大會影響計算效率。

圖2 空間信息與的關系圖

結(jié)合式(3)～式(5)可得到遍歷性度量目標函數(shù)，如式(6)：

式(6)中的∧k=1/(1+||k||2)s，主要目的是使輸入空間低頻信息放大。

2 空間搜索無碰撞軌跡求解

在建立了空間軌跡與空間信息的遍歷性度量的待優(yōu)化目標函數(shù)后，需要優(yōu)化目標函數(shù)得到當前時域的最優(yōu)控制，從而實現(xiàn)空間搜索無碰撞軌跡規(guī)劃。本文擬采用遍歷迭代控制算法（EISAC）對目標函數(shù)進行求解，得到當前時域最優(yōu)控制解析解。通過遍歷度量標準，可以得到搜索軌跡的待優(yōu)化目標函數(shù)：

其中terg0為自定義的遍歷搜索的初始時間，x∈x2為系統(tǒng)的狀態(tài)變量，Q∈R為系統(tǒng)的代價系數(shù)，其余系數(shù)見前述內(nèi)容。本文基于目標函數(shù)(7)，采用遍歷迭代控制策略，在每個時域內(nèi)優(yōu)化目標函數(shù)得到最優(yōu)的控制策略u*解析解：

式(8)中ψ=h(x)TρρTh(x)，ρ為最優(yōu)控制策略求解過程的中間變量，為系統(tǒng)狀態(tài)方程的狀態(tài)轉(zhuǎn)移矩陣，u1為上一時域的最優(yōu)控制策略，αd∈R-為調(diào)節(jié)系數(shù)且為常數(shù)，一般較小。上述變量具體求解方法見文獻[10,11]。

遍歷迭代控制算法流程如圖3所示。

圖3 EISAC算法流程圖

EISAC算法的主要流程如下：1）通過輸入空間計算輸入空間信息的傅里葉系數(shù)，2）基于輸入空間信息的傅里葉系數(shù)執(zhí)行遍歷迭代控制算法，3）利用微分方程求解系統(tǒng)的狀態(tài)x，計算遍歷迭代控制算法的中間變量，4）利用最優(yōu)控制策略求解空間搜索軌跡的最優(yōu)控制策略u*，5）然后計算遍歷迭代控制策略的作用時間λA和時刻τA，6）最后輸出空間搜索無碰撞軌跡。

3 空間搜索無碰撞軌跡仿真

為了驗證遍歷迭代控制算法能夠?qū)崿F(xiàn)空間搜索無碰撞軌跡規(guī)劃，本文在Matlab2018中進行仿真實驗。因為二階系統(tǒng)的軌跡連續(xù)性較一階系統(tǒng)好，所以本文采用二階系統(tǒng)對空間搜索可避障軌跡進行仿真規(guī)劃。二階系統(tǒng)的空間狀態(tài)方程：

其中x，y分別為二階系統(tǒng)在笛卡爾空間下的坐標，u1，u2為對應時域控制輸入量。

本文采用遍歷迭代控制算法對二階系統(tǒng)的空間搜索無碰撞軌跡進行數(shù)值仿真，其中遍歷迭代控制算法（EISAC）仿真的主要參數(shù)如表1所示。

表1 EISAC算法仿真參數(shù)表

3.1 搜索無碰撞軌跡仿真

本文基于遍歷迭代控制算法，得到了3類環(huán)境的空間搜索無碰撞軌跡規(guī)劃仿真，環(huán)境1是為了證明算法能夠?qū)崿F(xiàn)不同障礙下的遍歷軌跡規(guī)劃，環(huán)境2是為了證明算法能夠?qū)崿F(xiàn)半封閉障礙下的軌跡規(guī)劃，環(huán)境3是為了證明算法能夠?qū)崿F(xiàn)復雜環(huán)境的軌跡規(guī)劃。遍歷迭代控制算法（EISAC）的MATLAB仿真結(jié)果如圖4所示。

圖4中輸入空間信息為圖中輸入環(huán)境行。本文根據(jù)遍歷迭代控制算法，在MATLAB中得到了10、20、40、60時間步長的空間搜索無碰撞軌跡規(guī)劃的仿真結(jié)果（如圖4中的輸出軌跡行所示）。

圖4 遍歷迭代控制（EISAC）算法仿真結(jié)果

仿真結(jié)果表明，1）機器人搜索空間的無碰撞軌跡覆蓋面積隨著時間的增加而增加；2）本文采用的遍歷迭代控制算法（EISAC）能夠適用于不同環(huán)境甚至復雜環(huán)境下的空間搜索任務；3）本文采用的遍歷迭代控制算法不僅能夠?qū)崿F(xiàn)機器人在空間中的搜索任務，而且還能實現(xiàn)避障功能。

3.2 目標函數(shù)收斂性分析

仿真過程中目標函數(shù)的變化情況如圖5所示。圖5表明，本文基于遍歷測度理論建立的空間搜索模型能夠較快的收斂，從而進一步證明了算法的可行性。

圖5 遍歷迭代控制的代價變化趨勢圖

4 結(jié)語

針對機器人空間搜索無碰撞軌跡規(guī)劃問題，本文基于遍歷測度理論建立了空間搜索軌跡與空間信息的度量標準，并基于該遍歷度量標準的模型建立了待優(yōu)化的目標函數(shù)。然后通過遍歷迭代控制算法優(yōu)化目標函數(shù)得到了最優(yōu)控制量。最后，不同環(huán)境的空間搜尋無碰撞軌跡規(guī)劃仿真表明：基于遍歷測度理論建立的空間搜索算法不僅能用于不同環(huán)境機器人的無碰撞搜索軌跡，而且目標函數(shù)能較快的收斂，從而證明了算法的有效性。