Matlab在智能采摘机械手中的应用

Matlab在智能采摘机械手中的应用——RB03机器人的运动空间分析及其避障规划仿真在Matlab中用数值法对广州数控RB03机器人进行工作空间的分析，然后根据机器手参数，建立了机械手的工作空间模型。为了提高农业采摘机器人在工作空间的避障能力，实现避障行为控制，首先，以圆形包络线对采摘空间的障碍物进行C-空间建模，通过坐标变换简化障碍物空间的计算；其次，使用A*算法，通过仿真验证其有效性。1.1广州数控RB03机器人简介图1.1RBO3机器人本体表1.1RB03机械手连杆D-H参数1.2RB03机器人的工作空间仿真与分析机器人工作空间是衡量机械手工作能力的一个重要的运动学指标。机器人工作空间的求解方法主要有解析法、图解法及数值法。解析法用方程表示工作空间关节iαi(°)ia（mm）di(mm)θi(°)关节角度范围(°)1901553800-170~17020360090-60~15039010000-170~754-9003650-190~190590000-125~1256001500-360~360的边界，但是表达式过于复杂，不适用于工程应用；图解法直观性强，但不太适合当主运动自由度过多时；数值法简单，可以分析任意结构形式的机器人工作空间。结合本文中的RB03机械手，下面采用数值法分析机械手工作空间。蒙特卡洛模拟方法是数值法的一种，其主要思路是随机生成各个关节在取值范围内的离散值，然后通过运动学方程计算出末端执行器的末端点，当离散值趋于连续时，得到的末端点的集合就组成了机械手的工作空间，方法直观。利用MATLAB实现，具体步骤如下：1)根据各关节的取值范围，利用下式5.1可以获取各关节的随机量，其中rand函数产生N个[0,1]之间的随机数，为关节变量的最小值，为关节变量的最大值。𝜃𝑖=𝜃𝑖min+(𝜃𝑖𝑚𝑎𝑥−𝜃𝑖min)×𝑟𝑎𝑛𝑑(𝑁,1)（5.1）2)将上式5.1得到的N个随机关节角度值代入到机械手的正运动学方程中，得到末端执行器的N个随机位姿。调用画图命令，绘制工作空间，如图1.2所示。图1.21.3RB03采摘机械手的避障规划算法当机器人移动到合适的采摘位置后，面临的问题是如何规划各个机械手的运动进行采摘。当没有障碍物时，可以采用点到点（PTP）的运动方式，此时只需要进行轨迹规划即可；但是当有障碍物时，则需要借助路径规划避开障碍物从而完成采摘任务。本节主要讨论当有前置障碍物时，利用C-空间法把障碍物映射到C-空间，然后通过A*搜索算法寻找一条避障路径的避障路径规划方法。为了得到C-空间映射，首先进行建模。根据采摘机器人机械手的特点，其大臂、小臂以及末端执行器可简化为一个平面3R(3个旋转型关节)型串联机械手；位于采摘平面的障碍物就可以简化为：圆形障碍物，是指在采摘平面内的未成熟果实；直线段障碍物是指茎干或枝干在采摘平面上的投影，而矩形障碍物是指组成矩形多边形的茎干或枝干在采摘平面上的投影。为了简化计算，本章把障碍物模型全部简化为圆形障碍物，机械手模型各关节以中轴线建模，把其厚度w并入障碍物进行建模，简化为直线段。以大臂关节为例，如图1.3所示，大臂关节简化为长度为L1的直线段，虚线段为另一极限碰撞位姿，半径为r的阴影部分为圆形障碍物；为了补偿关节的厚度，把机械手的厚度w加入圆形障碍物的半径，R即模拟的最终用于计算的直径，以此建立障碍物模型。如此类推建立长细条的多边形障碍物或矩形障碍物，如图5.6所示，建立多个圆形障碍物进行建模，因此需要导出机械手与圆形障碍物的碰撞极限角的公式。a圆形障碍物简化模型b矩形障碍物简化模型图1.3求出上述极限角后进行C空间障碍建模，如图1.4所示图1.4一个圆形障碍物在三维C-空间的映射当建立好障碍物在C-空间的映射后，利用A*搜索算法在该空间内搜索一条从始点到终点的无障碍路径，完成避障规划。1.4使用A*算法实现无障碍路径搜索通过上节的计算，得出了三个关节角的碰撞区间，即可完成障碍物在C-空间的映射；建立了三维的C-空间，进行C-空间障碍的栅格化表示，以及将C-空间的栅格信息利用合适的数据结构描述后，利用A*搜索算法实现路径搜索。A*搜索算法步骤为：1)确定C-空间坐标系OXYZ、初始位姿和目标位姿2)计算坐标变换矩阵，把初始位姿和目标位置变换到新的坐标系中3)在新的坐标系中实施平面A*搜索算法，计算出一系列路径点4)把计算出的路径点通过坐标变换矩阵的逆矩阵，反算在OXYZ坐标系中的个关节角的变量通过坐标变换后的C-空间进行平面搜索，把搜索结果按照坐标变换逆变换返回三维C-空间的三个关节的角变量，这样大大减少了在三维搜索空间搜索的节点，减少了计算量。图1.5基于C-空间的障碍空间映射仿真算法),,(AAAzyx),,(BBBzyxT1T开始以5°为最小单位离散化关节角为I段设置J个关节的参数和K个障碍物的参数置k=1，j=1k=K+1?是结束置i0=0否以关节j为对象计算位置i1是否碰撞碰撞否？否置i0=i0+1i0=I?是保存关节j的碰撞角调用递归子程序是否图1.6C空间避障搜索1.5小结本章节针对RB03采摘机器人的空间避障行为进行了三方面的研究：首先对其工作空间进行了分析，由于工作空间有限，为了实现有效采摘，对其适采空间进行分析和建模，提高了其采摘效率；其次进行了工作空间的避障规划建模与仿真，分别对机械手臂和障碍物模型化，建立了基于关节变量的C-空间，利用A*算法在C-空间搜索无碰撞路径，提高了其避障能力。参考文献［1］彭礼辉.机械臂运动学与路径规划研究［D］.湖南：湖南工业大学，2012［2］姚立健.茄子收获机器人视觉系统和机械臂避障规划研究［D］.南京：南京林业大学，2008［3］戈志勇.番茄采摘机械手运动学仿真与避障算法研究［D］.镇江：江苏大学，2007附录注：本论文部分M文件（1）机器人工作空间仿真symsx1x2x3x4x5x6z1z2z3z4z5z6a=[];b=[];c=[];N=10000;z1=link([pi/21550380],'sta');z2=link([036000],'sta');z3=link([pi/210000],'sta');z4=link([-pi/200365],'sta');z5=link([pi/2000],'sta');z6=link([00015000],'sta');r=robot({z1z2z3z4z5z6});x1=(-170+340*rand(N,1))*pi/180;x2=(-60+210*rand(N,1))*pi/180;x3=(-170+245*rand(N,1))*pi/180;x4=(-190+380*rand(N,1))*pi/180;x5=(-125+250*rand(N,1))*pi/180;x6=(-360+720*rand(N,1))*pi/180;q=[x1x2x3x4x5x6];t=fkine(r,q);forn=1:Na=[t(1,4,n)];b=[t(2,4,n)];c=[t(3,4,n)];if(c=0)continue;endplot3(a,b,c,'+');holdonendplot3(a,b,c)gridonholdofffigure(2);a2=360;a3=100;a4=365;a5=150;xx=155+a2*cos(x2)+a3*cos(x2+x3)+a4*sin(x2+x3)+a5*sin(x2+x3+x5);zz=380+a2*sin(x2)+a3*sin(x2+x3)-a4*cos(x2+x3)-a5*cos(x2+x3+x5);forn=1:Na=xx(n);b=zz(n);plot(a,b,'+')holdonendgridon（2）求机器手空间极限角clearclcxy=[];grid.unit=5;grid.num=360/grid.unit;grid.array=zeros(grid.num,grid.num,grid.num);grid.temp=[];grid.draw=zeros(grid.num,grid.num,grid.num);arm.L=0;%臂长arm.W=0;%臂宽的一半arm.R=0;%障碍物的半径和手臂宽之和，用于计算arm.theta_limit_up=0;%上下左右4个极限角arm.theta_limit_down=0;arm.theta_limit_left=0;arm.theta_limit_right=0;obstacle.x=0;obstacle.y=0;obstacle.z=0;obstacle.r=0;arm_dabi.L=360;arm_dabi.W=50;arm_xiaobi.L=515;arm_xiaobi.W=50;obs.x=315;obs.y=150;obs.z=530;obs.r=10;origin.x=155;%关节2中心点坐标origin.y=0;origin.z=380;[arm_dabiobs]=cal_theta_limit(origin,arm_dabi,obs);子文件cal_theta_limit.mfunction[armobstacle]=cal_theta_limit(origin,arm,obstacle)arm.R=arm.W+obstacle.r;obstacle.D=sqrt((obstacle.x-origin.x)^2+(obstacle.y-origin.y)^2+(obstacle.z-origin.z)^2);ifobstacle.Dsqrt(arm.L^2+arm.W^2)+obstacle.r;arm.theta_limit_up=nan;arm.theta_limit_down=nan;arm.theta_limit_left=nan;arm.theta_limit_right=nan;elseifobstacle.D=sqrt(arm.L^2+arm.W^2)+obstacle.r&&...obstacle.Dsqrt(arm.L^2+arm.R^2)temp=acos((arm.L^2+obstacle.D^2-arm.R^2)/(2*arm.L*obstacle.D))*180/pi;temp1=atan((obstacle.y-origin.y)/(obstacle.x-origin.x))*180/pi;arm.theta_limit_left=temp1+temp;arm.theta_limit_right=temp1-temp;temp2=atan((obstacle.z-origin.z)/(obstacle.x-origin.x))*180/pi;arm.theta_limit_up=temp2+temp;arm.theta_limit_down=temp2-temp;elsetemp=asin(arm.R/obstacle.D)*180/pi;temp1=atan((obstacle.y-origin.y)/(obstacle.x-origin.x))*180/pi;arm.theta_limit_left=temp1+temp;arm.theta_limit_right=temp1-temp;temp2=atan((obstacle.z-origin.z)/(obstacle.x-origin.x))*180/pi;arm.theta_limit_up=temp2+temp;arm.theta_limit_down=temp2-temp;endend