机器人行走问题.doc

2012高教社杯全国大学生数学建模竞赛承 诺 书我们仔细阅读了中国大学生数学建模竞赛的竞赛规则.我们完全明白，在竞赛开始后参赛队员不能以任何方式（包括电话、电子邮件、网上咨询等）与队外的任何人（包括指导教师）研究、讨论与赛题有关的问题。我们知道，抄袭别人的成果是违反竞赛规则的, 如果引用别人的成果或其他公开的资料（包括网上查到的资料），必须按照规定的参考文献的表述方式在正文引用处和参考文献中明确列出。我们郑重承诺，严格遵守竞赛规则，以保证竞赛的公正、公平性。如有违反竞赛规则的行为，我们将受到严肃处理。我们授权全国大学生数学建模竞赛组委会，可将我们的论文以任何形式进行公开展示（包括进行网上公示，在书籍、期刊和其他媒体进行正式或非正式发表等）。我们参赛选择的题号是（从A/B/C/D中选择一项填写）： 我们的参赛报名号为（如果赛区设置报名号的话）： 所属学校（请填写完整的全名）： 参赛队员 (打印并签名) ：1. 2. 3. 指导教师或指导教师组负责人 (打印并签名)： 日期： 年 月 日赛区评阅编号（由赛区组委会评阅前进行编号）：2012高教社杯全国大学生数学建模竞赛编 号 专 用 页赛区评阅编号（由赛区组委会评阅前进行编号）：赛区评阅记录（可供赛区评阅时使用）：评阅人评分备注全国统一编号（由赛区组委会送交全国前编号）：全国评阅编号（由全国组委会评阅前进行编号）：机器人避障模型摘要： 本文针对机器人避障到达目标点的最优路径问题，应用解析几何的方法，结合已知条件和查阅资料中的信息，寻找一条从起始位置到达目标位置最短无碰撞路径。 对于问题一，为了确保机器人在行走过程中不会发生侧翻，首先将每个障碍物向外偏移10个单位，在考虑到机器人不能折线拐弯的情况下，对转弯处做平滑处理，利用解析几何与两点之间直线最短的原理，对路径做出最优规划，并计算出机器人总路程和总时间结果如下： OA路径的总路程，471.0372；总时间，96.01764； OB路径的总路程，853.1178；总时间，179.3428； OC路径的总路程，1092.8225；总时间，223.3306； OABCO路径的总路程：2714.3069；总时间：565.96792； 对于问题二，在问题一的基础上，考虑到加速度a0的大小对时间的影响，以及最大转弯速度为v()对路行时间的制约，建立时间的优化模型，运用LINGO编程对目标函数进行求解，结果如下：目标函数值为99.65254，变量值分别为；然后对结果进行分析，并给出其模型改进的方案。 本文所建立的路径规划模型结构清晰、层次分明，图表能直观地表达其路径安排情况，运用了一些常见的数学方法，并得到了相关资料中有力数据的支持，使其更具有推广价值。关键词：最优路径 解析几何 最优规划一.问题提出 图1是一个800800的平面场景图，在原点O(0, 0)点处有一个机器人，它只能在该平面场景范围内活动。图中有12个不同形状的区域是机器人不能与之发生碰撞的障碍物，障碍物的数学描述如下表：编号障碍物名称左下顶点坐标其它特性描述1正方形(300, 400)边长2002圆形圆心坐标(550, 450)，半径703平行四边形(360, 240)底边长140，左上顶点坐标(400, 330)4三角形(280, 100)上顶点坐标(345, 210)，右下顶点坐标(410, 100)5正方形(80, 60)边长1506三角形(60, 300)上顶点坐标(150, 435)，右下顶点坐标(235, 300)7长方形(0, 470)长220，宽608平行四边形(150, 600)底边长90，左上顶点坐标(180, 680)9长方形(370, 680)长60，宽12010正方形(540, 600)边长13011正方形(640, 520)边长8012长方形(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)出发，OA、OB、OC和OABCO的最短路径。(2) 机器人从O (0, 0)出发，到达A的最短时间路径。注：要给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器人行走的总距离和总时间。二.问题分析2.1背景 现代社会已经是一个现代化、智能化、自动化的时代，机器人的发展亦是如此， 随着传感器技术的发展，自主移动机器人已经成为当今研究的热点。避障是自主移动机器人应具备的功能，基于超声波传感器的移动机器人自主避障系统，可以利用超声波传感器达到，检测迅速、方便，且计算简单、易于做到实时控制的效果。本文的意义即在于可以通过此模型设计并检测出大量的实际数据，帮助编程人员设计出最完美的“机器人避障系统”。 2.1.1 问题一解析问题一要求机器人从O(0, 0)出发，OA、OB、OC和OABCO的最短路径。此问题可通过两点之间直线最短的原理，找出从O到A、B、C各点的最短路径，然后考虑其在转弯处的平滑，找出最短路径。2.1.1 问题二解析 问题二 要求机器人从O (0, 0)出发，到达A的最短时间路径。要想时间最省就必须先求出机器人的最大加速度，此问题可以通过二次函数与lingo软件予以解决。 符号说明： 代表机器人行走转弯处圆的圆心 机器人行走转弯处的圆心切线的斜率直线方程圆心圆心角弧长 机器人转弯时的速度机器人行走时的加速度 三.模型假设1. 假设所有障碍物均为规则图形；2. 假设机器人为质点；3. 假设第一问中机器人在直线行走时的速度为5个单位/秒。四.模型建立与求解4.1 建模准备4.1.1 基本思路 构造最短可行性路径的思想是：在可行走区域，从出发点O开始，自动避开障碍区域，寻找一条最短行走路径。问题要求机器人行走线路与障碍物间的最近距离为10个单位，否则将发生碰撞，若碰撞发生，则机器人无法完成行走。基于这个原因，在图1中给障碍区域的平面图形统一向外偏移10个单位，然后从O点出发，通过对图形的分析，找出最近路径的几个转折点，并用直线连接起来，得到一条最短路径（如图2所示），但问题规定机器人不能折线转弯，行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径，因此给图1中的路线进行平滑处理，便得到一条优化的最短路径。 连接O，A两点，得到OA线段，O点和A点之间有5号障碍物，通过对图形的分析得到到A点的两条方案，5号障碍物的几何中心点（155,135）落在OA图1 OA的路径设计草图所在直线的右下区域，得知最短距离应存在与5号障碍物左上方，由于机器人行走线路与障碍物间的最近距离为10个单位，因此以5号障碍物左上角点A5（80，135）为圆心，以=10为半径做圆，使其机器人在行走过程中不会发生碰撞，并证明其平滑效果最好。综上所述可得由O点到A点路径的最优设计图如下所示： 图2 OA的最优路径设计 如上图所设计出的 OA的最优路径中，包含了点与圆求切线的的算法，可以适合于本题中所有这类型算法，因此我们可以对其他算法进行归类，根据切线类型的不同，分为点和圆的切线、圆和圆的公切线两种情况，其整体思路如下图所示：图3 建模思路流程图4.1.2 模型 基于对模型的优化，先处理相切的情况，按照图4的流程安排，依次建立模型并对其求解，首先对点和圆的切线进行讨论，来解决后边一系列的问题。如上图，设为圆心，为圆外的已知点，直线L过点，其表达式可记为（点斜式）： 把式展开得：；其中，；在利用点和直线L的位置关系（点到直线的距离公式）得到： 然后把上述关系式展开得到：然后用二次函数求根公式对其k值进行求解，由于计算量太过庞大，用MATLAB来求解,过程如下： a=z1; b=z2; c=z3; (-b+sqrt(b2-4*a*c)/(2*a) (-b-sqrt(b2-4*a*c)/(2*a)； 上述的a、b、c是通过笔算得来的，算出的k值有两个，结合图像取合理的一个k值，并得到直线方程，然后进行后面的计算。4.1.3 模型 模型对点和圆的切线进行计算，接下来我们将对圆与圆的公切线进行讨论，圆与圆的公切线可分两种情况如下：1. 切点在同侧如上图所示：设两圆圆心分别为，两点，两元圆的公切线方程与直线方程平行，先用两点式写出直线方程如下： ； 通过整理得到直线方程为： ；其中，；设切线方程L为：；在利用切线方程L与直线方程的位置关系（两条平行直线的距离公式）得到： ；求出的值，便得到切线方程L。2. 切点在异侧 如图右所示：设两圆圆心分别为，两点，因切线与圆心连线的交点V为的中点，因此可得（点斜式）：；然后通过化简整理得：；其中，；在利用点或 和直线L的位置关系（点到直线的距离公式）得到： 后边的计算过程同模型中求k值的求法。4.1.4 模型 在模型、模型的基础上，求解切点的坐标、线段的长度和圆弧的长度；求切点 通过模型、模型中解出的直线方程L: ； 设圆C的方程为：； 其圆心坐标为： 令 时，可求出切点P的坐标。 求弧线 在中解出切线L与圆C的坐标，；利用两点直线距离公式求解的长度： ； 设圆心角为，用正弦定理得到与弦长和半径的表达式如下： ； 用反三角函数求出圆心角为的值：；最后用弧长公式弧长=求出弧长 ；4.1.4 路径选择在路径选择的问题中，首先考虑路径应在机器人的起始点和目的地之间的障碍物尽可能少的区域内，且尽可能地走直线；然后考虑到两点之间线段最短，我们把在满足起始点和目的地之间的障碍物尽可能少的条件下，在结合对图形的分析，画出几条可行的路径，如下图所示： 以OB路径为例，得到下图中的4条路径，并给出转折点的坐标；如上图所示，由于实际坐标点非整数点，会给计算造成一定的麻烦，因此给其取最近的整数点，两轴误差在0.42之内，并不影响对最短路径的选择，然后分别算出其路径的长度进行比较。计算得出：路径1长度870.87，路径2长度900.44，路径3长度1012.87，路径4长度1071.42；经比较OB确定为路径1，但规定机器人不能折线转弯，行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径，路径1进行平滑处理，便得到一条优化的最短路径如下图所示：图OB的最短路径图中所给的坐标是平滑处理后圆心的坐标，便与计算其路径的长度，同理可得OA、OC、OABCO的最短路径如下图所示： 按照图中给出的路径，运用模型、模型中的算法对图中的各条切线进行求解， 先确定每条线段所在直线的方程，然后在根据模型中的算法求出每段圆弧的起点和终点坐标，进而求出各段路径的长度，由于各个模型算法简单，采用手工计算，在计算过程中部分环节需用到用MATLAB来求解，其编程见模型，然后得出结果如下所示： OA路径的总路程：471.0372；总时间：96.01764； OB路径的总路程：853.1178；总时间：179.3428； OC路径的总路程：1092.8225；总时间：223.3306； OABCO路径的总路程：2714.3069；总时间：565.96792；由于数量庞大，只给出总路程与总时间的数据，每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标见附表1； 4.2 问题二由于机器人转弯时，所能承受的最大转弯速度为，。如果超过该速度，机器人将发生侧翻，无法完成行走。因此在本问题中，首先考虑的时候如何求解。对函数进行分析；已知个单位/秒，速度的大小只受变量影响，为复合函数，那么我们对其进行分解，令，；函数是开口向下以y轴对称的二次函数；当=0时，=10（不符题意），根据题意可知=10；由此可得：函数的定义域为10,+)，的值域为（-，0，函数在定义域10,+)上单调递减，函数 在定义域上单调递减 ，的值域为（0,1，当趋于+时，的值无限趋近于1；综上所述可得：函数的值域为2.5，5)。4.2.1 模型 在问题一中给半径赋=10，所以在上一问的假设前提下，机器人转弯时，所能承受的最大转弯速度为=2.5；O（0,0）A（300,300）P1P2 如上图所示，设机器人在行走时的加速度为（0）,在线段上的行走时间为,在弧线上的行走时间为,在线段上的行走时间为,起始速度和末始速度为0，在点的瞬时速度为，在点的瞬时速度； 如上图所示，机器人在线段OP1、P2A上行走时，可分为三段来计算其时间，设机器人在段行走所用时间为t1，长度为 ，各段与相互对应即i=j；通过查阅资料，找到有关机器人加速度设计的问题，【法国国家科学研究中心和西班牙foundation Fatronik公司研究人员设计出一台原型机，可以在一分钟内处理240个部件，在携带2公斤物体的情况下每平方秒可获得200米的加速度，目前机器人携带1公斤物体每平方秒加速度为100米。】在现有的基础上令加速度是合理的，由此可得出各段时间的表达式，并得到相应的目标函数及约束条件如下： min s.t. ；运用LINGO软件对目标函数进行求解，结果如下： 该报告说明：运行17步找到全局最优解，目标函数值为99.65254，变量值分别为。检验模型是否合理，通过给赋值用LINGO编程解出几组数据如下： 当=20时，T=99.70254； 当=40时，T=99.67129； 当=80时，T=99.65566； 当=120时，T =99.65046； 当=160时，T=99.64785； 通过这几组数据可得自变量加速度a0对目标函数T影响很小；还需考虑别的相关量，在本问中除了考虑加速度a0外，还应该考虑半径的变化，因此可以对模型进行改进。4.2.2模型改进方案 在上一问的基础上，考虑到半径的变化对其目标函数的影响，建立了关于半径与加速度a0的二元函数的数学模型如下：根据题意要求，对上式求其最小值，F是关于半径与加速度a0的二元函数，依据二元函数求最值问题，首先求出偏导数及，找出偏导数不存在的点；解方程组找出所有的驻点。；由于其运算量太大，运算过程繁杂，还有时间关系的影响，其结果不与求解，但所建立的模型以及其算法具有一定的推广价值。五.模型评价5.1 圆满解决问题整个建模过程不局限于单纯的文字描述，通过适量的图形、表格对文字描述本身的限制起到了补充解释的作用。上述做法可以明晰、直观的体现建模思路，便于读者更好地把握本模型的核心方法。 整个建模过程中对对模型中所涉及的数据进行处理过程中，以及模型求解的过程中，运用了LINGO和MATLAB软件的高效、快速、精确的性能特点进行软件编程，使得建模过程中的运算速度以及模型的准确性进一步提高。 在整个建模过程中选用多个方案，对各个路径进行分析、对比、优化，然后在各方案之间进行相互对比，选取最优方案从而选取取最优解。充分的体现了模型的精细程度与全面性。 整个建模过程中，充分的运用了平滑处理、解析几何、反三角函数、二次函数等数学方法，从而使得模型的实用性进一步增强。让模型更加简便明了，便于全面推广。 整个建模过程中充分的考虑了机器人的加速度，使得模型更加便于在实际中应用及检验。5.2 尚需进一步优化在模型建立求解的过程中，在第一问的几个模型中，计算时未能及时借助任何的数学软件，使得计算过程比较繁琐；但在第二问的模型建立求解中运用了MATLAB与LINGO使得模型得以改进，整个运算过程简单、清晰、明了。 在实际应用过程中，如果出现障碍物过多，或者障碍物形状不规则，甚至二者皆有的情况下，模型需要进一步的深化改进。六.模型推广 本文将采取“人工势场法”对模型进行推广，其基本思想是运用“动态路径规划法”获取障碍物的实际坐标信息，再利用本文中以上模型建立势场。 运用“动态路径规划法”实现动态参考路径的生成与规划。所采用的参考路径的动态选取和规划，优先考虑势场对目标方向的要求，在进行路径规划时优先考虑满足机器人转弯时的角速度，在此基础上尽量满足机器人的线速度，使机器人的行走路径尽量与势场要求一致，从而使机器人能够保持最佳的行走路径。 在转弯前机器人的实际方向可能与势场的目标方向相差较大，会使机器人在转弯方向上存在不连续性。在转弯时优先考虑机器人的最大加速度转弯，在方向与势场方向一致时（直线行走时）继续进行动态规划。 通过“人工势场法”的动态规划，可以使本模型更具灵活性多变性，更具便捷性推广性。七.参考文献【1】姜启源，数学模型（第三版），高等教育出版社，2000.9【2】袁新生等，LINGO和Excel在数学建模中的应用，北京：科学出版社（2007）【3】谢金星，薛毅，优化建模与LINDO/LINGO软件，北京：清华大学出版社（2005.7)【4】谭永基，数学模型，上海，复旦大学出版社，2011【5】杨启帆，数学建模，北京：高等教育出版社（2005.5）【6】闫晶 移动机器人避障系统设计 /Thesis_Y1404574.aspx 2012.09.07 万方数据。附录：附表1 起点坐标终点坐标圆弧圆心坐标圆弧半径直线长或圆弧长机器人行走总时间OA线段一（0，0)（70.506，213.1046)224.499444.89988圆弧一（70.506，213.1406）（76.6064，219.4066）（80，210）109.0513.6204线段二（76.6064，219.4066）(300，300)237.496847.49736机器人行走总距离471.037296.01764OB线段一（0，0）(50.1353，301.6396)305.777761.15554圆弧一（60.1353，301.6396）(51.6979，305.5746)（60，300）104.26621.70648线段二（51.6979，305.57460)(14.6979，439.6089)161.447232.28944圆弧二（141.6979，439.6089）（150，444.0304）（150，434.03）109.79483.91792线段三(150,444.0343)(222.1907,460.2429)73.98814.7976圆弧三(222.1907,460.2429)(230,470)(220,470)1013.49935.39972线段四(230,470)(230530)6012圆弧四(230,530)(225.4967,538.3538)(220,530)109.88833.955532线段五(225.4967,538.3538)(144.5033,591.6462)96.953619.39072圆弧五(144.5033,591.6462)(140.6916,596.3438)(150,600)106.14742.45896线段六(140.6916,596.3458)(100,700)111.355322.27106机器人行走总距离853.1178179.34276OC线段一（0，0）（422.0872，90.0817）431.592886.31856圆弧一（422.0872，90.0817）（428.6909，94.9149）（420，100）108.43093.37236线段二（428.6909，94.9149）（491.1217，204.6016）126.209325.24186圆弧二（491.1217，204.6016）（492.2017，206.2599）（500，200）101.98220.79288线段三（492.2017，206.2599）（727.9377513.9179）387.588677.51772圆弧三（727.9377，513.9179）（730，520）（720，520）106.53812.61524线段四（760，520）（730，600）8016圆弧四（730，600）（727.7178，606.3589）（720，600）106.89162.75564线段五（727.7178，606.3589）（700，640）43.5898.7178机器人行走总距离1092.8225233.33306OABCO线段一（0，0）（70.506，213.1406）224.499444.89988圆弧一（70.506，213.1406）（76.6064，219.4066）（80，210）109.0513.6204线段二（76.6064，219.4066）（219.0754，296.7803）227.999245.59984圆弧二（291.075，296.780）（297.2537，309.0814）（300，300）1015.18226.07288线段三（297.2537，309.0814）（229.5919，532.8946）233.822946.76458圆弧三（229.5719，532.8946）（225.4967，538.3538）（220，530）106.95172.78068线段四（225.4967，538.3538）（144.5033，591.6462）96.953619.39072圆弧四（144.5033，591.6462）（140.6916，596.3458）（150，600）106.14742.45896线段五（140.6916，596.3458）（105.7133，685.4465）95.720519.1441圆弧五（105.7133，685.4465）（115.0217，689.1007）（115，690）1020.03548.01416线段六（115.6079，699.0835）（270.5862，689.9828）155.245331.04906圆弧六（270.5862，689.9828）（272.0119，689.7955）（270，680）101.43920.57568线段七（272.0119，689.7955）（366.7444，670.3384）96.7119.342圆弧七（366.7444，670.3384）（369.3239，670）（370，680）102.6091.0436线段八（369.3239，670）（429.3239，670）6012圆弧八（429.3239，670）（435.109，671.8432）（430，680）106.1692.4676线段九（435.109，671.8432）（533.9915，738.299