机器人避障问题的解题分析(建模集训)

机器人避障问题的解题分析摘要：本文对2012年全国大学生数学建模竞赛D题机器人避障问题进行了全面分析，对最短路的设计进行了理论分析和证明，建立了机器人避障最短路径的几何模型，对最短时间路径问题通过建立非线性规划模型，有效地解决了转弯半径、圆弧圆心位置和行走时间等问题。

关键词：机器人避障；最短路径；Dijkstra算法；几何模型；非线性规划模型1 引言随着科学技术的进步和计算机技术的发展，机器人的应用越来越广泛，在机器人的应用中如何使机器人在其工作范围内为完成一项特定的任务寻找一条安全高效的行走路径，是人工智能领域的一个重要问题。

本文主要针对在一个场景中的各种静态障碍物，研究机器人绕过障碍物到达指定目的地的最短路径问题和最短时间问题。

本文以2012年“高教社”杯全国大学生数学建模竞赛D题“机器人避障问题”为例进行研究。

假设机器人的工作范围为800×800的平面正方形区域（如图1），其中有12个不同形状的静态障碍物，障碍物的数学描述（如表1）：图1 800×800平面场景图表1在原点O(0, 0)点处有一个机器人，它只能在该平面场景范围内活动，机器人不能与障碍物发生碰撞，障碍物外指定一点为机器人要到达的目标点。

规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径。

机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。

为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最近距离为10个单位，否则将发生碰撞，若碰撞发生，则机器人无法完成行走。

机器人直线行走的最大速度为50=v 个单位/秒。

