CN103090863A - 一种动态平台姿态和高度测量方法 - Google Patents
一种动态平台姿态和高度测量方法 Download PDFInfo
- Publication number
- CN103090863A CN103090863A CN2013100385211A CN201310038521A CN103090863A CN 103090863 A CN103090863 A CN 103090863A CN 2013100385211 A CN2013100385211 A CN 2013100385211A CN 201310038521 A CN201310038521 A CN 201310038521A CN 103090863 A CN103090863 A CN 103090863A
- Authority
- CN
- China
- Prior art keywords
- platform
- plane
- formula
- equation
- height
- 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.)
- Granted
Links
Images
Landscapes
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明提供一种动态平台的姿态和高度测量方法。技术方案是利用两台正交安装、竖直向下扫描的二维激光扫描仪获得路面扫描线;使用RANSAC算法估计出扫描线的表达式方程,利用两扫描线的表达式方程得到道路平面方程;以道路平面方程为参考解算平台的姿态和高度。本发明使用的设备简单,成本低。测量结果不受运动平台自身加速度的影响,没有漂移,测量精度高。
Description
技术领域
本发明属于光电测控技术领域,涉及一种姿态和高度测量方法,尤其是一种动态平台姿态和高度测量方法,进一步是涉及一种利用正交二维激光扫描原理进行路面上的动态平台姿态和高度测量的方法。
背景技术
目前动态平台姿态的高精度实时测量主要使用基于陀螺和加速度计的惯性测量***,并常与GPS(Global Position System,全球定位***)组合以保证长时间的测量精度,但这类***工作前的准备时间较长,价格昂贵,不适于在一些实时测量***中应用。基于GPS的姿态测量技术是当前研究的热点领域,但卫星信号在一些情况下可能无法正常接收,且姿态测量精度受限于接收机的基线长度。倾角传感器通过测量重力加速度在其敏感轴上的分量,解算出传感器相对于水平面的姿态角,主要用于静态条件下载体的姿态测量,仅在载体姿态角变化速度较小时精度较高,动态条件下载体平台的姿态角的快速变化将导致测量精度显著下降。
动态平台在实际工作过程中,由于运动、机械振动、平台(或安装支架)形变等原因,其姿态和高度都会发生变化,平台上安装的传感器的姿态和高度也会随之变化,这种变化在许多高精度测量***中都是必须实时测量并补偿的。比如,单目视觉定位***就需要实时测量相机的拍摄姿态和高度,这是保证定位精度的关键技术之一。
鉴于以上分析,有必要深入研究有别于传统方法且性价比更高的动态平台的姿态和高度测量方法。
发明内容
本发明要解决的技术问题是:提供一种动态平台的姿态和高度测量方法,实时测量出动态平台由于运动、机械振动、平台(或安装支架)形变等原因导致的姿态和高度变化。该方法算法简单、设备成本较为低廉、平台姿态和高度的测量频率和精度较高。
本发明的技术方案是:基于正交二维激光扫描的平台姿态和高度测量方法的基本思路为:由两台正交安装、竖直向下扫描的二维激光扫描仪获得路面扫描线;使用RANSAC算法估计出扫描线的表达式方程,利用两扫描线的表达式方程得到道路平面方程;以道路平面方程为参考解算平台的姿态和高度。
本发明所采用的具体技术方案如下:
将两台二维激光扫描仪固定在姿态和高度待测的平台上,两台扫描仪均以竖直向下的方式同步扫描路面,并且两台扫描仪的扫描平面相互正交。建立扫描仪***坐标系XS-scanner,原点为两个扫描仪的扫描平面与平台所在平面三个平面的交点,X轴平行于平台所在平面但垂直于平台前进方向,Y轴平行于平台所在平面且与平台前进方向相反,Z轴垂直于平台所在平面且向上为正向,X轴、Y轴和Z轴构成右手坐标系。实施下述步骤:
第一步,采集数据。
平台在路面上移动,即平台处于运动状态时。两台扫描仪S1和S2同步扫描,采集数据。设S1和S2在某一时刻T分别扫描获得一条扫描线,令其分别为L1和L2。每条扫描线由n个扫描点组成,L1在扫描仪***坐标系XS-scanner中第i个扫描点的坐标为(i=1,2,…,n),L2在扫描仪***坐标系XS-scanner中第i个扫描点的坐标为(i=1,2,…,n)。
第二步,计算T时刻扫描线L1和L2的表达式方程。
利用扫描线L1和L2所包含的扫描点坐标,使用RANSAC算法估计T时刻L1和L2的表达式方程,分别为:
上式中,(x,y,z)表示在扫描仪***坐标系XS-scanner中任意一点的坐标。
第三步,计算道路平面方程:
由扫描线L1和L2的直线方程,得到T时刻道路平面方程,如公式二所示:
其中:
第四步,解算平台姿态角和高度。
T时刻平台姿态角由如下公式计算出:
其中φ和θ分别为平台的俯仰角和横滚角。
T时刻平台上待测点M的高度值H由如下公式计算出:
其中,(xM, M,zM)为待测点M在扫描仪***坐标系中的坐标,事先用测量仪器测量得到。
则利用上述方法可测量任意时刻平台的姿态和平台上任意待测点的高度。
本发明的有益效果是:利用两台扫描仪实时获得的扫描线,可实时测量动态平台的高度和姿态。相对于传统姿态测量方法,使用的设备简单,成本低。测量结果不受运动平台自身加速度的影响,没有漂移,测量精度高。
附图说明
图1为动态平台姿态和高度测量方法的原理示意图;
图2为本发明的具体实施流程图;
图3为采用本发明的平台俯仰角测量结果与ADU传感器测量结果的对比图;
图4为采用本发明的平台横滚角测量结果与ADU传感器测量结果的对比图;
图5为采用本发明的平台实时高度测量结果。
具体实施方式
以下将结合说明书附图对本发明做进一步详细说明。
图1为动态平台姿态和高度测量方法的原理示意图,表示出扫描仪***坐标系的定义以及设备安装方式,X-Y-Z坐标系为扫描仪***坐标系XS-scaner,原点为两个扫描仪的扫描平面与平台所在平面三个平面的交点O,X轴平行于平台所在平面但垂直于平台前进方向,Y轴平行于平台所在平面且与平台前进方向相反,Z轴垂直于平台所在平面且向上为正向,X轴、Y轴和Z轴构成右手坐标系;将两台二维激光扫描仪S1和S2固定在姿态和高度待测的平台上,两台扫描仪均以竖直向下的方式同步扫描路面,并且两台扫描仪的扫描平面相互正交。以位于平台上任一高度的某固定点M作为待测点,m点为固定点M在路面上的投影点,显然线段Mm垂直于路面,其长度H即为待测高度。在任意时刻T由扫描仪S1和S2得到两条路面扫描线,利用RANSAC算法对两条路面扫描线实施直线估计,分别得到对应的直线L1和L2的方程表达式,RANSAC算法原理的介绍参见《计算机视觉中的数学方法》一书(科学出版社出版,吴福朝著)第338页至344页。由于L1和L2为位于路面上的直线,联立L1和L2的直线方程得到T时刻道路平面在X-Y-Z坐标系下的表达式方程。在获得T时刻的道路平面方程后,以道路平面的法线方向为参考获得T时刻的平台姿态,通过计算M点到道路平面的距离计算得到T时刻的相机高度。
图2给出了本发明具体实施的流程图。第一步,采集数据;第二步,计算T时刻扫描线L1和L2的表达式方程;第三步,计算道路平面方程;第四步,解算平台姿态角和待测点高度。
采用本发明的具体实施方式进行了实验,验证了本发明测量运动平台姿态和高度的精度。具体实施例中使用的主要设备说明如下:用于数据处理的计算机主要配置为:CPU为Intel Core2Duo Processor2.8GHz,使用4GB DDR2双通道内存;二维激光扫描仪采用的是德国SICK公司的LMS291-S05型激光扫描仪;姿态角传感器采用的是星网宇达公司的ADU5600传感器(以下简称ADU传感器),测量频率为100Hz,动态测量精度为1°。
基于加速度计的姿态传感器的测量精度受车辆加减速的影响较大,而本方法以道路平面为参考基准测量平台姿态,因此原理上车辆加减速不会影响其姿态测量结果。为验证该结论,实验中采用ADU传感器方法和本发明提供的方法进行对比。车辆在路面反复进行加速和减速运动,期间经历四次转弯,ADU传感器与扫描仪同步测量,获得的平台姿态和待测点高度测量结果分别如图3、图4和图5所示。如图3所示,虚线表示ADU传感器测得的平台俯仰角,实线表示采用本方法测得的平台俯仰角;车辆加减速对ADU传感器测量结果影响较大,获得的车辆俯仰角出现较大偏移,曲线在车辆停止后仍然有向上漂移的趋势,而本方法获得的车辆俯仰角波动较小,这与车辆行驶于路面的实验条件相符合。由于车辆在快速转弯过程中会出现明显的侧倾,如图4所示,虚线表示ADU传感器测得的平台横滚角,实线表示采用本方法测得的平台横滚角,两种方法均检测出了车辆侧倾导致的横滚角变化,曲线中出现的四个波峰对应着车辆的四次转弯运动,按常理推知:车辆在发生左右摆动倾斜时,其横滚角应以0°为中心波动,如图4所示,本文方法得到的曲线与该常理吻合较好,而由ADU传感器得到的曲线在后半段出现了明显的整体漂移误差。图5为运动平台行进过程中高度实时测量曲线,运动平台初始架设高度为1.45m(即平台上某一个固定点的高度),由于在行进过程中运载车辆加速、减速和转弯,平台高度不断变化,所以如图5所示,平台高度实时变化且围绕1.45m上下波动。
Claims (1)
1.一种动态平台姿态和高度测量方法,其特征在于,将两台二维激光扫描仪固定在姿态和高度待测的平台上,两台扫描仪均以竖直向下的方式同步扫描路面,并且两台扫描仪的扫描平面相互正交;建立扫描仪***坐标系XS-scanner,原点为两个扫描仪的扫描平面与平台所在平面三个平面的交点,X轴平行于平台所在平面但垂直于平台前进方向,Y轴平行于平台所在平面且与平台前进方向相反,Z轴垂直于平台所在平面且向上为正向,X轴、Y轴和Z轴构成右手坐标系;实施下述步骤:
第一步,采集数据:
平台在路面上移动时,两台扫描仪S1和S2同步扫描,采集数据;设S1和S2在某一时刻T分别扫描获得一条扫描线,令其分别为L1和L2;每条扫描线由n个扫描点组成,L1在扫描仪***坐标系XS-scanner中第i个扫描点的坐标为(i=1,2,…,n),L2在扫描仪***坐标系XS-scanner中第i个扫描点的坐标为(i=1,2,…,n);
第二步,计算T时刻扫描线L1和L2的表达式方程:
利用扫描线L1和L2所包含的扫描点坐标,使用RANSAC算法估计T时刻L1和L2的表达式方程,分别为:
上式中,(x,y,z)表示在扫描仪***坐标系XS-scanner中任意一点的坐标;
第三步,计算道路平面方程:
由扫描线L1和L2的直线方程,得到T时刻道路平面方程,如公式二所示:
其中:
第四步,解算平台姿态角和高度:
T时刻平台姿态角由如下公式计算出:
其中φ和θ分别为平台的俯仰角和横滚角;
T时刻平台上待测点M的高度值H由如下公式计算出:
其中,(xM,yM,zM)为待测点M在扫描仪***坐标系中的坐标。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310038521.1A CN103090863B (zh) | 2013-01-31 | 2013-01-31 | 一种动态平台姿态和高度测量方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310038521.1A CN103090863B (zh) | 2013-01-31 | 2013-01-31 | 一种动态平台姿态和高度测量方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103090863A true CN103090863A (zh) | 2013-05-08 |
CN103090863B CN103090863B (zh) | 2015-04-08 |
Family
ID=48203729
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310038521.1A Active CN103090863B (zh) | 2013-01-31 | 2013-01-31 | 一种动态平台姿态和高度测量方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103090863B (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105137468A (zh) * | 2015-09-24 | 2015-12-09 | 北京理工大学 | 光电式gps盲区环境下车辆可持续导航数据采集装置及方法 |
CN108398672A (zh) * | 2018-03-06 | 2018-08-14 | 厦门大学 | 基于前倾2d激光雷达移动扫描的路面与障碍检测方法 |
CN109532937A (zh) * | 2018-11-14 | 2019-03-29 | 成都天佑路航轨道交通科技有限公司 | 一种车载地铁限界检测方法及其检测*** |
CN109945785A (zh) * | 2019-03-26 | 2019-06-28 | 湖南安华鼎科技有限公司 | 一种平台倾角和高度实时测量方法及*** |
CN109945834A (zh) * | 2019-03-26 | 2019-06-28 | 长沙闪控信息科技有限公司 | 一种平台倾角和高度实时测量***及方法 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2003141511A (ja) * | 2001-11-07 | 2003-05-16 | Kawasaki Heavy Ind Ltd | 3次元形状認識方法、装置およびプログラム |
CN101114022A (zh) * | 2007-09-04 | 2008-01-30 | 国家***第二海洋研究所 | 无姿态信息条件下的航空多光谱扫描仪几何粗校正方法 |
CN101609149A (zh) * | 2009-07-13 | 2009-12-23 | 北京航空航天大学 | 一种提高机载激光雷达定姿精度的方法 |
CN101672655A (zh) * | 2009-09-25 | 2010-03-17 | 北京航空航天大学 | 基于双轴光电测量***的地面车位置、姿态及航向测量方法 |
CN101762273A (zh) * | 2010-02-01 | 2010-06-30 | 北京理工大学 | 一种深空探测器软着陆自主光学导航方法 |
CN102358325A (zh) * | 2011-06-29 | 2012-02-22 | 株洲南车时代电气股份有限公司 | 基于绝对坐标测量参考系的轨道参数测量装置及其方法 |
CN102564416A (zh) * | 2011-12-30 | 2012-07-11 | 浙江国自机器人技术有限公司 | 一种镜面清洁机器人进行三维重构与定位的***及方法 |
-
2013
- 2013-01-31 CN CN201310038521.1A patent/CN103090863B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2003141511A (ja) * | 2001-11-07 | 2003-05-16 | Kawasaki Heavy Ind Ltd | 3次元形状認識方法、装置およびプログラム |
CN101114022A (zh) * | 2007-09-04 | 2008-01-30 | 国家***第二海洋研究所 | 无姿态信息条件下的航空多光谱扫描仪几何粗校正方法 |
CN101609149A (zh) * | 2009-07-13 | 2009-12-23 | 北京航空航天大学 | 一种提高机载激光雷达定姿精度的方法 |
CN101672655A (zh) * | 2009-09-25 | 2010-03-17 | 北京航空航天大学 | 基于双轴光电测量***的地面车位置、姿态及航向测量方法 |
CN101762273A (zh) * | 2010-02-01 | 2010-06-30 | 北京理工大学 | 一种深空探测器软着陆自主光学导航方法 |
CN102358325A (zh) * | 2011-06-29 | 2012-02-22 | 株洲南车时代电气股份有限公司 | 基于绝对坐标测量参考系的轨道参数测量装置及其方法 |
CN102564416A (zh) * | 2011-12-30 | 2012-07-11 | 浙江国自机器人技术有限公司 | 一种镜面清洁机器人进行三维重构与定位的***及方法 |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105137468A (zh) * | 2015-09-24 | 2015-12-09 | 北京理工大学 | 光电式gps盲区环境下车辆可持续导航数据采集装置及方法 |
CN108398672A (zh) * | 2018-03-06 | 2018-08-14 | 厦门大学 | 基于前倾2d激光雷达移动扫描的路面与障碍检测方法 |
CN108398672B (zh) * | 2018-03-06 | 2020-08-14 | 厦门大学 | 基于前倾2d激光雷达移动扫描的路面与障碍检测方法 |
CN109532937A (zh) * | 2018-11-14 | 2019-03-29 | 成都天佑路航轨道交通科技有限公司 | 一种车载地铁限界检测方法及其检测*** |
CN109945785A (zh) * | 2019-03-26 | 2019-06-28 | 湖南安华鼎科技有限公司 | 一种平台倾角和高度实时测量方法及*** |
CN109945834A (zh) * | 2019-03-26 | 2019-06-28 | 长沙闪控信息科技有限公司 | 一种平台倾角和高度实时测量***及方法 |
CN109945834B (zh) * | 2019-03-26 | 2021-10-12 | 长沙闪控信息科技有限公司 | 一种平台倾角和高度实时测量***及方法 |
Also Published As
Publication number | Publication date |
---|---|
CN103090863B (zh) | 2015-04-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103090863B (zh) | 一种动态平台姿态和高度测量方法 | |
CN103454619B (zh) | 一种星载微波跟瞄雷达的电轴光学标定***及其标定方法 | |
CN105737852B (zh) | 激光测距仪位置测量与校正***及方法 | |
CN103925872A (zh) | 一种用于获取目标物空间分布的激光扫描测量装置和方法 | |
CN202599371U (zh) | 一种视觉传感器和三坐标测量机集成的测量装置 | |
CN109990761B (zh) | 水平度测量***及水平度测量方法 | |
CN101975560B (zh) | 一种面阵ccd靶面与安装定位面平行度的光学检测方法 | |
Charalampous et al. | Measuring sub-mm structural displacements using QDaedalus: a digital clip-on measuring system developed for total stations | |
JP6044971B2 (ja) | 推定方位角評価装置、移動体端末装置、推定方位角評価装置の制御プログラム、コンピュータ読み取り可能な記録媒体、推定方位角評価装置の制御方法、および測位装置 | |
CN103808256A (zh) | 一种非接触式物体平面运动测量装置及实现方法 | |
CN103162677A (zh) | 一种数字地质罗盘仪及地质体产状的测量方法 | |
CN105717499B (zh) | 激光测距仪偏转角测量与校正***及方法 | |
CN109470209A (zh) | 一种基于三轴加速度计的倾角测量方法 | |
CN109945785A (zh) | 一种平台倾角和高度实时测量方法及*** | |
CN103900603A (zh) | 平面或曲面内无导轨二维运动物体的位移和姿态的非接触测量方法 | |
JP6052090B2 (ja) | 磁気測定システム | |
CN102519671A (zh) | 一种用于测量陀螺仪静平衡的基于双目视觉的空间位姿测量装置 | |
CN105953820B (zh) | 一种惯性测量组合动态导航性能的光学标定装置 | |
CN205808428U (zh) | 惯性测量组合动态导航性能的光学标定*** | |
CN102519672B (zh) | 一种用于测量陀螺仪静平衡的基于单目原理的六自由度位姿测量装置 | |
CN103822768A (zh) | 一种超低频水平向振动台导轨不平顺静态补偿方法 | |
Bitenc et al. | Quantification of rock joint roughness using terrestrial laser scanning | |
CN105258699B (zh) | 基于重力实时补偿的惯性导航方法 | |
CN204329960U (zh) | 基于重锤的倾角动态测量装置 | |
CN202903175U (zh) | 一种多功能激光测距仪 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant |