2012年国赛高教杯数学建模D题机器人避障问题解题全过程文档及程序

2012年国赛高教杯数学建模

D题机器人避障问题

图1是一个800×800的平面场景图，在原点O(0, 0)点处有一个机器人，它只能在该平面场景范围内活动。图中有12个不同形状的区域是机器人不能与之发生碰撞的障碍物，障碍物的数学描述如下表：
在这里插入图片描述
在图1的平面场景中，障碍物外指定一点为机器人要到达的目标点（要求目标点与障碍物的距离至少超过10个单位）。规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径。机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最近距离为10个单位，否则将发生碰撞，若碰撞发生，则机器人无法完成行走。
机器人直线行走的最大速度为v=v(ρ)=v_0/(1+e^(10-0.1ρ2 ) )个单位/秒。机器人转弯时，最大转弯速度为，其中是转弯半径。如果超过该速度，机器人将发生侧
翻，无法完成行走。
请建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型。对场景图中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的最短时间路径。
注：要给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器人行走的总距离和总时间。
在这里插入图片描述

整体求解过程概述(摘要)

针对机器人避障问题，本文分别建立了机器人从区域中一点到达另一点的避障的最短路径、最短时间路径的非线性0-1整数规划模型。同时，本文为求带有NP属性的非线性0-1整数规划模型，构建了有效启发式算法，利用MATLAB软件编程，求得了O→A、O→B、O→C、O→A→B→A→C的最短路径，同时得到了O→A的最短时间路径，求得的各类最短路径均是全局最优。
针对区域中一点到达另一点的避障的最短路径问题，首先，本文证明了圆弧位置设定在需要绕过障碍物的顶角上，且圆弧半径为10个单位时，能够使得机器人从区域中一点到达另一点的行进路径最短；其次，本文将最短路径选择问题转化成了最短路径的优选问题，根据避障条件，建立了具有较高普适性的避障最短路径的优化模型。为便于求解，本文巧妙地将此优化模型转化成了以可行路径不与障碍物边界相交、不与圆弧相交为约束条件，以机器人从区域中一点达到另一点避障路径最短为目标的0-1规划模型；再次，本文构建了两种有效的启发式算法，利用MATLAB软件编程求得了O→A、O→B、O→C、O→A→B→A→C的最短路径，最短路径长分别为471.0372、853.7001、1088.1952、2725.1596，其中O–>A的最短路径为(0,0)→(70.5063,213.1405) →(75.975,219.1542)→（300,300)，对应圆弧的圆心坐标为(80,210)，O→B的最短路径，对应圆弧的圆心坐标：(60,300)、(150,435)、（220、470）、(220,530)、(150,600)， O→C经过的圆心：(410,100)、(230,60)、(720,520)，（720,600），(500,200)， O→A→B→C→O经过的圆心：(410,100)，(230,60)， (80,210)，(220,530)，(150,600)，(270,680)，(370,680)， (430,680)，(670,730)，(540,730)，(720,520)，(720,600)，(500,200)。
针对最短时间路径问题，我们建立了从o点出发到任意目标点的0-1非线性整数规划模型，同时针对题意要求，具体构建了从o点出发到A的最短时间路径的0-1非线性整数规划模型，利用LINGO软件求解，获得了机器人从o点出发，到达A的最短时间路径，求得最短时间路径下转弯半径为12.9885 ，同时最短时间路径时间长为94.2283个单位。相应圆弧的圆心坐标为（82.1414，207.9153），两切点坐标分别为（69.8045，211.9779）、（77.7492,220.1387）。

模型假设：

1.将机器人看成一个质点；
2.半径不变时，机器人在行进、转弯过程中能一直保持最大的速度；
3.机器人行走过程中不会意外停止；
4.忽略影响机器人行走非最小转弯半径以及最小安全距离因素。

问题分析：

求取最短路径的分析
本问题要求机器人从区域中一点到达另一点的避障最短路径。机器人只要做到转弯时的圆弧半径最小为10个单位、与障碍物最近距离单时刻保持大于10个单位，那么可行走的路径就有无数条，若想求得机器人从区域中一点到达另一点的避障最短路径，则需要建立避障的最短路径模型，而建立避障的最短路径模型有一定难度。根据对问题的分析，我们认为可以从简单做起，先确定小范围内最短路径条件，如圆弧位置的影响，圆弧半径的大小，避免与障碍物碰撞条件等，通过确定最短路径条件来建立避障的最短路径模型。对于最短路径的求取，我们可以通过确定穷举原则，利用穷举法来求解，当然也可以通过构建启发式算法的进行求解。
最短时间路径的分析
对于要建立最短时间路径模型来说，我们容易知道影响的因素有直线行走速度、转弯速度，同时还需要考虑使得最短时间路径条件，如圆弧位置（坐标）的影响，圆弧半径的大小，避免与障碍物碰撞条件等。对于直线行进，我们希望行进速度越大越好，对于机器人转弯时，转弯速度要有约束，要保证机器人不能发生侧翻。我们发现圆弧半径的大小与转弯速度紧密相连，从转弯速度公式来分析，当转弯半径增大时，最大转弯速度也增大，为在更短时间内行进到目标点，我们希望转弯速度为机器人的最大转弯速度较好，但有很大的可能是行进的路径不是最短的，即行进路径有很大可能在增加。于是，我们需要做的工作是，在满足最短时间路径条件时，找到一个圆弧的坐标位置，同时确定半径的大小，以求得最短时间路径。

