[人工智能]空间坐标系坐标变换及matlab代码实现

1.前言

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

2.坐标变换介绍

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

2.1 坐标平移

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

由矢量的平行四边形法则，一点$A$在两个坐标系NS和OS中的描述${}^{NS}{\mathbf{p}}_A$和${}^{OS}{\mathbf{p}}_A$满足以下的关系：
$${}^{NS}{\mathbf{p}}_A{=}^{OS}{\mathbf{p}}_A{+}^{NS}{\mathbf{p}}_{OS?O}$$

式中：

${}^{NS}{\mathbf{p}}_A$：$A$在坐标系NS中的坐标
${}^{OS}{\mathbf{p}}_A$：$A$在坐标系OS中的坐标
${}^{NS}{\mathbf{p}}_{OS?O}$：坐标系OS原点在坐标系NS中的坐标

2.2 坐标旋转

描述：坐标系NS和坐标系OS有相同的坐标原点，两者的姿态不同，用旋转矩阵${}_{OS}^{NS}\mathbf{R}$来描述坐标系OS相对于NS的姿态，同一点$A$在两个坐标系NS和OS中的描述${}^{NS}{\mathbf{p}}_A$和${}^{OS}{\mathbf{p}}_A$满足以下的关系：
$${}^{NS}{\mathbf{p}}_A{=}_{OS}^{NS}{\mathbf{R}}^{OS}{\mathbf{p}}_A$$

式中：

${}^{NS}{\mathbf{p}}_A$：$A$在坐标系NS中的坐标
${}^{OS}{\mathbf{p}}_A$：$A$在坐标系OS中的坐标
${}_{OS}^{NS}\mathbf{R}={[}^{NS}{\mathbf{x}}_{\mathbf{O}\mathbf{S}}{}^{NS}{\mathbf{y}}_{\mathbf{O}\mathbf{S}}{}^{NS}{\mathbf{z}}_{\mathbf{O}\mathbf{S}}]$，描述坐标系OS相对于NS的方位
【其中${}^{NS}{\mathbf{x}}_{\mathbf{O}\mathbf{S}}$ ${}^{NS}{\mathbf{y}}_{\mathbf{O}\mathbf{S}}$ ${}^{NS}{\mathbf{z}}_{\mathbf{O}\mathbf{S}}$分别是坐标系OS三个坐标轴对应的单位矢量$(1,0,0{)}^T$、$(0,1,0{)}^T$、$(0,0,1{)}^T$在坐标系NS中的坐标表示】

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

$$\begin{array}{rl}& {①}_{NS}^{OS}\mathbf{R}{=}_{OS}^{NS}{\mathbf{R}}^{?1}{=}_{OS}^{NS}{\mathbf{R}}^T\\ & ②det{(}_{NS}^{OS}\mathbf{R})=1\end{array}$$

2.3 刚体的一般变换

描述：坐标系NS和坐标系OS坐标原点不同，两者的姿态也不同，一点$A$在两个坐标系NS和OS中的描述${}^{NS}{\mathbf{p}}_A$和${}^{OS}{\mathbf{p}}_A$满足以下的关系：
$${}^{NS}{\mathbf{p}}_A{=}_{OS}^{NS}{\mathbf{R}}^{OS}{\mathbf{p}}_A{+}^{NS}{\mathbf{p}}_{OS?O}$$

式中：

${}^{NS}{\mathbf{p}}_A$：$A$在坐标系NS中的坐标
${}^{OS}{\mathbf{p}}_A$：$A$在坐标系OS中的坐标
${}_{OS}^{NS}\mathbf{R}={[}^{NS}{\mathbf{x}}_{\mathbf{O}\mathbf{S}}{}^{NS}{\mathbf{y}}_{\mathbf{O}\mathbf{S}}{}^{NS}{\mathbf{z}}_{\mathbf{O}\mathbf{S}}]$，描述坐标系OS相对于NS的方位
${}^{NS}{\mathbf{p}}_{OS?O}$：坐标系OS原点在坐标系NS中的坐标

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

2.4 齐次变换

为了统一坐标平移和旋转变换，定义齐次变换矩阵：
$$\mathbf{T}=\left[\begin{array}{cc}{}_{OS}^{NS}\mathbf{R}& {}^{NS}{\mathbf{p}}_{OS?O}\\ 0& 1\end{array}\right]$$

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

$$\left[\begin{array}{c}{}^{NS}{\mathbf{p}}_A\\ 1\end{array}\right]=\left[\begin{array}{cc}{}_{OS}^{NS}\mathbf{R}& {}^{NS}{\mathbf{p}}_{OS?O}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{}^{OS}{\mathbf{p}}_A\\ 1\end{array}\right]=\mathbf{T}\left[\begin{array}{c}{}^{OS}{\mathbf{p}}_A\\ 1\end{array}\right]$$

我们的目标就是求取旋转矩阵${}_{OS}^{NS}\mathbf{R}$和平移矢量${}^{NS}{\mathbf{p}}_{OS?O}$。

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

3.问题描述

如图，给定坐标系NS的三个坐标轴单位向量在坐标系OS中的坐标(两两正交)，以及NS的原点在坐标系OS中的坐标，现已知坐标系OS中点A的坐标，求在坐标系NS中的点A的坐标表示。

4.问题分析

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

NS的原点在坐标系OS中的坐标——${}^{OS}{\mathbf{p}}_{NS?O}$

坐标系OS中点A的坐标——${}^{OS}{\mathbf{p}}_A$

列出3.4中的公式：$$\left[\begin{array}{c}{}^{NS}{\mathbf{p}}_A\\ 1\end{array}\right]=\left[\begin{array}{cc}{}_{OS}^{NS}\mathbf{R}& {}^{NS}{\mathbf{p}}_{OS?O}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{}^{OS}{\mathbf{p}}_A\\ 1\end{array}\right]$$

我们需要求的是

${}_{OS}^{NS}\mathbf{R}$，${}^{NS}{\mathbf{p}}_{OS?O}$

①求${}_{OS}^{NS}\mathbf{R}$

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

$${}_{OS}^{NS}\mathbf{R}{=}_{NS}^{OS}{\mathbf{R}}^T$$

②求${}^{NS}{\mathbf{p}}_{OS?O}$
写出表达式
$${}^{NS}{\mathbf{p}}_A{=}_{OS}^{NS}{\mathbf{R}}^{OS}{\mathbf{p}}_A{+}^{NS}{\mathbf{p}}_{OS?O}$$

令$A$为坐标系NS的原点，
$${}^{NS}{\mathbf{p}}_{NS?O}{=}_{OS}^{NS}{\mathbf{R}}^{OS}{\mathbf{p}}_{NS?O}{+}^{NS}{\mathbf{p}}_{OS?O}$$

左边${}^{NS}{\mathbf{p}}_{NS?O}$很显然坐标系原点在坐标系NS中为零向量。故而
$$\begin{array}{rl}& ?0{=}_{OS}^{NS}{\mathbf{R}}^{OS}{\mathbf{p}}_{NS?O}{+}^{NS}{\mathbf{p}}_{OS?O}\\ & {?}^{NS}{\mathbf{p}}_{OS?O}={?}_{OS}^{NS}{\mathbf{R}}^{OS}{\mathbf{p}}_{NS?O}\\ & {?}^{NS}{\mathbf{p}}_{OS?O}={?}_{NS}^{OS}{\mathbf{R}}^T{?}^{OS}{\mathbf{p}}_{NS?O}\end{array}$$

综上：
$$\mathbf{T}=\left[\begin{array}{cc}{}_{NS}^{OS}{\mathbf{R}}^T& {?}_{NS}^{OS}{\mathbf{R}}^T{?}^{OS}{\mathbf{p}}_{NS?O}\\ 0& 1\end{array}\right]$$

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);%保留四位小数

测试了一系列点：

原坐标系中的坐标

利用上面的原理写出齐次矩阵

转换到新坐标系中的坐标

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