机器人基础之运动学

旋转关节的连杆运动的传递

角速度传递关系

理论公式

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  
  • 1
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值