CN116931041A - 一种轨迹确定方法及装置 - Google Patents

一种轨迹确定方法及装置 Download PDF

Info

Publication number
CN116931041A
CN116931041A CN202210320326.7A CN202210320326A CN116931041A CN 116931041 A CN116931041 A CN 116931041A CN 202210320326 A CN202210320326 A CN 202210320326A CN 116931041 A CN116931041 A CN 116931041A
Authority
CN
China
Prior art keywords
gnss
vehicle
track
measurement value
objective function
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
CN202210320326.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.)
Huawei Technologies Co Ltd
Original Assignee
Huawei Technologies 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 Huawei Technologies Co Ltd filed Critical Huawei Technologies Co Ltd
Priority to CN202210320326.7A priority Critical patent/CN116931041A/zh
Publication of CN116931041A publication Critical patent/CN116931041A/zh
Pending legal-status Critical Current

Links

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/53—Determining attitude
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • 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

Landscapes

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

Abstract

本申请公开了一种轨迹确定方法及装置，该方法包括：根据车辆在多个时刻的待估计状态变量、IMU的第一数据和GNSS的第二数据构建目标函数，第二数据包括多个GNSS测量值和每个GNSS测量值对应的方差；根据第一轨迹确定多个GNSS测量值中异常的测量值，第一轨迹是基于目标函数求解车辆在多个时刻的待估计状态变量获得；更新目标函数以剔除异常的测量值和异常的测量值对应的方差；求解更新后的目标函数，获得车辆的第二轨迹。实施本申请，能识别出参与轨迹求解的GNSS测量值中异常的GNSS测量值并剔除，实现了轨迹的优化，提高了求解的轨迹的精准度。

Description

