CN106291641A

CN106291641A - A kind of speed adaptive assisted location method

Info

Publication number: CN106291641A
Application number: CN201610813761.8A
Authority: CN
Inventors: 王飞雪; 周承松; 彭竞; 刘文祥; 李峥嵘; 李垣陵; 王勇; 楼生强; 徐博
Original assignee: National University of Defense Technology
Current assignee: National University of Defense Technology
Priority date: 2016-09-09
Filing date: 2016-09-09
Publication date: 2017-01-04

Abstract

The invention provides a speed self-adaptive assisted positioning method, which is characterized in that: first obtain the current position and speed information; then obtain the coefficient of variation of the speed according to the statistical properties of the constant speed error; if the coefficient of variation is less than the threshold value, use the position and speed The internal relationship of the speed assists positioning, and if the coefficient of variation exceeds the threshold, the speed assisting positioning is not performed. The speed self-adaptive auxiliary positioning method provided by the present invention can use relatively stable speed measurement results to assist positioning, improve positioning accuracy, and is suitable for dynamic single point positioning, especially for vehicle navigation.

Description

A Speed Adaptive Assisted Positioning Method

技术领域technical field

本发明涉及动态导航定位领域，具体的说是一种速度自适应辅助定位方法，其可运用在卫星导航接收机设计中。The invention relates to the field of dynamic navigation and positioning, in particular to a speed adaptive auxiliary positioning method, which can be used in the design of satellite navigation receivers.

背景技术Background technique

卡尔曼(Kalman filtering)滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。卡尔曼滤波方法总结系统所有过去的输入和扰动对系统的作用的最小参数的集合，结合未来的输入和系统的扰动确定当前系统的状态。卡尔曼滤波对于每个时刻的系统扰动和观测误差，只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上求得误差最小的真实信号估计值。因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。Kalman filtering is an algorithm that uses the linear system state equation to optimally estimate the system state through the input and output observation data of the system. The Kalman filter method summarizes the set of minimum parameters of all past inputs and disturbances of the system, and combines future inputs and system disturbances to determine the current state of the system. For the system disturbance and observation error at each moment, as long as some appropriate assumptions are made on their statistical properties, the Kalman filter can obtain the real filter with the smallest error in the average sense by processing the observation signal containing noise. signal estimate. Therefore, since the Kalman filter theory came out, it has been applied in many departments such as communication system, power system, aerospace, environmental pollution control, industrial control, radar signal processing, etc., and has achieved many successful application results.

但在动态目标的定位过程中，利用卡尔曼滤波对目标进行状态估计，会受到观测误差和动态模型误差的影响，从而导致状态估计精度较差，而利用先验信息能够提高状态估计精度。有研究人员提出以道路先验信息作为约束条件的卡尔曼滤波算法，但该方法必须实时获取道路信息，因此道路约束卡尔曼滤波算法存在一定局限性。另外，也有人提出速度约束卡尔曼滤波算法，实验证明，该方法的定位精度易受定速误差的影响。However, in the process of locating a dynamic target, using Kalman filter to estimate the state of the target will be affected by observation errors and dynamic model errors, resulting in poor state estimation accuracy, and using prior information can improve the state estimation accuracy. Some researchers proposed a Kalman filter algorithm with road prior information as a constraint condition, but this method must obtain road information in real time, so the road-constrained Kalman filter algorithm has certain limitations. In addition, a speed-constrained Kalman filter algorithm has also been proposed. Experiments have shown that the positioning accuracy of this method is easily affected by the constant speed error.

发明内容Contents of the invention

