一种车辆三维定位方法和系统与流程

1.本技术涉及车辆定位技术领域，尤其涉及一种车辆三维定位方法和系统。


背景技术：

2.车辆三维定位是指对车辆在三维空间中的位置进行预估，能够实现对于车辆运动状态的监控，可以应用于车辆导航系统指导用户驾驶。
3.在相关技术中，车辆导航系统普遍采用车辆在二维平面的运动信息对车辆进行定位和导航，其重点关注车辆在水平面方向的运动信息，忽视了车辆在垂直(高度)方向的运动信息，如此导致车辆在一些场景中定位精度不高。例如，车辆在立交桥或高架桥区域行驶时，车辆导航系统无法分辨桥上还是桥下；车辆在多层立体停车库时，车辆导航系统无法分辨车辆所在停车场层数
4.上述车辆定位技术，由于在高度方向上的运动信息不准或缺失，导致车辆车道级高精度定位实现困难，用户驾驶体验较差。


技术实现要素：

5.为了解决现有技术存在的上述技术问题，本技术提供了一种车辆三维定位方法和系统，提高了车辆三维定位精度。
6.一方面，本技术实施例提供了一种车辆三维定位方法，所述方法包括：
7.通过定位系统获取所述目标车辆的三维状态观测量；所述定位系统包括车辆里程计、惯性测量单元imu、气压计和卫星定位模块；
8.若确定所述三维状态观测量满足稳定性条件，根据所述三维状态观测量初始化所述定位系统中的卡尔曼滤波器；
9.若确定所述目标车辆处于运动状态，根据所述三维状态观测量，通过初始化后的所述卡尔曼滤波器确定所述目标车辆的三维状态。
10.在一种可能的实施方式中，所述三维状态观测量包括三维姿态观测量，所述三维姿态观测量包括航向角、横滚角和俯仰角，所述通过定位系统获取所述目标车辆的三维状态观测量包括：
11.通过所述车辆里程计获取所述目标车辆的速度信息，通过所述imu中的加速度计获取所述目标车辆的比力信息，以及通过所述imu中的陀螺仪获取所述目标车辆的角速度信息；
12.对所述速度信息、所述比力信息和所述角速度信息进行过滤；
13.根据过滤后的速度信息和过滤后的所述角速度信息，修正过滤后的所述比力信息，得到所述目标车辆的三轴重力加速度；
14.根据所述三轴重力加速度，计算所述目标车辆的横滚角和俯仰角；
15.通过所述卫星定位模块获取所述目标车辆的双天线航向角。
16.在一种可能的实施方式中，所述根据过滤后的速度信息，修正过滤后的所述比力
信息，得到所述目标车辆的三轴重力加速度包括：
17.根据所述速度信息，确定所述目标车辆的前向线性加速度；
18.根据所述速度信息和所述目标车辆航向轴角速度，确定所述目标车辆的向心加速度；
19.根据所述前向线性加速度和通过所述加速度计获取的前向比力，确定所述目标车辆的前向重力加速度；
20.根据所述向心加速度和通过所述加速度计获取的侧向比力，确定所述目标车辆的侧向重力加速度。
21.在一种可能的实施方式中，所述三维状态观测量包括三维位置观测量，所述通过定位系统获取所述目标车辆的三维位置观测量包括：
22.通过所述卫星定位模块获取所述目标车辆的三维位置信息；其中，所述三维位置信息所包括的固定解和浮点解用作所述三维位置观测量；
23.通过所述气压计获取所述目标车辆的相对高度位置信息；所述相对高度位置信息用作所述三维位置观测量，且是以所述固定解作为基准确定的。
24.在一种可能的实施方式中，所述方法还包括：
25.对所述相对高度位置信息进行有效性检验，得到第一检验结果；
26.若所述第一检验结果标识所述相对高度位置信息有效，则将所述相对高度位置信息作为所述目标车辆的高度位置观测量。
27.在一种可能的实施方式中，所述方法还包括：
28.若所述卫星定位模块获取的高度位置信息满足可用性条件，且所述气压计获取的所述相对高度位置信息不满足所述可用性条件，则对所述卫星定位模块获取的浮点解高度位置信息进行有效性检验，得到第二检验结果；
29.若所述第二检验结果标识所述浮点解有效，则将所述浮点解高度位置信息作为所述目标车辆的高度位置观测量。
30.在一种可能的实施方式中，所述方法还包括：
31.若所述卫星定位模块获取的高度位置信息满足可用性条件，且所述气压计获取的所述相对高度位置信息满足所述可用性条件，确定所述高度位置信息是否包括固定解；
32.若是，将所述固定解高度位置信息作为所述目标车辆的高度位置观测量；
33.若否，将所述卫星定位模块获取的高度位置信息和所述气压计获取的所述相对高度位置信息分别与卡尔曼状态递推预测高度作差，得到所述卫星定位模块对应的第一差值和所述气压计对应的第二差值；
34.若所述第一差值小于所述第二差值，将所述卫星定位模块获取的高度位置信息作为所述目标车辆的高度位置观测量；
35.若所述第一差值不小于所述第二差值，将所述气压计获取的所述相对高度位置信息作为所述目标车辆的高度位置观测量。
36.在一种可能的实施方式中，所述通过目标车辆定位系统获取所述目标车辆的三维速度观测量包括：
37.通过所述卫星定位模块获取所述目标车辆的三维速度信息，以及通过所述车辆里程计获取所述目标车辆的速度信息；
38.若所述卫星定位模块的地速大于第一阈值，且通过所述卫星定位模块获取所述目标车辆在高度方向的速度小于第二阈值，将所述卫星定位模块对应的所述三维速度信息和所述车辆里程计对应的所述速度信息作为所述目标车辆的三维速度观测量。
39.在一种可能的实施方式中，所述方法还包括：
40.若所述卫星定位模块被遮挡的时间超过时间阈值，剔除所述卫星定位模块解除遮挡后预设时间内的数据。
41.另一方面，本技术实施例提供了一种车辆三维定位系统，所述系统包括车辆里程计、惯性测量单元imu、气压计、卫星定位模块和卡尔曼滤波器；所述系统用于执行上述方面所述的车辆三维定位方法。
42.由上述技术方案可以看出，通过目标车辆定位系统获取目标车辆的三维状态观测量，若确定三维状态观测量满足稳定性条件，表明定位系统所包括的多个传感器输出的测量数据都趋于稳定状态，因此可以根据三维状态观测量初始化定位系统中的卡尔曼滤波器，为卡尔曼滤波器解算目标车辆三维状态提供了可靠的测量数据。若目标车辆处于运动状态时，表明目标车辆的三维状态发生了改变，因此根据三维状态观测量，通过初始化后的卡尔曼滤波器确定所述目标车辆的三维状态。由于定位系统包括车辆里程计、惯性测量单元、气压计和卫星定位模块，通过卡尔曼滤波器对由这多个传感器获取的测量数据进行融合解算，提高了定位系统的可靠性的同时，也提高了对于车辆进行三维定位的准确性，使得车辆在需要高度位置信息进行导航等场景中也有较好的定位精度，从而提高了用户驾驶体验。
附图说明
43.为了更清楚地说明本技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本技术中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
44.图1为本技术实施例提供的一种车辆三维定位方法的流程示意图；
45.图2为本技术实施例提供的一种车辆三维定位系统的结构示意图。
具体实施方式
46.为了使本技术领域的人员更好地理解本技术方案，下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。
47.在相关技术中，由于车辆高度信息缺失或不准，导致车辆定位精度不高，影响了用户驾驶体验。故此，本技术实施例提供了一种车辆三维定位方法和系统，提高了对于车辆三维定位的精度。
48.参见图1，图1为本技术实施例提供的一种车辆三维定位方法的流程示意图。如图1所示，该车辆三维定位方法包括以下步骤：
49.s101：通过定位系统获取所述目标车辆的三维状态观测量。
50.在本技术实施例中，定位系统包括车辆里程计、惯性测量单元(inertial measurement unit,imu)、气压计和卫星定位模块。其中，imu包括陀螺仪和加速度计。陀螺仪用于测量车辆三轴姿态角(或角速率)。加速度计用于测量车辆三轴加速度。卫星定位模块可以是全球定位系统(global positioning system,gps)，在此不作任何限定。
51.在定位过程中，利用上述定位系统所包括的多个传感器对目标车辆的三维状态进行测量，并将测量数据作为目标车辆的三维状态观测量。其中，三维状态观测量包括三维姿态观测量、三维位置观测量和三维速度观测量。
52.三维姿态观测量包括航向角、横滚角和俯仰角。本技术通过车辆里程计和imu获取目标车辆的横滚角和俯仰角，通过卫星定位模块获取目标车辆的航向角。由于通过传感器获取的测量数据存在噪声，因此在实际应用中，可以对测量数据进行滤波处理，降低噪声对有效数据的影响，提高定位精度。
53.具体的，通过车辆里程计获取目标车辆的速度信息，通过加速度计获取目标车辆的比力信息，以及通过陀螺仪获取目标车辆的角速度信息。然后，对速度信息、比力信息和角速度信息。在应用过程中，可以利用滤波器对前述测量数据进行过滤，在此不做任何限定。
54.可以理解的是，通过imu中的加速度计输出的是比力信息，而比力信息标识的是目标车辆非引力外力信息，即目标车辆实际加速度与引力只差，因此由imu获取的比力信息并非目标车辆真实的加速度信息，尤其是车辆在转弯或者加减速运动时，比力信息与车辆实际加速度存在较大的误差，因此不可以直接将比力信息作为车辆的加速度信息。
55.故此，在获取目标车辆的测量数据时，对由imu获取的比力信息进行修正。具体的，可以根据上述过滤后的速度信息和过滤后的角速度信息，对过滤后的比例信息进行修正，得到目标车辆的三轴重力加速度。然后，可以根据该三轴重力加速度，计算出目标车辆的横滚角和俯仰角。
56.此外，本技术通过卫星定位模块获取目标车辆的双天线航向角。基于此，将上述滤波、修正后得到横滚角、俯仰角和双天线航向角作为目标车辆的三维姿态观测量，用于估算目标车辆的三维姿态。
57.具体计算过程中，根据速度信息计算得到车辆前向线性加速度，并利用速度信息与车辆航向轴角速率计算得到车辆向心加速度，然后利用前向线性加速度减去车辆前向比力，得到车辆前向重力加速度分量，利用车辆向心加速度减去车辆侧向比力，得到车辆侧向重力加速度分量，并将车辆三轴加速度信息归一化，并利用归一化后的车辆三轴重力加速度分量通过公式asin(-a
x
)、atan(ay/az)计算得到俯仰角和横滚角。其中，a
x
表示前向重力加速度、ay表示侧向重力加速度、az表示天向加速度。
58.三维位置观测量包括目标车辆在二维平面的位置，也包括在高度方向上的位置。由于只依靠单传感器获取目标车辆的三维位置观测量，无法保证连续有效的定位，因此本技术结合卫星定位模块和气压计对目标车辆的三维位置进行测量。具体的，通过气压计获取目标车辆的高度位置信息，以及通过卫星定位模块获取目标车辆的三维位置信息。其中，将三维位置信息所包括的固定解和浮点解用作目标车辆的三维位置观测量。基于卫星定位模块和气压计获取目标车辆的三维位置观测量，与单传感器相比，提高了定位系统的可靠性和准确性。
59.一般情况下，卫星定位模块的定位精度高于气压计。因此，本技术在卫星定位模块的固定解更新时，利用固定解所包括的高度位置信息作为气压计的基准值，并将相对该基准值的相对高度信息作为目标车辆的三维位置观测量，解决了气压计因温度升高等环境因素造成基准偏移的问题，提高了通过气压计获取的高度位置信息的准确性。
60.三维速度观测量包括目标车辆在三轴方向的速度。本技术通过车辆里程计获取目标车辆的速度信息，以及通过卫星定位模块获取目标车辆的三维速度信息。同样的，相较于单传感器，提高了定位系统的可靠性和准确性。
61.在实际驾驶场景中，由于车辆爬坡存在速度限制，且卫星定位模块在低速下的测量数据精度较低，因此本技术通过设置阈值的方式从通过卫星定位模块获取的三维速度信息，以及通过车辆里程计获取的速度信息中选择较为准确的测量数据。具体的，若卫星定位模块的地速大于第一阈值，且通过卫星定位模块获取所述目标车辆在高度方向的速度小于第二阈值，将卫星定位模块对应的三维速度信息和车辆里程计对应的速度信息作为目标车辆的三维速度观测量。
62.上述通过多个传感器获取目标车辆的姿态观测量、三维位置观测量和三维速度观测量，为后续利用卡尔曼滤波器进行多信息融合解算目标车辆的三维姿态提供了可靠的基础数据，从而提高了定位系统的可靠性和准确性。
63.s102：若确定所述三维状态观测量满足稳定性条件，根据所述三维状态观测量初始化所述定位系统中的卡尔曼滤波器。
64.由于开始时刻定位系统中各传感器获取的测量数据可能是不稳定的，而不稳定的测量数据可信度较低，无法较为准确地对目标车辆进行定位。因此本技术通过设定稳定性条件，判断定位系统中传感器针对目标车辆的测量数据，即上述s101中的三维状态观测量是否可信。
65.若确定三维状态观测量满足稳定性条件，表明定位系统各传感器处于较为稳定的状态，其获取的测量数据是可信的，能够较为准确的反映目标车辆所处的状态。在此情况下，利用三维状态观测量对定位系统中卡尔曼滤波器进行初始化。若确定三维状态观测量不满足稳定性条件，表明定位系统各传感器的状态不够稳定，其获取的测量数据是不可信的，不适用于准确地定位出目标车辆。
66.卡尔曼滤波器(kalman filter)是一种高效率的递归滤波器(自回归滤波器)，它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。在本技术中，利用卡尔曼滤波器对目标车辆进行三维定位。
67.在初始化过程中，启动定位系统，待定位系统中各传感器针对目标车辆获取的测量数据输入稳定后，利用卫星定位模块和imu获取的横滚角、俯仰角和航向角对卡尔曼滤波器的姿态参数进行初始化，利用卫星定位模块获取的三维位置信息和气压计获取的高度信息对卡尔曼滤波器的位置参数进行初始化，以及利用卫星定位模块获取的三维速度信息和车辆里程计获取的速度信息对卡尔曼滤波器的速度参数进行初始化。
68.s103：若确定所述目标车辆处于运动状态，根据所述三维状态观测量，通过初始化后的所述卡尔曼滤波器确定所述目标车辆的三维状态。
69.由于卡尔曼滤波器是根据车辆前一时刻的运动状态预测后一时刻的运动状态，因此在实际应用中，需要先利用车辆里程计获取的速度信息判断目标车辆是否处于运动状
态，从而利用卡尔曼滤波器结合多传感器实现对于目标车辆的三维定位。
70.若确定目标车辆处于运动状态，则可以根据上述s101获取的三维状态观测量，通过卡尔曼滤波器利用方程对目标车辆进行最优估计，确定出目标车辆的三维状态。其中，三维状态包括三维姿态、三维位置和三维速度。
71.考虑到计算过程中涉及到误差协方差阵，若直接对目标车辆在三维空间中的运动状态进行解算，会导致较大的解算误差。因此在实际解算过程中，将平面与高度解算分开，即对误差协方差阵分别进行对应行和列的计算，消除对于其他行和列的影响，使得利用卡尔曼滤波器进行高度与平面的解算互不干扰，提高解算精度，从而提高了对于车辆三维定位的精度。
72.在实际应用过程中，卫星定位模块存在被遮挡的情况，在此场景中卫星定位模块输出的测量数据精度较低，甚至无法实时输出测量数据。故此，若检测到卫星定位模块被遮挡的时间超过时间阈值，剔除卫星定位模块解除遮挡后预设时间内的数据。其中，时间阈值和预设时间可以根据实际场景预先设定，在此不做任何限定。如此提高了利用卫星定位模块进行车辆三维定位的定位精度，提高了整个系统的可靠性和稳定性。
73.基于上述s101可知，通过气压计输出的测量数据基于基准校准后可以获取目标车辆的相对高度位置信息。在实际应用中，还可以对相对高度信息进行有效性校验，进一步提高对于车辆在高度方向上的定位精度。
74.具体的，对相对高度位置信息进行有效性检验，得到第一检验结果。若该第一检验结果标识该相对高度位置信息是有效的，表明该相对高度位置信息可以用作目标车辆的高度位置观测量。其中，有效性检验可以根据实际场景进行设定，在此不作任何限定。
75.在实际应用中，若卫星定位模块获取的高度位置信息满足可用性条件，且气压计获取的相对高度位置信息不满足可用性条件，则对卫星定位模块获取的浮点解高度位置信息进行有效性检验，得到第二检验结果。若第二检验结果标识浮点解有效，则将浮点解高度位置信息作为目标车辆的高度位置观测量。也就是说，当判断出只有卫星定位模块输出的高度位置信息可用时，需要对其浮点解进行有效性检验，确定有效是方可作为观测量进行观测更新。
76.若卫星定位模块获取的高度位置信息满足可用性条件，且气压计获取的相对高度位置信息满足可用性条件，确定高度位置信息是否包括固定解。若是，将固定解高度位置信息作为目标车辆的高度位置观测量。若否，将卫星定位模块获取的高度位置信息和气压计获取的相对高度位置信息分别与卡尔曼状态递推预测高度作差，得到卫星定位模块对应的第一差值和气压计对应的第二差值。若第一差值小于第二差值，将卫星定位模块获取的高度位置信息作为目标车辆的高度位置观测量；若第一差值不小于第二差值，将卫星定位模块获取的高度位置信息作为目标车辆的高度位置观测量。
77.也就是说，当判断出卫星定位模块和气压计高度信息均可用时，若卫星定位模块为固定解，则将其作为观测量，否则将卫星定位模块和气压计输出的高度位置信息分别与卡尔曼状态递推预测高度做差，并将两者做差，比较高度差大小，选择高度差最小的高度值作为滤波器的观测量。
78.上述对卫星定位模块和气压计输出的测量数据进行有效性检验，并且在解算过程中将高度与平面分开进行解算，提高了定位系统的可靠性的同时，也提高了定位精度。
79.在进行平面位置解算时，满足有效性检条件的卫星定位模块输出的三维位置信息可作为三维位置观测量进行观测更新。在速度解算方面，若判断出车辆在高度方向上以及的速度可用时，进行卡尔曼滤波高度方向速度更新。当判断出车辆静止时，卡尔曼滤波观测更新停止。
80.上述实施例提供的车辆三维定位方法，将传感器输出信息进行滤波处理，提高了车辆三维状态观测量的数据精度，进一步提高了基于该三维状态观测量计算车辆三维状态的精度；利用车速实时修正imu数据计算出车辆三轴重力加速度分量，能准确反映出车辆的加减速情况，避免了直接利用imu数据计算所造成的误差；利用车辆三轴重力加速度分量计算得到俯仰角与横滚角，并进行卡尔曼滤波进行多传感器信息融合最终输出车辆姿态，提高了姿态估计的准确性；将三维定位解算分解为平面解算与高度解算，使高度和平面解算互不干扰，降低了解算过程中存在的误差；使用车辆里程计实时输出的速度信息判断车辆运动状态，静止时停止状态更新，使车辆状态估计更符合实际；通过设计合理的定位系统初始化步骤，使定位结果更准确；利用融合算法进行数据处理，提高了车辆三维定位解算精度，使车辆在不同的行驶场景中均有较好的定位精度。
81.针对上述实施例提供的车辆三维定位方法，本技术实施例还提供了一种车辆三维定位系统。参见图2，图2为本技术实施例提供的一种车辆三维定位系统的结构示意图。如图2所示，该车辆三维定位系统200包括车辆里程计201、惯性测量单元imu 202、气压计203、卫星定位模块204和卡尔曼滤波器205；所述车辆三维定位系统200用于执行上述实施例提供的车辆三维定位方法。
82.上述实施例提供的车辆三维定位系统，通过目标车辆定位系统获取目标车辆的三维状态观测量，若确定三维状态观测量满足稳定性条件，表明定位系统所包括的多个传感器输出的测量数据都趋于稳定状态，因此可以根据三维状态观测量初始化定位系统中的卡尔曼滤波器，为卡尔曼滤波器解算目标车辆三维状态提供了可靠的测量数据。若目标车辆处于运动状态时，表明目标车辆的三维状态发生了改变，因此根据三维状态观测量，通过初始化后的卡尔曼滤波器确定所述目标车辆的三维状态。由于定位系统包括车辆里程计、惯性测量单元、气压计和卫星定位模块，通过卡尔曼滤波器对由这多个传感器获取的测量数据进行融合解算，提高了定位系统的可靠性的同时，也提高了对于车辆进行三维定位的准确性，使得车辆在需要高度位置信息进行导航等场景中也有较好的定位精度，从而提高了用户驾驶体验。
83.本领域普通技术人员可以理解，实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质可以是下述介质中的至少一种：只读存储器(英文：read-only memory，缩写：rom)、ram、磁碟或者光盘等各种可以存储程序代码的介质。
84.需要说明的是，本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于设备及系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所描述的设备及系统实施例仅仅是示意性的，其中作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的
部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
85.以上所述，仅为本技术的一种具体实施方式，但本技术的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本技术揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本技术的保护范围之内。因此，本技术的保护范围应该以权利要求的保护范围为准。