基于车体坐标系的激光雷达坐标转换方法

摘要

基于车体坐标系的激光雷达坐标转换方法,具体步骤为，瞬时激光束坐标系转化为激光扫描参考坐标系，根据瞬时激光与激光雷达坐标系的夹角和激光飞行的距离，解算出激光脚点相对于激光雷达坐标系的坐标，激光扫描参考坐标系转化惯性平台参考坐标系，测量激光雷达与惯性导航平台的安置夹角和位移，根据旋转平移公式，解算激光脚点相对于惯性平台参考坐标系的坐标；惯性平台参考坐标系转化为周期开始瞬时坐标系，本发明不需要将激光脚点数据转化为WGS-84坐标系，而是转化为扫描周期开始时刻汽车载体瞬时坐标系，即减少了运算量和数据依赖，又能保持点和汽车之间的相对坐标；此方法不依赖于GPS数据，尤其是避免了GPS失效时带来的内插或预测误差。

说明书

技术领域

本发明涉及一种坐标转换方法，尤其涉及一种基于车体坐标系的 激光雷达坐标转换方法。

背景技术

数据显示，我国民用汽车保有量已经达到1.5亿辆，随之而来的 是交通事故的不断增加，据统计，我国每年交通事故亡人数已经超过 了10万人；因此，需要利用先进的技术来保证安全驾驶，减少交通 事故的发生。

激光雷达作为智能交通系统的重要组成部分，在汽车安全与自动 驾驶中发挥着重要的作用。它能够精确测量出扫描方向物体的三维坐 标，具有测量精度高、采集频率高的特点。但是激光雷达采集的数据 要经过复杂的坐标转换才能转换同一个坐标系WGS-84坐标系中，转 化过程中需要实时的激光雷达数据、惯性导航INS数据、全球定位系 统GPS数据，然而这些数据的获取频率有着很大的差距，激光雷达的 频率为100KHZ甚至更高，INS频率大约为200HZ，GPS频率只有20HZ， 因此需要插值，这种高频率的插值会带来误差，影响数据采集速度和 精度。

测绘激光雷达和防撞激光雷达有着本质的区别，测绘激光雷达要 求测量区域内数据尽可能精确，但是对数据处理时间没有要求；防撞 激光雷达对数据的实时处理功能有着较高的要求，其总处理时间应该 达到100毫秒级别，即其扫描、解算、识别、决策总时间为100毫秒 左右。汽车载体相对于飞机载体有着高度的不确定性，飞机起飞后飞 行姿态相对比较平稳，INS数据变化平稳，GPS没有遮挡能实施准确 获取数据；但是在汽车行驶过程中路过城区、隧道等有遮挡的地方会 导致GPS失效，出现连续的没有GPS数据的时间，此时进行插值或预 测将会增大误差；汽车速度的变化，路面的高低起伏，甚至汽车的变 道都会导致INS误差的积累。防撞激光雷达的重点是扫描道路上是否 有障碍物，然而道路上的车辆和行人是随着时间的推移位置变化的， 因此将不同扫描周期的数据同时处理变的困难且没有意义。由于导航 信息是INS经过积分而产生，定位误差随时间推移而增大，长期精度 差，且每次使用之前需要较长的初始对准时间。传统的激光雷达数据 坐标转换要经过以下步骤：

1、瞬时激光束坐标系转化为激光扫描参考坐标系，根据瞬时激 光与激光雷达坐标系的夹角和激光飞行的距离，解算出激光脚点相对 于激光雷达坐标系的坐标：

$\left(\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right)=\left(\begin{array}{c}L\cos acos\beta \\ L\cos asin\beta \\ L\sin a\end{array}\right)$

其中(X_LY_LZ_L)^T为激光脚点在激光扫描参考坐标系的坐标，a 为瞬时激光束在XOZ平面内的投影与X轴的夹角；β为瞬时激光束在 XOY平面的投影与X轴的夹角。

2、激光扫描参考坐标系转化惯性平台参考坐标系，测量激光雷 达与惯性导航平台的安置夹角和位移，根据旋转平移公式，解算激光 脚点相对于惯性平台参考坐标系的坐标；安置完之后该坐标转换矩阵 不会变化，坐标转换公式为：

