基于混合整數(shù)線性規(guī)劃的爬壁機(jī)器人路徑規(guī)劃

岳榮剛

（中國空間技術(shù)研究院 錢學(xué)森空間技術(shù)實(shí)驗(yàn)室，北京 100094）

王少萍

（北京航空航天大學(xué) 自動(dòng)化科學(xué)與電氣工程學(xué)院，北京 100191）

爬壁機(jī)器人可以在地面、墻壁和天花板上運(yùn)動(dòng)，完成建筑物的清洗、噴漆、檢測等工作.紐約市立大學(xué)研制了數(shù)代爬壁機(jī)器人［1－2］，本文研究的機(jī)器人名為City-Climber.多數(shù)地面機(jī)器人在2D環(huán)境中運(yùn)動(dòng)，無人機(jī)在3D環(huán)境中運(yùn)動(dòng)，而爬壁機(jī)器人在一種受限的3D環(huán)境中運(yùn)動(dòng)，其運(yùn)動(dòng)被限制在三維空間中的平面上，如地面、墻壁和天花板等.這使得爬壁機(jī)器人的路徑規(guī)劃變得困難，要求機(jī)器人不僅能從一個(gè)平面運(yùn)動(dòng)到另一個(gè)平面，且要避免與其他機(jī)器人或障礙相撞.

混合整數(shù)線性規(guī)劃（MILP，Mixed Integer Linear Programming）對(duì)規(guī)劃和控制問題是一種強(qiáng)大的工具，它將非線性問題轉(zhuǎn)化為線性問題，并用AMPL和CPLEX等軟件求解.文獻(xiàn)［3］研究了爬壁機(jī)器人在三維工作環(huán)境中的全局路徑規(guī)劃，但未考慮避障因素；文獻(xiàn)［4］針對(duì)TSP（Traveling Salesman Problem）問題，介紹了立方體表面路徑規(guī)劃方法；文獻(xiàn)［5］針對(duì)圓柱形油罐焊接問題，提出了一種爬壁機(jī)器人路徑規(guī)劃方法.

本文在City-Climber動(dòng)力學(xué)模型基礎(chǔ)上，研究基于MILP的爬壁機(jī)器人3D路徑規(guī)劃問題，并針對(duì)City-Climber的特點(diǎn)提出一種代價(jià)函數(shù).

1 City-Climber模型解耦和線性化

在文獻(xiàn)［6］中，建立了City-Climber機(jī)器人的動(dòng)力學(xué)模型：

式中，（x，y）為機(jī)器人的幾何中心（質(zhì)心）；θ為機(jī)器人方位角；m為機(jī)器人的質(zhì)量；J為機(jī)器人的轉(zhuǎn)動(dòng)慣量；d為履帶輪到City-Climber的中心距；ui（θ，t）＝Pi（θ）Ui（t）為控制輸入，i＝1，2，3.

當(dāng)機(jī)器人在地面運(yùn)動(dòng)時(shí)，i＝1，有

當(dāng)機(jī)器人在墻壁運(yùn)動(dòng)時(shí)，i＝2，有

當(dāng)機(jī)器人在天花板運(yùn)動(dòng)時(shí)，i＝3，有

式中，fi1（i＝1，2，3）分別為地面、墻壁和天花板與一個(gè)履帶輪間的滾動(dòng)摩擦力；U1＝Uu1／Umax，U2＝Uu2／Umax分別為兩個(gè)驅(qū)動(dòng)電機(jī)的控制輸入，Uu1和Uu2分別為兩個(gè)驅(qū)動(dòng)電機(jī)的實(shí)際輸入電壓，Umax為兩個(gè)驅(qū)動(dòng)電機(jī)的最大輸入電壓；2fi1／αUmax（i＝1，2，3）相當(dāng)于地面、墻壁和天花板對(duì)機(jī)器人的摩擦力輸入，α為電機(jī)常數(shù)，N／V；Mri（i＝1，2，3）分別為地面、墻壁和天花板對(duì)機(jī)器人的阻力矩.

City-Climber方位角θ的存在使得數(shù)學(xué)模型存在耦合和非線性，本文參考文獻(xiàn)［7］中的方法，通過設(shè)置一系列容許控制方程，對(duì)City-Climber的3個(gè)數(shù)學(xué)模型分別進(jìn)行解耦和線性化.

1.1 地面模型的解耦和線性化

允許的最大輸入U(xiǎn)＊1（t）為下式描述的正方形：

式（1）、式（2）、式（5）中的控制輸入P1（θ）U1（t）取決于City-Climber的方位角θ，本文用與θ無關(guān)的物理量將其代替.最大化所有容許控制的交集：

新的控制輸入u1（t）∈U∩1（t）就是本文要找的物理量，將其代替控制輸入可對(duì)方程進(jìn)行解耦.下面給出U∩（t）的直觀描述，對(duì)于給定的θ，線性變換P1（θ）將正方形U＊1（t）映射到一個(gè)描述容許控制的錐體P1（θ）U＊1（t）上，而P1（θ）可以分解為旋轉(zhuǎn)矩陣Rz（θ）和一個(gè)與θ無關(guān)的矩陣P1（0）：

式中

P1（0）將正方形（t）映射到傾斜的正方形P1（0（t）（圖1），其對(duì)角線方向上滿足｜uθ｜≤2＋Mr1／αdUmax.Rz（θ）使斜正方形繞u1θ軸旋轉(zhuǎn)，得到實(shí)心錐體Rz（θ）P1（0（t）：

式中，半徑表達(dá)式為

得到的實(shí)心錐體如圖2所示，該錐體內(nèi)部的點(diǎn)即為允許的控制輸入取值.u1x，u1y和u1θ就是用來解耦的新的控制輸入量.

圖1 City-Climber在地面運(yùn)動(dòng)的映射P1（0）

圖2 容許控制約束的實(shí)心錐體

City-Climber地面運(yùn)動(dòng)的動(dòng)力學(xué)方程簡化為

該方程被線性化并已解耦，需滿足下式：

1.2 墻壁模型的解耦和線性化

與1.1節(jié)相同的方法，對(duì)City-Climber在墻壁運(yùn)動(dòng)的模型進(jìn)行解耦和線性化，得到與式（12）類似的動(dòng)力學(xué)方程，約束條件為

1.3 天花板模型的解耦和線性化

與1.1節(jié)相同的方法，對(duì)City-Climber在天花板運(yùn)動(dòng)的模型進(jìn)行解耦和線性化，得到與式（12）類似的動(dòng)力學(xué)方程，約束條件為

City-Climber的物理結(jié)構(gòu)決定了只要得到其速度和，就可以求得其方位角θ值.本文后續(xù)部分只考慮City-Climber動(dòng)力學(xué)微分方程中的前2個(gè)方程，即控制機(jī)器人平移自由度的方程，而不求解代表機(jī)器人方位角的第3個(gè)方程.

2 MILP的數(shù)學(xué)描述

MILP是線性規(guī)劃的一種，符合線性等式或不等式的約束條件，并要求線性約束中的一部分變量必須是整數(shù).

2.1 基于MILP方法的控制輸入問題描述

City-Climber平動(dòng)的動(dòng)力學(xué)微分方程為

滿足：

式中，umax代表控制輸入的最大值.

式（16）可以寫為狀態(tài)空間的形式：

離散化控制輸入，規(guī)定在兩個(gè)時(shí)間點(diǎn)之間，輸入量為常數(shù)，可得一系列線性離散時(shí)間控制方程.本文用下標(biāo)u表示控制輸入有關(guān)的變量，Nu為對(duì)控制輸入進(jìn)行離散化的步數(shù)，tu［k］為第k步的時(shí)間，Tu［k］＞0為第k步到k＋1步的時(shí)間間隔，k∈｛0，1，…，Nu－1｝.離散時(shí)間控制方程表述為

［8］，可得式（19）的解，［k］和yu［k］的值可用相同方法求得.

式（17）定義的控制輸入是非線性的，不能用MILP方法描述，但可用一系列線性不等式來逼近，這些不等式可定義為正多邊形，用于近似描述一個(gè)圓，邊數(shù)越多越精確.參考文獻(xiàn)［9］，本文用Mu個(gè)線性不等式約束定義一個(gè)正Mu邊形：

以上附加約束為線性變量，計(jì)算量增加有限.

2.2 用于爬壁機(jī)器人路徑規(guī)劃的新型代價(jià)函數(shù)

式中，w代表權(quán)值.

為將代價(jià)函數(shù)中的絕對(duì)值形式轉(zhuǎn)化為線性形式，引入輔助連續(xù)變量zx［k］和zy［k］及不等式約束：

將zx［k］最小化，并滿足ux［k］≤zx［k］和ux［k］≥ －zx［k］，即相當(dāng)于最小化｜ux［k］｜（對(duì)｜uy［k］｜同理）.既然機(jī)器人在地面、墻壁和天花板運(yùn)動(dòng)時(shí)能耗不同，可將代價(jià)函數(shù)寫為以下形式：

式中，i＝1，2，…，6為方形房間6個(gè)面的編號(hào)（見圖3）.

City-Climber在地面運(yùn)動(dòng)時(shí)（即i＝1），權(quán)值wx＝wf；在墻壁運(yùn)動(dòng)時(shí)（即i＝2～5），權(quán)值wx＝ww；在天花板運(yùn)動(dòng)時(shí)（即i＝6），權(quán)值wx＝wc.其中wf，ww和wc分別為機(jī)器人在地面、墻壁和天花板運(yùn)動(dòng)的權(quán)值.

圖3 方形房間

當(dāng)City-Climber從一個(gè)面運(yùn)動(dòng)到另一個(gè)面時(shí)，需越過房間的棱邊，是相對(duì)困難和危險(xiǎn)的，本文提出一種適用于爬壁機(jī)器人的新型代價(jià)函數(shù)：

式中，N1，N2，N3分別為地面型棱邊e1i、墻壁型棱邊e2i、天花板型棱邊e3i（i＝1，2，3，4）的條數(shù).（見圖3）；w1，w2，w3分別為地面型棱邊e1i、墻壁型棱邊e2i、天花板型棱邊e3i的權(quán)值.

至此，本文建立了控制輸入優(yōu)化問題的MILP表達(dá)式，即最小化式（23）和式（24），使其滿足式（19）、式（20）和式（22），以及相關(guān)的邊界條件.

3 方形房間描述

方形房間有6個(gè)內(nèi)表面（1個(gè)地面、4個(gè)墻壁和1個(gè)天花板）、12條棱邊（地面與墻壁的相交邊e11～e14，墻壁與墻壁的相交邊e21～e24，墻壁與天花板的相交邊e31～e34）和8個(gè)頂點(diǎn)（見圖3）.本文根據(jù)各表面的關(guān)系，建立圖4所示的連通樹，機(jī)器人可以沿一條線從一個(gè)表面（節(jié)點(diǎn)）運(yùn)動(dòng)到另一個(gè)表面（節(jié)點(diǎn)）.當(dāng)起點(diǎn)和終點(diǎn)分別位于兩個(gè)表面時(shí)，根據(jù)該連通樹，共有15種不同的組合情況：

圖4 連通樹

4 City-Climber路徑規(guī)劃仿真

路徑規(guī)劃問題本質(zhì)上是優(yōu)化問題，本文用AMPL建立數(shù)學(xué)模型，結(jié)合CPLEX求得最優(yōu)解，最后用Matlab軟件繪制最終路徑.本文所有仿真均基于無量綱動(dòng)力學(xué)模型，要得到實(shí)際的物理參數(shù)，需將參數(shù)乘以所除的常數(shù).

仿真中房間尺寸設(shè)置為1.6×1.2×1.0（x×y×z）.本文將City-Climber進(jìn)行壁面切換運(yùn)動(dòng)的權(quán)值設(shè)置為：地面與墻壁間權(quán)值為1，墻壁與墻壁間權(quán)值為2，墻壁與天花板間權(quán)值為3.下面結(jié)合本文提出的路徑規(guī)劃算法，介紹在房間里進(jìn)行路徑規(guī)劃的過程.

1）搜尋最小代價(jià)節(jié)點(diǎn)路徑：在圖4連通樹基礎(chǔ)上運(yùn)行A＊算法（一種靜態(tài)節(jié)點(diǎn)樹中求解最低代價(jià)的算法），由于地面到墻壁1的路徑被障礙物阻斷，找到的兩條最小代價(jià)節(jié)點(diǎn)路徑：地面→墻壁2→墻壁1，地面→墻壁4→墻壁1.

2）全局路徑規(guī)劃：分別將兩組壁面展開成為兩個(gè)平面，建立統(tǒng)一坐標(biāo)，再分別在這兩個(gè)平面上進(jìn)行兩次全局路徑規(guī)劃（圖5），然后在兩條全局路徑上各找到一組關(guān)鍵點(diǎn)：P1和P2，P3和P4.

