登山第七梯：激光扫描系统坐标系转换关系——你来我往

点云登山者

于 2023-11-16 19:41:58 发布

阅读量275

点赞数

分类专栏：点云算法登山100梯文章标签：算法线性代数

本文链接：https://blog.csdn.net/qq625924821/article/details/134292710

版权

点云算法登山100梯专栏收录该内容

9 篇文章 2 订阅

订阅专栏

一坐标系类别

1、激光扫描仪坐标系（一般由厂商自己定义）： $O_{L}-X_{L}Y_{L}Z_{L}$ ，MLS系统通常使用2D扫描仪，以激光发射中心为原点 $O_{L}$ ， $X_{L}$ 为扫描平面起始方向， $Z_{L}$ 在扫描平面内与 $X_{L}$ 垂直， $Y_{L}$ 垂直扫描平面，形成右手系 $O_{L}-X_{L}Y_{L}Z_{L}$ 。

2、惯导坐标系： $O_{I}-X_{I}Y_{I}Z_{I}$ ，原点 $O_{I}$ 一般定义为IMU的几何中心， $Y_{I}$ 指向载体行驶方向， $X_{I}$ 与 $Y_{I}$ 垂直向右， $Z_{I}$ 与 $X_{I}O_{I}Y_{I}$ 面垂直，形成右手系 $O_{I}-X_{I}Y_{I}Z_{I}$ 。

3、当地水平坐标系： $O_{H}-X_{H}Y_{H}Z_{H}$ ，原点 $O_{H}$ 一般与 $O_{I}$ 一致， $X_{H}$ 指向东，即卯酉圈方向， $Y_{H}$ 指向真北方向，即参考椭球子午圈方向， $Z_{H}$ 为参考椭球法向量，与 $X_{H}O_{H}Y_{H}$ 构成右手系。（注意：pos的姿态角是相对水平坐标系测定的）

4、GPS使用WGS84坐标系： $O_{W}-X_{W}Y_{W}Z_{W}$ ，原点 $O_{W}$ 为地球质心，以地球自转轴为 $Z_{W}$ ， $X_{W}$ 为指向赤道面内指向零度子午线， $Y_{W}$ 垂直 $X_{W}O_{W}Y_{W}$ 面，形成右手系 $O_{W}-X_{W}Y_{W}Z_{W}$ 。

二激光点云解算过程的坐标转换流程

总体流程：激光雷达扫描得到的每个点（时间、水平视场角、垂直视场角、扫描距离）——》 $O_{L}-X_{L}Y_{L}Z_{L}$ ——》 $O_{I}-X_{I}Y_{I}Z_{I}$ ——》 $O_{H}-X_{H}Y_{H}Z_{H}$ ——》 $O_{W}-X_{W}Y_{W}Z_{W}$ ：

$p_{W}=R_{H}^{W}(B,L)R_{I}^{H}(r,p,h)(R_{L}^{I}p_{L}+t_{L}^{I})+t_{I}^{W}(B,L,H)$

扫描仪按时间记录每个点的角度和距离信息，再通过极坐标转换为空间直角坐标，将其转换为扫描仪坐标系坐标；

$p_{L}=\begin{bmatrix} \rho cos\alpha cos\theta \\ \rho cos\alpha sin\theta \\ \rho sin\alpha \end{bmatrix}$

其中， $\rho$ 为扫描距离， $\alpha$ 为竖直角度值， $\theta$ 为水平角度值。

接着，根据扫描仪和IMU的相对位置关系，即安置参数，将扫描仪坐标转换为惯导坐标：

$p_{I}=R_{L}^{I}p_{L}+t_{L}^{I}$

$R_{L}^{I}$ 是扫描仪到惯导的旋转矩阵， $t_{L}^{I}$ 是扫描仪到惯导的平移矩阵，二者通常通过标定安置参数进行确定。

然后，计算载体的姿态角（roll/pitch/heading，翻滚角/俯仰角/航向角）；其中，roll可以理解为载体水平轴与水平位置的夹角，pitch可以理解为载体与行驶方向平行的中心轴与地面之间的夹角，heading为载体与行驶方向平行的中心轴与正北（地球北极）方向的夹角。利用pos的姿态角构建旋转矩阵，将惯导坐标旋转到当地水平坐标系。

通常惯导坐标系与当地水平坐标系的原点一致。 $R_{I}^{H}$ 为惯导坐标系到当地水平坐标系的旋转矩阵，具体为 $O_{I}-X_{I}Y_{I}Z_{I}$ 绕Y轴旋转顺时针r，绕X轴顺时针旋转p，绕Z轴顺时针旋转h形成复合矩阵：

$p_{H}=R_{I}^{H}(r,p,h))p_{I}$

$R_{I}^{H}(r,p,h)=R_{I}^{H}(h)R_{I}^{H}(p)R_{I}^{H}(r)$

其中：

$R_{I}^{H}(r)=\begin{bmatrix} cosr & 0 &sinr \\ 0 & 1 & 0\\ -sinr& 0 &cosr \end{bmatrix}$

$R_{I}^{H}(p)=\begin{bmatrix} 1 & 0 &0\\ 0 & cosp & -sinp\\0& sinp &cosp \end{bmatrix}$

