Sensor fusion学习-2.毫米波雷达到世界坐标的转换

在了解了基本的坐标转换理论之后，任意传感器到世界坐标系的转换就应该都可以进行转换了，这次趁着热乎试着写写毫米波雷达坐标到世界坐标系的转换
问题是额，我目前还没有接触到毫米波雷达实体，因此参考了[知乎专栏-无人驾驶干货铺](https://zhuanlan.zhihu.com/p/34675392)的数据

2.毫米波雷达到世界坐标的转换

前言：毫米波雷达可以得到目标的x，y坐标信息，没有目标 的z的信息，所以这也方便了我们进行坐标转换，直接可以将毫米波坐标系$Om$到世界坐标系$Ow$转换看作二维坐标系的转换，根据之前的基础进行旋转和平移。

毫米波数据坐标系的转换

由于毫米波雷达的测距和测速原理是基于多普勒效应，因此与常用的笛卡尔坐标系不同，毫米波雷达的原始数据是基于（距离+角度）的极坐标系。所以我们首先需要根据高中的知识进行数据的两种坐标系转换。
$\{\begin{array}{l}X=\rho \cos \theta \\ Y=\rho \sin \theta \end{array}$

由上一篇文章中的公式：我们可以得到毫米波坐标到世界坐标系的转换

旋转(绕Z轴)

$R\left(Z_A,\theta \right)=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$

平移

$\left[\begin{array}{c}{X}_m\\ Y_m\\ 1\end{array}\right]=R\left(Z_A,\theta \right)\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right]+T$

$T=\left[\begin{array}{c}{X}_W\\ Y_W\\ 1\end{array}\right]$

最后三个式子联立起来就好啦

最后，我也刚刚入门，非常可能有推导出错的地方，希望大家评论区教教我，十分感谢~

部分的知识，还有图片来自这篇文章，也是讲融合的，大家感兴趣可以看一下
毫米波雷达与视觉融合———联合标定及外参标定问题