WO2018072350A1 - 车辆轨迹预测方法及装置 - Google Patents
车辆轨迹预测方法及装置 Download PDFInfo
- Publication number
- WO2018072350A1 WO2018072350A1 PCT/CN2017/070853 CN2017070853W WO2018072350A1 WO 2018072350 A1 WO2018072350 A1 WO 2018072350A1 CN 2017070853 W CN2017070853 W CN 2017070853W WO 2018072350 A1 WO2018072350 A1 WO 2018072350A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- gnss data
- data point
- temporary
- temporary gnss
- point
- Prior art date
Links
Images
Classifications
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/01—Detecting movement of traffic to be counted or controlled
- G08G1/0104—Measuring and analyzing of parameters relative to traffic conditions
- G08G1/0125—Traffic data processing
- G08G1/0129—Traffic data processing for creating historical data or processing based on historical data
Definitions
- the present invention relates to the field of vehicle detection technologies, and in particular, to a vehicle trajectory prediction method and apparatus.
- GNSS data records can be used as a source to fit the vehicle's trajectory while the vehicle is in motion, but from the perspective of artificial intelligence, a large number of vehicle GNSS data can be used as the basis for excavating the vehicle's driving state.
- trajectory prediction techniques such as Markov chain method, inertial navigation method, Kalman filter method, particle filter method, least square method, Gaussian mixture model, neural network, machine learning method, topology theory and Other more complicated trajectory prediction methods.
- the above various trajectory prediction methods exist at the theoretical research level, and there are fewer algorithms that are more practical for vehicle trajectory prediction. Because there are computational resources, GNSS drift, storage resources, effectiveness, dependence on geographic information, and other vehicle location information in the embedded system, the above algorithms need to undergo a lot of mathematical operations, resulting in waste of resources.
- the main object of the present invention is to provide a method and apparatus for predicting vehicle trajectory, which aims to reduce the cost of local storage resources.
- the present invention provides a vehicle trajectory prediction method, and the vehicle trajectory prediction method includes the following steps:
- the predicted running trajectory of the vehicle is obtained according to the fitting history trajectory.
- the present invention also provides a vehicle trajectory prediction apparatus, and the vehicle trajectory prediction apparatus includes:
- An acquisition module for acquiring GNSS data of the vehicle An acquisition module for acquiring GNSS data of the vehicle
- a sampling module configured to extract a target key GNSS number in the GNSS data according to a preset sampling rule Data information of the site;
- a fitting module configured to fit a historical trajectory according to the preset fitting formula according to the data information
- the calculation module calculates the predicted running trajectory of the vehicle according to the fitting history trajectory.
- the historical data of the vehicle GNSS is mainly obtained by performing differential sampling on a large amount of historical GNSS data (latitude, latitude and altitude) of the vehicle to obtain data information of key GNSS data points, and the sampled historical data can still be High-precision fitting of the original vehicle GNSS history track.
- the purpose of this is to select key points that can reflect the historical trajectory of the original vehicle from a large amount of historical data of GNSS, which can reduce the cost of local storage resources and reduce the application traversal of GNSS data-based applications, and can meet the size limitation of DSRC communication transmission vehicle safety data information. .
- FIG. 1 is a schematic flow chart of an embodiment of a method for predicting a vehicle trajectory according to the present invention
- FIG. 2 is a relationship diagram of a vehicle coordinate system and a GNSS position coordinate system in an embodiment of a vehicle trajectory prediction method according to the present invention
- FIG. 3 is a first exemplary diagram of a vehicle trajectory formed by GNSS data points in an embodiment of a vehicle trajectory prediction method according to the present invention
- FIG. 4 is a second exemplary diagram of a vehicle trajectory formed by GNSS data points in an embodiment of a vehicle trajectory prediction method according to the present invention
- FIG. 5 is a third exemplary diagram of a vehicle trajectory formed by GNSS data points in an embodiment of a vehicle trajectory prediction method according to the present invention.
- FIG. 6 is a fourth exemplary diagram of a vehicle trajectory formed by GNSS data points in an embodiment of a vehicle trajectory prediction method according to the present invention.
- FIG. 7 is a diagram showing an example of a positional relationship of GNSS data points in an embodiment of a method for predicting a vehicle trajectory according to the present invention
- FIG. 8 is a schematic structural diagram of functional modules of an embodiment of a vehicle trajectory prediction apparatus according to the present invention.
- the vehicle trajectory prediction method includes:
- Step S10 acquiring GNSS data of the vehicle
- the vehicle trajectory prediction method provided by the embodiment of the invention is mainly applied to the vehicle-mounted system for predicting the running trajectory of the vehicle, thereby realizing the function of safe driving.
- GNSS is a global satellite navigation system that generates a large number of GNSS data points during the running of the vehicle.
- the GNSS data points include longitude, latitude, altitude, speed and angular velocity.
- Step S20 extracting data information of target key GNSS data points in the GNSS data according to a preset sampling rule
- Step S30 fitting a historical trajectory according to the preset fitting formula according to the data information
- Step S40 calculating a predicted running track of the vehicle according to the fitting history trajectory.
- the invention is a sub-patent implementation method of the vehicle safety event precise warning patent based on the DSRC V2X communication and the multi-sensor system, that is, based on the GNSS historical data fitting algorithm, the special sampling from the GNSS historical data is proposed to obtain the key historical points.
- These GNSS historical data points can be fitted to a high-accuracy vehicle travel path to mitigate the problem of using the vehicle's historical data to determine whether two vehicles will have a safety incident, the waste of computing resources, and the DSRC communication vehicle safety message packet size limit. .
- the historical data of the vehicle GNSS is mainly obtained by performing differential sampling on a large amount of historical GNSS data (latitude, latitude and altitude) of the vehicle to obtain data information of key GNSS data points, and the sampled historical data can still be High-precision fitting of the original vehicle GNSS history track.
- the purpose of this is to select key points that can reflect the historical trajectory of the original vehicle from a large amount of historical data of GNSS, which can reduce the cost of local storage resources and reduce the application traversal of GNSS data-based applications, and can meet the size limitation of DSRC communication transmission vehicle safety data information. .
- the X direction in the XY coordinate system is the traveling direction directly in front of the vehicle, and the Y direction is the lateral direction of the vehicle; in the NE coordinate system
- the N direction is the north direction
- the E direction is the east direction.
- RefHeading indicates the heading angle, increasing in a clockwise direction. The numerical conversion between the two coordinate systems is not described in detail here.
- K_PHDISTANCE_M is equal to the maximum effective distance of V2V communication transmission, which means that the minimum number of data stored in the vehicle must also satisfy the number of GNSS points greater than the distance of K_PHDISTANCE_M.
- K_PHDISTANCE_M is equal to the maximum effective distance of V2V communication transmission, which means that the minimum number of data stored in the vehicle must also satisfy the number of GNSS points greater than the distance of K_PHDISTANCE_M.
- GNSS trajectory of effective transmission distance composed of a series of latitude and longitude points
- 14 points represent a recorded GNSS trajectory point over the K_PHDISTANCE_M travel distance. We need to sample several key positions in these 14 points.
- PH_ActualError represents a variable representing the distance from the GNSS data point between any two consecutively sampled GNSS data points to the line center point of the corresponding position of the two sampled GNSS data points, the value of which should be less than a fixed threshold.
- the distance between the two points is PH_ActualError Above a certain threshold, we believe that the subsequent sampling of GNSS data points is inaccurate, and the sampling range needs to be further narrowed so that the trajectory between the two sampled GNSS data points can be accurately fitted. So the smaller the fixed threshold is set, the more GNSS data points will be sampled, and the larger the amount of computation and the amount of data transferred. However, the larger the fixed threshold setting, the less accurate the sampling. Based on this idea, all sampled GNSS data points are calculated, and the distances of all sampled GNSS data points in order are the minimum values of K_PHDISTANCE_M values.
- step S20 includes:
- the preset distance range may be a value corresponding to the K_PHDISTANCE_M.
- the method of extracting temporary GNSS data points may be set according to actual needs. For example, one GNSS data point may be extracted as a temporary GNSS data point every preset distance, or one GNSS data point may be extracted as a temporary GNSS data point every preset time.
- the manner of determining the target key GNSS data points according to the temporary GNSS data points may be set according to actual needs, and the following is described in detail in three different implementation manners.
- the foregoing extracting the temporary GNSS data points according to the temporary GNSS data points according to the preset trajectory fitting algorithm for reflecting the target GNSS data points of the trajectory includes:
- the second temporary GNSS data point located between the first temporary GNSS data point and the third temporary GNSS data point Set as the target key GNSS data point; set the next temporary GNSS data point of the first temporary GNSS data point as the initial calculation point, and set the next temporary GNSS data point of the second temporary GNSS data point as a second temporary GNSS data point, the next temporary GNSS data point of the third temporary GNSS data point is set as a third temporary GNSS data point for recalculation;
- P starting in these temporary GNSS data points is taken as the starting point for the precisely sampled GNSS data points.
- PH_ActualChordLength between the first temporary GNSS data point Pstarting and the corresponding position of the next point Pnext (ie, the third temporary GNSS data point) is calculated.
- PH_ActualChordLength value is greater than the first threshold K_PH_CHORDLENGTHTHRESHOLD.
- the GNSS data point indicating the sampling is selected such that the fitting path error is large, so only the first temporary is selected.
- the next temporary GNSS data point corresponding to the GNSS data point is used as a target key GNSS data point (added to PH_ConciseDataBuffer) to accurately reflect the vehicle's fitted trajectory.
- the next temporary GNSS data of the first temporary GNSS data point when the difference is greater than the first threshold or the calculated linear distance is greater than the first threshold The point is set as the initial calculation point, the next temporary GNSS data point of the second temporary GNSS data point is set as the second temporary GNSS data point, and the next temporary GNSS data point of the third temporary GNSS data point is set as the Three temporary GNSS data points.
- next temporary GNSS data point of the second temporary GNSS data point is set as the second temporary GNSS data point
- the next temporary GNSS data point of the third temporary GNSS data point is set Set to the third temporary GNSS data point.
- totalDist totalDist+incrementDist, where totalDist is the distance between the corresponding positions of consecutively sampled GNSS data points in PH_ConciseDataBuffer, The incrementDist is the connection distance between the corresponding positions of the first two sampled GNSS data points, that is, the connection distance between the first temporary GNSS data and the corresponding position of the third temporary GNSS data point.
- the GNSS data points of the most oldest samples are cleared to ensure that the total distance of the sampled GNSS data points is less than or equal to K_PHDISTANCE_M. Then calculate the latest estimated radius PH_EstimatedSumR. of the two points as the sampled GNSS data points. If the number of sampled GNSS data points saved in the PH_ConciseDataBuffer exceeds the maximum allowed number, the data of the oldestest is deleted. PH_ConciseDataBuffer will not overflow.
- the difference from the first embodiment is that PH_EstimatedR is calculated differently.
- the temporary GNSS data point is extracted according to a preset trajectory fitting algorithm. GNSS data points are used to reflect the target GNSS data points of the trajectory including:
- the PH_ActualChordLength calculates a linear distance from the first temporary GNSS data point to a corresponding position of the third temporary GNSS data point for the first temporary GNSS data point, a corner of the corresponding temporary GNSS data point to a corresponding position of the third temporary GNSS data point;
- the estimated radius of the sampled temporary GNSS data point P starting and the temporary GNSS data point P next is equal to K_PH_MAXESTIMATED RADIUS, and the PH_EstimatedR 2 value can be used according to Calculation. If the sampled temporary GNSS data point P next is not equal to K_PH_MAXESTIMATEDRADIUS, then PH_EstimatedR 2 is set to the calculated radius at that point. If the sampled temporary GNSS data point P starting is not equal to K_PH_MAXESTIMATED RADIUS, PH_EstimatedR 2 is set to the calculated radius of the sampled temporary GNSS data point. If none of the above is true, PH_EstimatedR 2 is equal to zero.
- the difference from the first embodiment is that The method for calculating the PH_ActualError is different.
- the foregoing target GNSS data points for extracting the temporary GNSS data points according to the preset trajectory fitting algorithm according to the preset trajectory fitting algorithm for reflecting the trajectory include:
- the next temporary GNSS data point of the third temporary GNSS data point is set as the third temporary GNSS data point for recalculation.
- the GNSS data points are first converted into points in the NE coordinate system, and then the shortest distance from the point D (x3, y3) to the line AC is calculated as d.
- Point B is the closest point on the line AC, according to the geometric relationship:
- d sqrt((x3-x) 2 +(y3-y) 2 ). This d is PH_ActualError.
- the present invention further provides a vehicle trajectory prediction apparatus corresponding to the above vehicle trajectory prediction method, and the vehicle trajectory prediction apparatus includes:
- the obtaining module 10 is configured to acquire GNSS data of the vehicle
- the sampling module 20 is configured to extract data information of target key GNSS data points in the GNSS data according to a preset sampling rule
- a fitting module 30 configured to fit a historical trajectory according to the preset fitting formula according to the data information
- the calculating module 40 calculates a predicted running track of the vehicle according to the fitting history trajectory.
- sampling module includes:
- a sampling unit configured to perform interval extraction of a preset number of temporary GNSS data points in the GNSS data points generated by the preset distance range
- an extracting unit configured to extract, according to the temporary GNSS data point, the temporary GNSS data point to reflect the target key GNSS data point of the trajectory according to a preset trajectory fitting algorithm.
- the preset distance range may be a value corresponding to the K_PHDISTANCE_M.
- the method of extracting temporary GNSS data points may be set according to actual needs. For example, one GNSS data point may be extracted as a temporary GNSS data point every preset distance, or one GNSS data point may be extracted as a temporary GNSS data point every preset time.
- the manner in which the target key GNSS data points are determined based on the temporary GNSS data points may be In order to set according to actual needs, the following is described in detail in three different implementations.
- the extracting unit is specifically configured to:
- the second temporary GNSS data point located between the first temporary GNSS data point and the third temporary GNSS data point Set as the target key GNSS data point; set the next temporary GNSS data point of the first temporary GNSS data point as the initial calculation point, and set the next temporary GNSS data point of the second temporary GNSS data point as a second temporary GNSS data point, the next temporary GNSS data point of the third temporary GNSS data point is set as a third temporary GNSS data point for recalculation;
- the extracting unit is specifically configured to: set a first temporary GNSS data point of the temporary GNSS data point as a target key GNSS data point;
- the PH_ActualChordLength calculates a linear distance from the first temporary GNSS data point to a corresponding position of the third temporary GNSS data point for the first temporary GNSS data point, a corner of the corresponding temporary GNSS data point to a corresponding position of the third temporary GNSS data point;
- the second temporary GNSS data point located between the first temporary GNSS data point and the third temporary GNSS data point Set as the target key GNSS data point; set the next temporary GNSS data point of the first temporary GNSS data point as the initial calculation point, and set the next temporary GNSS data point of the second temporary GNSS data point as a second temporary GNSS data point, the next temporary GNSS data point of the third temporary GNSS data point is set as a third temporary GNSS data point for recalculation;
- the extracting unit is specifically configured to:
- the next temporary GNSS data point of the third temporary GNSS data point is set as the third temporary GNSS data point for recalculation.
Landscapes
- Chemical & Material Sciences (AREA)
- Analytical Chemistry (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Traffic Control Systems (AREA)
- Navigation (AREA)
Abstract
一种车辆轨迹预测方法，所述车辆轨迹预测方法包括以下步骤：获取车辆的GNSS数据（S10）；根据预设采样规则提取GNSS数据中目标关键GNSS数据点的数据信息（S20）；根据所述数据信息按照预设的拟合公式拟合历史轨迹（S30）；根据拟合历史轨迹计算获得所述车辆的预测运行轨迹（S40）。还提供一种车辆轨迹预测装置。上述技术方案降低了本地存储资源耗费。
Description
本发明涉及车辆检测技术领域，尤其涉及车辆轨迹预测方法及装置。
目前，车辆在行驶过程中，大量的GNSS数据记录可以作为拟合车辆行驶轨迹的来源，但是从人工智能角度来看，大量车辆GNSS数据可以作为挖掘车辆行驶状态的基础。目前现有的轨迹预测技术较多，如基于马尔可夫链法、惯性导航法、卡尔曼滤波法、粒子滤波法、最小二乘法、高斯混合模型、神经网络、机器学习法、拓扑学理论以及其他较为复杂的轨迹预测方法。以上各种轨迹预测方法存在于理论研究水平，真正用于车辆轨迹预测较实用的算法较少。因为在嵌入式系统中存在着计算资源、GNSS飘移、存储资源、实效性、依赖于地理信息、依赖于其他车辆位置信息等要求，上述算法需要经过大量的数学运算，造成资源浪费。
发明内容
本发明的主要目的在于提供一种车辆轨迹预测方法及装置，旨在降低本地存储资源耗费。
为实现上述目的，本发明提供的一种车辆轨迹预测方法，所述车辆轨迹预测方法包括以下步骤：
获取车辆的GNSS数据；
根据预设采样规则提取GNSS数据中目标关键GNSS数据点的数据信息；
根据所述数据信息按照预设的拟合公式拟合历史轨迹；
根据拟合历史轨迹计算获得所述车辆的预测运行轨迹。
此外，为实现上述目的，本发明还提供一种车辆轨迹预测装置，所述车辆轨迹预测装置包括：
获取模块，用于获取车辆的GNSS数据；
采样模块，用于根据预设采样规则提取GNSS数据中目标关键GNSS数
据点的数据信息；
拟合模块，用于根据所述数据信息按照预设的拟合公式拟合历史轨迹；
计算模块，根据拟合历史轨迹计算获得所述车辆的预测运行轨迹。
本发明实施例中，对车辆GNSS历史数据拟合主要是针对车辆的大量历史GNSS数据(经度、纬度和高度)进行有差别采样得到关键GNSS数据点的数据信息，采样后的历史数据仍然可以以高精度拟合出原车辆GNSS历史轨。这样做的目的是从GNSS大量历史数据选取能反映原车辆历史轨迹的关键点，这样可以降低本地存储资源耗费以及减少基于GNSS数据的应用计算遍历，并且可以满足DSRC通信传输车辆安全数据信息大小限制。
图1为本发明车辆轨迹预测方法一实施例的流程示意图；
图2为本发明车辆轨迹预测方法一实施例中车辆坐标系和GNSS位置坐标系的关系图；
图3为本发明车辆轨迹预测方法一实施例中GNSS数据点构成的车辆轨迹第一示例图；
图4为本发明车辆轨迹预测方法一实施例中GNSS数据点构成的车辆轨迹第二示例图；
图5为本发明车辆轨迹预测方法一实施例中GNSS数据点构成的车辆轨迹第三示例图；
图6为本发明车辆轨迹预测方法一实施例中GNSS数据点构成的车辆轨迹第四示例图；
图7为本发明车辆轨迹预测方法一实施例中GNSS数据点位置关系示例图；
图8为本发明车辆轨迹预测装置一实施例的功能模块结构示意图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明提供一种车辆轨迹预测方法，参照图1，在一实施例中，该车辆轨迹预测方法包括：
步骤S10，获取车辆的GNSS数据；
本发明实施例提供的车辆轨迹预测方法主要应用车载系统中，用于对车辆的运行轨迹进行预测，从而实现安全驾驶的功能。
GNSS为全球卫星导航系统，在车辆的行驶过程中将产生大量的GNSS数据点，该GNSS数据点包括经度、纬度、高度、速度和角速度等。
步骤S20，根据预设采样规则提取GNSS数据中目标关键GNSS数据点的数据信息；
步骤S30，根据所述数据信息按照预设的拟合公式拟合历史轨迹；
步骤S40，根据拟合历史轨迹计算获得所述车辆的预测运行轨迹。
本发明是在DSRC V2X通信以及多传感器系统基础上的车辆安全事件精确预警专利中的子专利实现方式，即基于GNSS历史数据拟合算法，提出从GNSS历史数据中特殊采样来获取关键历史点，这些GNSS历史数据点可以拟合出高准确率的车辆行驶路径，用于减轻在利用车辆历史数据判断两辆车是否会会发生安全事故计算资源浪费问题以及满足DSRC通信车辆安全消息数据包大小限制。
本发明实施例中，对车辆GNSS历史数据拟合主要是针对车辆的大量历史GNSS数据(经度、纬度和高度)进行有差别采样得到关键GNSS数据点的数据信息，采样后的历史数据仍然可以以高精度拟合出原车辆GNSS历史轨。这样做的目的是从GNSS大量历史数据选取能反映原车辆历史轨迹的关键点，这样可以降低本地存储资源耗费以及减少基于GNSS数据的应用计算遍历，并且可以满足DSRC通信传输车辆安全数据信息大小限制。
具体地，如图2所示，车辆坐标系和GNSS位置坐标系(NE坐标)关系中，XY坐标系中X方向为车辆正前方行驶方向，Y方向为汽车侧向正向；NE坐标系中N方向为正北方向，E方向为正东方向。RefHeading表示航向角，顺时针方向递增。其中，两个坐标系之间的数值转换在此不作详述。
如图3所示，在路径历史拟合时需要定义两个量：K_PHDISTANCE_M
和PH_ActualError；K_PHDISTANCE_M等于V2V通信传输最大有效距离，代表车辆存储的最长数据个数最少也要满足大于K_PHDISTANCE_M距离上的GNSS点数目。在有效传输距离的GNSS轨迹上(一系列经纬点组成)，会根据轨迹曲线变化情况有差别的采样，如图3所示，14个点代表记录的一段超过K_PHDISTANCE_M行驶路程上的GNSS轨迹点。我们需要在这14个点中有差别的采样出几个关键位置点，我们的GNSS历史路径拟合目的就是在在这几个采样关键点上以高精度拟合出当前14个点代表的历史路径。PH_ActualError代表一个变量，表示在任意两个连续采样的GNSS数据点之间的GNSS数据点到这两个采样的GNSS数据点对应位置的连线中线点的距离，其值应该小于一个固定的阈值，若是我们挑选的后续的一个采样的GNSS数据点和当前采样的GNSS数据点之间的原始路径轨迹上有一个GNSS数据点到两个采样的GNSS数据点对应位置之间的连线中点距离PH_ActualError大于一定的阈值，我们认为，后续采样的GNSS数据点是不准确的，需要进一步缩小采样范围，以便于能精确拟合这两个采样的GNSS数据点之间的轨迹。所以当这个固定的阈值设置的越小，则采样的GNSS数据点会越多，计算量和数据传输量也越大。但固定的阈值设置越大，采样越不准确。基于这个思想会计算出所有采样的GNSS数据点，所有采样的GNSS数据点按照顺序连线的距离和应该是K_PHDISTANCE_M值的最小值。
进一步地，对于GNSS数据中目标关键GNSS数据点的采样方式以下将进行详细说明。具体地，在本实施例中，上述步骤S20包括：
在预置距离范围产生的GNSS数据点中进行间隔提取预置数量的临时GNSS数据点；
根据所述临时GNSS数据点按照预设的轨迹拟合算法提取所述临时GNSS数据点用于反映轨迹的目标关键GNSS数据点。
本实施例中，上述预置距离范围即可为上述K_PHDISTANCE_M对应的值。提取临时GNSS数据点的方式可以根据实际需要进行设置，例如可以每隔预设距离提取一个GNSS数据点作为临时GNSS数据点，也可以每隔预设时间提取一个GNSS数据点作为临时GNSS数据点。
具体地，在根据临时GNSS数据点确定目标关键GNSS数据点的方式可以根据实际需要进行设置，以下以三个不同的实现方式进行详细说明。具体
地，在第一实施方式中，上述根据所述临时GNSS数据点按照预设轨迹拟合算法提取所述临时GNSS数据点用于反映轨迹的目标关键GNSS数据点包括：
将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；
根据临时GNSS数据点的排列顺序设定初始计算点；
以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离；
判断计算得到的直线距离是否大于第一阈值；
当计算得到的直线距离小于或等于第一阈值时，计算第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；
根据计算得到的直线距离和转角计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值；
根据所述半径估测值计算所述第一临时GNSS数据点和第三临时GNSS数据点对应位置的连线中点到所述圆弧对应的圆心的中点距离；
计算所述半径估测值与中点距离的差值；
判断所述差值是否大于第二阈值；
当所述差值大于第二阈值或者当计算得到的直线距离大于所述第一阈值时，则将位于所述第一临时GNSS数据点和第三临时GNSS数据点中间的第二临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；
当所述差值小于或等于第二阈值时，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。
在本实施方式，由于真实GNSS路径遵循圆弧形状，则若要表示圆弧轨迹则至少需要三个采样GNSS轨迹位置点(即临时GNSS数据点)(参考图4)状态作为计算初始值。此时，初始化条件值：
i＝3
第一临时GNSS数据点,Pstarting＝Pi-2；
第二临时GNSS数据点,Pprevious＝Pi-1；
第三临时GNSS数据点，Pnext＝Pi；
elementPos＝0；
totalDist＝0；
incrementDist＝0。
在获取的在这些临时GNSS数据点中Pstarting作为精确采样的GNSS数据点的起点。
PH_ConciseDataBuffer[elementPos]＝Pstarting
elementPos++。
然后计算第一临时GNSS数据点Pstarting,和下个点Pnext(即第三临时GNSS数据点)对应位置之间的直线距离PH_ActualChordLength。检查PH_ActualChordLength值是否大于第一阈值K_PH_CHORDLENGTHTHRESHOLD。
当PH_ActualChordLength≤K_PH_CHORDLENGTHTHRESHOLD时，计算第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角
其中H1 and H2分别代表临时GNSS数据点中Pstarting和Pnext的航向角。从而可以根据
计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值PH_EstimatedR。在这一步当
非常小接近于0(直线路径),PH_EstimatedR通过上述工程师会非常大。为了检测
非常小的情况，我们设置一个阈值K_PHSMALLDELTAPHI_R，
会和这个阈值进行比较。如果
小于这个值，半径会很大，此时半径直接设为K_PH_MAXESTIMATEDRADIUS(即为预设值)。接着可以根据
计算第一临时GNSS数据点和第三临时GNSS数据点对应位置的连线中点到所述圆弧对应的圆心的中点距离d，最后根据PH_ActualError＝PH_EstimatedR-d得到半径估测值与中点距离的差值PH_ActualError。如果PH_ActualError大于允许的PH误差K_PHALLOWABLEERROR_M(预设值)或者当计算得到的直线距离大于所述第一阈值时，表示这个采样的GNSS数据点选取使得拟合路径误差大，所
以只有选这个第一临时GNSS数据点对应的下一个临时GNSS数据点作为一个目标关键GNSS数据点(加入到PH_ConciseDataBuffer中)才能准确的反映车辆拟合的轨迹。
然后进行初始采样的GNSS数据点的位置更新，具体地，在当差值大于第一阈值或计算得到的直线距离大于所述第一阈值时，将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点。采用程序语言表述如下：
Pstarting＝Pi-1；
Pnext＝Pi+1；
Pprevious＝Pi；
i＝i+1。
在当差值小于或等于第二阈值时，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点。采用程序语言表述如下：
Pnext＝Pi+1；
Pprevious＝Pi；
i＝i+1。
每当增加一个目标关键GNSS数据点时，PH_ConciseDataBuffer中连续采样的GNSS数据点之间的距离和，totalDist＝totalDist+incrementDist，其中totalDist为PH_ConciseDataBuffer中连续采样的GNSS数据点对应位置之间的距离和，incrementDist为前两个采样的GNSS数据点对应位置之间的连线距离，即为第一临时GNSS数据和第三临时GNSS数据点对应位置之间的连线距离。如果距离和totalDist大于等于K_PHDISTANCE_M，则清除最oldest的采样的GNSS数据点，保证采样的GNSS数据点总距离小于或等于K_PHDISTANCE_M。然后把计算出最新的作为采样的GNSS数据点的两个点的估测半径PH_EstimatedSumR.。如果PH_ConciseDataBuffer中保存的采样的GNSS数据点数目超过最大允许数目，则删除最Oldest的数据维持
PH_ConciseDataBuffer不会溢出。
具体地，在第二实施方式中，与第一实施方式的区别在于，PH_EstimatedR的计算方式不一样，本实施方式中，上述根据所述临时GNSS数据点按照预设轨迹拟合算法提取所述临时GNSS数据点用于反映轨迹的目标关键GNSS数据点包括：
将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；
根据临时GNSS数据点的排列顺序设定初始计算点；
以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离；
判断计算得到的直线距离是否大于第一阈值；
当计算得到的直线距离小于或等于第一阈值时，根据车辆的速度和航向角变化率计算第n-1个临时GNSS数据点的半径R2i，其中i＝n-1；将计算得到的半径大于第三阈值的临时GNSS数据点删除，并将对应的临时GNSS数据点的半径设置为第三阈值；
根据
计算第n-1个临时GNSS数据点的动态平均半径估测值PH_EstimatedR2；
根据PH_EstimatedR＝K_PH_RADIUSWEIGHTONE*PH_EstimatedR1+K_PH_RADIUSWEIGHTTWO*PH_EstimatedR2计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值PH_EstimatedR；其中K_PH_RADIUSWEIGHTONE和K_PH_RADIUSWEIGHTTWO为加权和，且K_PH_RADIUSWEIGHTONE和K_PH_RADIUSWEIGHTTWO之和为1；
其中所述PH_ActualChordLength为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离，
为第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；
计算所述半径估测值与中点距离的差值；
判断所述差值是否大于第二阈值；
当所述差值大于第二阈值或者当计算得到的直线距离大于所述第一阈值
时，则将位于所述第一临时GNSS数据点和第三临时GNSS数据点中间的第二临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；
当所述差值小于或等于第二阈值时，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。
如图5所示，假设在GNSS轨迹上面有N个点(P1…Pn)，P1为起始点(即第一临时GNSS数据点)，Pn为第三个点(即第三临时GNSS数据点)，而P2，…，Pn-1作为中间点集合(即第二临时GNSS数据点，本实施方式中，该第二临时GNSS数据点可以为多个)。在此，可以根据R2i＝ν/w计算各临时GNSS数据点的半径，其中ν为车辆速度，w为车辆航向角变化率，i＝n-1，n为临时GNSS数据点的位置。如果计算出来的R2(n-1)大于给定的第三阈值，则令R2(n-1)＝K_PH_MAXESTIMATEDRADIUS，然后删除这个GNSS位置点，并不计入平均半径估测值的计算过程中。
具体地，根据平均半径估测值计算半径估测值PH_EstimatedR时，当PH_EstimatedR2＝0，所有的关于PH_EstimatedR2相关的置为K_PH_MAXESTIMATEDRADIUS，K_PH_RADIUSWEIGHTONE＝1,K_PH_RADIUSWEIGHTTWO＝0。
采样的临时GNSS数据点Pstarting和临时GNSS数据点Pnext的半径估算值都等于K_PH_MAXESTIMATEDRADIUS，PH_EstimatedR2值利用则可以根据
进行计算。如果采样的临时GNSS数据点Pnext不等于K_PH_MAXESTIMATEDRADIUS，则PH_EstimatedR2设为该点上计算出来的半径。如果采样的临时GNSS数据点Pstarting不等于K_PH_MAXESTIMATEDRADIUS，PH_EstimatedR2设为该采样的临时GNSS数据点上计算出来的半径。如果上述均不成立，则PH_EstimatedR2等于0。
具体地，如图6所示，在第三实施方式中，与第一实施方式的区别在于，
PH_ActualError的计算方式不一样，本实施方式中，上述根据所述临时GNSS数据点按照预设轨迹拟合算法提取所述临时GNSS数据点用于反映轨迹的目标关键GNSS数据点包括：
将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；
根据临时GNSS数据点的排列顺序设定初始计算点；
以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离；
判断计算得到的直线距离是否大于第一阈值；
当计算得到的直线距离小于或等于第一阈值时，计算第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；
根据计算得到的直线距离和转角计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值；
根据NE坐标系获取第一临时GNSS数据点到第三临时GNSS数据点中间的目标临时GNSS数据点，所述目标临时GNSS数据点为第一临时GNSS数据点到第三临时GNSS数据点的临时GNSS数据点相对于第一临时GNSS数据点到第三临时GNSS数据点对应位置的连线距离最大的临时GNSS数据点；
判断所述目标临时GNSS数据点相对于第一临时GNSS数据点到第三临时GNSS数据点对应位置的连线距离是否大于第二阈值；
当所述标临时GNSS数据点对应位置的连线距离大于第二阈值或者当计算得到的直线距离大于所述第一阈值时，则将所述目标临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；
当所述差值小于或等于第二阈值时，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。
本实施方式中，在计算PH_ActualError值，定义PH数据点，P1是起点，Pn是一个点，P2到Pn-1为中间点。定义中间点到Pstarting和点Pnext连线距离为集合Di，i＝1，...，n。定义PH_ActualErro＝MAX(Di)；i＝2，…，n-1。
具体地，在计算Di时，先把GNSS数据点都转化为NE坐标系下的点，然后计算点D(x3,y3)到线AC的最短距离为d。
如图7所示，设A(x1,y1)，C(x2,y2)，B＝A+u(C-A),u取0-1之间值。点B是线AC上距离最近的点，根据几何关系式：
(D-B)dot(C-A)＝0,dot表示矢量点成，又由B＝A+u(C-A)得到：
[D-A-u(C-A)]dot(C-A)＝0。
解方程得到：
u＝((x3-x1)(x2-x1)+(y3-y1)(y2-y1))/||C–A||2。
u代入B(x，y)得到：
x＝x1+u(x2-x1)，
y＝y1+u(y2-y1)。
得到：d＝sqrt((x3-x)2+(y3-y)2)。该d为PH_ActualError。
参照图8，本发明对应上述车辆轨迹预测方法还提供一种车辆轨迹预测装置，所述车辆轨迹预测装置包括：
获取模块10，用于获取车辆的GNSS数据；
采样模块20，用于根据预设采样规则提取GNSS数据中目标关键GNSS数据点的数据信息；
拟合模块30，用于根据所述数据信息按照预设的拟合公式拟合历史轨迹；
计算模块40，根据拟合历史轨迹计算获得所述车辆的预测运行轨迹。
进一步地，所述采样模块包括：
采样单元，用于在预置距离范围产生的GNSS数据点中进行间隔提取预置数量的临时GNSS数据点；
提取单元，用于根据所述临时GNSS数据点按照预设的轨迹拟合算法提取所述临时GNSS数据点用于反映轨迹的目标关键GNSS数据点。
本实施例中，上述预置距离范围即可为上述K_PHDISTANCE_M对应的值。提取临时GNSS数据点的方式可以根据实际需要进行设置，例如可以每隔预设距离提取一个GNSS数据点作为临时GNSS数据点，也可以每隔预设时间提取一个GNSS数据点作为临时GNSS数据点。
具体地，在根据临时GNSS数据点确定目标关键GNSS数据点的方式可
以根据实际需要进行设置，以下以三个不同的实现方式进行详细说明。具体地，在第一实施方式中，所述提取单元具体用于：
将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；
根据临时GNSS数据点的排列顺序设定初始计算点；
以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离；
判断计算得到的直线距离是否大于第一阈值；
当计算得到的直线距离小于或等于第一阈值时，计算第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；
根据计算得到的直线距离和转角计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值；
根据所述半径估测值计算所述第一临时GNSS数据点和第三临时GNSS数据点对应位置的连线中点到所述圆弧对应的圆心的中点距离；
计算所述半径估测值与中点距离的差值；
判断所述差值是否大于第二阈值；
当所述差值大于第二阈值或者当计算得到的直线距离大于所述第一阈值时，则将位于所述第一临时GNSS数据点和第三临时GNSS数据点中间的第二临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；
当所述差值小于或等于第二阈值时，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。
在第二实施方式中，所述提取单元具体用于：将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；
根据临时GNSS数据点的排列顺序设定初始计算点；
以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点
到第三临时GNSS数据点对应位置的直线距离；
判断计算得到的直线距离是否大于第一阈值；
当计算得到的直线距离小于或等于第一阈值时，根据车辆的速度和航向角变化率计算第n-1个临时GNSS数据点的半径R2i，其中i＝n-1；将计算得到的半径大于第三阈值的临时GNSS数据点删除，并将对应的临时GNSS数据点的半径设置为第三阈值；
根据
计算第n-1个临时GNSS数据点的动态平均半径估测值PH_EstimatedR2；
根据PH_EstimatedR＝K_PH_RADIUSWEIGHTONE*PH_EstimatedR1+K_PH_RADIUSWEIGHTTWO*PH_EstimatedR2计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值PH_EstimatedR；其中K_PH_RADIUSWEIGHTONE和K_PH_RADIUSWEIGHTTWO为加权和，且K_PH_RADIUSWEIGHTONE和K_PH_RADIUSWEIGHTTWO之和为1；
其中所述PH_ActualChordLength为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离，
为第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；
计算所述半径估测值与中点距离的差值；
判断所述差值是否大于第二阈值；
当所述差值大于第二阈值或者当计算得到的直线距离大于所述第一阈值时，则将位于所述第一临时GNSS数据点和第三临时GNSS数据点中间的第二临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；
当所述差值小于或等于第二阈值时，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。
在第三实施方式中，所述提取单元具体用于：
将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；
根据临时GNSS数据点的排列顺序设定初始计算点；
以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离；
判断计算得到的直线距离是否大于第一阈值；
当计算得到的直线距离小于或等于第一阈值时，计算第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；
根据计算得到的直线距离和转角计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值；
根据NE坐标系获取第一临时GNSS数据点到第三临时GNSS数据点中间的目标临时GNSS数据点，所述目标临时GNSS数据点为第一临时GNSS数据点到第三临时GNSS数据点的临时GNSS数据点相对于第一临时GNSS数据点到第三临时GNSS数据点对应位置的连线距离最大的临时GNSS数据点；
判断所述目标临时GNSS数据点相对于第一临时GNSS数据点到第三临时GNSS数据点对应位置的连线距离是否大于第二阈值；
当所述标临时GNSS数据点对应位置的连线距离大于第二阈值或者当计算得到的直线距离大于所述第一阈值时，则将所述目标临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；
当所述差值小于或等于第二阈值时，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。
Claims (10)
- 一种车辆轨迹预测方法，其特征在于，所述车辆轨迹预测方法包括以下步骤：获取车辆的GNSS数据；根据预设采样规则提取GNSS数据中目标关键GNSS数据点的数据信息；根据所述数据信息按照预设的拟合公式拟合历史轨迹；根据拟合历史轨迹计算获得所述车辆的预测运行轨迹。
- 如权利要求1所述的车辆轨迹预测方法，其特征在于，所述根据预设采样规则提取GNSS数据中目标关键GNSS数据点的数据信息包括：在预置距离范围产生的GNSS数据点中进行间隔提取预置数量的临时GNSS数据点；根据所述临时GNSS数据点按照预设的轨迹拟合算法提取所述临时GNSS数据点用于反映轨迹的目标关键GNSS数据点。
- 如权利要求2所述的车辆轨迹预测方法，其特征在于，所述根据所述临时GNSS数据点按照预设的轨迹拟合算法提取所述临时GNSS数据点用于反映轨迹的目标关键GNSS数据点包括：将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；根据临时GNSS数据点的排列顺序设定初始计算点；以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离；判断计算得到的直线距离是否大于第一阈值；当计算得到的直线距离小于或等于第一阈值时，计算第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；根据计算得到的直线距离和转角计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值；根据所述半径估测值计算所述第一临时GNSS数据点和第三临时GNSS数据点对应位置的连线中点到所述圆弧对应的圆心的中点距离；计算所述半径估测值与中点距离的差值；判断所述差值是否大于第二阈值；当所述差值大于第二阈值或者当计算得到的直线距离大于所述第一阈值时，则将位于所述第一临时GNSS数据点和第三临时GNSS数据点中间的第二临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；当所述差值小于或等于第二阈值时，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。
- 如权利要求2所述的车辆轨迹预测方法，其特征在于，所述根据所述临时GNSS数据点按照预设轨迹拟合算法提取所述临时GNSS数据点用于反映轨迹的目标关键GNSS数据点包括：将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；根据临时GNSS数据点的排列顺序设定初始计算点；以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离；判断计算得到的直线距离是否大于第一阈值；当计算得到的直线距离小于或等于第一阈值时，根据车辆的速度和航向角变化率计算第n-1个临时GNSS数据点的半径R2i，其中i＝n-1；将计算得到的半径大于第三阈值的临时GNSS数据点删除，并将对应的临时GNSS数据点的半径设置为第三阈值；根据计算第n-1个临时GNSS数据点的动态平均半径估测值PH_EstimatedR2；
根据PH_EstimatedR＝K_PH_RADIUSWEIGHTONE*PH_EstimatedR1+K_PH_RADIUSWEIGHTTWO*PH_EstimatedR2计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值PH_EstimatedR；其 中K_PH_RADIUSWEIGHTONE和K_PH_RADIUSWEIGHTTWO为加权和，且K_PH_RADIUSWEIGHTONE和K_PH_RADIUSWEIGHTTWO之和为1；其中所述PH_ActualChordLength为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离，
为第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；
计算所述半径估测值与中点距离的差值；判断所述差值是否大于第二阈值；当所述差值大于第二阈值或者当计算得到的直线距离大于所述第一阈值时，则将位于所述第一临时GNSS数据点和第三临时GNSS数据点中间的第二临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；当所述差值小于或等于第二阈值时，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。 - 如权利要求2所述的车辆轨迹预测方法，其特征在于，所述根据所述临时GNSS数据点按照预设轨迹拟合算法提取所述临时GNSS数据点用于反映轨迹的目标关键GNSS数据点包括：将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；根据临时GNSS数据点的排列顺序设定初始计算点；以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离；判断计算得到的直线距离是否大于第一阈值；当计算得到的直线距离小于或等于第一阈值时，计算第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；根据计算得到的直线距离和转角计算第一临时GNSS数据点到第三 GNSS数据点之间对应位置形成的圆弧的半径估测值；根据NE坐标系获取第一临时GNSS数据点到第三临时GNSS数据点中间的目标临时GNSS数据点，所述目标临时GNSS数据点为第一临时GNSS数据点到第三临时GNSS数据点的临时GNSS数据点相对于第一临时GNSS数据点到第三临时GNSS数据点对应位置的连线距离最大的临时GNSS数据点；判断所述目标临时GNSS数据点相对于第一临时GNSS数据点到第三临时GNSS数据点对应位置的连线距离是否大于第二阈值；当所述标临时GNSS数据点对应位置的连线距离大于第二阈值或者当计算得到的直线距离大于所述第一阈值时，则将所述目标临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；当所述差值小于或等于第二阈值时，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。
- 一种车辆轨迹预测装置，其特征在于，所述车辆轨迹预测装置包括：获取模块，用于获取车辆的GNSS数据；采样模块，用于根据预设采样规则提取GNSS数据中目标关键GNSS数据点的数据信息；拟合模块，用于根据所述数据信息按照预设的拟合公式拟合历史轨迹；计算模块，根据拟合历史轨迹计算获得所述车辆的预测运行轨迹。
- 如权利要求6所述的车辆轨迹预测装置，其特征在于，所述采样模块包括：采样单元，用于在预置距离范围产生的GNSS数据点中进行间隔提取预置数量的临时GNSS数据点；提取单元，用于根据所述临时GNSS数据点按照预设轨迹拟合算法提取所述临时GNSS数据点用于反映轨迹的目标关键GNSS数据点。
- 如权利要求7所述的车辆轨迹预测装置，其特征在于，所述提取单元具体用于：将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；根据临时GNSS数据点的排列顺序设定初始计算点；以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离；判断计算得到的直线距离是否大于第一阈值；当计算得到的直线距离小于或等于第一阈值时，计算第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；根据计算得到的直线距离和转角计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值；根据所述半径估测值计算所述第一临时GNSS数据点和第三临时GNSS数据点的连线中点到所述圆弧对应的圆心的中点距离；计算所述半径估测值与中点距离的差值；判断所述差值是否大于第二阈值；当所述差值大于第二阈值或者当计算得到的直线距离大于所述第一阈值时，则将位于所述第一临时GNSS数据点和第三临时GNSS数据点中间的第二临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；当所述差值小于或等于第二阈值时，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。
- 如权利要求7所述的车辆轨迹预测装置，其特征在于，所述提取单元具体用于：将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；根据临时GNSS数据点的排列顺序设定初始计算点；以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点 到第三临时GNSS数据点对应位置的直线距离；判断计算得到的直线距离是否大于第一阈值；当计算得到的直线距离小于或等于第一阈值时，根据车辆的速度和航向角变化率计算第n-1个临时GNSS数据点的半径R2i，其中i＝n-1；将计算得到的半径大于第三阈值的临时GNSS数据点删除，并将对应的临时GNSS数据点的半径设置为第三阈值；根据计算第n-1个临时GNSS数据点的动态平均半径估测值PH_EstimatedR2；
根据PH_EstimatedR＝K_PH_RADIUSWEIGHTONE*PH_EstimatedR1+K_PH_RADIUSWEIGHTTWO*PH_EstimatedR2计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值PH_EstimatedR；其中K_PH_RADIUSWEIGHTONE和K_PH_RADIUSWEIGHTTWO为加权和，且K_PH_RADIUSWEIGHTONE和K_PH_RADIUSWEIGHTTWO之和为1；其中所述PH_ActualChordLength为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离，
为第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；
计算所述半径估测值与中点距离的差值；判断所述差值是否大于第二阈值；当所述差值大于第二阈值或者当计算得到的直线距离大于所述第一阈值时，则将位于所述第一临时GNSS数据点和第三临时GNSS数据点中间的第二临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；当所述差值小于或等于第二阈值时，将第二临时GNSS数据点的下一个临时GNSS数据点设定为第二临时GNSS数据点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。 - 如权利要求7所述的车辆轨迹预测装置，其特征在于，所述提取单元具体用于：将临时GNSS数据点的首个临时GNSS数据点设定为目标关键GNSS数据点；根据临时GNSS数据点的排列顺序设定初始计算点；以初始计算点为第一临时GNSS数据点计算所述第一临时GNSS数据点到第三临时GNSS数据点对应位置的直线距离；判断计算得到的直线距离是否大于第一阈值；当计算得到的直线距离小于或等于第一阈值时，计算第一临时GNSS数据点到第三临时GNSS数据点对应位置的转角；根据计算得到的直线距离和转角计算第一临时GNSS数据点到第三GNSS数据点对应位置之间形成的圆弧的半径估测值；根据NE坐标系获取第一临时GNSS数据点到第三临时GNSS数据点中间的目标临时GNSS数据点，所述目标临时GNSS数据点为第一临时GNSS数据点到第三临时GNSS数据点的临时GNSS数据点相对于第一临时GNSS数据点到第三临时GNSS数据点对应位置的连线距离最大的临时GNSS数据点；判断所述目标临时GNSS数据点相对于第一临时GNSS数据点到第三临时GNSS数据点对应位置的连线距离是否大于第二阈值；当所述标临时GNSS数据点对应位置的连线距离大于第二阈值或者当计算得到的直线距离大于所述第一阈值时，则将所述目标临时GNSS数据点设定为所述目标关键GNSS数据点；并将第一临时GNSS数据点的下一个临时GNSS数据点设定为初始计算点，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算；当所述差值小于或等于第二阈值时，将第三临时GNSS数据点的下一个临时GNSS数据点设定为第三临时GNSS数据点，以进行重新计算。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610919459.0 | 2016-10-20 | ||
| CN201610919459.0A CN106558219B (zh) | 2016-10-20 | 2016-10-20 | 车辆轨迹预测方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| WO2018072350A1 true WO2018072350A1 (zh) | 2018-04-26 |
Family
ID=58443849
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| PCT/CN2017/070853 WO2018072350A1 (zh) | 2016-10-20 | 2017-01-11 | 车辆轨迹预测方法及装置 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN106558219B (zh) |
| WO (1) | WO2018072350A1 (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110348163A (zh) * | 2019-07-19 | 2019-10-18 | 合肥工业大学 | 一种轮胎花纹边界连续相切圆弧的拟合方法 |
| CN112693455A (zh) * | 2021-01-21 | 2021-04-23 | 广州小鹏自动驾驶科技有限公司 | 移动轨迹拟合方法、装置、车辆和存储介质 |
| CN113465616A (zh) * | 2021-06-28 | 2021-10-01 | 湖北亿咖通科技有限公司 | 轨迹异常点检测方法和装置、电子设备、计算机程序产品及计算机可读存储介质 |
Families Citing this family (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107025787A (zh) * | 2017-04-11 | 2017-08-08 | 首都经济贸易大学 | 一种物流车队控制方法和系统 |
| CN107298100B (zh) * | 2017-05-16 | 2019-05-10 | 开易（北京）科技有限公司 | 一种基于高斯混合模型的车辆轨迹预测方法、系统 |
| CN107464272A (zh) * | 2017-07-19 | 2017-12-12 | 南京理工大学 | 基于关键点的中心扩散式气象传真图等值线的插值方法 |
| CN110021161B (zh) * | 2018-01-09 | 2021-12-21 | 株式会社日立制作所 | 一种交通流向的预测方法及系统 |
| CN110160541B (zh) | 2018-08-06 | 2022-02-22 | 腾讯大地通途（北京）科技有限公司 | 运动轨迹的重构方法和装置、存储介质、电子装置 |
| CN110111568A (zh) * | 2019-04-30 | 2019-08-09 | 重庆长安汽车股份有限公司 | 自适应的路径特征点动态提取方法及计算机可读存储介质 |
| CN113038387B (zh) * | 2021-03-12 | 2024-02-20 | 深圳泓越信息科技有限公司 | 低轨卫星网络中基于q学习的切换判决方法 |
| CN113407115A (zh) * | 2021-06-01 | 2021-09-17 | 东风汽车集团股份有限公司 | 一种可移动物体的历史轨迹记录存储系统和方法 |
| CN114001976B (zh) * | 2021-10-19 | 2024-03-12 | 杭州飞步科技有限公司 | 控制误差的确定方法、装置、设备及存储介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102707300A (zh) * | 2012-06-05 | 2012-10-03 | 大唐移动通信设备有限公司 | 一种gps轨迹优化方法、装置及系统 |
| CN104143260A (zh) * | 2013-05-10 | 2014-11-12 | 北京航天长峰科技工业集团有限公司 | 一种基于数据融合优化模型的车辆轨迹预测方法 |
| CN104966129A (zh) * | 2015-06-08 | 2015-10-07 | 浙江大学 | 一种车辆运行轨迹的分离方法 |
| CN105760958A (zh) * | 2016-02-24 | 2016-07-13 | 电子科技大学 | 一种基于车联网的车辆轨迹预测方法 |
| CN105913454A (zh) * | 2016-04-06 | 2016-08-31 | 东南大学 | 一种视频图像中运动目标的像素坐标轨迹预测方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104215249B (zh) * | 2014-08-26 | 2017-03-22 | 厦门市润铭电子科技有限公司 | 一种行车轨迹的平滑方法 |
| EP3009990B1 (en) * | 2014-10-18 | 2017-11-22 | Tata Consultancy Services Limited | Method and system for performing crash analysis of one or more vehicles |
-
2016
- 2016-10-20 CN CN201610919459.0A patent/CN106558219B/zh active Active
-
2017
- 2017-01-11 WO PCT/CN2017/070853 patent/WO2018072350A1/zh active Application Filing
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102707300A (zh) * | 2012-06-05 | 2012-10-03 | 大唐移动通信设备有限公司 | 一种gps轨迹优化方法、装置及系统 |
| CN104143260A (zh) * | 2013-05-10 | 2014-11-12 | 北京航天长峰科技工业集团有限公司 | 一种基于数据融合优化模型的车辆轨迹预测方法 |
| CN104966129A (zh) * | 2015-06-08 | 2015-10-07 | 浙江大学 | 一种车辆运行轨迹的分离方法 |
| CN105760958A (zh) * | 2016-02-24 | 2016-07-13 | 电子科技大学 | 一种基于车联网的车辆轨迹预测方法 |
| CN105913454A (zh) * | 2016-04-06 | 2016-08-31 | 东南大学 | 一种视频图像中运动目标的像素坐标轨迹预测方法 |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110348163A (zh) * | 2019-07-19 | 2019-10-18 | 合肥工业大学 | 一种轮胎花纹边界连续相切圆弧的拟合方法 |
| CN110348163B (zh) * | 2019-07-19 | 2022-09-13 | 合肥工业大学 | 一种轮胎花纹边界连续相切圆弧的拟合方法 |
| CN112693455A (zh) * | 2021-01-21 | 2021-04-23 | 广州小鹏自动驾驶科技有限公司 | 移动轨迹拟合方法、装置、车辆和存储介质 |
| CN113465616A (zh) * | 2021-06-28 | 2021-10-01 | 湖北亿咖通科技有限公司 | 轨迹异常点检测方法和装置、电子设备、计算机程序产品及计算机可读存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106558219B (zh) | 2020-05-12 |
| CN106558219A (zh) | 2017-04-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2018072350A1 (zh) | 车辆轨迹预测方法及装置 | |
| CN111307162B (zh) | 用于自动驾驶场景的多传感器融合定位方法 | |
| CN106908775B (zh) | 一种基于激光反射强度的无人车实时定位方法 | |
| Georgy et al. | Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation | |
| Song et al. | Enhancing GPS with lane-level navigation to facilitate highway driving | |
| AU2019233779B2 (en) | Vehicle tracking | |
| WO2018072362A1 (zh) | 车辆实时轨迹预测方法及预测系统 | |
| Jagadeesh et al. | A map matching method for GPS based real-time vehicle location | |
| CN106885576B (zh) | 一种基于多点地形匹配定位的auv航迹偏差估计方法 | |
| CN107274721B (zh) | 一种智能交通系统中多车辆协作定位方法 | |
| Huang et al. | An expectation-maximization-based interacting multiple model approach for cooperative driving systems | |
| CN104154916A (zh) | 一种基于激光陀螺捷联惯组的车载定位设备 | |
| CN111524169A (zh) | 用神经网络基于传感器数据和地图数据的图像配准的定位 | |
| Heirich et al. | Probabilistic localization method for trains | |
| Dawson et al. | Radar-based multisensor fusion for uninterrupted reliable positioning in GNSS-denied environments | |
| Nguyen et al. | Improving the accuracy of the autonomous mobile robot localization systems based on the multiple sensor fusion methods | |
| KR102103651B1 (ko) | 지도의 차로 개수를 활용한 파티클 필터링 퇴화 경감 방법 및 시스템 | |
| Song et al. | RFID/in-vehicle sensors-integrated vehicle positioning strategy utilising LSSVM and federated UKF in a tunnel | |
| Dean et al. | Terrain-based road vehicle localization on multi-lane highways | |
| Deusch et al. | Improving localization in digital maps with grid maps | |
| Kauffman et al. | Navigation via H-field signature map correlation and INS integration | |
| Zhu et al. | Research on localization vehicle based on multiple sensors fusion system | |
| Ahmed et al. | Minimizing GPS dependency for a vehicle’s trajectory identification by using data from smartphone inertial sensors and onboard diagnostics device | |
| CN115046546A (zh) | 一种基于车道线识别的自动驾驶汽车定位系统及方法 | |
| Sumithra et al. | An optimal innovation based adaptive estimation Kalman filter for accurate positioning in a vehicular ad-hoc network |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| 121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 17863175 Country of ref document: EP Kind code of ref document: A1 |
|
| NENP | Non-entry into the national phase |
Ref country code: DE |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 17863175 Country of ref document: EP Kind code of ref document: A1 |