matlab-unreal联合仿真中左手坐标系与右手坐标系中的位姿 ...

xiaozongpeng · 发表于 2021-11-23 08:44

matlab具有强大便利的运算仿真能力，支持有效仿真控制算法。unreal引擎能提供真实程度高的环境信息，支持有效仿真环境感知算法。通过matlab-unreal联合仿真，能够极大限度地针对无人机、自动驾驶汽车等应用场景，对系统进行快速高效地验证。
matlab-unreal联合仿真时，matlab中的控制算法与SLAM算法所依赖的程序库（不限于OpenGL、3D Max）均是右手系，unreal中则是左手系（除unreal外，Unity、 Direct3D均是左手系）。本次用线性变换的思路，对位姿在左右手坐标系下表达形式的相互转换进行说明。
<hr/>位姿分为平移与旋转，两者相互独立。平移可用向量表示，旋转可使用矩阵表示。在后续的表达中，向量与矩阵使用黑体正体英文字母表示，向量与矩阵的右下脚标表示左手系或右手系，如 $\textbf{P}_{l}$ 表示左手系中的一个向量。
向量在左右手系中的转换最简单，只需要将某一个轴分量取反。以Z轴取反为例，右手系中的向量 $\bold{P}_{r}(x, y, z)$ 在左手系中转换为点 $\bold{P}_{l}(x, y, z)$ ，用矩阵表示为：

$\bold{P}_{l}=\left[\begin{array}{c} x \\ y \\ -z \end{array}\right]=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{array}\right]\left[\begin{array}{l} x \\ y \\ z \end{array}\right]=\bold{S}_{T} \bold{P}_{r}$ （1）
向量从右手系到左手系的转换矩阵为：

$\bold{S}_{T}=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{array}\right]$ （2）
<hr/>在一组坐标系（基）下，旋转对一个向量进行线性变换变为另外一个向量。
在右手系下，一个旋转矩阵可定义为：

$\bold{R}_{r}=\left[\begin{array}{lll} r_{00} & r_{01} & r_{02} \\ r_{10} & r_{11} & r_{12} \\ r_{20} & r_{21} & r_{22} \end{array}\right]$ （3）
右手系下的向量 $\bold{P}_{r}$ 经过旋转变换为 $\bold{P}_{r}^{\prime}$ :

$\bold{P}_{r}^{\prime}=\left[\begin{array}{l} x^{\prime} \\ y^{\prime} \\ z^{\prime} \end{array}\right]=\left[\begin{array}{lll} r_{00} & r_{01} & r_{02} \\ r_{10} & r_{11} & r_{12} \\ r_{20} & r_{21} & r_{22} \end{array}\right]\left[\begin{array}{l} x \\ y \\ z \end{array}\right]=\bold{R}_{r} \bold{P}_{r}$

$x^{\prime} ， y^{\prime} ， z^{\prime}，x ， y ， z$ 均为常数。求解旋转在左右手系下的表达形式，可表达为右手系下的旋转与左手系下的旋转 $\bold{R}_{l}$ 的相互转换。由式（1）可知， $\bold{P}_{l}^{\prime}，\bold{P}_{l}$ 可由下式算出：

$\bold{P}_{l}=(x, y,-z)$

$\bold{P}_{l}^{\prime}=\left(x^{\prime}, y^{\prime},-z^{\prime}\right)$
又由式（1）可知：

$\bold{P}_{l}^{\prime}=\bold{S}_{T} \bold{P}_{r}^{\prime}=\bold{S}_{T} \bold{R}_{r} \bold{P}_{r}=\bold{S}_{T} \bold{R}_{r} \bold{S}_{T} \bold{P}_{l}$
定义左手系下的旋转为：

$\bold{R}_{l}=\bold{S}_{T} \bold{R}_{r} \bold{S}_{T}$ （4）
左手系中旋转的通俗的理解如下：

将左手系中的点变换到右手系中
根据右手系中的旋转矩阵进行旋转
将旋转后的点变换到左手系中

因此式（3）所示的右手系手中的旋转，可计算为：

$\bold{R}_{l}=\left[\begin{array}{ccc} r_{00} & r_{01} & -r_{02} \\ r_{10} & r_{11} & -r_{12} \\ -r_{20} & -r_{21} & r_{22} \end{array}\right]$
实际上旋转的表示方法不限于旋转矩阵，还有轴角表示法、四元数表示法、欧拉角表示法。欧拉角表示法直观，在程序调试时很有用，但存在严重的万向锁问题。实际的计算机运算中大量使用四元数。以上述的对z轴取反为例，左右手系中旋转向量 $\bold{R}$ 与四元数 $\bold{q}(w,x,y,z)$ , $w$ 为实部，的转换关系如下：

$\begin{gathered} \bold{q}_{l}(w)=\frac{\sqrt{1+\operatorname{tr}\left(R_{l}\right)}}{2}=\bold{q}_{r}(w) \\ \bold{q}_{l}(x)=\frac{-r_{12}-\left(-r_{21}\right)}{4 q_{0}}=-\frac{r_{12}-r_{21}}{4 q_{0}}=-\bold{q}_{r}(x) \\ \bold{q}_{l}(y)=\frac{-r_{20}-\left(-r_{02}\right)}{4 q_{0}}=-\frac{r_{20}-r_{02}}{4 q_{0}}=-\bold{q}_{r}(y) \\ \bold{q}_{l}(z)=\frac{r_{01}-r_{10}}{4 q_{0}}=\bold{q}_{r}(z) \end{gathered}$

		自动登录	找回密码
密码			立即注册