CN108873038B - 自主泊车定位方法及定位系统 - Google Patents

自主泊车定位方法及定位系统 Download PDF

Info

Publication number
CN108873038B
CN108873038B CN201811052200.6A CN201811052200A CN108873038B CN 108873038 B CN108873038 B CN 108873038B CN 201811052200 A CN201811052200 A CN 201811052200A CN 108873038 B CN108873038 B CN 108873038B
Authority
CN
China
Prior art keywords
vehicle
information
speed
attitude
angle
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
CN201811052200.6A
Other languages
English (en)
Other versions
CN108873038A (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.)
Wuhu Mengbo Technology Co ltd
Original Assignee
Wuhu Mengbo Technology Co ltd
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 Wuhu Mengbo Technology Co ltd filed Critical Wuhu Mengbo Technology Co ltd
Priority to CN201811052200.6A priority Critical patent/CN108873038B/zh
Publication of CN108873038A publication Critical patent/CN108873038A/zh
Application granted granted Critical
Publication of CN108873038B publication Critical patent/CN108873038B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • 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/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • 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

Landscapes

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

Abstract

本公开提供了一种自主泊车定位方法及定位系统，在所述方法及系统中，通过卡尔曼滤波融合超宽带定位系统得到车辆的位置的信息及全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息，获取车辆的初步定位；通过捷联惯性导航系统得到车辆的姿态、位置及速度的信息；通过双轮速系统得到车辆的速度及角度的信息；通过双目视觉系统得到车辆的相对位置及角度的信息；以及通过高精地图系统得到车辆的绝对位置的信息来得到姿态、位置及速度的第一及第二局部最优估计值，并且通过联邦卡尔曼滤波算法的融合处理，来得到车辆的姿态、位置及速度的全局最优估计值。

Description

