CN112526571A - 位姿数据的处理方法、处理装置、处理器和车辆系统 - Google Patents

位姿数据的处理方法、处理装置、处理器和车辆系统 Download PDF

Info

Publication number
CN112526571A
CN112526571A CN202011568934.7A CN202011568934A CN112526571A CN 112526571 A CN112526571 A CN 112526571A CN 202011568934 A CN202011568934 A CN 202011568934A CN 112526571 A CN112526571 A CN 112526571A
Authority
CN
China
Prior art keywords
data
initial
rtk
processing
threshold
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.)
Pending
Application number
CN202011568934.7A
Other languages
English (en)
Inventor
刘川川
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing PonyAi Science And Technology Co ltd
Original Assignee
Beijing PonyAi Science And Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing PonyAi Science And Technology Co ltd filed Critical Beijing PonyAi Science And Technology Co ltd
Priority to CN202011568934.7A priority Critical patent/CN112526571A/zh
Publication of CN112526571A publication Critical patent/CN112526571A/zh
Pending legal-status Critical Current

Links

Images

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/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本申请提供了一种位姿数据的处理方法、处理装置、处理器和车辆系统，其中，该方法包括：获取预定时间段内车辆对应的原始数据，原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据；至少采用反向卡尔曼滤波算法对原始数据进行处理，得到初始RTK数据和初始姿态数据，初始RTK数据为初始时刻的RTK数据经过反向卡尔曼滤波算法处理得到的数据；确定将初始RTK数据和初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对初始时刻的状态数据进行处理，得到滤波数据；采用RTS算法对滤波数据进行处理，得到处理后的RTK数据以及姿态数据。该方法可以得到一份较为完整的平滑轨迹。

Description