$\left(\begin{array}{c}{X}_I\\ Y_I\\ Z_I\end{array}\right)=R_I\left(\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right)+\left(\begin{array}{c}\Delta X_L\\ \Delta Y_L\\ {\mathrm{\Delta Z}}_L\end{array}\right)$

其中，(X_IY_IZ_I)^T为激光脚点相对于惯性平台参考坐标系的坐 标；R_I为由激光扫描参考坐标系和惯性平台参考坐标系的安置夹角 (a、b、c)得到的旋转矩阵；安装后旋转矩阵固定；(ΔX_LΔY_LΔZ_L)^T为 激光扫描参考坐标系原点和惯性平台参考坐标系原点之间的位移向 量，安装后该位移向量固定。

3、惯性平台参考坐标系转化为当地水平参考坐标系(天线GPS)， 根据惯性导航系统测量的三个姿态角，H(Heading方向角，绕Z轴旋 转)、P(Pitch俯仰角，绕Y轴旋转)、R(Roll侧滚角，绕X轴旋转) 和安置位移解算目标点相对于GPS坐标系的坐标；该坐标转换矩阵由 于INS测量的姿态角的不同而实时变化，但是位移向量不会变化，此 时坐标转换公式为：

$\left(\begin{array}{c}{X}_G\\ y_G\\ Z_G\end{array}\right)=R_G\left[\left(\begin{array}{c}{X}_I\\ Y_I\\ Z_I\end{array}\right)-\left(\begin{array}{c}\Delta X_I\\ \Delta Y_I\\ {\mathrm{\Delta Z}}_I\end{array}\right)\right]$

其中(X_GY_GZ_G)^T为激光脚点相对于当地水平参考坐标系的坐 标；(ΔX_IΔY_IΔZ_I)^T为惯性平台参考坐标系和当地水平参考坐标系之 间的位移向量；R_G为惯性导航系统测量的三个夹角(H、R、P)组成的 旋转公式。

4、当地水平参考坐标系转化为WGS-84坐标系，根据GPS测量的 纬度(B)、经度(L)、高度(H)数据和WGS-84的相关常系数，解算激光 脚点相对于WGS-84坐标系的坐标；该坐标转换矩阵由于GPS测量的 经纬度和高度不同而实时变化，此时坐标转换公式为：

$\left(\begin{array}{c}{X}_{84}\\ Y_{84}\\ Z_{84}\end{array}\right)=R_W\left(\begin{array}{c}{X}_G\\ Y_G\\ Z_G\end{array}\right)+\left(\begin{array}{c}\Delta X_G\\ {\mathrm{\Delta Y}}_G\\ {\mathrm{\Delta Z}}_G\end{array}\right)$

其中(X₈₄Y₈₄Z₈₄)^T表示激光脚点相对于WGS-84坐标系的坐标； R_W为当地水平坐标系到大地坐标系经纬度(B、L)的旋转公示，由GPS 的经纬度获得；(ΔX_GΔY_GΔZ_G)^T为天线(GPS)到地心的位移，由GPS 获得。

综上所述坐标转换公式为：

$\left(\begin{array}{c}{X}_{84}\\ Y_{84}\\ Z_{84}\end{array}\right)=R_W{R}_G\left[R_I\left(\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right)+\left(\begin{array}{c}\Delta X_L\\ {\mathrm{\Delta Y}}_L\\ {\mathrm{\Delta Z}}_L\end{array}\right)-\left(\begin{array}{c}\Delta X_I\\ \Delta Y_I\\ {\mathrm{\Delta Z}}_I\end{array}\right)\right]+\left(\begin{array}{c}\Delta X_G\\ {\mathrm{\Delta Y}}_G\\ {\mathrm{\Delta Z}}_G\end{array}\right)$

传统的车载激光雷达数据坐标系转换需要经过四次转换，且有三 次坐标转换的转换矩阵不同，因此需要对每个激光脚点计算转换矩 阵，其计算量相当巨大。INS频率和GPS频率远远小于激光频率，需 要进行预测或内插，带来误差；汽车路过城区、隧道等有遮挡的地方 会导致GPS失效，出现连续的没有GPS数据的时间，此时进行插值或 预测将会增大误差；INS定位误差随时间而增大，长期精度差。

忽略周期内INS、GPS的数据变化，采用最简单的坐标转换模型， 将该扫描周期内看成一个时刻，选取该时刻内的某一个INS数据和 GPS数据，即以上坐标转换矩阵的后三矩阵在一个扫描周期内不变， 减少了矩阵相乘的运算量。该算法虽然简单，但是误差太大，仅以汽 车位移误差为例，若汽车以25m/s(90km/h)的速度行驶,激光扫描 的频率是10HZ，则一个周期内汽车移动2.5m，即仅汽车运动误差就 达到2.5m，这显然是难以接受的。

发明内容

本发明提供了一种基于车体坐标系的激光雷达坐标转换方法，不 需要将激光脚点数据转化为WGS-84坐标系，而是转化为扫描周期开 始时刻汽车载体瞬时坐标系，即减少了运算量和数据依赖，又能保持 点和汽车之间的相对坐标；此方法不依赖于GPS数据，尤其是避免了 GPS失效时带来的内插或预测误差。

为实现上述目的，本发明所采用的技术方案是：基于车体坐标系 的激光雷达坐标转换方法,具体步骤如下：

S1：瞬时激光束坐标系转化为激光扫描参考坐标系，根据瞬时激 光与激光雷达坐标系的夹角和激光飞行的距离，解算出激光脚点相对 于激光雷达坐标系的坐标：

$\left(\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right)=\left(\begin{array}{c}L\cos acos\beta \\ L\cos asin\beta \\ L\sin a\end{array}\right)$

其中(X_LY_LZ_L)^T为激光脚点在激光扫描参考坐标系的坐标，该 激光扫描参考坐标系为右手坐标系，a为瞬时激光束在XOZ平面内的 投影与X轴的夹角；β为瞬时激光束在XOY平面的投影与X轴的夹角。

S2：激光扫描参考坐标系转化惯性平台参考坐标系，测量激光雷 达与惯性导航平台的安置夹角和位移，根据旋转平移公式，解算激光 脚点相对于惯性平台参考坐标系的坐标；本步骤中的旋转平移公式 为：

$\left(\begin{array}{c}{X}_I\\ Y_I\\ Z_I\end{array}\right)=R_I\left(\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right)+\left(\begin{array}{c}\Delta X_L\\ \Delta Y_L\\ {\mathrm{\Delta Z}}_L\end{array}\right)$

其中，(X_IY_IZ_I)^T为激光脚点相对于惯性平台参考坐标系的坐 标；R_I为由激光扫描参考坐标系和惯性平台参考坐标系之间的安置夹 角得到的旋转矩阵；(ΔX_LΔY_LΔZ_L)^T为激光扫描参考坐标系原点和惯 性平台参考坐标系原点之间的位移向量，激光雷达安装在车内后旋转 矩阵和位移向量固定。

S3：惯性平台参考坐标系转化为周期开始瞬时坐标系，其转化公 式为：

$\left(\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right)=\underset{i=0}{\overset{n}{\Sigma }}\left(\begin{array}{c}{\mathrm{\Delta X}}_i\\ {\mathrm{\Delta Y}}_i\\ {\mathrm{\Delta Z}}_i\end{array}\right)$

其中(X_nY_nZ_n)^T是相对于周期开始坐标轴n*t时刻的移动坐标； (ΔX_iΔX_iΔX_i)^T表示i时刻汽车行驶的位移向量。

综上所述得到最终坐标转换公式：

$\left(\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right)=R_G\left[R_I\left(\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right)+\left(\begin{array}{c}\Delta X_L\\ {\mathrm{\Delta Y}}_L\\ {\mathrm{\Delta Z}}_L\end{array}\right)\right]+\underset{i=0}{\overset{n}{\Sigma }}\left(\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right)$

进一步的，在步骤S3中位移向量是通过以下步骤得出的：

A1、在周期开始瞬时坐标系上，将汽车每个扫描周期内的行驶轨 迹进行微分，该周期内汽车做匀速直线运动，则汽车行驶的距离为：

l＝v×t

其中v为汽车的速度，t为运行时间；

