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

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

Info

Publication number
CN113405553A
CN113405553A CN202011205191.7A CN202011205191A CN113405553A CN 113405553 A CN113405553 A CN 113405553A CN 202011205191 A CN202011205191 A CN 202011205191A CN 113405553 A CN113405553 A CN 113405553A
Authority
CN
China
Prior art keywords
attitude
fusion
angle
calculation
unmanned vehicle
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
Application number
CN202011205191.7A
Other languages
English (en)
Other versions
CN113405553B (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)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

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

Description

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

Claims (3)

1.一种无人驾驶特种车辆姿态测量方法，包括车体的姿态矩阵，基于陀螺仪的姿态解算，基于加速度计的姿态解算，基于双天线的姿态解算，多传感器系统的数据融合，基于四元数的卡尔曼滤波器的解算，并包括如下步骤：
(A)将姿态矩阵记为
Figure FDA0002756744520000011
Figure FDA0002756744520000012
式中，ψ称为无人车的航向角，θ称为无人车的俯仰角，γ称为无人车的横滚角，这三个角构成无人车的姿态角；
(B)通过陀螺仪测算车无人车姿态角为：
Figure FDA0002756744520000013
(C)通过加速度计测算无人车的俯仰角和倾斜角为：
Figure FDA0002756744520000014
Figure FDA0002756744520000015
(D)通过双天线观测并解算无人车的航向角与俯仰角为：
Figure FDA0002756744520000016
Figure FDA0002756744520000017
(E)通过多传感器系统进行融合：包括数据层融合、特征层融合和决策层融合，并进行姿态解算；
(F)拓展卡尔曼滤波器，利用陀螺仪输出预测并更新四元数，利用加速度计和双天线GNSS输出作为观测量来修正预测，构建扩展卡尔曼滤波器，实现数据融合，得到关于无人车姿态的最优估计，输出姿态角。
2.根据权利要求1所述的一种无人驾驶特种车辆姿态测量方法，其特征在于：所述车体的姿态矩阵的解算方法采用四元数法进行计算。
3.根据权利要求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 true CN113405553A (zh) 2021-09-17
CN113405553B 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 (7)

* 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组合的自适应级联卡尔曼滤波方法
US20180017390A1 (en) * 2015-12-21 2018-01-18 Shanghai Huace Navigation Technology Ltd Method of determining gnss-ins vehicle attitude based on single antenna
CN108827301A (zh) * 2018-04-16 2018-11-16 南京航空航天大学 一种改进误差四元数卡尔曼滤波机器人姿态解算方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器

Patent Citations (7)

* 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深组合导航方法及装置
US20180017390A1 (en) * 2015-12-21 2018-01-18 Shanghai Huace Navigation Technology Ltd Method of determining gnss-ins vehicle attitude based on single antenna
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双天线姿态确定及点位标定方法研究", 《全球定位系统》 *
刘莹等: "旋转短基线北斗双天线/MIMU定向方法", 《中国惯性技术学报》 *
吕梦男等: "基于扩展卡尔曼滤波的车辆状态参数估计", 《农业装备与车辆工程》 *
张健永等: "基于扩展Kalman滤波的无人驾驶车姿态测量研究", 《辽宁工业大学学报(自然科学版)》 *
杨铁军等: "基于DSP的GPS双天线实时姿态测量系统实现", 《电波科学学报》 *
王润民等: "基于多传感器的智能车辆姿态解算方法", 《测控技术》 *

Also Published As

Publication number Publication date
CN113405553B (zh) 2023-05-26

Similar Documents

Publication Publication Date Title
CN109696663B (zh) 一种车载三维激光雷达标定方法和系统
CN110579740B (zh) 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN108931244B (zh) 基于列车运动约束的惯导误差抑制方法及系统
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN109781098B (zh) 一种列车定位的方法和系统
CN110243377B (zh) 一种基于分层式结构的集群飞行器协同导航方法
CN111380518B (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
CN103712598B (zh) 一种小型无人机姿态确定方法
CN108974054B (zh) 无缝列车定位方法及其系统
CN108387236A (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN114777812B (zh) 一种水下组合导航系统行进间对准与姿态估计方法
CN112378399A (zh) 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN102095424A (zh) 一种适合车载光纤航姿系统的姿态测量方法
CN115143954A (zh) 一种基于多源信息融合的无人车导航方法
CN110779514B (zh) 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置
CN109387198A (zh) 一种基于序贯检测的惯性/视觉里程计组合导航方法
CN114136310B (zh) 一种惯性导航系统误差自主抑制系统及方法
CN112697154B (zh) 一种基于矢量分配的自适应多源融合导航方法
CN110926483B (zh) 一种用于自动驾驶的低成本传感器组合定位系统及方法
CN112325878A (zh) 基于ukf与空中无人机节点辅助的地面载体组合导航方法
CN113405553A (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