圖5 全局路徑規(guī)劃

3）局部路徑規(guī)劃：根據(jù)關(guān)鍵點(diǎn)坐標(biāo)，分別進(jìn)行兩組局部路徑規(guī)劃：“地面→墻壁2→墻壁1”（見圖6），“地面→墻壁4→墻壁1”（篇幅所限未給圖）.由于地面上較長的障礙物與墻壁2和墻壁4發(fā)生了接觸，在局部路徑規(guī)劃中，該障礙也被視為墻壁2和墻壁4上的障礙，圖5a、圖5b中可以看到該障礙在墻壁2和墻壁4的位置.在圖5b中，關(guān)鍵點(diǎn)P3位于緩沖地帶內(nèi)部，它作為起始點(diǎn)在墻壁4上進(jìn)行局部路徑規(guī)劃時(shí)，AMPL無法求解，這時(shí)將關(guān)鍵點(diǎn)定義為緩沖線與房間棱邊的交點(diǎn)，而不再是全局路徑與房間棱邊的交點(diǎn).

圖6 “地面→墻壁2→墻壁1”局部路徑規(guī)劃

4）代價(jià)計(jì)算：節(jié)點(diǎn)路徑“地面→墻壁2→墻壁1”的總代價(jià)C＝7.532，節(jié)點(diǎn)路徑“地面→墻壁4→墻壁1”的總代價(jià)C′＝9.982.由于C＜C′，選擇“地面→墻壁2→墻壁1”中的路徑作為最終結(jié)果，圖7為3D環(huán)境中的最終路徑規(guī)劃結(jié)果.

圖7 “地面→墻壁2→墻壁1”3 D路徑規(guī)劃

5 結(jié) 論

本文以方形房間為環(huán)境，進(jìn)行了3D路徑規(guī)劃仿真，結(jié)果表明：基于MILP的路徑規(guī)劃新算法能使City-Climber有效避開所有障礙，在3D房間環(huán)境內(nèi)完成從一個(gè)表面到另一表面的路徑規(guī)劃.

參考文獻(xiàn) （References）

［1］Xiao J，Sadegh A，Elliot M，et al.Design of mobile robots with wall climbing capability［C］／／Proceedings of the 2005 IEEE／ASME International Conference on Advanced Intelligent Mechatronics.Monterey，USA：IEEE，2005：438－443

［2］Xiao J，Sadegh A.City-Climber：a new generation of wallclimbing robots，chapter 18，climbing and walking robots：towards new applications［M］.Vienna，Austria：I-Tech Education and Publishing，2007：383－402

［3］禹建麗，張曉梅，程思雅，等.爬壁機(jī)器人路徑規(guī)劃研究［J］.系統(tǒng)仿真學(xué)報(bào)，2009，21（15）：4748－4751

Yu Jianli，Zhang Xiaomei，Cheng Siya，el al.Approach on path planning for climbing robot［J］.Journal of System Simulation，2009，21（15）：4748－4751（in Chinese）

［5］Fu Zhuang，Zhao Yanzheng，Qian Zhiyuan，et al.Wall-climbing robot path planning for testing cylindrical oilcan weld based on voronoi diagram［C］／／IEEE International Conference on Intelligent Robots and Systems.Beijing：IEEE，2006：2749－2753

［6］岳榮剛，王少萍.一種爬壁機(jī)器人的動(dòng)力學(xué)建模［J］.北京航空航天大學(xué)學(xué)報(bào)，2013，39（5）：640－644

Yue Ronggang，Wang Shaoping.Dynamic modeling for a climbing robot［J］.Journal of Beijing University of Aeronautics and Astronautics，2013，39（5）：640－644（in Chinese）

［7］Kalmár-Nagy T，D'Andrea R，Ganguly P.Near-optimal dynamic trajectory generation and control of an omnidirectional robot［J］.Robot ＆ Autonomous Systems，2004，46（1）：47－64

［8］Ogata K.Discrete-time control systems［M］.Second Edition.New Jersey：Prentice Hall，1995：312－321

［9］Richards A，How J P.Aircraft trajectory planning with collision avoidance using mixed integer linear programming［C］／／Proceeding of the 2002American Control Conference.Anchorage，AK，USA：IEEE，2002：1936－1941