第六章-机器人路径规划

第6章机器人路径规划路径规划：一定的作业要求；一定的评定标准；寻找一条从起始状态到目标状态的无碰撞路径。126.1关节空间路径规划直角坐标空间到关节坐标空间的转换起始点途经点起始点途经点终点……为每一个关节设计一条光滑的运动曲线（运动时间相等）6.1.1三次多项式路径规划2个位置约束条件400,=fft2个速度约束条件00,=0ft满足2个位置约束条件，且光滑（连续）的轨迹曲线有多种。满足以上4个约束条件的至少是3次多项式230123taatatat2123232326taatattaat500,=fft00,=0ft230123taatatat212323taatat00230123121230023ffffffaaatatataaatat001220303032ffffaaatat00231223100010100001230ffffffatttaatta100231223100010100001230ffffffatttaattaExp:6-1600015,=75,00,=0,3fffttts6.1.2含途经点的三次多项式路径规划700,=fft00,=fft在每一个途经点不停歇（速度不为0）起始点途经点终点……0023012301212323fffffffaaatatataaatat001020203003232121fffffffffaaatttatt8途经点速度的确定1）由直角坐标速度通过雅可比矩阵变换成为关节速度1PJqqJP2）采用近似方法获取00,0,0,0ABD2CCC选取左右两端的斜率的平均值93）保证经过途经点的速度和加速度连续BEXP：以Fig6.3中B—D为例起始点C中间点D终点设计两个三次多项式路径。10ftt路径1时间经历：20ftt路径2时间经历：1232231011121320212223taatatattaatatat位置约束：102310111121131202320212222232BCfffCDfffaaatatataaatatat10速度约束：11122221222232122111121131210000023023ffffffataatattaatata加速度约束：121121131220262ffftatata约束条件确定了8个线性方程，可以解得两个三次多项式中的所有待定参数！6.1.3直线+抛物线路径规划11tabt但端点速度不为0（危害？）起始点和终点之间直线最短路径的优化方案。设计直线+抛物线的混合路径规划带有抛物线过渡域的线性轨迹为简化设计，假定首末两端抛物线的时间长度相等。路径相对于时间中点和位置中点是对称的。,hht1220012bttttat抛物线段的方程：抛物线的速度方程：0bttttta在抛物线的末端的速度：bbtat抛物线末端速度=直线段（恒速）的速度hbbbbhbttttatlet2htt所要求的运动持续时间20bbhbttt13在给定起始角、终止角和运行时间，以及加速度：220422fbttt确定抛物线与直线段的转折时间24hbt抛物线段加速度的约束条件：24when2hbbttt直线段长度=0，路径由两端抛物线组成。加速度增加，抛物线长度下降，极限情况抛物线长度=0.6.1.4含途经点的直线+抛物线路径规划14三个相邻的途经点j,k,l相邻途经点之间以线性函数相连；而途经点附近以抛物线过渡。Straightlinesegment,,jkjktjkdFullsection,jkjkt:Accelerationofparabolasectionatpointj:Accelerationofparabolasectionatpointkjk15三个相邻的途经点j,k,ldkjjkjktkkljkksgnkljkkkt1122jkdjkjktttt起始段，抛物线末端速度等于直线段速度211211d12112ttt2121121212ddttt1211sgn12121212dtttt16结束段，与起始段成对应关系。0000123412233410352510213dddtststs起始段：02150s212112121223510240.2750ddttts起始段加速段时间：静止到加速到匀速的时间170000123412233410352510213dddtststs1—2间匀速运动0211211d12150*0.2713.5/12tstt中间段（2-3）匀速运动速度：03223d23253510/1st02223125050/sgns2312221013.50.4750ts在以上加速度的条件下，速度由13.5°/s变化到-10°/s的耗时但是匀速运动时间暂时无法求到！因为末端减速段耗时未知！2—3有运动方向的变化（反向），显然有负加速度：18起始段直线段耗时：12121210.4720.271.5022dtttts中间段（2-3）匀速运动速度：03223d23253510/1st但是匀速运动时间暂时无法求到！结束段（3-4）的减速时间（由一定的速度到静止状态）：421025390.10250ts结束段（3-4）的匀速运动时间：03410255.10/30.05s192-3，3-4两端匀速运动速度由差异，那么3点末端需要一个加速03410255.10/30.05s3点末端加速段耗时：35.1010.00.09850ts03223d23253510/1st02334235050/sgns232323110.470.09810.7162222dtttts这样留给2-3段匀速运动的时间：203-4段匀速运动的时间（匀速速度-5.10°/s）：34344310.09830.1022.84922dtttts216.2直角坐标空间路径规划在直角坐标空间的轨迹规划系统中，作业是用机械手终端夹手的直角坐标节点序列确定的。直角坐标节点：表示夹手位姿的齐次变换矩阵。0123456PPPPPPP以上即为夹手必须经过的直角坐标节点。参照这些节点的位姿，可将手部描述为一连串的运动和动作。22在直角坐标中，将机器人的目标（节点）位姿通过以下矩阵方程表示：060base6toolbaseobjtTTCP以上等式表示为：机器人抓取位姿=操作对象期望抓取位姿160base0toolbaseobj6tTCPT6toolT求解，并通过逆向运动学得到各关节变量值。实时完成难度较大，需要预先进行路径的规划。236.3移动机器人路径规划移动机器人是集环境感知、动态决策、行为控制与执行等多功能于一体的综合性系统。移动机器人路径规划技术就是移动机器人在具有障碍物的环境中按照一定的评价标准（如工作代价最小、行走路径最短、行走时间最短等），寻找一条从给定起点到达目标终点的无碰路径。24传统路径规划大多基于图论的思想，通过一定的方法建立几何模型，进行空间路径的搜索，包括自由空间法、图搜索法、栅格解耦法、动态规划算法等。25智能路径规划是随着近年来人工智能的深入研究而发展起来的优化方法，其中有模糊逻辑法、神经网络法、遗传算法以及现在非常热门的仿生算法，如蚁群算法、免疫算法、粒子群算法、蜂群算法等。26全局路径规划，又称为静态或离线路径规划，作业的环境信息完全已知，主要方法有栅格法、可视图法、链接图法、概率路径图法、拓扑法等。局部路径规划局部路径规划，又称为动态或在线路径规划，作业环境部分未知或完全未知，主要方法有人工势场法、模糊逻辑算法、遗传算法、蚁群算法、免疫算法等。27无论何种路径规划方法，原始尺寸变换是必须进行的。286.4遗传算法简介遗传算法基于自然选择的生物进化，是一种模仿生物进化过程的随机方法。自适应、全局化优化、概率化搜索。6.4.1遗传算法基本框架求函数极值问题（极大、极小），用以下数学规划模型来描述：29决策向量：12TnxxxX用长度一定的二进制编码串分别表示n的决策向量：120101000101110101;0001;0111inxxxX染色体（个体）M个染色体构建成为一个群体。对群体中每一个染色体按照一定规则确定其适应度。tofindmaxiumiiFfX30使用遗传算子遗传算子：选择、交叉、变异Pt1Pt6.4.2遗传算法的运算过程316.4.3遗传算法算例1）个体编码②2）初始群体的产生②3）适应度的计算（解码③④）个体对应的目标函数值（适应度⑤）⑥⑦⑧）326）选择计算（复制计算）：适应度高的个体遗传到下一代原则？——与适应度成正比的概率！计算每个个体被遗传到下一代的概率⑥设计与适应度成正比的概率来选择被遗传的个体。33概率0.24的被选择了两次，概率0.35的被选择了一次，概率为0.17的被选择了0次⑦⑧。5）交叉运算：产生新个体的主要方式（单点交叉）随机配对⑨随机设置交叉点⑩（交叉点设置在被交叉染色体之前）1-2:2：011101；111001交叉运算结果：011001；1111011（11）346）变异运算：某一个或某几个基因的改变随机选择变异点（12）随机设置交叉点⑩（交叉点设置在被交叉染色体之前）3号染色体在第二位变异：101001变异运算结果：111001（13）注意2号染色体的变异结果！35获得子代群体P（1）——（14）重复适应度的计算——（15）（16）（17）已经有了明显的改进！实际上个体2即为最佳个体！366.4.4遗传算法的特点1）以决策变量的编码作为运算对象，可以借助于生物学中的基因、变异等概念。3）并行多点搜索（并行运算）。4）使用概率搜索技术。2）以目标函数值作为搜索信息，不需要其它诸如目标函数值导数等辅助信息。376.5基于遗传算法的移动机器人路径规划6.5.1离散空间中移动机器人路径规划以路径最短为评价指标1.规划空间的网格化建模直角坐标法、序号法2.路径规划遗传算法（1）个体编码方法网格序号编码：{0,1,11,21,22,23,33,44,…}38编码长度短、简明、直观（2）初始群体的产生网格序号编码：{0,1,11,21,22,23,33,44,…}编码长度是变化的间断无障碍路径：{0,99}；{0,2,20,45,75,87,99}…（3）适应度函数111FDn距离最短作为目标函数引入修正项，消除运行过程中间断点相距太远的过短路径。（4）遗传算子设计A、选择算子：使个体按照与适应度成正比的概率向下一代繁殖；B、交叉算子：通常意义下的交叉；重合点处的交叉（更佳）C、变异算子：随机