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

（1.重慶大學(xué) 機(jī)械傳動(dòng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，重慶 400044；2.重慶長(zhǎng)安汽車股份有限公司，重慶 400023）

0 引言

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

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

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

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

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

1.1 搜尋策略模型

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

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

1.2 搜尋軌跡度量標(biāo)準(zhǔn)

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

圖1 遍歷軌跡與輸入信息關(guān)系圖

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

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

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

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

圖2 空間信息與的關(guān)系圖

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

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

2 空間搜索無(wú)碰撞軌跡求解

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

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

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

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

圖3 EISAC算法流程圖

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

3 空間搜索無(wú)碰撞軌跡仿真

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

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

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

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

3.1 搜索無(wú)碰撞軌跡仿真

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

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

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

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

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

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

圖5 遍歷迭代控制的代價(jià)變化趨勢(shì)圖

4 結(jié)語(yǔ)

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