针对现有技术的不足，本发明提供一种速度自适应辅助定位方法，其特征在于：首先获取当前位置和速度信息；然后对根据定速误差的统计性质求取速度的变异系数；若变异系数小于阈值，则利用位置和速度的内部关系辅助定位，若变异系数超过阈值，则不进行速度辅助定位。Aiming at the deficiencies in the prior art, the present invention provides a speed self-adaptive auxiliary positioning method, which is characterized in that: first obtain the current position and speed information; then obtain the coefficient of variation of the speed according to the statistical properties of the constant speed error; if the coefficient of variation If it is less than the threshold, the internal relationship between position and speed is used to assist in positioning. If the coefficient of variation exceeds the threshold, speed-assisted positioning will not be performed.

具体的，使用多普勒测量所述速度，采样频率为1Hz，并使用测距码伪距定位所述位置。Specifically, the Doppler is used to measure the velocity, the sampling frequency is 1 Hz, and the position is positioned using a ranging code pseudo-range.

具体的，使用测距码伪距定位所述位置。Specifically, the position is positioned using a ranging code pseudo-range.

具体的，所述变异系数的建立包括如下步骤：Specifically, the establishment of the coefficient of variation includes the following steps:

步骤一：设定xy方向的所述速度变异系数为cv_xy；Step 1: set the coefficient of variation of the velocity in the _xy direction as cvxy;

步骤二：设定实际定速误差标准差值，且三维速度分量的误差服从标准差σ、均值为0的正态分布Step 2: Set the standard deviation value of the actual constant speed error, and the error of the three-dimensional velocity component A normal distribution with a standard deviation σ and a mean of 0

(εv_x,εv_y,εv_z)～(0,σ)；(εv _x ,εv _y ,εv _z )～(0,σ);

步骤三：利用速度分量误差的标准差σ构建集合U：Step 3: Use the standard deviation σ of the velocity component error to construct a set U:

${{\frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}}}{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}}},, \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}} + + σ σ}{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}} + + σ σ},, \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}} - - σ σ}{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}} + + σ σ},, \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}} + + σ σ}{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}} - - σ σ},, \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}} - - σ σ}{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}} - - σ σ}}};;$

步骤四：变异系数cv_xy为；Step 4: The coefficient of variation cv _xy is;

${cv cv}_{x x y the y} = = \frac{\overset{~ ~}{σ σ}}{\overset{~ ~}{u u}} \cdot \cdot 100100 % %$

其中，为集合U的样本标准差，为集合U的均值，即；in, is the sample standard deviation of the set U, is the mean value of the set U, namely;

$\overset{~ ~}{σ σ} = = \sqrt{\frac{{Σ Σ}_{i i = = 11}^{55} {(({U u}_{i i} - - \overset{~ ~}{u u}))}^{22}}{55 - - 11}}$

$\overset{~ ~}{u u} = = (({Σ Σ}_{i i = = 11}^{55} {U u}_{i i})) / / 55$

具体的，所述步骤可用于获得yz方向的速度变异系数cv_yz。Specifically, the steps can be used to obtain the velocity variation coefficient cv _yz in the yz direction.

具体的，所述实际定速误差标准差为0.2m/s。Specifically, the standard deviation of the actual constant speed error is 0.2m/s.

具体的，所述正态分布的标准差σ为0.1155m/s。Specifically, the standard deviation σ of the normal distribution is 0.1155m/s.

具体的，所述速度与位置的内部约束关系为Specifically, the internal constraint relationship between the speed and position is

$\{\begin{matrix} \frac{{x x}_{k k} - - {x x}_{k k - - 11}}{{y the y}_{k k} - - {y the y}_{k k - - 11}} = = \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}}}{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}}} \\ \frac{{x x}_{k k} - - {x x}_{k k - - 11}}{{z z}_{k k} - - {z z}_{k k - - 11}} = = \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}}}{{\overset{&OverBar; &OverBar;}{v v}}_{{z z}_{k k}}} \\ \frac{{y the y}_{k k} - - {y the y}_{k k - - 11}}{{z z}_{k k} - - {z z}_{k k - - 11}} = = \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}}}{{\overset{&OverBar; &OverBar;}{v v}}_{{z z}_{k k}}} \end{matrix}$