自主泊车定位方法及定位系统
技术领域
本公开涉及一种自主泊车定位方法及定位系统。
背景技术
在现有的自主泊车定位方法中，通常所采用的技术包括：UWB(超宽带)无线定位技术、GNSS(全球卫星定位系统)RTK定位技术、SINS(捷联惯性导航系统)定位技术、双目视觉定位技术、以及各技术融合的定位技术等。
但是，不管上述的单一定位方法还是各个方法相融合的定位方法，均难以满足自主泊车高精度定位和稳定性要求。
例如，在UWB无线定位技术中，常用TOA、TDOA、AOA、RSS等方法实现短距离无线定位，其主要应用GNSS信号可用性低和完全无GNSS信号场景，如地下车库、城市峡谷、树木密集区等环境。GNSS RTK定位技术利用可视卫星，接收基准站发送的差分修正信息，对流动站进行修正，达到厘米级定位精度，其主要应用于可视卫星足够多，GNSS信号质量好无遮挡的环境，在开阔的场景能稳定持续为车辆提供厘米级定位精度。SINS定位技术中，可以在无GNSS信号的场景下，给定初始状态和初始对准后，依靠陀螺仪计算角度增量和加速度计计算速度、位置增量来推算车辆位置和速度，但误差会随时间迅速累积，没有外部参考信息进行误差补尝，就难以维持长时段的高精度定位。双目视觉定位技术是基于多幅图像间的视觉差，利用视差计算的双目视觉定位误差修正当前位置误差，在视线良好和路标清晰的环境，辅助定位效果明显。SINS与GNSS RTK融合的定位技术，在GNSS定位不准和无GNSS信号情况下，SINS能提供短时间的定位和精度维持，车辆在室内长时段和复杂运动场景，其无法保证车辆高精度的定位和可用性。SINS、GNSS RTK及双目视觉融合的定位技术中，用双目视觉识别路标与数据库中的路标匹配，车辆能在复杂环境下，维持稳定的定位精度，在没有路标环境，如地下停车场环境，难以保证高精度的定位和可用性。而在SINS、GNSS RTK、视觉、激光雷达、及地图多者融合的定位技术中，根据SINS和GNSS RTK融合定位给定车辆的大致位置，用视觉和激光雷达、离线地图辅助修正车辆位置，从而实现高精度定位，但应用激光雷达，价格昂贵，不适合大范围的推广应用。
综上所述，从室外到室内停车场，GNSS定位不准和无GNSS信号，以及无城市道路线路参考情况，在室内停车场，难以获取高精度定位；而且在自主泊车应用中，导航定位精度需在厘米级，长时间的室内和GNSS差环境，目前融合算法，高精度定位鲁棒性差，受环境和自身传感器误差影响，精度难以维持在厘米级。
因此，在自主泊车定位的领域中，需要一种高精度、稳定性、易推广的定位技术。
发明内容
为了解决上述技术问题中的至少之一，本公开提供了一种自主泊车定位方法及定位系统。
根据本公开的一个方面，自主泊车定位方法，包括：通过超宽带定位系统得到车辆的位置的信息；通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息；通过捷联惯性导航系统得到车辆的姿态、位置及速度的信息；通过双轮速系统得到车辆的速度及角度的信息；通过双目视觉系统得到车辆的相对位置及角度的信息；以及通过高精地图系统得到车辆的绝对位置的信息；通过卡尔曼滤波算法来融合通过超宽带定位系统得到车辆的位置的信息、通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息、通过捷联惯性导航系统得到车辆的姿态、位置及速度的信息、及通过双轮速系统得到车辆的速度及角度的信息，从而求取车辆的姿态、位置及速度的第一局部最优估计值；以及通过卡尔曼滤波来融合通过捷联惯性导航系统得到车辆的姿态、位置及速度的信息、通过双轮速系统得到车辆的速度及角度的信息、通过双目视觉系统得到车辆的相对位置及角度的信息、及通过高精地图系统得到车辆的绝对位置的信息，从而求取车辆的姿态、位置及速度的第二局部最优估计值；以及通过联邦卡尔曼滤波算法来融合第一局部最优估计值及第二局部最优估计值，来得到车辆的姿态、位置及速度的全局最优估计值。
根据本公开的至少一个实施方式，在第一局部最优估计值的求取过程中，首先通过卡尔曼滤波算法来融合通过捷联惯性导航系统得到车辆的姿态、位置及速度的信息、和通过双轮速系统得到车辆的速度及角度的信息，从而得到修正后的车辆的姿态、位置及速度的信息，然后通过卡尔曼滤波算法来融合通过超宽带定位系统得到车辆的位置的信息、通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息、及修正后的车辆的姿态、位置及速度的信息，从而得到车辆的姿态、位置及速度的第一局部最优估计值。
根据本公开的至少一个实施方式，在第二局部最优估计值的求取过程中，首先通过卡尔曼滤波算法来融合通过双轮速系统得到车辆的速度及角度的信息、通过双目视觉系统得到车辆的相对位置及角度的信息、和通过高精地图系统得到车辆的绝对位置的信息，从而得到修正后的车辆的位置及角度的信息，然后通过卡尔曼滤波来融合修正后的车辆的姿态、位置及速度的信息及修正后的车辆的位置及角度的信息，从而得到车辆的姿态、位置及速度的第二局部最优估计值。
根据本公开的至少一个实施方式，还包括：在通过卡尔曼滤波进行融合处理之前还包括根据超宽带定位系统得到车辆的位置的信息及通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息来对初始化车辆的位置及速度。
根据本公开的至少一个实施方式，还包括：在初始化处理后，根据初始化的位置及速度对捷联惯性导航系统中的陀螺仪及加速度计进行校准，并且对双目视觉系统中的双目摄像机进行标定。
根据本公开的另一方面，一种自主泊车定位系统，包括：信息获取装置，通过超宽带定位系统得到车辆的位置的信息；通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息；通过捷联惯性导航系统得到车辆的姿态、位置及速度的信息；通过双轮速系统得到车辆的速度及角度的信息；通过双目视觉系统得到车辆的相对位置及角度的信息；以及通过高精地图系统得到车辆的绝对位置的信息；第一局部最优估计值获取装置，通过卡尔曼滤波算法来融合通过超宽带定位系统得到车辆的位置的信息、通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息、通过捷联惯性导航系统得到车辆的姿态、位置及速度的信息、及通过双轮速系统得到车辆的速度及角度的信息，从而求取车辆的姿态、位置及速度的第一局部最优估计值；第二局部最优估计值获取装置，通过卡尔曼滤波来融合通过捷联惯性导航系统得到车辆的姿态、位置及速度的信息、通过双轮速系统得到车辆的速度及角度的信息、通过双目视觉系统得到车辆的相对位置及角度的信息、及通过高精地图系统得到车辆的绝对位置的信息，从而求取车辆的姿态、位置及速度的第二局部最优估计值；以及全局最优估计值获取装置，通过联邦卡尔曼滤波算法来融合第一局部最优估计值及第二局部最优估计值，来得到车辆的姿态、位置及速度的全局最优估计值。
根据本公开的至少一个实施方式，在第一局部最优估计值获取装置中，首先通过卡尔曼滤波算法来融合通过捷联惯性导航系统得到车辆的姿态、位置及速度的信息、和通过双轮速系统得到车辆的速度及角度的信息，从而得到修正后的车辆的姿态、位置及速度的信息，然后通过卡尔曼滤波算法来融合通过超宽带定位系统得到车辆的位置的信息、通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息、及修正后的车辆的姿态、位置及速度的信息，从而得到车辆的姿态、位置及速度的第一局部最优估计值。
根据本公开的至少一个实施方式，在第二局部最优估计值获取装置中，首先通过卡尔曼滤波算法来融合通过双轮速系统得到车辆的速度及角度的信息、通过双目视觉系统得到车辆的相对位置及角度的信息、和通过高精地图系统得到车辆的绝对位置的信息，从而得到修正后的车辆的位置及角度的信息，然后通过卡尔曼滤波来融合修正后的车辆的姿态、位置及速度的信息及修正后的车辆的位置及角度的信息，从而得到车辆的姿态、位置及速度的第二局部最优估计值。
根据本公开的至少一个实施方式，还包括：初始化装置，在通过卡尔曼滤波进行融合处理之前还包括根据超宽带定位系统得到车辆的位置的信息及通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息来对初始化车辆的位置及速度。
根据本公开的至少一个实施方式，还包括：校准标定装置，在初始化处理后，根据初始化的位置及速度对捷联惯性导航系统中的陀螺仪及加速度计进行校准，并且对双目视觉系统中的双目摄像机进行标定。
附图说明
附图示出了本公开的示例性实施方式，并与其说明一起用于解释本公开的原理，其中包括了这些附图以提供对本公开的进一步理解，并且附图包括在本说明书中并构成本说明书的一部分。
图1示出了根据本公开的一个实施方式的方法的总体框架图。
图2示出了根据本公开的一个实施方式的方法总体流程图。
图3示出了根据本公开的一个实施方式的双目视觉系统与高精地图系统的信息融合的流程图。
图4示出了根据本公开的一个实施方式的计算车辆到道路边缘线的距离的示意图。
具体实施方式
下面结合附图和实施方式对本公开作进一步的详细说明。可以理解的是，此处所描述的具体实施方式仅用于解释相关内容，而非对本公开的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本公开相关的部分。
需要说明的是，在不冲突的情况下，本公开中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本公开。
根据本公开的技术方案，融合了来自UWB定位系统(超宽带定位系统)、GNSS RTK定位系统(全球卫星定位系统实时动态定位系统)、SINS(捷联惯性导航系统)、双轮速系统、双目视觉系统、高精地图系统的六类不同信号源的定位信息，实现车辆高精度定位和定位稳定性维持。
首先，参照图1与图2，对本公开的定位方法进行详细的说明。
通过UWB定位系统的位置信息及GNSS RTK定位系统的位置及速度信息，对待定位的车辆的位置及速度进行初始化，并将初始化的位置及速度作为初始化滤波参数，提供给后续处理。
对捷联惯性导航系统SINS中的陀螺仪及加速度计进行校准。并且基于上述初始化的位置及速度对双目视觉系统中使用的双目摄像机进行标定。
然后，融合通过捷联惯性导航系统SINS得到的姿态、位置及速度以及测得的双轮速系统得到的速度及角度信息，从而得到修正的姿态、位置及速度。而且可以将来自双目视觉系统的相对位置及角度的信息、来自高精地图的绝对位置的信息、以及来自双轮速系统速度及角度的信息进行算法融合，从而得到修正的位置及角度。该融合可以分别通过卡尔曼滤波算法来进行。并且可以将上述初始化滤波参数作为卡尔曼滤波算法的输入参数。
利用GNSS RTK定位系统及UWB定位系统的位置及速度的信息与上述修正后的姿态、位置、速度观测量进行卡尔曼滤波算法融合得到姿态、位置、速度的局部最优估计值；利用双目视觉和高精地图的信息与上述修正后的姿态、位置、速度观测量进行卡尔曼滤波算法融合得到姿态、位置、速度的局部最优估计值。
最后基于得到的两个局部最优估计值，进行联邦卡尔曼滤波算法融合，得到姿态、位置、速度的全局最优估计值。并且还可以根据全局最优估计值来对上述的局部最优估计值来进行误差校正等处理。
下面对可以用于本公开的常规的卡尔曼滤波算法进行简要的说明
首先，卡尔曼滤波算法及联邦卡尔曼滤波算法的系统状态量可以为：系统状态量
Figure BDA0001794885590000071
其中，δL,δλ,δh为纬度、经度、高度误差，δvE,δvN,δvU为东向、北向、高程方向的速度误差，
Figure BDA0001794885590000072
为东向、北向、高程方向的姿态角，εx,εy,εz为载体坐标系x,y,z轴上的陀螺仪漂移，
Figure BDA0001794885590000073
为载体坐标系x,y,z轴上的加速度计零偏。
系统状态噪声W＝[wx,wy,wz,ax,ay,az]T，wx,wy,wz,ax,ay,az分别为陀螺仪和加速度计在载体坐标系下的零均值白噪声。
系统状态方程为
Figure BDA0001794885590000074
F(t)为状态转移矩阵，Γ(t)为系统噪声输入矩阵；W(t)为系统噪声，其均值为0的白噪声。
卡尔曼滤波可以为离散卡尔曼滤波，该离散系统的状态方程与观测方程如下所示。
系统状态方程：Xk＝Fk,k-1Xk-1+Γk-1Wk-1，观测方程：Zk＝HkXk+Vk。
其中，Xk为系统状态向量，Fk,k-1为系统状态转移矩阵，Γk-1为系统噪声矩阵，Wk-1为系统零均值白噪声，Hk为观测矩阵，Vk为观测量零均值白噪声向量，Xk、Wk-1、Vk是互不相关。
各子系统的数据融合采用卡尔曼滤波算法，卡尔曼滤波算法流程如下：
状态一步预测方程：
Figure BDA0001794885590000081
状态更新方程：
Figure BDA0001794885590000082
卡尔曼滤波增益矩阵：
Figure BDA0001794885590000083
一步预测误差方差阵：
Figure BDA0001794885590000084
估计误差方差阵：Pk＝(I-KkHk)Pk,k-1
其中Qk-1为Wk-1对应的系统噪声协方差阵，Rk为Vk对应的观测噪声方差阵，
Figure BDA0001794885590000085
为状态的一步预测向量，
Figure BDA0001794885590000086
为k-1时刻状态Xk-1的预测向量，Pk,k-1为k-1时刻一步预测误差方差阵。
SINS系统与双轮速系统的信息融合
在仅使用SINS系统得到的信息的情况下，其误差随着时间会不断累积，通过引入双轮速，修正速度和航向角。具体而言，将SINS系统中计算的角度增量、速度，以双轮速计算的速度和角度作为观测量，输入至卡尔曼滤波器，进行算法融合，得到修正的姿态、位置和速度。其中引入了轮速传感器，用前进速度、车辆航向角误差作观测量。
轮速传感器引入，用于计算车辆的前进速度、车辆的横摆角度-航向角。
车辆左后轮线速度vwl(k)，车辆右后轮线速度vwr(k)，车辆的前进速度vcar(k)＝(vwl(k)+vwr(k))/2，车辆的横摆角速度wz(k)＝(vwl(k)-vwr(k))/D，D为右后轮和左后轮之间的距离。
依据轮速计算的航向角变化量，Δψcar＝wz(k)·Δt，ψ表示航向角，Δψcar表示车辆航向角的变化量。
Z0(t)＝H0(t)X(t)+V0(t)，其中Z0(t)为观测量矩阵，H0(t)为观测矩阵，V0(t)为观测噪声。
Figure BDA0001794885590000091
其中ψ表示航向角，V表示车辆行驶的速度。
双目视觉系统、双轮速系统及高精地图的信息融合
双目视觉能获取车辆相对位置和角度，引入高精地图绝对位置，修正相对位置和角度；具体实现，双目视觉获取的车道道路边缘线路、相对位置与高精地图的车道线路、车辆相对位置进行卡尔曼滤波算法融合，修正车辆的相对位置和角度。
在对双目视觉系统、双轮速系统及高精地图系统的信息进行卡尔曼滤波处理时，使用位置、航向角、俯仰角、车辆与道路边线距离误差作观测量。
双目视觉、双轮速和高精度地图融合获取位置、航向角、俯仰角、车辆与道路边线距离。
状态向量：XVM(t)＝[LVisionλVision hVisionψVision vcar]T，下标Vision表示双目视觉，L,λ,h分别为纬度、经度、高度，ψ表示航向角，vcar表示车速。
离散化的状态方程公式为：
Figure BDA0001794885590000101
W1为状态噪声，其满足零均值的高斯白噪声。
以双目视觉推算的相对位置和高精地图的绝对位置之差，作为位置观测量；双目视觉计算的航向、俯仰角和高精地图计算的航向、俯仰角之差作为角度观测量；双目视觉计算的车辆与视觉里道路边线的距离，和地图中车辆与地图里道路边线的距离差作为距离观测量。
Figure BDA0001794885590000102
Map表示高精地图，θ表示俯仰角，d表示车与道路边缘的距离，dVision表示用双目视觉计算的车辆与检测的道路边缘的距离，dMap表示车辆与地图中道路边缘的距离。
图3中示出了双目视觉系统与高精地图系统的信息融合来修正位置及角度的示意性流程图。在图3中，双目视觉系统的传感器进行监测，并且进行图像畸变校正/兴趣区域提取/检测算法的处理。然后进行道路线路监测/特征提取处理，并且基于以上来获取车辆的航向角和道路的边缘线，从而计算出相对位置、航向、俯仰角、车辆与道路边线的距离等信息，结合高精地图系统的相关信息，来对车辆的相对位置及角度进行修正，最后得到修正后的位置、航向角及俯仰角等。
并且如图4所示，道路边线上A(x1,y1)，B(x2,y2)两点是在摄像机坐标系下的坐标，高度信息短时间内变化小，则lAB斜率满足，
Figure BDA0001794885590000103
dVision＝(y1-k·x1)cos(arctank).
Figure BDA0001794885590000111
为车辆相对于道路的偏航角，
Figure BDA0001794885590000112
SINS系统和双轮速系统、与UWB系统和GNSS RTK系统的信息融合
下面对SINS系统和双轮速系统，与UWB系统和GNSS RTK系统融合后的姿态、位置、速度进行说明。
以输出的位置、速度差作为观测量。
其观测方程如下：Z1(t)＝H1(t)X(t)+V1(t)，其中Z1(t)为观测量矩阵，H1(t)为观测矩阵，V1(t)为观测噪声。
Figure BDA0001794885590000113
Z1(t)为观测量矩阵，下标INS表示SINS系统，下标REF表示UWB或GNSS RTK系统的参考系统量测值，Ve,Vn,Vu分别表示东向、北向、天向速度测量值，λ,L,h分别表示经度、纬度和高度。
Figure BDA0001794885590000114
其中Hp1(t)＝[diag[Rn+h(Re+h)cosL 1]03×12]，Hv1(t)＝[03×3diag[1 1 1]03×9]，Rn为当地参考椭球子午线曲率半径，Re为当地与子午线垂直的法线平面上的曲率半径，L表示车辆所在处的纬度值。
SINS系统和双轮速系统、与双目视觉系统和高精地图系统的信息融合
SINS系统和双轮速系统、与双目视觉系统和高精地图系统融合后的姿态、位置、速度如下。
其观测方程如下：Z2(t)＝H2(t)X(t)+V2(t)，其中Z2(t)为观测量矩阵，H2(t)为观测矩阵，V2(t)为观测噪声。
Figure BDA0001794885590000121
Z2(t)为观测量矩阵，下标INS表示SINS系统，下标VM表示双目视觉和高精地图融合系统的量测值，λ,L,h分别表示经度、纬度和高度，ψ表示航向角，θ表示俯仰角。
Figure BDA0001794885590000122
其中Hp2(t)＝[diag[Rn+h(Re+h)cosL 1]03×12]，
Hδ2(t)＝[03×6 diag[1 1 1]03×6].
下面，本公开中所使用的常规的联邦卡尔曼滤波进行说明。
主滤波器状态估计协方差矩阵，
Figure BDA0001794885590000123
Figure BDA0001794885590000124
其中β1+β2＝1，β1和β2分别表示子滤波器1和子滤波器2的分配系数。
主滤波器状态噪声的方差矩阵，
Figure BDA0001794885590000125
Figure BDA0001794885590000126
状态更新方程：
Figure BDA0001794885590000127
其中β1和β2根据场景不同，可以动态进行设置，能提高系统的容错性能。
本公开通过高精度多源数据融合算法，应用于自主泊车领域，解决室内外环境切换及室内环境下高精厘米级定位可靠性和可用性问题，能够为室内外环境提供准确和可靠的高精度厘米级定位。
根据本公开的另一方面，还提供了一种自主泊车的定位系统。在该系统中，通过信息获取装置，获取上述各个系统的信息，并且通过第一局部最优估计值获取装置来融合相关信息以求取车辆的姿态、位置及速度的第一局部最优估计值；通过第二局部最优估计值获取装置来融合相关信息以求取车辆的姿态、位置及速度的第二局部最优估计值；以及通过全局最优估计值获取装置来得到车辆的姿态、位置及速度的全局最优估计值。并且，也可以通过其他相应的部件来实现上述方法中相应的功能。
以上所描述的系统仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。并且还可以增加其他的单元等。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。
上述以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本发明各个实施例所述方法的部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM)、随机存取存储器(RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
在本说明书的描述中，参考术语“一个实施例/方式”、“一些实施例/方式”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例/方式或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例/方式或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例/方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例/方式或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例/方式或示例以及不同实施例/方式或示例的特征进行结合和组合。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
本领域的技术人员应当理解，上述实施方式仅仅是为了清楚地说明本公开，而并非是对本公开的范围进行限定。对于所属领域的技术人员而言，在上述公开的基础上还可以做出其它变化或变型，并且这些变化或变型仍处于本公开的范围内。

Claims (10)

1.一种自主泊车定位方法，其特征在于，包括：
通过超宽带定位系统得到车辆的位置的信息；通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息；通过捷联惯性导航系统得到车辆的姿态、位置及速度的信息；通过双轮速系统得到车辆的速度及角度的信息；通过双目视觉系统得到车辆的相对位置及角度的信息；以及通过高精地图系统得到车辆的绝对位置的信息；
通过卡尔曼滤波算法来融合通过超宽带定位系统得到车辆的位置的信息、通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息、通过所述捷联惯性导航系统得到车辆的姿态、位置及速度的信息、及通过所述双轮速系统得到车辆的速度及角度的信息，从而求取车辆的姿态、位置及速度的第一局部最优估计值；以及通过卡尔曼滤波来融合通过所述捷联惯性导航系统得到车辆的姿态、位置及速度的信息、通过所述双轮速系统得到车辆的速度及角度的信息、通过所述双目视觉系统得到车辆的相对位置及角度的信息、及通过所述高精地图系统得到车辆的绝对位置的信息，从而求取车辆的姿态、位置及速度的第二局部最优估计值；以及
通过联邦卡尔曼滤波算法来融合所述第一局部最优估计值及所述第二局部最优估计值，来得到车辆的姿态、位置及速度的全局最优估计值。
2.如权利要求1所述的方法，其特征在于，在所述第一局部最优估计值的求取过程中，首先通过卡尔曼滤波算法来融合通过所述捷联惯性导航系统得到车辆的姿态、位置及速度的信息、和通过所述双轮速系统得到车辆的速度及角度的信息，从而得到修正后的车辆的姿态、位置及速度的信息，然后通过卡尔曼滤波算法来融合通过超宽带定位系统得到车辆的位置的信息、通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息、及所述修正后的车辆的姿态、位置及速度的信息，从而得到车辆的姿态、位置及速度的所述第一局部最优估计值。
3.如权利要求2所述的方法，其特征在于，在所述第二局部最优估计值的求取过程中，首先通过卡尔曼滤波算法来融合通过所述双轮速系统得到车辆的速度及角度的信息、通过所述双目视觉系统得到车辆的相对位置及角度的信息、和通过所述高精地图系统得到车辆的绝对位置的信息，从而得到修正后的车辆的位置及角度的信息，然后通过卡尔曼滤波来融合所述修正后的车辆的姿态、位置及速度的信息及所述修正后的车辆的位置及角度的信息，从而得到车辆的姿态、位置及速度的第二局部最优估计值。
4.如权利要求1至3中任一项所述的方法，其特征在于，还包括：在通过所述卡尔曼滤波进行融合处理之前还包括根据所述超宽带定位系统得到车辆的位置的信息及通过所述全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息来初始化车辆的位置及速度。
5.如权利要求4所述的方法，其特征在于，还包括：在所述初始化后，根据初始化的位置及速度对所述捷联惯性导航系统中的陀螺仪及加速度计进行校准，并且对所述双目视觉系统中的双目摄像机进行标定。
6.一种自主泊车定位系统，其特征在于，包括：
信息获取装置，通过超宽带定位系统得到车辆的位置的信息；通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息；通过捷联惯性导航系统得到车辆的姿态、位置及速度的信息；通过双轮速系统得到车辆的速度及角度的信息；通过双目视觉系统得到车辆的相对位置及角度的信息；以及通过高精地图系统得到车辆的绝对位置的信息；
第一局部最优估计值获取装置，通过卡尔曼滤波算法来融合通过超宽带定位系统得到车辆的位置的信息、通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息、通过所述捷联惯性导航系统得到车辆的姿态、位置及速度的信息、及通过所述双轮速系统得到车辆的速度及角度的信息，从而求取车辆的姿态、位置及速度的第一局部最优估计值；
第二局部最优估计值获取装置，通过卡尔曼滤波来融合通过所述捷联惯性导航系统得到车辆的姿态、位置及速度的信息、通过所述双轮速系统得到车辆的速度及角度的信息、通过所述双目视觉系统得到车辆的相对位置及角度的信息、及通过所述高精地图系统得到车辆的绝对位置的信息，从而求取车辆的姿态、位置及速度的第二局部最优估计值；以及
全局最优估计值获取装置，通过联邦卡尔曼滤波算法来融合所述第一局部最优估计值及所述第二局部最优估计值，来得到车辆的姿态、位置及速度的全局最优估计值。
7.如权利要求6所述的系统，其特征在于，在第一局部最优估计值获取装置中，首先通过卡尔曼滤波算法来融合通过所述捷联惯性导航系统得到车辆的姿态、位置及速度的信息、和通过所述双轮速系统得到车辆的速度及角度的信息，从而得到修正后的车辆的姿态、位置及速度的信息，然后通过卡尔曼滤波算法来融合通过超宽带定位系统得到车辆的位置的信息、通过全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息、及所述修正后的车辆的姿态、位置及速度的信息，从而得到车辆的姿态、位置及速度的所述第一局部最优估计值。
8.如权利要求7所述的系统，其特征在于，在所述第二局部最优估计值获取装置中，首先通过卡尔曼滤波算法来融合通过所述双轮速系统得到车辆的速度及角度的信息、通过所述双目视觉系统得到车辆的相对位置及角度的信息、和通过所述高精地图系统得到车辆的绝对位置的信息，从而得到修正后的车辆的位置及角度的信息，然后通过卡尔曼滤波来融合所述修正后的车辆的姿态、位置及速度的信息及所述修正后的车辆的位置及角度的信息，从而得到车辆的姿态、位置及速度的第二局部最优估计值。
9.如权利要求6至8中任一项所述的系统，其特征在于，还包括：
初始化装置，在通过所述卡尔曼滤波进行融合处理之前还包括根据所述超宽带定位系统得到车辆的位置的信息及通过所述全球卫星定位系统实时动态定位系统得到车辆的位置及速度的信息来初始化车辆的位置及速度。
10.如权利要求9所述的系统，其特征在于，还包括：
校准标定装置，在所述初始化后，根据初始化的位置及速度对所述捷联惯性导航系统中的陀螺仪及加速度计进行校准，并且对所述双目视觉系统中的双目摄像机进行标定。
CN201811052200.6A 2018-09-10 2018-09-10 自主泊车定位方法及定位系统 Active CN108873038B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811052200.6A CN108873038B (zh) 2018-09-10 2018-09-10 自主泊车定位方法及定位系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811052200.6A CN108873038B (zh) 2018-09-10 2018-09-10 自主泊车定位方法及定位系统

Publications (2)

Publication Number Publication Date
CN108873038A CN108873038A (zh) 2018-11-23
CN108873038B true CN108873038B (zh) 2020-11-06

Family

ID=64323752

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811052200.6A Active CN108873038B (zh) 2018-09-10 2018-09-10 自主泊车定位方法及定位系统

Country Status (1)

Country Link
CN (1) CN108873038B (zh)

Families Citing this family (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110077392B (zh) * 2019-03-28 2020-08-25 惠州市德赛西威汽车电子股份有限公司 一种自动泊车定位系统的航迹推算方法
CN110155043B (zh) * 2019-05-09 2020-09-18 禾多科技（北京）有限公司 自动泊车二维定位方法
CN112147662B (zh) * 2019-06-28 2023-08-08 比亚迪股份有限公司 车辆定位方法和装置、车辆、存储介质、电子设备
CN110487267B (zh) * 2019-07-10 2021-06-04 湖南交工智能技术有限公司 一种基于vio＆uwb松组合的无人机导航系统及方法
CN110361008B (zh) * 2019-07-10 2020-12-25 北京智行者科技有限公司 地下车库自动泊车的定位方法及装置
CN112304302B (zh) * 2019-07-26 2023-05-12 北京魔门塔科技有限公司 一种多场景高精度车辆定位方法、装置及车载终端
CN110672093B (zh) * 2019-08-23 2023-08-04 华清科盛(北京)信息技术有限公司 一种基于uwb与惯导融合的车载导航定位方法
CN112446924A (zh) * 2019-09-02 2021-03-05 北京车和家信息技术有限公司 车辆的相机标定系统、车辆及相机标定方法
CN112556720B (zh) * 2019-09-25 2023-08-18 上海汽车集团股份有限公司 一种车辆惯性导航校准方法、系统及车辆
TWI724686B (zh) * 2019-12-13 2021-04-11 國立成功大學 應用高精地圖之定位定向系統與定位定向方法
CN113074751B (zh) * 2019-12-17 2023-02-07 杭州海康威视数字技术股份有限公司 一种视觉定位误差检测方法及装置
CN111267912B (zh) * 2020-03-12 2022-03-11 兰州交通大学 基于多源信息融合的列车定位方法和系统
CN111619553A (zh) * 2020-05-26 2020-09-04 三一专用汽车有限责任公司 工程车辆和工程车辆的泊车控制方法
CN111721289B (zh) * 2020-06-28 2022-06-03 阿波罗智能技术(北京)有限公司 自动驾驶中车辆定位方法、装置、设备、存储介质及车辆
CN113932820A (zh) * 2020-06-29 2022-01-14 杭州海康威视数字技术股份有限公司 对象检测的方法和装置
CN113203418B (zh) * 2021-04-20 2022-09-16 同济大学 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN113790731B (zh) * 2021-08-06 2022-12-02 荣耀终端有限公司 速度信息的生成方法及装置
CN113790726B (zh) * 2021-09-07 2024-03-29 中国科学院合肥物质科学研究院 一种融合相机、轮速计和单uwb信息的机器人室内定位方法
CN114394130B (zh) * 2021-12-27 2022-11-11 中国矿业大学 一种煤矿辅助运输车辆定位方法及定位系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103197279A (zh) * 2013-03-12 2013-07-10 中国矿业大学 一种移动目标协同定位系统及定位方法
CN105674993A (zh) * 2016-01-15 2016-06-15 武汉光庭科技有限公司 基于双目相机的高精度视觉定位地图生成系统及方法
CN106780699A (zh) * 2017-01-09 2017-05-31 东南大学 一种基于sins/gps和里程计辅助的视觉slam方法
CN106842269A (zh) * 2017-01-25 2017-06-13 北京经纬恒润科技有限公司 定位方法及系统
WO2017139432A1 (en) * 2016-02-09 2017-08-17 5D Robotics, Inc. Ultra wide band radar localization
CN107656301A (zh) * 2017-09-20 2018-02-02 北京航天发射技术研究所 一种基于多源信息融合的车载定位方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103197279A (zh) * 2013-03-12 2013-07-10 中国矿业大学 一种移动目标协同定位系统及定位方法
CN105674993A (zh) * 2016-01-15 2016-06-15 武汉光庭科技有限公司 基于双目相机的高精度视觉定位地图生成系统及方法
WO2017139432A1 (en) * 2016-02-09 2017-08-17 5D Robotics, Inc. Ultra wide band radar localization
CN106780699A (zh) * 2017-01-09 2017-05-31 东南大学 一种基于sins/gps和里程计辅助的视觉slam方法
CN106842269A (zh) * 2017-01-25 2017-06-13 北京经纬恒润科技有限公司 定位方法及系统
CN107656301A (zh) * 2017-09-20 2018-02-02 北京航天发射技术研究所 一种基于多源信息融合的车载定位方法

Also Published As

Publication number Publication date
CN108873038A (zh) 2018-11-23

Similar Documents

Publication Publication Date Title
CN108873038B (zh) 自主泊车定位方法及定位系统
US11002859B1 (en) Intelligent vehicle positioning method based on feature point calibration
CN110906923B (zh) 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆
US9921065B2 (en) Unit and method for improving positioning accuracy
CN111175795B (zh) Gnss/ins组合导航系统的两步抗差滤波方法及系统
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN104729506A (zh) 一种视觉信息辅助的无人机自主导航定位方法
KR20180101717A (ko) 맵들을 사용하는 차량 컴포넌트 제어
US20100274481A1 (en) System and method for collaborative navigation
CN111551186A (zh) 一种车辆实时定位方法、系统及车辆
CN113406682A (zh) 一种定位方法、装置、电子设备及存储介质
WO2022133294A1 (en) System and method for fusing dead reckoning and gnss data streams
CN112946681B (zh) 融合组合导航信息的激光雷达定位方法
KR20190040818A (ko) 차량 내부 센서, 카메라, 및 gnss 단말기를 이용한 3차원 차량 항법 시스템
CN105043392A (zh) 一种飞行器位姿确定方法及装置
CN112964291A (zh) 一种传感器标定的方法、装置、计算机存储介质及终端
US20020188386A1 (en) GPS based terrain referenced navigation system
CN115326084A (zh) 车辆定位方法、装置、计算机设备、存储介质
CN115407376A (zh) 车辆定位校准方法、装置、计算机设备、存储介质
Falco et al. Positioning based on tightly coupled multiple sensors: A practical implementation and experimental assessment
CN113405555B (zh) 一种自动驾驶的定位传感方法、系统及装置
CN116558512A (zh) 一种基于因子图的gnss与车载传感器融合定位方法及系统
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN115790613A (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