CN114643980A - 泊车路径实时调整方法、装置、设备及存储介质 - Google Patents

Abstract

本发明实施例公开了一种泊车路径实时调整方法、装置、设备及存储介质。该方法包括：进入自动泊车模式后，实时获取车辆在行驶过程中当前时刻的车辆位姿；根据当前时刻的车辆位姿，计算下一时刻的车辆预估位姿；基于当前泊车路径计算车辆泊车所需的行驶距离及车辆转角；采用MPC算法根据车辆预估位姿、行驶距离以及车辆转角对车辆的行驶路径进行调整，并基于调整后的路径泊车。本发明通过采用场端图像传感器以及轮速传感器采集参数预估下一时刻的车辆位姿，提高了车辆位姿预估的准确度；同时，通过预估下一时刻的车辆位姿，对当前泊车路径进行实时调整，使车辆按照最优泊车路径进行泊车，提高了自动泊车的效率和精度。

Description

泊车路径实时调整方法、装置、设备及存储介质

技术领域

本发明涉及路径规划领域，尤其涉及一种泊车路径实时调整方法、装置、设备及存储介质。

背景技术

近年来，自动泊车技术可以帮助人们安全可靠的完成泊车，其通过获取车辆位姿和车位位姿，然后规划一条可行路径，随后自动控制车辆跟随路径完成泊车。路径规划是自动泊车关键的环节，其基本要求是规划的路径可行并且安全无碰撞。传统自动泊车路径规划是根据图像和超声波融合识别的车位位姿创建泊车地图，获取车辆初始位姿，并根据轮速传感器以及方向盘转角传感器估计车辆位姿；从而根据跟踪控制策略将车辆泊入。

但在泊车低速场景下，通过方向盘转角传感器估算车辆姿态存在较大误差，特别是原地转方向盘的情况，导致车辆位姿估算不准确，进而影响泊车精度和效率，甚至可能发生擦碰事故。因此，如何提高泊车低速场景下车辆姿态估算的准确度是亟需解决的问题。

发明内容

有鉴于此，本发明的目的是为了克服现有技术中的不足，提供一种泊车路径实时调整方法、装置、设备及存储介质。

第一方面，本发明提供一种泊车路径实时调整方法，包括：

进入自动泊车模式后，基于场端图像传感器实时获取车辆在行驶过程中当前时刻的车辆位姿；

根据所述当前时刻的车辆位姿，计算下一时刻的车辆预估位姿；

基于当前泊车路径计算车辆泊车所需的行驶距离及车辆转角；

采用MPC算法根据所述车辆预估位姿、所述行驶距离以及所述车辆转角对车辆的行驶路径进行调整，并基于调整后的行驶路径泊车。

在一种实施方式中，所述根据所述当前时刻的车辆位姿，计算下一时刻的车辆预估位姿包括：

根据车辆运动学模型以及所述当前时刻的车辆位姿建立车辆后轮中心点轨迹方程；

基于所述车辆后轮中心点轨迹方程以及车辆轮速传感器所采集的参数，分别构建车辆前轮、后轮的状态向量和观测向量；

根据无损卡尔曼滤波算法对所述车辆前轮、后轮的状态向量和观测向量进行全局融合处理，得到下一时刻的车辆预估位姿。

在一种实施方式中，所述基于所述车辆后轮中心点轨迹方程以及车辆轮速传感器所采集的参数，分别构建车辆前轮、后轮的状态向量和观测向量包括：

获取场端图像传感器坐标系下进入预定泊车范围内初始时刻的车辆位姿，并根据所述车辆位姿计算车辆横摆角速度；

获取车辆轮速传感器采集并预处理过的参数，其中，所述参数包括前轴中心点的速度、等效车轮的前轮转角、车辆轴距、车辆后轴中点的纵向行驶速度；

采用车辆运动学模型并根据所述前轴中心点的速度、所述等效车轮的前轮转角、所述车辆轴距以及所述车辆位姿的姿态角计算后轮中心点轨迹方程；

根据所述后轮中心点轨迹方程、所述后轴车辆位姿、所述横摆角速度以及所述车辆后轴中点的纵向行驶速度分别构建车辆前轴、后轴的状态向量；

根据所述后轮中心点轨迹方程、所述横摆角速度以及所述车辆后轴中点的纵向行驶速度分别构建车辆前轴、后轴的观测向量。

在一种实施方式中，所述获取场端图像传感器坐标系下进入预定泊车范围内初始时刻的车辆位姿包括：

