CN111272165B - 一种基于特征点标定的智能车定位方法 - Google Patents

Abstract

本发明了提供一种基于特征点标定的智能车定位方法，包括：判断智能车是位于盲区还是非盲区；当智能车位于GNSS非盲区，将GNSS信号、里程计数据和惯性测量单元数据组合，通过卡尔曼滤波获取智能车当前位姿，利用激光雷达扫描智能车周围环境，提取角点、圆弧特征并通过处理得到角点、圆心作为特征点，计算特征点的全局坐标和权重存入当前特征点列表；将当前特征点列表中的特征点加入已有特征点列表，将已有特征点列表中满足条件的特征点加入可信任列表；当智能车位于GNSS盲区，使用激光雷达获取周围可信任列表的特征点作为观测量，融合里程计数据和惯性测量单元数据，通过粒子滤波方法获取智能车当前位姿，从而实现智能车从非盲区进入盲区的定位无缝切换。

Description

一种基于特征点标定的智能车定位方法

技术领域

本发明涉及智能车定位方法领域，具体是一种基于特征点标定的智能车定位方法。

背景技术

智能车的定位研究是无人驾驶中的热点和难点。在当前的定位方式研究中，由于GNSS信号存在精度不足的问题，常常以GNSS结合其他传感器如里程计、IMU进行融合定位，以取得更好的定位效果。但是当智能车进入隧道、枝叶浓密的林荫道等环境下，GNSS信号质量过差难以提供有效的定位信息，仅仅依赖里程计和IMU进行定位又会存在累积误差和漂移，难以保证定位精度要求。

发明内容

本发明的目的在于克服上述技术缺陷，提出了一种基于特征点标定的智能车定位方法，在GNSS信号良好时，使用GNSS结合里程计和IMU通过卡尔曼滤波得到当前定位位置，同时使用激光雷达观察周围角点、圆弧等特征进行记录和标定，待数据稳定后转换到全局坐标系下储存；当GNSS信号质量较差时，智能车通过激光雷达观测先前标定点对里程计和IMU推算信息通过粒子滤波过程进行评价和校正，即使在无GNSS信号也也能在较长一段时间内仍保持较为准确的定位效果，保证智能车的安全行驶和实时状态监测。该方法能够解决智能车在进入隧道、楼宇密集的城市道路、林荫道等GNSS盲区时定位效果不稳定的问题。

为实现上述目的，本发明提出了一种基于特征点标定的智能车定位方法，所述方法包括：

根据GNSS锁星数确定GNSS信号质量，由此判断智能车是位于盲区还是非盲区；

当智能车位于GNSS非盲区，将GNSS信号、里程计数据和惯性测量单元数据进行组合，通过卡尔曼滤波获取智能车当前位姿，同时利用激光雷达扫描智能车周围环境，提取角点、圆弧特征并通过处理得到角点、圆心作为特征点，计算特征点的全局坐标和权重存入当前特征点列表；

将当前特征点列表中的特征点加入已有特征点列表，将已有特征点列表中满足条件的特征点加入可信任列表；

当智能车位于GNSS盲区，使用激光雷达获取周围可信任列表的特征点作为观测量，融合里程计数据和惯性测量单元数据，通过粒子滤波方法获取智能车当前位姿，从而实现智能车从非盲区进入盲区的定位无缝切换。

作为上述方法的一种改进，所述当智能车位于GNSS非盲区，将GNSS信号、里程计数据和惯性测量单元数据进行组合，通过卡尔曼滤波获取智能车当前位姿；具体包括：

步骤1-1)通过智能车左后轮、右后轮的光电编码器，计算智能车位移和航向角变化量，具体过程如下：

计算单位采样时间T_S内左后轮和右后轮位置增量Δd_L和Δd_R：

其中，ΔN_L和ΔN_R分别为左后轮和右后轮采样时间内脉冲增量，D为车轮当量直径，P为光码盘光栅总数，m为编码器减速率；

计算t_k-1时刻到t_k时刻智能车的位移变化量ΔD_k和航向角变化量

其中，W为两后轮轴线长度；

计算t_k时刻的速度：

步骤1-2)从IMU中直接得到t_k时刻航向角加速度ω_k，对每一时刻角加速度ω_k进一步积分得到第二航向角变化量

航向角

步骤1-3)通过智能车采集到的GNSS信息通过坐标转换得到t_k时刻智能车位置

步骤1-4)将上述信息通过卡尔曼滤波器融合；定义智能车t_k-1时刻状态为：X_k-1＝[x_k-1,y_k-1,V_k-1,θ_k-1,ω_k-1]^T，运动和观测模型分别为：

运动过程状态转移函数为：

其中，

为t_k时刻智能车的预测状态，

为观测模型转移函数，

为加入噪声的观测量，c_k是协方差为Q的系统过程噪声，v_k是协方差为R的传感器测量噪声；

基于上述模型，获取智能车当前位姿。

作为上述方法的一种改进，所述卡尔曼滤波器的预测和更新过程，具体为：

预测状态量：

预测误差协方差矩阵：P_(k|k-1)＝AP_k-1A^T+Q

更新最优估计状态量：

更新卡尔曼增益：

更新误差协方差矩阵：P_k＝[I-K_kH]P_(k|k-1)

其中，