A2、初始时A点经过t时间到达B点，将AB之间的距离看成等 于A到B的行驶轨迹；此时AB向量相对于A瞬时的坐标(ΔXΔYΔZ)^T有以下公式：

$\left(\begin{array}{c}\Delta X\\ \Delta Y\\ \Delta Z\end{array}\right)=R_{\Delta I}\left(\begin{array}{c}I\\ 0\\ 0\end{array}\right)$

其中R_ΔI为旋转公式：R_ΔI＝R(H)R(P)R(R)

$R\left(R\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos R& -\sin R\\ 0& \sin R& conR\end{array}\right)$

$R\left(P\right)=\left(\begin{array}{ccc}cosP& 0& \sin P\\ 0& 1& 0\\ -\sin P& 0& \cos P\end{array}\right)$

$R\left(H\right)=\left(\begin{array}{ccc}\cos H& -\sin H& 0\\ \sin H& \cos H& 0\\ 0& 0& 1\end{array}\right)$

其中R(H)、R(R)、R(P)分别是惯性平台参考坐标系相对于周期 开始瞬时坐标系的姿态角，l为在该时间段内汽车的位移；

A3、由此计算出B点相对于起始时刻A的坐标为(X_BY_BZ_B)^T：

$\left(\begin{array}{c}{X}_B\\ Y_B\\ Z_B\end{array}\right)=\left(\begin{array}{c}\Delta X\\ \Delta Y\\ \Delta Z\end{array}\right)$

(ΔXΔYΔZ)^T汽车行驶的位移向量；由此可以得到各个点相对于 A瞬时坐标系的坐标。

本发明专利由于采用以上技术方案，能够取得如下的技术效果： 本发明可以简化激光雷达系统坐标转换模型，不用将坐标系转化成 WGS-84坐标系，减小了整体数据值的大小；以某个时刻的INS位置为 基础建立坐标系，简化了数据处理过程；在很短时间内认为INS系统 差值量不变，减少计算量，加快数据解算速度。不采用GPS数据，使 得不需要在低频率的GPS中插值数据，提高了数据精度，即使在隧道、 山林、高楼林立的城区都不必担心由于GPS失效引起的数据误差。

采用惯性导航系统在微小时间内的变化量，降低了INS的采样频 率，避免了INS定位误差随时间的推移而增大，且每次使用之前不需 要较长的初始对准时间，使其应用到车载成为可能。将运动轨迹进行 微分，采用累加和的形式，简化了汽车运动轨迹模型，同时尽可能的 逼近原始运动轨迹。

附图说明

本发明共有附图7幅：

图1是现有技术中坐标转换流程图；

图2是步骤S1中激光扫描参考坐标系；

图3是步骤S2中坐标转换示意图；

图4是当地水平坐标系和WGS-84坐标系；

图5是汽车运行轨迹微分示意图；

图6是汽车运动方向角示意图；

图7是本发明坐标转换流程图。

具体实施方式

下面通过具体实施例，并结合附图，对本发明的技术方案作进一 步的解释说明。

基于车体坐标系的激光雷达坐标转换方法,具体步骤如下：

S1：瞬时激光束坐标系转化为激光扫描参考坐标系，根据瞬时激 光与激光雷达坐标系的夹角和激光飞行的距离，解算出激光脚点相对 于激光雷达坐标系的坐标：

$\left(\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right)=\left(\begin{array}{c}L\cos acos\beta \\ L\cos asin\beta \\ L\sin a\end{array}\right)$

其中(X_LY_LZ_L)^T为激光脚点在激光扫描参考坐标系的坐标，该 激光扫描参考坐标系为右手坐标系，a为瞬时激光束在XOZ平面内的 投影与X轴的夹角；β为瞬时激光束在XOY平面的投影与X轴的夹角。

S2：激光扫描参考坐标系转化惯性平台参考坐标系，测量激光雷 达与惯性导航平台的安置夹角和位移，根据旋转平移公式，解算激光 脚点相对于惯性平台参考坐标系的坐标；本步骤中的旋转平移公式 为：

$\left(\begin{array}{c}{X}_I\\ Y_I\\ Z_I\end{array}\right)=R_I\left(\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right)+\left(\begin{array}{c}\Delta X_L\\ \Delta Y_L\\ {\mathrm{\Delta Z}}_L\end{array}\right)$

