CN114608553A - 一种自动驾驶汽车定位系统及方法 - Google Patents

一种自动驾驶汽车定位系统及方法 Download PDF

Info

Publication number
CN114608553A
CN114608553A CN202210156490.9A CN202210156490A CN114608553A CN 114608553 A CN114608553 A CN 114608553A CN 202210156490 A CN202210156490 A CN 202210156490A CN 114608553 A CN114608553 A CN 114608553A
Authority
CN
China
Prior art keywords
vehicle
positioning
positioning information
unit
wifi
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
CN202210156490.9A
Other languages
English (en)
Other versions
CN114608553B (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202210156490.9A priority Critical patent/CN114608553B/zh
Priority claimed from CN202210156490.9A external-priority patent/CN114608553B/zh
Publication of CN114608553A publication Critical patent/CN114608553A/zh
Application granted granted Critical
Publication of CN114608553B publication Critical patent/CN114608553B/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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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

Landscapes

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

Abstract

本发明公开了一种自动驾驶汽车定位系统及方法，方法包括：建立城市高层建筑群区域的局部区域位置地图，以得到位置和WiFi信号之间的对应关系；实时获取道路行驶中的车辆自身的经纬度定位信息，及车辆自身的速度与加速度信息；得到车辆在局部区域内融合后的相对位置定位信息；将得到的车辆在局部区域内融合后的相对位置定位信息转换为当前车辆的经纬度定位信息输出。本发明能够使自动驾驶汽车在城市高层建筑群中稳定获取自身的定位信号，提高了自动驾驶汽车的定位性能，同时为其后续系统中运动的规划决策提供充足的定位数据。

Description

一种自动驾驶汽车定位系统及方法
技术领域
本发明属于车辆定位技术领域，具体涉及一种适应于城市高层建筑群的自动驾驶汽车定位系统及方法。
背景技术
自动驾驶汽车(Autonomous vehicles；Self-driving automobile)又称无人驾驶汽车、电脑驾驶汽车、或轮式移动机器人，是一种通过电脑系统实现无人驾驶的智能汽车。
与人驾驶汽车不同的是，自动驾驶汽车需要实时获取当前自身的位置与姿态，为下一时刻的运动的规划和决策提供信息。目前自动驾驶汽车广泛应用的定位方法为GPS+惯导单元融合定位法，这种融合方式所需要的器件简单、成本低廉，但也存在一个问题：在城市中行驶时，GPS信号容易受到高层建筑物的遮挡，无法通过高频的GPS信号来修正惯导单元的位置与姿态的判定，这时便会导致GPS+惯导单元的融合定位效果不佳。
因此，如何提升自动驾驶汽车的定位稳定性，保证自动驾驶汽车可以在城市高层建筑的包围中稳定获取自身的位置与姿态，是目前自动驾驶领域亟需解决的问题。
发明内容
针对于上述现有技术的不足，本发明的目的在于提供一种自动驾驶汽车定位系统及方法，以解决现有技术中车辆GPS信号容易受到高层建筑物的遮挡，无法通过高频的GPS信号来修正惯导单元的位置与姿态的判定的问题。本发明能够使自动驾驶汽车在城市高层建筑群中稳定获取自身的定位信号，提高了自动驾驶汽车的定位性能，同时为其后续系统中运动的规划决策提供充足的定位数据。
为达到上述目的，本发明采用的技术方案如下：
本发明的一种自动驾驶汽车定位系统，包括：WiFi定位器单元、局部区域位置建图单元、GPS单元、惯导单元、多传感器融合定位单元及多类型坐标系转换单元；
所述GPS单元，用于获取车辆的实时经纬度定位信息，并将获得的信息传输给多传感器融合定位单元；
所述惯导单元，用于获取车辆的瞬时速度和瞬时加速度信息，并将获得的信息传输给多传感器融合定位单元；
所述局部区域位置建图单元，利用道路两侧建筑物中WiFi信号发射器发出的WiFi信号，建立城市高层建筑群区域的局部区域位置地图，以得到位置和WiFi信号之间的对应关系；
所述WiFi定位器单元，安装在车身内，接收道路两侧建筑物中WiFi信号发射器发出的WiFi信号，根据基于WiFi信号的局部位置匹配算法，计算出车辆在局部区域的相对位置定位信息，并将该相对位置定位信息发送给多传感器融合定位单元；
所述多传感器融合定位单元，用于获取GPS单元发送的经纬度定位信息、WiFi定位器单元发送的相对位置定位信息以及惯导单元发送的瞬时速度和瞬时加速度信息；若接收到的GPS单元发送的信号为非空信号，则将其发送给多类型坐标系转换单元，然后利用卡尔曼滤波算法将经多类型坐标系转换单元转换后的局部笛卡尔坐标系内的定位信息与惯导单元发送的信息进行融合，将输出的结果发送给多类型坐标系转换单元；若接收到的GPS单元发送的信号为空信号，则使用WiFi定位器单元接收到的车辆在局部区域的相对位置定位信息与惯导单元发送的信息进行融合，将输出的结果发送给多类型坐标系转换单元；
所述多类型坐标系转换单元，安装在车身内，用于将经纬度定位信息转换为地心坐标系定位信息，并进一步转换为局部笛卡尔坐标系内的定位信息；及将局部笛卡尔坐标系内的定位结果转换为地心坐标系内的定位信息，最后转换成当前车辆的经纬度定位信息。
进一步地，所述笛卡尔局部坐标系符合东北天坐标系的建立标准，x轴沿纬度方向指向东，y轴沿经度方向指向北，z轴垂直与地面指向天。
进一步地，所述位置和WiFi信号之间的对应关系具体为：将区域用一个矩形网格覆盖，在每一个网格点上，通过一段时间的数据采样得到每个位置点来自各WiFi信号发射器的接收信号强度，每一个点的接收信号强度是不同的，完成局部区域的定位，定位所用的坐标系为局部笛卡尔坐标系，应用东北天坐标系的方向朝向。
进一步地，所述基于WiFi信号的局部位置匹配算法如下式所示：
Figure BDA0003512911630000021
式中，(m,n)表示局部地图中的坐标点，(mzk,nzk)表示得到的定位结果，N表示区域中WiFi信号发射器的数量，
Figure BDA0003512911630000022
表示驶入该区域中车辆接收到的信号，Si,(m,n)表示建立城市高层建筑群区域的局部区域位置地图时所创建的点。
本发明还提供一种自动驾驶汽车定位方法，步骤如下：
1)建立城市高层建筑群区域的局部区域位置地图，以得到位置和WiFi信号之间的对应关系；
2)实时获取道路行驶中的车辆自身的经纬度定位信息，及车辆自身的速度与加速度信息；
3)根据步骤2)中获取的经纬度定位信息的类型，判定进行融合定位的方式：若经纬度定位信息为非空信号，则使用卡尔曼滤波算法将其与惯导单元获取的定位信息进行融合；若经纬度定位信息为空信号，则使用基于WiFi信号的局部位置匹配算法获取车辆当前的相对位置，然后采用卡尔曼滤波算法，对WiFi定位器单元接收到的车辆在局部区域内的相对位置定位信息与惯导单元获取的定位信息进行融合，得到车辆在局部区域内融合后的相对位置定位信息；
4)将所述步骤3)中得到的车辆在局部区域内融合后的相对位置定位信息转换为当前车辆的经纬度定位信息输出。
进一步地，所述步骤1)中建立城市高层建筑群区域的局部区域位置地图具体包括：
道路两侧建筑物中多个WiFi信号发射器发出WiFi信号，所述WiFi信号在每个点的信号强度由下述公式计算得到：
Figure BDA0003512911630000031
式中，d表示WiFi信号发射器与位置点的距离，β表示路径损耗系数，Pt表示WiFi信号发射器发送WiFi信号的功率，J表示WiFi信号发射器的天线增益系数，B表示WiFi信号发射器周围建筑物的墙体厚度的平均值，f表示WiFi信号的频率，α表示公路的宽度规格系数，根据不同的宽度设定不同的系数值：如果为双向四车道，则α＝7.5；如果为双向六车道，α＝11.25；如果为双向八车道，则α＝15。
进一步地，所述步骤3)中惯导单元获取的定位信息为：沿车辆行驶方向的速度
Figure BDA0003512911630000032
垂直于车辆行驶方向上的速度
Figure BDA0003512911630000033
沿道路方向的车辆加速度αk，垂直于道路方向的车辆加速度βk，Δt是前后两个状态量之间时间的差值，根据车辆的加速度与上一时刻得到的状态量计算得出当前时刻的状态量：
Figure BDA0003512911630000034
Figure BDA0003512911630000035
Figure BDA0003512911630000036
Figure BDA0003512911630000037
式中，
Figure BDA0003512911630000038
表示k时刻车辆沿行驶方向的速度状态量，
Figure BDA0003512911630000039
表示k-1时刻车辆沿行驶方向的速度状态量，
Figure BDA00035129116300000310
表示k时刻垂直于车辆行驶方向的速度状态量，
Figure BDA00035129116300000311
表示k-1时刻垂直于车辆行驶方向的速度状态量；mk表示k时刻车辆在局部区域坐标系上沿m轴的坐标点，nk表示k时刻车辆在局部区域坐标系上沿n轴的坐标点。
进一步地，所述步骤3)中采用的基于WiFi信号的局部位置匹配算法如下式所示：
Figure BDA0003512911630000041
式中，(m，n)表示局部地图中的坐标点，(mzk，nzk)表示得到的定位结果，N表示区域中WiFi信号发射器的数量，
Figure BDA0003512911630000042
表示驶入该区域中车辆接收到的信号，Si，(m，n)表示步骤1)中建立局部区域位置地图时所创建的点。
进一步地，所述步骤3)中采用卡尔曼滤波算法，对WiFi定位器单元接收到的车辆在局部区域内的相对位置定位信息与惯导单元获取的定位信息进行融合具体为如下式所示：
Figure BDA0003512911630000043
Figure BDA0003512911630000044
Figure BDA0003512911630000045
Figure BDA0003512911630000046
Pk＝(I-KkH)Pk
式中，xk-1表示k-1时刻的状态估计值，即车辆在局部笛卡尔坐标系中所处的坐标位置与速度，二者均由惯导单元计算得到；xk-1＝[pk-1，vk-1]T，pk-1＝[mk-1，nk-1]T，用于表示k-1时刻车辆在局部区域内所处的位置；
Figure BDA0003512911630000047
用于表示k-1时刻车辆自身纵向速度与横向速度，
Figure BDA0003512911630000048
表示车辆的纵向速度，
Figure BDA0003512911630000049
表示车辆的横向速度；
Figure BDA00035129116300000410
表示k时刻的先验状态估计值，是滤波计算的中间结果，即根据上一时刻(k-1时刻)的最优估计预测的k时刻的结果，是预测方程的结果；Pk-1和Pk分别表示k-1时刻和k时刻的后验估计协方差矩阵，表示此时状态不确定度，是滤波的结果之一，具体表达式为
Figure BDA0003512911630000051
Figure BDA0003512911630000052
Figure BDA0003512911630000053
表示k时刻的先验估计协方差矩阵，是滤波的中间计算结果，具体表达式为
Figure BDA0003512911630000054
H是状态变量到测量的转换矩阵，
Figure BDA0003512911630000055
zk表示测量值，zk＝[pzk，vzk]T，位置pzk＝[mzk，nzk]T，由基于WiFi信号的局部位置匹配算法得到，速度
Figure BDA0003512911630000056
由惯导单元测得，pzk和vzk分别表示车辆在局部笛卡尔坐标系中所处的位置与当前车辆的速度，Kk表示滤波增益矩阵，是滤波的中间计算结果；A表示状态转移矩阵，通过运动学公式确定，令
Figure BDA0003512911630000057
Q表示过程激励噪声协方差，用来表示状态转换矩阵与实际过程之间的误差，用xe＝[pe，ve]T表示状态误差量，由不确定的噪声引起的，根据具体道路情况分析，该误差量与道路不平整度呈正相关，设道路不平整度的评价指标为λ，pe∝λ，ve∝λ，具体表达式为
Figure BDA0003512911630000058
R表示测量噪声协方差，用xs＝[ps，vs]T表示测量误差，通过传感器的精度获取，具体表达式为
Figure BDA0003512911630000059
B表示可选的控制输入增益矩阵，此处设为空矩阵；uk-1表示k-1时刻的控制增益，此处设为0；xk＝[pk，vk]T表示最终的定位结果。
本发明的有益效果：
1、本发明保证了自动驾驶汽车在GPS定位信号被高楼遮挡时定位信号输出的稳定性，提高了自动驾驶车辆在城市道路中行驶的安全性。
2、本发明利用了城市高层建筑群中的多个WiFi信号发射器进行定位，无需额外布置用于定位的设备，大大节约了成本。
附图说明
图1为本发明定位方法原理图。
图2为本发明中的WiFi建图与定位匹配原理图。
具体实施方式
为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。
本发明的一种自动驾驶汽车定位系统，包括：WiFi定位器单元、局部区域位置建图单元、GPS单元、惯导单元、多传感器融合定位单元及多类型坐标系转换单元；
所述GPS单元，用于获取车辆的实时经纬度定位信息，并将获得的信息传输给多传感器融合定位单元；
所述惯导单元，用于获取车辆的瞬时速度和瞬时加速度信息，并将获得的信息传输给多传感器融合定位单元；
所述局部区域位置建图单元，利用道路两侧建筑物中WiFi信号发射器发出的WiFi信号，建立城市高层建筑群区域的局部区域位置地图，以得到位置和WiFi信号之间的对应关系；
所述WiFi定位器单元，安装在车身内，接收道路两侧建筑物中WiFi信号发射器发出的WiFi信号，根据基于WiFi信号的局部位置匹配算法，计算出车辆在局部区域的相对位置定位信息，并将该相对位置定位信息发送给多传感器融合定位单元；
其中，所述位置和WiFi信号之间的对应关系具体为：将区域用一个矩形网格覆盖，在每一个网格点上，通过一段时间的数据采样(5到15分钟，大约每秒采集一次)得到每个位置点来自各WiFi信号发射器的接收信号强度，每一个点的接收信号强度是不同的，完成局部区域的定位，定位所用的坐标系为局部笛卡尔坐标系，应用东北天坐标系的方向朝向。
具体地，所述基于WiFi信号的局部位置匹配算法如下式所示：
Figure BDA0003512911630000061
式中，(m,n)表示局部地图中的坐标点，(mzk,nzk)表示得到的定位结果，N表示区域中WiFi信号发射器的数量，
Figure BDA0003512911630000062
表示驶入该区域中车辆接收到的信号，Si,(m,n)表示建立城市高层建筑群区域的局部区域位置地图时所创建的点。
所述多传感器融合定位单元，用于获取GPS单元发送的经纬度定位信息、WiFi定位器单元发送的相对位置定位信息以及惯导单元发送的瞬时速度和瞬时加速度信息；若接收到的GPS单元发送的信号为非空信号(当GPS信号被高楼遮挡时，GPS单元发送的信号为空信号)，则将其发送给多类型坐标系转换单元，然后利用卡尔曼滤波算法将经多类型坐标系转换单元转换后的局部笛卡尔坐标系内的定位信息与惯导单元发送的信息进行融合，将输出的结果发送给多类型坐标系转换单元；若接收到的GPS单元发送的信号为空信号，则使用WiFi定位器单元接收到的车辆在局部区域的相对位置定位信息与惯导单元发送的信息进行融合，将输出的结果发送给多类型坐标系转换单元；
其中，所述笛卡尔局部坐标系符合东北天坐标系的建立标准，x轴沿纬度方向指向东，y轴沿经度方向指向北，z轴垂直与地面指向天。
所述多类型坐标系转换单元，安装在车身内，用于将经纬度定位信息转换为地心坐标系定位信息，并进一步转换为局部笛卡尔坐标系内的定位信息；及将局部笛卡尔坐标系内的定位结果转换为地心坐标系内的定位信息，最后转换成经纬度定位信息。
参照图1所示，本发明还提供一种自动驾驶汽车定位方法，基于上述系统，步骤如下：
1)建立城市高层建筑群区域的局部区域位置地图，以得到位置和WiFi信号之间的对应关系；
具体地，所述步骤1)中建立城市高层建筑群区域的局部区域位置地图具体包括：
道路两侧建筑物中多个WiFi信号发射器发出WiFi信号，所述WiFi信号在每个点的信号强度由下述公式计算得到：
Figure BDA0003512911630000071
式中，d表示WiFi信号发射器与位置点的距离，β表示路径损耗系数，Pt表示WiFi信号发射器发送WiFi信号的功率，J表示WiFi信号发射器的天线增益系数，B表示WiFi信号发射器周围建筑物的墙体厚度的平均值，f表示WiFi信号的频率，α表示公路的宽度规格系数，根据不同的宽度设定不同的系数值：如果为双向四车道，则α＝7.5；如果为双向六车道，α＝11.25；如果为双向八车道，则α＝15(道路上每一个点的信号强度S都是特殊的，尤其是在WiFi信号发射器数量较多的情况下，每一个WiFi信号发射器发出的WiFi信号在该点都有一个特殊的值，假设道路两侧有M个WiFi信号发射器，则在某点其全部S可以表示为一个M阶向量[S1，S2…SM]；将此局部区域使用矩形网格划分，分解为m*n个坐标点，每一个坐标点均对应一个特殊的M阶向量[S1，S2…SM](m，n))。
2)实时获取道路行驶中的车辆自身的经纬度定位信息，及车辆自身的速度与加速度信息；
3)根据步骤2)中获取的经纬度定位信息的类型，判定进行融合定位的方式：若经纬度定位信息为非空信号，则使用卡尔曼滤波算法将其与惯导单元获取的定位信息进行融合；若经纬度定位信息为空信号，则使用基于WiFi信号的局部位置匹配算法获取车辆当前的相对位置，然后采用卡尔曼滤波算法，对WiFi定位器单元接收到的车辆在局部区域内的相对位置定位信息与惯导单元获取的定位信息进行融合，得到车辆在局部区域内融合后的相对位置定位信息；
具体地，所述步骤3)中惯导单元获取的定位信息为：沿车辆行驶方向的速度
Figure BDA0003512911630000081
垂直于车辆行驶方向上的速度
Figure BDA0003512911630000082
沿道路方向的车辆加速度αk，垂直于道路方向的车辆加速度βk，Δt是前后两个状态量之间时间的差值，根据车辆的加速度与上一时刻得到的状态量计算得出当前时刻的状态量：
Figure BDA0003512911630000083
Figure BDA0003512911630000084
Figure BDA0003512911630000085
Figure BDA0003512911630000086
式中，
Figure BDA0003512911630000087
表示k时刻车辆沿行驶方向的速度状态量，
Figure BDA0003512911630000088
表示k-1时刻车辆沿行驶方向的速度状态量，
Figure BDA0003512911630000089
表示k时刻垂直于车辆行驶方向的速度状态量，
Figure BDA00035129116300000810
表示k-1时刻垂直于车辆行驶方向的速度状态量；mk表示k时刻车辆在局部区域坐标系上沿m轴的坐标点，nk表示k时刻车辆在局部区域坐标系上沿n轴的坐标点。
参照图2所示，所述步骤3)中采用的基于WiFi信号的局部位置匹配算法如下式所示：
Figure BDA00035129116300000811
式中，(m，n)表示局部地图中的坐标点，(mzk，nzk)表示得到的定位结果，N表示区域中WiFi信号发射器的数量，
Figure BDA00035129116300000812
表示驶入该区域中车辆接收到的信号，Si，(m，n)表示步骤1)中建立局部区域位置地图时所创建的点。
所述步骤3)中采用卡尔曼滤波算法，对WiFi定位器单元接收到的车辆在局部区域内的相对位置定位信息与惯导单元获取的定位信息进行融合具体为如下式所示：
Figure BDA00035129116300000813
Figure BDA0003512911630000091
Figure BDA0003512911630000092
Figure BDA0003512911630000093
Pk＝(I-KkH)Pk
式中，xk-1表示k-1时刻的状态估计值，即车辆在局部笛卡尔坐标系中所处的坐标位置与速度，二者均由惯导单元计算得到；xk-1＝[pk-1，vk-1]T，pk-1＝[mk-1，nk-1]T，用于表示k-1时刻车辆在局部区域内所处的位置；
Figure BDA0003512911630000094
用于表示k-1时刻车辆自身纵向速度与横向速度，
Figure BDA0003512911630000095
表示车辆的纵向速度，
Figure BDA0003512911630000096
表示车辆的横向速度；
Figure BDA0003512911630000097
表示k时刻的先验状态估计值，是滤波计算的中间结果，即根据上一时刻(k-1时刻)的最优估计预测的k时刻的结果，是预测方程的结果；Pk-1和Pk分别表示k-1时刻和k时刻的后验估计协方差矩阵，表示此时状态不确定度，是滤波的结果之一，具体表达式为
Figure BDA0003512911630000098
Figure BDA0003512911630000099
Figure BDA00035129116300000910
表示k时刻的先验估计协方差矩阵，是滤波的中间计算结果，具体表达式为
Figure BDA00035129116300000911
H是状态变量到测量(观测)的转换矩阵，
Figure BDA00035129116300000912
zk表示测量值(观测值)，zk＝[pzk，vzk]T，位置pzk＝[mzk，nzk]T，由基于WiFi信号的局部位置匹配算法得到，速度
Figure BDA00035129116300000913
由惯导单元测得，pzk和vzk分别表示车辆在局部笛卡尔坐标系中所处的位置与当前车辆的速度，Kk表示滤波增益矩阵，是滤波的中间计算结果；A表示状态转移矩阵，通过运动学公式确定，令
Figure BDA00035129116300000914
Q表示过程激励噪声协方差，用来表示状态转换矩阵与实际过程之间的误差，用xe＝[pe，ve]T表示状态误差量，由不确定的噪声引起的，根据具体道路情况分析，该误差量与道路不平整度呈正相关，设道路不平整度的评价指标为λ，pe∝λ，ve∝λ，具体表达式为
Figure BDA0003512911630000101
R表示测量噪声协方差，用xs＝[ps，vs]T表示测量误差，通过传感器的精度获取，具体表达式为
Figure BDA0003512911630000102
B表示可选的控制输入增益矩阵，此处设为空矩阵；uk-1表示k-1时刻的控制增益，此处设为0；xk＝[pk，vk]T表示最终的定位结果。
4)将所述步骤3)中得到的车辆在局部区域内融合后的相对位置定位信息转换为当前车辆的经纬度定位信息输出。
本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (8)

1.一种自动驾驶汽车定位系统，其特征在于，包括：WiFi定位器单元、局部区域位置建图单元、GPS单元、惯导单元、多传感器融合定位单元及多类型坐标系转换单元；
所述GPS单元，用于获取车辆的实时经纬度定位信息，并将获得的信息传输给多传感器融合定位单元；
所述惯导单元，用于获取车辆的瞬时速度和瞬时加速度信息，并将获得的信息传输给多传感器融合定位单元；
所述局部区域位置建图单元，利用道路两侧建筑物中WiFi信号发射器发出的WiFi信号，建立城市高层建筑群区域的局部区域位置地图，以得到位置和WiFi信号之间的对应关系；
所述WiFi定位器单元，安装在车身内，接收道路两侧建筑物中WiFi信号发射器发出的WiFi信号，根据基于WiFi信号的局部位置匹配算法，计算出车辆在局部区域的相对位置定位信息，并将该相对位置定位信息发送给多传感器融合定位单元；
所述多传感器融合定位单元，用于获取GPS单元发送的经纬度定位信息、WiFi定位器单元发送的相对位置定位信息以及惯导单元发送的瞬时速度和瞬时加速度信息；若接收到的GPS单元发送的信号为非空信号，则将其发送给多类型坐标系转换单元，然后利用卡尔曼滤波算法将经多类型坐标系转换单元转换后的局部笛卡尔坐标系内的定位信息与惯导单元发送的信息进行融合，将输出的结果发送给多类型坐标系转换单元；若接收到的GPS单元发送的信号为空信号，则使用WiFi定位器单元接收到的车辆在局部区域的相对位置定位信息与惯导单元发送的信息进行融合，将输出的结果发送给多类型坐标系转换单元；
所述多类型坐标系转换单元，安装在车身内，用于将经纬度定位信息转换为地心坐标系定位信息，并进一步转换为局部笛卡尔坐标系内的定位信息；及将局部笛卡尔坐标系内的定位结果转换为地心坐标系内的定位信息，最后转换成当前车辆的经纬度定位信息。
2.根据权利要求1所述的自动驾驶汽车定位系统，其特征在于，所述位置和WiFi信号之间的对应关系具体为：将区域用一个矩形网格覆盖，在每一个网格点上，通过一段时间的数据采样得到每个位置点来自各WiFi信号发射器的接收信号强度，每一个点的接收信号强度是不同的，完成局部区域的定位，定位所用的坐标系为局部笛卡尔坐标系，应用东北天坐标系的方向朝向。
3.根据权利要求1所述的自动驾驶汽车定位系统，其特征在于，所述基于WiFi信号的局部位置匹配算法如下式所示：
Figure FDA0003512911620000011
式中，(m,n)表示局部地图中的坐标点，(mzk,nzk)表示得到的定位结果，N表示区域中WiFi信号发射器的数量，
Figure FDA0003512911620000021
表示驶入该区域中车辆接收到的信号，Si,(m,n)表示建立城市高层建筑群区域的局部区域位置地图时所创建的点。
4.一种自动驾驶汽车定位方法，基于权利要求1-3中任意一项所述的系统，其特征在于，步骤如下：
1)建立城市高层建筑群区域的局部区域位置地图，以得到位置和WiFi信号之间的对应关系；
2)实时获取道路行驶中的车辆自身的经纬度定位信息，及车辆自身的速度与加速度信息；
3)根据步骤2)中获取的经纬度定位信息的类型，判定进行融合定位的方式：若经纬度定位信息为非空信号，则使用卡尔曼滤波算法将其与惯导单元获取的定位信息进行融合；若经纬度定位信息为空信号，则使用基于WiFi信号的局部位置匹配算法获取车辆当前的相对位置，然后采用卡尔曼滤波算法，对WiFi定位器单元接收到的车辆在局部区域内的相对位置定位信息与惯导单元获取的定位信息进行融合，得到车辆在局部区域内融合后的相对位置定位信息；
4)将所述步骤3)中得到的车辆在局部区域内融合后的相对位置定位信息转换为当前车辆的经纬度定位信息输出。
5.根据权利要求4所述的自动驾驶汽车定位方法，其特征在于，所述步骤1)中建立城市高层建筑群区域的局部区域位置地图具体包括：
道路两侧建筑物中多个WiFi信号发射器发出WiFi信号，所述WiFi信号在每个点的信号强度由下述公式计算得到：
Figure FDA0003512911620000022
式中，d表示WiFi信号发射器与位置点的距离，β表示路径损耗系数，Pt表示WiFi信号发射器发送WiFi信号的功率，J表示WiFi信号发射器的天线增益系数，B表示WiFi信号发射器周围建筑物的墙体厚度的平均值，f表示WiFi信号的频率，α表示公路的宽度规格系数，根据不同的宽度设定不同的系数值：如果为双向四车道，则α＝7.5；如果为双向六车道，α＝11.25；如果为双向八车道，则α＝15。
6.根据权利要求4所述的自动驾驶汽车定位方法，其特征在于，所述步骤3)中惯导单元获取的定位信息为：沿车辆行驶方向的速度
Figure FDA0003512911620000031
垂直于车辆行驶方向上的速度
Figure FDA0003512911620000032
沿道路方向的车辆加速度αk，垂直于道路方向的车辆加速度βk，Δt是前后两个状态量之间时间的差值，根据车辆的加速度与上一时刻得到的状态量计算得出当前时刻的状态量：
Figure FDA0003512911620000033
Figure FDA0003512911620000034
Figure FDA0003512911620000035
Figure FDA0003512911620000036
式中，
Figure FDA0003512911620000037
表示k时刻车辆沿行驶方向的速度状态量，
Figure FDA0003512911620000038
表示k-1时刻车辆沿行驶方向的速度状态量，
Figure FDA0003512911620000039
表示k时刻垂直于车辆行驶方向的速度状态量，
Figure FDA00035129116200000310
表示k-1时刻垂直于车辆行驶方向的速度状态量；mk表示k时刻车辆在局部区域坐标系上沿m轴的坐标点，nk表示k时刻车辆在局部区域坐标系上沿n轴的坐标点。
7.根据权利要求4所述的自动驾驶汽车定位方法，其特征在于，所述步骤3)中采用的基于WiFi信号的局部位置匹配算法如下式所示：
Figure FDA00035129116200000311
式中，(m,n)表示局部地图中的坐标点，(mzk,nzk)表示得到的定位结果，N表示区域中WiFi信号发射器的数量，
Figure FDA00035129116200000312
表示驶入该区域中车辆接收到的信号，Si,(m,n)表示步骤1)中建立局部区域位置地图时所创建的点。
8.根据权利要求5所述的自动驾驶汽车定位方法，其特征在于，所述步骤3)中采用卡尔曼滤波算法，对WiFi定位器单元接收到的车辆在局部区域内的相对位置定位信息与惯导单元获取的定位信息进行融合具体为如下式所示：
Figure FDA00035129116200000313
Figure FDA0003512911620000041
Figure FDA0003512911620000042
Figure FDA0003512911620000043
Pk＝(I-KkH)Pk
式中，xk-1表示k-1时刻的状态估计值，即车辆在局部笛卡尔坐标系中所处的坐标位置与速度，二者均由惯导单元计算得到；xk-1＝[pk-1,vk-1]T，pk-1＝[mk-1,nk-1]T，用于表示k-1时刻车辆在局部区域内所处的位置；
Figure FDA0003512911620000044
用于表示k-1时刻车辆自身纵向速度与横向速度；
Figure FDA0003512911620000045
表示车辆的纵向速度，
Figure FDA0003512911620000046
表示车辆的横向速度；
Figure FDA0003512911620000047
表示k时刻的先验状态估计值，是滤波计算的中间结果，即根据k-1时刻的最优估计预测的k时刻的结果，是预测方程的结果；Pk-1和Pk分别表示k－1时刻和k时刻的后验估计协方差矩阵，表示此时状态不确定度，是滤波的结果之一，具体表达式为
Figure FDA0003512911620000048
Figure FDA0003512911620000049
Figure FDA00035129116200000410
表示k时刻的先验估计协方差矩阵，是滤波的中间计算结果，具体表达式为
Figure FDA00035129116200000411
H是状态变量到测量的转换矩阵，
Figure FDA00035129116200000412
zk表示测量值，zk＝[pzk,vzk]T，位置pzk＝[mzk,nzk]T，由基于WiFi信号的局部位置匹配算法得到，速度
Figure FDA00035129116200000413
由惯导单元测得，pzk和vzk分别表示车辆在局部笛卡尔坐标系中所处的位置与当前车辆的速度，Kk表示滤波增益矩阵，是滤波的中间计算结果；A表示状态转移矩阵，通过运动学公式确定，令
Figure FDA00035129116200000414
Q表示过程激励噪声协方差，用来表示状态转换矩阵与实际过程之间的误差，用xe＝[pe,ve]T表示状态误差量，由不确定的噪声引起的，根据具体道路情况分析，该误差量与道路不平整度呈正相关，设道路不平整度的评价指标为λ，pe∝λ，ve∝λ，具体表达式为
Figure FDA0003512911620000051
R表示测量噪声协方差，用xs＝[ps,vs]T表示测量误差，通过传感器的精度获取，具体表达式为
Figure FDA0003512911620000052
B表示可选的控制输入增益矩阵，此处设为空矩阵；uk-1表示k-1时刻的控制增益，此处设为0；xk＝[pk,vk]T表示最终的定位结果。
CN202210156490.9A 2022-02-21 一种自动驾驶汽车定位系统及方法 Active CN114608553B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210156490.9A CN114608553B (zh) 2022-02-21 一种自动驾驶汽车定位系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210156490.9A CN114608553B (zh) 2022-02-21 一种自动驾驶汽车定位系统及方法

