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

机械臂运动控制实验

一、实物搭建
利用搭建图纸搭好底座部分之后，搭建相应的臂。搭建时确保相邻关节中心与连杆在一条直线上，方便分析与计算。
最终搭建图1所示。

图1

二、建模分析
利用D-H方程对机器臂进行分析，首先对于机器臂每个关节建立坐标系如图2所示。

图2

D-H参数表
#

0-1

1-2

2-H

将D-H参数代入相应的A矩阵，可得到机器人的正运动学方程如下：
,,
.

则根据机器人的正运动学方程为：。即

设机器臂末端最终期望的位置及姿态为：

逆运动学求解过程中，注意到该机器人只有三个自由度，因而它的最终姿态是无法控制的，故只能利用矩阵最后一列的分量来进行运算求解。
在等式两边同时左乘,使中解耦得：
,即
=
由方程两边矩阵分量（3，4）相等得

由方程两边矩阵分量（1，4）和（2，4）得

即

在等式两边同时左乘，即.
利用方程等式两边矩阵分量（1，4）和（2，4）得

联立方程组得

综上可得，机器臂的逆运动学方程为

三、编程求解及实现
在实验中关于距离的测量利用了乐高机器人中的15个孔的长杆，以一个孔为单位长度，则可获得相应的距离值
以期望位置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-d2^2-d3^2)/(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。在实际运行中发现当关节一转动过程中，由于关节二的齿轮固连在关节一上，因而发生相对运动，根据相应的齿数比计算发现，转动过程有。利用如下步骤即可将所求角度值转变为伺服电机应转动的角度值。

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所示。

图3

task 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）

分析造成误差的主要原因有：
1. 初始无法保持机械臂完全水平
2．齿轮之间转动无法保证准确转过已知角

3．伺服电机较重，导致机构发生较大形变
4．坐标纸放置位置与理想坐标位置有偏差
5．伺服电机本身存在空程差，尤其是最后一个关节电机会导致末端位置存在很大的浮动误差
6．三个杆的长度测量不够精确，会导致计算结果产生一定的系统误差

（注：专业文档是经验性极强的领域，无法思考和涵盖全面，素材和资料部分来自网络，供参考。可复制、编制，期待你的好评与关注）