空间坐标系坐标变换及matlab代码实现

1.前言

下文中描述坐标系使用了英文的简写
OS:Origin Space 原始空间坐标系
NS:New Space 新空间坐标系

2.坐标变换介绍

为了解决这个问题,可以利用机器人学中的位姿变换来进行描述,以下依次介绍坐标平移、坐标旋转和刚体的一般变换,再介绍齐次矩阵的使用(如果有了基础可以直接跳过这一部分),最后回过头来解决上面的问题。

2.1 坐标平移

空间坐标系坐标变换及matlab代码实现_第1张图片

描述:坐标系NS和坐标系OS有相同的方位,但是NS的坐标原点和OS的不重合。

由矢量的平行四边形法则,一点 A A A在两个坐标系NSOS中的描述 N S p A ^{NS}\mathbf{p}_{A} NSpA O S p A ^{OS}\mathbf{p}_{A} OSpA满足以下的关系:
N S p A = O S p A + N S p O S − O ^{NS}\mathbf{p}_{A} =^{OS}\mathbf{p}_{A} +^{NS}\mathbf{p}_{OS-O} NSpA=OSpA+NSpOSO

式中:

N S p A ^{NS}\mathbf{p}_{A} NSpA A A A在坐标系NS中的坐标
O S p A ^{OS}\mathbf{p}_{A} OSpA A A A在坐标系OS中的坐标
N S p O S − O ^{NS}\mathbf{p}_{OS-O} NSpOSO:坐标系OS原点在坐标系NS中的坐标

2.2 坐标旋转

空间坐标系坐标变换及matlab代码实现_第2张图片
描述:坐标系NS和坐标系OS有相同的坐标原点,两者的姿态不同,用旋转矩阵 O S N S R _{OS}^{NS}\mathbf{R} OSNSR来描述坐标系OS相对于NS的姿态,同一点 A A A在两个坐标系NSOS中的描述 N S p A ^{NS}\mathbf{p}_{A} NSpA O S p A ^{OS}\mathbf{p}_{A} OSpA满足以下的关系:
N S p A = O S N S R O S p A ^{NS}\mathbf{p}_{A}=_{OS}^{NS}\mathbf{R}^{OS}\mathbf{p}_{A} NSpA=OSNSROSpA

式中:

N S p A ^{NS}\mathbf{p}_{A} NSpA A A A在坐标系NS中的坐标
O S p A ^{OS}\mathbf{p}_{A} OSpA A A A在坐标系OS中的坐标
O S N S R = [ N S x O S N S y O S N S z O S ] _{OS}^{NS}\mathbf{R}=[^{NS}\mathbf{x_{OS}} \quad ^{NS}\mathbf{y_{OS}} \quad^{NS}\mathbf{z_{OS}}] OSNSR=[NSxOSNSyOSNSzOS],描述坐标系OS相对于NS的方位
【其中 N S x O S ^{NS}\mathbf{x_{OS}} NSxOS N S y O S ^{NS}\mathbf{y_{OS}} NSyOS N S z O S ^{NS}\mathbf{z_{OS}} NSzOS分别是坐标系OS三个坐标轴对应的单位矢量 ( 1 , 0 , 0 ) T (1,0,0)^T (1,0,0)T ( 0 , 1 , 0 ) T (0,1,0)^T (0,1,0)T ( 0 , 0 , 1 ) T (0,0,1)^T (0,0,1)T在坐标系NS中的坐标表示】

旋转矩阵的性质(这里不介绍具体的推导,机器人学的书有讲的)只讲结论:

① N S O S R = O S N S R − 1 = O S N S R T ② d e t ( N S O S R ) = 1 \begin{aligned} &①_{NS}^{OS}\mathbf{R}=_{OS}^{NS}\mathbf{R}^{-1}=_{OS}^{NS}\mathbf{R}^T\\&②det(_{NS}^{OS}\mathbf{R})=1 \end{aligned} NSOSR=OSNSR1=OSNSRTdet(NSOSR)=1

2.3 刚体的一般变换

