【某六自由度机械手运动学建模与分析案例3400字】

1 5 12刚体的位姿表示一一位置描述为机械手的大小和形状都不变。在笛卡尔坐标系下，空间中任何一点P相对于直表示，其中Px、Py、P₂为点P在坐标系{A}中的三个坐标刚体的位姿表示一一姿态描述在笛卡尔坐标系下，可以利用固定于物体的坐标系描述方位，方位又称为姿态。在刚体B上设置直角坐标系{B},利用与{B}的坐标轴平行的三个单位矢量和表示刚体B在基坐标系中的姿态，则可得到表示姿态的旋转矩阵如下：式中：可以证明，旋转矩阵AR,是单位正交矩阵，刚体的位姿表示一一位姿描述刚体的位置与姿态简称为位姿。相对于参考坐标系{A},坐标系{B}的原点位置和坐标轴的方位可以由位置矢量和旋转矩阵描述。为便于运算，将刚体B坐标变换——平移坐标变换如图6所示，坐标系{B}是坐标系{A}经过平移得到的，即两个坐标系的坐式中，AP为坐标系{B}相对于坐标系{A}的平移矢量。坐标变换一一旋转坐标变化如图3-4所示，坐标系{B}是由坐标系{A}经过旋坐标变换一一复合坐标变换经过坐标旋转得到坐标系{B},而坐标系{C}是坐标系{A}经过平移得到的，则空坐标变换一一齐次坐标变化将位置矢量用齐次坐标表示，则复合坐标变换可1.2机械手的运动学分析和Hartenberg提出采用一个4×4的齐次变换法矩阵来描述相邻杆件之间的空间如图3-6所示，设第i个关节的轴线为Ji,第i个连杆记为C,则连杆参数(1)连杆长度a₁:两个关节的关节轴线J;与J;+1的公垂线距离。(2)连杆扭转角α:由J;与公垂线组成平面P,Ji+1与平面P的夹角。(3)连杆偏移量d;:除第一和最后连杆外，中间连杆的两个关节轴线都有(4)关节角θ₁:关节J;的相邻两条公垂线a与a;-1在以J;为法线的平面上在建立连杆坐标系时，首先选定坐标系的原点0,然后选择Z轴和X,轴，(1)原点0:取关节轴线J;与J;+1的公垂线在J;+的交点为坐标系原点。(3)X轴：取关节轴线J;与J;+1的公垂线指向0的方向为X轴方向。再沿Z;₋1轴平移d;,然后沿X;轴平移a,最后绕X;轴旋转a角度，使得坐标系A=Rot(Z₋1,0)Trans(0,0,d,)Trans(a,0,0)Rot(X;,α;)关节角θ;扭转角α;连杆长度a₁偏移量d;100020030040050根据表3-1和式(3-14),将连杆参数值代入两个坐标系间的变换矩阵中可由此，可得到机械手的总变换矩阵为T,由式(3-17)表示p为机械手在基准坐标系中的坐标位置。参考表3-1机械手各关节D-H参数，取a₃=a₄=0.1、d₅=0.065,机械手模%该程序是依据D-H表示法建立机械手的对象模型%theta:D-H参数%d:D-H参数%a:D-H参数%alpha:D-H参数%sigma:默认0,旋转关节；1,移动关节L1=Link([0,0,0,0],'standard);%连杆1坐标系参数L2=Link([0,0,0,pi/2],'standard');%连杆2坐标系参数L3=Link([0,0,0.1,0],'standard');%连杆3坐标系参数L4=Link([0,0,0.1,0],'standard');%连杆4坐标系参数L5=Link([0,0.065,0,pi/2],'standard');%连杆5坐标系参数L6=Link([0,0,0,0],'standard);%连杆6坐标系参数robot=SerialLink([L1L2L3L4L5L6]);形如图3-8所示。00X-0.41.4几何分析法通过D-H法建立数学模型需要进行一系列的矩阵变换才能计算出系统的末端姿态。但是STM32的内存资源有限、计算速度较慢，实时性较差，况且使用C语言并无矩阵运算相关函数实现起来较为麻烦。基于以上原因，本项目的运动上图中j0、j1、j2、j3是指4个舵机转动的角度，L1、L2、L3指三节手臂的长度。末端执行器(夹子)中心的坐标为逆解目标(x,y,z)。简化为剔除姿态，x=(L1*sin(j1)+L2*sin(j1+j2)+L3*sin(j1+j2+j3))y=(L1*sin(j1)+L2*sin(j1+j2)+L3*sin(j1+j2+j3))*sz=L1*cos(j1)+L2*cos(j1+j2)+L3*cos“4+1”运动学求逆解，在只考虑位置的情况下一个位置能有很多解(三个方程四个未知数，未知数的个数大于方程的个数),先确定一个未知数的取值，底座舵机j0=atan2(y,x);先确定j1的值，再求解方程。但是所确定的j1的值不一定能求出解，也可1/先确定j1的值，并且把j1从-90°到90°都取一遍，算出所有有可能的解{j3=acos(pow(a,2)+pow(b,2)+pow(L1,2)-pow(L2,2)-pow(L3,2)-2*a*L1*sin(j1)-2*b*L1*cos(j1))/(//确定j1的角度后，就可以求出j3的角度。m=L2*sin(j1)+L3*sin(j1)*cos(j3)+L3*cos(j1)*sn=L2*cos(j1)+L3*cos(j1)*cos(j3)-L3*sin(j1)*sp=pow(pow(n,2)+pow(m,2),0.5j2=asin(t/p)-q;//求出4个角度后，我们先验算一遍这4个角度求出的值(x1,y1,z1)和我们的目x1=(L1*sin(j1)+L2*sin(j1+j2)+L3*sin(j1+j2+j3))y1=(L1*sin(j1)+L2*sin(j1+j