建立泊车地图坐标系以及场端图像传感器坐标系；

获取泊车地图坐标系下进入预定泊车范围内初始时刻的车辆初始位姿；

基于预定坐标转换公式，将所述泊车地图坐标系下的车辆初始位姿转换为所述场端图像传感器坐标系下的车辆位姿。

在一种实施方式中，所述根据无损卡尔曼滤波算法对所述车辆前轴、后轴的状态向量和观测向量进行全局融合处理，得到下一时刻的车辆预估位姿包括：

根据所述状态向量和所述观测向量构建车辆前轴、后轴对应的状态方程以及观测方程；

采用无损卡尔曼滤波算法对所述车辆前轴、后轴对应的状态方程以及观测方程分别进行局部估计计算，得到车辆前轴、后轴对应的车辆局部预估位姿；

对所述车辆前轴、后轴对应的车辆局部预估位姿进行全局融合计算，得到下一时刻的车辆预估位姿。

在一种实施方式中，将所述状态向量记为X，所述观测向量记为Z，其中，

T表示矩阵转置，所述车辆前轮对应的状态方程以及观测方程为：

所述车辆后轮对应的状态方程以及观测方程为：

其中，X_k+1表示k+1时刻的状态向量，T₀表示初始采用时间，

为车辆运动学模型中等效车轮的前轮转角，(x，y，θ)为泊车地图坐标系下的车辆位姿，(x_r，y_r，θ)为泊车地图坐标系下的后轮轴线车辆位姿，

和

分别为左前轮和右前轮轮速传感器信号，

和

为右后轮和右后轮轮速传感器信号，W_k为k时刻的状态噪声，δ_fl为车辆运动学模型中左前轮轴线和左后轮轴线的夹角，δ_fr为车辆运动学模型中右前轮轴线和右后轮轴线的夹角，ω为横摆角速度，v_k为后轮轴线中点的纵向行驶速度，l为车辆运动模型中车辆轴距，t为车辆运行时间，

为车辆运动学模型中后轴中点转弯半径。

在一种实施方式中，所述后轮中心点的轨迹方程为：

其中，t为车辆运行时间，(x_r，y_r，θ)为泊车地图坐标系下的后轮轴线车辆位姿，

为车辆运动学模型中等效车轮的前轮转角，l为车辆运动模型中车辆轴距，v为前轴中心点的速度。

第二方面，本发明还提供一种泊车路径实时调整装置，包括：

当前位姿获取模块，用于进入自动泊车模式后，基于场端图像传感器实时获取车辆在行驶过程中当前时刻的车辆位姿；

位姿预估模块，用于根据所述当前时刻的车辆位姿，计算下一时刻的车辆预估位姿；

计算模块，用于基于当前泊车路径计算车辆泊车所需的行驶距离及车辆转角；

路径调整模块，用于采用MPC算法根据所述车辆预估位姿、所述行驶距离以及所述车辆转角对车辆的行驶路径进行调整，并基于调整后的行驶路径泊车。

第三方面，本发明还提供一种泊车路径实时调整设备，所述泊车路径实时调整设备包括存储器和至少一个处理器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述的泊车路径实时调整方法。

第四方面，本发明还提供一种计算机可读存储介质，其存储有计算机程序，所述计算机程序被执行时，实施上述的泊车路径实时调整方法。

本发明的实施例具有如下优点：

本实施例在进入自动泊车模式后，实时获取车辆在行驶过程中当前时刻的车辆位姿；根据当前时刻的车辆位姿，计算下一时刻的车辆预估位姿；基于当前泊车路径计算车辆泊车所需的行驶距离及车辆转角；采用MPC算法根据车辆预估位姿、行驶距离以及车辆转角对车辆的行驶路径进行调整，并基于调整后的行驶路径泊车。本实施例通过采用场端图像传感器以及轮速传感器采集参数预估下一时刻的车辆位姿，提高了车辆位姿预估的准确度；同时，通过预估下一时刻的车辆位姿，对当前泊车路径进行实时调整，使车辆按照最优泊车路径进行泊车，提高了自动泊车的效率和精度。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。

图1为本发明实施例中泊车路径实时调整方法的第一个实施方式示意图；

图2为本发明实施例中车辆位姿示意图；

图3为本发明实施例中泊车路径实时调整方法的第二个实施方式示意图；

图4为本发明实施例中泊车路径实时调整方法的第三个实施方式示意图；