其中，(X_IY_IZ_I)^T为激光脚点相对于惯性平台参考坐标系的坐 标；R_I为由激光扫描参考坐标系和惯性平台参考坐标系之间的安置夹 角得到的旋转矩阵；(ΔX_LΔY_LΔZ_L)^T为激光扫描参考坐标系原点和惯 性平台参考坐标系原点之间的位移向量，激光雷达安装在车内后旋转 矩阵和位移向量固定。

S3：惯性平台参考坐标系转化为周期开始瞬时坐标系，其使用的 方向角是瞬时INS数值与周期开始时刻方向角的差值，避免了INS定位 误差随时间而增大，且每次使用之前不需要较长的初始对准时间；其 转化公式为：

$\left(\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right)=\underset{i=0}{\overset{n}{\Sigma }}\left(\begin{array}{c}{\mathrm{\Delta X}}_i\\ {\mathrm{\Delta Y}}_i\\ {\mathrm{\Delta Z}}_i\end{array}\right)$

其中(X_nY_nZ_n)^T是相对于周期开始坐标轴n*t时刻的移动坐标； (ΔX_iΔX_iΔX_i)^T表示i时刻汽车行驶的位移向量。

综上所述得到最终坐标转换公式：

$\left(\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right)=R_G\left[R_I\left(\begin{array}{c}{X}_L\\ Y_L\\ Z_L\end{array}\right)+\left(\begin{array}{c}\Delta X_L\\ {\mathrm{\Delta Y}}_L\\ {\mathrm{\Delta Z}}_L\end{array}\right)\right]+\underset{i=0}{\overset{n}{\Sigma }}\left(\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right)$

进一步的，在步骤S3中位移向量是通过以下步骤得出的：

A1、在周期开始瞬时坐标系上，将汽车每个扫描周期内的行驶轨 迹进行微分，每份的时间t为0.001s，该周期内汽车做匀速直线运动， 则汽车行驶的距离为：

l＝v×t

其中v为汽车的速度，t为运行时间；

A2、初始时，汽车的瞬时运动方向是X轴方向，如图5中的A 点所示，经过t时间到达B点，当t足够小时，AB之间的距离等于A 到B的行驶轨迹；此时AB向量相对于A瞬时的坐标(ΔXΔYΔZ)^T有 以下公式：

$\left(\begin{array}{c}\Delta X\\ \Delta Y\\ \Delta Z\end{array}\right)=R_{\Delta I}\left(\begin{array}{c}I\\ 0\\ 0\end{array}\right)$

其中R_ΔI为旋转公式：R_ΔI＝R(H)R(P)R(R)

$R\left(R\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos R& -\sin R\\ 0& \sin R& conR\end{array}\right)$

$R\left(P\right)=\left(\begin{array}{ccc}cosP& 0& \sin P\\ 0& 1& 0\\ -\sin P& 0& \cos P\end{array}\right)$

$R\left(H\right)=\left(\begin{array}{ccc}\cos H& -\sin H& 0\\ \sin H& \cos H& 0\\ 0& 0& 1\end{array}\right)$

其中R(H)、R(R)、R(P)分别是惯性平台参考坐标系相对于周期 开始瞬时坐标系的姿态角，l为在该时间段内汽车的位移；

A3、由此计算出B点相对于起始时刻A的坐标为(X_BY_BZ_B)^T：

$\left(\begin{array}{c}{X}_B\\ Y_B\\ Z_B\end{array}\right)=\left(\begin{array}{c}\Delta X\\ \Delta Y\\ \Delta Z\end{array}\right)$

(ΔXΔYΔZ)^T汽车行驶的位移向量；由此可以得到各个点相对于 A瞬时坐标系的坐标。

本发明可以简化激光雷达系统坐标转换模型，不用将坐标系转化 成WGS-84坐标系，减小了整体数据值的大小；以某个时刻的INS位 置为基础建立坐标系，简化了数据处理过程；在很短时间内认为INS 系统差值量不变，减少计算量，加快数据解算速度。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范 围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技 术范围内，根据本发明的技术方案及其发明构思加以等同替换或改 变，都应涵盖在本发明的保护范围之内。