《动手学机器人学》笔记

news2025/5/24 16:56:05

0.介绍
1.概述｜空间位置、姿态的描述（3×3）｜《动手学机器人学》
2.（2）-Robotics Toolbox①（V10.4）
3.齐次坐标与变换矩阵
4.一般形式的旋转变换矩阵
5.（轴角法）或（Rodrigues' rotation formula）或（罗德里格旋转公式）或（一般形式的旋转变换矩阵）详细推导证明
6.空间位姿表示方法-----RPY角
7.欧拉角 | 空间位姿表示方法 |《动手学机器人学》
8.标准DH参数详解+案例分析【1】

0.介绍

原课程来源于b站劉海濤LHT，链接：【《动手学机器人学》开课了】 https://www.bilibili.com/video/BV1CY4y1W7Ry/?share_source=copy_web&vd_source=338eeada513a018955017f069032f82c

1.概述｜空间位置、姿态的描述（3×3）｜《动手学机器人学》

1.1正运动学（forward kinematics，简写FK），根据机器人的关节角度，计算末端位姿
1.2逆运动学（inverse kinematics，简写IK），根据末端位姿，反解关节角
1.3 在这里插入图片描述
1.4除了空间某点位置还需要确定空间物体方位，用旋转矩阵表示

2.（2）-Robotics Toolbox①（V10.4）

2.1安装教程可以参考这个https://blog.csdn.net/m0_71721954/article/details/145087352。注意：设置路径的时候，到MATLAB\R2024b\toolbox这一层，否则容易出现找不到命令。在这里插入图片描述
2.2 相关命令

% 二维空间姿态
>> SE2(1,2,pi/3)
 

ans = 
    0.5000   -0.8660         1
    0.8660    0.5000         2
         0         0         1
>> trplot(ans)

在这里插入图片描述

% 纯平移
>> transl2(1,2)

ans =

     1     0     1
     0     1     2
     0     0     1
% 绕x轴旋转，角度一定要用弧度制
>> rotx(pi/3)

ans =

    1.0000         0         0
         0    0.5000   -0.8660
         0    0.8660    0.5000
% 绕y轴旋转
>> roty(pi/3)

ans =

    0.5000         0    0.8660
         0    1.0000         0
   -0.8660         0    0.5000
% 绕z轴旋转
>> rotz(pi/3)

ans =

    0.5000   -0.8660         0
    0.8660    0.5000         0
         0         0    1.0000
% 齐次
>> trotx(pi/3)

ans =

    1.0000         0         0         0
         0    0.5000   -0.8660         0
         0    0.8660    0.5000         0
         0         0         0    1.0000


>> troty(pi/3)

ans =

    0.5000         0    0.8660         0
         0    1.0000         0         0
   -0.8660         0    0.5000         0
         0         0         0    1.0000

>> trotz(pi/3)

ans =

    0.5000   -0.8660         0         0
    0.8660    0.5000         0         0
         0         0    1.0000         0
         0         0         0    1.0000

>> rotx(pi/3)

ans =

    1.0000         0         0
         0    0.5000   -0.8660
         0    0.8660    0.5000
% 三维轨迹
>> trplot(ans)
% 动画演示（图略）
>> tranimate(ans)

在这里插入图片描述

3.齐次坐标与变换矩阵

3.1齐次坐标用 $n + 1$ 维来代表 $n$ 维坐标

在这里插入图片描述
3.2 引入齐次坐标的目的是，合并矩阵运算中的乘法和加法
3.3 （特别重要）矩阵的左乘和右乘的运动解释是不一样的。对于固定坐标系：变化顺序“从右向左”；对于相对坐标系：变化顺序“从左向右”。
在这里插入图片描述

在这里插入图片描述
说明这个物体在空间里做刚体运动。

4.一般形式的旋转变换矩阵

4.1 通用的旋转变换矩阵：已知旋转轴和旋转角度，得到旋转矩阵
在这里插入图片描述
4.2 已知旋转矩阵，得到旋转轴和旋转角度（过程略）

旋转角度 $\theta=arccos(...)$
旋转轴

注意：存在多解，等效轴和转角不唯一，一般角度在0度到180度

这题就能解了
在这里插入图片描述
4.3 matlab中的函数

% 已知旋转角度和旋转轴，求旋转矩阵
>> angvec2r(pi/3,[1,0,0])

ans =

    1.0000         0         0
         0    0.5000   -0.8660
         0    0.8660    0.5000

>> rotx(pi/3)

ans =

    1.0000         0         0
         0    0.5000   -0.8660
         0    0.8660    0.5000
% 齐次
>> angvec2tr(pi/3,[1,0,0])

ans =

    1.0000         0         0         0
         0    0.5000   -0.8660         0
         0    0.8660    0.5000         0
         0         0         0    1.0000

4.4 练习题
在这里插入图片描述

