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

1.本发明属于车辆定位技术领域，具体涉及一种适应于城市高层建筑群的自动驾驶汽车定位系统及方法。


背景技术：

2.自动驾驶汽车(autonomous vehicles；self-driving automobile)又称无人驾驶汽车、电脑驾驶汽车、或轮式移动机器人，是一种通过电脑系统实现无人驾驶的智能汽车。
3.与人驾驶汽车不同的是，自动驾驶汽车需要实时获取当前自身的位置与姿态，为下一时刻的运动的规划和决策提供信息。目前自动驾驶汽车广泛应用的定位方法为gps 惯导单元融合定位法，这种融合方式所需要的器件简单、成本低廉，但也存在一个问题：在城市中行驶时，gps信号容易受到高层建筑物的遮挡，无法通过高频的gps信号来修正惯导单元的位置与姿态的判定，这时便会导致gps 惯导单元的融合定位效果不佳。
4.因此，如何提升自动驾驶汽车的定位稳定性，保证自动驾驶汽车可以在城市高层建筑的包围中稳定获取自身的位置与姿态，是目前自动驾驶领域亟需解决的问题。


技术实现要素：

5.针对于上述现有技术的不足，本发明的目的在于提供一种自动驾驶汽车定位系统及方法，以解决现有技术中车辆gps信号容易受到高层建筑物的遮挡，无法通过高频的gps信号来修正惯导单元的位置与姿态的判定的问题。本发明能够使自动驾驶汽车在城市高层建筑群中稳定获取自身的定位信号，提高了自动驾驶汽车的定位性能，同时为其后续系统中运动的规划决策提供充足的定位数据。
6.为达到上述目的，本发明采用的技术方案如下：
7.本发明的一种自动驾驶汽车定位系统，包括：wifi定位器单元、局部区域位置建图单元、gps单元、惯导单元、多传感器融合定位单元及多类型坐标系转换单元；
8.所述gps单元，用于获取车辆的实时经纬度定位信息，并将获得的信息传输给多传感器融合定位单元；
9.所述惯导单元，用于获取车辆的瞬时速度和瞬时加速度信息，并将获得的信息传输给多传感器融合定位单元；
10.所述局部区域位置建图单元，利用道路两侧建筑物中wifi信号发射器发出的wifi信号，建立城市高层建筑群区域的局部区域位置地图，以得到位置和wifi信号之间的对应关系；
11.所述wifi定位器单元，安装在车身内，接收道路两侧建筑物中wifi信号发射器发出的wifi信号，根据基于wifi信号的局部位置匹配算法，计算出车辆在局部区域的相对位置定位信息，并将该相对位置定位信息发送给多传感器融合定位单元；
12.所述多传感器融合定位单元，用于获取gps单元发送的经纬度定位信息、wifi定位器单元发送的相对位置定位信息以及惯导单元发送的瞬时速度和瞬时加速度信息；若接收
到的gps单元发送的信号为非空信号，则将其发送给多类型坐标系转换单元，然后利用卡尔曼滤波算法将经多类型坐标系转换单元转换后的局部笛卡尔坐标系内的定位信息与惯导单元发送的信息进行融合，将输出的结果发送给多类型坐标系转换单元；若接收到的gps单元发送的信号为空信号，则使用wifi定位器单元接收到的车辆在局部区域的相对位置定位信息与惯导单元发送的信息进行融合，将输出的结果发送给多类型坐标系转换单元；
13.所述多类型坐标系转换单元，安装在车身内，用于将经纬度定位信息转换为地心坐标系定位信息，并进一步转换为局部笛卡尔坐标系内的定位信息；及将局部笛卡尔坐标系内的定位结果转换为地心坐标系内的定位信息，最后转换成当前车辆的经纬度定位信息。
14.进一步地，所述笛卡尔局部坐标系符合东北天坐标系的建立标准，x轴沿纬度方向指向东，y轴沿经度方向指向北，z轴垂直与地面指向天。
15.进一步地，所述位置和wifi信号之间的对应关系具体为：将区域用一个矩形网格覆盖，在每一个网格点上，通过一段时间的数据采样得到每个位置点来自各wifi信号发射器的接收信号强度，每一个点的接收信号强度是不同的，完成局部区域的定位，定位所用的坐标系为局部笛卡尔坐标系，应用东北天坐标系的方向朝向。
16.进一步地，所述基于wifi信号的局部位置匹配算法如下式所示：
[0017][0018]
式中，(m,n)表示局部地图中的坐标点，(m
zk
,n
zk
)表示得到的定位结果，n表示区域中wifi信号发射器的数量，表示驶入该区域中车辆接收到的信号，s
i,(m,n)
表示建立城市高层建筑群区域的局部区域位置地图时所创建的点。
[0019]
本发明还提供一种自动驾驶汽车定位方法，步骤如下：
[0020]
1)建立城市高层建筑群区域的局部区域位置地图，以得到位置和wifi信号之间的对应关系；
[0021]
2)实时获取道路行驶中的车辆自身的经纬度定位信息，及车辆自身的速度与加速度信息；
[0022]
3)根据步骤2)中获取的经纬度定位信息的类型，判定进行融合定位的方式：若经纬度定位信息为非空信号，则使用卡尔曼滤波算法将其与惯导单元获取的定位信息进行融合；若经纬度定位信息为空信号，则使用基于wifi信号的局部位置匹配算法获取车辆当前的相对位置，然后采用卡尔曼滤波算法，对wifi定位器单元接收到的车辆在局部区域内的相对位置定位信息与惯导单元获取的定位信息进行融合，得到车辆在局部区域内融合后的相对位置定位信息；
[0023]
4)将所述步骤3)中得到的车辆在局部区域内融合后的相对位置定位信息转换为当前车辆的经纬度定位信息输出。
[0024]
进一步地，所述步骤1)中建立城市高层建筑群区域的局部区域位置地图具体包括：
[0025]
道路两侧建筑物中多个wifi信号发射器发出wifi信号，所述wifi信号在每个点的信号强度由下述公式计算得到：
[0026][0027]
式中，d表示wifi信号发射器与位置点的距离，β表示路径损耗系数，p
t
表示wifi信号发射器发送wifi信号的功率，j表示wifi信号发射器的天线增益系数，b表示wifi信号发射器周围建筑物的墙体厚度的平均值，f表示wifi信号的频率，α表示公路的宽度规格系数，根据不同的宽度设定不同的系数值：如果为双向四车道，则α＝7.5；如果为双向六车道，α＝11.25；如果为双向八车道，则α＝15。
[0028]
进一步地，所述步骤3)中惯导单元获取的定位信息为：沿车辆行驶方向的速度垂直于车辆行驶方向上的速度沿道路方向的车辆加速度αk，垂直于道路方向的车辆加速度βk，δt是前后两个状态量之间时间的差值，根据车辆的加速度与上一时刻得到的状态量计算得出当前时刻的状态量：
[0029][0030][0031][0032][0033]
式中，表示k时刻车辆沿行驶方向的速度状态量，表示k-1时刻车辆沿行驶方向的速度状态量，表示k时刻垂直于车辆行驶方向的速度状态量，表示k-1时刻垂直于车辆行驶方向的速度状态量；mk表示k时刻车辆在局部区域坐标系上沿m轴的坐标点，nk表示k时刻车辆在局部区域坐标系上沿n轴的坐标点。
[0034]
进一步地，所述步骤3)中采用的基于wifi信号的局部位置匹配算法如下式所示：
[0035][0036]
式中，(m，n)表示局部地图中的坐标点，(m
zk
，n
zk
)表示得到的定位结果，n表示区域中wifi信号发射器的数量，表示驶入该区域中车辆接收到的信号，s
i，(m，n)
表示步骤1)中建立局部区域位置地图时所创建的点。
[0037]
进一步地，所述步骤3)中采用卡尔曼滤波算法，对wifi定位器单元接收到的车辆在局部区域内的相对位置定位信息与惯导单元获取的定位信息进行融合具体为如下式所示：
[0038][0039]
[0040][0041][0042]
pk＝(i-kkh)pk[0043]
式中，x
k-1
表示k-1时刻的状态估计值，即车辆在局部笛卡尔坐标系中所处的坐标位置与速度，二者均由惯导单元计算得到；x
k-1
＝[p
k-1
，v
k-1
]
t
，p
k-1
＝[m
k-1
，n
k-1
]
t
，用于表示k-1时刻车辆在局部区域内所处的位置；用于表示k-1时刻车辆自身纵向速度与横向速度，表示车辆的纵向速度，表示车辆的横向速度；表示k时刻的先验状态估计值，是滤波计算的中间结果，即根据上一时刻(k-1时刻)的最优估计预测的k时刻的结果，是预测方程的结果；p
k-1
和pk分别表示k-1时刻和k时刻的后验估计协方差矩阵，表示此时状态不确定度，是滤波的结果之一，具体表达式为矩阵，表示此时状态不确定度，是滤波的结果之一，具体表达式为矩阵，表示此时状态不确定度，是滤波的结果之一，具体表达式为表示k时刻的先验估计协方差矩阵，是滤波的中间计算结果，具体表达式为h是状态变量到测量的转换矩阵，zk表示测量值，zk＝[p
zk
，v
zk
]
t
，位置p
zk
＝[m
zk
，n
zk
]
t
，由基于wifi信号的局部位置匹配算法得到，速度由惯导单元测得，p
zk
和v
zk
分别表示车辆在局部笛卡尔坐标系中所处的位置与当前车辆的速度，kk表示滤波增益矩阵，是滤波的中间计算结果；a表示状态转移矩阵，通过运动学公式确定，令q表示过程激励噪声协方差，用来表示状态转换矩阵与实际过程之间的误差，用xe＝[pe，ve]
t
表示状态误差量，由不确定的噪声引起的，根据具体道路情况分析，该误差量与道路不平整度呈正相关，设道路不平整度的评价指标为λ，pe∝
λ，ve∝
λ，具体表达式为r表示测量噪声协方差，用xs＝[ps，vs]
t
表示测量误差，通过传感器的精度获取，具体表达式为b表示可选的控制输入增益矩阵，
此处设为空矩阵；u
k-1
表示k-1时刻的控制增益，此处设为0；xk＝[pk，vk]
t
表示最终的定位结果。
[0044]
本发明的有益效果：
[0045]
1、本发明保证了自动驾驶汽车在gps定位信号被高楼遮挡时定位信号输出的稳定性，提高了自动驾驶车辆在城市道路中行驶的安全性。
[0046]
2、本发明利用了城市高层建筑群中的多个wifi信号发射器进行定位，无需额外布置用于定位的设备，大大节约了成本。
附图说明
[0047]
图1为本发明定位方法原理图。
[0048]
图2为本发明中的wifi建图与定位匹配原理图。
具体实施方式
[0049]
为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。
[0050]
本发明的一种自动驾驶汽车定位系统，包括：wifi定位器单元、局部区域位置建图单元、gps单元、惯导单元、多传感器融合定位单元及多类型坐标系转换单元；
[0051]
所述gps单元，用于获取车辆的实时经纬度定位信息，并将获得的信息传输给多传感器融合定位单元；
[0052]
所述惯导单元，用于获取车辆的瞬时速度和瞬时加速度信息，并将获得的信息传输给多传感器融合定位单元；
[0053]
所述局部区域位置建图单元，利用道路两侧建筑物中wifi信号发射器发出的wifi信号，建立城市高层建筑群区域的局部区域位置地图，以得到位置和wifi信号之间的对应关系；
[0054]
所述wifi定位器单元，安装在车身内，接收道路两侧建筑物中wifi信号发射器发出的wifi信号，根据基于wifi信号的局部位置匹配算法，计算出车辆在局部区域的相对位置定位信息，并将该相对位置定位信息发送给多传感器融合定位单元；
[0055]
其中，所述位置和wifi信号之间的对应关系具体为：将区域用一个矩形网格覆盖，在每一个网格点上，通过一段时间的数据采样(5到15分钟，大约每秒采集一次)得到每个位置点来自各wifi信号发射器的接收信号强度，每一个点的接收信号强度是不同的，完成局部区域的定位，定位所用的坐标系为局部笛卡尔坐标系，应用东北天坐标系的方向朝向。
[0056]
具体地，所述基于wifi信号的局部位置匹配算法如下式所示：
[0057][0058]
式中，(m,n)表示局部地图中的坐标点，(m
zk
,n
zk
)表示得到的定位结果，n表示区域中wifi信号发射器的数量，表示驶入该区域中车辆接收到的信号，s
i,(m,n)
表示建立城市高层建筑群区域的局部区域位置地图时所创建的点。
[0059]
所述多传感器融合定位单元，用于获取gps单元发送的经纬度定位信息、wifi定位
器单元发送的相对位置定位信息以及惯导单元发送的瞬时速度和瞬时加速度信息；若接收到的gps单元发送的信号为非空信号(当gps信号被高楼遮挡时，gps单元发送的信号为空信号)，则将其发送给多类型坐标系转换单元，然后利用卡尔曼滤波算法将经多类型坐标系转换单元转换后的局部笛卡尔坐标系内的定位信息与惯导单元发送的信息进行融合，将输出的结果发送给多类型坐标系转换单元；若接收到的gps单元发送的信号为空信号，则使用wifi定位器单元接收到的车辆在局部区域的相对位置定位信息与惯导单元发送的信息进行融合，将输出的结果发送给多类型坐标系转换单元；
[0060]
其中，所述笛卡尔局部坐标系符合东北天坐标系的建立标准，x轴沿纬度方向指向东，y轴沿经度方向指向北，z轴垂直与地面指向天。
[0061]
所述多类型坐标系转换单元，安装在车身内，用于将经纬度定位信息转换为地心坐标系定位信息，并进一步转换为局部笛卡尔坐标系内的定位信息；及将局部笛卡尔坐标系内的定位结果转换为地心坐标系内的定位信息，最后转换成经纬度定位信息。
[0062]
参照图1所示，本发明还提供一种自动驾驶汽车定位方法，基于上述系统，步骤如下：
[0063]
1)建立城市高层建筑群区域的局部区域位置地图，以得到位置和wifi信号之间的对应关系；
[0064]
具体地，所述步骤1)中建立城市高层建筑群区域的局部区域位置地图具体包括：
[0065]
道路两侧建筑物中多个wifi信号发射器发出wifi信号，所述wifi信号在每个点的信号强度由下述公式计算得到：
[0066][0067]
式中，d表示wifi信号发射器与位置点的距离，β表示路径损耗系数，p
t
表示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)
)。
[0068]
2)实时获取道路行驶中的车辆自身的经纬度定位信息，及车辆自身的速度与加速度信息；
[0069]
3)根据步骤2)中获取的经纬度定位信息的类型，判定进行融合定位的方式：若经纬度定位信息为非空信号，则使用卡尔曼滤波算法将其与惯导单元获取的定位信息进行融合；若经纬度定位信息为空信号，则使用基于wifi信号的局部位置匹配算法获取车辆当前的相对位置，然后采用卡尔曼滤波算法，对wifi定位器单元接收到的车辆在局部区域内的相对位置定位信息与惯导单元获取的定位信息进行融合，得到车辆在局部区域内融合后的相对位置定位信息；
1时刻车辆在局部区域内所处的位置；用于表示k-1时刻车辆自身纵向速度与横向速度，表示车辆的纵向速度，表示车辆的横向速度；表示k时刻的先验状态估计值，是滤波计算的中间结果，即根据上一时刻(k-1时刻)的最优估计预测的k时刻的结果，是预测方程的结果；p
k-1
和pk分别表示k-1时刻和k时刻的后验估计协方差矩阵，表示此时状态不确定度，是滤波的结果之一，具体表达式为矩阵，表示此时状态不确定度，是滤波的结果之一，具体表达式为矩阵，表示此时状态不确定度，是滤波的结果之一，具体表达式为表示k时刻的先验估计协方差矩阵，是滤波的中间计算结果，具体表达式为h是状态变量到测量(观测)的转换矩阵，zk表示测量值(观测值)，zk＝[p
zk
，v
zk
]
t
，位置p
zk
＝[m
zk
，n
zk
]
t
，由基于wifi信号的局部位置匹配算法得到，速度由惯导单元测得，p
zk
和v
zk
分别表示车辆在局部笛卡尔坐标系中所处的位置与当前车辆的速度，kk表示滤波增益矩阵，是滤波的中间计算结果；a表示状态转移矩阵，通过运动学公式确定，令q表示过程激励噪声协方差，用来表示状态转换矩阵与实际过程之间的误差，用xe＝[pe，ve]
t
表示状态误差量，由不确定的噪声引起的，根据具体道路情况分析，该误差量与道路不平整度呈正相关，设道路不平整度的评价指标为λ，pe∝
λ，ve∝
λ，具体表达式为r表示测量噪声协方差，用xs＝[ps，vs]
t
表示测量误差，通过传感器的精度获取，具体表达式为b表示可选的控制输入增益矩阵，此处设为空矩阵；u
k-1
表示k-1时刻的控制增益，此处设为0；xk＝[pk，vk]
t
表示最终的定位结果。
[0086]
4)将所述步骤3)中得到的车辆在局部区域内融合后的相对位置定位信息转换为当前车辆的经纬度定位信息输出。
[0087]
本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。