【机器人学1】刚体运动：旋转和平移变换

1. 基础知识

(1) 绕x轴旋转旋转矩阵Rot(φ,x)

(2) 绕y轴旋转旋转矩阵Rot(θ,y)

(3) 绕z轴旋转旋转矩阵Rot(φ,z)

(4) 坐标系间位置描述公式

说明：0和1分别代表坐标系0和坐标系1，R01表示坐标系1相对于坐标系0的旋转矩阵，p1表示p点在坐标系1下的坐标，p0点表示p点在坐标系0下的坐标。

(5) 旋转矩阵求逆

由正交性质，得到旋转矩阵的逆于转置相等，即：

2.matlab仿真

(1) 绕圆运动

问题描述：坐标系O1在O0中的初始位置如下图，此后坐标系O1在圆周P上运动，运动过程中其x轴始终指向圆心，求固连在坐标系O1上的矩形ABCD的实时位姿。
说明：可用于底盘运动过程中装在底盘上的狙击枪始终瞄准目标物体的操作

仿真实例
 
clc;
clear all;

% 矩形在基坐标系中的位置A(-1,-1),B(1,-1),C(1,1),D(-1,1)
A_O=[-1;-1;0];
B_O=[1;-1;0];
C_O=[1;1;0];
D_O=[-1;1;0];

p0_x=[]; % 圆 x 
p0_y=[]; % 圆 y 
r=15; % 圆半径

figure(1);

for i=0:1:100
   theta=-pi+i/100*2*pi;
   p0_x=[p0_x;40+r*cos(theta)];
   p0_y=[p0_y;60+r*sin(theta)];
   O1=[p0_x(end);p0_y(end);0];
   R1_0=[1 0 0;0 1 0;0 0 1]*Rot(theta+pi,'z');
   A_p=R1_0*A_O+O1;
   B_p=R1_0*B_O+O1;
   C_p=R1_0*C_O+O1;
   D_p=R1_0*D_O+O1;
   hold off;
   plot([A_p(1);B_p(1)],[A_p(2);B_p(2)],'k');
   hold on;
   plot([A_p(1);B_p(1)],[A_p(2);B_p(2)],'k');
   plot([B_p(1);C_p(1)],[B_p(2);C_p(2)],'k');
   plot([C_p(1);D_p(1)],[C_p(2);D_p(2)],'k');
   plot([D_p(1);A_p(1)],[D_p(2);A_p(2)],'k');
   plot(p0_x,p0_y,'r');
   axis([0,100,0,100]);
   axis equal;
   pause(0.1);
end

%% 旋转矩阵
function R=Rot(theta,ch)
% @brief: 绕某个轴的旋转矩阵的求法
% @param: theta,绕ch轴旋转的角度；ch,x、y、z中的某个轴
% @ret: 绕 ch 轴的旋转矩阵
% @birth: created by MY on 20200218
c=cos(theta);
s=sin(theta);
switch(ch)
    case'x'
        R=[1,0,0;0,c,-s;0,s,c];
    case'y'
        R=[c,0,s;0,1,0;-s,0,c];
    case'z'
        R=[c,-s,0;s,c,0;0,0,1];
end

(2) 遗留问题

将坐标轴O1’实时显示，实现如下效果：

附：代码链接

3.问题汇总

(1)For循环次数的具体多少对什么有影响？

循环次数越多，点数越密集，将代码里面绘制圆的部分改成这样：

(2)theta=-pi+i/1002pi 这个theta公式是怎么得来的？

起点在O1点，O1点是起点，起点对应的角度为180°(-180°)，所以要对应到

(3)p0_x=[p0_x;40+r*cos(theta)]; 方括号中的p0_x;有什么意义？

这个是矩阵的扩充，它的作用相当于下面:p0_x(end+1)= 40+r*cos(theta)

(4)R1_0=[1 0 0;0 1 0;0 0 1]*Rot(theta+pi,‘z’); 圆括号中的,'z’是什么意思？看你上面的公式中并没有,‘z’

旋转变换的公式，绕z轴旋转，二维平面的转动相当于绕z轴转

(5)plot([A_p(1);B_p(1)],[A_p(2);B_p(2)],‘k’); 其中的,'k’是什么意思？

颜色

(6)代码中的hold off 和 hold on;分别表示延迟和保持，在代码循环中有什么具体作用？去除可以吗？

不可以，hold on 是绘制矩形用的，因为绘制矩形的时候是一条线一条线用plot命令绘制的，如果没有 hold 每次绘制都会清除上一次绘制的线段。hold off相当于下次绘制的时候清除上一次绘制的矩形，如果没有这个会保持上一次绘制的内容，效果如下：

(7)pause(0.1); 这个在代码循环中有什么具体作用？去除可以吗？

不能，延时0.1s,去除了无法看到动画

(8)% @birth: created by MY on 20200218 这个是什么意思？

这个是注释，我写的函数用的是这个注释格式

(9)为什么2次用plot？scax放置的位置有什么说法？

两个坐标轴当然要用两个plot ,一次相当于绘制一个坐标轴(也就是一条直线)
scax就是个放大系数,不用太纠结，就是在原来的坐标位置上放大一个比例，如果是1的话会出现字在线段上，不美观

长期从事机器人学相关研究，涉及机械臂、轮式机器人、四足机器人的建模及仿真，可共同探讨机器人相关问题，可指导课程设计及毕业设计，详询QQ：3531225003。