基于混合整数线性规划的爬壁机器人路径规划

岳荣刚

（中国空间技术研究院 钱学森空间技术实验室，北京 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