本系列教程作者:小鱼
公众号:鱼香ROS
QQ交流群:139707339
教学视频地址:小鱼的B站
完整文档地址:鱼香ROS官网
版权声明:如非允许禁止转载与商业用途。
目录
- 《动手学机器人学》第一节:数学基础–矩阵
- 《动手学机器人学》第二节:MiniConda、JupyterNotebook与ROS2
- 《动手学机器人学》第三节:动手学数学基础,使用NumPy操作矩阵
- 《动手学机器人学》第四节(上):位姿描述
- 《动手学机器人学》第四节(下):一文搞懂坐标变换
- 《动手学机器人学》第五节(上):动手学空间姿态描述与变换
- 《动手学机器人学》第五节(中):使用ROS2的TF2进行坐标变换
- 《动手学机器人学》第五节(下):使用Python操作TF
- 更新中…欢迎关注小鱼公众号鱼香ROS,第一时间获取更新
正文
5.坐标变换
位姿是相对的,同一个物体在不同的参考坐标系下的位姿数据肯定是不同的。在后续的学习和使用当中,我们会经常需要求同一个点在不同坐标系的位姿表示,这就要求我们掌握坐标变换的方法了。
比如在手眼系统中,我们可以通过视觉算法获取到工件坐标系P
鱼相机坐标系C
之间的关系,我们想要控制机械臂的末端
运动到工件坐标系P
进行夹取,那么我们就要知道工件坐标系P
在机器人基坐标系B
下的位姿,如何获取呢?这就需要坐标进行变换了。
5.1 平移坐标变换
如图,坐标系{A}、{B}、{C}的姿态是相同的,其之间的姿态对应的旋转矩阵都是单位矩阵。在我们已知:
- {A}为参考坐标系,{B}的位置矢量 B A P = [ 1 , 1 , 1 ] T {^A_B}P=[1,1,1]^T BAP=[1,1,1]T
- {B}为参考坐标系,{C}的位置矢量 C B P = [ 3 , 0 , 1 ] T {^B_C}P=[3,0,1]^T CBP=[3,0,1]T
求:
- {A}为参考坐标系,{C}的位置矢量 C A P {^A_C}P CAP
大家可以利用简单的几何知识手算一下,再继续往下看
正确的答案应该是:
C
A
P
=
[
4
,
1
,
2
]
T
{^A_C}P=[4,1,2]^T
CAP=[4,1,2]T
算法也很简单,直接将
B
A
P
{^A_B}P
BAP鱼
C
B
P
{^B_C}P
CBP相加即可
C
A
P
=
B
A
P
+
C
B
P
=
[
1
,
1
,
1
]
T
+
[
3
,
0
,
1
]
T
=
[
4
,
1
,
2
]
T
{^A_C}P ={^A_B}P+{^B_C}P=[1,1,1]^T+[3,0,1]^T=[4,1,2]^T
CAP=BAP+CBP=[1,1,1]T+[3,0,1]T=[4,1,2]T
由此我们可以得出平移坐标变换可以求出同一个点({P})在相同姿态不同位置坐标系({A}鱼{B})下的不同表示,也可以得到坐标的平移方程
C
A
P
=
B
A
P
+
C
B
P
(坐标平移方程)
{^A_C}P ={^A_B}P+{^B_C}P \tag{坐标平移方程}
CAP=BAP+CBP(坐标平移方程)
小鱼说需要注意的是:在上述例子中,{A},{B},{C}三个坐标系的姿态都是相同的,所以
C
A
R
=
B
A
R
=
C
B
R
{^A_C}R={^A_B}R={^B_C}R
CAR=BAR=CBR,也都是是单位旋转矩阵
5.2 旋转坐标变换
如果坐标系之间姿态不同,同一个点的位姿在不同参考姿态下的位置和姿态也会不一样。
上图中,坐标系{A}鱼坐标系{B}原点重合(连名字都连一块了),我们已知:
- {B}坐标系绕{A}的z轴即 A z A_z Az旋转了 4 5 ∘ 45^\circ 45∘
- {B}鱼{C}姿态相同,{B}为参考坐标系,{C}的位置矢量为 C B P = [ 3 , 0 , 1 ] T {^B_C}P=[3,0,1]^T CBP=[3,0,1]T
求
- {A}为参考坐标系,{C}的位置矢量 C A P {^A_C}P CAP
- {A}为参考坐标系,{C}的旋转矩阵 C A R {^A_C}R CAR
这次相对来说有一些困难,不过我们依然可以手推出来,接着和小鱼的思路一起走一下
5.2.1 求 C A R {^A_C}R CAR
因为{B}鱼{C}姿态相同,所以{A}参考坐标系下{C}的姿态和{A}参考坐标系下{B}的姿态是相同的,即:
B
A
R
=
C
A
R
{^A_B}R={^A_C}R
BAR=CAR
接着只需要求
B
A
R
{^A_B}R
BAR即可,因为{B}坐标系绕{A}的z轴即
A
z
A_z
Az旋转了
4
5
∘
45^\circ
45∘,我们可以一一计算旋转矩阵各个元素的值
B
A
R
=
C
A
R
=
[
B
x
⋅
A
x
B
y
⋅
A
x
B
z
⋅
A
x
B
x
⋅
A
y
B
y
⋅
A
y
B
z
⋅
A
y
B
x
⋅
A
z
B
y
⋅
A
z
B
z
⋅
A
z
]
=
[
c
o
s
(
45
)
c
o
s
(
90
+
45
)
c
o
s
(
90
)
c
o
s
(
90
−
45
)
c
o
s
(
45
)
c
o
s
(
90
)
c
o
s
(
90
)
c
o
s
(
90
)
c
o
s
(
0
)
]
=
[
c
o
s
(
45
)
c
o
s
(
90
+
45
)
0
c
o
s
(
90
−
45
)
c
o
s
(
45
)
0
0
0
1
]
{^A_B}R ={^A_C}R = \begin{bmatrix} {B_{x}\cdot A_x} & {B_{y}\cdot A_x} & {B_{z}\cdot A_x}\\ {B_{x}\cdot A_y} & {B_{y}\cdot A_y} & {B_{z}\cdot A_y}\\ {B_{x}\cdot A_z} & {B_{y}\cdot A_z} & {B_{z}\cdot A_z}\\ \end{bmatrix} = \begin{bmatrix} {cos(45)} & cos(90+45) & cos(90)\\ {cos(90-45)} & cos(45) & cos(90)\\ {cos(90)} & cos(90) & cos(0)\\ \end{bmatrix} = \begin{bmatrix} {cos(45)} & cos(90+45) & 0\\ {cos(90-45)} & cos(45) & 0\\ {0} &0 &1\\ \end{bmatrix}
BAR=CAR=⎣⎡Bx⋅AxBx⋅AyBx⋅AzBy⋅AxBy⋅AyBy⋅AzBz⋅AxBz⋅AyBz⋅Az⎦⎤=⎣⎡cos(45)cos(90−45)cos(90)cos(90+45)cos(45)cos(90)cos(90)cos(90)cos(0)⎦⎤=⎣⎡cos(45)cos(90−45)0cos(90+45)cos(45)0001⎦⎤
将结果在出来,再利用工具来算一下,看看最终结果对不对
到此我们求出了 C A P {^A_C}P CAP鱼 C A R {^A_C}R CAR,但聪明的鱼粉肯定不会就此罢休,我们接着来举一反三
{B}绕着{A}的z轴旋转了45度我们计算出来了,那如果{B}绕着{A}旋转
θ
\theta
θ角度该如何呢?相信聪明的鱼粉心中已经有了答案
R
(
z
,
θ
)
=
[
c
o
s
θ
−
s
i
n
θ
0
s
i
n
θ
c
o
s
θ
0
0
0
1
]
(小鱼提示:重要方程1)
R(z,\theta)= \begin{bmatrix} {cos\theta} & -sin\theta & 0\\ {sin\theta} & cos\theta & 0\\ {0} &0 &1\\ \end{bmatrix} \tag{小鱼提示:重要方程1}
R(z,θ)=⎣⎡cosθsinθ0−sinθcosθ0001⎦⎤(小鱼提示:重要方程1)
那如果不是绕着z轴旋转,而是绕着x轴呢?
R
(
x
,
θ
)
=
[
1
0
0
0
c
o
s
θ
−
s
i
n
θ
0
s
i
n
θ
c
o
s
θ
]
R(x,\theta)= \begin{bmatrix} 1 & 0 & 0\\ 0 & {cos\theta} & -sin\theta \\ 0&{sin\theta} & cos\theta \\ \end{bmatrix}
R(x,θ)=⎣⎡1000cosθsinθ0−sinθcosθ⎦⎤
绕着y轴呢?
R
(
y
,
θ
)
=
[
c
o
s
θ
0
s
i
n
θ
0
1
0
−
s
i
n
θ
0
c
o
s
θ
]
R(y,\theta)= \begin{bmatrix} {cos\theta} & 0 & {sin\theta}\\ 0 &1 &0\\ {-sin\theta} & 0 &cos\theta \\ \end{bmatrix}
R(y,θ)=⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤
5.2.2 求 C A P ^A_CP CAP
现在我们已知
C
B
P
^B_CP
CBP和
B
A
R
^A_BR
BAR,那如何求
C
A
P
^A_CP
CAP呢,我们可以使用下面这个方程
C
A
P
=
B
A
R
C
B
P
(坐标旋转方程)
^A_CP = {^A_BR}{^B_CP}\tag{坐标旋转方程}
CAP=BARCBP(坐标旋转方程)
关于这个方程,其实是将
C
B
P
^B_CP
CBP在{B}上各轴的分量变换到了{A}的各轴上
B
A
R
{^A_BR}
BAR是一个
3
∗
3
3*3
3∗3的矩阵,
C
B
P
{^B_CP}
CBP是
3
∗
1
3*1
3∗1的矩阵,其相乘结果还是一个
3
∗
1
3*1
3∗1的矩阵,利用前几节学习的矩阵乘法手算或者使用numpy进行计算,得到最终结果为:
C
A
P
=
B
A
R
C
B
P
=
[
2.12
,
2.12
,
1
]
T
^A_CP = {^A_BR}{^B_CP}\ = [2.12,2.12,1]^T
CAP=BARCBP =[2.12,2.12,1]T
小思考:{C}的参考坐标系从{B}变成了{A},{C}在空间中的位置发生了变化了吗?
5.3平移旋转复合变换
在我们平时搞机(器人)时,一般情况下两个坐标系原点不重合姿态也不相同。我们将坐标变换拆分成先绕参考坐标系旋转,再绕参考坐标系平移两步,这样我们就得到了坐标的复合变换方程
C
A
P
=
B
A
R
C
B
P
+
B
A
P
^A_CP = {^A_BR}{^B_CP}+^A_BP
CAP=BARCBP+BAP
这个公式其实也很好理解,
B
A
R
C
B
P
{^A_BR}{^B_CP}
BARCBP就是将{C}在{B}上的各轴分量转换到{A}上,再和原来在{A}上各轴的分量再相加。不理解的同学也没事,后面我们使用最多的还是齐次变换矩阵,根据方程直接摝代码就行了。
6.左手还是右手
最后小鱼要说的是左手坐标系和右手坐标系,一般情况下我们都是使用右手坐标系搞机。
用右手确定的坐标系就是右手坐标系了,跟着小鱼一起做:
拿起你的右手,先给自己竖个大拇指,然后打开手掌,将大拇指的方向朝向下图中的蓝色z轴,让剩下的四根手指朝向红色的x轴,此时朝向手心外的就是绿绿的y轴的方向了。
如果你换左手,做上述动作,你会发现y轴是朝手心里的。
再放张图给肢体不协调的同学:
7.练习
光说不练假把式,我们来做个手眼转换题
如图🔓示,已知:
1.相机坐标系{C}为参考坐标系,工具坐标系{P}的位置矢量在相机坐标系{C}x,y,z
各轴投影为
2
,
1
,
2
2,1,2
2,1,2,并且工具坐标系和相机坐标系姿态相同。
2.机器人基坐标系{B}为参考坐标系,相机坐标系{C}在的位置矢量在{B}各轴的投影为 0 , 0 , 3 0,0,3 0,0,3,坐标系{C}和绕着坐标系{B}的x轴转了180度
可以参考下图看题目
求:
{B}为参考坐标系,坐标系{P}的位置矢量和旋转矩阵
答案:
位置矢量: [ 2 , − 1 , 1 ] T [2,-1,1]^T [2,−1,1]T
旋转矩阵: [ 1 0 0 0 − 1 0 0 0 − 1 ] \begin{bmatrix} 1 & 0 & 0\\ 0 & -1 & 0\\ 0 &0 & -1\\ \end{bmatrix} ⎣⎡1000−1000−1⎦⎤
参考文档
- 机器人学导论
- 机器人学基础
技术交流&&问题求助:
- 微信公众号及交流群:鱼香ROS
- 小鱼微信:AiIotRobot
- QQ交流群:139707339
- 版权保护:已加入“维权骑士”(rightknights.com)的版权保护计划
作者介绍:
我是小鱼,机器人领域资深玩家,现深圳某独脚兽机器人算法工程师一枚
初中学习编程,高中开始接触机器人,大学期间打机器人相关比赛实现月入2W+(比赛奖金)
目前在输出机器人学习指南、论文注解、工作经验,欢迎大家关注小鱼,一起交流技术,学习机器人