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

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

An intelligent vehicle location method based on feature point calibration

【技术实现步骤摘要】
一种基于特征点标定的智能车定位方法
本专利技术涉及智能车定位方法领域，具体是一种基于特征点标定的智能车定位方法。
技术介绍
智能车的定位研究是无人驾驶中的热点和难点。在当前的定位方式研究中，由于GNSS信号存在精度不足的问题，常常以GNSS结合其他传感器如里程计、IMU进行融合定位，以取得更好的定位效果。但是当智能车进入隧道、枝叶浓密的林荫道等环境下，GNSS信号质量过差难以提供有效的定位信息，仅仅依赖里程计和IMU进行定位又会存在累积误差和漂移，难以保证定位精度要求。
技术实现思路
本专利技术的目的在于克服上述技术缺陷，提出了一种基于特征点标定的智能车定位方法，在GNSS信号良好时，使用GNSS结合里程计和IMU通过卡尔曼滤波得到当前定位位置，同时使用激光雷达观察周围角点、圆弧等特征进行记录和标定，待数据稳定后转换到全局坐标系下储存；当GNSS信号质量较差时，智能车通过激光雷达观测先前标定点对里程计和IMU推算信息通过粒子滤波过程进行评价和校正，即使在无GNSS信号也也能在较长一段时间内仍保持较为准确的定位效果，保证智能车本文档来自技高网...

【技术保护点】
1.一种基于特征点标定的智能车定位方法，所述方法包括：/n根据GNSS锁星数确定GNSS信号质量，由此判断智能车是位于盲区还是非盲区；/n当智能车位于GNSS非盲区，将GNSS信号、里程计数据和惯性测量单元数据进行组合，通过卡尔曼滤波获取智能车当前位姿，同时利用激光雷达扫描智能车周围环境，提取角点、圆弧特征并通过处理得到角点、圆心作为特征点，计算特征点的全局坐标和权重存入当前特征点列表；/n将当前特征点列表中的特征点加入已有特征点列表，将已有特征点列表中满足条件的特征点加入可信任列表；/n当智能车位于GNSS盲区，使用激光雷达获取周围可信任列表的特征点作为观测量，融合里程计数据和惯性测量单元数...

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


2.根据权利要求1所述的基于特征点标定的智能车定位方法，其特征在于，所述当智能车位于GNSS非盲区，将GNSS信号、里程计数据和惯性测量单元数据进行组合，通过卡尔曼滤波获取智能车当前位姿；具体包括：
步骤1-1)通过智能车左后轮、右后轮的光电编码器，计算智能车位移和航向角变化量，具体过程如下：
计算单位采样时间TS内左后轮和右后轮位置增量ΔdL和ΔdR：






其中，ΔNL和ΔNR分别为左后轮和右后轮采样时间内脉冲增量，D为车轮当量直径，P为光码盘光栅总数，m为编码器减速率；
计算tk-1时刻到tk时刻智能车的位移变化量ΔDk和航向角变化量






其中，W为两后轮轴线长度；
计算tk时刻的速度：



步骤1-2)从IMU中直接得到tk时刻航向角加速度ωk，对每一时刻角加速度ωk进一步积分得到第二航向角变化量航向角
步骤1-3)通过智能车采集到的GNSS信息通过坐标转换得到tk时刻智能车位置
步骤1-4)将上述信息通过卡尔曼滤波器融合；定义智能车tk-1时刻状态为：Xk-1＝[xk-1,yk-1,Vk-1,θk-1,ωk-1]T，运动和观测模型分别为：






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



其中，为tk时刻智能车的预测状态，为观测模型转移函数，为加入噪声的观测量，ck是协方差为Q的系统过程噪声，vk是协方差为R的传感器测量噪声；
基于上述模型，获取智能车当前位姿。


3.根据权利要求2所述的基于特征点标定的智能车定位方法，其特征在于，所述卡尔曼滤波器的预测和更新过程，具体为：
预测状态量：
预测误差协方差矩阵：P(k|k-1)＝APk-1AT+Q
更新最优估计状态量：
更新卡尔曼增益：
更新误差协方差矩阵：Pk＝[I-KkH]P(k|k-1)
其中，是tk时刻的预测位姿状态量，A为状态转移矩阵，是tk-1时刻的最优位姿状态量，Pk-1是tk-1时刻已更新的误差协方差矩阵，Kk是tk时刻的卡尔曼增益，H为测量矩阵，观测量是tk时刻的最优位姿状态量。


4.根据权利要求3所述的基于特征点标定的智能车定位方法，其特征在于，所述，利用激光雷达扫描智能车周围环境，提取角点、圆弧特征并通过处理得到角点、圆心作为特征点，计算特征点的全局坐标和权重存入当前特征点列表；具体包括：
根据预先设计的点云密度阈值，去除激光雷达获取的点云数据中的个别噪声点；然后根据各点间欧几里得距离阈值将点云分割成不同的点云簇；
基于Hough变换机制从点云簇中提取圆弧特征，其对应的圆心作为特征点，将圆心坐标作为该特征点在车体局部坐标系中坐标(xL,yL)，并计算权重；
从点云簇中检测角点作为特征点，将角点坐标作为该特征点在车体局部坐标系中坐标(xL,yL)，并计算权重；
将特征点在车体局部坐标系中坐标(xL,yL)转换为全局坐标系下的坐标(xG,yG)，和权重一起存入当前特征点列表。


5.根据权利要求4所述的基于特征点标定的智能车定位方法，其特征在于，所述基于Hough变换机制从点云簇中提取圆弧特征，其对应的圆心作为特征点，将圆心坐标作为该特征点在车体局部坐标系中坐标(xL,yL)，并计算权重；具体包括：
对某一点云簇，将各点通过Hough变换对某一圆参数进行投票，表示公式为(x-a)2+(y-b)2＝R2；添加约束：a.票数大于阈值；b.合成圆的直径大于阈值；同时满足以上两约束的圆参数作为可观测的圆弧特征；
根据得到的圆弧特征确定圆心坐标和权重：
定义票数权重N为该圆特征得到的票数，圆弧权重其中，α为各扫描点围成圆弧对应圆心角，其公式为α＝arcsin(L/2R)，L为圆弧两端点连接得到的弦长，当圆弧中存在点到这条弦的距离大于R时，说明对应圆心角大于π，此时α＝2π-arcsin(L/2R)；
该圆弧对应的圆心为特征点，在车体局部坐标系中坐标为(xL,yL)；权重为


6.根据权利要求5所述的基于特征点标定的智能车定位方法，其特征在于，所述从点云簇中检测角点作为特征点，将角点坐标作为该特征点在车体局部坐标系中坐标(xL,yL)，并计算权重，具体包括：
在点云簇中检测断点：若一点和下一个点间距离大于断点检测阈值，则认为这两个点均为断点；由断点进一步分割点云簇；
将某一段点云簇Si起点和终点连接得到直线l，计算该点云簇中每个点到直线l的距离，找到距离最大的点Kp，若其到直线l距离dp大于设定阈值dmax，则认为该点为角点，并以该点为分界点将点云簇Si分为两个子点云簇，再分别对两个分割都点云簇执行上述角点判定过程，直到找到所有点云簇中角点；同时，添加两个约束：a.角点到两边直线端点距离大于一定阈值；b....

【专利技术属性】
技术研发人员：张新钰，郭世纯，高文举，刘华平，李骏，
申请(专利权)人：清华大学，
类型：发明
国别省市：北京;11