机器人转弯时，最大转弯速度为21.0100e1)(ρρ-+==v v v （ρ是转弯半径）。

如果超过该速度，机器人将发生侧翻，无法完成行走。

场景图中有4个目标点O(0, 0)，A(300, 300)，B(100, 700)，C(700, 640)，下面我们将研究机器人从O(0, 0)出发，求O→A、O→B、O→C和O→A→B→C→O的最短路径，以及机器人从O(0, 0)出发，到达A 的最短时间路径问题。

2 静态避障问题中机器人行走最短路径的分析 2.1 行走路径的设计在本例中障碍物有4种不同形状：矩形、平行四边形、三角形和圆形。

考虑到机器人本身的形状和大小，为研究方便起见，将机器人视为一个点。

机器人与障碍物之间的距离至少为10个单位，因此可以先用包络线画出机器人行走的危险区域（如图2），包络线内是机器人的禁入区。

图2 障碍物包络图对障碍物的一个角点来说，其禁入区的边界应由两条直线和一条圆弧组成，两条直线分别平行于角点的两条边，间距为10个单位，圆弧是以障碍物角点为圆心，半径为10个单位的四分之一圆弧。

可以证明具有圆形限定区域的最短路径由两部分组成，一部分是平面上的自然最短路径（直线段），另一部分是限定区域的部分边界（即绳子拉到最紧时的圆弧部分），这两部分是相切的，互相连接（如图3所示）。

由A绕过半圆形障碍物到达B点的路径有多条，其中最短路径为AEFB(E、F为切点)，其他路径与AB直线围成的区域都覆盖这一路径与AB直线围成的区域，由此证明[1]。

图3由此可以确定机器人的行走路径应为线圆结构，那么是否是转弯半径越小，行走路径就越短呢？为此需要求在已知两个固定点和圆弧圆心坐标的情况下，圆弧半径r为何值，才能使机器人的行走路径最短。

图4如图4，已知两个固定点()()1122,,,A a b B a b ，圆心(),O m n ，可以求得两切点坐标()()1122,,,C x y D x y ，设半径为r ，圆弧CD 所对的圆心角为ω，A B →的路径长度为L ,则()()()()22221122111122221122arctan arctan L AC BD r y b y b x a y b x a y b r x a x a ω=++⎛⎫--=-+--+-⋅- ⎪--⎝⎭ 将路径函数L 对r 求导，得11221122arctanarctan y b y bL x a x a --'=--- 因为11111111,,arctan0y b x a y b x a ->>>-,22222222,,arctan 0y bx a y b x a -<><-，所以0L '>.0L '>，则函数L 为单调递增函数，因此当圆弧半径r 逐渐增加时，机器人的行走路径会增大，r 逐渐降低时，机器人的行走路径会减小[2]，本题规定转弯半径最小为10个单位，所以在路径设定时应将转弯半径设定为最小值10个单位。

根据以上分析，对于静态障碍物机器人的行走路径应遵循以下三个原则： 原则一：机器人的行走路径为线圆结构，由两条切线和一段圆弧组成； 原则二：每个路口至多发生一次转弯，并以障碍物顶点为转弯圆弧的中心； 原则三：机器人转弯圆弧半径为最小允许半径10个单位。

2.2 最短路径的选择从起点到达目标点有多条路径，根据Dijkstra 算法可以找出从起点到达每一个目标点的最短路径。

本文采用带权的有向图表示机器人的行走路径，途中节点为障碍物的角点，边表示障碍物之间的联系，权表示线路的长度（节点之间的直线距离）。

从顶点出发，沿图的边到达另一顶点所经过的路径中，各边上权值之和最小的一条路径就是所求最短路径，Dijkstra 算法就是按路径的长度递增次序产生最短路径的算法[3]。

下面以 O B →为例，确定O B →的最短路径。

如图5所示，根据障碍物的形状和位置，本文给出了机器人从O(0, 0)出发避过障碍物到达目标B 点的4条较优路径。

图5画出O B →的非循环网络图（如图6）：图6运用Dijkstra 算法算出O B →的最短路径，最短路算法如下： 1、 起点O 记为0B ,终点B 记为n B ；2、 从网络的终点n B 开始，令它的标号n λ为零，并用方框记录在图6中；3、 计算结点i B 的标号i λ，设结点j B 已标号，结点i B 指向j B ，则i B 的标号可按算式：B 621306O 230 60 11217122599 21647017978162238B5 B3 B6 B2 B4 B1B9B7B8 0171 11299 569 237 399 705 159508 329{}min (,)i i jl i j λλ=+求出，其中i λ是i B 的标号，(),l i j 是结点i B 与j B 之间的直线距离；4、 重复上述计算，直到求得起点0B 的标号0λ为止，此标号0λ即为最短路的长度；5、 确定最短路径，从起点开始，顺网络的箭线前进，若有几条箭线，则选取箭线所指标号最小且满足条件(),,1j k l j k k j λλ=+≥+的结点为最短路径所经过的结点。

在图6中，最短路径为：35678O B B B B B B →→→→→→. 应用上述算法可得到从O 点出发，分别到达各目标点的最短路径：图7O A →的最短路径为： 2O A A →→（如图7）图8O C →的最短路径为：1451112O C C C C C C →→→→→→（如图8）图9O A B C O →→→→的最短路径为:1123134587123O A A B B B B C C C C C C C O O O O →→→→→→→→→→→→→→→→→3 最短路径计算模型3.1 单个目标点的最短路径根据前面制定的行走路径原则，起点到目标点无论中间障碍物有多少，最短路径都应该是若干个线圆结构所组成，圆弧中心为障碍物的顶点，半径为机器人转弯最小半径10个单位。

观察这四条路径，发现所有行走路径都可归结为以下三种类型： 类型一图10 线圆结构1如图10，设O （11,x y ）为起点，A （22,x y ）为目标点，C 和D 分别为直线与转弯圆弧的切点，障碍物的顶点33(,)M x y （即转弯圆弧的圆心），圆的半径为r ，OA 的长度为a ，OM 的长度为b ，AM 的长度为c ，,,,.OMA OMC AMD CMD ϕαβθ∠=∠=∠=∠=设O A →的长度为L ，则L OC AD CD =++，由图10可得以下关系：222121223131223232()()(()()()()a x x y y b x x y y c x x y y ⎧=-+-⎪⎪=-+-⎨⎪=-+-⎪⎩在OMA ∆中：222arccos()2b c a bcϕ+-= 在Rt OMC ∆中：arccos r b α= 在Rt AMD ∆中：arccos r cβ= 所以：2θπϕαβ=--- 从而可得：2222L b r c r r θ=-+-+这个模型运算简洁，只需将起点、目标点和障碍物顶点坐标输入模型，MATLAB 就能很快计算出来[4]，计算程序见附录1。

类型二：对于图11这种线圆结构，需要做简单的变换，才能求出A B →的路径长度。

图11 线圆结构2假设两圆心坐标分别为11(,)O x y 和22(,)O x y ',M 点为两圆心连线和两圆公切线的交点，坐标为33(,)M x y ，那么很容易可以求得:12312322x x x y y y +⎧=⎪⎪⎨+⎪=⎪⎩ 这样就可以利用类型一中的方法，先求A 到M 的长度，再求M 到B 的长度，分两段就可以求解。

同理如果有更多的转弯，同样可以按照此种方法分解。

类型三图12 线圆结构3如图12，如果两圆弧的公切线平行于两圆圆心连线，求A B →的路径长度。

设各点坐标分别为起点A (1x ,1y ),目标点B(22,x y ),障碍物顶点33(,)O x y ,障碍物顶点45'(,)O x y ),半径为,,,,,r a b c d e 分别是',',,,'AO OO AO BO BO 的长度，1'AOO α∠=，11,,AOC COD βθ∠=∠=222',,BOO BOF EOF αβθ∠=∠=∠=设A B →的长度为L ,则'L AC CD OO EF FB =++++ 解法如下：由图12，可以得到以下关系： 224141()()x x y y -+-224343()()x x y y -+-223131()()x x y y -+-223232()()d x x y y =-+-224242()()e x x y y =-+-在'AOO ∆中，由余弦定理可得：2221arccos 2b c a bcα+-=在Rt AOC ∆中,1β=arccos rc所以： 11132πθαβ=--同理：2222arccos 2b e d be α+-=；2β=arccos r e ；22232πθαβ=--则'L AC CD OO EF FB =++++222212+++c r r b r e r θθ=-+-222222222233=+(arccos arccos )(arccos arccos )2222c b a r b e d rc r r b r e r bc c be eππ+-+----++--+- 运用MATLAB 进行计算，MATLAB 计算程序见附录2. 3.2 多个目标点的最短路径机器人从起点出发，依次经过指定的中间目标点最后到达终点，是多个目标点的最短路径问题。