基于混合整數線性規劃的爬壁機器人路徑規劃

岳榮剛

（中國空間技術研究院 錢學森空間技術實驗室，北京 100094）

王少萍

（北京航空航天大學 自動化科學與電氣工程學院，北京 100191）

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

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

本文在City-Climber動力學模型基礎上，研究基于MILP的爬壁機器人3D路徑規劃問題，并針對City-Climber的特點提出一種代價函數.

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

在文獻［6］中，建立了City-Climber機器人的動力學模型：

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

當機器人在地面運動時，i＝1，有

當機器人在墻壁運動時，i＝2，有

當機器人在天花板運動時，i＝3，有

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

City-Climber方位角θ的存在使得數學模型存在耦合和非線性，本文參考文獻［7］中的方法，通過設置一系列容許控制方程，對City-Climber的3個數學模型分別進行解耦和線性化.

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

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

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

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

式中

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

式中，半徑表達式為

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

圖1 City-Climber在地面運動的映射P1（0）

圖2 容許控制約束的實心錐體

City-Climber地面運動的動力學方程簡化為

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

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

與1.1節相同的方法，對City-Climber在墻壁運動的模型進行解耦和線性化，得到與式（12）類似的動力學方程，約束條件為

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

與1.1節相同的方法，對City-Climber在天花板運動的模型進行解耦和線性化，得到與式（12）類似的動力學方程，約束條件為

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

2 MILP的數學描述

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

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

City-Climber平動的動力學微分方程為

滿足：

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

式（16）可以寫為狀態空間的形式：

離散化控制輸入，規定在兩個時間點之間，輸入量為常數，可得一系列線性離散時間控制方程.本文用下標u表示控制輸入有關的變量，Nu為對控制輸入進行離散化的步數，tu［k］為第k步的時間，Tu［k］＞0為第k步到k＋1步的時間間隔，k∈｛0，1，…，Nu－1｝.離散時間控制方程表述為

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

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

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

2.2 用于爬壁機器人路徑規劃的新型代價函數

式中，w代表權值.

為將代價函數中的絕對值形式轉化為線性形式，引入輔助連續變量zx［k］和zy［k］及不等式約束：

將zx［k］最小化，并滿足ux［k］≤zx［k］和ux［k］≥ －zx［k］，即相當于最小化｜ux［k］｜（對｜uy［k］｜同理）.既然機器人在地面、墻壁和天花板運動時能耗不同，可將代價函數寫為以下形式：

式中，i＝1，2，…，6為方形房間6個面的編號（見圖3）.

City-Climber在地面運動時（即i＝1），權值wx＝wf；在墻壁運動時（即i＝2～5），權值wx＝ww；在天花板運動時（即i＝6），權值wx＝wc.其中wf，ww和wc分別為機器人在地面、墻壁和天花板運動的權值.

圖3 方形房間

當City-Climber從一個面運動到另一個面時，需越過房間的棱邊，是相對困難和危險的，本文提出一種適用于爬壁機器人的新型代價函數：

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

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

3 方形房間描述

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

圖4 連通樹

4 City-Climber路徑規劃仿真

路徑規劃問題本質上是優化問題，本文用AMPL建立數學模型，結合CPLEX求得最優解，最后用Matlab軟件繪制最終路徑.本文所有仿真均基于無量綱動力學模型，要得到實際的物理參數，需將參數乘以所除的常數.

仿真中房間尺寸設置為1.6×1.2×1.0（x×y×z）.本文將City-Climber進行壁面切換運動的權值設置為：地面與墻壁間權值為1，墻壁與墻壁間權值為2，墻壁與天花板間權值為3.下面結合本文提出的路徑規劃算法，介紹在房間里進行路徑規劃的過程.

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

2）全局路徑規劃：分別將兩組壁面展開成為兩個平面，建立統一坐標，再分別在這兩個平面上進行兩次全局路徑規劃（圖5），然后在兩條全局路徑上各找到一組關鍵點：P1和P2，P3和P4.

圖5 全局路徑規劃

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

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

4）代價計算：節點路徑“地面→墻壁2→墻壁1”的總代價C＝7.532，節點路徑“地面→墻壁4→墻壁1”的總代價C′＝9.982.由于C＜C′，選擇“地面→墻壁2→墻壁1”中的路徑作為最終結果，圖7為3D環境中的最終路徑規劃結果.

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

5 結 論

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

參考文獻 （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］.系統仿真學報，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］.北京航空航天大學學報，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