$R_{I}^{H}(h)=\begin{bmatrix} cosh & -sinh &0\\ sinh & cosh& 0\\0& 0&1\end{bmatrix}$

最后，根据pos的(B,L,H，纬度、经度、大地高)构建变换矩阵，将当地水平坐标转换为WGS84坐标。

$p_{W}=R_{H}^{W}(B,L)p_{L}+t_{H}^{W}(B,L,H)$

$R_{H}^{W}(B,L)=R_{H}^{W}(B)R_{H}^{W}(L)$

$R_{H}^{W}(B,L)$ 为 $O_{H}-X_{H}Y_{H}Z_{H}$ 绕X轴顺时针旋转90°-B，再绕Z轴顺时针旋转90°+L形成的复合矩阵：

其中，

$R_{H}^{W}(B)=\begin{bmatrix} 1 & 0 &0\\ 0 & cos(90^{\circ}-B) & -sin(90^{\circ}-B) \\0& sin(90^{\circ}-B) &cos(90^{\circ}-B) \end{bmatrix}$

$R_{H}^{W}(L)=\begin{bmatrix} cos(90^{\circ}+L) & -sin(90^{\circ}+L) &0\\ sin(90^{\circ}+L) & cos(90^{\circ}+L) & 0\\0& 0&1\end{bmatrix}$

$t_{H}^{W}(B,L,H)$ 为载体在 $O_{W}-X_{W}Y_{W}Z_{W}$ 中的位置：

$t_{H}^{W}(B,L,H)=\begin{bmatrix} (N+H)cosBcosL \\ (N+H)cosBsinL \\ (N(1-e^{^{2}})+H)sinB) \end{bmatrix}$

其中， $N=a/\sqrt{1-e_{1}^{^{2}}sin^{2}B}$ 是WGS84参考椭球卯酉圈曲率半径， $a$ =6378137m为子午圈长轴半径， $e_{1}$ 为第一偏心率， $e_{1}^{^{2}}=0.00669437999013$ ，

三空间直角坐标系与其高斯投影平面坐标系之间的转换

实际工程应用中，采用参考椭球坐标系或其高斯投影平面坐标系。

空间直角坐标系转高斯投影坐标系的过程为：先将空间直角坐标转为经纬度和大地高，再将经纬度投影成高斯坐标 $O-xyH$ ，即参考椭球坐标系——>大地坐标系——>高斯投影平面坐标系

3.1 大地坐标转空间直角坐标

$\begin{bmatrix}X= (N+H)cosBcosL \\Y= (N+H)cosBsinL \\Z= (N(1-e_{1}^{^{2}})+H)sinB) \end{bmatrix}$

b为子午圈短轴半径。

3.2空间直角坐标转大地坐标

3.2.1 直接求解法

$\begin{matrix} L=arctan\frac{y}{x}\\ B=arctan\frac{Z+e_{2}^{2}\cdot b\cdot sin^{3}\theta }{\sqrt{X^{2}+Y^{2}}-e_{1}^{2}\cdot cos^{3}\theta }\\ H=\frac{\sqrt{X^{2}+Y^{2}}}{cosB}-N \end{matrix}$

$e_{1}$ 是第一偏心率， $e_{1}^{2}=1-\frac{b^{2}}{a^{2}}$ ， $e_{2}$ 是第二偏心率， $e_{2}^{2}=\frac{a^{2}}{b^{2}}-1$ ， $\theta =arctan\frac{2\cdot a}{\sqrt{x^{2}+y^{2}}\cdot b}$

3.2.2 迭代法

$\begin{matrix} L=arctan\frac{y}{x}\\ B=arctan\frac{Z+Ne^{2} sinB}{\sqrt{X^{2}+Y^{2}}}\\ H=\frac{\sqrt{X^{2}+Y^{2}}}{cosB}-N \end{matrix}$

迭代时，通过计算 $tanB_{0}=\frac{Z}{\sqrt{X^{2}+Y^{2}}}$ 得到B的初始值 $B_{0}$ ，然后通过初始值 $B_{0}$ 计算N和sinB，从而得到B的估计值，多次迭代，直到前后两次B值之差达到所设置的误差精度则停止。

3.3高斯投影正算

即从大地坐标到高斯投影平面坐标系的转换，转换公式为：

$\begin{Bmatrix} x=X+N\cdot t[\frac{1}{2}\cdot cos^{2}B\cdot l^{2}+\frac{1}{24}\cdot cos^{4}B\cdot l^{4}\cdot (5-t^{2}+9\cdot\eta ^{2})+ \frac{1}{720}\cdot cos^{6}B\cdot l^{6}\cdot (61-58\cdot t^{2}+t ^{4})]\\ y=N\cdot [cosB\cdot l+\frac{1}{6}\cdot cos^{3}B\cdot l^{3}\cdot (1-t^{2}+\eta ^{2})+\frac{1}{120}\cdot cos^{5}B\cdot l^{5}\cdot (5-18\cdot t^{2}+t^{4}+14\cdot \eta ^{2}-58\cdot \eta ^{2}\cdot t^{2})] \end{Bmatrix}$