机器人避障优化模型讲解

机器人避障优化模型摘要“机器人避障问题”是在一个规定的区域范围内，有12个位置各异、形状不同的障碍物分布，求机器人从出发点到达目标点以及由出发点经过途中的若干目标点到达最终目标点的避障最短路径及其最短时间，其中必须考虑如圆与切线的关系等问题。

基于优化模型，对于题目实际情况进行研究和分析，对两个问题都用合适的数学思想做出了相应的解答和处理，以此建立符合题意的数学模型。

问题一，要求建立机器人从原点出发到达以区域中另一点为终点的最短路径模型。

机器人的避障路径规划主要包括环境建模、路径搜索、路径平滑等环节，针对本题的具体情况，首先对图形进行分析，并用AutoCAD 软件进行环境建模，使其在障碍物外围延伸10个单位，然后考虑了障碍物对路径安全的影响再通过蚁群算法来求它的的最短路径，由于此时最短路径中存在转弯路径，需要用人工势场法进行路径平滑处理，从而使它的最短路径在蚁群算法算出的结果情况下，可以进一步缩短其路径，从而存在机器人以区域中一点到达另一点使其避障的路径达到最短，在最终求解时，通过matlab 软件求其最优解。

问题二，仿照问题一机器人避障路径规划的基本环节所建立的一般模型，再根据题二所提出的具体问题，建立机器人从O （0，0）出发，使达到A 的最短时间路径模型。

其中已知最大速度为50=v 个单位/秒，机器人转弯时，最大转弯速度为21.0100e 1)(ρρ-+==v v v ，其中ρ是转弯半径，并有ν为增函数。

且有0νν<恒成立，则可知行走路径应尽量减少走圆弧，且可时间由走两段直线加圆弧的时间之和。

关键词： 最短路径 蚁群算法 人工势场法 机器人避障一 、问题重述图1是一个800×800的平面场景图，在原点O(0, 0)点处有一个机器人，它只能在该平面场景范围内活动。

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

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

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

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

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

机器人转弯时，最大转弯速度为21.0100e 1)(ρρ-+==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 的最短时间路径。

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

O(0, 0)，A(300, 300)，B(100, 700)，C(700, 640)图1 800×800平面场景图二、问题分析2.1问题一要求机器人从原点按照一定行走规则绕过障碍物到达目标点的最短路径，且各有4问（即求O→A、O→B、O→C和O→A→B→C→O的最短距离），并要机器人不能发生碰撞，如碰撞则会导致机器人侧翻。

则考虑用AutoCAD软件进行环境建模，使其在障碍物外围延伸10个单位，然后再考虑障碍物对路径安全的影响再通过蚁群算法来求它的最短折线路径，再通过人工势场法对其路径进行平滑处理，从而得到线圆结构模型，然后列出R到每个目标点的可能路径的最短路径，再利用相关解析几何知识对不同路线长进行求解然后比较其大小便可得出R到目标点的最短路径。

2.2问题二要求求出O→A的最短时间，但是由于问题二与问题一有较强的相关性，可以借用模型一的相关数据和方法，而问题二不仅要求算出距离尽量可以最短，而且要速度相应的改变，使之达到最快，从而使时间达到最短，而不能如问题一一样ρ与的关系。

其中还要考虑拐弯处的半径范围，仅仅考虑路径最短的条件，还要考虑ν使问题尽量达到细致化。

最后机器人能够沿直线通过途中的目标点，然后建立优化模型对这两种方案分别进行优化，最终求得最短路径。

三、模型假设3.1 假设机器人能够抽象成点来处理。

3.2 假设问题二中机器人在直线上都是以最大速度50=V 个单位/秒行走。

3.3 机器人行驶路径只有直线或弧线。

四、符号说明π=∠+∠0021，…….，π=∠+∠i i 21五、模型的建立5.1 蚁群算法的路径寻优蚁群算法作为一种仿生算法在在机器人路径规划等优化问题得到了广泛的应用，蚁群算法中的几何建模相比栅格法建模，更为紧凑，在简单环境下或者局部区域内计算量小而且可以获得较高的精度[2]。

假设机器人通过感知系统探测到环境的几何信息，如下4图所示，实线表示障碍物的边界,虚线表示虚拟障碍物。

以起点O 为原点，建立直角坐标系，终点A 是(300,300)。

为了方便对移动机器人在规划过程中的处理，避免与障碍物发生碰撞，把障碍物边界至少向外扩展达10个单位，且机器人可看作一点，如下4图所示。

扩展后的障碍物顶点为P1 至P39，从起点O 到终点A 、B 、C 、O 的路径便可以由这些顶点组成，例如其中一条可达路径为: O → P3→ P14 → P15B P P →→→1716。

整个环境就被提取为三种几何信息: 虚拟障碍物顶点、实际障碍物顶点以及实际障碍物的边界及延伸虚拟边界相交出的折线、。

各自用途: 虚拟障碍物顶点作为蚁群算法的搜索结点、实际障碍物顶点与实际障碍物的边界、虚拟边界相交出的折线圆弧问题。

为了更清晰地阐述蚁群算法的基本原理[1]，以图论的思想描述路径规划问题：给定图G=<V,E>，其中，V 为节点集合；E 为相邻2 个节点连接组成的边的集合，并且知道边的权值。

路径规划问题就是在赋权图G 的2 个节点之间，找出一条权值最小的路径。

对于本文研究的机器人路径规划问题，它的赋权图G 为有向图蚁群算法的流程如图所示针对本题实际情况，首先提取环境的平面几何信息，建立了简单有效地搜索模型。

可通过引入蚁群算法（即引导“蚁群”有目的的进行搜索; 改进信息素更新策略，利用一种奖惩机制以增强蚁群对尽可能好的解的识别能力，如上图所示）。

并考虑障碍物对路径的影响，通过利用蚁群算法求得的路径是由结点序列组成的，是一条折线，其一阶导数在结点处均不连续的特点，运用人工势场法[3]对全局最优路径的结点进行平滑。

充分考虑了障碍物对路径安全的影响，广泛应用于机器人的避障和平滑轨迹控制。

证明了改进方法可以有效地找出最优可行路径。

5.2改进后的最短路径为下4图（图一、图二、图三、图四）5.2.1环境模型一机器人路径规划本质是指在规划空间内，依据环境信息，在某些评价标准下，找出从出发点到目标点最优的运动路线。

假设规划空间是一个有限区域的二维平面，区域里存在有限个障碍物。

在该区域里建立直角坐标系，目的就是为机器人寻找从起点到终点的一系列点的集合，这个点集所组成的路径不仅要安全避障，且还要最短。

目标函数可表示为(1)式：∑=---+-=Ni i i i i y y x x L 22121)()( ()1),(i i y x 为点的坐标，N 为点的个数由题意得，O 点到A 点有以下两种路径：1）、A P O →→2目标函数：∑=---+-=N i i i i i y y x x L 22121)()(此时各点坐标分别是：)0,0(O 、)220,70(2P 、)300,300(A ；此时N=2把其带入目标函数得：89.473=L2）、A P O →→4目标函数：∑=---+-=N i i i i i y y x x L 22121)()(此时各点坐标分别是：)0,0(O 、)50,240(4P 、)300,300(A ；此时N=2把其带入目标函数得：48.532=L经以上最终路径长比较得：最短路径：A P O →→25.2.2环境模型二从B O →同样根据环境模型一中的求解原理，经计算得：最短路径：B P P P P P O →→→→→→17161514135.2.3环境模型三从C O →同样根据环境模型一中的求解原理，最终经计算得：最短路径：C P P P P P O →→→→→→17209625.2.4环境模型四从O C B A O →→→→同样根据环境模型一中的求解原理，最终经计算得：最短路径：OP P P P P C P P P P P B P P A P O →→→→→→→→→→→→→→→→→2692017181637363533313六、模型求解6.1问题一利用蚁群算法求得的路径一般是连续的曲线，但是曲线中存在切线方向的突变，即曲线本身是连续的，但是一阶导数并不连续。

路径平滑的主要目的是：应用数学的方法，去掉凹凸点，使得搜索出的最优路径是连续的，并且它的一阶导数也连续。

如下图 所示，由于机器和驱动结构限制，机器人在从O 运动到E 后无法再前进，因此需要在 DE 与EF 之间通过人工势场法[3]进行平滑处理。

6.1.1模型一“A O →”如上图，设)0,0(O 为起点，A )300.300(为目标点，机器人经过拐点分别于隔离危险线拐角小圆弧的切点坐标),(1010y x 、),(2020y x ；).(1111y x 、),(2121y x ；…….；),(11i i y x 、),(22i i y x 。

各线段相交点的坐标：),(4040y x 、),(4141y x ，……，),(44i i y x 半径为r ，AO 的长度为a ，OE 的长度为b ，AE 的长度c 求A F DˆˆO 的长度，设为L. 解法如下：如上图可得有以下关系： 2424242422)300()300()()(300300y x c y x b a -+-=+=+=在AEO ∆中： 12)2arccos(1222∠-=∠-+=∠πbac b a 21tan},min{21∠•=OE AE r )10(≥r 从而可得： 038.4672)300()300(22222121min =∠+---+-=r y x y x L6.1.2模型二“B O →”如上图，设)0.0(O 为起点，B )700,100(为目标点。

利用A O →中求解原理可知B O →的总距离为L ,首先对B O →距离进行分段求解：1E E O →→、21E E E →→、321E E E →→ 、432E E E →→、B E E →→43距离分别是：L1、L2、L3、L4、L5；22tan},min{21111-+-∠•=i i i i E E OE Ri （注：40≤≤i ）)10(≥Ri 从而可得：min L = L1+L2+L3+L4+L5-(131.849)4332211=+++E E E E E E EE6.1.3模型三“C O →”如上图，设)0.0(O 为起点，C )640,700(为目标点。