是t_k时刻的预测位姿状态量，A为状态转移矩阵，

是t_k-1时刻的最优位姿状态量，P_k-1是t_k-1时刻已更新的误差协方差矩阵，K_k是t_k时刻的卡尔曼增益，H为测量矩阵，观测量

是t_k时刻的最优位姿状态量。

作为上述方法的一种改进，所述，利用激光雷达扫描智能车周围环境，提取角点、圆弧特征并通过处理得到角点、圆心作为特征点，计算特征点的全局坐标和权重存入当前特征点列表；具体包括：

根据预先设计的点云密度阈值，去除激光雷达获取的点云数据中的个别噪声点；然后根据各点间欧几里得距离阈值将点云分割成不同的点云簇；

基于Hough变换机制从点云簇中提取圆弧特征，其对应的圆心作为特征点，将圆心坐标作为该特征点在车体局部坐标系中坐标(x_L,y_L)，并计算权重；

从点云簇中检测角点作为特征点，将角点坐标作为该特征点在车体局部坐标系中坐标(x_L,y_L)，并计算权重；

将特征点在车体局部坐标系中坐标(x_L,y_L)转换为全局坐标系下的坐标(x_G,y_G)，和权重一起存入当前特征点列表。

作为上述方法的一种改进，所述基于Hough变换机制从点云簇中提取圆弧特征，其对应的圆心作为特征点，将圆心坐标作为该特征点在车体局部坐标系中坐标(x_L,y_L)，并计算权重；具体包括：

对某一点云簇，将各点通过Hough变换对某一圆参数进行投票，表示公式为(x-a)²+(y-b)²＝R²；添加约束：a.票数大于阈值；b.合成圆的直径大于阈值；同时满足以上两约束的圆参数作为可观测的圆弧特征；

根据得到的圆弧特征确定圆心坐标和权重：

定义票数权重

N为该圆特征得到的票数，圆弧权重

其中，α为各扫描点围成圆弧对应圆心角，其公式为α＝arcsin(L/2R)，L为圆弧两端点连接得到的弦长，当圆弧中存在点到这条弦的距离大于R时，说明对应圆心角大于π，此时α＝2π-arcsin(L/2R)；

该圆弧对应的圆心为特征点，在车体局部坐标系中坐标为(x_L,y_L)；权重为

作为上述方法的一种改进，所述从点云簇中检测角点作为特征点，将角点坐标作为该特征点在车体局部坐标系中坐标(x_L,y_L)，并计算权重，具体包括：

在点云簇中检测断点：若一点和下一个点间距离大于断点检测阈值，则认为这两个点均为断点；由断点进一步分割点云簇；

将某一段点云簇S_i起点和终点连接得到直线l，计算该点云簇中每个点到直线l的距离，找到距离最大的点K_p，若其到直线l距离d_p大于设定阈值d_max，则认为该点为角点，并以该点为分界点将点云簇S_i分为两个子点云簇，再分别对两个分割都点云簇执行上述角点判定过程，直到找到所有点云簇中角点；同时，添加两个约束：a.角点到两边直线端点距离大于一定阈值；b.两边直线扫描点个数大于设定值；将未满足上述两个约束的角点去除；

使用最小二乘法将得到的角点两边的子点云簇拟合为两条直线l₁和l₂，其中任意一条直线的拟合过程为：

设计代价函数为：

其中，ρ为车体局部坐标系原点到直线的垂直距离，α为x轴与直线法线的夹角，(a_s,b_s)为第s个激光雷达扫描点在车体局部坐标系的坐标；参数ε＝var(Δρ₁,Δρ₂,....,Δρ_S)用于衡量直线特征参数误差，Δρ_s表示直线中某一扫描点到直线的垂直距离，1≤s≤S；直线权重为

通过直线特征参数对两直线求交点，其交点的坐标作为该特征点在车体局部坐标系中坐标(x_L,y_L)；

该特征点的权重为

其中，

分别为直线l₁和l₂的权重。

作为上述方法的一种改进，所述将当前特征点列表中的特征点加入已有特征点列表，具体包括：

计算已有特征点列表中任一列所有记录坐标加权平均值，作为该列特征点的拟坐标X_s：

其中，下标f表示该特征点的记录坐标序号，Z_f为记录坐标，w_f为对应的权重；

从当前特征点列表取出一个特征点，与已有特征点列表进行匹配，当该特征点与已有特征点列表的某列的拟坐标距离小于阈值，则认为匹配成功，将该特征点坐标和权重信息加入这一列，若匹配不成功，则认为该特征点为新特征点，在已有特征点列表创建新的一列，保存这个新特征点。

作为上述方法的一种改进，所述将已有特征点列表中满足条件的特征点加入可信任列表，具体包括：

步骤2-1)对已有特征点列表的某一特征点，定义观察总次数N_A为自激光雷达扫描到某一特征点以来对周围环境扫描总次数，定义有效观察次数N_V为某一特征成功匹配的总次数，定义有效观察率为N_V/N_A×100％，则当特征点满足条件：N_V大于阈值且有效观察率大于阈值，则准备将其加入可信任列表；

定义最优估计点个数N＝N_V×β，式中β为最优估计率，再次计算加入新的记录坐标后的拟坐标：

取距Xs最近的N个点再次进行加权平均得到修正坐标：

