乐高机械臂运动建模分析控制

.机械臂运动控制实验一、实物搭建利用搭建图纸搭好底座部分之后，搭建相应的臂。搭建时确保相邻关节中心与连杆在一条直线上，方便分析与计算。最终搭建图1所示。图1二、建模分析利用D-H方程对机器臂进行分析，首先对于机器臂每个关节建立坐标系如图2所示。图2D-H参数表#da0-11d10901-220d202-H30d30将D-H参数代入相应的A矩阵，可得到机器人的正运动学方程如下：A1=cos(1)sin(1)00 0010 sin(1)-cos(1)00 00d11, A2=cos(2)sin(2)00 -sin(2)cos(2)00 0010 d2cos(2)d2sin(2)01,A3=cos(3)sin(3)00 -sin(3)cos(3)00 0010 d3cos(3)d3sin(3)01.则根据机器人的正运动学方程为：TH=A1*A2*A3。即TH=cos1sin100 0010 sin1-cos100 00d11*cos2sin200 sin2cos200 0010 d2cos2d2sin201*cos3sin300 sin3cos300 0010 d3cos3d3sin301=cos2+3*cos1cos2+3*sin1sin2+30 sin2+3*cos1sin2+3*sin1cos2+30 sin1-cos100 cos1*(d3cos2+3+d2cos2)sin1*(d3cos2+3+d2cos2)d1+d3sin2+3+d2sin2)1 设机器臂末端最终期望的位置及姿态为：pnoa=nx ny nz 0ox oy oz 0ax ay az 0noa1逆运动学求解过程中，注意到该机器人只有三个自由度，因而它的最终姿态是无法控制的，故只能利用矩阵最后一列的分量来进行运算求解。在等式A1*A2*A3=pnoa两边同时左乘A1-1,使1从2和3中解耦得：A2*A3=A1-1*pnoa,即cos2+3sin2+300 sin2+3cos2+300 0010 d3*cos2+3+d2*cos2d3*sin2+3+d2*sin201=nxcos1+ny sin(1) nz nxsin1-ny cos(1) 0oxcos1+oy sin(1) oz oxsin1-oy cos(1) 0axcos1+ay sin(1) az axsin1-ay cos(1) 0ncos1+o sin(1) a-d1nsin1-o cos(1) 1由方程两边矩阵分量（3，4）相等得1=atan(on)由方程两边矩阵分量（1，4）和（2，4）得2d22+2d32+2d2d3cos3=(ncos1+o sin(1)2+(a-d1)2即3=-arccos(a-d1)2+(ncos1+o sin(1)2-d22-d322d2d3)在等式A2*A3=A1-1*pnoa两边同时左乘A2-1，即A3=A2-1*A1-1*pnoa.利用方程等式两边矩阵分量（1，4）和（2，4）得a-d1*sin2+n cos1+o sin1cos2=d3cos3+d2,a-d1*cos2-n cos1+o sin1sin2=d3sin3联立方程组得2=arccos(n cos1+o sin1d3cos3+d2+a-d1d3 sin(3)(a-d1)2+(n cos1+o sin1)2)综上可得，机器臂的逆运动学方程为1=atan(on)3=-arccos(a-d1)2+(ncos1+o sin(1)2-d22-d322d2d3)2=arccos(n cos1+o sin1d3cos3+d2+a-d1d3 sin(3)(a-d1)2+(n cos1+o sin1)2)三、编程求解及实现在实验中关于距离的测量利用了乐高机器人中的15个孔的长杆，以一个孔为单位长度，则可获得相应的距离值d1=11.5,d2=20,d3=21.5.以期望位置n=20,o=15,a=0为例，运行MATLAB程序：d1=11.5;d2=20;d3=21.5;n=20;o=15;a=0;/以坐标（20，15，0）为例th1=atan(o/n)th3=-acos(a-d1)2+(n*cos(th1)+o*sin(th1)2-d22-d32)/(2*d2*d3)th2=acos(n*cos(th1)+o*sin(th1)*(d3*cos(th3)+d2)+(a-d1)*d3*sin(th3)/(a-d1)2+(n*cos(th1)+o*sin(th1)2)根据求出的各角度值，为了能够在乐高机械臂上实现位置的移动，由于前两个关节由齿轮、涡轮传动，因而需要求出各关节相应的等效传动比。根据相应的齿轮参数，得到关节一的传动比为1：72，关节二的传动比为1：168。在实际运行中发现当关节一转动过程中，由于关节二的齿轮固连在关节一上，因而发生相对运动，根据相应的齿数比计算发现，转动过程有2=13*1。利用如下步骤即可将所求角度值转变为伺服电机应转动的角度值。th2=th2-1/3*th1;th1=168*th1*180/pi;th2=72*th2*180/pi;th3=th3*180/pi; 根据计算得到的伺服电机A、B、C应该转过的角度th3、th2、th1。利用bricxcc编程如下，驱动电机转动相应的角度即可。注意在运行过程中需保持杆d2和d3保持平行，杆d1与d2垂直。如图3所示。图3task main() RotateMotor(OUT_C, 50, 6194); RotateMotor(OUT_B, 50, 997); RotateMotor(OUT_A, 50, 97);当运行到期望的坐标点时，如果继续选择另一个点，则可以重新利用MATLAB命令计算出新的转动角度，与之前转动角度相减，则可得到从当前状态移动到新的预期位置各个关节所需转动的角度。某些情况下，虽然求得各个关节应当转动的角度，但由于实际地面的约束，会使实际转动过程中无法达到相应的位置。对于这种情况，首先进行判断出现的条件，再进行具体的处理。如可转动第二个关节时多转动一定的角度，然后再转动第三个关节之后再反向转动相应的角度保证达到期望的目标状态。四、误差分析实验过程中发现，每次运行NXT程序使机械手到达期望位置时，都会存在一定的偏差。误差的范围约在一个单位长度范围内。期望坐标点第一次实验第二次实验第三次实验（20，15）（21，15.5）（20，16）（20.5，15.5）（10，10）（10.5，9.5）（11.5，10.5）（11，9.5）（7，16）（7，15）（7，16）（7.5，16）分析造成误差的主要原因