三轮控制算法_刘述亮

三轮机器人的运动学分析为了精确的定位及可靠地控制机器人，有必要对机器人进行运动学分析，已得到机器人的运动规律。为了计算各个原动件的驱动力、驱动转矩以及各个零部件的受力，应进行机构的动力学分析，从而为原动件选型、零部件的设计与校核提供理论依据。本章将首先对上一章所选择的各功能模块机构建立运动学和动力学模型，进行理论计算，得到相应的规律和参数，从而完成对理论分析。1.1机器人的运动分析1.1.1机器人运动标准模型三个全向轮驱动的机器人共有三个运动自由度，两个平动一个转动。在实际情况中实际制作出来的机器人不可能完全与理论设计的一样，这里通过合理假设简化问题、忽略次要因素。假设：1、不考虑机器人加工装配的误差，臂长都为0a，各臂间夹角都为120度轮子安装都与臂相垂直。2、不考虑机器人各部分受力后的变形导致几何尺寸的变化。3、地面水平无凹凸。图1.1三全向轮驱动机器人标准模型简图如图3.1所示，令321aaa分别为中心到各轮中心的矢量，长度称为臂长。)0),sin(),cos(()0),sin(),cos(()0,0,(34343323221aaaaaaaa(1.1)以逆时针为正，则机器人三轮转动的线速度单位方向向量为），，（），，（），，（0)sin()cos(0)sin()cos(0)sin()cos(611611367672221vvveee(1.2)三轮线速度大小为321VVV、、三个轮子中心及机器人中心移动的速度为），（），（），（），（0,0,0,0,2232221110yxyxyxyxvvvvvvvvvvvv(1.3)机器人旋转的角速度为）（,0,0则由矢量关系得iiavv0(1.4)viiievV(1.5)通过matlab编程求解得到机器人运动状态与机器人三轮转动的线速度的关系为avvVavvVavVyxyxy21233212321(1.6)令321VVVV，aaaA2123212310，yxvvRB(1.7)关系式写成矩阵形式为：RBAV(1.8)式（3.8）便是机器人运动与三个驱动轮的转动速度的关系式。1.1.2机器人运动变位模型在实际情况中由于误差总是存在不可避免，为了研究误差对机器人的控制产生的影响以及如何通过软件的补偿来消除这些影响。下面研究变位的一般情况的运动模型，考虑安装误差的三轮机器人模型如图3.2所示。图1.2三全向轮驱动机器人变位模型简图如图各臂长的误差为321aaa、、，以1臂为参考各臂的安装误差为32、，则三个臂用向量表示为：)0),sin(),cos(()0),sin(),cos(()0,0,(3343033430323220232202101aaaaaaaaaaaaa(1.9)以逆时针为正，三轮的安装误差为321、、，则机器人三轮转动的线速度单位方向向量为：），，（），，（），，（0)sin()cos(0)sin()cos(0)sin()cos(2261122611322672267212121vvveee(1.10)同理由式（3.4）及（3.5）解得：)sin()]cos()([)cos()]sin()()sin()]cos()([)cos()]sin()()sin()]([)cos(33611334303361133430322672322022672322021210121aavaavVaavaavVaavvVyxyxyx(3.11)由于一般误差不大，因此可以取其一阶表达式近似表示，其一阶表达式为：)()()()()()()(303233232132132123320223223212212212321011aavvVaavvVaavvVyxyxyx(1.12)则：3032332321321321232022322321221221231011aaaaaaA(1.13)二、三轮机器人定位算法2.1算法推导通过前面的分析可知机器人的速度与三个轮子的转动线速度具有一一对应关系，可以由机器人的运动速度和角速度计算出三轮的速度也可以通过已知三轮的速度计算出机器人的速度和角速度。由此便可以通过与电机轴相连的编码器得到三轮的速度，通过计算便可以得到机器人的运动速度及角速度，通过积分便可以得到机器人坐标和姿态。因此三编码器定位算法属于位置跟踪定位，因此该算法不可避免会有累积误差。具体算法推导过程如下：由式(3.8)可得：VARB1(2.1)由上式便可以得到机器人速度角速度与三个轮子转动线速度的关系。对于标准模型有：32131313131313233330VVVvvaaayx(2.2)对于变位模型，通过mathematic求解1A如下：(2.3)由（2.3）知，A-1结果太过复杂，在实际应用过程中不可能计算这样的表达式，故因采用数值法求解,用C编写矩阵求逆函数。A_control[0][0]///////矩阵AA_control[0][0]=-db1;A_control[0][1]=1.0;A_control[0][2]=A_encoder+da1;A_control[1][0]=-sqrt(3)/2.0+0.5*db2;A_control[1][1]=-0.5-sqrt(3)/2.0*db2;A_control[1][2]=A_encoder+da2;A_control[2][0]=sqrt(3)/2.0+0.5*db3;A_control[2][1]=-0.5+sqrt(3)/2.0*db3;A_control[2][2]=A_encoder+da3;voidget_A(void)////////求逆A-1{inti,j;A_d[0][0]=A_control[1][1]*A_control[2][2]-A_control[1][2]*A_control[2][1];A_d[0][1]=A_control[2][1]*A_control[0][2]-A_control[0][1]*A_control[2][2];A_d[0][2]=A_control[0][1]*A_control[1][2]-A_control[0][2]*A_control[1][1];A_d[1][0]=-A_control[1][0]*A_control[2][2]+A_control[1][2]*A_control[2][0];A_d[1][1]=A_control[0][0]*A_control[2][2]-A_control[0][2]*A_control[2][0];A_d[1][2]=-A_control[0][0]*A_control[1][2]+A_control[0][2]*A_control[1][0];A_d[2][0]=A_control[1][0]*A_control[2][1]-A_control[1][1]*A_control[2][0];A_d[2][1]=-A_control[0][0]*A_control[2][1]+A_control[0][1]*A_control[2][0];A_d[2][2]=A_control[0][0]*A_control[1][1]-A_control[0][1]*A_control[1][0];for(i=0;i3;i++){A_m=A_m+A_control[0][i]*A_d[i][0];}for(i=0;i3;i++)for(j=0;j3;j++){A_unrevised[i][j]=A_d[i][j]/A_m;}}由VARB1便可有编码器的转速计算出当前机器人在机器人坐标中的速度地面坐标的角速度。对于地面坐标有：cossinsincos0yxydyxxdtvvvvvvdt(2.4)在地面坐标下积分便可得到机器人的坐标。tyddtxdddvydvx00(2.5)2.2三编码器定位仿真为了在理论上分析三轮安装误差对定位的影响，利用matlab进行定位模拟。模拟的方法为：当三轮安装存在误差（即本是变位模型），定位时分别采用标准模型和变位模型，即比较机器人的实际轨迹与定位轨迹的误差。通过改变不同的变位参数比较两种模型的定位结果误差，结果如图2.1、2.2所示。图2.1沿x轴平动定位误差图2.2沿y轴平动定位误差由图2.2可以看出轮子安装偏差1度时机器人行走5m定位误差近20cm。因此必须根据变位模型进行软件补偿。如图2.3，模拟了1时三编码器定位与实际轨迹的差别，可看出安装误差对定位有很大的影响。图2.3圆周运动定位误差三、三轮机器人控制算法3.1路径规划路径规划是自主式移动机器人导航的基本环节之一。它是按照某一性能指标搜索一条从起始状态到目标状态的最优或近似最优的无碰路径。路径规划技术是机器人研究领域中的一个重要分支，是机器人导航中最重要的任务之一。就最简单的形式，路径规划问题可以按如下定义：设B为一机器人系统，这一系统共具有K个自由度，并假设B在一个二维或三维空间V中，在一组其几何性质已为该机器人系统所知的障碍物中，可以无碰撞运动。这样，对于B的路径规划问题即为：在空间V中，给定B的一个初始位姿Z1和一个目标位姿Z2，以及一组障碍物，寻找一条从Z1到Z2的连续的避碰最优路径，如果该路径存在，那么规划出这样一条运动路径。移动机器人路径规划主要解决三个问题：(l)使机器人能从初始点运动到目标点；(2)用一定的算法使机器人能绕开障碍物并且经过某些必须经过的点；(3)在完成以上任务的前提下尽量优化机器人运行轨迹。根据工作环境路径规划模型可分为两种：基于模型的全局路径规划，作业环境的全部信息已知，又称静态或离线路径规划；基于传感器的局部路径规划，作业环境信息全部未知或部分未知，又称动态或在线路径规划。局部路径规划和全局路径规划并没有本质区别，前者只是把全局路径规划的环境考虑得更复杂一些即环境是动态的，很多适用于全局路径规划的方法经过改进都可以用于局部路径规划而适用于局部路径规划的方法都可以适用于全局路径规划。由于对于本竞赛而言，机器人所处的环境信息全部己知的，机器人的路径规划完全离线(of-line)进行[15]，即通过一定算法，根据环境状况为机器人计算出一条可行路径。即全局路径规划方法，这种方法依据己获取的全局环境信息，给机器人规划出一条从起点至终点的运动路径。规划路径的精确程度取决于获取环境信息的准确程度。全局路径规划方法通常可以寻找最优解，但需要预先知道准确的全局环境信息。通常该方法计算量大，实时性差，不能较好地适应动态非确定环。基于环境建模的全局路径规划的方法主要有：自由空间法、构型空间法和栅格法、人工势场法等。机器人行走的目的是为了快速到达目的地，并避开障碍。因此机器人在行走过程中并不需要精确的控制其路径，但到达目的地必须是精确的。根据这一特点，我们模仿人工势产法来规划机器人的路径——吸引点路径规划算法。算法原理是通过在场地上设置一组吸引点，机器人始终受到前一个吸引点的吸引，直到到达这一吸引点的邻域内后更新吸引点，最终到达目的地。具体路径