CN115056799B - 一种用于自动泊车航迹推算的优化算法 - Google Patents

Abstract

本发明公开了一种用于自动泊车航迹推算的优化算法，克服了现有技术中主流的航迹推算方案计算行驶距离、坐标以及航向角时都会出现误差，导致定位精度受到影响的问题，包括以下步骤：S1：剔除轮速无效脉冲，利用轮速有效脉冲计算行驶的距离Δs；S2：利用卡尔曼滤波技术，计算车辆航向角变化量S3：利用车辆的运动学方程计算车辆定位。针对自动泊车这种低速场景，分别对Δs和进行优化，提高航迹推算的精度。

Description

一种用于自动泊车航迹推算的优化算法

技术领域

本发明涉及自动驾驶技术领域，特别涉及了一种用于自动泊车航迹推算的优化算法。

背景技术

自动泊车场景的车辆的航迹推算，指的是利用车载的传感器计算车辆相对于零点位置的横纵坐标和航向角。现有的航迹推算方案包括以下几种：

一：采用差分GPS或者高精度IMU(惯性测量单元)，前者可以直接输出车辆的绝对坐标，但在有遮挡的地方不能使用，比如地下停车场，后者不受外部环境的影响，可以通过IMU输出的横纵向加速度和绕Z轴的横摆角速度的积分来计算定位，但高精度IMU成本较高，不适合量产车辆配备，不具有实用性。

二：依靠车载的环视摄像头识别车位位置，进行视觉航迹推算，可以利用车上现有的环视摄像头采集图像，但视觉传感器不适合在无画线的车位使用，且精度容易受到光照变化的影响。

三：是目前主流的航迹推算的方案，利用轮速脉冲计算行驶的距离，用车载的陀螺仪输出的横摆角速度信号积分计算航向角的变化，再利用车辆的运动学方程计算车辆定位，该方案不受环境和光照的影响，并且传感器的成本低廉，可以用于大规模量产，解决了方案一和方案二存在的问题。但该方案没有考虑旋转编码器输出的车轮脉冲信号在车速较低的泊车场景下会出现无效脉冲的情况，会造成距离计算的误差；在计算车辆航向角变化的时候，直接将横摆角速度积分得出，导致航向角的计算误差随着距离的增加逐渐增大。

发明内容

本发明的目的是克服现有技术中主流的航迹推算方案计算行驶距离、坐标以及航向角时都会出现误差，导致定位精度受到影响的问题，提供了一种用于自动泊车航迹推算的优化算法，针对自动泊车这种低速场景，分别对Δs和进行优化，提高航迹推算的精度。

为了实现上述目的，本发明采用以下技术方案：一种用于自动泊车航迹推算的优化算法，包括下列步骤：

S1：剔除轮速无效脉冲，利用轮速有效脉冲计算行驶的距离Δs；

S2：利用卡尔曼滤波技术，计算车辆航向角变化量

S3：利用车辆的运动学方程计算车辆定位。

本发明能够在传统的航迹推算算法的基础上，针对自动泊车这种低速场景，分别针对Δs和进行优化，过滤掉无效的传感器测量值，提高航迹推算的精度。

作为优选，所述的步骤S1，剔除无效脉冲包括：

S1.1：判断两次采样之间，后轮脉冲增加量是否大于1，若是则剔除多余脉冲，只保留一个脉冲；

S1.2：判断在一侧车轮未出现脉冲的时候，另一侧车轮增加的脉冲数是否大于1，若是，则剔除多余脉冲，只保留一个脉冲。

根据计算，若在两次采样之间，某个后轮脉冲增加的量>1个，此时所需的轮速远大于外侧车轮最大轮速，因此，在泊车过程中不会出现两个连续的采样出现脉冲数增加>1的情况，出现的话只取一个。

且，若在一侧车轮未出现脉冲增加的时候，另一侧车轮已经增加了2个甚至更多的脉冲数，只有外侧车轮的速度>2倍的内侧车轮的速度，才会出现此种情况，但并不符合实际。

作为优选，所述的步骤S1.2，进一步表示为：

S1.2.1：设置后轴中心点速度区间[-VS，-a]、[a，VS]，判断后轴中心点速度是否在速度区间内；

S1.2.2：若在，则需要剔除多余脉冲，只保留一个脉冲，若不在，则不需要剔除多余脉冲。

VS表示车辆最大速度，a根据车辆不同可以有不同取值，但都是接近于0的数值。在实际的泊车过程中，很容易出现车辆前进时停车，然后再倒车的情况，这样就可能会出现一侧未出现脉冲，而另一侧出现了两个脉冲，或者一个采样周期内出现两个脉冲的情况，因此，在过滤无效脉冲的时候，需要设置一个速度区间。

作为优选，所述的步骤S2进一步表示为：

S2.1：测量横摆角速度，得到横摆角速度的测量值Z；

S2.2：利用卡尔曼滤波，得到横摆角速度的预测值ω′；

S2.3：利用卡尔曼滤波，将测量值Z与预测值ω′融合，得到融合后的横摆角速度ω1；

S2.4：利用融合后的横摆角速度ω1，计算车辆航向角

通过卡尔曼滤波将预测的横摆角速度和陀螺仪测量的横摆角速度融合的过程，得到融合后的ω1，然后计算航向角的变化，最终优化航迹推算的精度。

作为优选，所述步骤S2.2进一步表示为：

S2.2.1：取状态量其中ω表示上一时刻横摆角速度，表示ω的导数；

S2.2.2：对状态量进行预测，得到横摆角速度的预测值：

X′＝FX+u

P′＝FPF′+Q

式中，F是状态转移矩阵，u为控制输入，P为状态协方差矩阵，Q为过程噪声矩阵；

S2.2.3：计算预测值与测量值的差值y：

y＝z-Hx′

式中，H是测量矩阵，

S2.2.4：将预测值与测量值进行融合，得到融合后的横摆角速度ω1：

其中：

K＝P′H′S^-1

S＝HP′H′+R

式中，R表示测量噪声矩阵，K表示卡尔曼增益。

本发明利用卡尔曼滤波算法，将传感器的测量值和根据车辆状态的预测值相融合，得出横摆角速度的最优估计，然后再进行积分运算，可以使得的估计精度大大增加。

作为优选，所述步骤S3进一步表示为：

S3.1：建立车辆运动学方程；

S3.2：对车辆运动学方程进行离散化并转为与车速无关的递推公式，计算得到当前车辆坐标。

因此，本发明具有如下有益效果：1、在传统的航迹推算算法的基础上，针对自动泊车这种低速场景，分别对Δs和进行优化，提高航迹推算的精度；2、能够在计算Δs时，过滤掉无效的传感器测量值，减小距离推算误差；3、能够在计算时，利用卡尔曼滤波算法，将传感器的测量值和根据车辆状态的预测值相融合，得出横摆角速度的最优估计，然后再进行积分运算，增加的估计精度。

附图说明

图1为本发明方法的具体操作流程图。

图2为本发明中泊车时车轮转动示意图。

图3为本发明泊车时车辆坐标系原理图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

如图1所示的实施例中，可以看到一种用于自动泊车航迹推算的优化算法，其操作流程为：步骤一，剔除轮速无效脉冲，利用轮速有效脉冲计算行驶的距离Δs；步骤二，利用卡尔曼滤波技术，计算车辆航向角变化量步骤三，利用车辆的运动学方程计算车辆定位。本发明能够在传统的航迹推算算法的基础上，针对自动泊车这种低速场景，分别针对Δs和进行优化，过滤掉无效的传感器测量值，提高航迹推算的精度。

下面通过具体的例子，进一步说明本申请的技术方案。

本实施例中，最高车速为Vehicle Speed(VS)＝2km/h，控制器采样时间为20ms，方向盘最大转角Steer Wheel Angle＝500度，传动系传动比Steer Ratio＝15，轴距L＝2.8m，轮距Wk＝1.6m。

在不考虑传感器噪声和失效情况下，通过两个后轮的旋转编码器分别计算两后轮走过的距离，计算公式如下：

Δs＝NumOfPulse*DistancePerGear

DistancePerGear＝C_tire/NumOfGear

式中，NumOfPulse为两个采样周期中车轮走过的脉冲数，DistancePerGear指的是编码器每一个脉冲代表的距离，C_tire指的是轮胎滚动周长，NumOfGear指的是旋转编码器一周的齿数,对同一款车子来说是固定不变的。

采样指的是控制器采集脉冲数，后轴中心点走过的距离为两后车轮走过距离的平均值。

第一步：剔除轮速无效脉冲，利用轮速有效脉冲计算行驶的距离Δs

泊车工况中，会有频繁启停并切换前进后退的场景，而针对常见的光电旋转编码器来说，低速场景下会出现无效的脉冲，即实际上轮子只转过了1个轮齿，但传感器发出了2次脉冲信号，导致Δs出现误差。

考虑最特殊的情况，即车辆以最高车速2km/h，最大转角500度行驶，如图2所示，当车辆方向盘处于最大转角时，在两个采样时间点之间，车辆后轴行驶的前后位置，图中T0指的是第一个采样时间，T1指的是第二个采样时间，R_{后轴中心端}、Ri和Ro分别指的是后轴中心点、内侧车轮和外侧车轮的行驶半径。在Δt的时间内，内外车轮转过的角度是相同的，但由于半径不同，所以外侧车轮的行驶路径最大。