function [f,theta] = resolve_f_theta(T)
    nx = T(1,1); ox = T(1,2); ax = T(1,3);
    ny = T(2,1); oy = T(2,2); ay = T(2,3);
    nz = T(3,1); oz = T(3,2); az = T(3,3);

    theta = acos(0.5*(nx + oy + az - 1));

    if(theta == 0 || theta == 180)
        fprintf("error!");
    else
        fx = (oz - ay)/(2*sin(theta));
        fy = (ax - nz)/(2*sin(theta));
        fz = (ny - ox)/(2*sin(theta));

        f = [fx,fy,fz];
    end
end

>> [f,theta] = resolve_f_theta([0 1 0;0 0 -1;-1 0 0])

f =

    0.5774    0.5774   -0.5774


theta =

    2.0944

>> 2*pi/3

ans =

    2.0944

>> sqrt(3)/3

ans =

    0.5774

5.（轴角法）或（Rodrigues’ rotation formula）或（罗德里格旋转公式）或（一般形式的旋转变换矩阵）详细推导证明

5.1 一般形式旋转矩阵公式证明

在这里插入图片描述

在这里插入图片描述
这个公式的意义，空间中给定任意的向量v，绕着旋转轴和旋转角，得到旋转后的向量v‘

>> t=2*pi/3

t =

    2.0944

>> u = [sqrt(3)/3,sqrt(3)/3,-sqrt(3)/3]'

u =

    0.5774
    0.5774
   -0.5774

>> v = [1 2 3]'

v =

     1
     2
     3

>> result = cos(t)*v + (1-cos(t))*dot(u,v)*u+sin(t)*cross(u,v)

result =

    2.0000
   -3.0000
   -1.0000

进一步化简成矩阵形式
在这里插入图片描述

5.2 齐次坐标变换在这里插入图片描述

6.空间位姿表示方法-----RPY角

6.1 RPY角
在这里插入图片描述

syms alpha beta gamma T1 T2 T3 RPY

T1 = [cos(alpha) -sin(alpha) 0;sin(alpha) cos(alpha) 0; 0 0 1];

T2 = [cos(beta) 0 sin(beta);0 1 0;-sin(beta) 0 cos(beta)];

T3 = [1 0 0;0 cos(gamma) -sin(gamma); 0 sin(gamma) cos(gamma)];

RPY = T1*T2*T3;

display(RPY);

在这里插入图片描述

% 给RPY角输出旋转矩阵
>> rpy2r(pi/6,pi/3,pi/4)

ans =

    0.3536   -0.3062    0.8839
    0.3536    0.9186    0.1768
   -0.8660    0.2500    0.4330
% RPY角分别对应绕x,y,z轴旋转
>> rotz(pi/4)*roty(pi/3)*rotx(pi/6)

ans =

    0.3536   -0.3062    0.8839
    0.3536    0.9186    0.1768
   -0.8660    0.2500    0.4330
 
 >> rpy2tr(pi/6,pi/3,pi/4)

ans =

    0.3536   -0.3062    0.8839         0
    0.3536    0.9186    0.1768         0
   -0.8660    0.2500    0.4330         0
         0         0         0    1.0000
% 已知旋转矩阵得到RPY角
>> tr2rpy(ans)

ans =

    0.5236    1.0472    0.7854

>> pi/6

ans =

    0.5236

>> pi/3

ans =

    1.0472

>> pi/4

ans =

    0.7854
    
>> atan2(1,2)

ans =

    0.4636
% 转化为角度
>> atan2d(1,2)

ans =

   26.5651

6.2 在这里插入图片描述

>> T =[0.527 -0.574 0.628 3;0.369 0.819 0.439 2;-0.766 0 0.643 -4;0 0 0 1]

T =

    0.5270   -0.5740    0.6280    3.0000
    0.3690    0.8190    0.4390    2.0000
   -0.7660         0    0.6430   -4.0000
         0         0         0    1.0000

>> rpy=tr2rpy(T);
>> display(rpy)

rpy =

         0    0.8725    0.6109

>> rpy2r(rpy)

ans =

    0.5267   -0.5736    0.6274
    0.3688    0.8192    0.4393
   -0.7659         0    0.6429

7.欧拉角 | 空间位姿表示方法 |《动手学机器人学》

7.1 欧拉角
在这里插入图片描述

>> [alpha, beta, gamma] = resolve_zyz_theta([0.527 -0.574 0.628;0.369 0.819 0.439;-0.766 0 0.643])

alpha =

    0.6101


beta =

    0.8725


gamma =

     0
% 欧拉角变旋转矩阵
>> eul2r([alpha, beta, gamma])

ans =

    0.5269   -0.5729    0.6277
    0.3684    0.8196    0.4388
   -0.7659         0    0.6429

7.2 matlab常用函数

>> eul2r([pi/6 pi/4 pi/3])

ans =

   -0.1268   -0.7803    0.6124
    0.9268    0.1268    0.3536
   -0.3536    0.6124    0.7071

>> eul2tr([pi/6 pi/4 pi/3])

ans =

   -0.1268   -0.7803    0.6124         0
    0.9268    0.1268    0.3536         0
   -0.3536    0.6124    0.7071         0
         0         0         0    1.0000
% tr2eul(ans,'deg')能直接输出角度
>> tr2eul(ans)

ans =

    0.5236    0.7854    1.0472

>> ans*180/pi

ans =

   30.0000   45.0000   60.0000