CN116125819A - 轨迹修正方法、装置、电子设备和计算机可读存储介质 - Google Patents
轨迹修正方法、装置、电子设备和计算机可读存储介质 Download PDFInfo
- Publication number
- CN116125819A CN116125819A CN202310395800.7A CN202310395800A CN116125819A CN 116125819 A CN116125819 A CN 116125819A CN 202310395800 A CN202310395800 A CN 202310395800A CN 116125819 A CN116125819 A CN 116125819A
- Authority
- CN
- China
- Prior art keywords
- track
- target
- motion state
- observation data
- time difference
- 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.)
- Granted
Links
- 238000012937 correction Methods 0.000 title claims abstract description 67
- 238000000034 method Methods 0.000 title claims abstract description 42
- 239000011159 matrix material Substances 0.000 claims description 23
- 238000004590 computer program Methods 0.000 claims description 12
- 238000001914 filtration Methods 0.000 claims description 4
- 238000002715 modification method Methods 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 description 14
- 230000008447 perception Effects 0.000 description 8
- 230000004927 fusion Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 4
- 238000012986 modification Methods 0.000 description 4
- 230000004048 modification Effects 0.000 description 4
- 230000003068 static effect Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 3
- 238000007619 statistical method Methods 0.000 description 3
- 238000012546 transfer Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000005295 random walk Methods 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000009191 jumping Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
本申请提供一种轨迹修正方法、装置、电子设备和计算机可读存储介质，该方法包括获取目标观测数据和目标轨迹运动状态；其中，目标观测数据表示在物体的轨迹与观测数据匹配成功的情况下的观测数据，目标轨迹运动状态表示在物体的轨迹与观测数据匹配成功的情况下的物体轨迹运动状态；根据目标观测数据以及目标轨迹运动状态，确定轨迹预测运动状态；根据目标观测数据确定轨迹误差值；根据轨迹误差值，对轨迹预测运动状态进行修正，从而在物体的轨迹与观测数据匹配成功的情况下对误差进行观测校正从而提高物体轨迹预测的准确性。
Description
技术领域
本申请涉及路侧融合技术领域，具体而言，涉及一种轨迹修正方法、装置、电子设备和计算机可读存储介质。
背景技术
当前路侧融合系统中，对同一物体的跨传感器感知广泛存在感知结果位置、速度、类别差异过大，融合效果差，使得轨迹出现漂移、跳变等现象，其感知一致性是亟需解决的问题。
为了在融合端弥补感知一致性差的问题，维持预先轨迹的强线性假设方案经常被用于减少漂移、跳变现象出现次数，提高跨传感器融合质量，但往往会随着轨迹的延长而导致预先轨迹与真实轨迹的误差增大，且难以平滑的进行修正。
发明内容
本申请实施例的目的在于提供一种轨迹修正方法、装置、电子设备和计算机可读存储介质，用以解决目前轨迹维持预先轨迹的强线性假设方案存在的误差较大而难以平滑进行轨迹修正的问题。
第一方面，本发明提供一种轨迹修正方法，该方法包括：获取目标观测数据和目标轨迹运动状态；其中，目标观测数据表示在物体的轨迹与观测数据匹配成功的情况下的观测数据，目标轨迹运动状态表示在物体的轨迹与观测数据匹配成功的情况下的物体轨迹运动状态；根据目标观测数据以及目标轨迹运动状态，确定轨迹预测运动状态；根据目标观测数据确定轨迹误差值；根据轨迹误差值，对轨迹预测运动状态进行修正。
上述设计的轨迹修正方法，本方案首先获取物体的轨迹与观测数据匹配成功的情况的目标观测数据和目标轨迹运动状态，然后根据目标观测数据和目标轨迹运动状态对物体的轨迹进行预测，确定轨迹预测运动状态，并且根据目标观测数据确定轨迹误差值，从而根据轨迹误差值对轨迹预测运动状态进行修正。由于本方案中的轨迹误差值是通过观测的物体实际运动情况下确定的，因此，确定的轨迹误差值相比传统强线性假设的方式更加贴近于物体的实际运动情况，即确定的轨迹误差值更加准确，使得车辆在感知盲区中的速度更新成为可能，弥补运动模型引起的误差，进而通过轨迹误差值对轨迹预测运动状态进行修正之后，可以使得预测的物体轨迹更加准确。
在第一方面的可选实施方式中，其中，目标观测数据包括目标观测时间和目标观测运动状态；根据目标观测数据以及目标轨迹运动状态，确定轨迹预测运动状态，包括：根据目标观测时间，计算目标时间差；根据目标时间差、目标观测运动状态以及目标轨迹运动状态，确定轨迹预测运动状态。
在第一方面的可选实施方式中，根据目标观测时间计算目标时间差，包括：获取记录的上次轨迹修正的时间；计算目标观测时间与上次轨迹修正的时间的时间差，以获得目标时间差。
在第一方面的可选实施方式中，根据目标时间差、目标观测运动状态以及目标轨迹运动状态，确定轨迹预测运动状态，包括：将目标时间差、目标观测运动状态以及目标轨迹运动状态输入预设运动模型，并将预设运动模型输出的预测运动状态作为轨迹预测运动状态。
在第一方面的可选实施方式中，其中，目标观测数据包括目标观测时间和目标观测运动状态；根据目标观测数据确定轨迹误差值，包括：根据目标观测时间计算目标时间差；其中，目标时间差表示上次轨迹修正的时间到目标观测时间之间的时间差；利用预设误差校正方程，根据目标时间差和所述目标观测运动状态，计算轨迹误差值。本方案通过上次轨迹修正的时间到目标观测时间之间的时间差和目标观测运动状态以及预设误差校正方程计算轨迹误差值，从而使得计算得到的轨迹误差值更加贴近物体实际运动情况，进而提高轨迹误差值的准确度。
在第一方面的可选实施方式中，所述预设误差校正方程包括校正方程：
其中，为当前时刻误差状态量的预测值，为当前时刻的最终的滤波后状态误差，为上一时刻到当前时刻的运动方程，为上一时刻的观测误差状态，为上一时刻传感器噪声矩阵，表示传感器噪声模型，为当前时刻预测协方差矩阵，为上一时刻滤波后的协方差矩阵，为系统噪声，为卡尔曼增益，为当前时刻的运动方程，为当前时刻的观测噪声矩阵，为当前时刻滤波后的协方差矩阵，为当前时刻的目标观测量，为目标时间差；为连续时间运动方程；I21为单位矩阵。
在第一方面的可选实施方式中，在获取目标观测数据和目标轨迹运动状态之前，该方法还包括：获取预设时间差和目标轨迹运动状态；其中，预设时间差表示在物体的轨迹与观测数据匹配不成功的情况下，当前时间与上次轨迹修正的时间之间的时间差值；根据预设时间差和目标轨迹运动状态，确定第二轨迹预测运动状态；根据预设时间差，对第二轨迹预测运动状态的误差进行估计；根据估计的误差对第二轨迹预测运动状态进行修正。
第二方面，本发明提供一种轨迹修正装置，该装置包括获取模块、确定模块以及修正模块；获取模块，用于获取目标观测数据和目标轨迹运动状态；其中，目标观测数据表示在物体的轨迹与观测数据匹配成功的情况下的观测数据，目标轨迹运动状态表示在物体的轨迹与观测数据匹配成功的情况下的物体轨迹运动状态；确定模块，用于根据目标观测数据以及目标轨迹运动状态，确定轨迹预测运动状态；以及，根据目标观测数据确定轨迹误差值；修正模块，用于根据轨迹误差值，对轨迹预测运动状态进行修正。
上述设计的轨迹修正装置，本方案首先获取物体的轨迹与观测数据匹配成功的情况的目标观测数据和目标轨迹运动状态，然后根据目标观测数据和目标轨迹运动状态对物体的轨迹进行预测，确定轨迹预测运动状态，并且根据目标观测数据确定轨迹误差值，从而根据轨迹误差值对轨迹预测运动状态进行修正。由于本方案中的轨迹误差值是通过观测的物体实际运动情况下确定的，因此，确定的轨迹误差值相比传统强线性假设的方式更加贴近于物体的实际运动情况，即确定的轨迹误差值更加准确，使得车辆在感知盲区中的速度更新成为可能，弥补运动模型引起的误差，进而通过轨迹误差值对轨迹预测运动状态进行修正之后，可以使得预测的物体轨迹更加准确。
在第二方面的可选实施方式中，其中，目标观测数据包括目标观测时间和目标观测运动状态；该确定模块，具体用于根据目标观测时间，计算目标时间差；根据目标时间差、目标观测运动状态以及目标轨迹运动状态，确定轨迹预测运动状态。
在第二方面的可选实施方式中，该确定模块，还具体用于获取记录的上次轨迹修正的时间；计算目标观测时间与上次轨迹修正的时间的时间差，以获得目标时间差。
在第二方面的可选实施方式中，该确定模块，还具体用于将目标时间差、目标观测运动状态以及目标轨迹运动状态输入预设运动模型，并将预设运动模型输出的预测运动状态作为轨迹预测运动状态。
在第二方面的可选实施方式中，其中，目标观测数据包括目标观测时间和目标观测运动状态；该确定模块，还具体用于根据目标观测时间计算目标时间差；其中，目标时间差表示上次轨迹修正的时间到目标观测时间之间的时间差；利用预设误差校正方程，根据目标时间差和所述目标观测运动状态，计算轨迹误差值。
在第二方面的可选实施方式中，该获取模块，还用于获取预设时间差和目标轨迹运动状态；其中，预设时间差表示在物体的轨迹与观测数据匹配不成功的情况下，当前时间与上次轨迹修正的时间之间的时间差值；该确定模块，还用于根据预设时间差和目标轨迹运动状态，确定第二轨迹预测运动状态；估计模块，用于根据预设时间差，对第二轨迹预测运动状态的误差进行估计；该修正模块，还用于根据估计的误差对第二轨迹预测运动状态进行修正。
第三方面，本申请提供一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时执行第一方面、第一方面中任一可选的实现方式中的所述方法。
第四方面，本申请提供一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时执行第一方面、第一方面中任一可选的实现方式中的所述方法。
第五方面，本申请提供了一种计算机程序产品，所述计算机程序产品在计算机上运行时，使得计算机执行第一方面、第一方面中任一可选的实现方式中的所述方法。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1为本申请实施例提供的轨迹修正方法的第二流程示意图；
图2为本申请实施例提供的轨迹修正方法的第三流程示意图；
图3为本申请实施例提供的轨迹修正方法的第四流程示意图；
图4为本申请实施例提供的轨迹修正装置的结构示意图；
图5为本申请实施例提供的电子设备的结构示意图。
图标：400-获取模块；410-确定模块；420-修正模块；430-估计模块；5-电子设备；501-处理器；502-存储器；503-通信总线。
具体实施方式
下面将结合附图对本申请技术方案的实施例进行详细的描述。以下实施例仅用于更加清楚地说明本申请的技术方案，因此只作为示例，而不能以此来限制本申请的保护范围。
除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同；本文中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请；本申请的说明书和权利要求书及上述附图说明中的术语“包括”和“具有”以及它们的任何变形，意图在于覆盖不排他的包含。
在本申请实施例的描述中，技术术语“第一”“第二”等仅用于区别不同对象，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量、特定顺序或主次关系。在本申请实施例的描述中，“多个”的含义是两个以上，除非另有明确具体的限定。
在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。
在本申请实施例的描述中，术语“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。
在本申请实施例的描述中，术语“多个”指的是两个以上（包括两个），同理，“多组”指的是两组以上（包括两组），“多片”指的是两片以上（包括两片）。
在本申请实施例的描述中，技术术语“中心”“纵向”“横向”“长度”“宽度”“厚度”“上”“下”“前”“后”“左”“右”“竖直”“水平”“顶”“底”“内”“外”“顺时针”“逆时针”“轴向”“径向”“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请实施例的限制。
在本申请实施例的描述中，除非另有明确的规定和限定，技术术语“安装”“相连”“连接”“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；也可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请实施例中的具体含义。
本申请提供一种轨迹修正方法，该轨迹修正方法可应用于轨迹维持以及物体感知盲区预测的场景，该轨迹修正方法可应用于计算设备中，该计算设备可包括计算机、服务器、控制器、芯片以及上位机等，该轨迹修正算法可对物体当前维持的轨迹进行误差补偿，从而提高轨迹精度。如图1所示，该轨迹修正方法可通过如下方式实现，包括：
步骤S100：获取目标观测数据和目标轨迹运动状态。
步骤S110：根据目标观测数据以及目标轨迹运动状态，确定轨迹预测运动状态。
步骤S120：根据目标观测数据确定轨迹误差值。
步骤S130：根据轨迹误差值，对轨迹预测运动状态进行修正。
在上述设计的方案中，观测是指通过外部感知设备对目标物体进行观察，获得目标物体的特征，从而得到观测数据，观测数据例如可包括物体属性、速度特征、位置特征以及角速度特征等，其中，物体属性可包括物体的类别、形状、位置中心点等等。作为一种可能的实施方式，前文所描述的外部感知设备可以为多种传感器设备，即通过多种传感器设备对目标物体进行观测，其中，多种传感器设备包括但不限于多个相机和多种类型的激光雷达等等。
在上述实施方式中，目标观测数据表示在物体的轨迹与观测数据匹配成功的情况下的观测数据，目标轨迹运动状态表示在物体的轨迹与观测数据匹配成功的情况下的物体轨迹运动状态。
其中，物体的轨迹是指融合端将多个感知设备得到的一段时间的观测数据进行融合后得到物体运动数据信息，而多个感知设备对同一物体存在感知差异，因此，融合端形成的物体轨迹与观测数据可能由于多个感知设备的感知差异导致物体轨迹与观测数据的对象不匹配的情况，因此，需要对物体的轨迹与观测数据是否匹配进行判定。
具体的，本方案对物体的轨迹和观测数据进行判定的方式可通过如下方式：作为一种可能的实施方式，本方案可计算不同感知设备的感知结果之间的差异性，若差异性在预设差异范围，则表征物体的轨迹和观测数据匹配成功，若差异性不在预设差异范围，则表征物体的轨迹和观测数据匹配不成功。例如，本方案可计算不同感知设备的感知结果的欧式距离，若欧式距离在预设距离范围内，则表征物体的轨迹和观测数据匹配成功，若欧式距离不在预设距离范围内，则表征物体的轨迹和观测数据匹配不成功。
作为另一种可能的实施方式，本方案可计算融合端得到的物体轨迹的对象信息与每一感知设备的感知结果中的对象信息的差异，若差异在预设差异范围，则表征物体的轨迹和观测数据匹配成功，若差异不在预设差异范围，则表征物体的轨迹和观测数据匹配不成功。
本方案通过上述方式确定物体的轨迹与观测数据匹配成功的情况下，本方案即可获取物体的轨迹与观测数据匹配成功的时间节点下的观测数据和物体轨迹运动状态，从而获得目标观测数据和目标物体轨迹运动状态。
具体的，目标观测数据可包括目标观测时间和目标观测运动状态，该目标观测时间表示的是物体的轨迹与观测数据匹配成功的时间节点，目标观测运动状态表示的是物体的轨迹与观测数据匹配成功的时间节点下观测的物体的运动状态。目标物体轨迹运动状态表示的是物体的轨迹与观测数据匹配成功的时间节点下物体的运动状态，其中，物体的运动状态可包括物体的速度、物体的角速度、物体的位置、物体的航向角、物体的加速度等。
通过上述方式获得目标观测数据和目标物体轨迹运动状态的情况下，本方案可根据目标观测数据以及目标轨迹运动状态，对目标物体的轨迹进行预测，获得目标物体的轨迹预测运动状态，从而在现有轨迹的基础上进行运动预测。
另外，本方案还可根据目标观测数据确定轨迹误差值，即通过最新的感知结果作为观测量确定预测的轨迹预测运动状态的误差量，由于轨迹误差值是基于对目标物体进行实际观测确定的，因此，确定的轨迹误差值较为准确，在此基础上，本方案基于确定的轨迹误差值对轨迹预测运动状态进行修正，从而使得修正后的轨迹预测运动状态更加准确。
上述设计的轨迹修正方法，本方案首先获取物体的轨迹与观测数据匹配成功的情况的目标观测数据和目标轨迹运动状态，然后根据目标观测数据和目标轨迹运动状态对物体的轨迹进行预测，确定轨迹预测运动状态，并且根据目标观测数据确定轨迹误差值，从而根据轨迹误差值对轨迹预测运动状态进行修正。由于本方案中的轨迹误差值是通过观测的物体实际运动情况下确定的，因此，确定的轨迹误差值相比传统强线性假设的方式更加贴近于物体的实际运动情况，即确定的轨迹误差值更加准确，使得车辆在感知盲区中的速度更新成为可能，弥补运动模型引起的误差，进而通过轨迹误差值对轨迹预测运动状态进行修正之后，可以使得预测的物体轨迹更加准确。
在本实施例的可选实施方式中，前文描述到目标观测数据可包括目标观测时间和目标观测运动状态，在此基础上，对于步骤110可通过如下方式实现，如图2所示，包括：
步骤S200：根据目标观测时间，计算目标时间差。
步骤S210：根据目标时间差、目标观测运动状态以及目标轨迹运动状态，确定轨迹预测运动状态。
在上述实施方式中，目标时间差表示的是上次轨迹修正的时间到目标观测时间之间的时间差，即本方案预测的是在上次轨迹修正到当前轨迹与观测匹配成功这段时间的轨迹运动情况。
作为一种可能的实施方式，本方案可获取记录的上次轨迹修正的时间，然后计算目标观测时间与上次轨迹修正的时间的时间差，从而获得目标时间差。其中，上次轨迹修正的时间可具体为上次轨迹修正完毕时的时刻，也可以是上次轨迹修正过程中轨迹误差值生成的时刻，具体时刻点可根据实际应用场景进行适应性调整。
通过上述方式计算得到目标时间差的基础上，本方案可将目标时间差、目标观测运动状态以及目标轨迹运动状态输入预设运动模型中，获得预设运动模型输出的预测运动状态，从而得到轨迹预测运动状态。其中，预设运动模型可采用目前已有的匀速运动模型、匀加速运动模型以及匀速匀角速度运动模型等。
在本实施例的可选实施方式中，前文描述到目标观测数据可包括目标观测时间和目标观测运动状态，在此基础上，对于步骤120可通过如下方式实现，如图3所示，包括：
步骤S300：利用预设误差校正方程，根据目标时间差和目标观测运动状态，计算轨迹误差值。
在上述实施方式中，目标时间差与前文步骤S200描述的目标时间差的概念和计算方式一致，在这里不再赘述。
在计算得到目标时间差的情况下，本方案可利用预设误差校正方程，根据目标时间差和目标观测运动状态，计算轨迹误差值。
其中，所述预设误差校正方程包括校正方程：
其中，为当前时刻误差状态量的预测值，为当前时刻的最终的滤波后状态误差，为上一时刻到当前时刻的运动方程，为上一时刻的观测误差状态，为上一时刻传感器噪声矩阵，表示传感器噪声模型，为当前时刻预测协方差矩阵，为上一时刻滤波后的协方差矩阵，为系统噪声，为卡尔曼增益，为当前时刻的运动方程，为当前时刻的观测噪声矩阵，为当前时刻滤波后的协方差矩阵，为当前时刻的目标观测量，为目标时间差；为连续时间运动方程；I21为单位矩阵。
上述误差方程以卡尔曼滤波器的一般形式给出，可以表示为以下过程：
首先，进行预测状态量和观测状态量的设计，误差估计采取了高斯假设，以匀加速度运动为例，即：
；
；
其中，表示位置误差，表示速度误差，表示航向角误差，表示角速度误差，表示加速度误差，表示线加速度偏移误差，表示角速度偏移误差，为构造的观测量，表示位置误差，表示速度误差，表示航向角误差。
预测状态量可通过如下方式推导得出：
其中，、、、、分别表示目标位置、速度、加速度、航向角以及角加速度的微分误差方程，用于估计对应状态量的预测误差。、表示加速度和角速度在每一时刻的随机误差，由数学统计分析得出。、、、分别表示线加速度、角速度、线加速度零偏（匀速物体或静止物体线加速度偏差）、角速度零偏（直行物体角速度偏差）的随机游走噪声，由数学统计分析得出。
通过预测状态量的推导公式可以得出连续时间运动方程：
；
其中，表示单位矩阵。
观测状态量来源于预测位置与观测信息的差值，即：
；
；
；
其中，为观测位置误差，来源于预测位置信息和观测位置的差值；为速度误差，来源于预测速度信息和观测速度的差值；为航向角误差，来源于预测航向角信息和观测航向角的差值。
基于上述观测状态量的公式可得连续时间观测方程：
；
通过如下公式可得传感器噪声矩阵和观测噪声矩阵：
；
其中，、、、分别表示线加速度、角速度、线加速度零偏（匀速物体或静止物体线加速度偏差）、角速度零偏（直行物体角速度偏差）的随机游走噪声，由数学统计分析得出。
则噪声转移矩阵可以表示为：
；
观测噪声矩阵表示为：
；
其中，，,分别表示观测量在位置、速度和角度状态的观测噪声误差。
则观测噪声转移矩阵可以表示为
；
将上述所有公式整理并将其进行离散化，可得状态更新方程及观测转移方程：
其中，、以及均为对应的单位矩阵。
将上述公式用卡尔曼滤波一般形式即可得到校正方程。
前文描述的均是物体的轨迹与观测数据匹配成功的情况下的误差修正方式，在本实施例的可选实施方式中，在物体的轨迹与观测数据匹配不成功的情况下，本方案可获取预设时间差和目标估计运动状态，其中，该预设时间差表示物体的轨迹与观测数据匹配不成功的时间节点（当前时间）与上次轨迹修正的时间之间的时间差值。
然后根据预设时间差和目标轨迹运动状态确定第二轨迹预测运动状态，并且根据预设时间差对第二轨迹运动状态的误差进行估计，根据估计的误差对第二轨迹预测运动状态进行修正。
图4出示了本申请提供一种轨迹修正装置的示意性结构框图，应理解，该装置与图1至3中执行的方法实施例对应，能够执行前述的方法涉及的步骤，该装置具体的功能可以参见上文中的描述，为避免重复，此处适当省略详细描述。该装置包括至少一个能以软件或固件（firmware）的形式存储于存储器中或固化在装置的操作系统（operating system，OS）中的软件功能模块。具体地，该装置包括：获取模块400、确定模块410以及修正模块420；获取模块400，用于获取目标观测数据和目标轨迹运动状态；其中，目标观测数据表示在物体的轨迹与观测数据匹配成功的情况下的观测数据，目标轨迹运动状态表示在物体的轨迹与观测数据匹配成功的情况下的物体轨迹运动状态；确定模块410，用于根据目标观测数据以及目标轨迹运动状态，确定轨迹预测运动状态；以及，根据目标观测数据确定轨迹误差值；修正模块420，用于根据轨迹误差值，对轨迹预测运动状态进行修正。
上述设计的轨迹修正装置，本方案首先获取物体的轨迹与观测数据匹配成功的情况的目标观测数据和目标轨迹运动状态，然后根据目标观测数据和目标轨迹运动状态对物体的轨迹进行预测，确定轨迹预测运动状态，并且根据目标观测数据确定轨迹误差值，从而根据轨迹误差值对轨迹预测运动状态进行修正。由于本方案中的轨迹误差值是通过观测的物体实际运动情况下确定的，因此，确定的轨迹误差值相比传统强线性假设的方式更加贴近于物体的实际运动情况，即确定的轨迹误差值更加准确，进而通过轨迹误差值对轨迹预测运动状态进行修正之后，可以使得预测的物体轨迹更加准确。
在本实施例的可选实施方式中，其中，目标观测数据包括目标观测时间和目标观测运动状态；该确定模块410，具体用于根据目标观测时间，计算目标时间差；其中，目标时间差表示上次轨迹修正的时间到目标观测的时间之间的时间差；根据目标时间差、目标观测运动状态以及目标轨迹运动状态，确定轨迹预测运动状态。
在本实施例的可选实施方式中，该确定模块410，还具体用于获取记录的上次轨迹修正的时间；计算目标观测时间与上次轨迹修正的时间的时间差，以获得目标时间差。
在本实施例的可选实施方式中，该确定模块410，还具体用于将目标时间差、目标观测运动状态以及目标轨迹运动状态输入预设运动模型，并将预设运动模型输出的预测运动状态作为轨迹预测运动状态。
在本实施例的可选实施方式中，其中，目标观测数据包括目标观测时间和目标观测运动状态；该确定模块410，还具体用于根据目标观测时间计算目标时间差；其中，目标时间差表示上次轨迹修正的时间到目标观测时间之间的时间差；利用预设误差校正方程，根据目标时间差和所述目标观测运动状态，计算轨迹误差值。
在本实施例的可选实施方式中，该获取模块400，还用于获取预设时间差和目标轨迹运动状态；其中，预设时间差表示在物体的轨迹与观测数据匹配不成功的情况下，当前时间与上次轨迹修正的时间之间的时间差值；该确定模块410，还用于根据预设时间差和目标轨迹运动状态，确定第二轨迹预测运动状态；估计模块430，用于根据预设时间差，对第二轨迹预测运动状态的误差进行估计；该修正模块420，还用于根据估计的误差对第二轨迹预测运动状态进行修正。
根据本申请的一些实施例，如图5所示，本申请提供一种电子设备5，包括：处理器501和存储器502，处理器501和存储器502通过通信总线503和/或其他形式的连接机构（未标出）互连并相互通讯，存储器502存储有处理器501可执行的计算机程序，当计算设备运行时，处理器501执行该计算机程序，以执行时执行任一可选的实现方式中外端机执行的方法，例如步骤S100至步骤S130：获取目标观测数据和目标轨迹运动状态；根据目标观测数据以及目标轨迹运动状态，确定轨迹预测运动状态；根据目标观测数据确定轨迹误差值；根据轨迹误差值，对轨迹预测运动状态进行修正。
本申请提供一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器运行时执行前述任一可选的实现方式中的方法。
其中，存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（Static Random Access Memory, 简称SRAM），电可擦除可编程只读存储器（Electrically Erasable Programmable Read-Only Memory, 简称EEPROM），可擦除可编程只读存储器（Erasable Programmable Read Only Memory, 简称EPROM），可编程只读存储器（Programmable Red-Only Memory, 简称PROM），只读存储器（Read-OnlyMemory, 简称ROM），磁存储器，快闪存储器，磁盘或光盘。
本申请提供一种计算机程序产品，该计算机程序产品在计算机上运行时，使得计算机执行任一可选的实现方式中的方法。
最后应说明的是：以上各实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述各实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的范围，其均应涵盖在本申请的权利要求和说明书的范围当中。尤其是，只要不存在结构冲突，各个实施例中所提到的各项技术特征均可以任意方式组合起来。本申请并不局限于文中公开的特定实施例，而是包括落入权利要求的范围内的所有技术方案。
Claims (10)
1.一种轨迹修正方法，其特征在于，所述方法包括：
获取目标观测数据和目标轨迹运动状态；其中，所述目标观测数据表示在物体的轨迹与观测数据匹配成功的情况下的观测数据，所述目标轨迹运动状态表示在物体的轨迹与观测数据匹配成功的情况下的物体轨迹运动状态；
根据所述目标观测数据以及所述目标轨迹运动状态，确定轨迹预测运动状态；
根据所述目标观测数据确定轨迹误差值；
根据所述轨迹误差值，对所述轨迹预测运动状态进行修正。
2.根据权利要求1所述的方法，其特征在于，其中，所述目标观测数据包括目标观测时间和目标观测运动状态；
所述根据所述目标观测数据以及所述目标轨迹运动状态，确定轨迹预测运动状态，包括：
根据所述目标观测时间，计算目标时间差；
根据所述目标时间差、目标观测运动状态以及目标轨迹运动状态，确定所述轨迹预测运动状态。
3.根据权利要求2所述的方法，其特征在于，所述根据所述目标观测时间计算目标时间差，包括：
获取记录的上次轨迹修正的时间；
计算所述目标观测时间与所述上次轨迹修正的时间的时间差，以获得所述目标时间差。
4.根据权利要求2所述的方法，其特征在于，所述根据所述目标时间差、目标观测运动状态以及目标轨迹运动状态，确定所述轨迹预测运动状态，包括：
将所述目标时间差、目标观测运动状态以及目标轨迹运动状态输入预设运动模型，并将所述预设运动模型输出的预测运动状态作为所述轨迹预测运动状态。
5.根据权利要求1所述的方法，其特征在于，其中，所述目标观测数据包括目标观测时间和目标观测运动状态；
所述根据所述目标观测数据确定轨迹误差值，包括：
根据目标观测时间计算目标时间差；其中，所述目标时间差表示上次轨迹修正的时间到目标观测时间之间的时间差；
利用预设误差校正方程，根据所述目标时间差和所述目标观测运动状态，计算轨迹误差值。
6.根据权利要求5所述的方法，其特征在于，所述预设误差校正方程包括校正方程：
其中，为当前时刻误差状态量的预测值，为当前时刻的最终的滤波后状态误差，为上一时刻到当前时刻的运动方程，为上一时刻的观测误差状态，为上一时刻传感器噪声矩阵，表示传感器噪声模型，为当前时刻预测协方差矩阵，为上一时刻滤波后的协方差矩阵，为系统噪声，为卡尔曼增益，为当前时刻的运动方程，为当前时刻的观测噪声矩阵，为当前时刻滤波后的协方差矩阵，为当前时刻的目标观测量，为目标时间差；为连续时间运动方程；I21为单位矩阵。
7.根据权利要求1所述的方法，其特征在于，在所述获取目标观测数据和目标轨迹运动状态之前，所述方法还包括：
获取预设时间差和目标轨迹运动状态；其中，所述预设时间差表示在物体的轨迹与观测数据匹配不成功的情况下，当前时间与上次轨迹修正的时间之间的时间差值；
根据所述预设时间差和目标轨迹运动状态，确定第二轨迹预测运动状态；
根据所述预设时间差，对所述第二轨迹预测运动状态的误差进行估计；
根据估计的误差对所述第二轨迹预测运动状态进行修正。
8.一种轨迹修正装置，其特征在于，所述装置包括获取模块、确定模块以及修正模块；
所述获取模块，用于获取目标观测数据和目标轨迹运动状态；其中，所述目标观测数据表示在物体的轨迹与观测数据匹配成功的情况下的观测数据，所述目标轨迹运动状态表示在物体的轨迹与观测数据匹配成功的情况下的物体轨迹运动状态；
所述确定模块，用于根据所述目标观测数据以及所述目标轨迹运动状态，确定轨迹预测运动状态；以及，根据所述目标观测数据确定轨迹误差值；
所述修正模块，用于根据轨迹误差值，对所述轨迹预测运动状态进行修正。
9.一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述的方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310395800.7A CN116125819B (zh) | 2023-04-14 | 2023-04-14 | 轨迹修正方法、装置、电子设备和计算机可读存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310395800.7A CN116125819B (zh) | 2023-04-14 | 2023-04-14 | 轨迹修正方法、装置、电子设备和计算机可读存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116125819A true CN116125819A (zh) | 2023-05-16 |
| CN116125819B CN116125819B (zh) | 2023-07-07 |
Family
ID=86299399
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310395800.7A Active CN116125819B (zh) | 2023-04-14 | 2023-04-14 | 轨迹修正方法、装置、电子设备和计算机可读存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116125819B (zh) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2015094690A (ja) * | 2013-11-13 | 2015-05-18 | 株式会社デンソー | 車両用走行軌跡算出装置 |
| CN114001976A (zh) * | 2021-10-19 | 2022-02-01 | 杭州飞步科技有限公司 | 控制误差的确定方法、装置、设备及存储介质 |
| CN114620039A (zh) * | 2022-03-31 | 2022-06-14 | 阿波罗智能技术(北京)有限公司 | 轨迹修正方法、设备、云控平台和自动驾驶车辆 |
| CN114967465A (zh) * | 2022-06-02 | 2022-08-30 | 乐聚(深圳)机器人技术有限公司 | 轨迹规划方法、装置、电子设备及存储介质 |
| CN115157249A (zh) * | 2022-07-08 | 2022-10-11 | 深圳市优必选科技股份有限公司 | 一种动态目标跟踪方法、装置和机器人 |
| CN115507867A (zh) * | 2022-08-16 | 2022-12-23 | 福思(杭州)智能科技有限公司 | 目标轨迹预测方法、装置、电子装置和存储介质 |
| CN115743101A (zh) * | 2022-12-13 | 2023-03-07 | 中国重汽集团济南动力有限公司 | 车辆轨迹预测方法、轨迹预测模型训练方法和装置 |
-
2023
- 2023-04-14 CN CN202310395800.7A patent/CN116125819B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2015094690A (ja) * | 2013-11-13 | 2015-05-18 | 株式会社デンソー | 車両用走行軌跡算出装置 |
| CN114001976A (zh) * | 2021-10-19 | 2022-02-01 | 杭州飞步科技有限公司 | 控制误差的确定方法、装置、设备及存储介质 |
| CN114620039A (zh) * | 2022-03-31 | 2022-06-14 | 阿波罗智能技术(北京)有限公司 | 轨迹修正方法、设备、云控平台和自动驾驶车辆 |
| CN114967465A (zh) * | 2022-06-02 | 2022-08-30 | 乐聚(深圳)机器人技术有限公司 | 轨迹规划方法、装置、电子设备及存储介质 |
| CN115157249A (zh) * | 2022-07-08 | 2022-10-11 | 深圳市优必选科技股份有限公司 | 一种动态目标跟踪方法、装置和机器人 |
| CN115507867A (zh) * | 2022-08-16 | 2022-12-23 | 福思(杭州)智能科技有限公司 | 目标轨迹预测方法、装置、电子装置和存储介质 |
| CN115743101A (zh) * | 2022-12-13 | 2023-03-07 | 中国重汽集团济南动力有限公司 | 车辆轨迹预测方法、轨迹预测模型训练方法和装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116125819B (zh) | 2023-07-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP2876015B1 (en) | Apparatus and method for controlling lane keeping of vehicle | |
| US8824741B2 (en) | Method for estimating the roll angle in a travelling vehicle | |
| CN113514806B (zh) | 自动驾驶过程中障碍物确定方法、装置及电子设备 | |
| CN109059907B (zh) | 轨迹数据处理方法、装置、计算机设备和存储介质 | |
| JP2020134528A (ja) | 定常横方向偏差の除去方法、装置、記憶媒体、及びプログラム | |
| CN112257698B (zh) | 环视图车位检测结果的处理方法、装置、设备和存储介质 | |
| WO2018062291A1 (ja) | 他車線監視装置 | |
| CN110637209B (zh) | 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质 | |
| CN113551664A (zh) | 一种地图构建方法、装置、电子设备及存储介质 | |
| CN110398966B (zh) | 一种路径跟踪控制方法及路径跟踪系统 | |
| CN116645396A (zh) | 轨迹确定方法、装置、计算机可读存储介质及电子设备 | |
| CN116125819B (zh) | 轨迹修正方法、装置、电子设备和计算机可读存储介质 | |
| JP2020003463A (ja) | 自車位置推定装置 | |
| CN115451968A (zh) | 智能驾驶的多传感器同步融合方法和装置 | |
| CN111161324A (zh) | 一种基于自适应多模更新策略的目标跟踪方法 | |
| CN112874529B (zh) | 基于事件触发状态估计的车辆质心侧偏角估计方法及系统 | |
| JP2019206208A (ja) | 画像認識装置、画像認識方法、及び駐車支援システム | |
| CN111044053B (zh) | 单舵轮无人车的导航方法、装置和单舵轮无人车 | |
| CN115979288A (zh) | 一种航向角确定方法、电子设备和存储介质 | |
| CN113306559A (zh) | 在路形估计中对于垂直道路曲度的补偿 | |
| CN115388880B (zh) | 一种低成本记忆泊车建图与定位方法、装置及电子设备 | |
| CN115900735A (zh) | 车辆定位方法、装置、车辆及存储介质 | |
| CN113932815B (zh) | 稳健性优化Kalman滤波相对导航方法、装置、设备和存储介质 | |
| KR102427366B1 (ko) | 심층 신경망을 이용한 차선 추정 방법 및 장치 | |
| CN114643988A (zh) | 跟踪目标确定方法和装置、电子设备和存储介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |