1、.机器人避障问题的解题分析摘要：本文对 2012 年全国大学生数学建模竞赛 D 题机器人避障问题进行了全面分析，对最短路的设计进行了理论分析和证明，建立了机器人避障最短路径的几何模型，对最短时间路径问题通过建立非线性规划模型，有效地解决了转弯半径、圆弧圆心位置和行走时间等问题。关键词：机器人避障；最短路径；Dijkstra 算法；几何模型；非线性规划模型1 引言随着科学技术的进步和计算机技术的发展，机器人的应用越来越广泛，在机器人的应用中如何使机器人在其工作范围内为完成一项特定的任务寻找一条安全高效的行走路径，是人工智能领域的一个重要问题。本文主要针对在一个场景中的各种静态障碍物，研究机器人绕
2、过障碍物到达指定目的地的最短路径问题和最短时间问题。本文以 2012 年“高教社”杯全国大学生数学建模竞赛 D 题“机器人避障问题”为例进行研究。假设机器人的工作范围为 800800 的平面正方形区域（如图 1） ，其中有 12 个不同形状的静态障碍物，障碍物的数学描述（如表 1）：图 1 800800 平面场景图.表 1在原点O(0, 0)点处有一个机器人，它只能在该平面场景范围内活动，机器人不能与障碍物发生碰撞，障碍物外指定一点为机器人要到达的目标点。规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径。机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或
3、多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最近距离为10个单位，否则将发生碰撞，若碰撞发生，则机器人无法完成行走。机器人直线行走的最大速度为 个单位/秒。机器人转弯时，最大转弯速度为50v（ 是转弯半径） 。如果超过该速度，机器人将发生侧翻，无法完21.0e)(v成行走。场景图中有4个目标点O(0, 0)，A(300, 300)，B(100, 700)，C(700, 640)，下面我们将研究机器人从O(0, 0)出发，求OA、OB、OC和OABCO的最短路径，以及机器人从O(0, 0)出发，到达A的最短时间路径问题。2
4、静态避障问题中机器人行走最短路径的分析编号 障碍物名称 左下顶点坐标 其它特性描述1 正方形 (300, 400) 边长2002 圆形 圆心坐标(550, 450)，半径703 平行四边形 (360, 240) 底边长140，左上顶点坐标(400, 330)4 三角形 (280, 100) 上顶点坐标(345, 210)，右下顶点坐标(410, 100)5 正方形 (80, 60) 边长1506 三角形 (60, 300) 上顶点坐标(150, 435)，右下顶点坐标(235, 300)7 长方形 (0, 470) 长220，宽608 平行四边形 (150, 600) 底边长90，左上顶点坐标
5、(180, 680)9 长方形 (370, 680) 长60，宽12010 正方形 (540, 600) 边长13011 正方形 (640, 520) 边长8012 长方形 (500, 140) 长300，宽60.2.1 行走路径的设计在本例中障碍物有 4 种不同形状：矩形、平行四边形、三角形和圆形。考虑到机器人本身的形状和大小，为研究方便起见，将机器人视为一个点。机器人与障碍物之间的距离至少为 10 个单位，因此可以先用包络线画出机器人行走的危险区域（如图 2） ，包络线内是机器人的禁入区。图 2 障碍物包络图对障碍物的一个角点来说，其禁入区的边界应由两条直线和一条圆弧组成，两条直线分别平行
6、于角点的两条边，间距为 10 个单位，圆弧是以障碍物角点为圆心，半径为 10 个单位的四分之一圆弧。可以证明具有圆形限定区域的最短路径由两部分组成，一部分是平面上的自然最短路径（直线段） ，另一部分是限定区域的部分边界（即绳子拉到最紧时的圆弧部分） ，这两部分是相切的，互相连接（如图 3 所示） 。由 A 绕过半圆形障碍物到达 B 点的路径有多条，其中最短路径为 (E、F 为切点)，其他路径与 AB 直线围成的区域都AEB覆盖这一路径与 AB 直线围成的区域，由此证明 1。图 3由此可以确定机器人的行走路径应为线圆结构，那么是否是转弯半径越小，行走路径.就越短呢？为此需要求在已知两个固定点和圆
7、弧圆心坐标的情况下，圆弧半径 r 为何值，才能使机器人的行走路径最短。图 4如图 4，已知两个固定点 ，圆心 ，可以求得两切点坐标12,AabB,Omn，设半径为 ，圆弧 所对的圆心角为 ， 的路径长度为12,CxyDrCDAB,则L22221211 arctnarctnAB ybybxaybxaybxx 将路径函数 对 求导，得Lr12rctnrtL因为 , ，所以 .111,arctn0ybxybx222,arctn0ybxybxL，则函数 为单调递增函数，因此当圆弧半径 逐渐增加时，机器人的行走路0LL径会增大， 逐渐降低时，机器人的行走路径会减小 2，本题规定转弯半径最小为 10 个单
8、r位，所以在路径设定时应将转弯半径设定为最小值 10 个单位。根据以上分析，对于静态障碍物机器人的行走路径应遵循以下三个原则：原则一：机器人的行走路径为线圆结构，由两条切线和一段圆弧组成；原则二：每个路口至多发生一次转弯，并以障碍物顶点为转弯圆弧的中心；原则三：机器人转弯圆弧半径为最小允许半径 10 个单位。2.2 最短路径的选择 从起点到达目标点有多条路径，根据 Dijkstra 算法可以找出从起点到达每一个目标点的最短路径。本文采用带权的有向图表示机器人的行走路径，途中节点为障碍物的角点，边表示障碍物之间的联系，权表示线路的长度（节点之间的直线距离） 。从顶点出发，沿图的边到达另一顶点所经
9、过的路径中，各边上权值之和最小的一条路径就是所求最短路径，.Dijkstra 算法就是按路径的长度递增次序产生最短路径的算法 3。下面以 为例，确定 的最短路径。OBB如图 5 所示，根据障碍物的形状和位置，本文给出了机器人从 O(0, 0)出发避过障碍物到达目标 B 点的 4 条较优路径。图 5画出 的非循环网络图（如图 6）：OBB621306O23060112171225 9921647017978162238B5B3 B6B2 B4B1 B9B7 B7 7B8017111299569237399705159508 329图 6运用 Dijkstra 算法算出 的最短路径，最短路算法如下
10、：OB1、 起点 O 记为 ,终点 B 记为 ；0n2、 从网络的终点 开始，令它的标号 为零，并用方框记录在图 6 中；n.3、 计算结点 的标号 ，设结点 已标号，结点 指向 ，则 的标号可按算式：iBijBijBi求出，其中 是 的标号， 是结点 与 之间的直mn(,)iijljii,lij线距离；4、 重复上述计算，直到求得起点 的标号 为止，此标号 即为最短路的长度；0B005、 确定最短路径，从起点开始，顺网络的箭线前进，若有几条箭线，则选取箭线所指标号最小且满足条件 的结点为最短路径所经过的结点。,1jkljj在图 6 中，最短路径为： .35678OBB应用上述算法可得到从 点
11、出发，分别到达各目标点的最短路径：图 7的最短路径为： （如图 7）OA2OA图 8.的最短路径为： （如图 8）OC14512OCC图 9的最短路径为:OABCO1123134587123BCCO3 最短路径计算模型3.1 单个目标点的最短路径根据前面制定的行走路径原则，起点到目标点无论中间障碍物有多少，最短路径都应该是若干个线圆结构所组成，圆弧中心为障碍物的顶点，半径为机器人转弯最小半径 10 个单位。观察这四条路径，发现所有行走路径都可归结为以下三种类型：类型一 图 10 线圆结构 1如图 10，设 O（ ）为起点，A（ ）为目标点，C 和 D 分别为直线与转弯圆弧1,xy2,xy的切点
12、，障碍物的顶点 （即转弯圆弧的圆心） ，圆的半径为 ， 的长度为 ，3()MrOAa的长度为 ， 的长度为 ，bc,.OAMCM设 的长度为 L，则 ，由图 10 可得以下关系：OACD.22113322()()()()axybc在 中：OMA22arcos()ba在 中：RtCarcsb在 中：tAMDrosc所以：2从而可得：22Lbrcr这个模型运算简洁，只需将起点、目标点和障碍物顶点坐标输入模型，MATLAB 就能很快计算出来 4，计算程序见附录 1。类型二：对于图 11 这种线圆结构，需要做简单的变换，才能求出 的路径长度。 AB图 11 线圆结构 2假设两圆心坐标分别为 和 ,M
13、点为两圆心连线和两圆公切线的交1(,)Oxy(,)xy.点，坐标为 ，那么很容易可以求得:3(,)Mxy123xy这样就可以利用类型一中的方法，先求 A 到 M 的长度，再求 M 到 B 的长度，分两段就可以求解。同理如果有更多的转弯，同样可以按照此种方法分解。 类型三图 12 线圆结构 3如图 12，如果两圆弧的公切线平行于两圆圆心连线，求 的路径长度。AB设各点坐标分别为起点 A ( , ),目标点 B( ),障碍物顶点 ,障碍物1xy2,xy3()Oxy顶点 ),半径为 分别是 的长度，45(,)Oxy,rabcde,O，1A11,CD222,BFE设 的长度为 ,则BLACEB解法如下
14、：由图 12，可以得到以下关系：a= ,224141()()xyb= ,33c = ,2211()()xy,3232d44()()exy.在 中，由余弦定理可得：AO221arcosba在 中,RtC=arccos1rc所以： 132同理：； =arccos ；222arcosbedre223则 ALCDOEFB212+cber2 222 23 3=+(arcosaros)(acosarcos)bbdcr r er 运用 MATLAB 进行计算，MATLAB 计算程序见附录 2.3.2 多个目标点的最短路径 机器人从起点出发，依次经过指定的中间目标点最后到达终点，是多个目标点的最短路径问题。比
15、如 的最短路径的计算。由于机器人的行走路线为线OABCO圆结构，不能折线转弯，因此中间目标点应位于某个半径为 的圆周上，这里我们仍按照r最小允许半径为10个单位，则只需计算出过A 、B、C三点的圆心位置即可，这样就将多目标点的最短路径问题转化成了单目标点的最短路径问题。求过A、B、C三点的圆心位置的问题可通过建立非线性规划模型求得。（1） 过 A 点圆弧的圆心图 13.如图 13，障碍物顶点 ,顶点 , , ，切点1802O20530A1r，过 A 的圆弧圆心 ，最短路径为 ,12,ExyFmnL则 ALBC2 2212218010480OFnmn其 中建立非线性规划模型 222212in14
16、05353010arctn()arctn()ALmnyx s.t. 22 211 122112221 804800300xy EFOmnnyxAyA 切 线 的 长 度切 点 E在 圆 上切 点 F在 圆 上圆 心 与 切 点 连 线 与 切 线 垂 直点 在 圆 上运用 LINGO13 编程计算，计算程序见附录 3.（2） 过 B 点圆弧的圆心图 14如图 14，障碍物顶点 ,顶点 , , ，过 B1506O27068107B1r.的圆弧圆心 ，最短路径为 ,则,OmnBL12Or建立非线性规划模型 2222i150670688arctn()arctn()715BLm 22.100stm运用
17、 LINGO13 编程计算，计算程序见附录 4.（3） 过 C 点圆弧的圆心图 15如图 15，障碍物顶点 ,顶点 , , ，过16703O27067064C10rC 的圆弧圆心 ，圆 与圆 的公切线为 ,切点 ， ，圆,mn2AB1xy2By与圆 的公切线为 ,切点 ， ，最短路径为 ,O1EF3,xy4FxyCL则 CLABr2 22122167031046703Omnmn其 中 2222211706104706ABBnn建立非线性规划模型.22224312min67030706401arctn()arctn()CLmnyyxx 21 233 12244221243670017060.x
18、OymnxCnyxstmA切 点 A在 圆 上切 点 E在 圆 上切 点 B在 圆 上切 点 F在 圆 上点 在 圆 上圆 心 O与 切 点 连 线 与 公 切 线 AB垂 直圆 心 与 切 点 F连 线 与 公 切 线 E21 243 16070y624x3710ynBxE F垂 直的 斜 率 的 斜 率的 斜 率 =y2;y1210;end运行结果： Objective value: 482.2310Variable Value Reduced CostM 290.8854 0.000000N 304.1140 0.000000Y2 294.6634 0.000000Y1 219.4505
19、 0.000000X2 294.1546 0.000000X1 76.73080 0.0000004、过 B 点圆心编写LINGO程序model:min=(m-150)2+(n-600)2)(1/2)+(m-180)2+(n-680)2)(1/2)+10*(acos(-1)-atan(680-n)/(270-m)-atan(600-n)/(150-m);(m-100)2+(n-700)2=100;end.运行结果: Objective value: 219.9993Variable Value Reduced CostM 107.3884 0.000000N 693.2612 0.000000
20、5、过 C 点圆心编写LINGO程序model:min=(m-670)2+(n-730)2-400)(1/2)+(m-720)2+(n-600)2-400)(1/2)+10*(atan(y2-y1)/(x2-x1)-atan(y4-y3)/(x4-x3);(x1-720)2+(y1-600)2=100;(x3-670)2+(y3-730)2=100;(x2-m)2+(y2-n)2=100;(x4-m)2+(y4-n)2=100; (700-m)2+(640-n)2=100;(n-y4)/(m-x4)*(y4-y3)/(x4-x3)=-1;(n-y2)/(m-x2)*(y2-y1)/(x2-x1
21、)=-1;(y2-y1)/(x2-x1)(n-600)/(m-720);(y4-y3)/(x4-x3)700;y4=640;y2670;y1600;end运行结果： Objective value: 130.7340Model Class: NLPVariable Value Reduced CostM 709.6622 0.000000N 637.4229 0.000000.Y2 634.6639 0.000000Y1 600.0000 0.000000X2 700.0503 0.000000X1 710.0000 0.000000Y4 641.3609 0.000000Y3 733.938
22、1 0.000000X4 718.8542 0.000000X3 679.1919 0.0000006、 第一种类型的切点例如求起点 O 绕过障碍物 5 到达 A 点的第一个切点坐标.编写 M 文件，保存为“切点 1.m”:syms x y x1 y1 x2 y2f1=(x2-x)2+(y2-y)2=100);f2=(x2-x1)2+(y2-y1)2-100=(x-x1)2+(y-y1)2);x,y=solve(f1,f2,x,y);在命令窗口输入 x1=0;y1=0;x2=80;y2=210; eval(x,y)ans = 70.5060 213.140689.1772 206.0277根据
23、题意取障碍物左侧的点，得第一个切点坐标为（70.5060，213.1406） 。7、 第二种类型的切点例如求从 A 点到障碍物 7 的切点，运用 MATLAB 求解.编写 M 文件，保存为“切点 2.m”:syms x y x1 y1 x2 y2f1=(x1-x)2+(y1-y)2=100);f2=(y=y1+10*sqrt(1+(y2-y1)/(x2-x1)2)+(y2-y1)/(x2-x1)*(x-x1);x,y=solve(f1,f2,x,y);在命令窗口输入： x1=290.8814;y1=304.1051;x2=220;y2=530; eval(x,y)ans = 300.4227
24、307.0990.公切线的切点的坐标为（300.4227，307.0990） ，同样算法，只需交换一下坐标就可得到另一切点的坐标为（229.5413，532.9939）. 8、最短路径计算结果汇总：的最短路径OA路径 起点坐标 终点坐标 圆心坐标 圆弧半径 直线或圆弧长线段 1d0.00000.000070.5060213.1406224.4995圆弧 l70.5060213.140676.6064219.406680.0000210.000010.0000 9.0510线段 1276.6064219.4066300.0000300.0000237.4868总路程：471.0373 总时间：9
25、6.0177的最短路径OBOB路径 起点坐标 终点坐标 圆心坐标 圆弧半径 直线或圆弧长线段 21d0.00000.000050.1353 301.6396305.7776圆弧 l50.1353301.639651.6795305.547060.0000300.000010.0000 4.2330线段 251.6795305.5470141.6795440.5470162.2498圆弧 l141.6795440.5470147.9621444.7901150.0000435.000010.0000 7.7756线段 23d147.9621 444.7901222.0379460.209975.
26、6637圆弧 l222.0379 460.2099230.0000470.0000220.0000470.000010.0000 13.6557线段 24230.0000470.0000230.0000530.000060.0000圆弧 l230.0000530.0000225.4967538.3538220.0000530.000010.0000. 9.8883线段 25d225.4967 538.3538144.5033591.646296.9537圆弧 l144.5033 591.6462140.6916596.3458150.0000600.000010.0000 6.1474线段 2
27、6140.6916 596.3458100.0000700.0000111.3553总路程：853.7001 总时间：179.0800的最短路径OCOC路径 起点坐标 终点坐标 圆心坐标 圆弧半径 直线或圆弧长线段 31d0.00000.0000232.1149 50.2262237.4868.圆弧 31l232.1149 50.2262232.1693 50.2381230.000060.000010.0000 0.7860线段 2d232.1693 50.2381412.1693 90.2381184.3909圆弧 3l412.1693 90.2381418.3448 94.4897410
28、.0000100.000010.0000 7.6852线段 418.3448 94.4897491.6552 205.5103133.0413圆弧 3l491.6552 205.5103492.0623 206.0822500.0000200.000010.0000 3.3127线段 4d492.0623 206.0822727.9377 513.9178387.8144圆弧 3l727.9377 513.9178730.0000520.0000720.0000520.000.10.0000 6.5381线段 5730.0000520.0000730.0000600.000080.0000圆弧
29、 3l730.0000600.0000727.7178 606.3589720.0000600.000010.0000 6.8916线段 6d727.7178 606.3589700.0000640.000043.5889总路程：1091.5359 总时间：223.3499的最短路径OABCOO路径 起点坐标 终点坐标 圆心坐标 圆弧半径 直线或圆弧长线段 41d0.00000.000070.5060213.1406224.4995圆弧 l70.5060,213.140676.7308 219.450580.0000210.000010.0000 9.1704线段 4276.7308 219.
30、4505294.1546 294.6635230.9169圆弧 l294.1546 294.6635300.4266 307.1081290.8854304.114010.0000 15.4315线段 43d300.4266 307.1081229.5412 532.9941236.7619圆弧 l229.5412 532.9941225.4967 538.3538220.0000530.000010.0000 6.8481线段 4225.498538.3538144.5033 591.646296.9590圆弧 l144.5033 591.6462140.9044 595.8442150.0
31、000600.000010.0000 5.6022线段 45d140.9044 595.844298.2928 689.1054102.5349圆弧 l98.2928 689.1054108.2012 703.2281107.3884693.261210.0000 20.8073.线段 46d108.2012 703.2281270.8128 689.9669163.1514圆弧 l270.8128 689.9669272.0000 689.7980270.0000680.000010.0000 5.2668线段 47272.0000 689.7980368.0000 670.202097.9
32、778圆弧 l368.0000 670.2020370.0000670.0000370.0000680.000010.0000 2.0136线段 48d370.0000670.0000430.0000670.000060.0000圆弧 l430.0000670.0000435.5878 671.7068430.0000680.000010.0000 5.9291线段 49435.5878 671.7068534.4122 738.2932119.1638圆弧 l534.4122 738.2932540.0000740.0000540.0000730.000010.0000 5.9291线段 4
33、10d540.0000740.0000670.0000740.0000130.0000圆弧 l670.0000740.0000679.7899732.0390670.0000730.000010.0000 13.6359线段 41679.7909 732.0343699.8713 635.388698.6230圆弧 l699.8713 635.3886702.7722 630.1753709.6622637.422910.0000 3.3580线段 412d702.7722 630.1753726.8900 607.247633.3854圆弧 l726.8900 607.2476730.000
34、0600.0000720.0000600.000010.0000 5.2082线段 413730.0000600.0000730.0000520.000080.0000圆弧 l730.0000520.0000727.9377 513.9178720.0000520.000010.0000 6.5381线段 41d727.9377 513.9178492.0623 206.0822387.5900圆弧 l492.0623 206.0822491.6552 205.5103500.0000200.000010.0000 3.3127线段 415491.6552 205.5103418.3448 9
35、4.4897126.2100圆弧 l418.3448 94.4897412.1693 90.2381420.0000100.000010.0000 7.6852线段 416d412.1693 90.2381232.1693 50.2381184.3909圆弧 l232.1693 50.2381232.1149 50.2262230.000060.000010.0000 0.7860.线段 417d232.1149 50.22620.00000.0000237.4868总路程：2727.1735 总时间：568.93919、最短时间路径的计算编写 LINGO 程序，应用 LINGO13 求解mo
36、del:min=(x12+y12)(1/2)+(300-x2)2+(300-y2)2)(1/2)/5+r*(atan(y1/x1)-atan(300-y2)/(300-x2)/(5/(1+exp(10-0.1*(r2);y1/x1=210/80;(300-y2)/(300-x2)=10+(m-80)2+(n-210)2;(m-x1)2+(n-y1)2=r2;(m-x2)2+(n-y2)2=r2;(m2+n2-x12-y12)1/2=10;(m-x1)2+(n-y1)2=(m-x2)2+(n-y2)2;(y1/x1)*(n-y1)/(m-x1)=-1;(300-y2)/(300-x2)*(n-y2)/(m-x2)=-1;End运行结果：Objective value: 94.34054 Variable Value Reduced CostX1 69.80490 0.000000Y1 212.7391 0.000000X2 76.98770 0.000000Y2 220.1178 0.000000R 11.71858 0.000000M 80.93940 0.000000N 209.0856 0.000000