空间坐标系坐标变换及matlab代码实现_第3张图片
描述:坐标系NS和坐标系OS坐标原点不同,两者的姿态也不同,一点 A A A在两个坐标系NSOS中的描述 N S p A ^{NS}\mathbf{p}_{A} NSpA O S p A ^{OS}\mathbf{p}_{A} OSpA满足以下的关系:
N S p A = O S N S R O S p A + N S p O S − O ^{NS}\mathbf{p}_{A} =_{OS}^{NS}\mathbf{R}^{OS}\mathbf{p}_{A} +^{NS}\mathbf{p}_{OS-O} NSpA=OSNSROSpA+NSpOSO

式中:

N S p A ^{NS}\mathbf{p}_{A} NSpA A A A在坐标系NS中的坐标
O S p A ^{OS}\mathbf{p}_{A} OSpA A A A在坐标系OS中的坐标
O S N S R = [ N S x O S N S y O S N S z O S ] _{OS}^{NS}\mathbf{R}=[^{NS}\mathbf{x_{OS}} \quad ^{NS}\mathbf{y_{OS}} \quad^{NS}\mathbf{z_{OS}}] OSNSR=[NSxOSNSyOSNSzOS],描述坐标系OS相对于NS的方位
N S p O S − O ^{NS}\mathbf{p}_{OS-O} NSpOSO:坐标系OS原点在坐标系NS中的坐标

推导实际上是引入一个中间过渡坐标系NS',把一般变换看作是坐标旋转和坐标平移的共同结果,推导略去,读者感兴趣可以自行推导一下,也可以看看机器人学的相关书籍。

2.4 齐次变换

为了统一坐标平移和旋转变换,定义齐次变换矩阵
T = [ O S N S R N S p O S − O 0 1 ] \mathbf{T}=\begin{bmatrix}_{OS}^{NS}\mathbf{R}&^{NS}\mathbf{p}_{OS-O}\\ 0&1\end{bmatrix} T=[OSNSR0NSpOSO1]

这样,3.3中的复合变换可以写成

[ N S p A 1 ] = [ O S N S R N S p O S − O 0 1 ] [ O S p A 1 ] = T [ O S p A 1 ] \begin{bmatrix}^{NS}\mathbf{p}_{A}\\1\end{bmatrix}=\begin{bmatrix}_{OS}^{NS}\mathbf{R}&^{NS}\mathbf{p}_{OS-O}\\ 0&1\end{bmatrix}\begin{bmatrix}^{OS}\mathbf{p}_{A}\\1\end{bmatrix}=\mathbf{T}\begin{bmatrix}^{OS}\mathbf{p}_{A}\\1\end{bmatrix} [NSpA1]=[OSNSR0NSpOSO1][OSpA1]=T[OSpA1]

我们的目标就是求取旋转矩阵 O S N S R _{OS}^{NS}\mathbf{R} OSNSR和平移矢量 N S p O S − O ^{NS}\mathbf{p}_{OS-O} NSpOSO

现在我们已经搞清楚坐标系变换的概念了,那么下面这个问题你能试着解决吗?先不看具体怎么解,自己做做看?

3.问题描述

如图,给定坐标系NS的三个坐标轴单位向量在坐标系OS中的坐标(两两正交),以及NS的原点在坐标系OS中的坐标,现已知坐标系OS中点A的坐标,求在坐标系NS中的点A的坐标表示。
空间坐标系坐标变换及matlab代码实现_第4张图片

4.问题分析

由题目分析,我们知道的条件有:
坐标系NS的三个坐标轴单位向量在坐标系OS中的坐标(两两正交)—— N S O S R _{NS}^{OS}\mathbf{R} NSOSR

NS的原点在坐标系OS中的坐标—— O S p N S − O ^{OS}\mathbf{p}_{NS-O} OSpNSO

坐标系OS中点A的坐标—— O S p A ^{OS}\mathbf{p}_{A} OSpA

列出3.4中的公式: [ N S p A 1 ] = [ O S N S R N S p O S − O 0 1 ] [ O S p A 1 ] \begin{bmatrix}^{NS}\mathbf{p}_{A}\\1\end{bmatrix}=\begin{bmatrix}_{OS}^{NS}\mathbf{R}&^{NS}\mathbf{p}_{OS-O}\\ 0&1\end{bmatrix}\begin{bmatrix}^{OS}\mathbf{p}_{A}\\1\end{bmatrix} [NSpA1]=[OSNSR0NSpOSO1][OSpA1]

我们需要求的是

O S N S R _{OS}^{NS}\mathbf{R} OSNSR N S p O S − O ^{NS}\mathbf{p}_{OS-O} NSpOSO

①求 O S N S R _{OS}^{NS}\mathbf{R} OSNSR

这个问题很简单,由旋转矩阵的定义,可以知道,转置一下即可

O S N S R = N S O S R T _{OS}^{NS}\mathbf{R}=_{NS}^{OS}\mathbf{R}^T OSNSR=NSOSRT

②求 N S p O S − O ^{NS}\mathbf{p}_{OS-O} NSpOSO
写出表达式
N S p A = O S N S R O S p A + N S p O S − O ^{NS}\mathbf{p}_{A} =_{OS}^{NS}\mathbf{R}^{OS}\mathbf{p}_{A} +^{NS}\mathbf{p}_{OS-O} NSpA=OSNSROSpA+NSpOSO

A A A为坐标系NS的原点,
N S p N S − O = O S N S R O S p N S − O + N S p O S − O ^{NS}\mathbf{p}_{NS-O} =_{OS}^{NS}\mathbf{R}^{OS}\mathbf{p}_{NS-O} +^{NS}\mathbf{p}_{OS-O} NSpNSO=OSNSROSpNSO+NSpOSO

左边 N S p N S − O ^{NS}\mathbf{p}_{NS-O} NSpNSO很显然坐标系原点在坐标系NS中为零向量。故而
⇒ 0 = O S N S R O S p N S − O + N S p O S − O ⇒ N S p O S − O = − O S N S R O S p N S − O ⇒ N S p O S − O = − N S O S R T ⋅ O S p N S − O \begin{aligned}&\Rightarrow \mathbf{0} =_{OS}^{NS}\mathbf{R}^{OS}\mathbf{p}_{NS-O} +^{NS}\mathbf{p}_{OS-O} \\& \Rightarrow ^{NS}\mathbf{p}_{OS-O}=-_{OS}^{NS}\mathbf{R}^{OS}\mathbf{p}_{NS-O}\\&\Rightarrow ^{NS}\mathbf{p}_{OS-O}=-_{NS}^{OS}\mathbf{R}^T \cdot^{OS}\mathbf{p}_{NS-O}\end{aligned} 0=OSNSROSpNSO+NSpOSONSpOSO=OSNSROSpNSONSpOSO=NSOSRTOSpNSO

综上:
T = [ N S O S R T − N S O S R T ⋅ O S p N S − O 0 1 ] \mathbf{T}=\begin{bmatrix}_{NS}^{OS}\mathbf{R}^T&-_{NS}^{OS}\mathbf{R}^T \cdot^{OS}\mathbf{p}_{NS-O}\\ 0&1\end{bmatrix} T=[NSOSRT0NSOSRTOSpNSO1]

4.问题求解

%% 给定坐标系NS的三个坐标轴单位向量在坐标系OS中的坐标,都是归一化矢量且两两正交x0 y0 z0 ... x3 y3 z3 x y z都可以进行修改
p_NSo_in_OS=[x0 y0 z0];
XVector_NS_in_OS=[x1 y1 z1];
YVector_NS_in_OS=[x2 y2 z2];
ZVector_NS_in_OS=[x3 y3 z3];
R_NS_in_OS=[NS_XVector',NS_YVector',NS_ZVector'];
p_OSo_in_NS=-R_NS_in_OS'*p_NSo_in_OS';
p_A_in_OS=[x y z];
T=[R_NS_in_OS',p_OSo_in_NS;0,0,0,1];
p_A_in_NS=T*[p_A_in_OS';1];
p_A_in_NS=Data_Projection_ns(1:3,:);
p_A_in_NS=round(p_A_in_NS',4);%保留四位小数

测试了一系列点:

原坐标系中的坐标
空间坐标系坐标变换及matlab代码实现_第5张图片
利用上面的原理写出齐次矩阵
在这里插入图片描述
转换到新坐标系中的坐标
空间坐标系坐标变换及matlab代码实现_第6张图片

可以看到z坐标的维度都变成一样的了。是可以进行实现的。

你可能感兴趣的:(科研,MATLAB,matlab,坐标旋转,坐标平移,刚体的一般变换,齐次矩阵)