机器人基础之运动学
旋转关节的连杆运动的传递
角速度传递关系
理论公式
i + 1 ω i + 1 = i i + 1 R i ω i + θ ˙ i + 1 i + 1 Z i + 1 {^{i+1}}\omega_{i+1} = {^{i+1}_i}R{^i}\omega_i+\dot\theta_{i+1}{^{i+1}}Z_{i+1} i+1ωi+1=ii+1Riωi+θ˙i+1i+1Zi+1其中 θ ˙ i + 1 i + 1 Z i + 1 = [ 0 0 θ ˙ i + 1 ] \dot\theta_{i+1}{^{i+1}}Z_{i+1} = \left[ \begin{matrix} 0\\ 0 \\ \dot\theta_{i+1} \end{matrix} \right] θ˙i+1i+1Zi+1=⎣⎡00θ˙i+1⎦⎤
MATLAB实现
function angVec_Vector_New = angVelTrans(R_Matrix, angVec_Vector_Original, dTheta)
% 计算连杆角速度传递关系,由坐标系{i}向坐标系{i+1}传递
% R_Matrix:坐标系{i}相对于坐标系{i+1}的姿态矩阵;
% angVec_Vector_Original:坐标系{i}的角速度;
% dTheta:坐标系{i+1}相对于坐标系{i}的转动速度
Z = [0; 0; 1];
angVec_Vector_New = R_Matrix * angVec_Vector_Original + Z * dTheta;
线速度传递关系
理论公式
i + 1 v i + 1 = i i + 1 R ( i v i + i ω i × i p i + 1 ) {^{i+1}}v_{i+1} = {^{i+1}_i}R({^i}v_i+{^i}\omega_i \times {^i}p_{i+1}) i+1vi+1=ii+1R(ivi+iωi×ipi+1) 其中, i p i + 1 {^i}p_{i+1} ipi+1表示坐标系{i+1}的原点在坐标系{i}中的位置矢量。
MATLAB实现
function linVec_Vector_New = linVelTrans(R_Matrix, linVec_Vector_Original, angVec_Vector_Original, p_Vector)
% 计算连杆线速度传递关系,由坐标系{i}向坐标系{i+1}传递
% R_Matrix:坐标系{i}相对于坐标系{i+1}的姿态矩阵;
% linVec_Vector_Original:坐标系{i}的线速度;
% angVec_Vector_Original:坐标系{i}的角速度;
% p_Vector:坐标系{i+1}的原点在坐标系{i}中的位置矢量
linVec_Vector_New = R_Matrix * (linVec_Vector_Original + angVec_Vector_Original * p_Vector);
角加速度传递关系
理论公式
i + 1 ω ˙ i + 1 = i i + 1 R i ω ˙ i + i i + 1 R i ω i × θ ˙ i + 1 i + 1 Z i + 1 + θ ¨ i + 1 i + 1 Z i + 1 {^{i+1}}\dot\omega_{i+1} = {^{i+1}_i}R{^i}\dot\omega_i + {^{i+1}_i}R{^i}\omega_i \times \dot\theta_{i+1}{^{i+1}}Z_{i+1}+ \ddot\theta_{i+1}{^{i+1}}Z_{i+1} i+1ω˙i+1=ii+1Riω˙i+ii+1Riωi×θ˙i+1i+1Zi+1+θ¨i+1i+1Zi+1
MATLAB实现
function angAccel_Vector_New = angAccelTrans(R_Matrix, angAccel_Vector_Original, angVec_Vector_Original, dTheta, ddTheta)
% 计算连杆角加速度传递关系,由坐标系{i}向坐标系{i+1}传递
% R_Matrix:坐标系{i}相对于坐标系{i+1}的姿态矩阵;
% angAccel_Vector_Original:坐标系{i}的角加速度;
% angVec_Vector_Original:坐标系{i}的角速度;
% dTheta:坐标系{i+1}相对于坐标系{i}的转动速度;
% ddTheta:坐标系{i+1}相对于坐标系{i}的转动加速度
Z = [0; 0; 1];
angAccel_Vector_New = R_Matrix * angAccel_Vector_Original + cross(R_Matrix * angVec_Vector_Original, Z * dTheta) + Z * ddTheta;
线加速度传递关系
理论公式
i + 1 v ˙ i + 1 = i i + 1 R [ i v ˙ i + i ω ˙ i × i p i + 1 + i ω i × ( i ω i × i p i + 1 ) ] {^{i+1}}\dot v_{i+1} = {^{i+1}_i}R [ {^i}\dot v_i+{^i}\dot\omega_i \times {^i}p_{i+1} + {^i}\omega_i \times ({^i}\omega_i \times {^i}p_{i+1})] i+1v˙i+1=ii+1R[iv˙i+iω˙i×ipi+1+iωi×(iωi×ipi+1)]
MATLAB实现
function linAccel_Vector_New = linAccelTrans(R_Matrix, linAccel_Vector_Original, angAccel_Vector_Original, angVec_Vector_Original, p_Vector)
% 计算连杆线加速度传递关系,由坐标系{i}向坐标系{i+1}传递
% R_Matrix:坐标系{i}相对于坐标系{i+1}的姿态矩阵;
% linAccel_Vector_Original:坐标系{i}的线加速度;
% angAccel_Vector_Original:坐标系{i}的角加速度;
% angVec_Vector_Original:坐标系{i}的角速度;
% p_Vector:坐标系{i+1}的原点在坐标系{i}中的位置矢量
linAccel_Vector_New = R_Matrix * (linAccel_Vector_Original + cross(angAccel_Vector_Original, p_Vector) + cross(angVec_Vector_Original, cross(angVec_Vector_Original, p_Vector)));
算例
已知一六自由度机械臂的DH参数表,以及各旋转轴转动的角速度,计算坐标系{6}相对于基坐标系{0}的速度和加速度。
clear
DH_parameter = [
0, 0, 0, 0.3795;
-30, -90/180*pi, 0, -0.9058;
340, 0, 0, 0.0432;
-40, -90/180*pi, 338, 0.8626;
0, 90/180*pi,0, 1.3394;
0, -90/180*pi, 0, -1.5708;];
dTheta_ang = [3, 4, 2, 11, 14, 12];
ddTheta_ang = [4, 5, 1, 8, 7, 9];
dTheta_rad = dTheta_ang / 180 * pi;
ddTheta_rad = ddTheta_ang / 180 * pi;
angVel = [0; 0; 0];
vel = [0; 0; 0];
angAccel = [0; 0; 0];
accel = [0; 0; 0];
for i = 1 : 6
T_Matrix = calT(DH_parameter, i - 1, i);
P = T_Matrix(1:3, 4);
T_inv = calTInv(T_Matrix);
R = T_inv(1:3, 1:3);
cal_angVel = angVelTrans(R, angVel, dTheta_rad(i));
cal_vel = linVelTrans(R, vel, angVel, P);
cal_angAccel = angAccelTrans(R, angAccel, angVel, dTheta_rad(i), ddTheta_rad(i));
cal_accel = linAccelTrans(R, accel, angAccel, angVel, P);
angVel = cal_angVel;
vel = cal_vel;
angAccel = cal_angAccel;
accel = cal_accel;
end
计算得到的速度和加速度分别为:
vel =
-12.1244
10.1339
42.1290
accel =
-9.2624
10.1711
48.2447