机器人避障问题
摘要
本文研究了机器人避障最短路径和最短时间路径的问题。主要研究了在一个区域中存在12个不同形状障碍物，由出发点到达目标点以及由出发点经过途中的若干目标点到达最终目标点的多种情形，寻找出一条恰当的从给出发点到目标点的运动路径使机器人在运动中能安全、无碰撞的绕过障碍物而使用的路径和时间最短。由于规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径，机器人不能折线转弯。所以只要给定的出发点到目标点存在至少一个障碍物，我们都可以认为最短路径一定是由线和圆弧所组成，因此我们建立了切线圆结构，这样无论路径多么复杂，我们都可以将路径划分为若干个这种切线圆结构来求解。在没有危险碰撞的情况下，圆弧的半径越小，路径应该越短，因此我们尽量选择最小的圆弧半径以达到最优。对于途中经过节点的再到达目标点的状况，我们采用了两种方案，一种是在拐点和节点都采用最小转弯半径的形式，另一种是适当扩大拐点处的转弯半径，使得机器人能够沿直线通过途中的目标点。然后建立了最优化模型对两种方案分别进行求解，把可能路径的最短路径采用穷举法列举出来，用lingo工具箱求解得出了机器人从O(0, 0)出发，O→A、O→B、O→C和O→A→B→C→O的最短路径；利用matlab中的fminbnd函数求极值的方法求出了机器人从O (0, 0)出发，到达A的最短时间路径。本文提出一种最短切线圆路径的规划方法，其涉及的理论并不高深，只是应用了几何知识和计算机程序、数学工具计算，计算简易，便于实现，能搞提高运行效率。 问题一
O→A 最短路径为：LOA=471.0372 O→B 最短路径为：LOB1?853.8014 O→C 最短路径为：LOC4=1054.0
O→A→B→C→O最短路径为：
问题二机器人从O (0, 0)出发，到达A的最短时间路径：
最短时间是94.5649，圆弧的半径是11.5035,路径长LOA?472.4078
关键词 最短路径；避障路径；最优化模型；解析几何；数学工具
1
一 、问题重述
图1是一个800×800的平面场景图，在原点O(0, 0)点处有一个机器人，它只能在该平面场景范围内活动。图中有12个不同形状的区域是机器人不能与之发生碰撞的障碍物，障碍物的数学描述如下表： 编号 障碍物名左下顶点坐其它特性描述 称 标 1 正方形 (300, 400) 边长200 2 圆形 圆心坐标(550, 450)，半径70 3 平行四边(360, 240) 底边长140，左上顶点坐标(400, 330) 形 4 三角形 (280, 100) 上顶点坐标(345, 210)，右下顶点坐标(410, 100) 5 正方形 (80, 60) 边长150 6 三角形 (60, 300) 上顶点坐标(150, 435)，右下顶点坐标(235, 300) 7 长方形 (0, 470) 长220，宽60 8 平行四边(150, 600) 底边长90，左上顶点坐标(180, 680) 形 9 长方形 (370, 680) 长60，宽120 10 正方形 (540, 600) 边长130 11 正方形 (640, 520) 边长80 12 长方形 (500, 140) 长300，宽60 在图1的平面场景中，障碍物外指定一点为机器人要到达的目标点（要求目标点与障碍物的距离至少超过10个单位）。规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径。机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最近距离为10个单位，否则将发生碰撞，若碰撞发生，则机器人无法完成行走。
机器人直线行走的最大速度为v0?5个单位/秒。机器人转弯时，最大转弯速度为
v0v?v(?)?，其中?是转弯半径。如果超过该速度，机器人将发生侧 10?0.1?21?e翻，无法完成行走。
请建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型。对场景图中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的最短时间路径。 注：要给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器人行走的总距离和总时间。
2
图1 800×800平面场景图
二、 问题分析
1、要求求定点O(0, 0)按照一定的行走规则绕过障碍物到达目标点的最短路径，我们
先可以包络线画出机器人行走的危险区域，在没有危险碰撞的情况下，圆弧的半径越小，路径应该越短。这样的话，拐角处就是一个以方形或三角形障碍物的顶点为圆心半径为10个单位的圆弧，如果是圆形障碍物就应该是以障碍物的圆心为圆心、障碍物的半径长加上10为半径的圆弧。
2、若经过中间的若干点按照一定的规则绕过障碍物到达目标点，这使我们考虑就不仅仅是经过障碍物拐点的问题，也应该考虑经过路径中的目标点处转弯的问题，这时简单的线圆结构就不能解决这种问题，我们在拐点及途中目标点处都采用最小转弯半径的形式，也可以适当的变换拐点处的拐弯半径，使机器人能够沿直线通过途中的目标点，然后建立优化模型对这两种方案分别进行优化，最终求得最短路径。 3、这样机器人行走的路径就是由切线段、内公切线、外公切线以及圆弧组成的这里称之为切线圆路径。
3
三、 模型假设
1、假设障碍物只包含长方形、正方形、三角形、圆形。
2、假设机器人能够抽象成点来处理。 3、路径不考虑走平面场地的边界。
五、符号说明
在计算机程序输入的原始数据中:(T,V,W,r)表示T是起点坐标,V是圆弧的圆心坐标,W是目标节点坐标,r是圆弧半径.
为便于叙述和计算，根据已知条件我们给12个障碍物中的11个方形和三角形顶点用字母Tij或Sij表示，其中T或S表示障碍物，Tij中i表示第i号障碍物，Sij中i表示第9+i号障碍物，j表示从左下角开始按顺时针数起第几个顶点。如下表一所示：
表一
编号 1 2 3 4 5 6 7 8 9 10 11 12 障碍物名称 正方形T1 圆形T2 三角形T 4 正方形T5 三角形T 6 长方形T 7 长方形T9 正方形S1 正方形S2 长方形S3 左下顶点坐标 T11(300, 400) 左上顶点坐标 右上顶点坐标 右下顶点坐标 T12（300，600） T13（500，600） T14（500，400） T32（400，330） T33（540，330） T34（500，240） T42（345，210） T62（150，435） T72（0，530） T43（410，100） T64（235，300） T52（80，210） T53（230，210） T54（230，60） T73（220，530） T74（220，470） 圆心坐标T2 (550, 450)，半径70 T 41(280, 100) T51 (80, 60) T 61(60, 300) T 71(0, 470) T91 (370, 680) S11(540, 600) S21 (640, 520) S31(500, 140) 平行四边形T3 T31 (360, 240) 平行四边形T8 T 81(150, 600) T82（180，680） T83（270，680） T84（240，600） T92（370，800） T93（430，800） T94（430，680） S12（540，730） S13（670，730） S14（670，600） S22（640，600） S23（720，600） S24（720，520） S32（500，200） S33（800，200） S34（800，140） 注：各个障碍物顶点如需要设计圆弧，粗略表示路径时圆弧的位置简单用障碍物顶点字母表示（如：O?T61?T62?T74?T73?T81?B表示从点O 经过6号三角形左顶角为圆心的圆弧到6号三角形上顶角为圆心的圆弧到7号方形右下角为圆心的圆弧到7号方形右上角为圆心的圆弧到8号菱形左下角为圆心的圆弧到达点B。标出的经过顶点都是需要设计圆弧的） 符号 L 符号说明 路径的总长度 第i段切线的长度 第j段圆弧的长度 di lj
4
r 转弯半径 障碍物上的任意点与行走路径之间的最短距离 以顶点Tij或Sij为圆心的圆弧的两个切点 路径时间 k Pij,Qij t 六、模型的建立与求解
由于规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径，机器人不能折线转弯。据此可以这样认为，起点到目标点无论中间障碍物有多少，最短路径都应该是若干个切线圆结构所组成。易知，求两点之间的最短路径中的转弯半径越小路径就越短，我们应该按照最小的转弯半径来算才能达到最优。根据要求机器人行走线路与障碍物间的最近距离为10个单位，因此在方形及三角形顶点转弯的地方圆弧半径r?10，我们尽量取以顶点为圆心半径为r=10个单位的圆弧，如果是圆形障碍物就应该是以障碍物的圆心为圆心、障碍物的半径长加上10为半径的圆弧，只有在必要的时候对半径作适当的加大调整。
6.1 模型? 求从起点O(0, 0)到目标点A(300, 300)的最短路径。
经过观察很显然从O到A有两条选择的路径（其它路径需要经过过多的障碍物路径显然比较长不必考虑）如图6.11所示，一条是从5号障碍物的左上角走OPQA，一条是从右下角走OHJA。他们的路径结构图是类似的如图6.12所示。
65060055050045040035030025020015010050PQTJH1002003004005006007008009001000100O50100 5