模型的建立与求解整体论文缩略图

在这里插入图片描述

全部论文请见下方“ 只会建模 QQ名片” 点击QQ名片即可

程序代码：

function mind=distancebetweenlines(A,B,C,D,M)
Ax=A(1);Ay=A(2);
Bx=B(1);By=B(2);
Cx=C(1);Cy=C(2);
Dx=D(1);Dy=D(2);
if (A(1)-B(1))~=0
    k=(By-Ay)/(Bx-Ax);
    b=By-k*Bx;
    ABXXL=linspace(A(1),B(1),M);
    ABYXL=k.*ABXXL+b;
else
    ABXXL=linspace(A(1),B(1),M);
    ABYXL=linspace(A(2),B(2),M);
end
if (C(1)-D(1))~=0
    k=(Dy-Cy)/(Dx-Cx);
    b=Dy-k*Dx;
    CDXXL=linspace(C(1),D(1),M);
    CDYXL=k.*CDXXL+b;
else
    CDXXL=linspace(C(1),D(1),M);
    CDYXL=linspace(C(2),D(2),M);
end
mind=100000;
for i=1:M
    for j=1:M
        if sqrt((ABXXL(i)-CDXXL(j))^2+(ABYXL(i)-CDYXL(j))^2)<=mind
            mind=sqrt((ABXXL(i)-CDXXL(j))^2+(ABYXL(i)-CDYXL(j))^2);
        end
    end
end

clear
clc
close all
theta=0:pi/100:2*pi;
zb{1}=[300 400;500 400;500 600;300 600];
zb{2}=[550 450;70 70];
zb{3}=[360 240;400 330;540 330;500 240];
zb{4}=[280 100;345 210;410 100];
zb{5}=[80 60;80 150+60;150+80 150+60;150+80 60;];
zb{6}=[60 300; 150 435;235 300];
zb{7}=[0 470;220 470;220 530;0 530];
zb{8}=[150 600;240 600;270 680;180 680];
zb{9}=[370 680;430 680;430 800;370 800];
zb{10}=[540 600;670 600;670 730;540 730];
zb{11}=[640 520;720 520;720 600;640 600];
zb{12}=[500 140;800 140;800 200;500 200];
zb{13}=[0 0;0 800;800 800;800 0;];
zb{14}=[0 0 79];
zb{15}=[300 300 65];
zb{16}=[100 700 66];
zb{17}=[700 640 67];
for i=1:length(zb)
    temp=zb{i};
    if size(zb{i},1)==2
        x=temp(2,1)*cos(theta)+temp(1,1);
        y=temp(2,2)*sin(theta)+temp(1,2);
        plot(x,y,'r-');hold on
    elseif size(zb{i},1)==3
        for j=1:length(temp)
            plot([temp(:,1);temp(1,1)],[temp(:,2);temp(1,2)],'b-');hold on
        end
    elseif size(zb{i},1)==4
        for j=1:length(temp)
            plot([temp(:,1);temp(1,1)],[temp(:,2);temp(1,2)],'b-');hold on
        end
    elseif size(zb{i},1)==1
        plot(temp(1),temp(2),'r.','MarkerSize',12);text(temp(1)+10,temp(2)+20,char(temp(3)));hold on
    end
    if i<=12&i~=2
        for j=1:size(temp,1)
            temp1=temp(j,:);
            x=10*cos(theta)+temp1(1);
            y=10*sin(theta)+temp1(2);
            plot(x,y,'r-');hold on
        end
    end
end
axis([0 800 0 800]);grid on
hold off
%下面计算切点坐标
for i=1:4
    x=[];y=[];kk=1;
    temp=zb{13+i};A=temp(1:2);
    for j=1:12
        temp2=zb{j};
        if j==2
            r=70;temp2=temp2(1,:);
        else
            r=10;
        end
        for k=1:size(temp2,1)
            B=temp2(k,:);
            if ~ismember(B(1),[0,800]) & ~ismember(B(2),[0,800])
                [x(:,kk) y(:,kk)]=qiedian(A,B,r);
                kk=kk+1;
            end
        end
    end
    qdzb(:,:,i)=[reshape(x,1,2*length(x));reshape(y,1,2*length(y))];
end