图5为本发明实施例中场端图像传感器坐标系于泊车地图坐标系示意图；

图6为本发明实施例中车辆运动学模型示意图；

图7为本发明实施例中泊车路径实时调整方法的第四个实施方式示意图；

图8为本发明实施例中泊车路径实时调整装置的一个实施例示意图。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语) 具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。

实施例1

请参照图1，本实施例提出一种泊车路径实时调整方法，下面对该方法进行详细说明。

110，进入自动泊车模式后，基于场端图像传感器实时获取车辆在行驶过程中当前时刻的车辆位姿。

在车辆(汽车)进入自动泊车模式后，根据场端图像传感器实时采集获取车辆在行驶过程中当前时刻的车辆位姿，本实施例中的场端图像传感器安装于停车场，比如安装在地下停车场天花板。

示范性地，如图2所示，根据场端图像传感器可获取到场端图像传感器坐标系目标停车位位姿

以及车辆位姿(x_g,y_g,θ_g)，其中，车辆进入预定泊车范围内的车辆初始位姿为

根据该车辆位姿可构建泊车地图以及车辆泊车时的初始路径轨迹。

120，根据当前时刻的车辆位姿，计算下一时刻的车辆预估位姿。

在一种可行的实施方式中，如图3所示，步骤120可具体包括如下步骤：

121，根据车辆运动学模型以及当前时刻的车辆位姿建立车辆后轮中心点轨迹方程。

122，基于车辆后轮中心点轨迹方程以及车辆轮速传感器所采集的参数，分别构建车辆前轮、后轮的状态向量和观测向量。

在一种可行的实施方式中，如图4所示，步骤122可具体包括如下步骤：

1221，获取场端图像传感器坐标系下进入预定泊车范围内初始时刻的车辆位姿，并根据车辆位姿计算车辆横摆角速度。

1222，获取车辆轮速传感器采集并预处理过的参数。

1223，采用车辆运动学模型并根据前轴中心点的速度、等效车轮的前轮转角、车辆轴距以及车辆位姿的姿态角计算后轮中心点轨迹方程。

1224，根据后轮中心点轨迹方程、后轴车辆位姿、横摆角速度以及车辆后轴中点的纵向行驶速度分别构建车辆前轴、后轴的状态向量。

1225，根据后轮中心点轨迹方程、横摆角速度以及车辆后轴中点的纵向行驶速度分别构建车辆前轴、后轴的观测向量。

构建泊车地图坐标系以及场端图像传感器坐标系；获取泊车地图坐标系下进入预定泊车范围内初始时刻的车辆初始位姿；基于预定坐标转换公式，将泊车地图坐标系下的车辆初始位姿转换为场端图像传感器坐标系下的车辆位姿。

示范性地，如图5所示，x_go_gy_g为场端图像传感器坐标系，xoy为泊车地图坐标系，泊车地图坐标系原点o在x_go_gy_g坐标系下位置为

场端图像传感器检测的车辆位姿(x_g,y_g,θ_g)与泊车地图坐标系下的车辆位姿 (x,y,θ)转换公式如下：

根据车辆姿态角θ可计算车辆横摆角速度

其中，θ为车辆中轴线与场端图像传感器坐标系中x轴正方向的夹角(车辆姿态角)。

如图6所示，设定泊车时的车速在6km/h以下，由此假定车辆无侧滑现象，在此基础上建立车辆的运动学模型。具体的，要控制车辆的运动，首先要对车辆的运动建立数字化模型，模型建立的越准确，对车辆运动的描述越准确，对车辆的跟踪控制的效果就越好。除了真实反映车辆特性外，建立的模型也应该尽可能的简单易用。本实施例根据车辆在泊车过程中的运动情况建立自行车模型，其中，自行车模型(Bicycle Model)是一种常见的车辆运动学模型。自行车模型(Bicycle Model)的建立基于如下假设：1) 不考虑车辆在垂直方向(Z轴方向)的运动，即假设车辆的运动是一个二维平面上的运动。2)假设车辆左右侧轮胎在任意时刻都拥有相同的转向角度和转速；这样车辆的左右两个轮胎的运动可以合并为一个轮胎来描述。3) 假设车辆行驶速度变化缓慢，忽略前后轴载荷的转移。4)假设车身和悬架系统都是刚性系统。5)假设车辆的运动和转向是由前轮驱动 (front-wheel-only)的。

获取车辆上的车辆轮速传感器采集并预处理过的参数，根据车辆轮速传感器所采集的车辆各车轮轮速以及各车轴的速度，对其基于车辆运动学模型进行相应预处理计算得到各参数，其中，各参数包括前轴中心点的速度、等效车轮的前轮转角、车辆轴距、车辆后轴中点的纵向行驶速度。

根据车辆运动学模型、所获取的当前时刻的车辆位姿以及各参数构建车辆后轮中心点轨迹方程，如下公式所示：

其中，t为车辆运行时间，(x_r，y_r，θ)为泊车地图坐标系下的后轮轴线车辆位姿(后轴车辆位姿)，

为车辆运动学模型中等效车轮的前轮转角， l为车辆运动模型中车辆轴距，v为前轴中心点的速度。

由于车辆轮速传感器采集的参数来自多处位置，故采用无损卡尔曼滤波算法(UKF)根据后轮中心点轨迹方程、泊车地图坐标系下的后轴车辆位姿、横摆角速度以及车辆后轴中点的纵向行驶速度分别构建车辆前轴、后轴的状态向量以及观测向量。

示范性地，状态向量为

ω为横摆角速度，v_k为后轴中点的纵向行驶速度；观测向量为

其中，(x_r，y_r，θ)为泊车地图坐标系下的后轴车辆位姿，T表示矩阵转置。

123，根据无损卡尔曼滤波算法对车辆前轮、后轮的状态向量和观测向量进行全局融合处理，得到下一时刻的车辆预估位姿。

在一种可行的实施方式中，如图7所示，步骤123可具体包括如下步骤：

1231，根据状态向量和观测向量构建车辆前轴、后轴对应的状态方程以及观测方程。

1232，采用无损卡尔曼滤波算法对车辆前轴、后轴对应的状态方程以及观测方程分别进行局部估计计算，得到车辆前轴、后轴对应的车辆局部预估位姿。

1233，对车辆前轴、后轴对应的车辆局部预估位姿进行全局融合计算，得到下一时刻的车辆预估位姿。

采用无损卡尔曼滤波算法设计两个子滤波器；子滤波器1包含的观测变量来源于前轮轮速传感器信号和上述步骤所计算出的横摆角速度；根据车辆运动学模型以及车辆前轴对应的状态向量、观测向量，可以计算出子滤波器1的状态方程和观测方程，即车辆前轴的状态方程以及观测方程为：

并计算出子滤波器2的状态方程和观测方程，即车辆后轴的状态方程以及观测方程为：

其中，X_k+1表示k+1时刻的状态向量，θ_k为当前k时刻下的车辆姿态角，T₀表示初始采用时间，

为车辆运动学模型中等效车轮的前轮转角， (x，y，θ)为泊车地图坐标系下的车辆位姿，(x_r，y_r，θ)为泊车地图坐标系下的后轮轴线车辆位姿，

和

分别为左前轮和右前轮轮速传感器信号，

和

为右后轮和右后轮轮速传感器信号，W_k为k时刻的状态噪声，δ_fl为车辆运动学模型中左前轮轴线和左后轮轴线的夹角，δ_fr为车辆运动学模型中右前轮轴线和右后轮轴线的夹角，ω为横摆角速度，v_k为后轮轴线中点的纵向行驶速度，l为车辆运动模型中车辆轴距，t为车辆运行时间，

为车辆运动学模型中后轴中点转弯半径。

可以理解：

其中，ε的数值为车辆后轮轮距的一半。

采用无损卡尔曼滤波算法对车辆前轴、后轴对应的状态方程以及观测方程分别进行局部估计计算，得到车辆前轴、后轴对应的车辆局部预估位姿，再对该局部预估位姿进行全局融合计算，得到下一时刻的车辆预估位姿；即子滤波器1以及子滤波器2的局部估计结果在主滤波器进行融合，最终得到全局融合估计结果，得到下一时刻的车辆位姿。

130，基于当前泊车路径计算车辆泊车所需的行驶距离及车辆转角。

140，采用MPC算法根据车辆预估位姿、行驶距离以及车辆转角对车辆的行驶路径进行调整，并基于调整后的行驶路径泊车。

当车辆进入预定泊车范围后，可规划好预定最优泊车路径，车辆根据预定最优泊车路径进行泊车时，可得到当前泊车路径。由于车辆在泊车过程中其速度及转角不可控，因此当前泊车路径与预定最优泊车路径不存在完全等同的情况，需要对当前泊车路径进行实时调整，不断接近该预定最优泊车路径。具体地，将预定最优泊车路径进行路径划分，并基于该当前泊车路径以及预定最优泊车路径计算车辆泊车所需的行驶距离以及车辆转角，如图7所示，基于圆弧-直线-圆弧平行泊车路径划分三段路径(L₁、 L₂、L₃)，且三段路径长度如下：

其中，L₁、L₃为第一段和第三段的路径长度，保持转弯半径和对应圆周角相同。L₂为中间连接两段弧的直线路径。θ′为

h为车辆和目标停车位间的侧向距离，R_min为车辆最小转弯半径。

采用MPC算法根据车辆预估位姿、所计算出的行驶距离以及车辆转角对车辆的行驶路径进行调整，并基于调整后的行驶路径泊车。首先采用 MPC算法截取当前车辆位置的最近点前后的预定数量的若干点生成局部路径，然后根据车辆位姿计算相对时间，之后按照预定均分距离使用线性插值算法对路径进行均分重置，对重置路径进行光滑处理，重新求解车辆横摆角，计算每个路径点的曲率，设置最后一个路径点速度为0，以此对车辆的当前泊车路径实时调整，使其不断接近于预定最优泊车路径，从而根据调整后的路径进行泊车。

本发明实施例通过场端图像传感器实时检测车辆位姿，再根据轮速传感器和场端图像传感器所采集的参数以及无损卡尔曼滤波算法预估下一时刻的车辆位姿，根据下一时刻的车辆位姿对当前泊车路径实时调整，从而根据调整后的路径泊车，解决了在泊车低速场景下车辆姿态预估不准确的问题，同时提高自动泊车的精度和效率。

实施例2

请参照图8，本发明实施例还提供一种泊车路径实时调整装置，包括：

当前位姿获取模块801，用于进入自动泊车模式后，基于场端图像传感器实时获取车辆在行驶过程中当前时刻的车辆位姿；

位姿预估模块802，用于根据所述当前时刻的车辆位姿，计算下一时刻的车辆预估位姿；

计算模块803，用于基于当前泊车路径计算车辆泊车所需的行驶距离及车辆转角；

路径调整模块804，用于采用MPC算法根据所述车辆预估位姿、所述行驶距离以及所述车辆转角对车辆的行驶路径进行调整，并基于调整后的行驶路径泊车。

上述的泊车路径实时调整装置对应于实施例1的泊车路径实时调整方法；实施例1中的任何可选项也适用于本实施例，这里不再详述。

本发明实施例还提供了一种泊车路径实时调整设备，该所述泊车路径实时调整设备包括存储器和至少一个处理器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述实施例的泊车路径实时调整方法。

