CN113405553B - 一种无人驾驶特种车辆姿态测量方法 - Google Patents

一种无人驾驶特种车辆姿态测量方法 Download PDF

Info

Publication number
CN113405553B
CN113405553B CN202011205191.7A CN202011205191A CN113405553B CN 113405553 B CN113405553 B CN 113405553B CN 202011205191 A CN202011205191 A CN 202011205191A CN 113405553 B CN113405553 B CN 113405553B
Authority
CN
China
Prior art keywords
unmanned
vehicle
attitude
gesture
fusion
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
Application number
CN202011205191.7A
Other languages
English (en)
Other versions
CN113405553A (zh
Inventor
赵忠义
黄海龙
张健永
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Liaoning University of Technology
Original Assignee
Liaoning University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Liaoning University of Technology filed Critical Liaoning University of Technology
Priority to CN202011205191.7A priority Critical patent/CN113405553B/zh
Publication of CN113405553A publication Critical patent/CN113405553A/zh
Application granted granted Critical
Publication of CN113405553B publication Critical patent/CN113405553B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种无人驾驶特种车辆姿态测量方法，包括包括车体的姿态矩阵，基于陀螺仪的姿态解算，基于加速度计的姿态解算，基于双天线的姿态解算，多传感器系统的数据融合，基于四元数的卡尔曼滤波器的解算，并包括如下步骤：将姿态矩阵记为
Figure DDA0002756744550000011
通过陀螺仪测算车无人车姿态角；通过加速度计测算无人车的俯仰角和倾斜角，本发明的有益效果：本发明通过结合基于陀螺仪、基于加速度计以及基于双天线GNSS三种算法的优缺点，对车体上搭载的多个传感器输出相应的数据进行解算，并通过多源信息融合技术以及基于四元数的扩展卡尔曼滤波器，提高了姿态解算结果的精确度，比单独的传感器输出的数据更加可靠，减少了多余信息的干扰而增加了关联信息的综合利用。

Description

一种无人驾驶特种车辆姿态测量方法
技术领域：
本发明属于测量方法技术领域，特别涉及一种无人驾驶特种车辆姿态测量方法。
背景技术：
随着社会的进步与发展，人们的生活也日益得到了质与量的改变，汽车这一物品也早已经进入千家万户，为人们的生活带来了极大的便利。而伴随着交通事故的发生，以及部分危险工作不再适宜人们的参与，导致了社会对无人驾驶车以及无人驾驶技术的探索研究。无人驾驶车的出现不仅减轻了道路交通的压力，也为环境保护起到了推动的作用。对于一些危险性场所，可以使用无人驾驶车进入工作，保障了人们的生命安全与物力财产。无人驾驶车能够安全可靠的行驶与工作，离不开外界环境的信息反馈。在其工作时，实时地获取它的姿态信息是极其重要的，这不仅可以向外界传递出当前所处的状况，也可以让外界可以更好地控制无人驾驶车，虽然随着科技的发展，传感器的发展也得到了更新，但是目前很难用单一的传感器来获取无人驾驶车的姿态信息，单一的传感器输出的信息，难免会随着时间而产生漂移与累积误差。
发明内容：
本发明的目的就在于为了解决上述问题而提供一种，解决了现有的设备所存在的缺点。
为了解决上述问题，本发明提供了一种技术方案：
一种无人驾驶特种车辆姿态测量方法，包括包括车体的姿态矩阵，基于陀螺仪的姿态解算，基于加速度计的姿态解算，基于双天线的姿态解算，多传感器系统的数据融合，基于四元数的卡尔曼滤波器的解算，并包括如下步骤：
(A)将姿态矩阵记为
Figure GDA0004187145330000012
Figure GDA0004187145330000011
式中，ψ称为无人车的航向角，θ称为无人车的俯仰角，γ称为无人车的横滚角，这三个角构成无人车的姿态角；
(B)通过陀螺仪测算车无人车姿态角为：
Figure GDA0004187145330000021
(C)通过加速度计测算无人车的俯仰角和倾斜角为：
Figure GDA0004187145330000022
Figure GDA0004187145330000023
(D)通过双天线观测并解算无人车的航向角与俯仰角为：
Figure GDA0004187145330000024
Figure GDA0004187145330000025
(E)通过多传感器系统进行融合：包括数据层融合、特征层融合和决策层融合，并进行姿态解算；
(F)拓展卡尔曼滤波器，利用陀螺仪输出预测并更新四元数，利用加速度计和双天线GNSS输出作为观测量来修正预测，构建扩展卡尔曼滤波器，实现数据融合，得到关于无人车姿态的最优估计，输出姿态角。
作为优选，所述车体的姿态矩阵的解算方法采用四元数法进行计算。
作为优选，所述多传感器系统的数据融合是基于对多个传感器输出的信息只做过初级处理的数据，然后将这些数据进行融合处理，从而得到所需的系统信息；特征级融合先收集每个传感器的观测信息，然后对其进行特征提取，从而得到一组特征向量，利用模式识别的方法处理特征向量，得到最终所需的特征信息；决策级融合首先对每个传感器单独地获得目标的属性特征，然后分析所收集的多个特征信息进行可信度判断，接着依照相应的方法对识别结果进行分析，从而制定出最佳方案。
本发明的有益效果：
本发明通过结合基于陀螺仪、基于加速度计以及基于双天线GNSS三种算法的优缺点，对车体上搭载的多个传感器输出相应的数据进行解算，并通过多源信息融合技术以及基于四元数的扩展卡尔曼滤波器，提高了姿态解算结果的精确度，比单独的传感器输出的数据更加可靠，减少了多余信息的干扰而增加了关联信息的综合利用。
附图说明：
为了易于说明，本发明由下述的具体实施及附图作以详细描述。
图1为本发明的姿态测量结构图；
图2为本发明的数据层融合结构图；
图3为本发明的特征层融合结构图；
图4为本发明的决策层融合结构图；
图5为本发明的扩展卡尔曼滤波器流程图；
图6为本发明的扩展卡尔曼滤波器时的姿态估计结构图。
具体实施方式：
如图1-6所示，本具体实施方式采用以下技术方案：一种无人驾驶特种车辆姿态测量方法，包括车体的姿态矩阵，基于陀螺仪的姿态解算，基于加速度计的姿态解算，基于双天线的姿态解算，多传感器系统的数据融合，基于四元数的卡尔曼滤波器的解算，并包括如下步骤：
(A)将姿态矩阵记为
Figure GDA0004187145330000032
Figure GDA0004187145330000031
式中，ψ称为无人车的航向角，θ称为无人车的俯仰角，γ称为无人车的横滚角，这三个角构成无人车的姿态角；
其中所用的坐标系主要有：
地球坐标系(e系)
地理坐标系(g系)
车体坐标系(b系)
导航坐标系(n系)
(B)通过陀螺仪测算车无人车姿态角为：
Figure GDA0004187145330000041
(C)通过加速度计测算无人车的俯仰角和倾斜角为：
Figure GDA0004187145330000042
Figure GDA0004187145330000043
(D)通过双天线观测并解算无人车的航向角与俯仰角为：
Figure GDA0004187145330000044
Figure GDA0004187145330000045
(E)通过多传感器系统进行融合：包括数据层融合、特征层融合和决策层融合，并进行姿态解算；
(F)拓展卡尔曼滤波器，采用间接式滤波，误差状态预测如下所示：
Figure GDA0004187145330000046
误差协方差预测如下所示：
Figure GDA0004187145330000047
计算卡尔曼增益矩阵如下所示：
Figure GDA0004187145330000048
更新误差状态的估计值如下所示：
Figure GDA0004187145330000049
更新误差协方差如下所示：
Pk＝(I-KkHk)Pk/k-1
利用陀螺仪输出预测并更新四元数，利用加速度计和双天线GNSS输出作为观测量来修正预测，构建扩展卡尔曼滤波器，实现数据融合，得到关于无人车姿态的最优估计，输出姿态角。
其中，所述姿态矩阵的解算方法采用四元数法进行计算，姿态矩阵的求解关系是求解无人车姿态信息的首要问题，而姿态解算方法则直接影响着姿态结果的精度和计算难度，最常用的姿态解算方法有三种：欧拉角法、方向余弦法和四元数法，欧拉角法的优点是只需要求解三个微分方程，但缺点是内含超越函数，导致工作量增加；方向余弦法优点是没有欧拉方程的退化现象，缺点是需要解算九个一阶微分方程，增加了计算机的计算任务量；四元数法求解时不含超越函数，虽然微分方程比欧拉角法多一个，但是计算量相比于欧拉角法依然少了很多，且四元数规范化处理也比方向余弦法简单得多，所以计算量也比方向余弦法合理了许多。
其中，所述多传感器系统的数据融合是基于对多个传感器输出的信息只做过初级处理的数据，然后将这些数据进行融合处理，从而得到所需的系统信息；特征级融合先收集每个传感器的观测信息，然后对其进行特征提取，从而得到一组特征向量，利用模式识别的方法处理特征向量，得到最终所需的特征信息；决策级融合首先对每个传感器单独地获得目标的属性特征，然后分析所收集的多个特征信息进行可信度判断，接着依照相应的方法对识别结果进行分析，从而制定出最佳方案，数据级融合的特点是刚开始不需要对传感器数据进行过度处理，从而保留了初始数据的有用特征，防止了传感器原始数据的遗漏；特征级融合不仅可以尽量保留有用信息，也可以让状态信息避免了单一枯燥的可能性，除此之外，特征级融合并不需要通信带宽过大，避免了算法太过复杂的问题，弥补了实时性不良的缺点；决策级融合并不像数据级融合那样需要大量数据，因此融合所需的算法也较为简单，运算时间也缩短，抗干扰能力强，融合的可信度比较高，一般情况下，系统实时性会随着融合层次的升级而优化，只是融合性能会随之降低。数据融合层次有相应的优点与缺点，因此在实际应用时，应该考虑到所涉及到的因素的权重比，比如通信带宽、传感器的优劣等等，从而确定最合适的融合层次。
以上显示和描述了本发明的基本原理和主要特征和本发明的优点，本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内，本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (2)

1.一种无人驾驶特种车辆姿态测量方法，包括车体的姿态矩阵，基于陀螺仪的姿态解算，基于加速度计的姿态解算，基于双天线的姿态解算，多传感器系统的数据层融合，基于四元数的卡尔曼滤波器的解算，并包括如下步骤：
(A)将姿态矩阵记为
Figure FDA0004187145320000011
Figure FDA0004187145320000012
式中，ψ称为无人车的航向角，θ称为无人车的俯仰角，γ称为无人车的横滚角，这三个角构成无人车的姿态角；
(B)通过陀螺仪测算车无人车姿态角为：
Figure FDA0004187145320000013
(C)通过加速度计测算无人车的俯仰角θ和横滚角γ；
(D)通过双天线观测并解算无人车的航向角与俯仰角为：
Figure FDA0004187145320000014
Figure FDA0004187145320000015
(E)通过多传感器系统进行融合：包括数据层融合、特征层融合和决策层融合，并进行姿态解算；所述多传感器系统的数据层融合是基于对多个传感器输出的信息只做过初级处理的数据，然后将这些数据进行融合处理，从而得到所需的系统信息；特征层融合先收集每个传感器的观测信息，然后对其进行特征提取，从而得到一组特征向量，利用模式识别的方法处理特征向量，得到最终所需的特征信息；决策层融合首先对每个传感器单独地获得目标的属性特征，然后分析所收集的多个特征信息进行可信度判断，接着依照相应的方法对识别结果进行分析，从而制定出最佳方案；
(F)拓展卡尔曼滤波器，利用陀螺仪输出预测并更新四元数，利用加速度计和双天线GNSS输出作为观测量来修正预测，构建扩展卡尔曼滤波器，实现数据层融合，得到关于无人车姿态的最优估计，输出姿态角。
2.根据权利要求1所述的一种无人驾驶特种车辆姿态测量方法，其特征在于：所述车体的姿态矩阵的解算方法采用四元数法进行计算。
CN202011205191.7A 2020-11-30 2020-11-30 一种无人驾驶特种车辆姿态测量方法 Active CN113405553B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011205191.7A CN113405553B (zh) 2020-11-30 2020-11-30 一种无人驾驶特种车辆姿态测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011205191.7A CN113405553B (zh) 2020-11-30 2020-11-30 一种无人驾驶特种车辆姿态测量方法

Publications (2)

Publication Number Publication Date
CN113405553A CN113405553A (zh) 2021-09-17
CN113405553B true CN113405553B (zh) 2023-05-26

Family

ID=77677422

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011205191.7A Active CN113405553B (zh) 2020-11-30 2020-11-30 一种无人驾驶特种车辆姿态测量方法

Country Status (1)

Country Link
CN (1) CN113405553B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7216055B1 (en) * 1998-06-05 2007-05-08 Crossbow Technology, Inc. Dynamic attitude measurement method and apparatus
CN103245963A (zh) * 2013-05-09 2013-08-14 清华大学 双天线gnss/ins深组合导航方法及装置
CN107478223A (zh) * 2016-06-08 2017-12-15 南京理工大学 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN107525503A (zh) * 2017-08-23 2017-12-29 王伟 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN108827301A (zh) * 2018-04-16 2018-11-16 南京航空航天大学 一种改进误差四元数卡尔曼滤波机器人姿态解算方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105823481B (zh) * 2015-12-21 2019-02-12 上海华测导航技术股份有限公司 一种基于单天线的gnss-ins车辆定姿方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7216055B1 (en) * 1998-06-05 2007-05-08 Crossbow Technology, Inc. Dynamic attitude measurement method and apparatus
CN103245963A (zh) * 2013-05-09 2013-08-14 清华大学 双天线gnss/ins深组合导航方法及装置
CN107478223A (zh) * 2016-06-08 2017-12-15 南京理工大学 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN107525503A (zh) * 2017-08-23 2017-12-29 王伟 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN108827301A (zh) * 2018-04-16 2018-11-16 南京航空航天大学 一种改进误差四元数卡尔曼滤波机器人姿态解算方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
一种GNSS双天线姿态确定及点位标定方法研究;代桃高等;《全球定位系统》;20191231;第44卷(第6期);全文 *
基于DSP的GPS双天线实时姿态测量系统实现;杨铁军等;《电波科学学报》;20021225(第06期);正文第2节 *
基于多传感器的智能车辆姿态解算方法;王润民等;《测控技术》;20160918(第09期);全文 *
基于扩展Kalman滤波的无人驾驶车姿态测量研究;张健永等;《辽宁工业大学学报(自然科学版)》;20200815(第04期);全文 *
基于扩展卡尔曼滤波的车辆状态参数估计;吕梦男等;《农业装备与车辆工程》;20180531;第56卷(第5期);全文 *
旋转短基线北斗双天线/MIMU定向方法;刘莹等;《中国惯性技术学报》;20191015(第05期);全文 *

Also Published As

Publication number Publication date
CN113405553A (zh) 2021-09-17

Similar Documents

Publication Publication Date Title
CN110579740B (zh) 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN109931926B (zh) 一种基于站心坐标系的小型无人机无缝自主式导航方法
Jiancheng et al. Study on innovation adaptive EKF for in-flight alignment of airborne POS
CN110553644B (zh) 一种矿用电铲精准定位系统和方法
CN112639502A (zh) 机器人位姿估计
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN102654404B (zh) 一种提高航姿参考系统解算精度和系统抗干扰能力的方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN109781098B (zh) 一种列车定位的方法和系统
CN110285804B (zh) 基于相对运动模型约束的车辆协同导航方法
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN102981151A (zh) 相控阵雷达电控波束稳定方法
US20230408260A1 (en) Operating Modes of Magnetic Navigation Devices
CN105333869A (zh) 一种基于自适应ekf的无人侦察机同步定位与构图方法
CN102095424A (zh) 一种适合车载光纤航姿系统的姿态测量方法
CN112378399A (zh) 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法
Gu et al. Sensor fusion in mobile robot: some perspectives
Mori et al. Adaptive sensor fault detection and isolation using unscented kalman filter for vehicle positioning
CN115143954A (zh) 一种基于多源信息融合的无人车导航方法
CN115265532A (zh) 一种用于船用组合导航中的辅助滤波方法
US11680802B2 (en) Correlating overlapping magnetic measurement data from multiple magnetic navigation devices and updating a geomagnetic map with that data
Wang et al. UGV‐UAV robust cooperative positioning algorithm with object detection
CN109387198A (zh) 一种基于序贯检测的惯性/视觉里程计组合导航方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant