高教社杯数学建模D题机器人避障问题论文Word格式文档下载.docx
《高教社杯数学建模D题机器人避障问题论文Word格式文档下载.docx》由会员分享，可在线阅读，更多相关《高教社杯数学建模D题机器人避障问题论文Word格式文档下载.docx（18页珍藏版）》请在冰豆网上搜索。
最优化模型；
解析几何；
数学工具
一、问题重述
图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个单位，否则将发生碰撞，若碰撞发生，则机器人无法完成行走。
机器人直线行走的最大速度为个单位/秒。
机器人转弯时，最大转弯速度为，其中是转弯半径。
如果超过该速度，机器人将发生侧
翻，无法完成行走。
请建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型。
对场景图中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个方形和三角形顶点用字母或表示，其中T或S表示障碍物，中i表示第i号障碍物，中i表示第9+i号障碍物，j表示从左下角开始按顺时针数起第几个顶点。
如下表一所示：
表一
左上顶点坐标
右上顶点坐标
右下顶点坐标
正方形T1
T11（300,400）
T12（300，600）
T13（500，600）
T14（500，400）
圆形T2
圆心坐标T2（550,450），半径70
平行四边形T3
T31（360,240）
T32（400，330）
T33（540，330）
T34（500，240）
三角形T4
T41（280,100）
T42（345，210）
T43（410，100）
正方形T5
T51（80,60）
T52（80，210）
T53（230，210）
T54（230，60）
三角形T6
T61（60,300）
T62（150，435）
T64（235，300）
长方形T7
T71（0,470）
T72（0，530）
T73（220，530）
T74（220，470）
平行四边形T8
T81（150,600）
T82（180，680）
T83（270，680）
T84（240，600）
长方形T9
T91（370,680）
T92（370，800）
T93（430，800）
T94（430，680）
正方形S1
S11（540,600）
S12（540，730）
S13（670，730）
S14（670，600）
正方形S2
S21（640,520）
S22（640，600）
S23（720，600）
S24（720，520）
长方形S3
S31（500,140）
S32（500，200）
S33（800，200）
S34（800，140）
各个障碍物顶点如需要设计圆弧，粗略表示路径时圆弧的位置简单用障碍物顶点字母表示（如：
表示从点O经过6号三角形左顶角为圆心的圆弧到6号三角形上顶角为圆心的圆弧到7号方形右下角为圆心的圆弧到7号方形右上角为圆心的圆弧到8号菱形左下角为圆心的圆弧到达点B。
标出的经过顶点都是需要设计圆弧的）
符号
符号说明
L
路径的总长度
第段切线的长度
第段圆弧的长度
转弯半径
障碍物上的任意点与行走路径之间的最短距离
以顶点或为圆心的圆弧的两个切点
t
路径时间
六、模型的建立与求解
据此可以这样认为，起点到目标点无论中间障碍物有多少，最短路径都应该是若干个切线圆结构所组成。
易知，求两点之间的最短路径中的转弯半径越小路径就越短，我们应该按照最小的转弯半径来算才能达到最优。
根据要求机器人行走线路与障碍物间的最近距离为10个单位，因此在方形及三角形顶点转弯的地方圆弧半径，我们尽量取以顶点为圆心半径为r=10个单位的圆弧，如果是圆形障碍物就应该是以障碍物的圆心为圆心、障碍物的半径长加上10为半径的圆弧，只有在必要的时候对半径作适当的加大调整。
6.1模型求从起点O（0,0）到目标点A（300,300）的最短路径。
经过观察很显然从O到A有两条选择的路径（其它路径需要经过过多的障碍物路径显然比较长不必考虑）如图6.11所示，一条是从5号障碍物的左上角走，一条是从右下角走。
他们的路径结构图是类似的如图6.12所示。
图6.11
图6.12
设为起点，为目标点，和分别为机器人经过拐点分别于隔离危险线拐角小圆弧的切点，圆心为,圆的半径为，OA的长度为，的长度为，的长度为，角度=,.求的长度，设为.
解法如下：
如上图可得有以下关系：
：
在：
在中：
所以：
从而可得路径长：
（1.1）
即模型为：
，
由已知条件知即
利用计算机计算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;
结果算得的长度=471.0372
又由
（1）
和
（2）
计算得点为圆心的圆弧两切点的坐标为。
同样，通过计算可得从路径从右下角方向（圆心T的坐标改为5号障碍物右下角顶点坐标（230,60））走时长度498.4259.
很显然机器人从5号障碍物左上角走的路径小于机器人5号障碍物右下角走的路径要短。
因此从起点O（0,0）到目标点A（300,300）的最短路径是，长度=471.0372，圆弧PQ的切点坐标为。
同时可以验证6号三角形障碍物右下角顶点到切线AQ距离大于10，所以过的路径是安全的也是最短的。
下面考虑问题二：
从O到A的最短时间路径
由式（1.1）可求得时间路径的目标函数为：
利用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
升级会员 