存储器可包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据泊车路径实时调整设备的使用所创建的数据(比如车辆预估位姿等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有机器可运行指令，所述计算机可运行指令在被处理器调用和运行时，所述计算机可运行指令促使所述处理器运行上述实施例的泊车路径实时调整方法的步骤。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器 (ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种泊车路径实时调整方法，其特征在于，包括：

进入自动泊车模式后，实时获取车辆在行驶过程中当前时刻的车辆位姿；

根据所述当前时刻的车辆位姿，计算下一时刻的车辆预估位姿；

基于当前泊车路径计算车辆泊车所需的行驶距离及车辆转角；

采用MPC算法根据所述车辆预估位姿、所述行驶距离以及所述车辆转角对车辆的行驶路径进行调整，并基于调整后的行驶路径泊车。

2.根据权利要求1所述的泊车路径实时调整方法，其特征在于，所述根据所述当前时刻的车辆位姿，计算下一时刻的车辆预估位姿包括：

根据车辆运动学模型以及所述当前时刻的车辆位姿建立车辆后轮中心点轨迹方程；

基于所述车辆后轮中心点轨迹方程以及车辆各传感器所采集的参数，分别构建车辆前轮、后轮的状态向量和观测向量；

根据无损卡尔曼滤波算法对所述车辆前轮、后轮的状态向量和观测向量进行全局融合处理，得到下一时刻的车辆预估位姿。

3.根据权利要求2所述的泊车路径实时调整方法，其特征在于，所述基于所述车辆后轮中心点轨迹方程以及车辆各传感器所采集的参数，分别构建车辆前轮、后轮的状态向量和观测向量包括：

获取场端图像传感器坐标系下进入预定泊车范围内初始时刻的车辆位姿，并根据所述车辆位姿计算车辆横摆角速度；

获取车辆各传感器采集并预处理过的参数，其中，所述参数包括前轴中心点的速度、等效车轮的前轮转角、车辆轴距、车辆后轴中点的纵向行驶速度；

采用车辆运动学模型并根据所述前轴中心点的速度、所述等效车轮的前轮转角、所述车辆轴距以及所述车辆位姿的姿态角计算后轮中心点轨迹方程；

根据所述后轮中心点轨迹方程、所述后轴车辆位姿、所述横摆角速度以及所述车辆后轴中点的纵向行驶速度分别构建车辆前轴、后轴的状态向量；

根据所述后轮中心点轨迹方程、所述横摆角速度以及所述车辆后轴中点的纵向行驶速度分别构建车辆前轴、后轴的观测向量。

4.根据权利要求3所述的泊车路径实时调整方法，其特征在于，所述获取场端图像传感器坐标系下进入预定泊车范围内初始时刻的车辆位姿包括：

建立泊车地图坐标系以及场端图像传感器坐标系；

获取泊车地图坐标系下进入预定泊车范围内初始时刻的车辆初始位姿；

基于预定坐标转换公式，将所述泊车地图坐标系下的车辆初始位姿转换为所述场端图像传感器坐标系下的车辆位姿。

5.根据权利要求4所述的泊车路径实时调整方法，其特征在于，所述根据无损卡尔曼滤波算法对所述车辆前轴、后轴的状态向量和观测向量进行全局融合处理，得到下一时刻的车辆预估位姿包括：

根据所述状态向量和所述观测向量构建车辆前轴、后轴对应的状态方程以及观测方程；

采用无损卡尔曼滤波算法对所述车辆前轴、后轴对应的状态方程以及观测方程分别进行局部估计计算，得到车辆前轴、后轴对应的车辆局部预估位姿；

对所述车辆前轴、后轴对应的车辆局部预估位姿进行全局融合计算，得到下一时刻的车辆预估位姿。

6.根据权利要求5所述的泊车路径实时调整方法，其特征在于，将所述状态向量记为X，所述观测向量记为Z，其中，

T表示矩阵转置，所述车辆前轮对应的状态方程以及观测方程为：

所述车辆后轮对应的状态方程以及观测方程为：

其中，X_k+1表示k+1时刻的状态向量，T₀表示初始采用时间，

为车辆运动学模型中等效车轮的前轮转角，(x，y，θ)为泊车地图坐标系下的车辆位姿，(x_r，y_r，θ)为泊车地图坐标系下的后轮轴线车辆位姿，

和

分别为左前轮和右前轮轮速传感器信号，

和

为右后轮和右后轮轮速传感器信号，W_k为k时刻的状态噪声，δ_fl为车辆运动学模型中左前轮轴线和左后轮轴线的夹角，δ_fr为车辆运动学模型中右前轮轴线和右后轮轴线的夹角，ω为横摆角速度，v_k为后轮轴线中点的纵向行驶速度，l为车辆运动模型中车辆轴距，t为车辆运行时间，

为车辆运动学模型中后轴中点转弯半径。

7.根据权利要求2所述的泊车路径实时调整方法，其特征在于，所述后轮中心点的轨迹方程为：

其中，t为车辆运行时间，(x_r，y_r，θ)为泊车地图坐标系下的后轮轴线车辆位姿，

为车辆运动学模型中等效车轮的前轮转角，l为车辆运动模型中车辆轴距，v为前轴中心点的速度。

8.一种泊车路径实时调整装置，其特征在于，包括：

当前位姿获取模块，用于进入自动泊车模式后，实时获取车辆在行驶过程中当前时刻的车辆位姿；

位姿预估模块，用于根据所述当前时刻的车辆位姿，计算下一时刻的车辆预估位姿；

计算模块，用于基于当前泊车路径计算车辆泊车所需的行驶距离及车辆转角；

路径调整模块，用于采用MPC算法根据所述车辆预估位姿、所述行驶距离以及所述车辆转角对车辆的行驶路径进行调整，并基于调整后的行驶路径泊车。

9.一种泊车路径实时调整设备，其特征在于，所述泊车路径实时调整设备包括存储器和至少一个处理器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-7中任一项所述的泊车路径实时调整方法。

10.一种计算机存储介质，其特征在于，其存储有计算机程序，所述计算机程序被执行时，实施根据权利要求1-7中任一项所述的泊车路径实时调整方法。

Images