CN105277950B - 基于车体坐标系的激光雷达坐标转换方法 - Google Patents
基于车体坐标系的激光雷达坐标转换方法 Download PDFInfo
- Publication number
- CN105277950B CN105277950B CN201510634093.8A CN201510634093A CN105277950B CN 105277950 B CN105277950 B CN 105277950B CN 201510634093 A CN201510634093 A CN 201510634093A CN 105277950 B CN105277950 B CN 105277950B
- Authority
- CN
- China
- Prior art keywords
- mtd
- mtr
- mrow
- msub
- mtable
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Traffic Control Systems (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
基于车体坐标系的激光雷达坐标转换方法,具体步骤为，瞬时激光束坐标系转化为激光扫描参考坐标系，根据瞬时激光与激光雷达坐标系的夹角和激光飞行的距离，解算出激光脚点相对于激光雷达坐标系的坐标，激光扫描参考坐标系转化惯性平台参考坐标系，测量激光雷达与惯性导航平台的安置夹角和位移，根据旋转平移公式，解算激光脚点相对于惯性平台参考坐标系的坐标；惯性平台参考坐标系转化为周期开始瞬时坐标系，本发明不需要将激光脚点数据转化为WGS‑84坐标系，而是转化为扫描周期开始时刻汽车载体瞬时坐标系，即减少了运算量和数据依赖，又能保持点和汽车之间的相对坐标；此方法不依赖于GPS数据，尤其是避免了GPS失效时带来的内插或预测误差。
Description
技术领域
本发明涉及一种坐标转换方法，尤其涉及一种基于车体坐标系的激光雷达坐标转换方法。
背景技术
数据显示，我国民用汽车保有量已经达到1.5亿辆，随之而来的是交通事故的不断增加，据统计，我国每年交通事故亡人数已经超过了10万人；因此，需要利用先进的技术来保证安全驾驶，减少交通事故的发生。
激光雷达作为智能交通***的重要组成部分，在汽车安全与自动驾驶中发挥着重要的作用。它能够精确测量出扫描方向物体的三维坐标，具有测量精度高、采集频率高的特点。但是激光雷达采集的数据要经过复杂的坐标转换才能转换同一个坐标系WGS-84坐标系中，转化过程中需要实时的激光雷达数据、惯性导航INS数据、全球定位***GPS数据，然而这些数据的获取频率有着很大的差距，激光雷达的频率为100KHZ甚至更高，INS频率大约为200HZ，GPS频率只有20HZ，因此需要插值，这种高频率的插值会带来误差，影响数据采集速度和精度。
测绘激光雷达和防撞激光雷达有着本质的区别，测绘激光雷达要求测量区域内数据尽可能精确，但是对数据处理时间没有要求；防撞激光雷达对数据的实时处理功能有着较高的要求，其总处理时间应该达到100毫秒级别，即其扫描、解算、识别、决策总时间为100毫秒左右。汽车载体相对于飞机载体有着高度的不确定性，飞机起飞后飞行姿态相对比较平稳，INS数据变化平稳，GPS没有遮挡能实施准确获取数据；但是在汽车行驶过程中路过城区、隧道等有遮挡的地方会导致GPS失效，出现连续的没有GPS数据的时间，此时进行插值或预测将会增大误差；汽车速度的变化，路面的高低起伏，甚至汽车的变道都会导致INS误差的积累。防撞激光雷达的重点是扫描道路上是否有障碍物，然而道路上的车辆和行人是随着时间的推移位置变化的，因此将不同扫描周期的数据同时处理变的困难且没有意义。由于导航信息是INS经过积分而产生，定位误差随时间推移而增大，长期精度差，且每次使用之前需要较长的初始对准时间。传统的激光雷达数据坐标转换要经过以下步骤：
1、瞬时激光束坐标系转化为激光扫描参考坐标系，根据瞬时激光与激光雷达坐标系的夹角和激光飞行的距离，解算出激光脚点相对于激光雷达坐标系的坐标：
其中(XL YL ZL)T为激光脚点在激光扫描参考坐标系的坐标，a为瞬时激光束在XOZ平面内的投影与X轴的夹角；β为瞬时激光束在XOY平面的投影与X轴的夹角。
2、激光扫描参考坐标系转化惯性平台参考坐标系，测量激光雷达与惯性导航平台的安置夹角和位移，根据旋转平移公式，解算激光脚点相对于惯性平台参考坐标系的坐标；安置完之后该坐标转换矩阵不会变化，坐标转换公式为：
其中，(XI YI ZI)T为激光脚点相对于惯性平台参考坐标系的坐标；RI为由激光扫描参考坐标系和惯性平台参考坐标系的安置夹角(a、b、c)得到的旋转矩阵；安装后旋转矩阵固定；(ΔXL ΔYL ΔZL)T为激光扫描参考坐标系原点和惯性平台参考坐标系原点之间的位移向量，安装后该位移向量固定。
3、惯性平台参考坐标系转化为当地水平参考坐标系(天线GPS)，根据惯性导航***测量的三个姿态角，H(Heading方向角，绕Z轴旋转)、P(Pitch俯仰角，绕Y轴旋转)、R(Roll侧滚角，绕X轴旋转)和安置位移解算目标点相对于GPS坐标系的坐标；该坐标转换矩阵由于INS测量的姿态角的不同而实时变化，但是位移向量不会变化，此时坐标转换公式为：
其中(XG YG ZG)T为激光脚点相对于当地水平参考坐标系的坐标；(ΔXI ΔYI ΔZI)T为惯性平台参考坐标系和当地水平参考坐标系之间的位移向量；RG为惯性导航***测量的三个夹角(H、R、P)组成的旋转公式。
4、当地水平参考坐标系转化为WGS-84坐标系，根据GPS测量的纬度(B)、经度(L)、高度(H)数据和WGS-84的相关常系数，解算激光脚点相对于WGS-84坐标系的坐标；该坐标转换矩阵由于GPS测量的经纬度和高度不同而实时变化，此时坐标转换公式为：
其中(X84 Y84 Z84)T表示激光脚点相对于WGS-84坐标系的坐标；RW为当地水平坐标系到大地坐标系经纬度(B、L)的旋转公示，由GPS的经纬度获得；(ΔXG ΔYG ΔZG)T为天线(GPS)到地心的位移，由GPS获得。
综上所述坐标转换公式为：
传统的车载激光雷达数据坐标系转换需要经过四次转换，且有三次坐标转换的转换矩阵不同，因此需要对每个激光脚点计算转换矩阵，其计算量相当巨大。INS频率和GPS频率远远小于激光频率，需要进行预测或内插，带来误差；汽车路过城区、隧道等有遮挡的地方会导致GPS失效，出现连续的没有GPS数据的时间，此时进行插值或预测将会增大误差；INS定位误差随时间而增大，长期精度差。
忽略周期内INS、GPS的数据变化，采用最简单的坐标转换模型，将该扫描周期内看成一个时刻，选取该时刻内的某一个INS数据和GPS数据，即以上坐标转换矩阵的后三矩阵在一个扫描周期内不变，减少了矩阵相乘的运算量。该算法虽然简单，但是误差太大，仅以汽车位移误差为例，若汽车以25m/s(90km/h)的速度行驶,激光扫描的频率是10HZ，则一个周期内汽车移动2.5m，即仅汽车运动误差就达到2.5m，这显然是难以接受的。
发明内容
本发明提供了一种基于车体坐标系的激光雷达坐标转换方法，不需要将激光脚点数据转化为WGS-84坐标系，而是转化为扫描周期开始时刻汽车载体瞬时坐标系，即减少了运算量和数据依赖，又能保持点和汽车之间的相对坐标；此方法不依赖于GPS数据，尤其是避免了GPS失效时带来的内插或预测误差。
为实现上述目的，本发明所采用的技术方案是：基于车体坐标系的激光雷达坐标转换方法,具体步骤如下：
S1：瞬时激光束坐标系转化为激光扫描参考坐标系，根据瞬时激光与激光雷达坐标系的夹角和激光飞行的距离，解算出激光脚点相对于激光雷达坐标系的坐标：
其中(XL YL ZL)T为激光脚点在激光扫描参考坐标系的坐标，该激光扫描参考坐标系为右手坐标系，a为瞬时激光束在XOZ平面内的投影与X轴的夹角；β为瞬时激光束在XOY平面的投影与X轴的夹角。
S2：激光扫描参考坐标系转化惯性平台参考坐标系，测量激光雷达与惯性导航平台的安置夹角和位移，根据旋转平移公式，解算激光脚点相对于惯性平台参考坐标系的坐标；本步骤中的旋转平移公式为：
其中，(XI YI ZI)T为激光脚点相对于惯性平台参考坐标系的坐标；RI为由激光扫描参考坐标系和惯性平台参考坐标系之间的安置夹角得到的旋转矩阵；(ΔXL ΔYL ΔZL)T为激光扫描参考坐标系原点和惯性平台参考坐标系原点之间的位移向量，激光雷达安装在车内后旋转矩阵和位移向量固定。
S3：惯性平台参考坐标系转化为周期开始瞬时坐标系，其转化公式为：
其中(Xn Yn Zn)T是相对于周期开始坐标轴n*t时刻的移动坐标；(ΔXi ΔXi ΔXi)T表示i时刻汽车行驶的位移向量。
综上所述得到最终坐标转换公式：
进一步的，在步骤S3中位移向量是通过以下步骤得出的：
A1、在周期开始瞬时坐标系上，将汽车每个扫描周期内的行驶轨迹进行微分，该周期内汽车做匀速直线运动，则汽车行驶的距离为：
l＝v×t
其中v为汽车的速度，t为运行时间；
A2、初始时A点经过t时间到达B点，将AB之间的距离看成等于A到B的行驶轨迹；此时AB向量相对于A瞬时的坐标(ΔX ΔY ΔZ)T有以下公式：
其中RΔI为旋转公式：RΔI＝R(H)R(P)R(R)
其中R(H)、R(R)、R(P)分别是惯性平台参考坐标系相对于周期开始瞬时坐标系的姿态角，l为在该时间段内汽车的位移；
A3、由此计算出B点相对于起始时刻A的坐标为(XB YB ZB)T：
(Δ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：瞬时激光束坐标系转化为激光扫描参考坐标系，根据瞬时激光与激光雷达坐标系的夹角和激光飞行的距离，解算出激光脚点相对于激光雷达坐标系的坐标：
其中(XL YL ZL)T为激光脚点在激光扫描参考坐标系的坐标，该激光扫描参考坐标系为右手坐标系，a为瞬时激光束在XOZ平面内的投影与X轴的夹角；β为瞬时激光束在XOY平面的投影与X轴的夹角。
S2：激光扫描参考坐标系转化惯性平台参考坐标系，测量激光雷达与惯性导航平台的安置夹角和位移，根据旋转平移公式，解算激光脚点相对于惯性平台参考坐标系的坐标；本步骤中的旋转平移公式为：
其中，(XI YI ZI)T为激光脚点相对于惯性平台参考坐标系的坐标；RI为由激光扫描参考坐标系和惯性平台参考坐标系之间的安置夹角得到的旋转矩阵；(ΔXL ΔYL ΔZL)T为激光扫描参考坐标系原点和惯性平台参考坐标系原点之间的位移向量，激光雷达安装在车内后旋转矩阵和位移向量固定。
S3：惯性平台参考坐标系转化为周期开始瞬时坐标系，其使用的方向角是瞬时INS数值与周期开始时刻方向角的差值，避免了INS定位误差随时间而增大，且每次使用之前不需要较长的初始对准时间；其转化公式为：
其中(Xn Yn Zn)T是相对于周期开始坐标轴n*t时刻的移动坐标；(ΔXi ΔXi ΔXi)T表示i时刻汽车行驶的位移向量。
综上所述得到最终坐标转换公式：
进一步的，在步骤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有以下公式：
其中RΔI为旋转公式：RΔI＝R(H)R(P)R(R)
其中R(H)、R(R)、R(P)分别是惯性平台参考坐标系相对于周期开始瞬时坐标系的姿态角，l为在该时间段内汽车的位移；
A3、由此计算出B点相对于起始时刻A的坐标为(XB YB ZB)T：
(ΔX ΔY ΔZ)T汽车行驶的位移向量；由此可以得到各个点相对于A瞬时坐标系的坐标。
本发明可以简化激光雷达***坐标转换模型，不用将坐标系转化成WGS-84坐标系，减小了整体数据值的大小；以某个时刻的INS位置为基础建立坐标系，简化了数据处理过程；在很短时间内认为INS***差值量不变，减少计算量，加快数据解算速度。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。
Claims (1)
1.基于车体坐标系的激光雷达坐标转换方法,其特征在于，具体步骤如下：
S1：瞬时激光束坐标系转化为激光扫描参考坐标系，根据瞬时激光与激光雷达坐标系的夹角和激光飞行的距离，解算出激光脚点相对于激光雷达坐标系的坐标：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>X</mi>
<mi>L</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Y</mi>
<mi>L</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mi>L</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>L</mi>
<mi> </mi>
<mi>cos</mi>
<mi> </mi>
<mi>a</mi>
<mi> </mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>L</mi>
<mi> </mi>
<mi>cos</mi>
<mi> </mi>
<mi>a</mi>
<mi> </mi>
<mi>sin</mi>
<mi>&beta;</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>L</mi>
<mi> </mi>
<mi>sin</mi>
<mi> </mi>
<mi>a</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中(XL YL ZL)T为激光脚点在激光扫描参考坐标系的坐标，该激光扫描参考坐标系为右手坐标系，a为瞬时激光束在XOZ平面内的投影与X轴的夹角；β为瞬时激光束在XOY平面的投影与X轴的夹角；
S2：激光扫描参考坐标系转化惯性平台参考坐标系，测量激光雷达与惯性导航平台的安置夹角和位移，根据旋转平移公式，解算激光脚点相对于惯性平台参考坐标系的坐标；本步骤中的旋转平移公式为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>X</mi>
<mi>I</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Y</mi>
<mi>I</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mi>I</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<msub>
<mi>R</mi>
<mi>I</mi>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>X</mi>
<mi>L</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Y</mi>
<mi>L</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mi>L</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&Delta;X</mi>
<mi>L</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&Delta;Y</mi>
<mi>L</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&Delta;Z</mi>
<mi>L</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中，(XI YI ZI)T为激光脚点相对于惯性平台参考坐标系的坐标；RI为由激光扫描参考坐标系和惯性平台参考坐标系之间的安置夹角得到的旋转矩阵；(ΔXL ΔYL ΔZL)T为激光扫描参考坐标系原点和惯性平台参考坐标系原点之间的位移向量，激光雷达安装在车内后旋转矩阵和位移向量固定；
S3：惯性平台参考坐标系转化为周期开始瞬时坐标系，其转化公式为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>X</mi>
<mi>n</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Y</mi>
<mi>n</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mi>n</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
<mi>n</mi>
</munderover>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&Delta;X</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&Delta;Y</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&Delta;Z</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中(Xn Yn Zn)T是相对于周期开始坐标轴n*t时刻的移动坐标；(ΔXi ΔXi ΔXi)T表示i时刻汽车行驶的位移向量；
综上所述得到最终坐标转换公式：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>X</mi>
<mi>n</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Y</mi>
<mi>n</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mi>n</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<msub>
<mi>R</mi>
<mi>G</mi>
</msub>
<mrow>
<mo>&lsqb;</mo>
<mrow>
<msub>
<mi>R</mi>
<mi>I</mi>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>X</mi>
<mi>L</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Y</mi>
<mi>L</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mi>L</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&Delta;X</mi>
<mi>L</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&Delta;Y</mi>
<mi>L</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&Delta;Z</mi>
<mi>L</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mo>&rsqb;</mo>
</mrow>
<mo>+</mo>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
<mi>n</mi>
</munderover>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>X</mi>
<mi>i</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Y</mi>
<mi>i</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mi>i</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
所述位移向量是通过以下步骤得出的：
A1、在周期开始瞬时坐标系上，将汽车每个扫描周期内的行驶轨迹进行微分，该周期内汽车做匀速直线运动，则汽车行驶的距离为：
l＝v×t
其中v为汽车的速度，t为运行时间；
A2、初始时A点经过t时间到达B点，将AB之间的距离看成等于A到B的行驶轨迹；此时AB向量相对于A瞬时的坐标(ΔX ΔY ΔZ)T有以下公式：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&Delta;</mi>
<mi>X</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&Delta;</mi>
<mi>Y</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&Delta;</mi>
<mi>Z</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<msub>
<mi>R</mi>
<mrow>
<mi>&Delta;</mi>
<mi>I</mi>
</mrow>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>l</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中RΔI为旋转公式：RΔI＝R(H)R(P)R(R)
<mrow>
<mi>R</mi>
<mrow>
<mo>(</mo>
<mi>R</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<mi>R</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<mi>R</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<mi>R</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>n</mi>
<mi>R</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mrow>
<mi>R</mi>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<mi>P</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<mi>P</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<mi>P</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<mi>P</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mrow>
<mi>R</mi>
<mrow>
<mo>(</mo>
<mi>H</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<mi>H</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<mi>H</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<mi>H</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<mi>H</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中R(H)、R(R)、R(P)分别是惯性平台参考坐标系相对于周期开始瞬时坐标系的姿态角，l为在该时间段内汽车的位移；
A3、计算出B点相对于起始时刻A的坐标为(XB YB ZB)T：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>X</mi>
<mi>B</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Y</mi>
<mi>B</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mi>B</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>X</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>Y</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>Z</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
(ΔX ΔY ΔZ)T为汽车行驶的位移向量；由此可以得到各个点相对于A点瞬时坐标系的坐标。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510634093.8A CN105277950B (zh) | 2015-09-29 | 2015-09-29 | 基于车体坐标系的激光雷达坐标转换方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510634093.8A CN105277950B (zh) | 2015-09-29 | 2015-09-29 | 基于车体坐标系的激光雷达坐标转换方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105277950A CN105277950A (zh) | 2016-01-27 |
| CN105277950B true CN105277950B (zh) | 2017-09-15 |
Family
ID=55147292
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510634093.8A Active CN105277950B (zh) | 2015-09-29 | 2015-09-29 | 基于车体坐标系的激光雷达坐标转换方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105277950B (zh) |
Families Citing this family (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018008085A1 (ja) * | 2016-07-05 | 2018-01-11 | 三菱電機株式会社 | 認知領域推定装置、認知領域推定方法および認知領域推定プログラム |
| CN106405555B (zh) * | 2016-09-23 | 2019-01-01 | 百度在线网络技术（北京）有限公司 | 用于车载雷达***的障碍物检测方法和装置 |
| CN109001711B (zh) * | 2018-06-05 | 2020-06-26 | 北京智行者科技有限公司 | 多线激光雷达标定方法 |
| CN109029277A (zh) * | 2018-06-27 | 2018-12-18 | 常州沃翌智能科技有限公司 | 一种隧道形变监测***及方法 |
| CN109459759B (zh) * | 2018-11-13 | 2020-06-30 | 中国科学院合肥物质科学研究院 | 基于四旋翼无人机激光雷达***的城市地形三维重建方法 |
| CN111398980B (zh) * | 2018-12-29 | 2023-06-06 | 广东瑞图万方科技股份有限公司 | 一种机载LiDAR数据处理的方法及装置 |
| CN109974717B (zh) * | 2019-03-13 | 2021-05-25 | 浙江吉利汽车研究院有限公司 | 一种地图上目标点的重定位方法、装置及终端 |
| CN110200552B (zh) * | 2019-06-20 | 2020-11-13 | 小狗电器互联网科技（北京）股份有限公司 | 对激光雷达的测量端点去畸变的方法和扫地机 |
| CN110779517A (zh) * | 2019-11-08 | 2020-02-11 | 北京煜邦电力技术股份有限公司 | 激光雷达的数据处理方法及装置、存储介质和计算机终端 |
| CN112261221B (zh) * | 2020-09-21 | 2021-10-26 | 电子科技大学 | 一种基于智能终端的人体跌倒检测方法 |
| CN112946619B (zh) * | 2021-02-06 | 2024-03-19 | 宁波泽为科技有限公司 | 雷达探测目标绝对定位的方法、装置、***及介质 |
| CN113745808B (zh) * | 2021-08-11 | 2023-05-16 | 中国电子科技集团公司第三十八研究所 | 一种天线与转台齐次坐标变换模块 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2001215275A (ja) * | 2000-02-03 | 2001-08-10 | Mitsubishi Electric Corp | レーザレーダ装置 |
| CN103020966A (zh) * | 2012-12-04 | 2013-04-03 | 南京大学 | 一种基于建筑物轮廓约束的航空与地面LiDAR数据自动配准方法 |
| CN104198765A (zh) * | 2014-09-15 | 2014-12-10 | 大连楼兰科技股份有限公司 | 车辆运动加速度检测的坐标系转换方法 |
-
2015
- 2015-09-29 CN CN201510634093.8A patent/CN105277950B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2001215275A (ja) * | 2000-02-03 | 2001-08-10 | Mitsubishi Electric Corp | レーザレーダ装置 |
| CN103020966A (zh) * | 2012-12-04 | 2013-04-03 | 南京大学 | 一种基于建筑物轮廓约束的航空与地面LiDAR数据自动配准方法 |
| CN104198765A (zh) * | 2014-09-15 | 2014-12-10 | 大连楼兰科技股份有限公司 | 车辆运动加速度检测的坐标系转换方法 |
Non-Patent Citations (1)
| Title |
|---|
| 车载激光雷达农田三维地形测量方法研究与***开发;董康;《中国优秀硕士学位论文全文数据库 基础科学辑》;20130215;第28-29页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105277950A (zh) | 2016-01-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105277950B (zh) | 基于车体坐标系的激光雷达坐标转换方法 | |
| Liu et al. | Automated vehicle sideslip angle estimation considering signal measurement characteristic | |
| KR102464758B1 (ko) | 추가 차량의 결정된 요 파라미터(들)에 근거한 자율주행 차량 제어 | |
| CN113819914B (zh) | 一种地图构建方法及装置 | |
| CN105015521B (zh) | 一种基于磁钉的大型车辆自动停靠装置 | |
| JP6161697B2 (ja) | ハンドルを有する自律車両の進路を制御する方法 | |
| Xiong et al. | Application improvement of A* algorithm in intelligent vehicle trajectory planning | |
| CN104943694A (zh) | 用于确定和补偿传感器的未对准的***和方法 | |
| CN102495631A (zh) | 一种无人驾驶车辆跟踪预定轨迹的智能控制方法 | |
| CN111142091A (zh) | 一种融合车载信息的自动驾驶***激光雷达在线标定方法 | |
| CN104428686A (zh) | 用于获得车辆位置的方法和车辆 | |
| Nguyen | Loosely coupled GPS/INS integration with Kalman filtering for land vehicle applications | |
| Liu et al. | Optimum path-tracking control for inverse problem of vehicle handling dynamics | |
| CN113819905A (zh) | 一种基于多传感器融合的里程计方法及装置 | |
| US20220340160A1 (en) | Systems and methods for simulation supported map quality assurance in an autonomous vehicle context | |
| CN113252022A (zh) | 一种地图数据处理方法及装置 | |
| Liu et al. | Application of optimal control method to path tracking problem of vehicle | |
| CN111708010B (zh) | 一种可移动设备的定位方法、装置、***及可移动设备 | |
| Domina et al. | Path following controller for autonomous vehicles | |
| US11794737B2 (en) | Vehicle operation | |
| Wang et al. | Research on Motion Distortion Correction Method of Intelligent Vehicle Point Cloud Based on High Frequency Inertial Measurement Unit. | |
| CN104280248B (zh) | 一种轨道式车载测距传感器性能试验*** | |
| Gao et al. | State observation of nonlinear off-road vehicle system under complex maneuver condition | |
| CN112776820A (zh) | 一种用于智能汽车测试的驾驶员指示***及方法 | |
| Kim et al. | Vehicle localization using radar calibration with disconnected GPS |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| C41 | Transfer of patent application or patent right or utility model | ||
| SE01 | Entry into force of request for substantive examination | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20160128 Address after: 116023 Dalian province high tech park, Hui Park, No. 7, take off the garden of the 2 phase of the 11 floor of the Applicant after: Dalian Loulan Technology Co., Ltd. Address before: 116023 Dalian province high tech park, Hui Park, No. 7, take off the garden of the 2 phase of the 11 floor of the Applicant before: XI'AN ZHIJI ANTENNA TECHNOLOGY CO., LTD. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |