机器人避障问题摘要 本文研究了机器人避障最短路径和最短时间路径的问题。

主要研究了在一个区域中存在12个不同形状障碍物，由出发点到达目标点以及由出发点经过途中的若干目标点到达最终目标点的多种情形，寻找出一条恰当的从给出发点到目标点的运动路径使机器人在运动中能安全、无碰撞的绕过障碍物而使用的路径和时间最短。

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

所以只要给定的出发点到目标点存在至少一个障碍物，我们都可以认为最短路径一定是由线和圆弧所组成，因此我们建立了切线圆结构，这样无论路径多么复杂，我们都可以将路径划分为若干个这种切线圆结构来求解。

在没有危险碰撞的情况下，圆弧的半径越小，路径应该越短，因此我们尽量选择最小的圆弧半径以达到最优。

对于途中经过节点的再到达目标点的状况，我们采用了两种方案，一种是在拐点和节点都采用最小转弯半径的形式，另一种是适当扩大拐点处的转弯半径，使得机器人能够沿直线通过途中的目标点。

然后建立了最优化模型对两种方案分别进行求解，把可能路径的最短路径采用穷举法列举出来，用lingo 工具箱求解得出了机器人从O(0,0)出发，O→A、O→B、O→C 和O→A→B→C→O 的最短路径；利用matlab 中的fminbnd 函数求极值的方法求出了机器人从O(0,0)出发，到达A 的最短时间路径。

本文提出一种最短切线圆路径的规划方法，其涉及的理论并不高深，只是应用了几何知识和计算机程序、数学工具计算，计算简易，便于实现，能搞提高运行效率。

问题一O→A 最短路径为：OA L =471.0372O→B 最短路径为：=1OB L 853.8014O→C 最短路径为：4OC L =1054.0O→A→B→C→O 最短路径为：问题二机器人从O(0,0)出发，到达A 的最短时间路径：最短时间是94.5649，圆弧的半径是11.5035,路径长4078.472=OA L关键词最短路径；避障路径；最优化模型；解析几何；数学工具一、问题重述图1是一个800×800的平面场景图，在原点O(0,0)点处有一个机器人，它只能在该平面场景范围内活动。

图中有12个不同形状的区域是机器人不能与之发生碰撞的障碍障碍物的距离至少超过10个单位）。

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

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

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

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

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

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

请建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型。

对场景图中4个点O(0,0)，A(300,300)，B(100,700)，C(700,640)，具体计算：(1)机器人从O(0,0)出发，O→A、O→B、O→C和O→A→B→C→O的最短路径。

(2)机器人从O(0,0)出发，到达A 的最短时间路径。

注：要给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器人行走的总距离和总时间。

图1800×800平面场景图二、 问题分析1、要求求定点O(0,0)按照一定的行走规则绕过障碍物到达目标点的最短路径，我们先可以包络线画出机器人行走的危险区域，在没有危险碰撞的情况下，圆弧的半径越小，路径应该越短。

这样的话，拐角处就是一个以方形或三角形障碍物的顶点为圆心半径为10个单位的圆弧，如果是圆形障碍物就应该是以障碍物的圆心为圆心、障碍物的半径长加上10为半径的圆弧。

2、若经过中间的若干点按照一定的规则绕过障碍物到达目标点，这使我们考虑就不仅仅是经过障碍物拐点的问题，也应该考虑经过路径中的目标点处转弯的问题，这时简单的线圆结构就不能解决这种问题，我们在拐点及途中目标点处都采用最小转弯半径的形式，也可以适当的变换拐点处的拐弯半径，使机器人能够沿直线通过途中的目标点，然后建立优化模型对这两种方案分别进行优化，最终求得最短路径。

3、这样机器人行走的路径就是由切线段、内公切线、外公切线以及圆弧组成的这里称之为切线圆路径。

三、 模型假设1、假设障碍物只包含长方形、正方形、三角形、圆形。

2、假设机器人能够抽象成点来处理。

3、路径不考虑走平面场地的边界。

五、符号说明在计算机程序输入的原始数据中:(T,V,W,r)表示T 是起点坐标,V 是圆弧的圆心坐标,W 是目标节点坐标,r 是圆弧半径.为便于叙述和计算，根据已知条件我们给12个障碍物中的11个方形和三角形顶点用字母ij T 或ij S 表示，其中T 或S 表示障碍物，ij T 中i 表示第i 号障碍物，ij S 中i 表示第9+i 号障碍物，j 表示从左下角开始按顺时针数起第几个顶点。

如下表一所示：表一点字母表示（如：6162747381O T T T T T B →→→→→→表示从点O 经过6号三角形左顶角为圆心的圆弧到6号三角形上顶角为圆心的圆弧到7号方形右下角为圆心的圆弧到7号方形右上角为圆心的圆弧到8号菱形左下角为圆心的圆弧到达点B 。

标出的经过顶点六、模型的建立与求解由于规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径，机器人不能折线转弯。

据此可以这样认为，起点到目标点无论中间障碍物有多少，最短路径都应该是若干个切线圆结构所组成。

易知，求两点之间的最短路径中的转弯半径越小路径就越短，我们应该按照最小的转弯半径来算才能达到最优。

根据要求机器人行走线路与障碍物间的最近距离为10个单位，因此在方形及三角形顶点转弯的地方圆弧半径10r ≥，我们尽量取以顶点为圆心半径为r=10个单位的圆弧，如果是圆形障碍物就应该是以障碍物的圆心为圆心、障碍物的半径长加上10为半径的圆弧，只有在必要的时候对半径作适当的加大调整。

6.1模型I 求从起点O(0,0)到目标点A(300,300)的最短路径。

经过观察很显然从O 到A 有两条选择的路径（其它路径需要经过过多的障碍物路径显然比较长不必考虑）如图6.11所示，一条是从5号障碍物的左上角走OPQA ，一条是从右下角走OHJA 。

他们的路径结构图是类似的如图6.12所示。

图6.11图6.12设),11y x O （为起点，),(22y x A 为目标点，33(,)P x y 和44(,)Q x y 分别为机器人经过拐点分别于隔离危险线拐角小圆弧的切点，圆心为55(,)T x y ,圆的半径为r ，OA 的长度为a ，OT 的长度为b ，AT 的长度为c ，角度OTA ∠=,,,,OTA OTP ATQ PTQ αβγθ∠=∠=∠=∠=.求的长度，设为L .解法如下：如上图可得有以下关系：OTA ∆在中：OA =在Rt OTP ∆：在Rt OTP ∆中：所以：从而可得路径长： Lr θ=（1.1）即模型I 为：min L r θ=， =arccos rb β，222arccos()2b c a bcα+-=由已知条件知(00),(300300),(80,210)O A T ，，即 ,01=x ,01=y .10210,80,300,3005522=====r y x y x ，，利用计算机计算lingo 算法(T 是起点坐标V 是圆弧圆心坐标W 是过程目标点坐标r 圆弧是半径)：functionresult=zongchang(T,V,W,r)TV=sqrt((T(1)-V(1))^2+(T(2)-V(2))^2);TW=sqrt((T(1)-W(1))^2+(T(2)-W(2))^2);VW=sqrt((V(1)-W(1))^2+(V(2)-W(2))^2);alpha1=acos((TV^2+VW^2-TW^2)/(2*TV*VW));alpha2=acos(r/TV);alpha3=acos(r/VW);alpha4=2*pi-alpha1-alpha2-alpha3;TS1=sqrt(TV^2-r^2);S2W=sqrt(VW^2-r^2);S1S2hu=r*alpha4;result=TS1+S1S2hu+S2W;结果算得的长度OA L =471.0372又由22222515131312225353()()10()()()()x x y y x x y y x x y y r ⎧-+--=-+-⎨-+-=⎩（1） 和22222525254532225454(-)()10()()()()x x y y x x y y x x y y r ⎧+--=-+-⎨-+-=⎩（2） 计算得点52T 为圆心的圆弧两切点的坐标为5252(70.506,213.1406),(76.6064,219.4066)P Q 。

同样，通过计算可得从路径从右下角方向（圆心T 的坐标改为5号障碍物右下角顶点坐标54T （230,60））走时OHJA 长度OAL '=498.4259. 很显然机器人从5号障碍物左上角52T 走的路径OPQA 小于机器人5号障碍物右下角54T 走的路径OHJA 要短。

因此从起点O(0,0)到目标点A(300,300)的最短路径是，长度OA L =471.0372，圆弧PQ 的切点坐标为(70.506,213.1406),(76.6064,219.4066)P Q 。

同时可以验证6号三角形障碍物右下角顶点到切线AQ 距离大于10，所以过52T 的OPQA 路径是安全的也是最短的。

下面考虑问题二：从O 到A 的最短时间路径由式（1.1）可求得时间路径的目标函数为：2100.15min 551r t r e θ-=++÷+，1080≥≥t利用matlab 中的fminbnd 函数求极值f1='(x*(2*pi-2.3231-acos(x/224.7221)-acos(x/237.6973))/(5/(1+exp(10-0.1*x^2))))+sqrt(224.7221^2-x^2)/5+sqrt(237.6973^2-x^2)/5';[x_min,f_min,flag]=fminbnd(f1,10,50)得结果：x_min=11.5035，f_min=94.5649，flag=1即最短时间是94.5649，52T 圆弧的半径是11.5035。

利用式（1）（2）算得两切点坐标)8453.2201648.76(),5395.2130546.69(5252，，Q P ,路径长4078.472=OA L .6.2模型II 求从起点O(0,0)到目标点B(100,700)的最短路径。