一种轨迹确定方法及装置
技术领域
本申请涉及定位领域，尤其涉及一种轨迹确定方法及装置。
背景技术
卫星信号受遮挡会影响全球导航卫星系统GNSS的定位精度。在卫星信号有遮挡的情况下，车辆常采用组合导航的方式进行导航定位，例如，将惯性测量单元IMU和GNSS进行组合导航，以提高车辆在卫星信号遮挡场景下的定位精度。
但在复杂城市道路环境下，可能出现一段区域内卫星信号都存在遮挡，导致车辆出现长时间GNSS测量异常现象，但车辆在求解轨迹时由于识别不出GNSS输出的存在偏差的测量值，导致获得的轨迹的精度低。
发明内容
本申请公开了一种轨迹确定方法和装置，能够在卫星信号遮挡导致GNSS测量长时间异常的情况下能有效识别出异常的GNSS测量值，提高了求解的轨迹的精准度。
第一方面，本申请提供了一种轨迹确定方法，该方法包括：根据车辆在多个时刻的待估计状态变量、惯性测量单元IMU的第一数据和全球导航卫星系统GNSS的第二数据构建目标函数，第二数据包括多个GNSS测量值和每个GNSS测量值对应的方差；根据第一轨迹确定这多个GNSS测量值中异常的测量值，第一轨迹是基于目标函数求解车辆在多个时刻的待估计状态变量获得；更新目标函数，以使更新后的目标函数不包括异常的测量值和异常的测量值对应的方差；求解更新后的目标函数，获得车辆的第二轨迹。
上述方法可以应用于车辆、或者车辆内的装置、部件或芯片等，例如，车载电脑、车载工业控制计算机(Industrial Personal Computer，IPC)等。
其中，车辆在多个时刻的待估计状态变量是需要求解出的变量。车辆在某时刻的待估计状态变量包括车辆在该时刻的待估计位置变量。一种实现方式中，车辆在某时刻的待估计状态变量还包括车辆在该时刻的待估计速度变量和车辆在该时刻的待估计姿态变量。
第二数据源于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的采样频率也可以称作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测量值中异常的测量值，方便后续从目标函数中剔除该异常的测量值对应的GNSS因子，有利于提高目标函数的准确率以及求解的轨迹的精准度。
可选地，该方法还包括：根据第二轨迹确定更新后的目标函数中的任一GNSS测量值无异常；将第二轨迹作为车辆的最优轨迹。
也就是说，在根据第二轨迹确定更新后的目标函数中的任一GNSS测量值无异常的情况下，将第二轨迹作为车辆的最优轨迹。
实施上述实现方式，有效提高了求解的轨迹的精准度，在卫星信号遮挡导致GNSS测量长时间异常的情况下能提高车辆的定位精度。
第二方面，本申请提供了一种轨迹确定装置，该装置包括：函数处理单元，用于根据车辆在多个时刻的待估计状态变量、惯性测量单元IMU的第一数据和全球导航卫星系统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测量值无异常；轨迹获取单元用于将第二轨迹作为车辆的最优轨迹。
第三方面，本申请提供了一种轨迹确定装置，该装置包括处理器和存储器，其中，存储器用于存储程序指令；所述处理器调用所述存储器中的程序指令，使得装置执行第一方面或者第一方面的任一可能的实现方式中的方法。
第四方面，本申请提供了一种计算机可读存储介质，包括计算机指令，当所述计算机指令在被处理器运行时，实现上述第一方面或者第一方面的任一可能的实现方式中的方法。
第五方面，本申请提供了一种计算机程序产品，当该计算机程序产品被处理器执行时，实现上述第一方面或者第一方面的任一可能的实施例中的所述方法。该计算机程序产品，例如可以为一个软件安装包，在需要使用上述第一方面的任一种可能的设计提供的方法的情况下，可以下载该计算机程序产品并在处理器上执行该计算机程序产品，以实现第一方面或者第一方面的任一可能的实施例中的所述方法。
第六方面，本申请提供了一种车辆，该车辆包括如上述第二方面或第二方面的任一可能的实现方式的轨迹确定装置，或者包括如上述第三方面或者第三方面的任一可能的轨迹确定装置。
上述第二方面至第六方面的技术效果与上述第一方面相同，在此不再赘述。
附图说明
图1是本申请实施例提供的一种系统架构的示意图；
图2是本申请实施例提供的一种轨迹确定方法的流程图；
图3是本申请实施例提供的一种多个GNSS测量值的分布示意图；
图4是本申请实施例提供的又一种多个GNSS测量值的分布示意图；
图5是本申请实施例提供的又一种轨迹确定方法的流程图；
图6是本申请实施例提供的一种轨迹确定装置的结构示意图；
图7是本申请实施例提供的一种轨迹确定装置的结构示意图。
具体实施方式
在本申请实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。本申请实施例中的说明书和权利要求书中的术语“第一”、“第二”等是用于区别不同对象，而不是用于描述特定顺序。
需要说明的是，本申请中采用诸如“第一”、“第二”的前缀词，仅仅为了区分不同的描述对象，对被描述对象的位置、顺序、优先级、数量或内容等没有任何限定作用。例如，被描述对象为“字段”，则“第一字段”和“第二字段”中“字段”之前的序数词并不限制“字段”之间的位置或顺序，“第一”和“第二”并不限制其修饰的“字段”是否在同一个消息中，也不限制“第一字段”和“第二字段”的先后顺序。再如，被描述对象为“等级”，则“第一等级”和“第二等级”中“等级”之前的序数词并不限制“等级”之间的优先级。再如，被描述对象的数量并不受前缀词的限制，可以是一个或者多个，以“第一设备”为例，其中“设备”的数量可以是一个或者多个。此外，不同前缀词修饰的对象可以相同或不同，例如，被描述对象为“设备”，则“第一设备”和“第二设备”可以是同一个设备、相同类型的设备或者不同类型的设备；再如，被描述对象为“信息”，则“第一信息”和“第二信息”可以是相同内容的信息或者不同内容的信息。总之，本申请实施例中对用于区分描述对象的前缀词的使用不构成对所描述对象的限制，对所描述对象的陈述参见权利要求或实施例中上下文的描述，不应因为使用这种前缀词而构成多余的限制。
需要说明的是，本申请实施例中采用诸如“a1、a2、……和an中的至少一项(或至少一个)”等的描述方式，包括了a1、a2、……和an中任意一个单独存在的情况，也包括了a1、a2、……和an中任意多个的任意组合情况，每种情况可以单独存在。例如，“a、b和c中的至少一项”的描述方式，包括了单独a、单独b、单独c、a和b组合、a和c组合、b和c组合，或abc三者组合的情况。
为了便于理解，下面先对本申请实施例可能涉及的相关术语等进行介绍。
(1)IMU
惯性测量单元(Inertial Measurement Unit，IMU)，也可以称作惯性传感器，它主要包括加速度计和陀螺仪这两种惯性元件，其中，加速度计用于检测车辆相对于惯性坐标系的加速度信号，陀螺仪用于检测车辆相对于惯性坐标系的角速度信号，对于检测到的加速度信号和角速度信号进行处理可计算获得车辆的位置、速度、姿态等信息。
需要说明的是，IMU提供的是一个相对的定位信息，也就是说，它测量车辆相对于起点的位移，单独使用IMU不能获得车辆的绝对位置。另外，尽管IMU的更新频率高，但IMU会受到误差累积影响，导致导航定位准确率会随着时间递增而降低，所以只能在很短的时间内时依赖IMU进行定位。因此，IMU通常与全球定位系统(Global Positioning System，GPS)或GNSS等配合一起使用，称为组合惯导或组合导航。
(2)GNSS
全球导航卫星系统(Global Navigation Satellite System，GNSS)，它泛指所有的卫星导航系统，包括全球的、区域的和增强的，如美国的GPS、俄罗斯的Glonass、欧洲的Galileo、中国的北斗卫星导航系统，以及相关的增强系统，如美国的广域增强系统(WideArea Augmentation System，WAAS)、欧洲静地导航重叠服务(European GeostationaryNavigation Overlay Service，EGNOS)和日本的多功能运输卫星增强系统(Multi-Functional Satellite Augmentation System，MSAS)等，还涵盖在建和以后要建设的其他卫星导航系统。
GNSS是能在地球表面或近地空间的任何地点为用户提供全天候的三维坐标、速度以及时间信息的空基无线电导航定位系统。也就是说，GNSS可以提供车辆在世界坐标系中的绝对位置信息。可以理解，在信号良好时，GNSS单独工作就能得到可靠的导航结果，但因信号遮挡等导致可搜索到的卫星数量小于四颗星时，GNSS无法进行导航解算。因此，将GNSS与IMU组合导航，GNSS可以向IMU提供准确的位置信息作为参考，而IMU又能输出高采样率的位置信息，且当GNSS因信号遮挡或干扰等中断工作时，IMU仍能继续工作以提高定位系统的稳定性。
在复杂城市道路环境中，由于卫星信号的长时间遮挡，车辆会出现的长时间GNSS定位异常：GNSS连续输出的多个位置点的定位数据有偏差且GNSS输出的这多个位置点中每个位置点的定位方差都较小，也就是说，这多个位置点的定位方差错误指示了GNSS输出的对应位置点的定位数据精度，导致这些位置点的异常的定位数据参与了轨迹的求解。可以看出，在出现长时间GNSS异常的情况下，由于异常的定位数据未被识别出，导致车辆在求解轨迹时引入了异常的GNSS测量值，导致结求解的轨迹精度低、定位不准。
针对上述问题，本申请实施例提出一种轨迹确定方法，能够在复杂遮挡环境下实现车辆的轨迹的准确求解，提高了轨迹的精度。
下面将结合附图，对本申请中的技术方案进行描述。
参见图1，图1示例性地给出了一种系统架构图。该系统用于确定车辆的轨迹。如图1所示，该系统包括组合导航设备和计算设备，其中，组合导航设备和计算设备通过有线或无线的方式进行通信。组合导航设备和计算设备位于同一车辆中。车辆可以是私家车、环卫机器人、无人快递车辆等可以实时定位的装置或设备。
组合导航设备包括惯性传感器IMU和高精度定位器(例如GNSS、全球定位系统GPS等)，其中，IMU主要包括用于检测角速度的陀螺仪和用于检测加速度的加速度计，高精度定位器用于测量车辆的位置和速度，其中，位置包括经度、纬度和高度(海拔)。另外，IMU的采样频率高于高精度定位器的采样频率(或称为IMU的更新频率高于高精度定位器的更新频率)，也就是说，在相同时间内IMU采集数据的次数多于GNSS采集数据的次数。为了叙述方便，下述中高精度定位器以GNSS为例进行说明，但本申请实施例并不限定高精度定位器仅为GNSS。
在一些可能的实施例中，组合导航设备还包括轮速计、里程计(odometer，ODOM)等，其中，里程计用于测量车辆行驶的里程，轮速计用于测量车辆的车轮的转速。
计算设备是具有计算功能的设备，其可以集成于车载电脑、车载工业控制计算机(Industrial Personal Computer，IPC)等车载终端中，计算设备也可以是独立于车辆外的计算装置，例如，服务器、计算机，计算设备可通过软件和/或硬件的方法实现，本申请实施例不做具体限定。计算设备用于根据车辆在多个时刻的待估计状态变量、IMU输出的第一数据和GNSS输出的第二数据构建目标函数，第二数据包括多个GNSS测量值和每个GNSS测量值对应的方差；根据目标函数求解车辆在多个时刻的待估计位置变量，获得第一轨迹；根据第一轨迹确定多个GNSS测量值中异常的测量值；更新目标函数，以获得车辆的最优轨迹，其中，更新后的目标函数不包括异常的测量值和异常的测量值对应的方差；以及求解更新后的目标函数，获得车辆的第二轨迹。
需要说明的是，图1仅为示例性架构图，但不限定图1所示系统包括的网元的数量。虽然图1未示出，但除图1所示的功能实体外，图1还可以包括其他功能实体。另外，本申请实施例提供的方法可以应用于图1所示的通信系统，当然本申请实施例提供的方法也可以适用其他通信系统，本申请实施例对此不予限制。
参见图2，图2是本申请实施例提供的一种轨迹确定方法的流程图。以该方法应用于车辆为例，但本申请实施例并不限定该方法仅应用于车辆，还可应用于车辆内的装置、部件或芯片，例如，车载电脑、图1中的计算设备等。该方法包括但不限于以下步骤：
S101：获取IMU的第一数据和GNSS的第二数据，第二数据包括多个GNSS测量值和每个GNSS测量值对应的方差。
在本申请实施例中，IMU和GNSS位于同一车辆中。
其中，第一数据包括多个IMU采样时刻对应的IMU数据，每个IMU采样时刻对应的IMU数据包括IMU在该IMU采样时刻采集到的加速度和角速度；第二数据包括多个GNSS测量值和多个GNSS测量值中每个GNSS测量值对应的方差，每个GNSS测量值有对应的GNSS采样时刻，GNSS测量值包括GNSS在相应地GNSS采样时刻采集到的位置信息，GNSS测量值对应的方差包括GNSS采集到的位置信息对应的位置方差。需要说明的是，每个GNSS测量值对应的方差也可以称作每个GNSS测量值的原始方差。
第二数据来自GNSS。示例性地，第二数据中的多个GNSS测量值可以是GNSS在预设时间段内采集到的多个GNSS测量值，第二数据中的多个GNSS测量值也可以是车辆对GNSS在预设时间段内采集到的各个GNSS测量值进行筛选后获得的。
在一种实现方式中，GNSS测量值还包括GNSS在相应地GNSS采样时刻采集到的航向信息，则GNSS测量值对应的方差还包括航向信息对应的航向方差。
可以理解，由于IMU的采样频率大于GNSS的采样频率，故在相同的预设时间段内，IMU采样时刻的数量大于GNSS采样时刻的数量，即相邻两个GNSS采样时刻之间有多个IMU采样时刻。
在一种实现方式中，第一数据与第二数据为同一预设时间段内采集到的数据，车辆可以先后或者同时获取第一数据和第二数据。基于数据量的大小，第一数据可以是IMU一次或者分多次发送至车辆，同理，第二数据也可以是GNSS一次或者分多次发送至车辆，在此不作具体限定。
S102：根据车辆在多个时刻的待估计状态变量、第一数据和第二数据构建目标函数。
其中，待估计状态变量包括待估计位置变量、待估计速度变量和待估计姿态变量。以多个时刻中的第一时刻为例，车辆在第一时刻的待估计状态变量包括车辆在第一时刻的待估计位置变量、车辆在第一时刻的待估计速度变量和车辆在第一时刻的待估计姿态变量。
在本申请实施例中，需先根据车辆在多个时刻的待估计状态变量、第一数据和第二数据构建GNSS因子和IMU预积分因子，再基于GNSS因子和IMU预积分因子构建目标函数。
下面分别描述GNSS因子和IMU预积分因子。
(1)GNSS因子
GNSS因子用于指示车辆在GNSS采样时刻的待估计位置变量与GNSS在该GNSS采样时刻采集到的GNSS测量值之间的定位误差。每个GNSS测量值有对应的GNSS因子。
一具体实施中，以第k个GNSS测量值为例，第k个GNSS测量值为多个GNSS测量值中的任意一个，第k个GNSS测量值为GNSS在时刻t采集到的，第k个GNSS测量值对应的GNSS因子可以根据车辆在时刻t的待估计位置变量、第k个GNSS测量值和第k个GNSS测量值对应的方差确定。
示例性地，GNSS因子指示的定位误差可以是带权重的定位误差。第k个GNSS测量值对应的GNSS因子可以表示为下述公式(1)：
其中，表示第k个GNSS测量值对应的GNSS因子，Pk表示第k个GNSS测量值(其为GNSS在时刻t采集到的)，Pt表示车辆在时刻t的待估计位置变量，w表示第k个GNSS测量值对应的信息矩阵，w可以根据第k个GNSS测量值对应的方差获得。时刻t为GNSS采样时刻，时刻t也属于上述多个时刻。
在一种实现方式中，在Pk为三维坐标信息时，Pk对应的方差包括GNSS在x轴上测得的方差σx、y轴上测得的方差σy和z轴上测得的方差σz，则w例如可以表示为矩阵在一些可能的实施例中，w也可以表示为矩阵/>σaverage是σx、σy和σz三者的平均值，即表示为/>w还可以是其他表示方式，在此不作具体限定。
为了尽可能使得GNSS测量值在时间或空间上分布均匀，以提高基于目标函数解算的轨迹的精度，还可以采用GNSS测量值的原始方差经方差调整参数调整后的方差构建GNSS因子。每个GNSS测量值有自身对应的方差调整参数。
一具体实施中，第k个GNSS测量值对应的GNSS因子可以根据车辆在时刻t的待估计位置变量、第k个GNSS测量值、第k个GNSS测量值对应的方差和第k个GNSS测量值对应的方差调整参数确定，其中，时刻t为第k个GNSS测量值的采样时刻。
示例性地，第k个GNSS测量值对应的GNSS因子可以表示为下述公式(2)：
其中，ck表示第k个GNSS测量值对应的方差调整参数，w、Pt和Pk具体可参考上述公式(1)中有关参数的说明，在此不再赘述。
第k个GNSS测量值对应的方差调整参数可以是下述两种表示方式中的任意一种：
方式A：
第k个GNSS测量值对应的方差调整参数为第k个GNSS测量值与第(k-1)个GNSS测量值之间车辆的行驶距离与第一距离的比值。
其中，第(k-1)个GNSS测量值为第k个GNSS测量值的前一个相邻的测量值，第k个GNSS测量值与第(k-1)个GNSS测量值之间车辆的行驶距离可以是基于第k个GNSS测量值和第(k-1)个GNSS测量值计算获得的欧式距离。
其中，第一距离用于指示GNSS的可控制范围。在复杂城市道路环境中，车辆的GNSS接收到的卫星信号可能为经过高楼、树木等环境中目标体多次反射后的卫星信号，也就是说，GNSS接收到的卫星信号是存在误差的，在车辆的位置信息的变化量小于第一距离时，认为车辆上的GNSS接收到的卫星信号存在的误差是相同的。
在此情况下，ck例如可以表示为下述公式(3)：
其中，ck表示第k个GNSS测量值对应的方差调整参数，表示第k个GNSS测量值与第(k-1)个GNSS测量值之间车辆的行驶距离，D表示第一距离。
方式B：
第k个GNSS测量值对应的方差调整参数是根据车辆在t时刻的行驶速度、GNSS的采样频率和第一距离确定。其中，t时刻为第k个GNSS测量值的采样时刻。
在此情况下，ck例如可以表示为下述公式(4)：
其中，ck表示第k个GNSS测量值对应的方差调整参数，vt表示车辆在t时刻的行驶速度，f表示GNSS的采样频率，D表示第一距离。
在一些可能的实施例中，还可以在构建GNSS因子时引入鲁棒核函数，以提高基于GNSS因子构建的目标函数对噪声的鲁棒性。示例性地，鲁棒核函数例如可以是huber核函数。
在本申请实施例中，除了可以通过上述方差调整参数改善GNSS测量值在时间或空间上的分布以提高后续基于目标函数求解的轨迹的精度，还可以通过限定第二数据中的多个GNSS测量值的选取方式，以使参与目标函数构建的多个GNSS测量值在时间或空间上分布均匀，提高构建的目标函数的精准性。具体可参考下述方式C或方式D：
方式C：
上述多个GNSS测量值中任意两个相邻的GNSS测量值之间车辆的行驶距离为第二距离。
示例性地，沿着车辆的行驶轨迹每隔一定距离(例如，第二距离)从GNSS输出的原始数据中选择一个GNSS测量值，例如，沿着车辆的行驶轨迹每隔5米选取一个GNSS测量值，依据此方式获得上述第二数据中的多个GNSS测量值，且多个GNSS测量值中各个相邻两个GNSS测量值之间车辆的行驶距离均相同。
参见图3，图3是本申请实施例提供的一种多个GNSS测量值的分布示意图。在图3中，加粗带箭头的线表示车辆的行驶轨迹，实心三角形表示GNSS测量值指示的位置点，可以看出，多个GNSS测量值在行驶轨迹上均匀分布，并且GNSS测量值的选取是车辆每行驶一固定距离选取一个GNSS测量值。
在一种实现方式中，第二距离小于等于上述方差调整参数中的第一距离。
方式D：
上述多个GNSS测量值中的任一GNSS测量值满足以下条件中的至少一项：GNSS测量值是固定解；和GNSS测量值指示的位置处的可观测卫星颗数超过预设阈值。
需要说明的是，GNSS测量值为固定解时的定位精度(或定位质量)要好于GNSS测量值为非固定解时的定位精度(或定位质量)，GNSS测量值为固定解是指基于载波相位计算出该GNSS测量值时对应的模糊度为整数。
示例性地，将车辆的行驶轨迹按照预设距离(例如，第二距离)均匀分段，从每段轨迹中分别选取最优的GNSS测量值，每段轨迹中最优的GNSS测量值例如可以是该段轨迹中的固定解和/或该段轨迹中可观测卫星颗数超过预设阈值的位置点对应的GNSS测量值。从每段固定长度的轨迹中获得的最优的GNSS测量值的数量可以是一个，也可以是多个，在此不作具体限定。
参见图4，图4是本申请实施例提供的又一种多个GNSS测量值的分布示意图。在图4中，加粗带箭头的线表示车辆的行驶轨迹，实心三角形表示GNSS测量值指示的位置点，通过实心圆点将车辆的行驶轨迹分成了多段具有相同长度的轨迹，可以看出，每段轨迹中选取了至少一个GNSS测量值，每段轨迹中选取的GNSS测量值满足上述方式D中对GNSS测量值的限定。
在一些可能的实施例中，还可以将限定多个GNSS测量值的选取方式与使用方差调整参数构建GNSS因子这两者结合使用。例如方式C与上述公式(2)结合，即公式(2)使用的GNSS测量值符合方式C中的限定条件。又例如方式D与上述公式(2)结合，即公式(2)中的使用的GNSS测量值符合方式D中对GNSS测量值的限定。
(2)IMU预积分因子
IMU预积分因子用于指示IMU在上述多个时刻中的两个相邻时刻之间的状态增量残差。其中，状态增量残差可以根据位置增量残差、速度增量残差和姿态增量残差获得。
其中，位置增量残差可以根据相邻时刻对应的两个待估计位置变量和IMU在该相邻时刻之间的位置增量获得；速度增量残差可根据相邻时刻对应的两个待估计速度变量和IMU在该相邻时刻之间的速度增量获得；姿态增量残差可根据相邻时刻对应的两个待估计姿态变量和IMU在该相邻时刻之间的姿态增量获得。IMU在相邻时刻之间的位置增量、速度增量和姿态增量可通过该相邻时刻之间各个IMU采样时刻对应的IMU数据获得。
IMU预积分因子例如可以表示为下述公式(5)；
其中，IMU预积分因子表示i时刻和j时之间的状态增量残差，i时刻和j时刻是上述多个时刻中任意两个相邻时刻，且j时刻是i时刻的下一个时刻。rimu_p表示i时刻和j时之间的位置增量残差，rimu_v表示i时刻和j时之间的速度增量残差，rimu_a表示i时刻和j时之间的姿态增量残差。C表示旋转变换矩阵，也可以称为姿态矩阵。Pj和Pi分别表示车辆在j时刻的待估计位置变量和车辆在i时刻的待估计位置变量，Vj和Vi分别表示车辆在j时刻的待估计速度变量和车辆在i时刻的待估计速度变量，qi和qj分别表示IMU在j时刻和i时刻的待估计姿态变量(或称为待估计姿态四元数)，gn表示重力加速度，Δt表示i时刻和j时刻之间的时间间隔。/>表示在i时刻车辆坐标系下对IMU数据积分获得的位置增量，/>表示在i时刻车辆坐标系下对IMU数据积分获得的速度增量，/>表示在i时刻车辆坐标系下对IMU数据积分获得的姿态增量。/>的计算与[i，j]时间段内的IMU测量的加速度和角速度有关，在此不再赘述。
根据上述GNSS因子和IMU预积分因子，即可构建包含多个时刻的待估计位置变量、IMU状态残差增量和GNSS测量方差的目标函数。
示例性地，目标函数为第一函数与第二函数之和，其中，第一函数用于指示多个GNSS测量值中各个GNSS测量值对应的GNSS因子之和，第二函数用于指示多个时刻中各个相邻时刻之间的IMU状态残差增量之和。
目标函数的表达式可参考公式(6)所示：
其中，多个时刻包括tT，tT+1，…，tN，即多个时刻的个数为N；分别表示车辆在多个时刻的待估计状态变量，也是车辆在多个时刻的待求解变量，以车辆在tT时刻的待估计状态变量/>为例，/>表示车辆在tT时刻的待估计位置变量，/>表示车辆在tT时刻的待估计速度变量，/>表示车辆在tT时刻的待估计姿态变量；/>即为上述中的第一函数，M表示当前参与目标函数计算的GNSS测量值的个数，首次构建该目标函数时M即为上述第二数据中多个GNSS测量值的数量，/>表示第k个GNSS测量值对应的GNSS因子，/>可以是上述公式(1)，也可以是上述公式(2)，在此不作具体限定；/>即为上述中的第二函数，i和j表示多个时刻{tT，tT+1，…，tN}中的两个相邻时刻，/>表示时刻i和时刻j对应的IMU预积分因子，也表示时刻i和时刻j之间的状态增量残差。
需要说明的是，公式(6)所示的目标函数只是一种示例，目标函数还可以表示为公式(6)的变形或者其他公式，例如，目标函数除了包括上述GNSS因子和IMU预积分因子外，目标函数还可以包括轮速因子，轮速因子用于指示车辆任一时刻的速度与轮速计在该时刻采集到的速度之间的误差。
S103：根据第一轨迹确定多个GNSS测量值中异常的测量值，第一轨迹是基于目标函数求解车辆在上述多个时刻的待估计状态变量获得。
在本申请实施例中，在构建目标函数后，根据目标函数求解车辆在上述多个时刻的待估计状态变量，获得第一轨迹；根据第一轨迹确定多个GNSS测量值中异常的测量值。
其中，求解目标函数即为求解上述公式(6)所示的目标函数的最小值，即可获得上述也就求解出车辆在多个时刻{tT，tT+1，…，tN}中每个时刻的待估计状态变量，也就求解出车辆在多个时刻中每个时刻的待估计位置变量。以tT时刻为例，求解出的车辆在tT时刻的待估计位置变量可以称作车辆在tT时刻的待估计位置信息，即获得了车辆在多个时刻{tT，tT+1，…，tN}中每个时刻的待估计位置信息，也就是获得了第一轨迹，其中，第一轨迹包括车辆在这多个时刻的待估计位置信息。另外，目标函数的求解可以通过因子图算法、卡尔曼滤波或其他方法，在此不作具体限定。
一具体实施中，根据第一轨迹确定多个GNSS测量值中异常的测量值，包括：第一轨迹包括车辆在多个时刻的待估计位置信息，GNSS测量值A为多个GNSS测量值中的任意一个，以GNSS测量值A为例说明判断GNSS测量值A为异常的测量值的过程，具体地，根据GNSS测量值A的采样时刻，查找第一轨迹中GNSS测量值A对应的待估计位置信息B；在GNSS测量值A与待估计位置信息B的差值与GNSS测量值A对应的方差的比值大于第一阈值时，确定GNSS测量值A为异常的测量值。需要说明的是，待估计位置信息B即为求解出的车辆在GNSS测量值A的采样时刻的待估计位置变量。依据此方式分别判断当前目标函数中多个GNSS测量值中的每个GNSS测量值是否异常，由此可确定当前目标函数中异常的GNSS测量值。
在一种实现方式中，GNSS测量值A与待估计位置信息B的差值可以是指x、y、z轴三个维度中任意一个维度上的差值的绝对值，也可以是指x、y、z轴三个维度中至少两个维度上的差值的加权求和。
另一具体实施中，确定GNSS测量值A为异常的测量值的条件也可以是：GNSS测量值A与待估计位置信息B的差值大于预设误差阈值。
S104：更新目标函数，以使更新后的目标函数不包括上述异常的测量值和异常的测量值对应的方差。
在本申请实施中，更新目标函数，包括：目标函数包括上述多个GNSS测量值中每个GNSS测量值对应的GNSS因子，删除目标函数中上述异常的测量值对应的GNSS因子。
以上述公式(6)所示的目标函数为例，假设经过S103确定多个GNSS测量值中的第2个GNSS测量值和第5个GNSS测量值均异常，则对公式(6)所示的目标函数进行更新时，会删除公式(6)中第2个GNSS测量值对应的GNSS因子以及删除第5个GNSS测量值对应的GNSS因子/>故此次更新后的目标函数不包括异常的测量值对应的GNSS因子，即/>和/>也就是说，此次更新后，/>和/>不再参与目标函数的构建。
S105：求解更新后的目标函数，获得车辆的第二轨迹。
在本申请实施例中，目标函数每更新一次，会重新求解更新后的目标函数，以获得车辆的第二轨迹。具体地，重新求解更新后的目标函数获得车辆的第二轨迹是指：基于更新后的目标函数求解车辆在上述多个时刻的待估计状态变量，获得第二轨迹。
在一些可能的实施例中，车辆在获得第二轨迹后，还可以根据第二轨迹确定该更新后的目标函数中任一GNSS测量值无异常；将第二轨迹作为车辆的最优轨迹。也就是说，在根据第二轨迹确定更新后的目标函数中任一GNSS测量值无异常的情况下，将第二轨迹作为车辆的最优轨迹。需要说明的是，根据第二轨迹确定该更新后的目标函数是否存在异常的GNSS测量值的方法可参考上述S103的相关叙述，在此不再赘述。
在一些可能的实施例中，若根据第二轨迹确定更新后的目标函数中存在某GNSS测量值为异常的测量值，在此情况下，再对当前的目标函数进行更新以剔除异常的测量值对应的GNSS因子，并基于更新后的目标函数重新求解车辆的轨迹，如此循环，直至基于最新求解的轨迹判断出当前的目标函数不存在异常的GNSS测量值。
另外，求解出的车辆的轨迹可以用于城市复杂遮挡场景下车辆的组合导航定位，也可以用于地图的采集和构建等，在此不作限定。
可以看到，实施本申请实施例，能够在卫星信号遮挡导致GNSS测量长时间连续出现偏差的情况下识别出异常的测量值。另外，通过对GNSS测量值对应的方差进行调整或者对用于构建GNSS因子的多个GNSS测量值的筛选，有效改善了GNSS数据在时空上分布的均匀性，使得异常的GNSS测量值的剔除更加有效，实现了对求解的轨迹的优化，有利于提高轨迹的精度。
参见图5，图5是本申请实施例提供的又一种轨迹确定方法的流程图。以该方法应用于车辆为例，但并不限定该方法仅用于车辆，还可应用于车辆内的装置、部件或芯片，例如，车载电脑、图1中的计算设备等。该方法包括但不限于以下步骤：
S201：根据车辆在多个时刻的待估计状态变量、IMU输出的第一数据和GNSS输出的第二数据构建目标函数，第二数据包括多个GNSS测量值和每个GNSS测量值对应的方差。此步骤具体可参考图2实施例中的S101和S102，在此不再赘述。
S202：基于目标函数求解车辆在多个时刻的待估计状态变量，获得第一轨迹。
S203：根据当前解算出的轨迹判断当前的目标函数中是否存在异常的GNSS测量值。
具体地，在当前的目标函数中存在异常的GNSS测量值时，执行下述S204；在当前的目标函数中不存在异常的GNSS测量值时，执行下述S206。
在首次执行判断当前的目标函数是否存在异常的GNSS测量值时，当前解算出的轨迹即为第一轨迹，当前的目标函数中的GNSS测量值包括S201中的多个GNSS测量值，在此情况下，根据当前解算出的轨迹判断当前的目标函数中是否存在异常的GNSS测量值即为：根据第一轨迹判断多个GNSS测量值中是否存在异常的GNSS测量值。
参考图2实施例中S103，判断方法可以是：以当前目标函数中的GNSS测量值1为例，GNSS测量值1与第一轨迹中GNSS测量值1对应的待估计位置信息之间的差值与GNSS测量值1对应的方差的比值超过第一阈值时，确定GNSS测量值1异常。
同理，若当前目标函数中的GNSS测量值2与第一轨迹中GNSS测量值2对应的待估计位置信息之间的差值与GNSS测量值2对应的方差的比值超过第一阈值时，确定GNSS测量值2异常。
需要说明的是，S103中异常的测量值等同于异常的GNSS测量值。
S204：更新目标函数以删除目标函数中异常的GNSS测量值对应的GNSS因子。
可以理解，通过删除目标函数中异常的GNSS测量值对应的GNSS因子实现对目标函数的更新，更新后的目标函数不包括异常的GNSS测量值和异常的GNSS测量值对应的方差。
S205：基于更新后的目标函数求解车辆在多个时刻的待估计状态变量，获得第二轨迹。
在获得第二轨迹后，跳转至S203循环执行。
为了叙述清楚循环的过程，将S201中构建的目标函数称作目标函数1。
在获得第一轨迹后，车辆先执行第一轮异常判断，即根据第一轨迹确定目标函数1中是否存在异常的GNSS测量值，在确定目标函数1中存在异常的GNSS测量值后，对目标函数1进行更新，获得目标函数2。
根据目标函数2求解车辆在多个时刻的待估计状态变量，获得第二轨迹。在获得第二轨迹后，车辆先执行第二轮异常判断，即根据第二轨迹确定目标函数2中是否存在异常的GNSS测量值，在确定目标函数2中存在异常的GNSS测量值后，对目标函数2进行更新，获得目标函数3，…，如此循环，直至根据当前求解出的轨迹判断当前的目标函数中不存在异常的GNSS测量值时，循环结束。
在一些可能的实施例中，随着异常判断的轮次的递增，上述用于判断异常的第一阈值的取值也会相对减小至保持不变。例如，在执行第一轮异常判断时，第一阈值取值为5，随着异常判断的轮次的递增，第一阈值逐次取值为4.5、4，最后取值为3就不再变化。
在一些可能的实施例中，循环结束的条件也可以是S205跳转至S203的次数达到预设循环次数。即当S205跳转至S203的次数达到预设循环次数时，结束循环，执行S206。
S206：将当前解算出的轨迹作为车辆的最优轨迹。
具体地，在根据当前解算出的轨迹判断当前的目标函数中不存在异常的GNSS测量值时，将当前解算出的轨迹作为车辆的最优轨迹。
可以看到，实施本申请实施例，能够在卫星信号遮挡导致GNSS测量长时间连续出现偏差的情况下识别出异常的测量值，异常的测量值的剔除实现了对组合导航场景下求解的轨迹的优化，提高了求解的轨迹的精准度。
参见图6，图6是本申请实施例提供的一种轨迹确定装置的结构示意图，轨迹确定装置30包括函数处理单元310、异常检测单元312和轨迹获取单元314。该轨迹确定装置30可以通过硬件、软件或者软硬件结合的方式来实现。
其中，函数处理单元310，用于根据车辆在多个时刻的待估计状态变量、IMU的第一数据和GNSS的第二数据构建目标函数，第二数据包括多个GNSS测量值和每个GNSS测量值对应的方差；异常检测单元312，用于根据第一轨迹确定上述多个GNSS测量值中异常的测量值，第一轨迹是基于目标函数求解车辆在多个时刻的待估计状态变量获得；函数处理单元310还用于更新目标函数，以使更新后的目标函数不包括上述异常的测量值和异常的测量值对应的方差；轨迹获取单元314，用于求解更新后的目标函数，获得车辆的第二轨迹。
该轨迹确定装置30可用于实现图2实施例所描述的方法。在图2实施例中，函数处理单元310可用于执行S101、S102和S104，异常检测单元312可用于执行S103，轨迹获取单元314可用于执行S105。该轨迹确定装置30还可用于实现图5实施例所描述的方法，为了说明书的简洁，在此不再赘述。
以上图6所示实施例中的各个单元的一个或多个可以软件、硬件、固件或其结合实现。所述软件或固件包括但不限于计算机程序指令或代码，并可以被硬件处理器所执行。所述硬件包括但不限于各类集成电路，如中央处理单元(central processing unit，CPU)、数字信号处理器(digital signal processor，DSP)、现场可编程门阵列(field-programmable gate array，FPGA)或专用集成电路(application-specific integratedcircuit，ASIC)。
参见图7，图7是本申请实施例提供的又一种轨迹确定装置的结构示意图。
在图7中，轨迹确定装置40包括：处理器401、通信接口402、存储器403和总线404。处理器401、存储器403和通信接口402之间通过总线404通信。应理解，本申请不限定轨迹确定装置40中的处理器、存储器的个数。
轨迹确定装置40可以是上述图1中的计算设备。在一些可能的实施例中，轨迹确定装置40可以是车辆、车辆内的装置、部件或芯片等，例如，车载电脑等。
总线404可以是外设部件互连标准(peripheral component interconnect，PCI)总线或扩展工业标准结构(extended industry standard architecture，EISA)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，图7中仅用一条线表示，但并不表示仅有一根总线或一种类型的总线。总线404可包括在轨迹确定装置40各个部件(例如，存储器403、处理器401、通信接口402)之间传送信息的通路。
处理器401可以包括中央处理器(central processing unit，CPU)、微处理器(micro processor，MP)或者数字信号处理器(digital signal processor，DSP)等处理器中的任意一种或多种。
存储器403用于提供存储空间，存储空间中可以存储操作系统和计算机程序等数据。存储器403可以是随机存取存储器(random access memory，RAM)、可擦除可编程只读存储器(erasable programmable read only memory，EPROM)、只读存储器(read-onlymemory，ROM)，或便携式只读存储器(compact disc read memory，CD-ROM)等中的一种或者多种的组合。存储器403可以单独存在，也可以集成于处理器401内部。
通信接口402可用于为处理器401提供信息输入或输出。或者可替换的，该通信接口402可用于接收外部发送的数据和/或向外部发送数据，可以为包括诸如以太网电缆等的有线链路接口，也可以是无线链路(如Wi-Fi、蓝牙、通用无线传输等)接口。或者可替换的，通信接口402还可以包括与接口耦合的发射器(如射频发射器、天线等)，或者接收器等。
该轨迹确定装置40中的处理器401用于读取存储器403中存储的计算机程序，用于执行前述的方法，例如图2或图5所描述的轨迹确定方法。
在一种可能的设计方式中，轨迹确定装置40可为执行图2或图5所示方法的执行主体中的一个或多个模块，该处理器401可用于读取存储器中存储的一个或多个计算机程序，用于执行以下操作：
根据车辆在多个时刻的待估计状态变量、IMU的第一数据和GNSS的第二数据构建目标函数，第二数据包括多个GNSS测量值和每个GNSS测量值对应的方差；
根据第一轨迹确定多个GNSS测量值中异常的测量值，第一轨迹是基于目标函数求解车辆在多个时刻的待估计状态变量获得；
更新目标函数，以使更新后的目标函数不包括异常的测量值和异常的测量值对应的方差；
求解更新后的目标函数，获得车辆的第二轨迹。
在本文上述的实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详细描述的部分，可以参见其他实施例的相关描述。
需要说明的是，本领域普通技术人员可以看到上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质包括只读存储器(Read-Only Memory，ROM)、随机存储器(Random AccessMemory，RAM)、可编程只读存储器(Programmable Read-only Memory，PROM)、可擦除可编程只读存储器(Erasable Programmable Read Only Memory，EPROM)、一次可编程只读存储器(One-time Programmable Read-Only Memory，OTPROM)、电子抹除式可复写只读存储(Electrically-Erasable Programmable Read-Only Memory，EEPROM)、只读光盘(CompactDisc Read-Only Memory，CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。
本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机程序产品存储在一个存储介质中，包括若干指令用以使得一个设备(可以是个人计算机，服务器，或者网络设备、机器人、单片机、芯片、机器人等)执行本申请各个实施例所述方法的全部或部分步骤。

Claims (21)

1.一种轨迹确定方法，其特征在于，所述方法包括：
根据车辆在多个时刻的待估计状态变量、惯性测量单元IMU的第一数据和全球导航卫星系统GNSS的第二数据构建目标函数，所述第二数据包括多个GNSS测量值和每个GNSS测量值对应的方差；
根据第一轨迹确定所述多个GNSS测量值中异常的测量值，所述第一轨迹是基于所述目标函数求解所述车辆在多个时刻的待估计状态变量获得；
更新所述目标函数，以使更新后的目标函数不包括所述异常的测量值和所述异常的测量值对应的方差；
求解所述更新后的目标函数，获得所述车辆的第二轨迹。
2.根据权利要求1所述的方法，其特征在于，所述目标函数包括所述每个GNSS测量值对应的GNSS因子，所述更新所述目标函数，包括：
删除所述目标函数中所述异常的测量值对应的GNSS因子。
3.根据权利要求2所述的方法，其特征在于，所述第二数据包括第一时刻采集的第一GNSS测量值和所述第一GNSS测量值对应的第一方差，所述车辆在所述第一时刻的待估计状态变量包括所述车辆在所述第一时刻的待估计位置变量，所述第一GNSS测量值对应的GNSS因子是根据所述待估计位置变量、所述第一GNSS测量值和所述第一方差获得。
4.根据权利要求3所述的方法，其特征在于，所述第一GNSS测量值对应的GNSS因子还包括所述第一GNSS测量值对应的方差调整参数，所述第二数据还包括第二GNSS测量值，所述第二GNSS测量值为所述第一GNSS测量值的前一个相邻的测量值，所述方差调整参数为所述第一GNSS测量值与所述第二GNSS测量值之间所述车辆的行驶距离与第一距离的比值。
5.根据权利要求3所述的方法，其特征在于，所述第一GNSS测量值对应的GNSS因子还包括所述第一GNSS测量值对应的方差调整参数，所述方差调整参数是根据所述车辆在所述第一时刻的行驶速度、所述GNSS的采样频率和第一距离确定。
6.根据权利要求1-5任一项所述的方法，其特征在于，所述多个GNSS测量值中任意两个相邻的GNSS测量值之间所述车辆的行驶距离为第二距离。
7.根据权利要求1-6任一项所述的方法，其特征在于，所述多个GNSS测量值中的任意一GNSS测量值满足以下条件中的至少一项：
GNSS测量值是固定解；和
GNSS测量值指示的位置处的可观测卫星颗数超过预设阈值。
8.根据权利要求1-7任一项所述的方法，其特征在于，第三GNSS测量值为所述多个GNSS测量值中的任意一个，所述第一轨迹包括所述车辆在所述多个时刻的待估计位置信息，所述根据第一轨迹确定所述多个GNSS测量值中异常的测量值，包括：
根据所述第三GNSS测量值的采集时刻，查找获得所述第一轨迹中所述第三GNSS测量值对应的第一待估计位置信息；
在所述第三GNSS测量值与所述第一待估计位置信息的差值与所述第三GNSS测量值对应的方差的比值超过第一阈值时，确定所述第三GNSS测量值为异常的测量值。
9.根据权利要求1-8任一项所述的方法，其特征在于，所述方法还包括：
根据所述第二轨迹确定所述更新后的目标函数中的任一GNSS测量值无异常；
将所述第二轨迹作为所述车辆的最优轨迹。
10.一种轨迹确定装置，其特征在于，所述装置包括：
函数处理单元，用于根据车辆在多个时刻的待估计状态变量、惯性测量单元IMU的第一数据和全球导航卫星系统GNSS的第二数据构建目标函数，所述第二数据包括多个GNSS测量值和每个GNSS测量值对应的方差；
异常检测单元，用于根据第一轨迹确定所述多个GNSS测量值中异常的测量值，所述第一轨迹是基于所述目标函数求解所述车辆在多个时刻的待估计状态变量获得；
所述函数处理单元还用于更新所述目标函数，以使更新后的目标函数不包括所述异常的测量值和所述异常的测量值对应的方差；
轨迹获取单元，用于求解所述更新后的目标函数，获得所述车辆的第二轨迹。
11.根据权利要求10所述的装置，其特征在于，所述目标函数包括所述每个GNSS测量值对应的GNSS因子，所述函数处理单元，具体用于：
删除所述目标函数中所述异常的测量值对应的GNSS因子。
12.根据权利要求11所述的装置，其特征在于，所述第二数据包括第一时刻采集的第一GNSS测量值和所述第一GNSS测量值对应的第一方差，所述车辆在所述第一时刻的待估计状态变量包括所述车辆在所述第一时刻的待估计位置变量，所述第一GNSS测量值对应的GNSS因子是根据所述待估计位置变量、所述第一GNSS测量值和所述第一方差获得。
13.根据权利要求12所述的装置，其特征在于，所述第一GNSS测量值对应的GNSS因子还包括所述第一GNSS测量值对应的方差调整参数，所述第二数据还包括第二GNSS测量值，所述第二GNSS测量值为所述第一GNSS测量值的前一个相邻的测量值，所述方差调整参数为所述第一GNSS测量值与所述第二GNSS测量值之间所述车辆的行驶距离与第一距离的比值。
14.根据权利要求12所述的装置，其特征在于，所述第一GNSS测量值对应的GNSS因子还包括所述第一GNSS测量值对应的方差调整参数，所述方差调整参数是根据所述车辆在所述第一时刻的行驶速度、所述GNSS的采样频率和第一距离确定。
15.根据权利要求10-14任一项所述的装置，其特征在于，所述多个GNSS测量值中任意两个相邻的GNSS测量值之间所述车辆的行驶距离为第二距离。
16.根据权利要求10-15任一项所述的装置，其特征在于，所述多个GNSS测量值中的任意一GNSS测量值满足以下条件中的至少一项：
GNSS测量值是固定解；和
GNSS测量值指示的位置处的可观测卫星颗数超过预设阈值。
17.根据权利要求10-16任一项所述的装置，其特征在于，第三GNSS测量值为所述多个GNSS测量值中的任意一个，所述第一轨迹包括所述车辆在所述多个时刻的待估计位置信息，所述异常检测单元，具体用于：
根据所述第三GNSS测量值的采集时刻，查找获得所述第一轨迹中所述第三GNSS测量值对应的第一待估计位置信息；
在所述第三GNSS测量值与所述第一待估计位置信息的差值与所述第三GNSS测量值对应的方差的比值超过第一阈值时，确定所述第三GNSS测量值为异常的测量值。
18.根据权利要求10-17任一项所述的装置，其特征在于，
所述异常检测单元用于根据所述第二轨迹确定所述更新后的目标函数中的任一GNSS测量值无异常；
所述轨迹获取单元用于将所述第二轨迹作为所述车辆的最优轨迹。
19.一种轨迹确定装置，其特征在于，所述装置包括存储器和处理器，所述存储器存储计算机程序指令，所述处理器运行所述计算机程序指令以使所述装置执行如权利要求1-9任一项所述的方法。
20.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有程序指令，所述程序指令用于实现权利要求1-9中任一项所述的方法。
21.一种计算机程序产品，其特征在于，当所述计算机程序产品在处理器上运行时，使得用于轨迹确定的装置执行如权利要求1-9任一项所述的方法。
CN202210320326.7A 2022-03-29 2022-03-29 一种轨迹确定方法及装置 Pending CN116931041A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210320326.7A CN116931041A (zh) 2022-03-29 2022-03-29 一种轨迹确定方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210320326.7A CN116931041A (zh) 2022-03-29 2022-03-29 一种轨迹确定方法及装置

Publications (1)

Publication Number Publication Date
CN116931041A true CN116931041A (zh) 2023-10-24

Family

ID=88375833

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210320326.7A Pending CN116931041A (zh) 2022-03-29 2022-03-29 一种轨迹确定方法及装置

Country Status (1)

Country Link
CN (1) CN116931041A (zh)

Similar Documents

Publication Publication Date Title
US20200216076A1 (en) Method for determining the location of an ego-vehicle
CA2838768C (en) Method and apparatus for real-time positioning and navigation of a moving platform
CN110631593A (zh) 一种用于自动驾驶场景的多传感器融合定位方法
CN112577521B (zh) 一种组合导航误差校准方法及电子设备
CN111077549B (zh) 位置数据修正方法、装置和计算机可读存储介质
CN110057356B (zh) 一种隧道内车辆定位方法及装置
US20150153460A1 (en) Sequential Estimation in a Real-Time Positioning or Navigation System Using Historical States
CN110319850B (zh) 一种获取陀螺仪的零点偏移的方法及装置
CN113063425B (zh) 车辆定位方法、装置、电子设备及存储介质
CN107764273B (zh) 一种车辆导航定位方法及系统
CN108254775A (zh) 车载导航系统及其实现方法
CN115407376A (zh) 车辆定位校准方法、装置、计算机设备、存储介质
CN113009816B (zh) 时间同步误差的确定方法及装置、存储介质及电子装置
CN111197994B (zh) 位置数据修正方法、装置、计算机设备和存储介质
CN116242373A (zh) 一种融合多源数据的高精度导航定位方法及系统
CN113917512B (zh) 自动驾驶车辆的定位方法、装置、电子设备及存储介质
CN116931041A (zh) 一种轨迹确定方法及装置
RU2617147C1 (ru) Способ начального ориентирования гироскопической навигационной системы для наземных подвижных объектов
KR20140142610A (ko) 위치 측정 장치 및 방법
JP2014153113A (ja) 速度推定装置及びプログラム
CN110398765B (zh) 定位方法和装置、无人驾驶设备
KR101964059B1 (ko) 실내 주차장 안내 시스템을 위한 바퀴 속력 센서 기반 차량 측위 시스템
CN110082805A (zh) 一种三维定位装置和方法
Ciecko et al. Analysis of the Accuracy and Usefulness of MEMS Chipsets Embedded in Popular Mobile Phones in Inertial Navigation
TWI720923B (zh) 定位系統以及定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication