CN112147662B - 车辆定位方法和装置、车辆、存储介质、电子设备 - Google Patents
车辆定位方法和装置、车辆、存储介质、电子设备 Download PDFInfo
- Publication number
- CN112147662B CN112147662B CN201910578628.2A CN201910578628A CN112147662B CN 112147662 B CN112147662 B CN 112147662B CN 201910578628 A CN201910578628 A CN 201910578628A CN 112147662 B CN112147662 B CN 112147662B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- filter
- covariance
- local filter
- moment
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 230000004927 fusion Effects 0.000 claims abstract description 35
- 238000013507 mapping Methods 0.000 claims abstract description 22
- 238000005516 engineering process Methods 0.000 claims abstract description 16
- 230000001133 acceleration Effects 0.000 claims description 41
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000004422 calculation algorithm Methods 0.000 claims description 14
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000007704 transition Effects 0.000 claims description 8
- 238000001914 filtration Methods 0.000 claims description 7
- 238000004590 computer program Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 5
- 230000006870 function Effects 0.000 claims description 4
- 239000000758 substrate Substances 0.000 claims 2
- 238000004891 communication Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 7
- 238000012545 processing Methods 0.000 description 4
- 230000005236 sound signal Effects 0.000 description 4
- 238000007499 fusion processing Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- KLDZYURQCUYZBL-UHFFFAOYSA-N 2-[3-[(2-hydroxyphenyl)methylideneamino]propyliminomethyl]phenol Chemical compound OC1=CC=CC=C1C=NCCCN=CC1=CC=CC=C1O KLDZYURQCUYZBL-UHFFFAOYSA-N 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 201000001098 delayed sleep phase syndrome Diseases 0.000 description 1
- 208000033921 delayed sleep phase type circadian rhythm sleep disease Diseases 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000002372 labelling Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
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
Abstract
本公开涉及一种车辆定位方法和装置、车辆、存储介质、电子设备。所述方法包括：获取由全球定位系统GPS确定的所述车辆的第一位置信息；获取由超宽带UWB技术确定的所述车辆的第二位置信息；将所述第一位置信息、所述第二位置信息通过联邦卡尔曼滤波器进行数据融合，以确定所述车辆的融合位置信息；将所述融合位置信息所指示的位置映射在电子地图中的预定轨道上。通过上述技术方案，采用联邦卡尔曼滤波器，对GPS和UWB两种不同方式获得的车辆位置数据进行融合，能够用较少的传感器实现精确定位，可靠性高，容错性较好，并且，将车辆的融合位置信息指示的位置映射到预定的轨道中，使轨道车辆的定位更加准确。
Description
技术领域
本公开涉及车辆定位领域，具体地，涉及一种车辆定位方法和装置、车辆、存储介质、电子设备。
背景技术
现有的车辆定位方法通常包括全球定位系统(Global Positioning System，GPS)、超宽带(UltraWideband，UWB)技术等。目前在组合导航中，传感器的数量和种类越来越多，如惯导、GPS，里程计等。组合导航系统需要把传感器传来的数据进行综合处理，在信息处理过程中集中式融合方式较容易实现，通过把各个传感器的信息送到中心站进行集中处理，最后获得当前时刻的最优解。
然而，集中式融合处理方式获得的位置精度仍然无法满足要求较高的定位场景需求，系统的容错性较差。
发明内容
本公开的目的是提供一种高效、准确的车辆定位方法和装置、车辆、存储介质、电子设备。
为了实现上述目的，本公开提供一种车辆定位方法。所述方法包括：
获取由全球定位系统GPS确定的所述车辆的第一位置信息；
获取由超宽带UWB技术确定的所述车辆的第二位置信息；
将所述第一位置信息、所述第二位置信息通过联邦卡尔曼滤波器进行数据融合，以确定所述车辆的融合位置信息；
将所述融合位置信息所指示的位置映射在电子地图中的预定轨道上。
可选地，所述联邦卡尔曼滤波器包括：
第一局部滤波器，用于对所述第一位置信息进行滤波，生成第一状态变量的第一估计值和第一协方差；
第二局部滤波器，用于对所述第二位置信息进行滤波，生成第二状态变量的第二估计值和第二协方差；
主滤波器，用于根据当前时刻的第一估计值、当前时刻的第一协方差、当前时刻的第二估计值和当前时刻的第二协方差生成当前时刻的全局估计值和当前时刻的全局协方差，并将当前时刻的全局估计值和当前时刻的全局协方差反馈至所述第一局部滤波器和所述第二局部滤波器，以使所述第一局部滤波器根据当前时刻的全局估计值和当前时刻的全局协方差生成下一时刻的第一估计值和下一时刻的第一协方差，所述第二局部滤波器根据当前时刻的全局估计值和当前时刻的全局协方差生成下一时刻的第二估计值和下一时刻的第二协方差。
可选地，在所述第一局部滤波器中，根据由GPS确定的当前时刻所述车辆的速度、位置、加速度、加速度误差，确定下一时刻所述车辆的速度和位置；
在所述第二局部滤波器中，根据由UWB技术确定的当前时刻所述车辆的速度、位置、加速度、加速度误差，确定下一时刻所述车辆的速度和位置。
可选地，所述第一局部滤波器的状态方程为：
所述第二局部滤波器的状态方程为：
所述第一局部滤波器的观测方程为：
所述第二局部滤波器的观测方程为：
其中，k为时刻，T1为采样周期，第一状态变量为分别为由GPS确定的所述车辆的水平位置、水平速度、纵向位置和纵向速度，第二状态变量为分别为由UWB技术确定的所述车辆的水平位置、水平速度、纵向位置和纵向速度，u1(k)为所述第一局部滤波器中所述车辆在k时刻的加速度，u2(k)为所述第二局部滤波器中所述车辆在k时刻的加速度，w1(k)为所述第一局部滤波器中所述车辆在k时刻的加速度误差，w2(k)为所述第二局部滤波器中所述车辆在k时刻的加速度误差，Z1(k)为所述第一局部滤波器中所述车辆在k时刻的位置，Z2(k)为所述第二局部滤波器中所述车辆在k时刻的位置，V1(k)为所述第一局部滤波器中所述车辆在k时刻的位置误差，V2(k)为所述第二局部滤波器中所述车辆在k时刻的位置误差。
可选地，所述第一局部滤波器的模型为：
X1(k+1)＝Φ1(k)X1(k)+U1(k)+W1(k)
Z1(k)＝H1(k)X1(k)+V1(k)
其中，所述第一局部滤波器的算法为：
其中，
所述第二局部滤波器的模型为：
X2(k+1)＝Φ2(k)X2(k)+U2(k)+W2(k)
Z2(k)＝H2(k)X2(k)+V2(k)
其中，所述第二局部滤波器的算法为：
其中，
其中，X1(k)为所述第一局部滤波器在k时刻的第一状态变量，为k+1时刻的第一估计值，P1(k+1)为所述第一局部滤波器在k+1时刻的第一协方差，Φ1(k)为所述第一局部滤波器在k时刻的状态转移矩阵，H1(k+1)为所述第一局部滤波器在k+1时刻的观测矩阵，X2(k)为所述第二局部滤波器在k时刻的第二状态变量，/>为k+1时刻的第二估计值，P2(k+1)为所述第二局部滤波器在k+1时刻的第二协方差，Φ2(k)为所述第二局部滤波器在k时刻的状态转移矩阵，H2(k+1)为所述第二局部滤波器在k+1时刻的观测矩阵。
可选地，所述主滤波器的算法为：
其中，为k时刻的全局估计值，Pg(k)为k时刻的全局协方差，Q1(k)为W1(k)的协方差，Q2(k)为W2(k)的协方差，Qg(k)为融合后的加速度误差的协方差，β1、β2为系数，且β1+β2＝1。
可选地，所述主滤波器在判定GPS故障或不工作时，设置β1＝0，在判定UWB故障或不工作时，设置β2＝0。
可选地，所述方法还包括：
将所述融合位置信息所指示的位置映射在电子地图中的预定轨道上；
计算在所述电子地图中所述车辆从启动到所映射的位置的第一行程；
根据所述车辆里程仪的记录结果确定所述车辆从启动到所述融合位置信息所指示的位置的第二行程；
若所述第一行程和所述第二行程之差在预定的范围内，将所映射的位置进行输出。
可选地，将所述融合位置信息所指示的位置映射在电子地图中的预定轨道上，包括：
通过牛顿迭代法计算出所述预定轨道上，与所述融合位置信息所指示的位置距离最小的点，作为映射在所述预定轨道上的点。
可选地，所述预定轨道上的点到所述融合位置信息所指示的位置的距离为：
所述预定轨道的函数方程为：
y(x)＝ax3+bx2+cx+d
牛顿迭代法的计算公式为：
xn+1＝xn-f′(xn)/f″(xn)
其中，x、y分别为所述预定轨道上的点的横坐标和纵坐标，x0、y0分别为所述融合位置信息所指示的位置的横坐标和纵坐标，映射点的坐标为(xn+1,y(xn+1))。
可选地，所述装置包括：
第一获取模块，用于获取由全球定位系统GPS确定的所述车辆的第一位置信息；
第二获取模块，用于获取由超宽带UWB技术确定的所述车辆的第二位置信息；
融合模块，用于将所述第一位置信息、所述第二位置信息通过联邦卡尔曼滤波器进行数据融合，以确定所述车辆的融合位置信息；
映射模块，用于将所述融合位置信息所指示的位置映射在电子地图中的预定轨道上。
本公开还提供一种车辆，包括本公开提供的上述车辆定位装置。
本公开还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现本公开提供的上述方法的步骤。
本公开还提供一种电子设备，包括：存储器，其上存储有计算机程序；处理器，用于执行所述存储器中的所述计算机程序，以实现本公开提供的上述方法的步骤。
通过上述技术方案，采用联邦卡尔曼滤波器，对GPS和UWB两种不同方式获得的车辆位置数据进行融合，能够用较少的传感器实现精确定位，可靠性高，容错性较好，并且，将车辆的融合位置信息指示的位置映射到预定的轨道中，使轨道车辆的定位更加准确。
本公开的其他特征和优点将在随后的具体实施方式部分予以详细说明。
附图说明
附图是用来提供对本公开的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本公开，但并不构成对本公开的限制。在附图中：
图1是一示例性实施例提供的车辆定位方法的流程图；
图2是一示例性实施例提供的采用联邦卡尔曼滤波器运算的示意图；
图3是另一示例性实施例提供的车辆定位方法的示意图；
图4是一示例性实施例提供的车辆定位装置的框图；
图5是根据一示例性实施例示出的一种电子设备的框图。
具体实施方式
以下结合附图对本公开的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本公开，并不用于限制本公开。
本公开提供一种车辆定位方法。图1是一示例性实施例提供的车辆定位方法的流程图。如图1所示，该方法可以包括以下步骤。
步骤S11，获取由全球定位系统GPS确定的车辆的第一位置信息。
步骤S12，获取由超宽带UWB技术确定的车辆的第二位置信息。
步骤S13，将第一位置信息、第二位置信息通过联邦卡尔曼滤波器进行数据融合，以确定车辆的融合位置信息。
步骤S14，将融合位置信息所指示的位置映射在电子地图中的预定轨道上。
也就是，第一位置信息可以是GPS接收机发送的位置信息。第二位置信息可以是UWB接收机发送的位置信息。
当本公开的方法应用于有轨车辆时，还可以结合轨道对有轨车辆进行精确定位。预定轨道可以是车辆所在的轨道(例如，云轨轨道)。由于融合位置信息所指示的位置在电子地图中输出时，由于存在误差，有可能并不在其轨道上。因此，需要将际位置信息所指示的位置映射在电子地图中的预定轨道上。映射到轨道上的位置，可以认为是更加准确的位置。
通过上述技术方案，采用联邦卡尔曼滤波器，对GPS和UWB两种不同方式获得的车辆位置数据进行融合，能够用较少的传感器实现精确定位，可靠性高，容错性较好，并且，将车辆的融合位置信息指示的位置映射到预定的轨道中，使轨道车辆的定位更加准确。
本领域技术人员可以理解的是，步骤S11和步骤S12可以以任意的顺序实施，图1中仅是一示例性的实施例。
图2是一示例性实施例提供的采用联邦卡尔曼滤波器运算的示意图。如图2所示，联邦卡尔曼滤波器可以包括第一局部滤波器、第二局部滤波器和主滤波器。
第一局部滤波器用于对第一位置信息进行滤波，生成第一状态变量的第一估计值和第一协方差。
第二局部滤波器用于对第二位置信息进行滤波，生成第二状态变量的第二估计值和第二协方差。
主滤波器，用于根据当前时刻的第一估计值、当前时刻的第一协方差、当前时刻的第二估计值和当前时刻的第二协方差生成当前时刻的全局估计值和当前时刻的全局协方差，并将当前时刻的全局估计值和当前时刻的全局协方差反馈至第一局部滤波器和第二局部滤波器，以使第一局部滤波器根据当前时刻的全局估计值和当前时刻的全局协方差生成下一时刻的第一估计值和下一时刻的第一协方差，第二局部滤波器根据当前时刻的全局估计值和当前时刻的全局协方差生成下一时刻的第二估计值和下一时刻的第二协方差。
也就是，主滤波器接收第一局部滤波器发送的当前时刻的第一估计值、当前时刻的第一协方差，并接收第二局部滤波器发送的当前时刻的第二估计值、当前时刻的第二协方差，进行数据融合后，输出当前时刻的全局估计值的最优值及其协方差。同时，将最优值及其协方差返回第一局部滤波器和第二局部滤波器，以使第一局部滤波器和第二局部滤波器利用该最优值及其协方差，生成下一时刻的估计值和协方差。如此进行更新。
这样，下一时刻的状态最优值是考虑了上一时刻的状态最优值得出的，因此，采用分布式联邦卡尔曼来进行数据的融合处理，定位精确，可靠性高。
其中，在第一局部滤波器中，根据由GPS确定的当前时刻车辆的速度、位置、加速度、加速度误差，确定下一时刻车辆的速度和位置；在第二局部滤波器中，根据由UWB技术确定的当前时刻车辆的速度、位置、加速度、加速度误差，确定下一时刻车辆的速度和位置。这样，在第一和第二局部滤波器中，考虑实时的加速度值，可以利用迭代计算的方法来建立状态方程，计算各自的状态变量的估计值，结果较准确。
具体地，第一局部滤波器的状态方程可以为：
第二局部滤波器的状态方程可以为：
第一局部滤波器的观测方程可以为：
第二局部滤波器的观测方程可以为：
其中，k为时刻，T1为采样周期，第一状态变量为分别为由GPS确定的车辆的水平位置、水平速度、纵向位置和纵向速度，第二状态变量为/>分别为由UWB技术确定的车辆的水平位置、水平速度、纵向位置和纵向速度(以电子地图中的直角坐标系为参考)，u1(k)为第一局部滤波器中车辆在k时刻的加速度，u2(k)为第二局部滤波器中车辆在k时刻的加速度(加速度是车辆获取到的)，w1(k)为第一局部滤波器中车辆在k时刻的加速度误差，w2(k)为第二局部滤波器中车辆在k时刻的加速度误差，Z1(k)为第一局部滤波器中车辆在k时刻的位置，Z2(k)为第二局部滤波器中车辆在k时刻的位置，V1(k)为第一局部滤波器中车辆在k时刻的位置误差，V2(k)为第二局部滤波器中车辆在k时刻的位置误差。
第一局部滤波器的模型可以为：
X1(k+1)＝Φ1(k)X1(k)+U1(k)+W1(k)
Z1(k)＝H1(k)X1(k)+V1(k)
其中，
第一局部滤波器的算法可以为：
其中，
第二局部滤波器的模型可以为：
X2(k+1)＝Φ2(k)X2(k)+U2(k)+W2(k)
Z2(k)＝H2(k)X2(k)+V2(k)
其中，
第二局部滤波器的算法可以为：
其中，
其中，X1(k)为第一局部滤波器在k时刻的第一状态变量，即， 为k+1时刻的第一估计值，P1(k+1)为第一局部滤波器在k+1时刻的第一协方差，Φ1(k)为第一局部滤波器在k时刻的状态转移矩阵，H1(k+1)为第一局部滤波器在k+1时刻的观测矩阵，X2(k)为第二局部滤波器在k时刻的第二状态变量，即，/> 为k+1时刻的第二估计值，P2(k+1)为第二局部滤波器在k+1时刻的第二协方差，Φ2(k)为第二局部滤波器在k时刻的状态转移矩阵，H2(k+1)为第二局部滤波器在k+1时刻的观测矩阵。·T为转置，·-1为逆。
主滤波器的算法可以为：
其中，为k时刻的全局估计值，Pg(k)为k时刻的全局协方差，Q1(k)为W1(k)的协方差，Q2(k)为W2(k)的协方差，Qg(k)为融合后的加速度误差的协方差，β1、β2为系数，且β1+β2＝1。
β1、β2体现了第一位置信息和第二位置信息在生成状态最优值时的权重。通常可以设置β1＝β2＝0.5。主滤波器在判定GPS故障或不工作时，可以设置β1＝0；在判定UWB故障或不工作时，可以设置β2＝0。
举例来说，若未获取到第一位置信息，则可以认为GPS不工作，若未获取到第二位置信息，则可以认为UWB不工作。若获取到的第一位置信息所指示的位置不连续，则可以认为GPS故障，若获取到的第二位置信息所指示的位置不连续，则可以认为UWB故障。
故障或不工作的定位系统，通过将其系数设置为零，使得在下一时刻确定状态最优值时，对该故障或不工作的定位系统对应的位置信息不与考虑，这样，当一个传感器失效时，也能够输出较准确的位置信息。
图1中的所映射的位置可以直接在电子地图中进行输出。在又一实施例中，在图1的基础上，所述方法还可以包括以下步骤：
计算在电子地图中车辆从启动到所映射的位置的第一行程；
根据车辆里程仪的记录结果确定车辆从启动到融合位置信息所指示的位置的第二行程；
若第一行程和第二行程之差在预定的范围内，将所映射的位置进行输出。
在电子地图中，该预定轨道从起点到车辆到所映射的位置之间的路程(第一行程)可以由计算机计算得出。
车辆里程仪的记录结果为车辆从启动到当前(车辆到达融合位置信息所指示的位置)走过的路程(第二行程)，可以直接从里程仪获取得到。
第一行程和第二行程在理论上应该是相同的。如果第一行程和第二行程之差超出了预定的范围，则可以说明联邦卡尔曼滤波器输出的融合位置信息所指示的所映射位置的误差较大，此时不宜将其进行输出。反之，可以认为所映射位置的误差较小，可以输出。可以通过在电子地图或预定轨道中标注位置的方式进行输出。
图3是另一示例性实施例提供的车辆定位方法的示意图。如图3所示，第一局部滤波器向主滤波器发送第一估计值和第一协方差P1，第二局部滤波器向主滤波器发送第二估计值/>和第二协方差P2。主滤波器生成全局估计值/>和全局协方差Pg并输出，同时将全局估计值/>和带有系数的协方差β-1Pg返回第一局部滤波器，将全局估计值/>和带有系数的协方差β2 -1Pg返回第二局部滤波器。
并且，联邦卡尔曼滤波器的输出映射到预定轨道后，所确定的行程(第一行程)与里程计确定的行程(第二行程)进行比较后输出。
该实施例中，考虑了轨道位置，对联邦卡尔曼滤波器输出的位置进行了一定程度上的校正，并且通过里程计的记录结果对定位结果进行了进一步的校验，使得定位结果更加准确。
在一实施例中，将融合位置信息所指示的位置映射在电子地图中的预定轨道上，可以包括：通过牛顿迭代法计算出预定轨道上，与融合位置信息所指示的位置距离最小的点，作为映射在预定轨道上的点。
具体地，预定轨道上的点到融合位置信息所指示的位置的距离为：
预定轨道的函数方程为：
y(x)＝ax3+bx2+cx+d
牛顿迭代法的计算公式为：
xn+1＝xn-f′(xn)/f″(xn)
其中，x、y分别为预定轨道上的点的横坐标和纵坐标，x0、y0分别为融合位置信息所指示的位置的横坐标和纵坐标，映射点的坐标为(xn+1,y(xn+1))。
在迭代过程中，可以在xn+1-xn的值小于预定的阈值时，终止迭代。这样，通过计算，能够将融合位置信息所指示的位置映射在电子地图中的预定轨道上，准确性较高，数据处理速度快。
本公开还提供一种车辆定位装置。图4是一示例性实施例提供的车辆定位装置的框图。如图4所示，车辆定位装置10可以包括第一获取模块11、第二获取模块12、融合模块13和映射模块14。
第一获取模块11用于获取由全球定位系统GPS确定的车辆的第一位置信息。
第二获取模块12用于获取由超宽带UWB技术确定的车辆的第二位置信息。
融合模块13用于将第一位置信息、第二位置信息通过联邦卡尔曼滤波器进行数据融合，以确定车辆的融合位置信息。
映射模块14用于将融合位置信息所指示的位置映射在电子地图中的预定轨道上。
可选地，所述联邦卡尔曼滤波器包括第一局部滤波器、第二局部滤波器和主滤波器。
第一局部滤波器用于对所述第一位置信息进行滤波，生成第一状态变量的第一估计值和第一协方差。
第二局部滤波器用于对所述第二位置信息进行滤波，生成第二状态变量的第二估计值和第二协方差。
主滤波器用于根据当前时刻的第一估计值、当前时刻的第一协方差、当前时刻的第二估计值和当前时刻的第二协方差生成当前时刻的全局估计值和当前时刻的全局协方差，并将当前时刻的全局估计值和当前时刻的全局协方差反馈至所述第一局部滤波器和所述第二局部滤波器，以使所述第一局部滤波器根据当前时刻的全局估计值和当前时刻的全局协方差生成下一时刻的第一估计值和下一时刻的第一协方差，所述第二局部滤波器根据当前时刻的全局估计值和当前时刻的全局协方差生成下一时刻的第二估计值和下一时刻的第二协方差。
可选地，在第一局部滤波器中，根据由GPS确定的当前时刻车辆的速度、位置、加速度、加速度误差，确定下一时刻车辆的速度和位置；
在第二局部滤波器中，根据由UWB技术确定的当前时刻车辆的速度、位置、加速度、加速度误差，确定下一时刻车辆的速度和位置。
可选地，所述第一局部滤波器的状态方程为：
所述第二局部滤波器的状态方程为：
所述第一局部滤波器的观测方程为：
所述第二局部滤波器的观测方程为：
其中，k为时刻，T1为采样周期，第一状态变量为分别为由GPS确定的所述车辆的水平位置、水平速度、纵向位置和纵向速度，第二状态变量为分别为由UWB技术确定的所述车辆的水平位置、水平速度、纵向位置和纵向速度，u1(k)为所述第一局部滤波器中所述车辆在k时刻的加速度，u2(k)为所述第二局部滤波器中所述车辆在k时刻的加速度，w1(k)为所述第一局部滤波器中所述车辆在k时刻的加速度误差，w2(k)为所述第二局部滤波器中所述车辆在k时刻的加速度误差，Z1(k)为所述第一局部滤波器中所述车辆在k时刻的位置，Z2(k)为所述第二局部滤波器中所述车辆在k时刻的位置，V1(k)为所述第一局部滤波器中所述车辆在k时刻的位置误差，V2(k)为所述第二局部滤波器中所述车辆在k时刻的位置误差。
可选地，所述第一局部滤波器的模型为：
X1(k+1)＝Φ1(k)X1(k)+U1(k)+W1(k)
Z1(k)＝H1(k)X1(k)+V1(k)
其中，
所述第一局部滤波器的算法为：
其中，
所述第二局部滤波器的模型为：
X2(k+1)＝Φ2(k)X2(k)+U2(k)+W2(k)
Z2(k)＝H2(k)X2(k)+V2(k)
其中，
所述第二局部滤波器的算法为：
其中，
其中，X1(k)为所述第一局部滤波器在k时刻的第一状态变量，为k+1时刻的第一估计值，P1(k+1)为所述第一局部滤波器在k+1时刻的第一协方差，Φ1(k)为所述第一局部滤波器在k时刻的状态转移矩阵，H1(k+1)为所述第一局部滤波器在k+1时刻的观测矩阵，X2(k)为所述第二局部滤波器在k时刻的第二状态变量，/>为k+1时刻的第二估计值，P2(k+1)为所述第二局部滤波器在k+1时刻的第二协方差，Φ2(k)为所述第二局部滤波器在k时刻的状态转移矩阵，H2(k+1)为所述第二局部滤波器在k+1时刻的观测矩阵。
可选地，所述主滤波器的算法为：
其中，为k时刻的全局估计值，Pg(k)为k时刻的全局协方差，Q1(k)为W1(k)的协方差，Q2(k)为W2(k)的协方差，Qg(k)为融合后的加速度误差的协方差，β1、β2为系数，且β1+β2＝1。
可选地，所述主滤波器在判定GPS故障或不工作时，设置β1＝0，在判定UWB故障或不工作时，设置β2＝0。
可选地，所述装置10还可以包括计算模块、确定模块和输出模块。
计算模块用于计算在电子地图中所述车辆从启动到所映射的位置的第一行程。
确定模块用于根据所述车辆里程仪的记录结果确定所述车辆从启动到所述融合位置信息所指示的位置的第二行程。
输出模块用于若所述第一行程和所述第二行程之差在预定的范围内，将所映射的位置进行输出。
可选地，映射模块包括映射子模块。
映射子模块用于通过牛顿迭代法计算出所述预定轨道上，与所述融合位置信息所指示的位置距离最小的点，作为映射在所述预定轨道上的点。
可选地，在映射子模块中，所述预定轨道上的点到所述融合位置信息所指示的位置的距离为：
所述预定轨道的函数方程为：
y(x)＝ax3+bx2+cx+d
牛顿迭代法的计算公式为：
xn+1＝xn-f′(xn)/f″(xn)
其中，x、y分别为所述预定轨道上的点的横坐标和纵坐标，x0、y0分别为所述融合位置信息所指示的位置的横坐标和纵坐标，映射点的坐标为(xn+1,y(xn+1))。
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
通过上述技术方案，采用联邦卡尔曼滤波器，对GPS和UWB两种不同方式获得的车辆位置数据进行融合，能够用较少的传感器实现精确定位，可靠性高，容错性较好，并且，将车辆的融合位置信息指示的位置映射到预定的轨道中，使轨道车辆的定位更加准确。
本公开还提供一种车辆，包括本公开提供的上述车辆定位装置10。
图5是根据一示例性实施例示出的一种电子设备的框图。如图5所示，该电子设备500可以包括处理器501和存储器502。该电子设备500还可以包括多媒体组件503，输入/输出(I/O)接口504，以及通信组件505中的一者或多者。
其中，处理器501用于控制该电子设备500的整体操作，以完成上述的车辆定位方法中的全部或部分步骤。存储器502用于存储各种类型的数据以支持在该电子设备500的操作，这些数据例如可以包括用于在该电子设备500上操作的任何应用程序或方法的指令，以及应用程序相关的数据，例如联系人数据、收发的消息、图片、音频、视频等等。该存储器502可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，例如静态随机存取存储器(Static Random Access Memory，简称SRAM)，电可擦除可编程只读存储器(Electrically Erasable Programmable Read-Only Memory，简称EEPROM)，可擦除可编程只读存储器(Erasable Programmable Read-Only Memory，简称EPROM)，可编程只读存储器(Programmable Read-Only Memory，简称PROM)，只读存储器(Read-Only Memory，简称ROM)，磁存储器，快闪存储器，磁盘或光盘。多媒体组件503可以包括屏幕和音频组件。其中屏幕例如可以是触摸屏，音频组件用于输出和/或输入音频信号。例如，音频组件可以包括一个麦克风，麦克风用于接收外部音频信号。所接收的音频信号可以被进一步存储在存储器502或通过通信组件505发送。音频组件还包括至少一个扬声器，用于输出音频信号。I/O接口504为处理器501和其他接口模块之间提供接口，上述其他接口模块可以是键盘，鼠标，按钮等。这些按钮可以是虚拟按钮或者实体按钮。通信组件505用于该电子设备500与其他设备之间进行有线或无线通信。无线通信，例如Wi-Fi，蓝牙，近场通信(Near FieldCommunication，简称NFC)，2G、3G、4G、NB-IOT、eMTC、或其他5G等等，或它们中的一种或几种的组合，在此不做限定。因此相应的该通信组件505可以包括：Wi-Fi模块，蓝牙模块，NFC模块等等。
在一示例性实施例中，电子设备500可以被一个或多个应用专用集成电路(Application Specific Integrated Circuit，简称ASIC)、数字信号处理器(DigitalSignal Processor，简称DSP)、数字信号处理设备(Digital Signal Processing Device，简称DSPD)、可编程逻辑器件(Programmable Logic Device，简称PLD)、现场可编程门阵列(Field Programmable Gate Array，简称FPGA)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述的车辆定位方法。
在另一示例性实施例中，还提供了一种包括程序指令的计算机可读存储介质，该程序指令被处理器执行时实现上述的车辆定位方法的步骤。例如，该计算机可读存储介质可以为上述包括程序指令的存储器502，上述程序指令可由电子设备500的处理器501执行以完成上述的车辆定位方法。
以上结合附图详细描述了本公开的优选实施方式，但是，本公开并不限于上述实施方式中的具体细节，在本公开的技术构思范围内，可以对本公开的技术方案进行多种简单变型，这些简单变型均属于本公开的保护范围。
另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本公开对各种可能的组合方式不再另行说明。
此外，本公开的各种不同的实施方式之间也可以进行任意组合，只要其不违背本公开的思想，其同样应当视为本公开所公开的内容。
Claims (12)
1.一种车辆定位方法，其特征在于，所述方法包括：
获取由全球定位系统GPS确定的所述车辆的第一位置信息；
获取由超宽带UWB技术确定的所述车辆的第二位置信息；
将所述第一位置信息、所述第二位置信息通过联邦卡尔曼滤波器进行数据融合，以确定所述车辆的融合位置信息；
将所述融合位置信息所指示的位置映射在电子地图中的预定轨道上；
所述联邦卡尔曼滤波器包括：
第一局部滤波器，用于对所述第一位置信息进行滤波，生成第一状态变量的第一估计值和第一协方差；
第二局部滤波器，用于对所述第二位置信息进行滤波，生成第二状态变量的第二估计值和第二协方差；
主滤波器，用于根据当前时刻的第一估计值、当前时刻的第一协方差、当前时刻的第二估计值和当前时刻的第二协方差生成当前时刻的全局估计值和当前时刻的全局协方差，并将当前时刻的全局估计值和当前时刻的全局协方差反馈至所述第一局部滤波器和所述第二局部滤波器，以使所述第一局部滤波器根据当前时刻的全局估计值和当前时刻的全局协方差生成下一时刻的第一估计值和下一时刻的第一协方差，所述第二局部滤波器根据当前时刻的全局估计值和当前时刻的全局协方差生成下一时刻的第二估计值和下一时刻的第二协方差；
所述第一局部滤波器的状态方程为：
所述第二局部滤波器的状态方程为：
所述第一局部滤波器的观测方程为：
所述第二局部滤波器的观测方程为：
其中，为时刻，/>为采样周期，第一状态变量为/>，/>分别为由GPS确定的所述车辆的水平位置、水平速度、纵向位置和纵向速度，第二状态变量为/>，/>分别为由UWB技术确定的所述车辆的水平位置、水平速度、纵向位置和纵向速度，/>为所述第一局部滤波器中所述车辆在/>时刻的加速度，/>为所述第二局部滤波器中所述车辆在/>时刻的加速度，/>为所述第一局部滤波器中所述车辆在/>时刻的加速度误差，/>为所述第二局部滤波器中所述车辆在/>时刻的加速度误差，/>为所述第一局部滤波器中所述车辆在/>时刻的位置，/>为所述第二局部滤波器中所述车辆在/>时刻的位置，/>为所述第一局部滤波器中所述车辆在/>时刻的位置误差，/>为所述第二局部滤波器中所述车辆在/>时刻的位置误差。
2.根据权利要求1所述的方法，其特征在于，
在所述第一局部滤波器中，根据由GPS确定的当前时刻所述车辆的速度、位置、加速度、加速度误差，确定下一时刻所述车辆的速度和位置；
在所述第二局部滤波器中，根据由UWB技术确定的当前时刻所述车辆的速度、位置、加速度、加速度误差，确定下一时刻所述车辆的速度和位置。
3.根据权利要求1所述的方法，其特征在于，
所述第一局部滤波器的模型为：
其中，，/>
所述第一局部滤波器的算法为：
其中，，/>
所述第二局部滤波器的模型为：
其中，，/>
所述第二局部滤波器的算法为：
其中，，/>
其中，为所述第一局部滤波器在/>时刻的第一状态变量，/>为/>时刻的第一估计值，/>为所述第一局部滤波器在/>时刻的第一协方差，/>为所述第一局部滤波器在/>时刻的状态转移矩阵，/>为所述第一局部滤波器在/>时刻的观测矩阵，/>为所述第二局部滤波器在/>时刻的第二状态变量，/>为/>时刻的第二估计值，/>为所述第二局部滤波器在/>时刻的第二协方差，/>为所述第二局部滤波器在/>时刻的状态转移矩阵，/>为所述第二局部滤波器在/>时刻的观测矩阵，为/>的协方差，/>为/>的协方差，/>为/>时刻GPS的观测噪声方差阵，/>为/>时刻UWB的观测噪声方差阵。
4.根据权利要求3所述的方法，其特征在于，所述主滤波器的算法为：
其中，为/>时刻的全局估计值，/>为/>时刻的全局协方差，/>为/>的协方差，/>为/>的协方差，/>为融合后的加速度误差的协方差，/>为系数，且。
5.根据权利要求4所述的方法，其特征在于，所述主滤波器在判定GPS故障或不工作时，设置，在判定UWB故障或不工作时，设置/>。
6.根据权利要求1所述的方法，其特征在于，所述方法还包括：
计算在所述电子地图中所述车辆从启动到所映射的位置的第一行程；
根据所述车辆里程仪的记录结果确定所述车辆从启动到所述融合位置信息所指示的位置的第二行程；
若所述第一行程和所述第二行程之差在预定的范围内，将所映射的位置进行输出。
7.根据权利要求1所述的方法，其特征在于，将所述融合位置信息所指示的位置映射在电子地图中的预定轨道上，包括：
通过牛顿迭代法计算出所述预定轨道上，与所述融合位置信息所指示的位置距离最小的点，作为映射在所述预定轨道上的点。
8.根据权利要求7所述的方法，其特征在于，所述预定轨道上的点到所述融合位置信息所指示的位置的距离为：
所述预定轨道的函数方程为：
牛顿迭代法的计算公式为：
其中，分别为所述预定轨道上的点的横坐标和纵坐标，/>分别为所述融合位置信息所指示的位置的横坐标和纵坐标，映射点的坐标为/>，/>为系数。
9.一种车辆定位装置，其特征在于，所述装置包括：
第一获取模块，用于获取由全球定位系统GPS确定的所述车辆的第一位置信息；
第二获取模块，用于获取由超宽带UWB技术确定的所述车辆的第二位置信息；
融合模块，用于将所述第一位置信息、所述第二位置信息通过联邦卡尔曼滤波器进行数据融合，以确定所述车辆的融合位置信息；
映射模块，用于将所述融合位置信息所指示的位置映射在电子地图中的预定轨道上；
所述联邦卡尔曼滤波器包括：
第一局部滤波器，用于对所述第一位置信息进行滤波，生成第一状态变量的第一估计值和第一协方差；
第二局部滤波器，用于对所述第二位置信息进行滤波，生成第二状态变量的第二估计值和第二协方差；
主滤波器，用于根据当前时刻的第一估计值、当前时刻的第一协方差、当前时刻的第二估计值和当前时刻的第二协方差生成当前时刻的全局估计值和当前时刻的全局协方差，并将当前时刻的全局估计值和当前时刻的全局协方差反馈至所述第一局部滤波器和所述第二局部滤波器，以使所述第一局部滤波器根据当前时刻的全局估计值和当前时刻的全局协方差生成下一时刻的第一估计值和下一时刻的第一协方差，所述第二局部滤波器根据当前时刻的全局估计值和当前时刻的全局协方差生成下一时刻的第二估计值和下一时刻的第二协方差；
所述第一局部滤波器的状态方程为：
所述第二局部滤波器的状态方程为：
所述第一局部滤波器的观测方程为：
所述第二局部滤波器的观测方程为：
其中，为时刻，/>为采样周期，第一状态变量为/>，/>分别为由GPS确定的所述车辆的水平位置、水平速度、纵向位置和纵向速度，第二状态变量为/>，分别为由UWB技术确定的所述车辆的水平位置、水平速度、纵向位置和纵向速度，/>为所述第一局部滤波器中所述车辆在/>时刻的加速度，/>为所述第二局部滤波器中所述车辆在/>时刻的加速度，/>为所述第一局部滤波器中所述车辆在/>时刻的加速度误差，/>为所述第二局部滤波器中所述车辆在/>时刻的加速度误差，/>为所述第一局部滤波器中所述车辆在/>时刻的位置，/>为所述第二局部滤波器中所述车辆在/>时刻的位置，/>为所述第一局部滤波器中所述车辆在/>时刻的位置误差，/>为所述第二局部滤波器中所述车辆在/>时刻的位置误差。
10.一种车辆，其特征在于，包括权利要求9所述的车辆定位装置。
11.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1-8中任一项所述方法的步骤。
12.一种电子设备，其特征在于，包括：
存储器，其上存储有计算机程序；
处理器，用于执行所述存储器中的所述计算机程序，以实现权利要求1-8中任一项所述方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910578628.2A CN112147662B (zh) | 2019-06-28 | 2019-06-28 | 车辆定位方法和装置、车辆、存储介质、电子设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910578628.2A CN112147662B (zh) | 2019-06-28 | 2019-06-28 | 车辆定位方法和装置、车辆、存储介质、电子设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112147662A CN112147662A (zh) | 2020-12-29 |
| CN112147662B true CN112147662B (zh) | 2023-08-08 |
Family
ID=73891157
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910578628.2A Active CN112147662B (zh) | 2019-06-28 | 2019-06-28 | 车辆定位方法和装置、车辆、存储介质、电子设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112147662B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112966059B (zh) * | 2021-03-02 | 2023-11-24 | 北京百度网讯科技有限公司 | 针对定位数据的数据处理方法、装置、电子设备和介质 |
| CN113959457B (zh) * | 2021-10-20 | 2024-03-26 | 中国第一汽车股份有限公司 | 一种自动驾驶车辆的定位方法、装置、车辆和介质 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6240367B1 (en) * | 1998-11-27 | 2001-05-29 | Ching-Fang Lin | Full fusion positioning method for vehicle |
| CN107656301A (zh) * | 2017-09-20 | 2018-02-02 | 北京航天发射技术研究所 | 一种基于多源信息融合的车载定位方法 |
| CN108873038A (zh) * | 2018-09-10 | 2018-11-23 | 芜湖盟博科技有限公司 | 自主泊车定位方法及定位系统 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8193978B2 (en) * | 2007-11-14 | 2012-06-05 | Golba Llc | Positioning system and method using GPS with wireless access points |
-
2019
- 2019-06-28 CN CN201910578628.2A patent/CN112147662B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6240367B1 (en) * | 1998-11-27 | 2001-05-29 | Ching-Fang Lin | Full fusion positioning method for vehicle |
| CN107656301A (zh) * | 2017-09-20 | 2018-02-02 | 北京航天发射技术研究所 | 一种基于多源信息融合的车载定位方法 |
| CN108873038A (zh) * | 2018-09-10 | 2018-11-23 | 芜湖盟博科技有限公司 | 自主泊车定位方法及定位系统 |
Non-Patent Citations (1)
| Title |
|---|
| "一种 UWB/GPS 组合定位方式";胡权等;《南京理工大学学报》;第42卷(第1期);第76-81页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112147662A (zh) | 2020-12-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109435955B (zh) | 一种自动驾驶系统性能评估方法、装置、设备及存储介质 | |
| CN107884800B (zh) | 观测时滞系统的组合导航数据解算方法、装置及导航设备 | |
| CN108061875B (zh) | 基于无线局域网络节点的车辆定位 | |
| CN112147662B (zh) | 车辆定位方法和装置、车辆、存储介质、电子设备 | |
| JP6302848B2 (ja) | 地図生成システム及び地図生成方法 | |
| CN102645222A (zh) | 一种卫星惯性导航方法和设备 | |
| CN112732857B (zh) | 路网处理方法、装置、电子设备以及存储介质 | |
| CN110851545A (zh) | 地图绘制方法、装置及设备 | |
| CN108646261B (zh) | 一种车辆gps数据集的数据处理方法和装置 | |
| CN109405827B (zh) | 终端定位方法及装置 | |
| US20230417931A1 (en) | Method for real-time position estimate correction of a movable object | |
| CN107943859B (zh) | 海量传感器数据收集处理及反馈的系统和方法 | |
| US11415423B2 (en) | Map information management device, map information management system, and map information management method | |
| WO2019071838A1 (zh) | 一种定位方法及设备 | |
| CN108693544B (zh) | 车辆定位的方法、装置及存储介质 | |
| JP2005156308A (ja) | 移動体軌跡情報の異常判定方法及びプログラム | |
| US11525678B2 (en) | Use of offline algorithm to determine location from previous sensor data when location is requested | |
| WO2020133214A1 (zh) | 观测数据转换方法、设备、可移动平台及存储介质 | |
| CN112013864A (zh) | 远程启动车辆导航的方法、装置、设备及存储介质 | |
| CN109270566B (zh) | 导航方法、导航效果测试方法、装置、设备和介质 | |
| CN111090112A (zh) | 设备gps定位取点方法、装置、终端设备及可读存储介质 | |
| KR102519496B1 (ko) | 차량 가속도계 전진축을 캘리브레이션하는 방법 및 장치 | |
| CN115060289A (zh) | 定位轨迹精度评估方法、装置及电子设备、存储介质 | |
| JP2009104330A (ja) | 死角車両検出システム、路側装置、車載装置および死角車両検出方法 | |
| CN114285114A (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 |