由阿克曼转向关系可知，后轴中心端的转弯半径为：

R_{后轴中心端}＝L/(tan(SteerWheelAngle/SteerRatio))

带入本实施例中的数据，得到：

而外侧的行驶半径为：

Ro＝R_{后轴中心端}+Wk/2

代入本实施例中的数据，得到：

Ro＝4+0.8＝4.8m

因此，外侧车轮的速度为：

Speed_Ro＝VehicleSpeed×Ro/R

代入本实施例中的数据，得到：

代入本实施例中的数据，可以得到，车辆以最高车速2km/h，最大转角500度行驶时，外侧车轮速度是2.4km/h。

(1)在两次采样之间，若某个后轮脉冲增加的量>1个，此时所需的轮速为(以后轮脉冲增加两个为例)：

Vehicle_wheel＝DistancePerGear×2/Δt

式中，2表示后轮脉冲增加两个，Δt表示采样间隔时间，本实施例中为20ms，DistancePerGear本实施例中取4。

得到Vehicle_wheel＝2m/s＝7.2km/h，远大于2.4km/h，因此，在泊车过程中不会出现两个连续的采样出现脉冲数增加>1的情况，出现的话需要剔除多余脉冲，只保留一个脉冲。

(2)在一侧车轮未出现脉冲增加的时候，另一侧车轮已经增加了2个甚至更多的脉冲数。

内侧车轮的行驶半径：

Ri＝R_{后轴中心端}-Wk/2

代入本实施例中的数据，得到：

Ri＝4-0.8＝3.2m

因此，内侧车轮的速度为：

Speed_Ri＝VehicleSpeed×Ri/R

代入本实施例中的数据，得到：

得到内侧车轮轮速为1.6km/h，而只有外侧车轮的速度>2倍的内侧车轮的速度，才可能出现上述情况，因此，若在一侧车轮未出现脉冲的时候，另一侧车轮增加的脉冲数大于1，则剔除多余脉冲，只保留一个脉冲，由此过滤掉无效的脉冲。

但在实际的泊车过程中，很容易出现车辆前进时停车，然后再倒车的情况，这样就可能会出现一侧未出现脉冲，而另一侧出现了两个脉冲，或者一个采样周期内出现两个脉冲的情况，因此，在过滤无效脉冲的时候，设置一个速度区间，本实施例中速度区间为：[0.2km/h,2km/h]或者[-2km/h,-0.2km/h]。

该速度区间为后轴中心点的速度，后轮中心点的速度为内侧车轮轮速与外侧车轮轮速的平均值，即若后轴中心点速度在[0.2km/h,2km/h]或者[-2km/h,-0.2km/h]区间内，出现一侧未出现脉冲，而另一侧出现了两个脉冲，或者一个采样周期内出现两个脉冲的情况，则需要剔除无效脉冲；若后轴中心点速度在[-0.2km/h，0.2km/h]，出现一侧未出现脉冲，而另一侧出现了两个脉冲，或者一个采样周期内出现两个脉冲的情况，则不需要剔除无效脉冲。

第二步：利用卡尔曼滤波技术，计算车辆航向角变化量

取状态量其中ω表示上一时刻得到的融合后的横摆角速度，表示ω的导数。

车载IMU可以测量得到车辆的纵向加速度a,在低速情况下，忽略车辆的轮胎侧偏角和质心侧偏角，则：

其中，δ为前轮转角。

利用卡尔曼滤波，对状态量进行预测，得到横摆角速度的预测值：

X′＝FX+u

P′＝FPF′+Q

其中，F是状态转移矩阵，u为控制输入，P为状态协方差矩阵，Q为过程噪声矩阵。

计算预测值与测量值的差值y：

y＝z-HX′

式中，H是测量矩阵，

将预测值与测量值进行融合，得到融合后的横摆角速度ω1：

P1＝(I-KH)P′

其中：

K＝P′H′S^-1

S＝HP′H′+R1

式中，R1表示测量噪声矩阵，K表示卡尔曼增益，不断重复这个过程，得到实时的横摆角速度，其中X1表示下一个周期中的X，P1表示下一个周期中的P。

第三步：利用车辆的运动学方程计算车辆定位

车辆坐标系如图3所示，车辆运动学方程如下：

式中，x,y,分别指的是车辆后轴中心点的横坐标、纵坐标和航向角，v为后轴中心点的车速，坐标原点指的是程序初始化时候的位置，分别表示x,y,的导数。

将车辆运动学方程离散化并转化为与车速v无关的递推公式如下：

其中，由此得到车辆当前坐标。

式中，x_t+1、y_t+1、指的是当前x坐标和y坐标和航向角，x_t、y_t、指的是上一周期的x坐标、y坐标和航向角，Δs指的是两个采样周期间车轮的距离变化量，指的是两个采样周期间航向角度的变化量。

本申请在传统的航迹推算算法的基础上，针对自动泊车这种低速场景，分别设计算法针对Δs和进行优化，过滤掉无效的传感器测量值，提高航迹推算的精度。针对Δs，传统的算法是根据两后轮的脉冲变化推算车轮行驶的距离，然后求平均数即为后轴中心点行驶的距离，但是没有考虑传感器可能存在的无效脉冲。旋转编码器在低速工况下可能会出现无效脉冲，即实际行驶距离只行驶了一个传感器轮齿的距离，但却发出了两个甚至更多的脉冲信号，这样会造成距离推算的误差。本发明针对这一点，然后结合泊车的低速工况的特点，设计算法，过滤掉无效的脉冲。

针对车载的陀螺仪可以直接测量车辆的横摆角速度，传统的算法直接将测量值进行积分处理得到车辆航向角的变化，即但陀螺仪的数据较真值会有误差，在积分的过程中会放大这种误差，因此，传统的算法随着航迹推算的距离越远，其误差越大。本发明利用卡尔曼滤波算法，将传感器的测量值和根据车辆状态的预测值相融合，得出横摆角速度的最优估计，然后再进行积分运算，可以使得的估计精度大大增加。

以上所述的实施例只是本发明的一种较佳的方案，并非对本发明作任何形式上的限制，在不超出权利要求所记载的技术方案的前提下还有其它的变体及改型。

Claims (6)

1.一种用于自动泊车航迹推算的优化算法，其特征在于，它包括下列步骤：

S1：剔除轮速无效脉冲，利用轮速有效脉冲计算行驶的距离Δs；剔除无效脉冲包括：

S1.1：判断两次采样之间，后轮脉冲增加量是否大于1，若是则剔除多余脉冲，只保留一个脉冲；

S1.2：判断在一侧车轮未出现脉冲的时候，另一侧车轮增加的脉冲数是否大于1，若是，则剔除多余脉冲，只保留一个脉冲；

S2：利用卡尔曼滤波技术，计算车辆航向角变化量

S3：利用车辆的运动学方程计算车辆定位。

2.根据权利要求1所述的一种用于自动泊车航迹推算的优化算法，其特征在于，所述的步骤S1，剔除无效脉冲包括：

S1.1：判断两次采样之间，后轮脉冲增加量是否大于1，若是则剔除多余脉冲，只保留一个脉冲；

S1.2：判断在一侧车轮未出现脉冲的时候，另一侧车轮增加的脉冲数是否大于1，若是，则剔除多余脉冲，只保留一个脉冲。

3.根据权利要求2所述的一种用于自动泊车航迹推算的优化算法，其特征在于，所述的步骤S1.2，进一步表示为：

S1.2.1：设置后轴中心点速度区间[-VS，-a]、[a，VS]，判断后轴中心点速度是否在速度区间内；其中，VS表示车辆最大速度，a根据车辆不同有不同取值，但都是解决于0的数值；

S1.2.2：若在，则需要剔除多余脉冲，只保留一个脉冲，若不在，则不需要剔除多余脉冲。

4.根据权利要求1或2或3所述的一种用于自动泊车航迹推算的优化算法，其特征在于，所述的步骤S2进一步表示为：

S2.1：测量横摆角速度，得到横摆角速度的测量值Z；

S2.2：利用卡尔曼滤波，得到横摆角速度的预测值ω′；

S2.3：利用卡尔曼滤波，将测量值Z与预测值ω′融合，得到融合后的横摆角速度ω1；

S2.4：利用融合后的横摆角速度ω1，计算车辆航向角

5.根据权利要求4所述的一种用于自动泊车航迹推算的优化算法，其特征在于，所述步骤S2.2进一步表示为：

S2.2.1：取状态量其中ω表示上一时刻的横摆角速度，表示ω的导数；

S2.2.2：对状态量进行预测，得到横摆角速度的预测值：

X′＝FX+u

P′＝FPF′+Q

式中，F是状态转移矩阵，u为控制输入，P为状态协方差矩阵，Q为过程噪声矩阵；

S2.2.3：计算预测值与测量值的差值y：

y＝z-HX′

式中，H是测量矩阵，

S2.2.4：将预测值与测量值进行融合，得到融合后的横摆角速度ω1：

其中：

K＝P′H′S^-1

S＝HP′H′+R1

式中，R1表示测量噪声矩阵，K表示卡尔曼增益。

6.根据权利要求1或2所述的一种用于自动泊车航迹推算的优化算法，其特征在于，所述步骤S3进一步表示为：

S3.1：建立车辆运动学方程；

S3.2：对车辆运动学方程进行离散化并转为与车速无关的递推公式，计算得到当前车辆坐标。