[发明专利]一种基于改进卡尔曼滤波的公交进站停车测评方法

权利要求书

1.一种基于改进卡尔曼滤波的公交进站停车测评方法，其特征在于：利用组合导航系统获取被测公交车在行驶过程中的基础参数信息，采用相对位置推算和改进卡尔曼滤波进行数据处理，实现对进站停车完成时车辆侧面与站台距离、进站停车完成时车辆轴线与泊车位外侧边线夹角、进站停车完成时车头与车前标线的最大距离、以及被测公交车进站过程稳定性等指标的测评，具体步骤包括：

步骤一：基础参数采集

为被测公交车安装双天线组合导航系统，组合导航的系统时间以北斗时间为参考基准，测评所需的基础参数信息包括：进站过程中被测公交车的经纬度坐标、速度、横摆角速度、侧向加速度、航向角；被测公交车的参数，即车辆宽度、长度，后侧主天线至车头、车辆侧边的距离；站台边线的经纬度坐标，以四个点的坐标表示站台边线；

步骤二：数据处理

在获取测评所需基础参数的基础上，数据处理包括两部分，一部分是需要根据车后侧主天线的运动轨迹及车辆参数推算车辆四个角点的运动轨迹，此部分数据处理的目的是对公交进站停车的完成度进行评价；另一部分是考虑横摆角速度和侧向加速度数据受传感器噪声影响而波动太大，需要对该两组数据进行改进卡尔曼滤波处理，此部分数据处理的目的是对公交进站停车过程的稳定性进行评价；

子步骤1：定义与计算被测公交车四角点运动轨迹

以车辆左前角为起点，定义为A点，顺时针对被测公交车四个角点依次定义为A点，B点，C点，D点；车辆后侧主天线定义为E点；车头车尾中点分别定义为M点和N点；定义主天线至车头距离为Lf，至车辆侧边距离为Ll，车辆宽度为W，长度为L以便于计算；读取所得航向角定义为Dir；在获取被测公交车行驶轨迹时，因获取轨迹数据为组合导航所输出的经纬度信息，故需要对其进行高斯投影变换，使其单位化为米，便于解算测评指标项：

其中：

Ex(x,y)为横坐标变换函数，Ey(x,y)为纵坐标变换函数，x为纬度，y为经度；

采用弧度制计算数据处理所需角度：

则根据三角函数可计算得到被测公交车四个角点运动轨迹为：

其中，Ax(x,y)为A点横坐标变换函数，Ay(x,y)为A点纵坐标变换函数，Bx(x,y)为B点横坐标变换函数，By(x,y)为B点纵坐标变换函数，Cx(x,y)为C点横坐标变换函数，Cy(x,y)为C点纵坐标变换函数，Dx(x,y)为D点横坐标变换函数，Dy(x,y)为D点纵坐标变换函数，x为纬度，y为经度；

子步骤2：横摆角速度和侧向加速度的改进卡尔曼滤波处理

横摆角速度和侧向加速度可以表征车辆运动过程的稳定性，但由于受传感器噪声影响，直接采集获取的横摆角速度和侧向加速度数据波动较大，直接用于判断稳定性会产生较大误差，故需要进行滤波处理；采用改进卡尔曼滤波器的方法，其处理过程如下：

(1)由横摆角加速度与侧向加速度关系，可得：

其中，表示n-1时刻计算所得侧向加速度，表示n-1时刻计算所得横摆角速度，为n时刻先验侧向加速度，为n时刻先验横摆角速度，建立两者关系，V为车辆行驶速度；

(2)计算k时刻的先验估计协方差矩阵：

其中为n时刻的先验估计协方差矩阵，P_n-1为n-1时刻的后验估计协方差矩阵，Q为过程误差矩阵；

(3)计算卡尔曼增益：

其中H为状态变量到测量的状态转换矩阵，这里值为R为测量误差矩阵；

(4)计算后验估计值：

其中为n时刻后验侧向加速度，为n时刻先验横摆角速度，两者为滤波后的所得结果,Z_n为观测值矩阵，作为滤波的输入部分；

(5)更新协方差矩阵：

(6)引入遗忘因子：

在被测公交横摆角速度和侧向加速度数据的处理过程中，考虑车辆运动状态变化的瞬时性，新数据比老数据更能反映参数变化状况，故引入遗忘因子，修改新老数据在递推过程中的权重：

当γ较小时，遗忘较快；当γ较大时，遗忘较慢；此处为避免新数据对估值无法修正，故γ在(0.95，0.995)中取值；

步骤三：公交进站停车测评

子步骤1：完成度测评

(1)进站停车完成时车辆轴线与泊车位外侧边线夹角测评

首先，对于航向角的范围需要进行分类讨论：

再与车站外侧边线坐标整合计算：

其中(X1，Y1)，(X2，Y2)分别为已进行高斯投影坐标转换的车站外侧边线O点和P点坐标；

当夹角大于0时，车头朝向车站外侧，当夹角小于0时，车尾朝向车站外侧；当夹角满足-3°≤θ＜3°时进站停车符合标准；

(2)进站停车完成时测试车辆侧面距站台的距离测评

车辆侧面距站台的距离可通过三角形面积进行计算：

三边长分别为：

由海伦公式可知：

其中(X3，Y3)，(X4，Y4)分别为已进行高斯投影坐标转换的车站内侧边线S点和T点坐标；

由此可得角点A距离站台的距离函数ADis(x，y)，并依次类推得到其他B，C，D三个角点距离站台的距离函数BDis(x，y)，CDis(x，y)，DDis(x，y)，可结合夹角情况判断车辆角点与站台的距离：当θ≤0时，取BDis(x，y)和CDis(x，y)中最小值，当θ≥0时，取ADis(x，y)和DDis(x，y)中最小值，所得值d₁，d₂为所求距离，当0.05m≤d₁≤0.25m且0.05m≤d₂≤0.25m时，进站停车符合要求；

(3)进站停车完成时车头与车前标线的最大距离测评

车头距车前边线距离通过三角形面积进行计算：

三边长分别为：

由海伦公式可知：

其中(X2，Y2)，(X3，Y3)分别为已进行高斯投影坐标转换的车站边线P点和S点坐标；

由此可得角点A距车前标线的距离函数AFDis(x，y)，并由此类推得角点B距车前标线的距离函数BFDis(x，y)可根据实际情况选择AFDis(x，y)或BFDis(x，y)中最大值作为最大距离M_e，当0.05m≤M_e＜0.25m时，符合进站停车要求；

子步骤2：进站过程稳定性测评

被测公交车进站停车过程的稳定性也是衡量进站停车功能的关键指标之一，需要满足进站过程中的侧向加速度和横摆角速度最大值小于一定阈值，即

其中，表示经过改进卡尔曼滤波处理的侧向加速度值，a_{y_threshold}为侧向加速度阈值，表示经过改进卡尔曼滤波处理的横摆角速度值，ω_{z_threshold}为横摆角速度阈值，在整个过程中横摆角速度和侧向加速度均未大于阈值，则进站停车稳定性符合标准。