权重为：w_F＝1/ε_V，ε_V为最优估计点的分布方差，同时对ε_V进行放大，剔除放大后分布范围外的特征点坐标；

若w_F大于阈值，则将该特征点的修正坐标X_F和权重w_F加入可信任列表。

作为上述方法的一种改进，所述当智能车位于GNSS盲区，使用激光雷达获取周围可信任的特征点作为观测量，融合里程计数据和惯性测量单元数据，通过粒子滤波方法获取智能车当前位姿，具体包括：

步骤3-1)以智能车进入GNSS盲区前一时刻的最优位姿

作为粒子群的初始分布，以

和误差协方差矩阵P₀构建概率密度函数作为提议分布

随机抽取M个初始粒子，在提议分布

内各粒子在(x₀,y₀,θ₀)周围作高斯分布，则t_k时刻第i个粒子的状态表示为：

权重

步骤3-2)粒子预测过程：

在t_k时刻，运动方程为

其中，状态转移函数为：

式中，d_k-1为协方差矩阵Q₁的系统过程噪声；

步骤3-3)根据激光雷达获取智能车周围环境的当前特征点，作为观测量，同时从可信任列表中进行特征点的匹配，计算各个粒子对匹配出的特征点的预测观测量，由此计算预测观测量和观测量的差值；

步骤3-4)权重更新：

根据预测观测量和观测量的差值，更新各粒子权重：

其中，N为从可信任表中与当前智能车周围环境匹配的特征点个数，σ_x、σ_y表示激光雷达的测量方差，

表示特征点权重；

和

为第i个粒子对第n个特征点的预测观测量和观测量的误差，1≤n≤N；

步骤3-5)权重归一化：

步骤3-6)计算有效粒子数

当N_eff小于阈值，进行重采样过程，采用轮盘赌法进行采样，权重较大的粒子由较大可能被保留，重采样完成后所有粒子权值归一化为1/M，否则，进入步骤3-7)；

步骤3-7)状态估计输出：

在t_k时刻，状态估计值为

由此得到智能车当前位姿。

作为上述方法的一种改进，所述根据激光雷达获取智能车周围环境的当前特征点，作为观测量，同时从可信任列表中进行特征点的匹配，计算各个粒子对匹配出的特征点的预测观测量，由此计算预测观测量和观测量的差值；具体包括：

在t_k时刻，在步骤3-2)后得到第i个粒子预测状态

以第i个粒子的预测位置

为原点，以智能车前进方向为X方向，垂直X轴指向车体左侧为Y轴方向，Z轴垂直向上建立第i个粒子的局部坐标系；

逐一计算可信任列表中各特征点同各粒子在全局坐标系下x，y轴向距离

和

下标表示第j个特征点，上标表示第i个粒子；进一步得到欧几里得距离

若此距离小于激光雷达高精度检测范围，则第j个特征点在检测范围内，将此特征点储存到对比列表中，并重新给定序号n代表其顺序，共得到N个符合要求的特征点；将第n个特征点对应的全局坐标系下距离转换到i个粒子的局部坐标系下距离：

和

为第n个特征点在第i个粒子下的预测观测量；

在当前时刻t_k，通过激光雷达扫描周围环境，通过检测圆点和角点，计算在车体局部坐标系下的特征点坐标(x_q,y_q)，存入当前特征点列表，q表示其序号，(x_q,y_q)为观测量，共有q_max个当前特征点；

逐一计算当前特征点列表中的特征点坐标对应的观测量，与对比列表中第n个标定点在第i个粒子下的预测观测量的差

和

最后使用最近邻法进行联立，对对比列表中特征点n求最小值

求得最小值对应序号为q₀，则n与q₀联立，此时对应误差为

和

记为

和

遍历对比列表，执行该过程，完成特征点识别与匹配。

与现有的智能车定位方法相比，本发明的有益效果为：

1、本发明的方法在GNSS信号良好时提前进行周围环境特征观察记录，标定点的坐标包含了GNSS绝对定位的信息，进入GNSS盲区即可启用对智能车定位位置进行评价和更新，无需智能车预先在盲区进行局部建图，可广泛应用于车辆进入隧道、林荫道、城市道路、室外进入室内等场景，非常具有实用性，且不同阶段采用不同的融合定位方法，可有效提高智能车定位精度，为智能车行驶安全提供保障；

2、本发明的方法能够解决智能车在进入隧道、楼宇密集的城市道路、林荫道等GNSS盲区时定位效果不稳定的问题。

附图说明

图1是本发明的基于特征点标定的智能车定位方法的流程图；

图2是GNSS良好情况下智能车通过激光雷达观察标定周围环境特征示意图；

图3是特征点的反复标定和修正流程图；

图4是以观察点坐标求取可信任标定点坐标的示意图；

图5是多传感器通过卡尔曼滤波融合定位过程图；

图6是智能车进入林荫道后通过观测可信任标定点进行定位的示意图；

图7是GNSS盲区下多传感器通过粒子滤波融合定位流程图。

具体实施方式

本发明公开了一种基于特征点标定的智能车定位方法，即使在楼宇密集的城市道路、林荫道、隧道等GNSS信号较差环境也可保持较长时间的高定位精度。在GNSS信号良好的状态下，采集车载里程计和IMU数据，与GNSS定位位置进行卡尔曼滤波融合，得到较为准确的当前位置，同时激光雷达不断扫描观察周围环境，提取环境中角点、圆弧特征信息，并拟合出角点、圆心坐标，智能车行进过程中不停地对各个特征点进行扫描和标定，最终得到各特征点较为稳定、准确的坐标值转换到全局坐标系下进行储存。当GNSS信号变差严重影响定位效果时，定位方法转变为以里程计和IMU数据为预测信息，激光雷达对特征标定点观测量进行更新的粒子滤波方法，从而得到较为准确的车辆定位位置。

IMU为惯性测量单元(Inertial measurement unit)，是一个测量物体三轴姿态角(或角速率)以及加速度的装置，其中陀螺仪及加速度计是IMU的主要元件。里程计包括：分别设置在左后轮和右后轮的光电编码器。

下面结合附图对本发明的技术方案作进一步详细说明。

如图1所示，本发明提出了一种基于特征点标定的智能车定位方法，包括以下步骤：

步骤1)如图2所示，智能车正常行驶在路面上，一段时间后将进入林荫道内，在GNSS信号良好的情况下，智能车在道路上正常行驶过程中，采用激光雷达观察路牙、两边树木躯干等特征明显静态物体，提取角点、圆弧特征并通过处理得到角点、圆心作为特征点坐标，经过多次观察和标定后加入可信任表，经过坐标变换储存在全局坐标系(大地坐标系)下备用，进入GNSS盲区时即作为绝对坐标点启用为激光雷达提供指导性观测量，具体过程如下：

(1.1)激光雷达数据的预处理过程。行进过程中得到的原始点云数据要经过预先处理，这样可以大大简化后续工作，具体过程如下：

(1.1.1)去除噪声点。根据预先设计的点云密度阈值，去除点云中的个别噪声点；

(1.1.2)扫描点聚类。根据各点间欧几里得距离阈值将点云分割成不同的点云簇；

(1.2)特征点判定和坐标求取过程，具体包括以下步骤：

(1.2.1)首先进行圆弧特征的判定和处理，圆弧特征判定及圆心坐标求取过程如下：

(1.2.1.1)基于Hough变换机制对某一圆特征进行提取。对某一点云簇，将各点通过Hough变换对某一圆参数进行投票，表示公式为(x-a)²+(y-b)²＝R²。添加约束：a.票数大于阈值；b.合成圆的直径大于阈值。满足以上两约束的圆参数才可作为可观测的圆弧特征。

(1.2.1.2)根据得到的圆弧特征确定圆心坐标和权重。定义票数权重

N为该圆特征得到的票数，圆弧权重

其中，α为各扫描点围成圆弧对应圆心角，其公式为α＝arcsin(L/2R)，L为圆弧两端点连接得到的弦长，当圆弧中存在点到这条弦的距离大于R时，说明对应圆心角大于π，此时α＝2π-arcsin(L/2R)。最终得到圆心坐标权重

(1.2.2)特征点种类之一的角点特征判定及求取过程如下：

(1.2.2.1)断点检测。一个点云簇中，若一点和下一个点间距离大于断点检测阈值，则认定这两个点为断点；

(1.2.2.2)角点判定。得到由断点进一步分割的点云簇后，将某一段点云簇S_i起点和终点连接得到直线l，计算该点云簇中每个点到直线l的距离，找到距离最大的点K_j，若其到l距离d_j大于设定阈值d_max，则认为该点为角点，并以该点为分界点将点云簇S_i分为两个子点云簇，再分别对两个分割都点云簇执行上述角点判定过程，直到找到所有点云簇中角点。同时，添加两个约束：a.角点到两边直线端点距离大于一定阈值；b.两边直线扫描点个数大于设定值。将未满足条件的角点去除。

(1.2.2.3)直线特征参数估计。使用最小二乘法将上一步骤得到的角点两边的子点云簇拟合为直线，拟合过程中的代价函数为：

J(α,ρ)＝min∑(ρ-x_icosα-y_isinα)²

式中，ρ为车体局部坐标系原点到直线的垂直距离，α为坐标系x轴与直线法线的夹角，(x_i,y_i)为第i个扫描点在局部坐标系中坐标。参数ε＝var(Δρ₁,Δρ₂,....,Δρ_N)用于衡量直线特征参数误差，Δρ_m表示直线中某一扫描点到直线的垂直距离，定义直线权重

用于后续角点的权重计算。

(1.2.2.4)拟合求交得到角点坐标和权重。得到各直线特征参数后对两直线求交点，即得到角点在局部坐标系下坐标。得到的角点坐标权重为

其中，

分别为其两边直线权重。

(1.2.3)通过以上步骤得到特征点在车体局部坐标系中坐标(x_L,y_L)后，还要通过坐标转换，得到全局坐标系(大地坐标系)下特征点坐标(x_G,y_G)，和权重一起存入当前特征点列表；

转换公式为：

其中，(x₀,y₀)为智能车在全局坐标系中坐标，由GNSS融合多传感器定位得到。

(1.3)特征点的反复标定和修正；

其中的特征点记录在可信任列表和已有特征点列表，已有特征点列表的每一列记录一个特征点，包括多个记录坐标和权重，可信任列表存储了修正后的特征点的坐标和权重。

如图3所示，具体步骤如下：

(1.3.1)根据所使用激光雷达型号确定特征点观测范围，只有在较高的检测精度范围内才可进行特征点的判定及标定过程；

(1.3.2)计算已有特征点列表中任一列所有记录坐标加权平均值，作为该列特征点的拟坐标X_s：

其中，下标f表示该特征点的记录坐标序号，Z_f为记录坐标，w_f为对应的权重；

从当前特征点列表取出一个特征点，与已有特征点列表进行匹配，当该特征点与已有特征点列表的某列的拟坐标距离小于阈值，则认为匹配成功，将该特征点坐标和权重信息加入这一列，进入步骤(1.3.3)；若匹配不成功，则认为该特征点为新特征点，在已有特征点列表创建新的一列，保存这个新特征点。

(1.3.3)定义观察总次数N_A为自激光雷达扫描到某一特征点以来对周围环境扫描总次数，定义有效观察次数N_V为某一特征成功匹配的总次数，定义有效观察率为N_V/N_A×100％，则当特征点满足条件：N_V大于阈值且有效观察率大于阈值，则准备将其加入可信任列表；

(1.3.4)定义最优估计点个数N＝N_V×β，式中β为最优估计率，再次计算加入新的记录坐标后的拟坐标：

取距Xs最近的N个点再次进行加权平均得到修正坐标：

权重为：w_F＝1/ε_V，ε_V为最优估计点的分布方差，同时对ε_V进行放大，剔除方差分布范围外的特征点坐标；

(1.3.5)通过反复标定和修正过程，若观测到的某一特征w_F大于阈值，则将该特征点的修正坐标X_F和权重w_F加入可信任列表，并按照步骤(1.3.3)和(1.3.4)不断更新位置和其权重，待到GNSS信号变差时就作为可信任特征点启用。如图4所示。

步骤2)将GNSS信息、里程计数据与IMU数据进行组合，通过卡尔曼滤波得到全局坐标系下的智能车当前位姿，如图5所示，具体过程如下：

(2.1)以智能车启动时车体中心为原点O_G，智能车前进方向为X_G轴方向，垂直X_G轴指向车体左侧为Y_G轴方向，Z_G轴垂直向上建立车体局部坐标系；本方法中已经事先将搭载的里程计和IMU惯性单元坐标系转换到车体局部坐标系下，各传感器时间钟已校准完毕，且只考虑智能车在水平面内运动且没有俯仰和翻滚动作，因此Z_G轴坐标始终为0且只有偏航角的变化，对里程计、IMU和GNSS信息进行卡尔曼滤波融合得到智能车在全局坐标系下的当前位姿(x,y,θ)，其中，θ为航向角，即车辆当前航向和X轴正向夹角；

(2.2)通过智能车左后轮、右后轮的光电编码器，计算智能车位移和航向角变化量，具体过程如下：

(2.2.1)t₀时刻，智能车在里程计坐标系中位姿(x₀,y₀,0)为(0,0,0)；

(2.2.2)计算单位采样时间T_S内左后轮和右后轮位置增量Δd_L和Δd_R：

其中，ΔN_L和ΔN_R分别为左后轮和右后轮采样时间内脉冲增量，D为车轮当量直径，P为光码盘光栅总数，m为编码器减速率；

(2.2.3)计算t_k-1时刻到t_k时刻智能车的位移变化量ΔD_k和航向角变化量

式中，W为两后轮轴线长度；WO上标表示数据来源：里程计；

(2.2.4)计算t_k时刻的速度：

(2.3)由IMU中可直接得到t_k时刻航向角加速度ω_k，对每一时刻角加速度ω_k进一步积分可得航向角变化量

航向角

(2.4)将智能车采集到的GNSS信息转换为全局坐标系中坐标，在WGS-84坐标系中原始经纬度(φ,λ)进行高斯投影，转换到地心地固坐标系，再转换车体局部坐标系，得到t_k时刻车辆位置

(2.5)将上述信息通过卡尔曼滤波器融合；定义智能车t_k-1时刻状态为：X_k-1＝[x_k-1,y_k-1,V_k-1,θ_k-1,ω_k-1]^T，运动和观测模型分别为：

运动过程状态转移函数为：

其中，

为t_k时刻智能车的预测状态，

为观测模型转移函数，

为加入噪声的观测量；c_k是协方差为Q的系统过程噪声，v_k是协方差为R的传感器测量噪声，两噪声矩阵均为高斯分布的白噪声，系统过程噪声c_k初始值可根据经验值设置，传感器测量噪声v_k根据里程计、IMU、GNSS各设备质量自适应调节。

滤波器预测和更新过程为：

预测状态量：

预测误差协方差矩阵：P_(k|k-1)＝AP_k-1A^T+Q

更新最优估计状态量：

更新卡尔曼增益：

更新误差协方差矩阵：P_k＝[I-K_kH]P_(k|k-1)

其中，

是t_k时刻的预测位姿状态量，A为状态转移矩阵，

是t_k-1时刻的最优位姿状态量，P_k-1是t_k-1时刻已更新的误差协方差矩阵，K_k是t_k时刻的卡尔曼增益，H为测量矩阵，观测量

是t_k时刻的最优位姿状态量。智能车在行进过程中，重复以上预测和更新过程，即完成卡尔曼滤波过程，最终得到较为准确的车辆当前位置。

步骤3)智能车进入林荫道时GNSS锁星数会变少，GNSS信号不稳定，以GNSS锁星数判定GNSS信号质量以决定定位方式转换时机，当锁星数大于14时，使用GNSS通过卡尔曼滤波融合多传感器进行定位，锁星数小于14，即认为智能车进入GNSS信号较差区域，GNSS信号不可用，切换为激光雷达融合多传感器进行粒子滤波定位。

(4)如图6所示，当智能车进入林荫道后，GNSS信号变差，认为智能车进入GNSS盲区，使用激光雷达观察可信任的环境特征点提供指导性的观测量，融合里程计和IMU信息，通过粒子滤波方法修正单纯的航迹推算定位结果，得到智能车当前位姿，具体过程如下：

(4.1)粒子群初始化。

同步骤(2.1)建立车体局部坐标系，实现车辆前后无缝定位，以智能车进入GNSS盲区前一时刻的最优位姿

作为粒子群的初始分布，以

和误差协方差矩阵P₀构建概率密度函数作为提议分布

随机抽取M个初始粒子，在提议分布

内各粒子在(x₀,y₀,θ₀)周围作高斯分布，则t_k时刻第i个粒子的状态表示为：

权重

(4.2)粒子预测过程。

在t_k时刻，根据智能车控制信息u_k推算t_k时刻各粒子位姿分布，即根据里程计和IMU信息得到粒子的位移和航向估计，里程计信息获取同步骤(2.2)，得到ΔD和

由IMU中角加速度进一步积分得到

运动方程为

其中，状态转移函数为：

式中，d_k-1为协方差为Q₁的系统过程噪声；

(4.3)特征点识别与匹配。

在t_k时刻，在步骤3-2)后得到第i个粒子预测状态

以第i个粒子的预测位置

为原点，以智能车前进方向为X方向，垂直X轴指向车体左侧为Y轴方向，Z轴垂直向上建立第i个粒子的局部坐标系；

逐一计算可信任列表中各特征点同各粒子在全局坐标系下x，y轴向距离

和

下标表示第j个特征点，上标表示第i个粒子；进一步得到欧几里得距离

若此距离小于激光雷达高精度检测范围，则第j个特征点在检测范围内，将此特征点储存到对比列表中，并重新给定序号n代表其顺序，共得到N个符合要求的特征点；将第n个特征点对应的全局坐标系下距离转换到第i个粒子的局部坐标系下距离：

和

为第i个粒子对第n个特征点的预测观测量；

在当前时刻t_k，通过激光雷达扫描周围环境，通过检测圆点和角点，计算在车体局部坐标系下的特征点坐标(x_q,y_q)，存入当前特征点列表，q表示其序号，(x_q,y_q)为观测量，共有q_max个当前特征点；

逐一计算当前特征点列表中的特征点坐标对应的观测量，与对比列表中第n个标定点在第i个粒子下的预测观测量的差

和

最后使用最近邻法进行联立，对对比列表中特征点n求最小值

求得最小值对应序号为q₀，则n与q₀联立，此时对应误差为

和

记为

和

遍历对比列表，执行该过程，完成特征点识别与匹配。

(4.4)权重更新。

根据观测量和预测观测量的差，更新各粒子权重：

其中，N表示从可信任表中与当前智能车周围环境匹配的特征点个数，σ_x、σ_y表示激光雷达的测量方差，

表示特征点权重。

(4.5)权重归一化。公式为：

(4.6)重采样。计算有效粒子数

当N_eff小于阈值，进行重采样过程，采用轮盘赌法进行采样，权重较大的粒子由较大可能被保留，重采样完成后所有粒子权值归一化为1/M，否则，进入步骤(4.7)；

(4.7)状态估计输出。

t_k时刻系统状态估计值为

得到当前车辆定位位置，重复步骤(4.2)到本步骤，即完成粒子滤波定位过程。

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (3)

1.一种基于特征点标定的智能车定位方法，所述方法包括：

根据GNSS锁星数确定GNSS信号质量，由此判断智能车是位于盲区还是非盲区；

当智能车位于GNSS非盲区，将GNSS信号、里程计数据和惯性测量单元数据进行组合，通过卡尔曼滤波获取智能车当前位姿，同时利用激光雷达扫描智能车周围环境，提取角点、圆弧特征并通过处理得到角点、圆心作为特征点，计算特征点的全局坐标和权重存入当前特征点列表；

将当前特征点列表中的特征点加入已有特征点列表，将已有特征点列表中满足条件的特征点加入可信任列表；

当智能车位于GNSS盲区，使用激光雷达获取周围可信任列表的特征点作为观测量，融合里程计数据和惯性测量单元数据，通过粒子滤波方法获取智能车当前位姿，从而实现智能车从非盲区进入盲区的定位无缝切换；

所述当智能车位于GNSS非盲区，将GNSS信号、里程计数据和惯性测量单元数据进行组合，通过卡尔曼滤波获取智能车当前位姿；具体包括：

步骤1-1)通过智能车左后轮、右后轮的光电编码器，计算智能车位移和航向角变化量，具体过程如下：

计算单位采样时间T_S内左后轮和右后轮位置增量Δd_L和Δd_R：

其中，ΔN_L和ΔN_R分别为左后轮和右后轮采样时间内脉冲增量，D为车轮当量直径，P为光码盘光栅总数，m为编码器减速率；

计算t_k-1时刻到t_k时刻智能车的位移变化量ΔD_k和航向角变化量

其中，W为两后轮轴线长度；

计算t_k时刻的速度：

步骤1-2)从IMU中直接得到t_k时刻航向角加速度ω_k，对每一时刻角加速度ω_k进一步积分得到第二航向角变化量

航向角

步骤1-3)通过智能车采集到的GNSS信息通过坐标转换得到t_k时刻智能车位置

步骤1-4)将上述信息通过卡尔曼滤波器融合；定义智能车t_k-1时刻状态为：X_k-1＝[x_k-1,y_k-1,V_k-1,θ_k-1,ω_k-1]^T，运动和观测模型分别为：

运动过程状态转移函数为：

其中，

为t_k时刻智能车的预测状态，

为观测模型转移函数，

为加入噪声的观测量，c_k是协方差为Q的系统过程噪声，v_k是协方差为R的传感器测量噪声；

基于上述模型，获取智能车当前位姿；

所述卡尔曼滤波器的预测和更新过程，具体为：

预测状态量：

预测误差协方差矩阵：P_(k|k-1)＝AP_k-1A^T+Q

更新最优估计状态量：

更新卡尔曼增益：

更新误差协方差矩阵：P_k＝[I-K_kH]P_(k|k-1)

其中，

是t_k时刻的预测位姿状态量，A为状态转移矩阵，

是t_k-1时刻的最优位姿状态量，P_k-1是t_k-1时刻已更新的误差协方差矩阵，K_k是t_k时刻的卡尔曼增益，H为测量矩阵，观测量

是t_k时刻的最优位姿状态量；

所述，利用激光雷达扫描智能车周围环境，提取角点、圆弧特征并通过处理得到角点、圆心作为特征点，计算特征点的全局坐标和权重存入当前特征点列表；具体包括：

根据预先设计的点云密度阈值，去除激光雷达获取的点云数据中的个别噪声点；然后根据各点间欧几里得距离阈值将点云分割成不同的点云簇；

基于Hough变换机制从点云簇中提取圆弧特征，其对应的圆心作为特征点，将圆心坐标作为该特征点在车体局部坐标系中坐标(x_L,y_L)，并计算权重；

从点云簇中检测角点作为特征点，将角点坐标作为该特征点在车体局部坐标系中坐标(x_L,y_L)，并计算权重；

将特征点在车体局部坐标系中坐标(x_L,y_L)转换为全局坐标系下的坐标(x_G,y_G)，和权重一起存入当前特征点列表；

所述基于Hough变换机制从点云簇中提取圆弧特征，其对应的圆心作为特征点，将圆心坐标作为该特征点在车体局部坐标系中坐标(x_L,y_L)，并计算权重；具体包括：

对某一点云簇，将各点通过Hough变换对某一圆参数进行投票，表示公式为(x-a)²+(y-b)²＝r²；添加约束：(1)票数大于阈值；(2)合成圆的直径大于阈值；同时满足以上两约束的圆参数作为可观测的圆弧特征；

根据得到的圆弧特征确定圆心坐标和权重：

定义票数权重

N为该圆特征得到的票数，圆弧权重

其中，α为各扫描点围成圆弧对应圆心角，其公式为α＝arcsin(L/2r)，L为圆弧两端点连接得到的弦长，当圆弧中存在点到这条弦的距离大于r时，说明对应圆心角大于π，此时α＝2π-arcsin(L/2r)；

该圆弧对应的圆心为特征点，在车体局部坐标系中坐标为(x_L,y_L)；权重为

所述从点云簇中检测角点作为特征点，将角点坐标作为该特征点在车体局部坐标系中坐标(x_L,y_L)，并计算权重，具体包括：

在点云簇中检测断点：若一点和下一个点间距离大于断点检测阈值，则认为这两个点均为断点；由断点进一步分割点云簇；

将某一段点云簇S_i起点和终点连接得到直线l，计算该点云簇中每个点到直线l的距离，找到距离最大的点K_p，若其到直线l距离d_p大于设定阈值d_max，则认为该点为角点，并以该点为分界点将点云簇S_i分为两个子点云簇，再分别对两个分割都点云簇执行上述角点判定过程，直到找到所有点云簇中角点；同时，添加两个约束：(1)角点到两边直线端点距离大于一定阈值；(2)两边直线扫描点个数大于设定值；将未满足上述两个约束的角点去除；

使用最小二乘法将得到的角点两边的子点云簇拟合为两条直线l₁和l₂，其中任意一条直线的拟合过程为：

设计代价函数为：

其中，ρ为车体局部坐标系原点到直线的垂直距离，α为x轴与直线法线的夹角，(a_s,b_s)为第s个激光雷达扫描点在车体局部坐标系的坐标；参数ε＝var(Δρ₁,Δρ₂,....,Δρ_S)用于衡量直线特征参数误差，Δρ_s表示直线中某一扫描点到直线的垂直距离，1≤s≤S；直线权重为

通过直线特征参数对两直线求交点，其交点的坐标作为该特征点在车体局部坐标系中坐标(x_L,y_L)；