Publications (2)

Publication Number Publication Date
CN114608553A true CN114608553A (zh) 2022-06-10
CN114608553B CN114608553B (zh) 2024-05-24

Family

ID=

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2002039138A1 (en) * 2000-11-06 2002-05-16 Lin Ching Fang Positioning and ground proximity warning method and system thereof for vehicle
KR101454153B1 (ko) * 2013-09-30 2014-11-03 국민대학교산학협력단 가상차선과 센서 융합을 통한 무인 자율주행 자동차의 항법시스템
CN106840179A (zh) * 2017-03-07 2017-06-13 中国科学院合肥物质科学研究院 一种基于多传感器信息融合的智能车定位方法
CN107064974A (zh) * 2017-02-28 2017-08-18 广东工业大学 一种抑制城市峡谷多径卫星信号的定位方法和系统
CN109581449A (zh) * 2018-12-14 2019-04-05 安徽江淮汽车集团股份有限公司 一种自动驾驶汽车的定位方法及系统
CN110208842A (zh) * 2019-05-28 2019-09-06 长安大学 一种车联网环境下车辆高精度定位方法
CN111949943A (zh) * 2020-07-24 2020-11-17 北京航空航天大学 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法
CN112083726A (zh) * 2020-09-04 2020-12-15 湖南大学 一种面向园区自动驾驶的双滤波器融合定位系统
CN112346103A (zh) * 2020-10-29 2021-02-09 清华大学 基于v2x的智能网联汽车动态协同定位方法与装置
CN113654555A (zh) * 2021-09-14 2021-11-16 上海智驾汽车科技有限公司 一种基于多传感器数据融合的自动驾驶车辆高精定位方法
KR20210153795A (ko) * 2020-06-10 2021-12-20 박경식 자율자동차 측위 방법

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2002039138A1 (en) * 2000-11-06 2002-05-16 Lin Ching Fang Positioning and ground proximity warning method and system thereof for vehicle
KR101454153B1 (ko) * 2013-09-30 2014-11-03 국민대학교산학협력단 가상차선과 센서 융합을 통한 무인 자율주행 자동차의 항법시스템
CN107064974A (zh) * 2017-02-28 2017-08-18 广东工业大学 一种抑制城市峡谷多径卫星信号的定位方法和系统
CN106840179A (zh) * 2017-03-07 2017-06-13 中国科学院合肥物质科学研究院 一种基于多传感器信息融合的智能车定位方法
CN109581449A (zh) * 2018-12-14 2019-04-05 安徽江淮汽车集团股份有限公司 一种自动驾驶汽车的定位方法及系统
CN110208842A (zh) * 2019-05-28 2019-09-06 长安大学 一种车联网环境下车辆高精度定位方法
KR20210153795A (ko) * 2020-06-10 2021-12-20 박경식 자율자동차 측위 방법
CN111949943A (zh) * 2020-07-24 2020-11-17 北京航空航天大学 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法
CN112083726A (zh) * 2020-09-04 2020-12-15 湖南大学 一种面向园区自动驾驶的双滤波器融合定位系统
CN112346103A (zh) * 2020-10-29 2021-02-09 清华大学 基于v2x的智能网联汽车动态协同定位方法与装置
CN113654555A (zh) * 2021-09-14 2021-11-16 上海智驾汽车科技有限公司 一种基于多传感器数据融合的自动驾驶车辆高精定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
张利 等: "一种 5G 高精度定位搭载自动驾驶车辆的解决方案", 长江信息通信, 15 November 2021 (2021-11-15), pages 29 - 32 *
彭文正 等: "多传感器信息融合的自动驾驶车辆定位与速度估计", 传感技术学报, vol. 33, no. 8, 15 August 2020 (2020-08-15), pages 1140 - 1148 *
徐文轩 等: "无人驾驶汽车环境感知与定位技术", 汽车科技, 25 November 2021 (2021-11-25), pages 52 - 60 *

