CN114001976B - 控制误差的确定方法、装置、设备及存储介质 - Google Patents

控制误差的确定方法、装置、设备及存储介质 Download PDF

Info

Publication number
CN114001976B
CN114001976B CN202111216720.8A CN202111216720A CN114001976B CN 114001976 B CN114001976 B CN 114001976B CN 202111216720 A CN202111216720 A CN 202111216720A CN 114001976 B CN114001976 B CN 114001976B
Authority
CN
China
Prior art keywords
time point
target
determining
state vector
control error
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.)
Active
Application number
CN202111216720.8A
Other languages
English (en)
Other versions
CN114001976A (zh
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.)
Hangzhou Fabu Technology Co Ltd
Original Assignee
Hangzhou Fabu 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 Hangzhou Fabu Technology Co Ltd filed Critical Hangzhou Fabu Technology Co Ltd
Priority to CN202111216720.8A priority Critical patent/CN114001976B/zh
Publication of CN114001976A publication Critical patent/CN114001976A/zh
Application granted granted Critical
Publication of CN114001976B publication Critical patent/CN114001976B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01M—TESTING STATIC OR DYNAMIC BALANCE OF MACHINES OR STRUCTURES; TESTING OF STRUCTURES OR APPARATUS, NOT OTHERWISE PROVIDED FOR
    • G01M17/00—Testing of vehicles
    • G01M17/007—Wheeled or endless-tracked vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Feedback Control In General (AREA)
  • Traffic Control Systems (AREA)

Abstract

本申请提供一种控制误差的确定方法、装置、设备及存储介质，该方法包括：获取目标轨迹的相关数据，相关数据包括N个时间点、目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度，针对各时间点执行以下步骤，以得到车辆执行目标轨迹时的控制误差：基于卡尔曼滤波，确定第n个时间点的状态向量，n为大于或等于1且小于或等于N的整数，基于误差模型，根据第n个时间点的目标航向角变化率、目标车辆速度和状态向量，确定第n个时间点的控制误差，误差模型是根据车辆的历史轨迹数据获得的，误差模型用于反映航向角变化率、车辆速度和控制误差的对应关系。本申请能够准确地确定车辆执行目标轨迹时的控制误差。

Description

控制误差的确定方法、装置、设备及存储介质
技术领域
本申请涉及车辆控制技术领域，尤其涉及一种控制误差的确定方法、装置、设备及存储介质。
背景技术
近年来，车辆的自动驾驶技术越来越成熟。随之而来的是，应用了自动驾驶技术的车辆，用途也越来越广泛。
目前，车辆的自动驾驶是车辆执行预设的目标轨迹在道路上自动行驶。而在车辆自动行驶过程中，车辆的实际轨迹可能会偏离目标轨迹，即实际轨迹与目标轨迹在横向位置存在控制误差。在控制误差较大时，说明目标轨迹的设置是不合理的。因此，为给车辆设置合理的目标轨迹，如何准确地确定车辆执行目标轨迹的控制误差，是目前亟待解决的问题。
发明内容
本申请提供一种控制误差的确定方法、装置、设备及存储介质，以准确地确定车辆执行目标轨迹的控制误差。
第一方面，本申请提供一种控制误差的确定方法，包括：
获取目标轨迹的相关数据，相关数据包括N个时间点、目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度，N为大于或等于1的整数；
针对各时间点执行以下步骤，以得到车辆执行目标轨迹时的控制误差：
基于卡尔曼滤波，确定第n个时间点的状态向量，n为大于或等于1且小于或等于N的整数；
基于误差模型，根据第n个时间点的目标航向角变化率、目标车辆速度和状态向量，确定第n个时间点的控制误差，误差模型是根据车辆的历史轨迹数据获得的，误差模型用于反映航向角变化率、车辆速度和控制误差的对应关系。
可选的，基于卡尔曼滤波，确定第n个时间点的状态向量，包括：根据第n-1个时间点获得的卡尔曼滤波数据，获得对应的卡尔曼增益，卡尔曼滤波数据包括第n-1个时间点的状态向量的协方差矩阵和观测方程的协方差矩阵；根据卡尔曼增益，更新第n-1个时间点的状态向量和状态向量的协方差矩阵；基于误差模型，根据更新后的第n-1个时间点的状态向量以及第n-1个时间点的目标航向角变化率和目标车辆速度，确定第n个时间点的状态向量。
可选的，基于卡尔曼滤波，确定第n个时间点的状态向量之前，还包括：基于误差模型，初始化卡尔曼滤波对应的状态方程的协方差矩阵和观测方程的协方差矩阵；根据状态方程的协方差矩阵，初始化状态向量的协方差矩阵。
可选的，得到车辆执行目标轨迹时的控制误差，包括：基于预测步长，获取预测步长包含的M个时间点分别对应的控制误差，M为大于1且小于或等于N的整数；根据M个时间点分别对应的控制误差，确定目标轨迹的控制误差。
可选的，该控制误差的确定方法还包括：获取历史轨迹的相关数据，历史轨迹的相关数据包括T个时间点、历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差，T为大于等于1的整数；根据历史轨迹的相关数据和初始状态空间方程，确定误差模型。
可选的，根据历史轨迹的相关数据和初始状态空间方程，确定误差模型，包括：根据历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差以及初始状态空间方程，将初始状态空间方程转化为矩阵方程；根据矩阵方程，采用最小二乘法，获取误差模型对应的各***方程参数；根据各***方程参数，确定误差模型。
第二方面，本申请提供一种控制误差的确定装置，包括：
获取模块，用于获取目标轨迹的相关数据，相关数据包括N个时间点、目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度，N为大于或等于1的整数；
处理模块，用于针对各时间点执行以下步骤，以得到车辆执行目标轨迹时的控制误差：
基于卡尔曼滤波，确定第n个时间点的状态向量，n为大于或等于1且小于或等于N的整数；
基于误差模型，根据第n个时间点的目标航向角变化率、目标车辆速度和状态向量，确定第n个时间点的控制误差，误差模型是根据车辆的历史轨迹数据获得的，误差模型用于反映航向角变化率、车辆速度和控制误差的对应关系。
可选的，处理模块具体用于：根据第n-1个时间点获得的卡尔曼滤波数据，获得对应的卡尔曼增益，卡尔曼滤波数据包括第n-1个时间点的状态向量的协方差矩阵和观测方程的协方差矩阵；根据卡尔曼增益，更新第n-1个时间点的状态向量和状态向量的协方差矩阵；基于误差模型，根据更新后的第n-1个时间点的状态向量以及第n-1个时间点的目标航向角变化率和目标车辆速度，确定第n个时间点的状态向量。
可选的，处理模块还用于，在基于卡尔曼滤波，确定第n个时间点的状态向量之前，基于误差模型，初始化卡尔曼滤波对应的状态方程的协方差矩阵和观测方程的协方差矩阵；根据状态方程的协方差矩阵，初始化状态向量的协方差矩阵。
可选的，处理模块具体用于：基于预测步长，获取预测步长包含的M个时间点分别对应的控制误差，M为大于1且小于或等于N的整数；根据M个时间点分别对应的控制误差，确定目标轨迹的控制误差。
可选的，该控制误差的确定装置还包括确定模块，用于获取历史轨迹的相关数据，历史轨迹的相关数据包括T个时间点、历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差，T为大于等于1的整数；根据历史轨迹的相关数据和初始状态空间方程，确定误差模型。
可选的，确定模块具体用于：根据历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差以及初始状态空间方程，将初始状态空间方程转化为矩阵方程；根据矩阵方程，采用最小二乘法，获取误差模型对应的各***方程参数；根据各***方程参数，确定误差模型。
第三方面，本申请提供一种电子设备，包括：处理器，以及与处理器通信连接的存储器；
存储器存储计算机执行指令；
处理器执行存储器存储的计算机执行指令，以实现如本申请第一方面所述的控制误差的确定方法。
第四方面，本申请一种计算机可读存储介质，计算机可读存储介质中存储有计算机程序指令，计算机程序指令被处理器执行时，实现如本申请第一方面所述的控制误差的确定方法。
第五方面，本申请提供一种计算机程序产品，包括计算机程序，计算机程序被处理器执行时实现如本申请第一方面所述的控制误差的确定方法。
本申请提供的控制误差的确定方法、装置、设备及存储介质，通过获取目标轨迹的相关数据，相关数据包括N个时间点、目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度，针对各时间点执行以下步骤，以得到车辆执行目标轨迹时的控制误差：基于卡尔曼滤波，确定第n个时间点的状态向量，n为大于或等于1且小于或等于N的整数，基于误差模型，根据第n个时间点的目标航向角变化率、目标车辆速度和状态向量，确定第n个时间点的控制误差，误差模型是根据车辆的历史轨迹数据获得的，误差模型用于反映航向角变化率、车辆速度和控制误差的对应关系。由于本申请基于根据车辆的历史轨迹数据获得的误差模型，进行卡尔曼滤波处理，确定车辆执行目标轨迹时的控制误差，因此，能够准确地确定车辆执行目标轨迹的控制误差，从而根据目标轨迹的控制误差为给车辆设置更加合理的目标轨迹。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做一简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本申请一实施例提供的应用场景示意图；
图2为本申请一实施例提供的控制误差的确定方法的流程图；
图3为本申请另一实施例提供的控制误差的确定方法的流程图；
图4为本申请又一实施例提供的控制误差的确定方法的流程图；
图5为本申请一实施例提供的误差模型的确定方法流程图；
图6(a)为本申请一实施例提供的真实的控制误差的示意图；
图6(b)为本申请一实施例提供的基于角度误差模型输出的控制误差的示意图；
图6(c)为本申请一实施例提供的基于角度误差模型进行卡尔曼滤波处理输出的控制误差的示意图；
图7为本申请一实施例提供的控制误差的确定装置的结构示意图；
图8为本申请一实施例提供的电子设备的结构示意图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在控制领域，由于控制对象的不确定性，控制算法能保证控制误差是收敛，但无法对未来控制误差进行预测。目前，自动驾驶车辆按照预设的目标轨迹在道路上自动行驶时，车辆的实际轨迹可能会偏离目标轨迹，即实际轨迹与目标轨迹在横向位置存在控制误差，因此，如何准确地确定车辆执行目标轨迹的控制误差，是目前亟待解决的问题。
基于上述问题，本申请提供一种控制误差的确定方法、装置、设备及存储介质，基于建立的误差模型进行卡尔曼滤波处理，能够准确地确定车辆执行目标轨迹的控制误差。
以下，首先对本申请提供的方案的应用场景进行示例说明。
图1为本申请一实施例提供的应用场景示意图。如图1所示，本应用场景中，自动驾驶港口集卡110在港区内部道路120上行驶前，基于预先确定的目标轨迹，确定执行该目标轨迹的控制误差，进而参考该控制误差设置合理的目标轨迹，根据设置后的目标轨迹在港区内部道路120上行驶。其中，自动驾驶港口集卡110基于预先确定的目标轨迹，确定执行该目标轨迹的控制误差的具体实现过程可以参见下述各实施例的方案。
需要说明的是，图1仅是本申请实施例提供的一种应用场景的示意图，本申请实施例不对图1中包括的设备进行限定，也不对图1中设备之间的位置关系进行限定。
接下来，通过具体实施例介绍控制误差的确定方法。
图2为本申请一实施例提供的控制误差的确定方法的流程图。本申请实施例的方法可以应用于电子设备中，该电子设备例如为自动驾驶车辆的微控制单元(Microcontroller Unit，MCU)。如图2所示，本申请实施例的方法包括：
S201、获取目标轨迹的相关数据，相关数据包括N个时间点、目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度。
其中，N为大于或等于1的整数。
本申请实施例中，示例性地，已经预先确定了自动驾驶车辆对应的目标轨迹，可以理解，该目标轨迹对应有多个时间点的目标航向角变化率和目标车辆速度。因此，响应于控制误差的确定指令，可以根据目标轨迹获取目标轨迹的相关数据，该相关数据包括N个时间点、目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度。
针对S201步骤中各时间点执行以下步骤，以得到车辆执行目标轨迹时的控制误差：
S202、基于卡尔曼滤波，确定第n个时间点的状态向量，n为大于或等于1且小于或等于N的整数。
S203、基于误差模型，根据第n个时间点的目标航向角变化率、目标车辆速度和状态向量，确定第n个时间点的控制误差，误差模型是根据车辆的历史轨迹数据获得的，误差模型用于反映航向角变化率、车辆速度和控制误差的对应关系。
示例性地，误差模型比如为如下的状态空间方程：
xk+1＝Axk+Buk
yk＝Cxk+Duk
其中，A、B、C、D表示***方程参数；xk表示第k个时间点的状态向量；xk+1表示第k+1个时间点的状态向量；yk表示第k个时间点误差模型输出的控制误差；uk表示第k个时间点误差模型的输入向量，uk＝[v,w]，其中，v表示第k个时间点的目标车辆速度，w表示第k个时间点车辆目标航向角变化率。
误差模型中，已经确定了A、B、C、D这四个***方程参数，将xk、uk输入至误差模型中，可以得到误差模型输出的控制误差yk。
示例性地，可以预先确定目标轨迹对应的初始状态向量，基于卡尔曼滤波和初始状态向量，确定第1个时间点的状态向量，将第1个时间点的目标航向角变化率、目标车辆速度和状态向量输入至误差模型(即上述状态空间方程)，确定第1个时间点的控制误差；然后基于卡尔曼滤波和第1个时间点的状态向量，确定第2个时间点的状态向量，将第2个时间点的目标航向角变化率、目标车辆速度和状态向量输入至误差模型，确定第2个时间点的控制误差。依次类推，可以基于卡尔曼滤波，确定第n个时间点的状态向量，将第n个时间点的目标航向角变化率、目标车辆速度和状态向量输入至误差模型，确定第n个时间点的控制误差。从而得到目标轨迹对应各时间点的控制误差，进而得到车辆执行目标轨迹时的控制误差。对于如何根据基于卡尔曼滤波，确定第n个时间点的状态向量，可参考后续实施例，此处不再赘述。
可以理解，S202步骤和S203步骤是针对N个时间点中包含的每一个时间点，逐个时间点进行迭代处理的过程。
在得到了车辆执行目标轨迹时的控制误差后，可以参考该控制误差为车辆设置更加合理的目标轨迹。
本申请实施例提供的控制误差的确定方法，通过获取目标轨迹的相关数据，相关数据包括N个时间点、目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度，针对各时间点执行以下步骤，以得到车辆执行目标轨迹时的控制误差：基于卡尔曼滤波，确定第n个时间点的状态向量，n为大于或等于1且小于或等于N的整数，基于误差模型，根据第n个时间点的目标航向角变化率、目标车辆速度和状态向量，确定第n个时间点的控制误差，误差模型是根据车辆的历史轨迹数据获得的，误差模型用于反映航向角变化率、车辆速度和控制误差的对应关系。由于本申请实施例基于根据车辆的历史轨迹数据获得的误差模型，进行卡尔曼滤波处理，确定车辆执行目标轨迹时的控制误差，因此，能够准确地确定车辆执行目标轨迹的控制误差，从而根据目标轨迹的控制误差为给车辆设置更加合理的目标轨迹。
图3为本申请另一实施例提供的控制误差的确定方法的流程图。在上述实施例的基础上，本申请实施例对如何确定控制误差进行进一步说明。如图3所示，本申请实施例的方法可以包括：
S301、获取目标轨迹的相关数据，相关数据包括N个时间点、目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度。
其中，N为大于或等于1的整数。
该步骤的具体描述可以参见图2所示实施例中S201的相关描述，此处不再赘述。
S302、基于误差模型，初始化卡尔曼滤波对应的状态方程的协方差矩阵和观测方程的协方差矩阵。
该步骤中，示例性，误差模型比如为上述实施例中的状态空间方程，因此，可以基于误差模型，初始化卡尔曼滤波对应的状态方程的协方差矩阵，该状态方程的协方差矩阵比如用Q表示，并可以初始化卡尔曼滤波对应的观测方程的协方差矩阵，该观测方程的协方差矩阵比如用R表示。
S303、根据状态方程的协方差矩阵，初始化状态向量的协方差矩阵。
示例性地，在获得了状态方程的协方差矩阵Q后，可以根据状态方程的协方差矩阵Q，可以通过如下公式一初始化状态向量的协方差矩阵：
P＝B×Q×BT 公式一
其中，P表示状态向量的协方差矩阵；BT表示B的转置矩阵。
针对S301步骤中各时间点执行以下步骤，以得到车辆执行目标轨迹时的控制误差：
本申请实施例中，图2中S202步骤可以进一步包括如下的S304至S306三个步骤：
S304、根据第n-1个时间点获得的卡尔曼滤波数据，获得对应的卡尔曼增益，卡尔曼滤波数据包括第n-1个时间点的状态向量的协方差矩阵和观测方程的协方差矩阵。
示例性地，根据第n-1个时间点的状态向量的协方差矩阵P和观测方程的协方差矩阵R，可以通过如下公式二获得第n-1个时间点对应的卡尔曼增益：
Mn-1＝P×CT/(C×P×CT+R) 公式二
其中，CT表示C的转置矩阵。
S305、根据卡尔曼增益，更新第n-1个时间点的状态向量和状态向量的协方差矩阵。
示例性地，在获得了第n-1个时间点对应的卡尔曼增益Mn-1后，可以通过如下公式三更新第n-1个时间点的状态向量和状态向量的协方差矩阵：
其中，表示更新后的第n-1个时间点的状态向量；xn-1表示更新前的第n-1个时间点的状态向量；yn-1表示第n-1个时间点的控制误差；un-1表示第n-1个时间点误差模型的输入向量，un-1＝[vn-1,wn-1]，其中，vn-1表示第n-1个时间点的目标车辆速度，wn-1表示第n-1个时间点的目标航向角变化率。
同时，可以通过如下公式四更新第n-1个时间点的状态向量的协方差矩阵：
其中，表示更新后的第n-1个时间点的状态向量的协方差矩阵；Pn-1表示更新前的第n-1个时间点的状态向量的协方差矩阵；I表示对角线上的元素为1的单位矩阵。
S306、基于误差模型，根据更新后的第n-1个时间点的状态向量以及第n-1个时间点的目标航向角变化率和目标车辆速度，确定第n个时间点的状态向量。
示例性地，在获得了更新后的第n-1个时间点的状态向量和状态向量的协方差矩阵之后，可以根据更新后的第n-1个时间点的状态向量以及第n-1个时间点的目标航向角变化率和目标车辆速度，通过如下公式五确定第n个时间点的状态向量xn：
同时，可以通过如下公式六获得第n个时间点的状态向量的协方差矩阵：
其中，Pn表示第n个时间点的状态向量的协方差矩阵；AT表示A的转置矩阵。
S307、基于误差模型，根据第n个时间点的目标航向角变化率、目标车辆速度和状态向量，确定第n个时间点的控制误差。
该步骤的具体描述可以参见图2所示实施例中S203的相关描述，此处不再赘述。
可以理解，S304步骤至S307步骤是针对N个时间点中包含的每一个时间点，逐个时间点进行迭代处理的过程。
本申请实施例提供的控制误差的确定方法，通过获取目标轨迹的相关数据，相关数据包括N个时间点、目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度，基于误差模型，初始化卡尔曼滤波对应的状态方程的协方差矩阵和观测方程的协方差矩阵，根据状态方程的协方差矩阵，初始化状态向量的协方差矩阵，针对各时间点执行以下步骤，以得到车辆执行目标轨迹时的控制误差：根据第n-1个时间点获得的卡尔曼滤波数据，获得对应的卡尔曼增益，卡尔曼滤波数据包括第n-1个时间点的状态向量的协方差矩阵和观测方程的协方差矩阵，根据卡尔曼增益，更新第n-1个时间点的状态向量和状态向量的协方差矩阵，基于误差模型，根据更新后的第n-1个时间点的状态向量以及第n-1个时间点的目标航向角变化率和目标车辆速度，确定第n个时间点的状态向量，基于误差模型，根据第n个时间点的目标航向角变化率、目标车辆速度和状态向量，确定第n个时间点的控制误差。由于本申请实施例基于根据车辆的历史轨迹数据获得的误差模型，进行卡尔曼滤波处理，确定车辆执行目标轨迹时的控制误差，因此，能够准确地确定车辆执行目标轨迹的控制误差，从而根据目标轨迹的控制误差为给车辆设置更加合理的目标轨迹。
在上述实施例的基础上，图4为本申请又一实施例提供的控制误差的确定方法的流程图。如图4所示，本申请实施例的方法可以包括：
S401、获取目标轨迹的相关数据，相关数据包括N个时间点、目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度。
其中，N为大于或等于1的整数。
该步骤的具体描述可以参见图2所示实施例中S201的相关描述，此处不再赘述。
S402、基于预测步长，获取预测步长包含的M个时间点分别对应的控制误差，M为大于1且小于或等于N的整数。
示例性地，假设目标轨迹包括两个预测步长，分别为第一预测步长、第二预测步长。每一个预测步长包含M个时间点，则可以根据上述S304至S306三个步骤确定每一个预测步长对应的状态向量，然后针对每一个预测步长包含的M个时间点中的各时间点执行以下步骤，以得到M个时间点分别对应的控制误差：
基于误差模型，根据第m个时间点的目标航向角变化率、目标车辆速度和预测步长对应的状态向量，确定第m个时间点的控制误差。其中，该误差模型比如为上述实施例中的状态空间方程。
S403、根据M个时间点分别对应的控制误差，确定目标轨迹的控制误差。
该步骤中，在获得了预测步长包含的M个时间点分别对应的控制误差后，可以根据M个时间点分别对应的控制误差，确定目标轨迹的控制误差。
本申请实施例提供的控制误差的确定方法，能够基于预测步长，准确地得到车辆执行目标轨迹时的控制误差，从而根据目标轨迹的控制误差为给车辆设置更加合理的目标轨迹。
图5为本申请一实施例提供的误差模型的确定方法流程图。如图5所示，本申请实施例的方法可以包括：
S501、获取历史轨迹的相关数据，历史轨迹的相关数据包括T个时间点、历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差。
其中，T为大于等于1的整数。
示例性地，可以根据已经存储的车辆的历史轨迹，获取历史轨迹的相关数据，历史轨迹的相关数据可以包括T个时间点、历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差。其中，可以通过传感器获得实际车辆速度；实际控制误差可以为车辆的角度误差，或者，车辆的位置误差，若实际控制误差为车辆的角度误差，则可以通过传感器获得车辆运行时的实际朝向角度，进而确定车辆的角度误差为车辆运行时的实际朝向角度与车辆历史轨迹的目标朝向角度的差值。
S502、根据历史轨迹的相关数据和初始状态空间方程，确定误差模型。
该步骤中，在获得了历史轨迹的相关数据后，基于上述实施例中的状态空间方程，可以确定误差模型。
进一步地，根据历史轨迹的相关数据和初始状态空间方程，确定误差模型，可以包括：根据历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差以及初始状态空间方程，将初始状态空间方程转化为矩阵方程；根据矩阵方程，采用最小二乘法，获取误差模型对应的各***方程参数；根据各***方程参数，确定误差模型。
示例性地，初始状态空间方程比如上述实施例中的状态空间方程，其中***方程参数A、B、C、D是待确定的参数。根据获得的历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差，将初始状态空间方程转化为如下的矩阵方程：
然后，根据该矩阵方程，采用最小二乘法，求解如下优化方程，以获取误差模型对应的各***方程参数A、B、C、D：
在确定了***方程参数A、B、C、D后，可以根据各***方程参数，确定误差模型，即获得了上述实施例中状态空间方程的***方程参数A、B、C、D分别对应的取值。
可选的，上述实施例中的***方程参数A、B、C、D的阶数可以为多阶设置。示例性地，***方程参数A、B、C、D比如设置为3阶(即3*3的矩阵)，采用上述实施例中获取***方程参数A、B、C、D的方法获得的***方程参数A、B、C、D，能够更加准确地用于确定车辆执行目标轨迹时的控制误差。
在上述实施例的基础上，示例性地，可以根据历史轨迹包含的各个时间点的车辆的角度误差，获得对应的角度误差模型，或者，可以根据历史轨迹包含的各个时间点的车辆的位置误差，获得对应的位置误差模型，或者，可以根据历史轨迹包含的各个时间点的车辆的纵向速度误差，获得对应的纵向速度误差模型。
基于本申请实施例提供的误差模型的确定方法确定的误差模型，进行卡尔曼滤波处理，能够准确地确定车辆执行目标轨迹时的控制误差。
图6(a)为本申请一实施例提供的真实的控制误差的示意图，图6(b)为本申请一实施例提供的基于角度误差模型输出的控制误差的示意图，图6(c)为本申请一实施例提供的基于角度误差模型进行卡尔曼滤波处理输出的控制误差的示意图。如图6(a)至图6(c)所示，在相同的坐标系(横坐标表示待确定控制误差的轨迹点的个数，纵坐标表示角度误差)中，基于图6(a)，从图6(b)可以看出，利用角度误差模型可以比较准确地得到当前时刻对应的控制误差；基于图6(a)，从图6(c)可以看出，利用角度误差模型也能够准确地预测出未来时刻的控制误差。
下述为本申请装置实施例，可以用于执行本申请方法实施例。对于本申请装置实施例中未披露的细节，请参照本申请方法实施例。
图7为本申请一实施例提供的控制误差的确定装置的结构示意图，如图7所示，本申请实施例的控制误差的确定装置700包括：获取模块701和处理模块702。其中：
获取模块701，用于获取目标轨迹的相关数据，相关数据包括N个时间点、目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度，N为大于或等于1的整数。
处理模块702，用于针对各时间点执行以下步骤，以得到车辆执行目标轨迹时的控制误差：
基于卡尔曼滤波，确定第n个时间点的状态向量，n为大于或等于1且小于或等于N的整数；
基于误差模型，根据第n个时间点的目标航向角变化率、目标车辆速度和状态向量，确定第n个时间点的控制误差，误差模型是根据车辆的历史轨迹数据获得的，误差模型用于反映航向角变化率、车辆速度和控制误差的对应关系。
一些实施例中，处理模块702可以具体用于：根据第n-1个时间点获得的卡尔曼滤波数据，获得对应的卡尔曼增益，卡尔曼滤波数据包括第n-1个时间点的状态向量的协方差矩阵和观测方程的协方差矩阵；根据卡尔曼增益，更新第n-1个时间点的状态向量和状态向量的协方差矩阵；基于误差模型，根据更新后的第n-1个时间点的状态向量以及第n-1个时间点的目标航向角变化率和目标车辆速度，确定第n个时间点的状态向量。
可选的，处理模块702还可以用于，在基于卡尔曼滤波，确定第n个时间点的状态向量之前，基于误差模型，初始化卡尔曼滤波对应的状态方程的协方差矩阵和观测方程的协方差矩阵；根据状态方程的协方差矩阵，初始化状态向量的协方差矩阵。
可选的，处理模块702可以具体用于：基于预测步长，获取预测步长包含的M个时间点分别对应的控制误差，M为大于1且小于或等于N的整数；根据M个时间点分别对应的控制误差，确定目标轨迹的控制误差。
一些实施例中，该控制误差的确定装置700还包括确定模块703，用于获取历史轨迹的相关数据，历史轨迹的相关数据包括T个时间点、历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差，T为大于等于1的整数；根据历史轨迹的相关数据和初始状态空间方程，确定误差模型。
可选的，确定模块703可以具体用于：根据历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差以及初始状态空间方程，将初始状态空间方程转化为矩阵方程；根据矩阵方程，采用最小二乘法，获取误差模型对应的各***方程参数；根据各***方程参数，确定误差模型。
本实施例的装置，可以用于执行上述任一所示方法实施例的技术方案，其实现原理和技术效果类似，此处不再赘述。
图8为本申请一实施例提供的电子设备的结构示意图。示例性地，电子设备例如为自动驾驶车辆的MCU。参照图8，电子设备800包括处理组件801，其进一步包括一个或多个处理器，以及由存储器802所代表的存储器资源，用于存储可由处理组件801的执行的指令，例如应用程序。存储器802中存储的应用程序可以包括一个或一个以上的每一个对应于一组指令的模块。此外，处理组件801被配置为执行指令，以执行上述任一方法实施例。
电子设备800还可以包括一个电源组件803被配置为执行电子设备800的电源管理，一个有线或无线网络接口804被配置为将电子设备800连接到网络，和一个输入输出(I/O)接口805。电子设备800可以操作基于存储在存储器602的操作***，例如WindowsServerTM，Mac OS XTM，UnixTM,LinuxTM，FreeBSDTM或类似。
本申请还提供一种计算机可读存储介质，计算机可读存储介质中存储有计算机执行指令，当处理器执行计算机执行指令时，实现如上的控制误差的确定方法的方案。
本申请还提供一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现如上的控制误差的确定方法的方案。
上述的计算机可读存储介质，上述可读存储介质可以是由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器(SRAM)，电可擦除可编程只读存储器(EEPROM)，可擦除可编程只读存储器(EPROM)，可编程只读存储器(PROM)，只读存储器(ROM)，磁存储器，快闪存储器，磁盘或光盘。可读存储介质可以是通用或专用计算机能够存取的任何可用介质。
一种示例性的可读存储介质耦合至处理器，从而使处理器能够从该可读存储介质读取信息，且可向该可读存储介质写入信息。当然，可读存储介质也可以是处理器的组成部分。处理器和可读存储介质可以位于专用集成电路(Application Specific IntegratedCircuits，ASIC)中。当然，处理器和可读存储介质也可以作为分立组件存在于控制误差的确定装置中。
本领域普通技术人员可以理解：实现上述各方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成。前述的程序可以存储于一计算机可读取存储介质中。该程序在执行时，执行包括上述各方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。
最后应说明的是：以上各实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述各实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的范围。

Claims (9)

1.一种控制误差的确定方法，其特征在于，包括：
获取目标轨迹的相关数据，所述相关数据包括N个时间点、所述目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度，N为大于或等于1的整数；
针对各时间点执行以下步骤，以得到车辆执行所述目标轨迹时的控制误差：
基于卡尔曼滤波，确定第n个时间点的状态向量，n为大于或等于1且小于或等于N的整数；
基于误差模型，根据第n个时间点的目标航向角变化率、目标车辆速度和状态向量，确定第n个时间点的控制误差，所述误差模型是根据车辆的历史轨迹数据获得的，所述误差模型用于反映航向角变化率、车辆速度和控制误差的对应关系；
所述基于卡尔曼滤波，确定第n个时间点的状态向量，包括：
根据第n-1个时间点获得的卡尔曼滤波数据，获得对应的卡尔曼增益，所述卡尔曼滤波数据包括所述第n-1个时间点的状态向量的协方差矩阵和观测方程的协方差矩阵；
根据所述卡尔曼增益，更新第n-1个时间点的状态向量和状态向量的协方差矩阵；
基于所述误差模型，根据更新后的所述第n-1个时间点的状态向量以及所述第n-1个时间点的目标航向角变化率和目标车辆速度，确定第n个时间点的状态向量。
2.根据权利要求1所述的控制误差的确定方法，其特征在于，所述基于卡尔曼滤波，确定第n个时间点的状态向量之前，还包括：
基于所述误差模型，初始化所述卡尔曼滤波对应的状态方程的协方差矩阵和观测方程的协方差矩阵；
根据所述状态方程的协方差矩阵，初始化状态向量的协方差矩阵。
3.根据权利要求1或2所述的控制误差的确定方法，其特征在于，所述得到车辆执行所述目标轨迹时的控制误差，包括：
基于预测步长，获取所述预测步长包含的M个时间点分别对应的控制误差，M为大于1且小于或等于N的整数；
根据所述M个时间点分别对应的控制误差，确定所述目标轨迹的控制误差。
4.根据权利要求1或2所述的控制误差的确定方法，其特征在于，还包括：
获取历史轨迹的相关数据，所述历史轨迹的相关数据包括T个时间点、所述历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差，T为大于等于1的整数；
根据所述历史轨迹的相关数据和初始状态空间方程，确定所述误差模型。
5.根据权利要求4所述的控制误差的确定方法，其特征在于，所述根据所述历史轨迹的相关数据和初始状态空间方程，确定所述误差模型，包括：
根据所述历史轨迹对应T个时间点的目标航向角变化率、实际车辆速度和实际控制误差以及初始状态空间方程，将所述初始状态空间方程转化为矩阵方程；
根据所述矩阵方程，采用最小二乘法，获取所述误差模型对应的各***方程参数；
根据所述各***方程参数，确定所述误差模型。
6.一种控制误差的确定装置，其特征在于，包括：
获取模块，用于获取目标轨迹的相关数据，所述相关数据包括N个时间点、所述目标轨迹对应N个时间点的目标航向角变化率和目标车辆速度，N为大于或等于1的整数；
处理模块，用于针对各时间点执行以下步骤，以得到车辆执行所述目标轨迹时的控制误差：
基于卡尔曼滤波，确定第n个时间点的状态向量，n为大于或等于1且小于或等于N的整数；
基于误差模型，根据第n个时间点的目标航向角变化率、目标车辆速度和状态向量，确定第n个时间点的控制误差，所述误差模型是根据车辆的历史轨迹数据获得的，所述误差模型用于反映航向角变化率、车辆速度和控制误差的对应关系；
所述处理模块，具体用于根据第n-1个时间点获得的卡尔曼滤波数据，获得对应的卡尔曼增益，所述卡尔曼滤波数据包括所述第n-1个时间点的状态向量的协方差矩阵和观测方程的协方差矩阵；
根据所述卡尔曼增益，更新第n-1个时间点的状态向量和状态向量的协方差矩阵；
基于所述误差模型，根据更新后的所述第n-1个时间点的状态向量以及所述第n-1个时间点的目标航向角变化率和目标车辆速度，确定第n个时间点的状态向量。
7.一种电子设备，其特征在于，包括：处理器，以及与所述处理器通信连接的存储器；
所述存储器存储计算机执行指令；
所述处理器执行所述存储器存储的计算机执行指令，以实现如权利要求1至5中任一项所述的控制误差的确定方法。
8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机程序指令，所述计算机程序指令被处理器执行时，实现如权利要求1至5中任一项所述的控制误差的确定方法。
9.一种计算机程序产品，包括计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5中任一项所述的控制误差的确定方法。
CN202111216720.8A 2021-10-19 2021-10-19 控制误差的确定方法、装置、设备及存储介质 Active CN114001976B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111216720.8A CN114001976B (zh) 2021-10-19 2021-10-19 控制误差的确定方法、装置、设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111216720.8A CN114001976B (zh) 2021-10-19 2021-10-19 控制误差的确定方法、装置、设备及存储介质

Publications (2)

Publication Number Publication Date
CN114001976A CN114001976A (zh) 2022-02-01
CN114001976B true CN114001976B (zh) 2024-03-12

Family

ID=79923234

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111216720.8A Active CN114001976B (zh) 2021-10-19 2021-10-19 控制误差的确定方法、装置、设备及存储介质

Country Status (1)

Country Link
CN (1) CN114001976B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116125819B (zh) * 2023-04-14 2023-07-07 智道网联科技(北京)有限公司 轨迹修正方法、装置、电子设备和计算机可读存储介质

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2005062984A2 (en) * 2003-12-24 2005-07-14 Automotive Systems Laboratory, Inc. Road curvature estimation system
DE102010033530A1 (de) * 2010-06-24 2011-12-29 Continental Teves Ag & Co. Ohg Verfahren und System zur Fahrspurenmittenführung eines Kraftfahrzeugs
CN106558219A (zh) * 2016-10-20 2017-04-05 深圳市元征科技股份有限公司 车辆轨迹预测方法及装置
CN110675435A (zh) * 2019-10-08 2020-01-10 嘉兴智驾科技有限公司 基于卡尔曼滤波和χ2检测平滑处理的车辆轨迹跟踪方法
CN111640140A (zh) * 2020-05-22 2020-09-08 北京百度网讯科技有限公司 目标跟踪方法、装置、电子设备及计算机可读存储介质
CN112365743A (zh) * 2020-10-12 2021-02-12 中国民用航空总局第二研究所 航空器航迹定位数据偏移的修正方法及装置
CN112622895A (zh) * 2020-12-30 2021-04-09 威伯科汽车控制***（中国）有限公司 一种应用于自动驾驶的轨迹控制的预估控制方法
CN112651557A (zh) * 2020-12-25 2021-04-13 际络科技(上海)有限公司 轨迹预测***及方法、电子设备及可读存储介质
CN112733270A (zh) * 2021-01-08 2021-04-30 浙江大学 车辆行驶轨迹预测和轨迹偏离危险度评估的***与方法
CN112793585A (zh) * 2021-02-07 2021-05-14 厦门金龙联合汽车工业有限公司 一种自动驾驶的轨迹跟踪控制方法
CN113120001A (zh) * 2021-05-11 2021-07-16 雄狮汽车科技(南京)有限公司 自动驾驶车辆的横向控制方法、装置及车辆

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6542809B2 (en) * 2001-02-26 2003-04-01 Honeywell International Inc. Method and system for displaying target vehicle position information

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2005062984A2 (en) * 2003-12-24 2005-07-14 Automotive Systems Laboratory, Inc. Road curvature estimation system
DE102010033530A1 (de) * 2010-06-24 2011-12-29 Continental Teves Ag & Co. Ohg Verfahren und System zur Fahrspurenmittenführung eines Kraftfahrzeugs
CN106558219A (zh) * 2016-10-20 2017-04-05 深圳市元征科技股份有限公司 车辆轨迹预测方法及装置
CN110675435A (zh) * 2019-10-08 2020-01-10 嘉兴智驾科技有限公司 基于卡尔曼滤波和χ2检测平滑处理的车辆轨迹跟踪方法
CN111640140A (zh) * 2020-05-22 2020-09-08 北京百度网讯科技有限公司 目标跟踪方法、装置、电子设备及计算机可读存储介质
CN112365743A (zh) * 2020-10-12 2021-02-12 中国民用航空总局第二研究所 航空器航迹定位数据偏移的修正方法及装置
CN112651557A (zh) * 2020-12-25 2021-04-13 际络科技(上海)有限公司 轨迹预测***及方法、电子设备及可读存储介质
CN112622895A (zh) * 2020-12-30 2021-04-09 威伯科汽车控制***（中国）有限公司 一种应用于自动驾驶的轨迹控制的预估控制方法
CN112733270A (zh) * 2021-01-08 2021-04-30 浙江大学 车辆行驶轨迹预测和轨迹偏离危险度评估的***与方法
CN112793585A (zh) * 2021-02-07 2021-05-14 厦门金龙联合汽车工业有限公司 一种自动驾驶的轨迹跟踪控制方法
CN113120001A (zh) * 2021-05-11 2021-07-16 雄狮汽车科技(南京)有限公司 自动驾驶车辆的横向控制方法、装置及车辆

Also Published As

Publication number Publication date
CN114001976A (zh) 2022-02-01

Similar Documents

Publication Publication Date Title
CN107817790B (zh) 一种计算车辆轨迹的曲率的方法和装置
CN109313809B (zh) 一种图像匹配方法、装置及存储介质
CN109584299B (zh) 一种定位方法、定位装置、终端及存储介质
CN114001976B (zh) 控制误差的确定方法、装置、设备及存储介质
US9501327B2 (en) Concurrently processing parts of cells of a data structure with multiple processes
CN113139696B (zh) 一种轨迹预测模型构建方法及轨迹预测方法、装置
CN111626489A (zh) 基于时序差分学习算法的最短路径规划方法和装置
CN111508072A (zh) 地图构建方法、装置、电子设备及存储介质
CN111045433B (zh) 一种机器人的避障方法、机器人及计算机可读存储介质
US20210358166A1 (en) Methods, apparatuses, systems, and storage media for loading visual localization maps
CN112765548A (zh) 用于传感器融合定位的协方差确定方法、定位方法和装置
CN116560401A (zh) 一种无人机编队中僚机控制指令的确定方法及终端设备
CN115950432A (zh) 一种室内外移动机器人的无缝定位方法、设备及存储介质
CN112729349B (zh) 里程计在线标定的方法、装置、电子设备及存储介质
CN110533158B (zh) 模型建构方法、***及非易失性电脑可读取记录介质
CN115993132A (zh) 视觉惯性里程计初始化方法、装置及飞行汽车
CN110807818A (zh) 基于关键帧的rgb-dslam方法和装置
CN112669196A (zh) 一种硬件加速引擎中因子图优化数据的方法和设备
CN111678515A (zh) 设备状态估计方法、装置、电子设备及存储介质
KR102478451B1 (ko) 연산 장치 및 이를 이용한 시스템의 상태 추정 방법
CN116448105B (zh) 位姿更新方法、装置、电子设备和存储介质
CN113360276B (zh) 一种基于健康状态的无人机***任务规划方法及装置
CN117348577B (zh) 一种生产工艺仿真检测方法、装置、设备以及介质
CN117351449B (zh) 基于极坐标加权的道路可通行区域边界优化方法
CN113804192B (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