位姿数据的处理方法、处理装置、处理器和车辆系统
技术领域
本申请涉及车辆定位领域，具体而言，涉及一种位姿数据的处理方法、处理装置、计算机可读存储介质、处理器和车辆系统。
背景技术
现有技术中，通常都会对GNSS(Global Navigation Satellite System，全球导航卫星系统)/IMU(Inertial Measurement Unit，惯性测量单元)原始数据进行事后处理，以得到车辆的高精度位置和姿态，其中，GNSS Receiver是全球卫星导航接收机，IMU是惯性测量单元。目前，常规的事后处理方法中：首先对GNSS和IMU原始观测数据以及GNSS基站差分数据进行转换，转成内部的格式，然后，处理GNSS得到精密的卫星定位结果，之后，融合IMU数据同时进行正反向卡尔曼滤波处理，对正反处理完得到的轨迹进行组合，最后，对结果进行平滑处理得到用户需要的车辆的高精度位置和姿态信息。
上述的方法中，由于正向卡尔曼滤波和反向卡尔曼滤波可以同时处理也可以单独处理，然后对正反处理后的结果进行组合平滑。由于不管正向卡尔曼滤波还是反向卡尔曼滤波都需要一个初始对准的过程，当数据采集一开始就在车辆低速运动过程中或者数据采集结束也是在低速运动中，那么，处理过程只能依靠动态对准进行卡尔曼滤波的初始化(需要满足车速大于一定的阈值)，所以在车速低于这个阈值也就是在完成初始对准之前是没有任何结果的，即使使用正反向卡尔慢滤波也只能得到初始对准之前的单向推算轨迹，无法得到初始对准之前的双向推算轨迹，也无法获取初始对准之前的双向平滑轨迹，进而无法进行双方向结果组合并进行平滑处理。因此，该方法平滑轨迹并不完整，得到的结果并不精确。
因此，亟需一种可以得到完整的平滑轨迹的事后处理方法。
在背景技术部分中公开的以上信息只是用来加强对本文所描述技术的背景技术的理解，因此，背景技术中可能包含某些信息，这些信息对于本领域技术人员来说并未形成在本国已知的现有技术。
发明内容
本申请的主要目的在于提供一种位姿数据的处理方法、处理装置、计算机可读存储介质、处理器和车辆系统，以解决现有技术中的GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确的问题。
根据本发明实施例的一个方面，提供了一种位姿数据的处理方法，包括：获取预定时间段内车辆对应的原始数据，所述原始数据包括GNSS接收机输出的RTK(Real TimeKinematic，实时动态定位)数据以及IMU检测得到的姿态数据，所述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个所述位置数据、一个所述速度数据以及一个所述位置标准差数据，所述姿态数据包括多个角速度数据以及多个加速度数据；至少采用反向卡尔曼滤波算法对所述原始数据进行处理，得到初始RTK数据和初始姿态数据，所述初始RTK数据为初始时刻的所述RTK数据经过所述反向卡尔曼滤波算法处理得到的数据，所述初始姿态数据为初始时刻的所述姿态数据经过所述反向卡尔曼滤波算法处理得到的数据；确定将所述初始RTK数据和所述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对所述初始时刻的状态数据进行处理，得到滤波数据；采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据。
可选地，至少采用反向卡尔曼滤波对所述原始数据进行处理，得到初始RTK数据和初始姿态数据，包括：对所述原始数据进行预处理，得到预处理后数据，所述预处理至少包括以下之一：删除不满足第一预定条件的所述原始数据、对不满足第二预定条件的所述原始数据进行降权，所述第一预定条件为所述位置标准差在第一预定范围，所述第二预定条件为所述位置标准差在第二预定范围，所述第一预定范围和所述第二预定范围不同；采用反向卡尔曼滤波对所述预处理后数据进行处理，得到所述初始RTK数据和所述初始姿态数据。
可选地，对所述原始数据进行预处理，得到预处理后数据，包括：确定第一阈值，第二阈值和第三阈值，所述第三阈值大于等于所述第二阈值，所述第二阈值大于等于所述第一阈值；根据所述第一阈值、所述第二阈值和所述第三阈值，确定所述第一预定范围和所述第二预定范围，所述第一预定范围为大于三倍的所述第一阈值的范围，所述第二预定范围包括第一子范围、第二子范围、第三子范围和第四子范围，所述第一子范围为小于等于三倍的所述第一阈值且大于两倍的所述第一阈值的范围，所述第二子范围为大于所述第三阈值的范围，所述第三子范围为小于等于两倍的所述第一阈值且大于所述第一阈值的范围，第四子范围为小于所述第二阈值的范围；删除所述位置标准差在所述第一预定范围内的所述原始数据；对所述位置标准差在所述第二预定范围内的所述原始数据进行降权。
可选地，确定第一阈值，第二阈值和第三阈值，包括：将所述原始数据中的所有的所述位置标准差按照由小到大排列，形成预定数列；确定所述第一阈值为预定数列的前68％中的最大值；确定所述第二阈值为所述预定数列的前80％中的最大值；确定所述第三阈值为所述预定数列中的前95％中的最大值。
可选地，采用所述反向卡尔曼滤波对所述预处理后数据进行处理，得到所述初始RTK数据和所述初始姿态数据，包括：对所述预处理后数据按照时间从后至前的顺序做惯性导航初始对准；对所述预处理后数据按照时间从后至前的顺序执行惯性导航积分运算；对所述预处理后数据按照时间从后至前的顺序执行卡尔曼滤波预测及更新；在所述卡尔曼滤波算法运行到所述初始时刻的所述预处理后数据时，得到所述初始RTK数据和所述初始姿态数据。
可选地，在确定将所述初始RTK数据和所述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对所述初始时刻的状态数据进行处理，得到滤波数据之后，在采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据之前，所述方法还包括：按照时间从前到后的顺序，存储所述正向卡尔曼滤波算法对所述初始时刻的状态数据处理过程中的中间参数，所述中间参数包括卡尔曼滤波的状态向量(位置、速度、姿态、陀螺仪零偏、加速度计零偏)、状态误差协方差矩阵、状态向量的转移矩阵以及状态向量的误差。
可选地，采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据，包括：按照时间从后到前的顺序，对所述中间参数执行所述RTS算法，计算平滑增益；采用所述平滑增益按照时间从后到前的顺序修正状态向量，得到修正状态向量，所述状态向量包括：位置、速度、姿态、陀螺仪零偏以及加速度计零偏。
根据本发明实施例的另一方面，还提供了一种位姿数据的处理方法，包括：获取预定时间段内车辆对应的原始数据，所述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，所述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个所述位置数据、一个所述速度数据以及一个所述位置标准差数据，所述姿态数据包括多个角速度数据以及多个加速度数据；对所述原始数据进行预处理，得到预处理后数据，所述预处理至少包括以下之一：删除不满足第一预定条件的所述原始数据、对不满足第二预定条件的所述原始数据进行降权，所述第一预定条件为所述位置标准差在第一预定范围，所述第二预定条件为所述位置标准差在第二预定范围，所述第一预定范围和所述第二预定范围不同；采用反向卡尔曼滤波算法对所述预处理后数据进行处理，得到初始RTK数据和初始姿态数据，所述初始RTK数据为初始时刻的所述RTK数据经过所述反向卡尔曼滤波算法处理得到的数据，所述初始姿态数据为初始时刻的所述姿态数据经过所述反向卡尔曼滤波算法处理得到的数据；确定将所述初始RTK数据和所述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对所述初始时刻的状态数据进行处理，得到滤波数据；采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据。
可选地，所述原始数据进行预处理，得到预处理后数据，包括：确定第一阈值，第二阈值和第三阈值，所述第三阈值大于等于所述第二阈值，所述第二阈值大于等于所述第一阈值；根据所述第一阈值、所述第二阈值和所述第三阈值，确定所述第一预定范围和所述第二预定范围，所述第一预定范围为大于三倍的所述第一阈值的范围，所述第二预定范围包括第一子范围、第二子范围、第三子范围和第四子范围，所述第一子范围为小于等于三倍的所述第一阈值且大于两倍的所述第一阈值的范围，所述第二子范围为大于所述第三阈值的范围，所述第三子范围为小于等于两倍的所述第一阈值且大于所述第一阈值的范围，第四子范围为小于所述第二阈值的范围；删除所述位置标准差在所述第一预定范围内的所述原始数据；对所述位置标准差在所述第二预定范围内的所述原始数据进行降权。
可选地，采用所述反向卡尔曼滤波对所述预处理后数据进行处理，得到所述初始RTK数据和所述初始姿态数据，包括：对所述预处理后数据按照时间从后至前的顺序做惯性导航初始对准；对所述预处理后数据按照时间从后至前的顺序执行惯性导航积分运算；对所述预处理后数据按照时间从后至前的顺序执行卡尔曼滤波预测及更新；在所述卡尔曼滤波算法运行到所述初始时刻的所述预处理后数据时，得到所述初始RTK数据和所述初始姿态数据。
可选地，在确定将所述初始RTK数据和所述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对所述初始时刻的状态数据进行处理，得到滤波数据之后，在采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据之前，所述方法还包括：按照时间从前到后的顺序，存储所述正向卡尔曼滤波算法对所述初始时刻的状态数据处理过程中的中间参数，所述中间参数包括卡尔曼滤波的状态向量(位置、速度、姿态、陀螺仪零偏、加速度计零偏)、状态误差协方差矩阵、状态向量的转移矩阵以及状态向量的误差。
可选地，采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据，包括：按照时间从后到前的顺序，对所述中间参数执行所述RTS算法，计算平滑增益；采用所述平滑增益按照时间从后到前的顺序修正状态向量，得到修正状态向量，所述状态向量包括：位置、速度、姿态、陀螺仪零偏以及加速度计零偏。
根据本发明实施例的再一方面，还提供了一种位姿数据的处理装置，包括第一获取单元、第一处理单元、第一确定单元和第二处理单元，其中，所述第一获取单元用于获取预定时间段内车辆对应的原始数据，所述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，所述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个所述位置数据、一个所述速度数据以及一个所述位置标准差数据，所述姿态数据包括多个角速度数据以及多个加速度数据；所述第一处理单元用于至少采用反向卡尔曼滤波算法对所述原始数据进行处理，得到初始RTK数据和初始姿态数据，所述初始RTK数据为初始时刻的所述RTK数据经过所述反向卡尔曼滤波算法处理得到的数据，所述初始姿态数据为初始时刻的所述姿态数据经过所述反向卡尔曼滤波算法处理得到的数据；所述第一确定单元用于确定将所述初始RTK数据和所述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对所述初始时刻的状态数据进行处理，得到滤波数据；所述第二处理单元用于采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据。
根据本发明实施例的又一方面，还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的程序，其中，所述程序执行任意一种所述的方法。
根据本发明实施例的另一方面，还提供了一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行任意一种所述的方法。
根据本发明实施例的再一方面，还提供了一种车辆系统，包括车辆、一个或多个处理器，存储器以及一个或多个程序，其中，所述一个或多个程序被存储在所述存储器中，并且被配置为由所述一个或多个处理器执行，所述一个或多个程序包括用于执行任意一种所述的方法。
本申请的位姿数据的处理方法，首先获取预定时间段内车辆对应的原始数据，所述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据；然后至少采用反向卡尔曼滤波算法对所述原始数据进行处理，得到初始RTK数据和初始姿态数据；之后确定将所述初始RTK数据和所述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对所述初始时刻的状态数据进行处理，得到滤波数据；最后采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据。相比现有技术，本申请的所述方法，通过采用先反向卡尔曼滤波算法得到所述初始RTK数据和所述初始姿态数据，然后依据所述初始RTK数据和所述初始姿态数据可以直接完成初始对准，再进行正向卡尔曼滤波，待完成正向卡尔曼滤波算法后，再采用RTS算法进行平滑处理，这样就可以得到一份较为完整的平滑轨迹，保证了得到的所述RTK数据以及所述姿态数据较为准确，有效地缓解了现有的GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确的问题。同时，相比现有技术，所述方法不需要对GNSS原始数据进行处理，不需要记录基站差分数据，也不需要对反向卡尔曼滤波与正向卡尔曼滤波的处理结果进行组合，保证了对位姿数据的处理的速度较快，效率较高。
附图说明
构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1示出了根据本申请的实施例的位姿数据的处理方法生成的流程示意图；
图2示出了根据本申请的另一种实施例的位姿数据的处理方法生成的流程示意图；
图3示出了根据本申请的一种实施例的位姿数据的处理装置的示意图；
图4示出了根据本申请的另一种实施例的位姿数据的处理装置的示意图；
图5示出了根据现有的位姿数据的处理方法得到的轨迹示意图；
图6示出了根据本申请的实施例的位姿数据的处理方法得到的轨迹示意图；
图7示出了根据本申请的车辆系统得到的车辆轨迹示意图。
具体实施方式
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。
需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
应该理解的是，当元件(诸如层、膜、区域、或衬底)描述为在另一元件“上”时，该元件可直接在该另一元件上，或者也可存在中间元件。而且，在说明书以及权利要求书中，当描述有元件“连接”至另一元件时，该元件可“直接连接”至该另一元件，或者通过第三元件“连接”至该另一元件。
正如背景技术中所说的，现有技术中的GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确，为了解决上述问题，本申请的一种典型的实施方式中，提供了一种位姿数据的处理方法、处理装置、计算机可读存储介质、处理器和车辆系统。
根据本申请的实施例，提供了一种位姿数据的处理方法。
图1是根据本申请实施例的位姿数据的处理方法的流程图。如图1所示，该方法包括以下步骤：
步骤S101，获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，上述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个上述位置数据、一个上述速度数据以及一个上述位置标准差数据，上述姿态数据包括多个角速度数据以及多个加速度数据；
步骤S102，至少采用反向卡尔曼滤波算法对上述原始数据进行处理，得到初始RTK数据和初始姿态数据，上述初始RTK数据为初始时刻的上述RTK数据经过上述反向卡尔曼滤波算法处理得到的数据，上述初始姿态数据为初始时刻的上述姿态数据经过上述反向卡尔曼滤波算法处理得到的数据；
步骤S103，确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；
步骤S104，采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。
上述的位姿数据的处理方法，首先获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据；然后至少采用反向卡尔曼滤波算法对上述原始数据进行处理，得到初始RTK数据和初始姿态数据；之后确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；最后采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。相比现有技术，本申请的上述方法，通过采用先反向卡尔曼滤波算法得到上述初始RTK数据和上述初始姿态数据，然后依据上述初始RTK数据和上述初始姿态数据可以直接完成初始对准，再进行正向卡尔曼滤波，待完成正向卡尔曼滤波算法后，再采用RTS算法进行平滑处理，这样就可以得到一份较为完整的平滑轨迹，保证了得到的上述RTK数据以及上述姿态数据较为准确，有效地缓解了现有的GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确的问题。同时，相比现有技术，上述方法不需要对GNSS原始数据进行处理，不需要记录基站差分数据，也不需要对反向卡尔曼滤波与正向卡尔曼滤波的处理结果进行组合，保证了对位姿数据的处理的速度较快，效率较高。
图5和图6分别示出了根据现有技术和根据本申请的位姿数据的处理方法得到的车辆的平滑轨迹的示意图，由图5和图6可见，在初始对准之前，现有技术中的正反向卡尔曼滤波算法只能得到单向推算轨迹，无法得到初始对准之前的双向推算轨迹，也无法获取初始对准之前的双向平滑轨迹，根据本申请的位姿数据的处理方法，则可以得到较为完整的平滑轨迹。
根据本申请的一种具体的实施例，至少采用反向卡尔曼滤波对上述原始数据进行处理，得到初始RTK数据和初始姿态数据，包括：对上述原始数据进行预处理，得到预处理后数据，上述预处理至少包括以下之一：删除不满足第一预定条件的上述原始数据、对不满足第二预定条件的上述原始数据进行降权，上述第一预定条件为上述位置标准差在第一预定范围，上述第二预定条件为上述位置标准差在第二预定范围，上述第一预定范围和上述第二预定范围不同；采用反向卡尔曼滤波对上述预处理后数据进行处理，得到上述初始RTK数据和上述初始姿态数据。这样保证了经过处理后得到的上述初始RTK数据和上述初始姿态数据较为准确，为后续得到上述RTK数据以及上述姿态数据提供了较为准确地数据基础。
在如城市峡谷等场景下，实时获取的上述RTK数据的精度不够高，为了进一步地保证得到的上述初始RTK数据和上述初始姿态数据较为准确，根据本申请的另一种具体的实施例，对上述原始数据进行预处理，得到预处理后数据，包括：确定第一阈值，第二阈值和第三阈值，上述第三阈值大于等于上述第二阈值，上述第二阈值大于等于上述第一阈值；根据上述第一阈值、上述第二阈值和上述第三阈值，确定上述第一预定范围和上述第二预定范围，上述第一预定范围为大于三倍的上述第一阈值的范围，上述第二预定范围包括第一子范围、第二子范围、第三子范围和第四子范围，上述第一子范围为小于等于三倍的上述第一阈值且大于两倍的上述第一阈值的范围，上述第二子范围为大于上述第三阈值的范围，上述第三子范围为小于等于两倍的上述第一阈值且大于上述第一阈值的范围，第四子范围为小于上述第二阈值的范围；删除上述位置标准差在上述第一预定范围内的上述原始数据；对上述位置标准差在上述第二预定范围内的上述原始数据进行降权。这样保证了得到的上述预处理后数据比较可靠准确，为后续得到上述初始RTK数据和上述初始姿态数据提供了数据支撑。
在实际的应用过程中，确定第一阈值，第二阈值和第三阈值，包括：将上述原始数据中的所有的上述位置标准差按照由小到大排列，形成预定数列；确定上述第一阈值为预定数列的前68％中的最大值；确定上述第二阈值为上述预定数列的前80％中的最大值；确定上述第三阈值为上述预定数列中的前95％中的最大值。这样进一步地保证得到的预处理后数据较为准确，进一步地为后续得到上述初始RTK数据和上述初始姿态数据提供了较为准确的数据基础。
一种具体的实施例中，上述第一预定范围表示RTK质量非常差，上述第一子范围和上述第二子范围表示RTK质量差，上述第三子范围和上述第四子范围表示RTK质量一般，对上述位置标准差在上述第二预定范围内的上述原始数据进行降权，表示对上述原始数据中的位置标准差数据进行放大，即在初始位置标准差数据的基础上乘以一个系数，对于在不同预定范围内的上述位置标准差数据，对应的系数不同，比如，位于RTK质量一般范围内的上述位置标准差数据对应的系数为5，位于RTK质量差范围内的上述位置标准差数据对应的系数为10，这样，位于RTL质量好范围内的上述位置标准差数据，其所乘的系数较小，这样就是逐级降权。当然，上述系数还可以为其他值，上述RTK质量等级也可以按照其他范围进行划分。
本申请的再一种具体的实施例中，采用上述反向卡尔曼滤波对上述预处理后数据进行处理，得到上述初始RTK数据和上述初始姿态数据，包括：对上述预处理后数据按照时间从后至前的顺序做惯性导航初始对准；对上述预处理后数据按照时间从后至前的顺序执行惯性导航积分运算；对上述预处理后数据按照时间从后至前的顺序执行卡尔曼滤波预测及更新；在上述卡尔曼滤波算法运行到上述初始时刻的上述预处理后数据时，得到上述初始RTK数据和上述初始姿态数据。通过上述方法，进一步地保证了通过上述反向卡尔曼滤波处理，得到地上述初始RTK数据和上述初始姿态数据较为准确可靠。
根据本申请的又一种具体的实施例，在确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据之后，在采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据之前，上述方法还包括：按照时间从前到后的顺序，存储上述正向卡尔曼滤波算法对上述初始时刻的状态数据处理过程中的中间参数，上述中间参数包括卡尔曼滤波的状态向量(位置、速度、姿态、陀螺仪零偏、加速度计零偏)、状态误差协方差矩阵、状态向量的转移矩阵以及状态向量的误差。存储上述中间参数，方便了后续根据上述中间参数，得到处理后的上述RTK数据以及上述姿态数据。
上述中间参数还可以包括其他数据，具体地一种实施例中，上述中间参数还包括卡尔曼滤波的预测状态向量和状态误差的更新协方差矩阵。
在实际的应用过程中，采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据，包括：按照时间从后到前的顺序，对上述中间参数执行上述RTS算法，计算平滑增益；采用上述平滑增益按照时间从后到前的顺序修正状态向量，得到修正状态向量，上述状态向量包括：位置、速度、姿态、陀螺仪零偏以及加速度计零偏。这样进一步地保证了可以较为准确的得到处理后的上述RTK数据以及上述姿态数据，进一步地保证了后续得到的上述RTK数据以及上述姿态数据较为准确，进一步地缓解了现有的GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确的问题。
具体的，上述RTS(Rauch-Tung-Striebel)算法的原理如下：
Figure BDA0002861935530000081
Figure BDA0002861935530000091
其中，Ak就是每次迭代的平滑增益。
Figure BDA0002861935530000092
Figure BDA0002861935530000093
是正向卡尔曼滤波算法保存下来的k时刻的更新的状态向量误差。
Figure BDA0002861935530000094
是正向卡尔曼滤波算法保存下来的k时刻到k+1时刻的预测的状态向量误差。
Pk|k是正向卡尔曼滤波算法保存下来的k时刻更新的状态向量误差的协方差矩阵。
Pk+1|k是正向卡尔曼滤波算法保存下来的k时刻到k+1时刻的预测的状态向量误差的协方差矩阵。
Φk是正向卡尔曼滤波算法保存下来的k时刻的状态向量误差的转移矩阵。
根据本申请的另一种典型的实施例，还提供了一种位姿数据的处理方法，如图2所示，所示方法包括如下步骤：
步骤S201，获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，上述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个上述位置数据、一个上述速度数据以及一个上述位置标准差数据，上述姿态数据包括多个角速度数据以及多个加速度数据；
步骤S202，对上述原始数据进行预处理，得到预处理后数据，上述预处理至少包括以下之一：删除不满足第一预定条件的上述原始数据、对不满足第二预定条件的上述原始数据进行降权，上述第一预定条件为上述位置标准差在第一预定范围，上述第二预定条件为上述位置标准差在第二预定范围，上述第一预定范围和上述第二预定范围不同；
步骤S203，采用反向卡尔曼滤波算法对上述预处理后数据进行处理，得到初始RTK数据和初始姿态数据，上述初始RTK数据为初始时刻的上述RTK数据经过上述反向卡尔曼滤波算法处理得到的数据，上述初始姿态数据为初始时刻的上述姿态数据经过上述反向卡尔曼滤波算法处理得到的数据；
步骤S204，确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；
步骤S205，采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。
上述的位姿数据的处理方法，首先获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据；然后对上述原始数据进行预处理，得到预处理后数据；之后采用反向卡尔曼滤波算法对上述预处理后数据进行处理，得到初始RTK数据和初始姿态数据；再确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；最后采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。相比现有技术，本申请的上述方法，通过采用先反向卡尔曼滤波算法得到上述初始RTK数据和上述初始姿态数据，然后依据上述初始RTK数据和上述初始姿态数据可以直接完成初始对准，再进行正向卡尔曼滤波，待完成正向卡尔曼滤波算法后，再采用RTS算法进行平滑处理，这样就可以得到一份较为完整的平滑轨迹，保证了得到的上述RTK数据以及上述姿态数据较为准确，有效地缓解了现有的GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确的问题。同时，相比现有技术，上述方法不需要对GNSS原始数据进行处理，不需要记录基站差分数据，也不需要对反向卡尔曼滤波与正向卡尔曼滤波的处理结果进行组合，保证了对位姿数据的处理的速度较快，效率较高。
图5和图6分别示出了根据现有技术和根据本申请的位姿数据的处理方法得到的车辆的平滑轨迹的示意图，由图5和图6可见，在初始对准之前，现有技术中的正反向卡尔曼滤波算法只能得到单向推算轨迹，无法得到初始对准之前的双向推算轨迹，也无法获取初始对准之前的双向平滑轨迹，根据本申请的位姿数据的处理方法，则可以得到较为完整的平滑轨迹。
根据本申请的另一种具体的实施例，上述原始数据进行预处理，得到预处理后数据，包括：确定第一阈值，第二阈值和第三阈值，上述第三阈值大于等于上述第二阈值，上述第二阈值大于等于上述第一阈值；根据上述第一阈值、上述第二阈值和上述第三阈值，确定上述第一预定范围和上述第二预定范围，上述第一预定范围为大于三倍的上述第一阈值的范围，上述第二预定范围包括第一子范围、第二子范围、第三子范围和第四子范围，上述第一子范围为小于等于三倍的上述第一阈值且大于两倍的上述第一阈值的范围，上述第二子范围为大于上述第三阈值的范围，上述第三子范围为小于等于两倍的上述第一阈值且大于上述第一阈值的范围，第四子范围为小于上述第二阈值的范围；删除上述位置标准差在上述第一预定范围内的上述原始数据；对上述位置标准差在上述第二预定范围内的上述原始数据进行降权。这样保证了得到的上述预处理后数据比较可靠准确，为后续得到上述初始RTK数据和上述初始姿态数据提供了数据支撑。
在实际的应用过程中，确定第一阈值，第二阈值和第三阈值，包括：将上述原始数据中的所有的上述位置标准差按照由小到大排列，形成预定数列；确定上述第一阈值为预定数列的前68％中的最大值；确定上述第二阈值为上述预定数列的前80％中的最大值；确定上述第三阈值为上述预定数列中的前95％中的最大值。这样进一步地保证得到的预处理后数据较为准确，进一步地为后续得到上述初始RTK数据和上述初始姿态数据提供了较为准确的数据基础。
在实际的应用过程中，采用上述反向卡尔曼滤波对上述预处理后数据进行处理，得到上述初始RTK数据和上述初始姿态数据，包括：对上述预处理后数据按照时间从后至前的顺序做惯性导航初始对准；对上述预处理后数据按照时间从后至前的顺序执行惯性导航积分运算；对上述预处理后数据按照时间从后至前的顺序执行卡尔曼滤波预测及更新；在上述卡尔曼滤波算法运行到上述初始时刻的上述预处理后数据时，得到上述初始RTK数据和上述初始姿态数据。通过上述方法，进一步地保证了通过上述反向卡尔曼滤波处理，得到地上述初始RTK数据和上述初始姿态数据较为准确可靠。
根据本申请的又一种具体的实施例，在确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据之后，在采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据之前，上述方法还包括：按照时间从前到后的顺序，存储上述正向卡尔曼滤波算法对上述初始时刻的状态数据处理过程中的中间参数，上述中间参数包括卡尔曼滤波的状态向量(位置、速度、姿态、陀螺仪零偏、加速度计零偏)、状态误差协方差矩阵、状态向量的转移矩阵以及状态向量的误差。存储上述中间参数，方便了后续根据上述中间参数，得到处理后的上述RTK数据以及上述姿态数据。
上述中间参数还可以包括其他数据，具体地一种实施例中，上述中间参数还包括卡尔曼滤波的预测状态向量和状态误差的更新协方差矩阵。
本申请的再一种具体的实施例中，采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据，包括：按照时间从后到前的顺序，对上述中间参数执行上述RTS算法，计算平滑增益；采用上述平滑增益按照时间从后到前的顺序修正状态向量，得到修正状态向量，上述状态向量包括：位置、速度、姿态、陀螺仪零偏以及加速度计零偏。这样进一步地保证了可以较为准确的得到处理后的上述RTK数据以及上述姿态数据，进一步地保证了后续得到的上述RTK数据以及上述姿态数据较为准确，进一步地缓解了现有的GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确的问题。
具体的，上述RTS(Rauch-Tung-Striebel)算法的原理如下：
Figure BDA0002861935530000111
Figure BDA0002861935530000112
其中，Ak就是每次迭代的平滑增益。
Figure BDA0002861935530000113
Figure BDA0002861935530000114
是正向卡尔曼滤波算法保存下来的k时刻的更新的状态向量误差。
Figure BDA0002861935530000121
是正向卡尔曼滤波算法保存下来的k时刻到k+1时刻的预测的状态向量误差。
Pk|k是正向卡尔曼滤波算法保存下来的k时刻更新的状态向量误差的协方差矩阵。
Pk+1|k是正向卡尔曼滤波算法保存下来的k时刻到k+1时刻的预测的状态向量误差的协方差矩阵。
Φk是正向卡尔曼滤波算法保存下来的k时刻的状态向量误差的转移矩阵。
需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
本申请实施例还提供了一种位姿数据的处理装置，需要说明的是，本申请实施例的位姿数据的处理装置可以用于执行本申请实施例所提供的用于位姿数据的处理方法。以下对本申请实施例提供的位姿数据的处理装置进行介绍。
图3是根据本申请实施例的位姿数据的处理装置的示意图。如图3所示，该装置包括：第一获取单元10、第一处理单元20、第一确定单元30和第二处理单元40，其中，上述第一获取单元10用于获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，上述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个上述位置数据、一个上述速度数据以及一个上述位置标准差数据，上述姿态数据包括多个角速度数据以及多个加速度数据；上述第一处理单元20用于至少采用反向卡尔曼滤波算法对上述原始数据进行处理，得到初始RTK数据和初始姿态数据，上述初始RTK数据为初始时刻的上述RTK数据经过上述反向卡尔曼滤波算法处理得到的数据，上述初始姿态数据为初始时刻的上述姿态数据经过上述反向卡尔曼滤波算法处理得到的数据；上述第一确定单元30用于确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；上述第二处理单元40用于采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。
上述的位姿数据的处理装置，通过上述第一获取单元获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据；通过上述第一处理单元至少采用反向卡尔曼滤波算法对上述原始数据进行处理，得到初始RTK数据和初始姿态数据，通过上述第一确定单元确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；通过上述第二处理单元采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。相比现有技术，本申请的上述装置，通过采用先反向卡尔曼滤波算法得到上述初始RTK数据和上述初始姿态数据，然后依据上述初始RTK数据和上述初始姿态数据可以直接完成初始对准，再进行正向卡尔曼滤波，待完成正向卡尔曼滤波算法后，再采用RTS算法进行平滑处理，这样就可以得到一份较为完整的平滑轨迹，保证了得到的上述RTK数据以及上述姿态数据较为准确，有效地缓解了现有的GNSS/IMU原始数据的事后处理装置难以得到完整的平滑轨迹而导致得到的结果不准确的问题。同时，相比现有技术，上述装置不需要对GNSS原始数据进行处理，不需要记录基站差分数据，也不需要对反向卡尔曼滤波与正向卡尔曼滤波的处理结果进行组合，保证了对位姿数据的处理的速度较快，效率较高。
根据本申请的一种具体的实施例，上述第一处理单元包括第一预处理模块和第一处理模块，其中，上述第一预处理模块用于对上述原始数据进行预处理，得到预处理后数据，上述预处理至少包括以下之一：删除不满足第一预定条件的上述原始数据、对不满足第二预定条件的上述原始数据进行降权，上述第一预定条件为上述位置标准差在第一预定范围，上述第二预定条件为上述位置标准差在第二预定范围，上述第一预定范围和上述第二预定范围不同；上述第一处理模块用于采用反向卡尔曼滤波对上述预处理后数据进行处理，得到上述初始RTK数据和上述初始姿态数据。这样保证了经过处理后得到的上述初始RTK数据和上述初始姿态数据较为准确，为后续得到上述RTK数据以及上述姿态数据提供了较为准确地数据基础。
在如城市峡谷等场景下，实时获取的上述RTK数据的精度不够高，为了进一步地保证得到的上述初始RTK数据和上述初始姿态数据较为准确，根据本申请的另一种具体的实施例，上述第一预处理模块包括第一确定子模块、第二确定子模块、删除子模块和降权子模块，其中，上述第一确定子模块用于确定第一阈值，第二阈值和第三阈值，上述第三阈值大于等于上述第二阈值，上述第二阈值大于等于上述第一阈值；上述第二确定子模块用于根据上述第一阈值、上述第二阈值和上述第三阈值，确定上述第一预定范围和上述第二预定范围，上述第一预定范围为大于三倍的上述第一阈值的范围，上述第二预定范围包括第一子范围、第二子范围、第三子范围和第四子范围，上述第一子范围为小于等于三倍的上述第一阈值且大于两倍的上述第一阈值的范围，上述第二子范围为大于上述第三阈值的范围，上述第三子范围为小于等于两倍的上述第一阈值且大于上述第一阈值的范围，第四子范围为小于上述第二阈值的范围；上述删除子模块用于删除上述位置标准差在上述第一预定范围内的上述原始数据；上述降权子模块用于对上述位置标准差在上述第二预定范围内的上述原始数据进行降权。这样保证了得到的上述预处理后数据比较可靠准确，为后续得到上述初始RTK数据和上述初始姿态数据提供了数据支撑。
在实际的应用过程中，上述第一确定子模块还用于将上述原始数据中的所有的上述位置标准差按照由小到大排列，形成预定数列；确定上述第一阈值为预定数列的前68％中的最大值；确定上述第二阈值为上述预定数列的前80％中的最大值；确定上述第三阈值为上述预定数列中的前95％中的最大值。这样进一步地保证得到的预处理后数据较为准确，进一步地为后续得到上述初始RTK数据和上述初始姿态数据提供了较为准确的数据基础。
一种具体的实施例中，上述第一预定范围表示RTK质量非常差，上述第一子范围和上述第二子范围表示RTK质量差，上述第三子范围和上述第四子范围表示RTK质量一般，对上述位置标准差在上述第二预定范围内的上述原始数据进行降权，表示对上述原始数据中的位置标准差数据进行放大，即在初始位置标准差数据的基础上乘以一个系数，对于在不同预定范围内的上述位置标准差数据，对应的系数不同，比如，位于RTK质量一般范围内的上述位置标准差数据对应的系数为5，位于RTK质量差范围内的上述位置标准差数据对应的系数为10，这样，位于RTL质量好范围内的上述位置标准差数据，其所乘的系数较小，这样就是逐级降权。当然，上述系数还可以为其他值，上述RTK质量等级也可以按照其他范围进行划分。
本申请的再一种具体的实施例中，上述第一处理模块包括对准子模块、运算子模块、更新子模块和得到子模块，其中，上述对准子模块用于对上述预处理后数据按照时间从后至前的顺序做惯性导航初始对准；上述运算子模块用于对上述预处理后数据按照时间从后至前的顺序执行惯性导航积分运算；上述更新子模块用于对上述预处理后数据按照时间从后至前的顺序执行卡尔曼滤波预测及更新；上述得到子模块用于在上述卡尔曼滤波算法运行到上述初始时刻的上述预处理后数据时，得到上述初始RTK数据和上述初始姿态数据。通过上述装置，进一步地保证了通过上述反向卡尔曼滤波处理，得到地上述初始RTK数据和上述初始姿态数据较为准确可靠。
根据本申请的又一种具体的实施例，上述装置还包括第一存储单元，上述第一存储单元用于在确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据之后，在采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据之前，按照时间从前到后的顺序，存储上述正向卡尔曼滤波算法对上述初始时刻的状态数据处理过程中的中间参数，上述中间参数包括卡尔曼滤波的状态向量(位置、速度、姿态、陀螺仪零偏、加速度计零偏)、状态误差协方差矩阵、状态向量的转移矩阵以及状态向量的误差。存储上述中间参数，方便了后续根据上述中间参数，得到处理后的上述RTK数据以及上述姿态数据。
上述中间参数还可以包括其他数据，具体地一种实施例中，上述中间参数还包括卡尔曼滤波的预测状态向量和状态误差的更新协方差矩阵。
在实际的应用过程中，上述第二处理单元包括第一执行模块和第一修正模块，其中，上述第一执行模块用于按照时间从后到前的顺序，对上述中间参数执行上述RTS算法，计算平滑增益；上述第一修正模块用于采用上述平滑增益按照时间从后到前的顺序修正状态向量，得到修正状态向量，上述状态向量包括：位置、速度、姿态、陀螺仪零偏以及加速度计零偏。这样进一步地保证了可以较为准确的得到处理后的上述RTK数据以及上述姿态数据，进一步地保证了后续得到的上述RTK数据以及上述姿态数据较为准确，进一步地缓解了现有的GNSS/IMU原始数据的事后处理装置难以得到完整的平滑轨迹而导致得到的结果不准确的问题。
具体的，上述RTS(Rauch-Tung-Striebel)算法的原理如下：
Figure BDA0002861935530000141
Figure BDA0002861935530000151
其中，Ak就是每次迭代的平滑增益。
Figure BDA0002861935530000152
Figure BDA0002861935530000153
是正向卡尔曼滤波算法保存下来的k时刻的更新的状态向量误差。
Figure BDA0002861935530000154
是正向卡尔曼滤波算法保存下来的k时刻到k+1时刻的预测的状态向量误差。
Pk|k是正向卡尔曼滤波算法保存下来的k时刻更新的状态向量误差的协方差矩阵。
Pk+1|k是正向卡尔曼滤波算法保存下来的k时刻到k+1时刻的预测的状态向量误差的协方差矩阵。
Φk是正向卡尔曼滤波算法保存下来的k时刻的状态向量误差的转移矩阵。
本申请实施例还提供了一种位姿数据的处理装置，需要说明的是，本申请实施例的位姿数据的处理装置可以用于执行本申请实施例所提供的用于位姿数据的处理方法。以下对本申请实施例提供的位姿数据的处理装置进行介绍。
图4是根据本申请实施例的位姿数据的处理装置的示意图。如图4所示，该装置包括：第二获取单元50、预处理单元60、第三处理单元70、第二确定单元80和第四处理单元90，其中，上述第二获取单元50用于获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，上述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个上述位置数据、一个上述速度数据以及一个上述位置标准差数据，上述姿态数据包括多个角速度数据以及多个加速度数据；上述预处理单元60用于对上述原始数据进行预处理，得到预处理后数据，上述预处理至少包括以下之一：删除不满足第一预定条件的上述原始数据、对不满足第二预定条件的上述原始数据进行降权，上述第一预定条件为上述位置标准差在第一预定范围，上述第二预定条件为上述位置标准差在第二预定范围，上述第一预定范围和上述第二预定范围不同；上述第三处理单元70用于采用反向卡尔曼滤波算法对上述预处理后数据进行处理，得到初始RTK数据和初始姿态数据，上述初始RTK数据为初始时刻的上述RTK数据经过上述反向卡尔曼滤波算法处理得到的数据，上述初始姿态数据为初始时刻的上述姿态数据经过上述反向卡尔曼滤波算法处理得到的数据；上述第二确定单元80用于确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；上述第四处理单元90用于采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。
上述的位姿数据的处理装置，通过上述第二获取单元获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，通过上述预处理单元对上述原始数据进行预处理，得到预处理后数据，通过上述第三处理单元采用反向卡尔曼滤波算法对上述预处理后数据进行处理，得到初始RTK数据和初始姿态数据，通过上述第二确定单元确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据，通过上述第四处理单元采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。相比现有技术，本申请的上述装置，通过采用先反向卡尔曼滤波算法得到上述初始RTK数据和上述初始姿态数据，然后依据上述初始RTK数据和上述初始姿态数据可以直接完成初始对准，再进行正向卡尔曼滤波，待完成正向卡尔曼滤波算法后，再采用RTS算法进行平滑处理，这样就可以得到一份较为完整的平滑轨迹，保证了得到的上述RTK数据以及上述姿态数据较为准确，有效地缓解了现有的GNSS/IMU原始数据的事后处理装置难以得到完整的平滑轨迹而导致得到的结果不准确的问题。同时，相比现有技术，上述装置不需要对GNSS原始数据进行处理，不需要记录基站差分数据，也不需要对反向卡尔曼滤波与正向卡尔曼滤波的处理结果进行组合，保证了对位姿数据的处理的速度较快，效率较高。
根据本申请的另一种具体的实施例，上述预处理单元包括第一确定模块、第二确定模块、删除模块和降权模块，其中，上述第一确定模块用于确定第一阈值，第二阈值和第三阈值，上述第三阈值大于等于上述第二阈值，上述第二阈值大于等于上述第一阈值；上述第二确定模块用于根据上述第一阈值、上述第二阈值和上述第三阈值，确定上述第一预定范围和上述第二预定范围，上述第一预定范围为大于三倍的上述第一阈值的范围，上述第二预定范围包括第一子范围、第二子范围、第三子范围和第四子范围，上述第一子范围为小于等于三倍的上述第一阈值且大于两倍的上述第一阈值的范围，上述第二子范围为大于上述第三阈值的范围，上述第三子范围为小于等于两倍的上述第一阈值且大于上述第一阈值的范围，第四子范围为小于上述第二阈值的范围；上述删除模块用于删除上述位置标准差在上述第一预定范围内的上述原始数据；上述降权模块用于对上述位置标准差在上述第二预定范围内的上述原始数据进行降权。这样保证了得到的上述预处理后数据比较可靠准确，为后续得到上述初始RTK数据和上述初始姿态数据提供了数据支撑。
在实际的应用过程中，上述第一确定模块包括排列子模块、第三确定子模块、第四确定子模块和第五确定子模块，其中，上述排列子模块用于将上述原始数据中的所有的上述位置标准差按照由小到大排列，形成预定数列；上述第三确定子模块用于确定上述第一阈值为预定数列的前68％中的最大值；上述第四确定子模块用于确定上述第二阈值为上述预定数列的前80％中的最大值；上述第五确定子模块用于确定上述第三阈值为上述预定数列中的前95％中的最大值。这样进一步地保证得到的预处理后数据较为准确，进一步地为后续得到上述初始RTK数据和上述初始姿态数据提供了较为准确的数据基础。
在实际的应用过程中，上述第三处理单元包括对准模块、运算模块、更新模块和得到模块，其中，上述对准模块用于对上述预处理后数据按照时间从后至前的顺序做惯性导航初始对准；上述运算模块用于对上述预处理后数据按照时间从后至前的顺序执行惯性导航积分运算；上述更新模块用于对上述预处理后数据按照时间从后至前的顺序执行卡尔曼滤波预测及更新；上述得到模块用于在上述卡尔曼滤波算法运行到上述初始时刻的上述预处理后数据时，得到上述初始RTK数据和上述初始姿态数据。通过上述装置，进一步地保证了通过上述反向卡尔曼滤波处理，得到地上述初始RTK数据和上述初始姿态数据较为准确可靠。
根据本申请的又一种具体的实施例，上述装置还包括第二存储单元，上述第二存储单元用于在确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据之后，按照时间从前到后的顺序，存储上述正向卡尔曼滤波算法对上述初始时刻的状态数据处理过程中的中间参数，上述中间参数包括卡尔曼滤波的状态向量(位置、速度、姿态、陀螺仪零偏、加速度计零偏)、状态误差协方差矩阵、状态向量的转移矩阵以及状态向量的误差。存储上述中间参数，方便了后续根据上述中间参数，得到处理后的上述RTK数据以及上述姿态数据。
上述中间参数还可以包括其他数据，具体地一种实施例中，上述中间参数还包括卡尔曼滤波的预测状态向量和状态误差的更新协方差矩阵。
本申请的再一种具体的实施例中，上述第四处理单元包括第二执行模块和第二修正模块，其中，上述第二执行模块用于按照时间从后到前的顺序，对上述中间参数执行上述RTS算法，计算平滑增益；上述第二修正模块用于采用上述平滑增益按照时间从后到前的顺序修正状态向量，得到修正状态向量，上述状态向量包括：位置、速度、姿态、陀螺仪零偏以及加速度计零偏。这样进一步地保证了可以较为准确的得到处理后的上述RTK数据以及上述姿态数据，进一步地保证了后续得到的上述RTK数据以及上述姿态数据较为准确，进一步地缓解了现有的GNSS/IMU原始数据的事后处理装置难以得到完整的平滑轨迹而导致得到的结果不准确的问题。
具体的，上述RTS(Rauch-Tung-Striebel)算法的原理如下：
Figure BDA0002861935530000171
Figure BDA0002861935530000172
其中，Ak就是每次迭代的平滑增益。
Figure BDA0002861935530000173
Figure BDA0002861935530000174
是正向卡尔曼滤波算法保存下来的k时刻的更新的状态向量误差。
Figure BDA0002861935530000175
是正向卡尔曼滤波算法保存下来的k时刻到k+1时刻的预测的状态向量误差。
Pk|k是正向卡尔曼滤波算法保存下来的k时刻更新的状态向量误差的协方差矩阵。
Pk+1|k是正向卡尔曼滤波算法保存下来的k时刻到k+1时刻的预测的状态向量误差的协方差矩阵。
Φk是正向卡尔曼滤波算法保存下来的k时刻的状态向量误差的转移矩阵。
需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
上述位姿数据的处理装置包括处理器和存储器，上述第一获取单元、上述第一处理单元、上述第一确定单元和上述第二处理单元等均作为程序单元存储在存储器中，由处理器执行存储在存储器中的上述程序单元来实现相应的功能。
处理器中包含内核，由内核去存储器中调取相应的程序单元。内核可以设置一个或以上，通过调整内核参数来解决现有技术中GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确的问题。
存储器可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)，存储器包括至少一个存储芯片。
本发明实施例提供了一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时实现上述位姿数据的处理方法。
本发明实施例提供了一种处理器，上述处理器用于运行程序，其中，上述程序运行时执行上述位姿数据的处理方法。
本发明实施例提供了一种设备，设备包括处理器、存储器及存储在存储器上并可在处理器上运行的程序，处理器执行程序时实现至少以下步骤：
步骤S101，获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，上述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个上述位置数据、一个上述速度数据以及一个上述位置标准差数据，上述姿态数据包括多个角速度数据以及多个加速度数据；
步骤S102，至少采用反向卡尔曼滤波算法对上述原始数据进行处理，得到初始RTK数据和初始姿态数据，上述初始RTK数据为初始时刻的上述RTK数据经过上述反向卡尔曼滤波算法处理得到的数据，上述初始姿态数据为初始时刻的上述姿态数据经过上述反向卡尔曼滤波算法处理得到的数据；
步骤S103，确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；
步骤S104，采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。
处理器执行程序时还可以实现至少以下步骤：
步骤S201，获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，上述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个上述位置数据、一个上述速度数据以及一个上述位置标准差数据，上述姿态数据包括多个角速度数据以及多个加速度数据；
步骤S202，对上述原始数据进行预处理，得到预处理后数据，上述预处理至少包括以下之一：删除不满足第一预定条件的上述原始数据、对不满足第二预定条件的上述原始数据进行降权，上述第一预定条件为上述位置标准差在第一预定范围，上述第二预定条件为上述位置标准差在第二预定范围，上述第一预定范围和上述第二预定范围不同；
步骤S203，采用反向卡尔曼滤波算法对上述预处理后数据进行处理，得到初始RTK数据和初始姿态数据，上述初始RTK数据为初始时刻的上述RTK数据经过上述反向卡尔曼滤波算法处理得到的数据，上述初始姿态数据为初始时刻的上述姿态数据经过上述反向卡尔曼滤波算法处理得到的数据；
步骤S204，确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；
步骤S205，采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。
本文中的设备可以是服务器、PC、PAD、手机等。
本申请还提供了一种计算机程序产品，当在数据处理设备上执行时，适于执行初始化有至少如下方法步骤的程序：
步骤S101，获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，上述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个上述位置数据、一个上述速度数据以及一个上述位置标准差数据，上述姿态数据包括多个角速度数据以及多个加速度数据；
步骤S102，至少采用反向卡尔曼滤波算法对上述原始数据进行处理，得到初始RTK数据和初始姿态数据，上述初始RTK数据为初始时刻的上述RTK数据经过上述反向卡尔曼滤波算法处理得到的数据，上述初始姿态数据为初始时刻的上述姿态数据经过上述反向卡尔曼滤波算法处理得到的数据；
步骤S103，确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；
步骤S104，采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。
适于执行初始化话可以有至少如下方法步骤的程序：
步骤S201，获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，上述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个上述位置数据、一个上述速度数据以及一个上述位置标准差数据，上述姿态数据包括多个角速度数据以及多个加速度数据；
步骤S202，对上述原始数据进行预处理，得到预处理后数据，上述预处理至少包括以下之一：删除不满足第一预定条件的上述原始数据、对不满足第二预定条件的上述原始数据进行降权，上述第一预定条件为上述位置标准差在第一预定范围，上述第二预定条件为上述位置标准差在第二预定范围，上述第一预定范围和上述第二预定范围不同；
步骤S203，采用反向卡尔曼滤波算法对上述预处理后数据进行处理，得到初始RTK数据和初始姿态数据，上述初始RTK数据为初始时刻的上述RTK数据经过上述反向卡尔曼滤波算法处理得到的数据，上述初始姿态数据为初始时刻的上述姿态数据经过上述反向卡尔曼滤波算法处理得到的数据；
步骤S204，确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；
步骤S205，采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。
根据本申请的再一种典型的实施例，还提供了一种车辆系统，包括车辆、一个或多个处理器，存储器以及一个或多个程序，其中，上述一个或多个程序被存储在上述存储器中，并且被配置为由上述一个或多个处理器执行，上述一个或多个程序包括用于执行任意一种上述的方法。
上述的车辆系统，包括车辆、一个或多个处理器，存储器以及一个或多个程序，上述一个或多个程序包括用于执行任意一种上述的方法。相比现有技术，本申请的车辆系统，通过采用先反向卡尔曼滤波算法得到上述初始RTK数据和上述初始姿态数据，然后依据上述初始RTK数据和上述初始姿态数据可以直接完成初始对准，再进行正向卡尔曼滤波，待完成正向卡尔曼滤波算法后，再采用RTS算法进行平滑处理，这样就可以得到一份较为完整的平滑轨迹，保证了得到的上述RTK数据以及上述姿态数据较为准确，有效地缓解了现有的GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确的问题。同时，相比现有技术，上述车辆系统不需要对GNSS原始数据进行处理，不需要记录基站差分数据，也不需要对反向卡尔曼滤波与正向卡尔曼滤波的处理结果进行组合，保证了对位姿数据的处理的速度较快，效率较高。
图7示出了根据本申请的车辆系统得到的车辆轨迹示意图。其中，A点为本申请的车辆系统的轨迹开始位置，B点为现有技术中的车辆的轨迹开始位置，采用现有技术对无人车地图采集数据进行事后处理，获取到的车辆高精度轨迹会在数据采集的一开始有不同程度的轨迹缺失(缺失的时间长度大概为0-10s)，而采用本申请的处理方法可以得到从数据采集一开始的所有高精度轨迹。
在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如上述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。
上述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
上述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例上述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
从以上的描述中，可以看出，本申请上述的实施例实现了如下技术效果：
1)、本申请的位姿数据的处理方法，首先获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据；然后至少采用反向卡尔曼滤波算法对上述原始数据进行处理，得到初始RTK数据和初始姿态数据；之后确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；最后采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。相比现有技术，本申请的上述方法，通过采用先反向卡尔曼滤波算法得到上述初始RTK数据和上述初始姿态数据，然后依据上述初始RTK数据和上述初始姿态数据可以直接完成初始对准，再进行正向卡尔曼滤波，待完成正向卡尔曼滤波算法后，再采用RTS算法进行平滑处理，这样就可以得到一份较为完整的平滑轨迹，保证了得到的上述RTK数据以及上述姿态数据较为准确，有效地缓解了现有的GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确的问题。同时，相比现有技术，上述方法不需要对GNSS原始数据进行处理，不需要记录基站差分数据，也不需要对反向卡尔曼滤波与正向卡尔曼滤波的处理结果进行组合，保证了对位姿数据的处理的速度较快，效率较高。
2)、本申请的位姿数据的处理方法，首先获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据；然后对上述原始数据进行预处理，得到预处理后数据；之后采用反向卡尔曼滤波算法对上述预处理后数据进行处理，得到初始RTK数据和初始姿态数据；再确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；最后采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。相比现有技术，本申请的上述方法，通过采用先反向卡尔曼滤波算法得到上述初始RTK数据和上述初始姿态数据，然后依据上述初始RTK数据和上述初始姿态数据可以直接完成初始对准，再进行正向卡尔曼滤波，待完成正向卡尔曼滤波算法后，再采用RTS算法进行平滑处理，这样就可以得到一份较为完整的平滑轨迹，保证了得到的上述RTK数据以及上述姿态数据较为准确，有效地缓解了现有的GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确的问题。同时，相比现有技术，上述方法不需要对GNSS原始数据进行处理，不需要记录基站差分数据，也不需要对反向卡尔曼滤波与正向卡尔曼滤波的处理结果进行组合，保证了对位姿数据的处理的速度较快，效率较高。
3)、本申请的位姿数据的处理装置，通过上述第一获取单元获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据；通过上述第一处理单元至少采用反向卡尔曼滤波算法对上述原始数据进行处理，得到初始RTK数据和初始姿态数据，通过上述第一确定单元确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据；通过上述第二处理单元采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。相比现有技术，本申请的上述装置，通过采用先反向卡尔曼滤波算法得到上述初始RTK数据和上述初始姿态数据，然后依据上述初始RTK数据和上述初始姿态数据可以直接完成初始对准，再进行正向卡尔曼滤波，待完成正向卡尔曼滤波算法后，再采用RTS算法进行平滑处理，这样就可以得到一份较为完整的平滑轨迹，保证了得到的上述RTK数据以及上述姿态数据较为准确，有效地缓解了现有的GNSS/IMU原始数据的事后处理装置难以得到完整的平滑轨迹而导致得到的结果不准确的问题。同时，相比现有技术，上述装置不需要对GNSS原始数据进行处理，不需要记录基站差分数据，也不需要对反向卡尔曼滤波与正向卡尔曼滤波的处理结果进行组合，保证了对位姿数据的处理的速度较快，效率较高。
4)、本申请的位姿数据的处理装置，通过上述第二获取单元获取预定时间段内车辆对应的原始数据，上述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，通过上述预处理单元对上述原始数据进行预处理，得到预处理后数据，通过上述第三处理单元采用反向卡尔曼滤波算法对上述预处理后数据进行处理，得到初始RTK数据和初始姿态数据，通过上述第二确定单元确定将上述初始RTK数据和上述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对上述初始时刻的状态数据进行处理，得到滤波数据，通过上述第四处理单元采用RTS算法对上述滤波数据进行处理，得到处理后的上述RTK数据以及上述姿态数据。相比现有技术，本申请的上述装置，通过采用先反向卡尔曼滤波算法得到上述初始RTK数据和上述初始姿态数据，然后依据上述初始RTK数据和上述初始姿态数据可以直接完成初始对准，再进行正向卡尔曼滤波，待完成正向卡尔曼滤波算法后，再采用RTS算法进行平滑处理，这样就可以得到一份较为完整的平滑轨迹，保证了得到的上述RTK数据以及上述姿态数据较为准确，有效地缓解了现有的GNSS/IMU原始数据的事后处理装置难以得到完整的平滑轨迹而导致得到的结果不准确的问题。同时，相比现有技术，上述装置不需要对GNSS原始数据进行处理，不需要记录基站差分数据，也不需要对反向卡尔曼滤波与正向卡尔曼滤波的处理结果进行组合，保证了对位姿数据的处理的速度较快，效率较高。
5)、本申请的车辆系统，包括车辆、一个或多个处理器，存储器以及一个或多个程序，上述一个或多个程序包括用于执行任意一种上述的方法。相比现有技术，本申请的车辆系统，通过采用先反向卡尔曼滤波算法得到上述初始RTK数据和上述初始姿态数据，然后依据上述初始RTK数据和上述初始姿态数据可以直接完成初始对准，再进行正向卡尔曼滤波，待完成正向卡尔曼滤波算法后，再采用RTS算法进行平滑处理，这样就可以得到一份较为完整的平滑轨迹，保证了得到的上述RTK数据以及上述姿态数据较为准确，有效地缓解了现有的GNSS/IMU原始数据的事后处理方法难以得到完整的平滑轨迹而导致得到的结果不准确的问题。同时，相比现有技术，上述车辆系统不需要对GNSS原始数据进行处理，不需要记录基站差分数据，也不需要对反向卡尔曼滤波与正向卡尔曼滤波的处理结果进行组合，保证了对位姿数据的处理的速度较快，效率较高。
以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (16)

1.一种位姿数据的处理方法，其特征在于，包括：
获取预定时间段内车辆对应的原始数据，所述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，所述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个所述位置数据、一个所述速度数据以及一个所述位置标准差数据，所述姿态数据包括多个角速度数据以及多个加速度数据，所述姿态数据包括多个角速度数据以及多个加速度数据；
至少采用反向卡尔曼滤波算法对所述原始数据进行处理，得到初始RTK数据和初始姿态数据，所述初始RTK数据为初始时刻的所述RTK数据经过所述反向卡尔曼滤波算法处理得到的数据，所述初始姿态数据为初始时刻的所述姿态数据经过所述反向卡尔曼滤波算法处理得到的数据；
确定将所述初始RTK数据和所述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对所述初始时刻的状态数据进行处理，得到滤波数据；
采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据。
2.根据权利要求1所述的方法，其特征在于，至少采用反向卡尔曼滤波对所述原始数据进行处理，得到初始RTK数据和初始姿态数据，包括：
对所述原始数据进行预处理，得到预处理后数据，所述预处理至少包括以下之一：删除不满足第一预定条件的所述原始数据、对不满足第二预定条件的所述原始数据进行降权，所述第一预定条件为所述位置标准差在第一预定范围，所述第二预定条件为所述位置标准差在第二预定范围，所述第一预定范围和所述第二预定范围不同；
采用反向卡尔曼滤波对所述预处理后数据进行处理，得到所述初始RTK数据和所述初始姿态数据。
3.根据权利要求2所述的方法，其特征在于，对所述原始数据进行预处理，得到预处理后数据，包括：
确定第一阈值，第二阈值和第三阈值，所述第三阈值大于等于所述第二阈值，所述第二阈值大于等于所述第一阈值；
根据所述第一阈值、所述第二阈值和所述第三阈值，确定所述第一预定范围和所述第二预定范围，所述第一预定范围为大于三倍的所述第一阈值的范围，所述第二预定范围包括第一子范围、第二子范围、第三子范围和第四子范围，所述第一子范围为小于等于三倍的所述第一阈值且大于两倍的所述第一阈值的范围，所述第二子范围为大于所述第三阈值的范围，所述第三子范围为小于等于两倍的所述第一阈值且大于所述第一阈值的范围，第四子范围为小于所述第二阈值的范围；
删除所述位置标准差在所述第一预定范围内的所述原始数据；
对所述位置标准差在所述第二预定范围内的所述原始数据进行降权。
4.根据权利要求3所述的方法，其特征在于，确定第一阈值，第二阈值和第三阈值，包括：
将所述原始数据中的所有的所述位置标准差按照由小到大排列，形成预定数列；
确定所述第一阈值为预定数列的前68％中的最大值；
确定所述第二阈值为所述预定数列的前80％中的最大值；
确定所述第三阈值为所述预定数列中的前95％中的最大值。
5.根据权利要求2所述的方法，其特征在于，采用所述反向卡尔曼滤波对所述预处理后数据进行处理，得到所述初始RTK数据和所述初始姿态数据，包括：
对所述预处理后数据按照时间从后至前的顺序做惯性导航初始对准；
对所述预处理后数据按照时间从后至前的顺序执行惯性导航积分运算；
对所述预处理后数据按照时间从后至前的顺序执行卡尔曼滤波预测及更新；
在所述卡尔曼滤波算法运行到所述初始时刻的所述预处理后数据时，得到所述初始RTK数据和所述初始姿态数据。
6.根据权利要求1至5中任一项所述的方法，其特征在于，在确定将所述初始RTK数据和所述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对所述初始时刻的状态数据进行处理，得到滤波数据之后，在采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据之前，所述方法还包括：
按照时间从前到后的顺序，存储所述正向卡尔曼滤波算法对所述初始时刻的状态数据处理过程中的中间参数，所述中间参数包括卡尔曼滤波的状态向量(位置、速度、姿态、陀螺仪零偏、加速度计零偏)、状态误差协方差矩阵、状态向量的转移矩阵以及状态向量的误差。
7.根据权利要求6所述的方法，其特征在于，采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据，包括：
按照时间从后到前的顺序，对所述中间参数执行所述RTS算法，计算平滑增益；
采用所述平滑增益按照时间从后到前的顺序修正状态向量，得到修正状态向量，所述状态向量包括：位置、速度、姿态、陀螺仪零偏以及加速度计零偏。
8.一种位姿数据的处理方法，其特征在于，包括：
获取预定时间段内车辆对应的原始数据，所述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，所述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个所述位置数据、一个所述速度数据以及一个所述位置标准差数据，所述姿态数据包括多个角速度数据以及多个加速度数据，所述姿态数据包括多个角速度数据以及多个加速度数据；
对所述原始数据进行预处理，得到预处理后数据，所述预处理至少包括以下之一：删除不满足第一预定条件的所述原始数据、对不满足第二预定条件的所述原始数据进行降权，所述第一预定条件为所述位置标准差在第一预定范围，所述第二预定条件为所述位置标准差在第二预定范围，所述第一预定范围和所述第二预定范围不同；
采用反向卡尔曼滤波算法对所述预处理后数据进行处理，得到初始RTK数据和初始姿态数据，所述初始RTK数据为初始时刻的所述RTK数据经过所述反向卡尔曼滤波算法处理得到的数据，所述初始姿态数据为初始时刻的所述姿态数据经过所述反向卡尔曼滤波算法处理得到的数据；
确定将所述初始RTK数据和所述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对所述初始时刻的状态数据进行处理，得到滤波数据；
采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据。
9.根据权利要求8所述的方法，其特征在于，所述原始数据进行预处理，得到预处理后数据，包括：
确定第一阈值，第二阈值和第三阈值，所述第三阈值大于等于所述第二阈值，所述第二阈值大于等于所述第一阈值；
根据所述第一阈值、所述第二阈值和所述第三阈值，确定所述第一预定范围和所述第二预定范围，所述第一预定范围为大于三倍的所述第一阈值的范围，所述第二预定范围包括第一子范围、第二子范围、第三子范围和第四子范围，所述第一子范围为小于等于三倍的所述第一阈值且大于两倍的所述第一阈值的范围，所述第二子范围为大于所述第三阈值的范围，所述第三子范围为小于等于两倍的所述第一阈值且大于所述第一阈值的范围，第四子范围为小于所述第二阈值的范围；
删除所述位置标准差在所述第一预定范围内的所述原始数据；
对所述位置标准差在所述第二预定范围内的所述原始数据进行降权。
10.根据权利要求8所述的方法，其特征在于，采用所述反向卡尔曼滤波对所述预处理后数据进行处理，得到所述初始RTK数据和所述初始姿态数据，包括：
对所述预处理后数据按照时间从后至前的顺序做惯性导航初始对准；
对所述预处理后数据按照时间从后至前的顺序执行惯性导航积分运算；
对所述预处理后数据按照时间从后至前的顺序执行卡尔曼滤波预测及更新；
在所述卡尔曼滤波算法运行到所述初始时刻的所述预处理后数据时，得到所述初始RTK数据和所述初始姿态数据。
11.根据权利要求8至10中任一项所述的方法，其特征在于，在确定将所述初始RTK数据和所述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对所述初始时刻的状态数据进行处理，得到滤波数据之后，在采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据之前，所述方法还包括：
按照时间从前到后的顺序，存储所述正向卡尔曼滤波算法对所述初始时刻的状态数据处理过程中的中间参数，所述中间参数包括卡尔曼滤波的状态向量(位置、速度、姿态、陀螺仪零偏、加速度计零偏)、状态误差协方差矩阵、状态向量的转移矩阵以及状态向量的误差。
12.根据权利要求11所述的方法，其特征在于，采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据，包括：
按照时间从后到前的顺序，对所述中间参数执行所述RTS算法，计算平滑增益；
采用所述平滑增益按照时间从后到前的顺序修正状态向量，得到修正状态向量，所述状态向量包括：位置、速度、姿态、陀螺仪零偏以及加速度计零偏。
13.一种位姿数据的处理装置，其特征在于，包括：
第一获取单元，用于获取预定时间段内车辆对应的原始数据，所述原始数据包括GNSS接收机输出的RTK数据以及IMU检测得到的姿态数据，所述RTK数据包括多个位置数据、多个速度数据以及多个位置标准差数据，一个时刻对应一个所述位置数据、一个所述速度数据以及一个所述位置标准差数据，所述姿态数据包括多个角速度数据以及多个加速度数据；
第一处理单元，用于至少采用反向卡尔曼滤波算法对所述原始数据进行处理，得到初始RTK数据和初始姿态数据，所述初始RTK数据为初始时刻的所述RTK数据经过所述反向卡尔曼滤波算法处理得到的数据，所述初始姿态数据为初始时刻的所述姿态数据经过所述反向卡尔曼滤波算法处理得到的数据；
第一确定单元，用于确定将所述初始RTK数据和所述初始姿态数据作为正向卡尔曼滤波算法的初始时刻的状态数据，并采用正向卡尔曼滤波算法对所述初始时刻的状态数据进行处理，得到滤波数据；
第二处理单元，用于采用RTS算法对所述滤波数据进行处理，得到处理后的所述RTK数据以及所述姿态数据。
14.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的程序，其中，所述程序执行权利要求1至12中任意一项所述的方法。
15.一种处理器，其特征在于，所述处理器用于运行程序，其中，所述程序运行时执行权利要求1至12中任意一项所述的方法。
16.一种车辆系统，其特征在于，包括：车辆、一个或多个处理器，存储器以及一个或多个程序，其中，所述一个或多个程序被存储在所述存储器中，并且被配置为由所述一个或多个处理器执行，所述一个或多个程序包括用于执行权利要求1至12中任意一项所述的方法。
CN202011568934.7A 2020-12-25 2020-12-25 位姿数据的处理方法、处理装置、处理器和车辆系统 Pending CN112526571A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011568934.7A CN112526571A (zh) 2020-12-25 2020-12-25 位姿数据的处理方法、处理装置、处理器和车辆系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011568934.7A CN112526571A (zh) 2020-12-25 2020-12-25 位姿数据的处理方法、处理装置、处理器和车辆系统

Publications (1)

Publication Number Publication Date
CN112526571A true CN112526571A (zh) 2021-03-19

Family

ID=74976657

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011568934.7A Pending CN112526571A (zh) 2020-12-25 2020-12-25 位姿数据的处理方法、处理装置、处理器和车辆系统

Country Status (1)

Country Link
CN (1) CN112526571A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113568018A (zh) * 2021-07-14 2021-10-29 广东汇天航空航天科技有限公司 定位信息的更新方法、装置、电子设备及存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105806338A (zh) * 2016-03-17 2016-07-27 孙红星 基于三向卡尔曼滤波平滑器的gnss/ins组合定位定向算法
CN107193026A (zh) * 2017-05-06 2017-09-22 千寻位置网络有限公司 伪距定位平滑方法及系统、定位终端
CN109946703A (zh) * 2019-04-10 2019-06-28 北京小马智行科技有限公司 一种传感器姿态调整方法及装置

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105806338A (zh) * 2016-03-17 2016-07-27 孙红星 基于三向卡尔曼滤波平滑器的gnss/ins组合定位定向算法
CN107193026A (zh) * 2017-05-06 2017-09-22 千寻位置网络有限公司 伪距定位平滑方法及系统、定位终端
CN109946703A (zh) * 2019-04-10 2019-06-28 北京小马智行科技有限公司 一种传感器姿态调整方法及装置

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
许西安;: "几种周跳探测和修复方法的比较研究", 科技创新与生产力, no. 06, 10 June 2017 (2017-06-10) *
许长辉: "GNSS/INS组合系统的抗差卡尔曼滤波", 中国矿业大学学报, vol. 39, no. 5, 30 September 2010 (2010-09-30), pages 1 - 6 *
魏帅;闫钧宣;滕霖;: "惯性/卫星组合导航数据后处理算法研究", 计算机仿真, no. 11, 15 November 2016 (2016-11-15) *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113568018A (zh) * 2021-07-14 2021-10-29 广东汇天航空航天科技有限公司 定位信息的更新方法、装置、电子设备及存储介质