其中，Δt为时间间隔，(u,v,w)为直线方向向量，(x_k,y_k,z_k)为k时刻的位置值，为k时刻和k-1时刻的速度均值。Among them, Δt is the time interval, (u, v, w) is the straight line direction vector, (x _k , y _k , z _k ) is the position value at time k, is the average velocity at time k and k-1.

具体的，所述阈值为20％。Specifically, the threshold is 20%.

具体的，若所述变异系数超过阈值，利用先验信息约束卡尔曼滤波算法进行定位。Specifically, if the coefficient of variation exceeds a threshold, a priori information is used to constrain the Kalman filter algorithm for positioning.

具体的，利用投影法将所述先验信息约束到所述卡尔曼滤波算法，其将卡尔曼无约束状态估计向量通过约束条件函数直接投影到约束面，得到约束状态估计向量X′为：Specifically, the projection method is used to constrain the prior information to the Kalman filter algorithm, which converts the Kalman unconstrained state estimation vector Directly project to the constraint surface through the constraint condition function, and the constraint state estimation vector X′ is obtained as:

${X x}^{' '} = = \overset{^^}{X x} - - {W W}^{- - 11} {B B}^{T T} {(({BW BW}^{- - 11} {B B}^{T T}))}^{- - 11} ((B B \overset{^^}{X x} - - D D.))$

其中，B为状态参数的系数矩阵，D为常数矩阵，W为任意对称的正定权矩阵。Among them, B is the coefficient matrix of state parameters, D is a constant matrix, and W is any symmetrical positive definite weight matrix.

本发明提供的速度自适应辅助定位方法能够灵活在位置与速度的内部关系辅助定位和卡尔曼滤波算法辅助定位中选择，能够利用较稳定的测速结果辅助定位，提高定位精度，适合于动态单点定位，尤其是车载导航。The speed self-adaptive auxiliary positioning method provided by the present invention can flexibly choose between the internal relationship auxiliary positioning of position and speed and the auxiliary positioning of the Kalman filter algorithm, and can use relatively stable speed measurement results to assist positioning, improve positioning accuracy, and is suitable for dynamic single points Positioning, especially car navigation.

附图说明Description of drawings

本发明的下列附图在此作为本发明的一部分用于理解发明。附图中示出了本发明的实施例及其描述，用来解释本发明的原理。The following drawings of the invention are hereby included as part of the invention for understanding the invention. The accompanying drawings illustrate embodiments of the invention and description thereof to explain principles of the invention.

图1为本发明的实现结构示意图。Fig. 1 is a schematic diagram of the realization structure of the present invention.

具体实施方式detailed description

在下文的描述中，给出了具体的细节以便提供对本发明更为彻底的理解。然而，对于本领域技术人员而言显而易见的是，本发明可以无需一个或多个这些细节而得以实施。在其他的例子中，为了避免与本发明发生混淆，对于本领域公知的一些技术特征未进行描述。In the following description, specific details are given in order to provide a more thorough understanding of the present invention. It will be apparent, however, to one skilled in the art that the present invention may be practiced without one or more of these details. In other examples, some technical features known in the art are not described in order to avoid confusion with the present invention.

应当理解的是，本发明能够以不同形式实施，而不应当解释为局限于这里提出的实施例。相反地，提供这些实施例将使公开彻底和完全，并且将本发明的范围完全地传递给本领域技术人员。It should be understood that the invention can be embodied in different forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art.

为了彻底理解本发明，将在下列的描述中提出详细的步骤，以便阐释本发明提出的技术方案。本发明的较佳实施例详细描述如下，然而除了这些详细描述外，本发明还可以具有其他实施方式。In order to thoroughly understand the present invention, detailed steps will be provided in the following description, so as to explain the technical solution proposed by the present invention. Preferred embodiments of the present invention are described in detail below, however, the present invention may have other embodiments besides these detailed descriptions.

在现有研究中，以道路先验信息约束卡尔曼滤波算法存在着必须实时获取道路先验信息的局限性，而以速度为约束条件的卡尔曼滤波算法的定位精度则易受定速误差的影响。本发明的技术方案提出了一种速度自适应辅助定位方法。首先，采用常规卡尔曼滤波获取当前位置和速度信息；然后对根据定速误差的统计性质求取速度的变异系数；若变异系数小于阈值，则利用位置与速度的内部关系辅助定位，若变异系数超过阈值，则不进行速度辅助定位。图1是本发明的实现结构示意图。In existing studies, the Kalman filter algorithm constrained by road prior information has the limitation of obtaining road prior information in real time, while the positioning accuracy of the Kalman filter algorithm constrained by speed is easily affected by the constant speed error. influences. The technical solution of the invention proposes a speed adaptive auxiliary positioning method. First, the conventional Kalman filter is used to obtain the current position and speed information; then the coefficient of variation of the speed is obtained according to the statistical properties of the constant speed error; if the coefficient of variation is less than the threshold, the internal relationship between position and speed is used to assist positioning, if the coefficient of variation If the threshold value is exceeded, speed-assisted positioning will not be performed. Fig. 1 is a schematic diagram of the realization structure of the present invention.

首先，采用常规卡尔曼滤波法获取当前位置和速度信息。优选的，使用多普勒进行测速，速度采样频率优选为1Hz，同时使用测距码进行伪距定位。从而获得当前位置和速度。First, the conventional Kalman filter method is used to obtain the current position and velocity information. Preferably, Doppler is used for speed measurement, and the speed sampling frequency is preferably 1 Hz, and a ranging code is used for pseudo-range positioning. To obtain the current position and velocity.

其次，根据定速误差的统计性质求取速度的变异系数。由于存在定速误差，速度与位置的内部约束关系不一定准确。若定速误差对两者的内部关系影响超过一定阈值时，直接利用速度辅助定位，定位结果将出现不可估量的偏差。因此，需要建立自适应因子，实时判断当前速度能否约束定位。Secondly, the coefficient of variation of the speed is obtained according to the statistical properties of the constant speed error. Due to the existence of constant speed errors, the internal constraint relationship between speed and position is not necessarily accurate. If the impact of the fixed speed error on the internal relationship between the two exceeds a certain threshold, if the speed is directly used to assist positioning, the positioning result will have an immeasurable deviation. Therefore, it is necessary to establish an adaptive factor to judge whether the current speed can constrain the positioning in real time.

由于无法获知定速误差的真实值，考虑从定速误差的统计特性着手。实际中定速误差标准差为0.2m/s，则速度分量的误差服从标准差_σ约为0.1155m/s、均值为0的正态分布，即Since the real value of the constant speed error cannot be known, consider starting from the statistical characteristics of the constant speed error. In practice, the standard deviation of constant speed error is 0.2m/s, then the error of speed component It obeys a normal distribution with a standard deviation _σ of about 0.1155m/s and a mean of 0, that is

(εv_x,εv_y,εv_z)～(0,σ)(εv _x ,εv _y ,εv _z )～(0,σ)

以xy方向为例，利用速度分量误差的标准差σ构建集合U：Taking the xy direction as an example, the set U is constructed using the standard deviation σ of the velocity component error:

${{\frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}}}{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}}},, \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}} + + σ σ}{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}} + + σ σ},, \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}} - - σ σ}{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}} + + σ σ},, \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}} + + σ σ}{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}} - - σ σ},, \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}} - - σ σ}{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}} - - σ σ}}}$

若集合内五个值的离散程度越大，说明定速误差对速度分量间的比值影响越大；若离散程度小，则说明定速误差对其速度分量间比值影响小。以集合中五个值的离散程度来衡量定速误差对速度分量间比值的影响是可行的。If the degree of dispersion of the five values in the set is greater, it means that the constant speed error has a greater influence on the ratio between the speed components; if the degree of dispersion is small, it means that the constant speed error has little influence on the ratio between the speed components. It is feasible to measure the influence of constant speed error on the ratio between speed components by the degree of dispersion of the five values in the set.

集合U中的元素U_i在各历元处的平均值是不一样的，若利用标准差衡量离散程度显得不合理，而采用标准差与平均值的比值(变异系数)来表示离散程度则更为合理。以xy方向为例，其变异系数cv_xy的求取公式为：The average value of elements U _i in the set U is different at each epoch. It is unreasonable to use the standard deviation to measure the degree of dispersion, and it is more reasonable to use the ratio of the standard deviation to the average value (variation coefficient) to represent the degree of dispersion. as reasonable. Taking the xy direction as an example, the calculation formula of the variation coefficient cv _xy is:

${cv cv}_{x x y the y} = = \frac{\overset{~ ~}{σ σ}}{\overset{~ ~}{u u}} \cdot &Center Dot; 100100 % %$

其中，为集合U的样本标准差，为集合U的均值，即in, is the sample standard deviation of the set U, is the mean value of the set U, namely

$\overset{~ ~}{u u} = = (({Σ Σ}_{i i = = 11}^{55} {U u}_{i i})) / / 55$

同理，求出yz方向的变异系数cv_yz。以变异系数(cv_xy,cv_yz)作为自适应因子。Similarly, find the coefficient of variation cv _yz in the yz direction. The coefficient of variation (cv _xy , cv _yz ) is used as the adaptive factor.

若变异系数小于特定阈值，则利用位置和速度的内部关系辅助定位；若变异系数超过特定阈值，则不进行速度辅助定位。If the coefficient of variation is less than a specific threshold, the internal relationship between position and speed is used to assist positioning; if the coefficient of variation exceeds a specific threshold, speed-assisted positioning is not performed.

计算速度和位置的内部关系如下：由于车辆行驶速度、行驶范围受到车辆性能、交通法规和道路宽度等的限制，假设车辆在较短时间间隔Δt内的行驶轨迹是直线，平均速度为接收机在Δt内始末时刻速度观测值的平均值。基于该假设，在Δt内车辆的位置和速度满足：The internal relationship between the calculated speed and position is as follows: Since the vehicle’s driving speed and driving range are limited by vehicle performance, traffic regulations and road width, etc., assuming that the vehicle’s driving trajectory within a short time interval Δt is a straight line, the average speed of the receiver is The average value of velocity observations at the beginning and end of Δt. Based on this assumption, the position and velocity of the vehicle within Δt satisfy:

$\frac{{x x}_{k k} - - {x x}_{k k - - 11}}{μ μ} = = \frac{{y the y}_{k k} - - {y the y}_{k k - - 11}}{v v} = = \frac{{z z}_{k k} - - {z z}_{k k - - 11}}{ω ω}$

$\frac{{\overset{&OverBar; &OverBar;}{v v}}_{{x x}_{k k}} \cdot \cdot Δ Δ t t}{μ μ} = = \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{y the y}_{k k}} \cdot \cdot Δ Δ t t}{v v} = = \frac{{\overset{&OverBar; &OverBar;}{v v}}_{{z z}_{k k}} \cdot &Center Dot; Δ Δ t t}{ω ω}$

其中，(u,v,w)为直线方向向量，(x_k,y_k,z_k)为k时刻的位置值，为k时刻和k-1时刻的速度均值。Among them, (u, v, w) is the straight line direction vector, (x _k , y _k , z _k ) is the position value at time k, is the average velocity at time k and k-1.

则速度与位置的内部约束关系为：Then the internal constraint relationship between speed and position is:

使用变异系数来衡量序列离散程度，变异系数不超过某阈值则认为该数据稳定合格，这在工业制造、医学等行业有广泛应用。例如，在白细胞过氧化物传感器研制中，制备固定细胞膜的重现性变异系数为5.5％；在机器发动机使用寿命可靠性研究中设为30％～40％；在学生考试成绩的稳定评估中设为20％；在GBT27404-2008实验室质量控制规范中，根据成分浓度的不同其变异系数门限变动在1.3％～43％区间。The coefficient of variation is used to measure the degree of dispersion of the sequence. If the coefficient of variation does not exceed a certain threshold, the data is considered stable and qualified. This is widely used in industrial manufacturing, medicine and other industries. For example, in the development of the leukocyte peroxide sensor, the reproducibility coefficient of variation for preparing fixed cell membranes is 5.5%; it is set to 30% to 40% in the study of machine engine service life reliability; 20%; in GBT27404-2008 laboratory quality control specification, the variation coefficient threshold varies from 1.3% to 43% according to the concentration of ingredients.

优选的，本发明的变异系数的阈值d取20％，实际应用可根据观测环境调整阈值。Preferably, the threshold value d of the coefficient of variation in the present invention is 20%, and the threshold value can be adjusted according to the observation environment in practical applications.

当cv_xy、cv_yz都小于阈值d时，则认为定速误差对速度分量间的比值影响较小，满足速度约束滤波条件，选择位置与速度的内部关系辅助定位；反之，则不使用位置与速度的内部关系辅助定位，而优选先验信息约束传统卡尔曼滤波方法辅助定位。When both cv _xy and cv _yz are smaller than the threshold d, it is considered that the constant velocity error has little influence on the ratio between the velocity components, and the velocity constraint filtering condition is satisfied, and the internal relationship between position and velocity is selected to assist positioning; otherwise, the position and velocity are not used The internal relationship of velocity assists localization, while the traditional Kalman filter method with optimal prior information constrains localization.

将先验信息约束融入卡尔曼滤波中的方法主要有伪测量法、度降维法和投影法等，本发明使用投影法。投影法将卡尔曼无约束状态估计向量通过约束条件函数直接投影到约束面，得到约束状态估计向量X′。为求得X′，需根据先验信息建立状态参数间的关系式，即状态约束方程:The methods for integrating prior information constraints into Kalman filtering mainly include pseudo-measurement method, dimensionality reduction method and projection method, etc. The present invention uses the projection method. Kalman unconstrained state estimation vector Directly project to the constraint surface through the constraint condition function to obtain the constraint state estimation vector X′. In order to obtain X′, it is necessary to establish the relationship between the state parameters according to the prior information, that is, the state constraint equation:

BX′-D＝0BX'-D=0

B为状态参数的系数矩阵，D为常数矩阵。再以投影法原则建立目标函数：B is the coefficient matrix of the state parameters, and D is the constant matrix. Then establish the objective function according to the principle of projection method:

$min min {(({X x}^{' '} - - \overset{^^}{X x}))}^{T T} W W (({X x}^{' '} - - \overset{^^}{X x}))$

其中，W为任意对称的正定权矩阵。Among them, W is any symmetric positive definite weight matrix.

为了同时满足状态约束方程和投影法目标函数，构造拉格朗日条件式，其中λ为拉格朗日乘数向量：In order to satisfy both the state constraint equation and the objective function of the projection method, the Lagrangian conditional expression is constructed, where λ is the Lagrange multiplier vector:

$Ω Ω = = {(({X x}^{' '} - - \overset{^^}{X x}))}^{T T} W W (({X x}^{' '} - - \overset{^^}{X x})) + + 22 {λ λ}^{T T} (({BX BX}^{' '} - - D D.))$

为求得当Ω最小时的X′，令Ω对X′和λ的一阶偏导数等于零，即。To obtain X' when Ω is the smallest, let the first partial derivative of Ω with respect to X' and λ be equal to zero, ie.

$\frac{\partial \partial Ω Ω}{\partial \partial {X x}^{' '}} &DoubleRightArrow; &DoubleRightArrow; 22 W W (({X x}^{' '} - - \overset{^^}{X x})) + + 22 {B B}^{T T} λ λ = = 00$

$\frac{\partial \partial Ω Ω}{\partial \partial λ λ} &DoubleRightArrow; &DoubleRightArrow; 22 {BX BX}^{' '} - - 22 D D. = = 00$

最后，解得约束状态估计量Finally, the constraint state estimator is solved

根据该约束状态估计量采用先验条件约束的卡尔曼滤波算法进行辅助定位。According to the constrained state estimator, the Kalman filter algorithm constrained by the prior condition is used for auxiliary positioning.

本发明能够利用较稳定的测速结果辅助定位，提高定位精度，适合于动态单点定位，尤其是车载导航。The present invention can use relatively stable speed measurement results to assist positioning, improve positioning accuracy, and is suitable for dynamic single-point positioning, especially for vehicle navigation.

综上所述，虽然本发明已以较佳实施例揭露如上，然其并非用以限定本发明，任何本领域普通技术人员，在不脱离本发明的精神和范围内，当可作各种更动与润饰，因此本发明的保护范围当视权利要求书界定的范围为准。In summary, although the present invention has been disclosed above with preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art may make various modifications without departing from the spirit and scope of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope defined in the claims.

Claims

1. A speed adaptive auxiliary positioning method, characterized in that: first obtain current position and speed information; then obtain the coefficient of variation of speed according to the statistical properties of constant speed error; if the coefficient of variation is less than a threshold value, then use position and speed If the coefficient of variation exceeds the threshold, the speed-assisted positioning will not be performed.

2. The speed adaptive assisted positioning method according to claim 1, wherein the speed is measured using Doppler, the sampling frequency is 1 Hz, and the position is positioned using a ranging code pseudo-range.

3. The speed adaptive assisted positioning method according to claim 1, wherein the position is positioned using a ranging code pseudo-range.

4. the speed adaptive auxiliary positioning method as claimed in claim 1, is characterized in that, the establishment of described coefficient of variation comprises the steps:

Step 1: set the coefficient of variation of the velocity in the _xy direction as cvxy;

Step 2: Set the standard deviation value of the actual constant speed error, and the error of the three-dimensional velocity component A normal distribution with a standard deviation σ and a mean of 0

(εv _x ,εv _y ,εv _z )～(0,σ);

Step 3: Use the standard deviation σ of the velocity component error to construct a set U:

Step 4: The coefficient of variation cv _xy is;

in, is the sample standard deviation of the set U, is the mean value of the set U, namely;

.

5 . The speed adaptive assisted positioning method according to claim 4 , wherein said step can be used to obtain the coefficient of variation cv _yz of the speed in the yz direction. 6 .

6. The speed adaptive assisted positioning method according to claim 4, wherein the standard deviation of the actual constant speed error is 0.2m/s.

7. The speed adaptive assisted positioning method according to claim 4, wherein the standard deviation σ of the normal distribution is 0.1155m/s.

8. The speed adaptive assisted positioning method according to claim 1, wherein the internal constraint relationship between the speed and the position is

Among them, Δt is the time interval, (u, v, w) is the straight line direction vector, (x _k , y _k , z _k ) is the position value at time k, is the average velocity at time k and k-1.

9. The speed adaptive assisted positioning method according to claim 1, wherein the threshold is 20%.

10. The speed adaptive assisted positioning method as claimed in claim 1, wherein if the coefficient of variation exceeds a threshold value, the prior information is constrained to the Kalman filter algorithm by using the projection method, and the Kalman unconstrained state estimation vector Directly project to the constraint surface through the constraint condition function, and the constraint state estimation vector X′ is obtained as:

Among them, B is the coefficient matrix of state parameters, D is a constant matrix, and W is any symmetrical positive definite weight matrix.