9_机械臂运动学_正解C++推导验证
1.前置知识
1.1 D-H矩阵表达式(改进型)
相邻连杆间坐标系变换通式：
R = Rot T = Trans
i-1iT = Rx(αi-1)Tx(ai-1)Rz(θi)Tz(di) (9-1)
i-1iT = Screwx(ai-1, αi-1 )Screwz(di, θi)
ScrewQ(r, φ)代表沿Q轴平移r，再绕Q轴旋转角度φ的组合变换。由矩阵连乘计算表达式(9-1)得i-1iT的一般表达式为：
1.2 连续的连杆变换
如果已经定义了连杆坐标系和相应的连杆参数，就可以直接建立运动学方程。分别计算出各个连杆变换矩阵就能得出各个连杆参数的值。把这些连杆变换矩阵连乘就能得到一个坐标系{N}相对于坐标系{0}的变换矩阵：
0NT = 01T12T23T...N-1NT
变换矩阵0NT是关于n个关节变量的函数。如果能得到机器人各个关节位置传感器的值，机器人末端连杆在笛卡尔坐标系里的位置和姿态就能通过0NT计算出来。
1.3 使用C++ Eigen库验证遨博机械臂
/* D-H参数表 */
double a[6] = {0.0, 0.0, 408.0, 376.0, 0.0, 0.0};
double al[6] = {0.0, -M_PI/2, M_PI, M_PI, -M_PI/2, M_PI/2};
double d[6] = {98.50, 121.50, 0.0, 0.0, 102.50, 94.0};
//各关节角度,单位弧度
double th[6] = {0.0, 0.0, 0.0, 0.0,0.0, 0.0,};
cos(th[i]), -sin(th[i]), 0.0, a[i],
sin(th[i])*cos(al[i]), cos(th[i])*cos(al[i]), -sin(al[i]), -sin(al[i])*d[i],
sin(th[i])*sin(al[i]), cos(th[i])*sin(al[i]), cos(al[i]), cos(al[i])*d[i],
0.0, 0.0, 0.0, 1.0;for(int i=0; i<6; i++){
Eigen::Matrix4d tmp;
tmp << cos(th[i]), -sin(th[i]), 0.0, a[i],
sin(th[i])*cos(al[i]), cos(th[i])*cos(al[i]), -sin(al[i]), -sin(al[i])*d[i],
sin(th[i])*sin(al[i]), cos(th[i])*sin(al[i]), cos(al[i]), cos(al[i])*d[i],
0.0, 0.0, 0.0, 1.0;
T = tmp;
if(0 == i) T06 = tmp;
else
T06 = LastT*T;
LastT = T06;
}程序结果：
显然和遨博机械臂对不上，不过这个数值和之前matlab仿真的结果是一致的。所以需要加上offset调整一下。
double offset[6] = {M_PI, -M_PI/2, 0.0, -M_PI/2, 0.0, 0.0};
double th[6] = {offset[0], offset[1], 0.0, offset[3], 0.0, 0.0};
验证角度为30度，并打印姿态：
double th[6] = {M_PI/6+offset[0], M_PI/6+offset[1], M_PI/6, M_PI/6+offset[3], M_PI/6, M_PI/6};
结果是正确的。
预计年前还有3篇笔记用来记录机械臂运动学6~8之间的学习记录。大概分别是刚体的转动、刚体空间转动的其他表示、齐次变换与D-H矩阵。