Similar Documents

Publication Publication Date Title
CN106840179B (zh) 一种基于多传感器信息融合的智能车定位方法
CN108225302B (zh) 一种石化工厂巡检机器人定位系统和方法
CN111949943B (zh) 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法
CN109946730B (zh) 一种车路协同下基于超宽带的车辆高可靠融合定位方法
Milanés et al. Autonomous vehicle based in cooperative GPS and inertial systems
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
AU2011202456B2 (en) Traffic Management System for a Passageway Environment
CN101285686B (zh) 一种农业机械导航分级定位的方法和系统
JP2024050990A (ja) 判定装置
CN107132563B (zh) 一种里程计结合双天线差分gnss的组合导航方法
KR100779510B1 (ko) 정찰 로봇 및 정찰 로봇 운행 제어시스템
US11117590B2 (en) Method and system for determining effective wind speeds for motor vehicles
CN110187372B (zh) 一种低速无人车园区内组合导航方法及系统
CN112378410B (zh) 车辆行驶盲区校准方法、装置、设备及存储介质
CN113033494B (zh) 基于地理空间信息数据测绘的测绘数据采集系统
CN112188386B (zh) 一种基于etc信号强度的车辆定位方法
CN114199240A (zh) 无gps信号下二维码、激光雷达与imu融合定位系统及方法
CN108885269A (zh) 导航方法、导航装置和导航系统
CN109781118A (zh) 一种无人车的位置跟踪方法
CN211427151U (zh) 一种应用于封闭场地无人驾驶货运车辆上的自动引导系统
CN110057371B (zh) 一种基于压缩感知的行星巡视器主动路径规划方法
CN114608553B (zh) 一种自动驾驶汽车定位系统及方法
CN114608553A (zh) 一种自动驾驶汽车定位系统及方法
CN115951369A (zh) 面向复杂港口环境的多传感器融合定位方法
CN113063441B (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