该特征点的权重为

其中，

分别为直线l₁和l₂的权重；

所述将当前特征点列表中的特征点加入已有特征点列表，具体包括：

计算已有特征点列表中任一列所有记录坐标加权平均值，作为该列特征点的拟坐标X_s：

其中，下标f表示该特征点的记录坐标序号，Z_f为记录坐标，w_f为对应的权重；

从当前特征点列表取出一个特征点，与已有特征点列表进行匹配，当该特征点与已有特征点列表的某列的拟坐标距离小于阈值，则认为匹配成功，将该特征点坐标和权重信息加入这一列，若匹配不成功，则认为该特征点为新特征点，在已有特征点列表创建新的一列，保存这个新特征点；

所述将已有特征点列表中满足条件的特征点加入可信任列表，具体包括：

步骤2-1)对已有特征点列表的某一特征点，定义观察总次数N_A为自激光雷达扫描到某一特征点以来对周围环境扫描总次数，定义有效观察次数N_V为某一特征成功匹配的总次数，定义有效观察率为N_V/N_A×100％，则当特征点满足条件：N_V大于阈值且有效观察率大于阈值，则准备将其加入可信任列表；

定义最优估计点个数N＝N_V×β，式中β为最优估计率，再次计算加入新的记录坐标后的拟坐标：

取距Xs最近的N个点再次进行加权平均得到修正坐标：

权重为：w_F＝1/ε_V，ε_V为最优估计点的分布方差，同时对ε_V进行放大，剔除放大后分布范围外的特征点坐标；

若w_F大于阈值，则将该特征点的修正坐标X_F和权重w_F加入可信任列表。

2.根据权利要求1所述的基于特征点标定的智能车定位方法，其特征在于，所述当智能车位于GNSS盲区，使用激光雷达获取周围可信任的特征点作为观测量，融合里程计数据和惯性测量单元数据，通过粒子滤波方法获取智能车当前位姿，具体包括：

步骤3-1)以智能车进入GNSS盲区前一时刻的最优位姿

作为粒子群的初始分布，以

和误差协方差矩阵P₀构建概率密度函数作为提议分布

随机抽取M个初始粒子，在提议分布

内各粒子在(x₀,y₀,θ₀)周围作高斯分布，则t_k时刻第i个粒子的状态表示为：

权重

步骤3-2)粒子预测过程：

在t_k时刻，运动方程为

其中，状态转移函数为：

式中，d_k-1为协方差矩阵Q₁的系统过程噪声；

步骤3-3)根据激光雷达获取智能车周围环境的当前特征点，作为观测量，同时从可信任列表中进行特征点的匹配，计算各个粒子对匹配出的特征点的预测观测量，由此计算预测观测量和观测量的差值；

步骤3-4)权重更新：

根据预测观测量和观测量的差值，更新各粒子权重：

其中，N为从可信任表中与当前智能车周围环境匹配的特征点个数，σ_x、σ_y表示激光雷达的测量方差，

表示特征点权重；

和

为第i个粒子对第n个特征点的预测观测量和观测量的误差，1≤n≤N；

步骤3-5)权重归一化：

步骤3-6)计算有效粒子数

当

小于阈值，进行重采样过程，采用轮盘赌法进行采样，权重较大的粒子由较大可能被保留，重采样完成后所有粒子权值归一化为1/M，否则，进入步骤3-7)；

步骤3-7)状态估计输出：

在t_k时刻，状态估计值为

由此得到智能车当前位姿。

3.根据权利要求2所述的基于特征点标定的智能车定位方法，其特征在于，所述根据激光雷达获取智能车周围环境的当前特征点，作为观测量，同时从可信任列表中进行特征点的匹配，计算各个粒子对匹配出的特征点的预测观测量，由此计算预测观测量和观测量的差值；具体包括：

在t_k时刻，在步骤3-2)后得到第i个粒子预测状态

以第i个粒子的预测位置

为原点，以智能车前进方向为X方向，垂直X轴指向车体左侧为Y轴方向，Z轴垂直向上建立第i个粒子的局部坐标系；

逐一计算可信任列表中各特征点同各粒子在全局坐标系下x，y轴向距离

和

下标表示第j个特征点，上标表示第i个粒子；进一步得到欧几里得距离

若此距离小于激光雷达高精度检测范围，则第j个特征点在检测范围内，将此特征点储存到对比列表中，并重新给定序号n代表其顺序，共得到N个符合要求的特征点；将第n个特征点对应的全局坐标系下距离转换到i个粒子的局部坐标系下距离：

和

为第n个特征点在第i个粒子下的预测观测量；

在当前时刻t_k，通过激光雷达扫描周围环境，通过检测圆点和角点，计算在车体局部坐标系下的特征点坐标(x_q,y_q)，存入当前特征点列表，q表示其序号，(x_q,y_q)为观测量，共有q_max个当前特征点；

逐一计算当前特征点列表中的特征点坐标对应的观测量，与对比列表中第n个标定点在第i个粒子下的预测观测量的差

和

最后使用最近邻法进行联立，对对比列表中特征点n求最小值

求得最小值对应序号为q₀，则n与q₀联立，此时对应误差为

和

记为

和

遍历对比列表，执行该过程，完成特征点识别与匹配。

Images