Similar Documents

Publication Publication Date Title
US11313684B2 (en) Collaborative navigation and mapping
CN110084832B (zh) 相机位姿的纠正方法、装置、系统、设备和存储介质
CN111811506A (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
CN107888828A (zh) 空间定位方法及装置、电子设备、以及存储介质
CN112697169B (zh) 车辆定位的初始化方法、装置、处理器和车辆
CN114565863B (zh) 无人机图像的正射影像实时生成方法、装置、介质及设备
FR2695202A1 (fr) Système embarqué de navigation pour un engin aérien comportant un radar à visée latérale et à synthèse d'ouverture.
CN111932616B (zh) 一种利用并行计算加速的双目视觉惯性里程计方法
CN114136315B (zh) 一种基于单目视觉辅助惯性组合导航方法及系统
US20220398825A1 (en) Method for generating 3d reference points in a map of a scene
CN110806220A (zh) 一种惯导系统初始对准方法及装置
CN115077541A (zh) 自动驾驶车辆的定位方法、装置及电子设备、存储介质
CN112526571A (zh) 位姿数据的处理方法、处理装置、处理器和车辆系统
Han et al. Precise positioning with machine learning based Kalman filter using GNSS/IMU measurements from android smartphone
CN111383324A (zh) 点云地图的构建方法、装置、计算机设备和存储介质
CN116381760B (zh) Gnss rtk/ins紧耦合定位方法、装置及介质
CN109061705B (zh) 基于tensorflow的数据处理方法及终端、差分定位平台
CN111142133B (zh) 基于多个连续运行参考站的后处理定位方法和系统
CN116772858A (zh) 车辆定位方法、装置、定位设备及存储介质
CN114088103B (zh) 车辆定位信息的确定方法和装置
CN116045964A (zh) 高精地图更新方法及装置
CN113959433B (zh) 一种组合导航方法及装置
CN115236645A (zh) 激光雷达的姿态确定方法以及姿态确定装置
CN114690226A (zh) 基于载波相位差分技术辅助的单目视觉测距方法及系统
US20230078005A1 (en) Navigation assistance method for a mobile carrier

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