机器人避障问题

1/19障问题要障碍物区域1/19 1 1/19域，机器人从以原点(0,0)出发前往不同的目标点，并且不能与障碍物发生碰撞。障碍标称形形123456789在机器人的行进过程中，规定机器人所走的路径为直线和圆弧所组成(不可有折线转弯)，其中与直线相切的圆弧为不与障碍物发生碰撞的转弯路径，也可以由多个相切弧路径组成，每个圆弧的半径最小为10个单位，否则将发生碰撞，导致机器人无v=v(p)=v01+e100.1p2，其中p是转弯半径。若超过该速度，则机器人侧翻，无法完2/19。 3模型假设1、假设障碍物的位置固定不变，且只包含长方形，正方形，平行四边形，三角形，圆形，题目中所给的数据准确无误。3、假设机器人的性能足够好，在行走的时候不出现故障，且能准确的沿着圆弧转弯。4符号说明表1符号说明表L1L2r机器人过圆弧长度TT(2)5模型的建立与求解(2)5.1问题一(求解最短路径)定机器人的行走路径由直线段和圆弧组成(转弯路径由与直线路径相切的一段圆弧组r1)圆弧半径与路径长度关系(1)(1)r=arccosarba2+b2-c2再根据机器人所走路线结构和公式(2)计算出的角度，可建立路径长度关于圆弧3/19dLrra2dLrra2b2c2drab2abL(r)a2r2b2r2ra2r2b2r2(2)r(3)ab2aba2r2b2r2(2-arccosr-arccosr-arccosa2b2c2)rab2ab2)求解最短路径下的r根据路径与圆弧半径的关系式(3)，对未知量r求导，求出最短路径下的r。求导2arccos-arccos-arccos()4dL0()(5)理由：根据求导结果(4)，可知当圆弧半径r逐渐增大时，机器人行走弧线结构的3求解路径长度以及点坐标(路径起始点和终点坐标)的方法交。三，弧长的求解(1)已知点到圆弧4/195/19AO=(m-a1)2+(nAC=AO2-CO2BO=(m-a2)2+(nO在求出线段AC、BD后，可以根据方程求解确切的切点C、D坐标：(|AC=(x-a)2+(y-b)2求切点C坐标〈1111|r=(x1-m)2+(y1-n)2由于方程组(8)、(9)均为二元二次方程组，可以用MATLAB软件进行求解。(2)切点连线与圆心平行(6)(7)(8)(9)AB=OO'=(m-m)2+(n-n)22121在结合AB的长度，和三角形勾股定理建立方程，求出切点坐标(|r=(x-m)2+(y-n)2〈1111求切点A坐标|(x1-m2)2+(y1-n2)2=AB2+r2(|r=(x-m)2+(y-n)2〈2222求切点B坐标|(x2-m1)2+(y2-n1)2=AB2+r2(3)切点连线与圆心连线相交 6/19CD=2(12_CD=2(12_m)2+(12_n)2_r22121根据所求CD长度和已知坐标点，列出关于切点的函数方程组并求解出(计算机求解)切点具体坐标。函数方程组具体如下：(x_m)2+(y_n1112_x)2+(2_x)2+(12_y)2=21212(x_m)2+(y_n)2222_x)2+(2_x)2+(12_y)2=22222(4)求弧长 a=三COD=arccos(14)(15)(16)〈11(16)C=a*r(17)根据前面已证明绘图原则和求解路径长度和起始点坐标的方法，在求得线路O)A,O)B,O)C的最短路径的基础上求得O)A)B)C)O1)O)A最短路径(1)根据所制定的绘制路径的原则，在场景图中，绘制全部的O)A路径，有两(2)根据制定的求解路径长度和起始点坐标的方法进行求解，可得出O)A所走7/198/19度(弧长)TA(76.6,29.4)(300,300)度(弧长)TA(300,300)2)OB最短路径X所示(2)根据制定的求解路径长度和起始点坐标的方法进行求解，可得出OB所走起点坐标终点坐标圆弧角度(弧长)1,6)(50.2,301.6)(51.7,305.5)24°61626162(51.7,305.5)(141.7,440.5)(141.7,440.5)(147.9,444.8)45°6272627220.04,460.2)(220.04,460.2)(230,470)78°72737273(230,530)(225.5,538.35)57°7381T7381(144.5,591.6)(144.5,591.6)(140.7,596.3)35°(140.7,596.3)(100,700)起点坐标终点坐标圆弧角度(弧长)51635163637363737381T7381,0)(70.3,210.05)(70.3,210.05)(76.01,219.03)48°,290.84)(239.02,290.84)(244.98,300.65)70°.65)591.64)(144.51,591.64)(140.69,596.38)35°(140.69,596.38)(100,700)起点坐标终点坐标圆弧角度(弧长)51635163637363730.22)(232.11,50.22)(239.84,59.611)77°(244.99,299.79)(244.98,300.65)5°.65)9/19T→TT→T7381(140.69,596.38)(100,700)3)OC最短路径X所示(2)根据制定的求解路径长度和起始点坐标的方法进行求解，可得出OC所走圆弧角度路线距离(弧长)61626162T→T6274T→T7412T→T1294T→T94102T→T130102103T→C103机器人行走总路程1239.802O→T→T→T→T→T→C52322114113圆弧角度路线距离(弧长)T→T5232T→T322T→T1702114T→T80114113T→C43.5113机器人行走总路程1059.992O→T→T→T→T→T→C54322114113圆弧角度路线距离(弧长)T→T5432T→T322T→T1702114T→T80114113T→C43.5113机器人行走总路程1055.063A、B、C时的圆弧圆心位置和圆弧半径ii建立优化方程组，再借助LINGO对优化方程进行求解。例如在障碍物5经过A再到障碍物7的路径中，设经过A的转弯圆弧的圆心坐标为(x,y)，经过障碍5和障碍7的转弯圆心坐标为(80,210)，(220,530)，根据关系建立优化方程为：121min=l1+l2+EF;l1=@sqrt((x-80)^2+(@sqrt(100-(x-300)^2)+300-210)^2);l2=@sqrt((x-220)^2+(@sqrt(100-(x-300)^2)+300-530)^2);EF=10*(3.1415926-@acos((l1^2+l2^2-140^2-320^2)/(2*l1*l2)));x<300;y=@sqrt(100-(x-300)^2)+300;解得圆心坐标为(290.8854，304.1140)，半径长度为10。A如图9所示 同理可证，过点B的圆弧圆心为(108.1715,694.0966)；过点C的圆弧圆心为(709.7939,642.0198)，圆弧半径均为10。O长)弧长)54总距离总距离3、由于无法从9号障碍物上方行走，因此从B→C的路径也只有一条：738183919194T→TT→CT→TT→S1142S→T2323254T→O总路程T=+dDET=+dDE12v5.2问题二(求解最短时间路径)d=((904r221r50500r2)2+(21021r24r50500r2)22525505050502525d=((220+r2r56500r2)2+(90+r2+r56500r2)2825565056502825119911d=r(arccos(1+1((1563r29r5650