用于自动驾驶车辆的定位方法、系统及计算机可读介质技术方案

本发明专利技术公开了一种用于自动驾驶车辆的定位方法，主要将NDT模块输出的定位信息、车辆ODOM输出的速度信息在2D空间通过无际卡尔曼滤波算法进行数据融合,其ODOM输出的数据代入预测模型推算车辆位姿信息，并将其与NDT定位信息进行状态更新，将其结果与NDT的其它位姿信息进行组合得出3D信息，最终实现车辆定位。本发明专利技术还提供了一种用于自动驾驶车辆的定位系统及计算机可读介质。本发明专利技术能够输出高频、高精度的定位信息，具有鲁棒性强，通用型广等优点，适用于车辆自动驾驶技术；同时，整个系统的稳定性高。

【技术实现步骤摘要】
用于自动驾驶车辆的定位方法、系统及计算机可读介质
本专利技术属于自动驾驶控制领域，特别涉及一种用于自动驾驶车辆的定位方法、系统及计算机可读介质。
技术介绍
高精度定位是自动驾驶领域的关键技术，目前常用高精度组合导航、多线激光雷达、Camera等传感器结合高精度地图实现车辆定位，主要采用卡尔曼滤波、SLAM等算法。目前主要分成绝对定位和相对定位，其中绝对定位需要依赖价格高昂的GPS和IMU设备，在车辆长期经过隧道等GPS信号、磁场环境不稳定地方，其定位偏差较大。相对定位是在已经构建的先验地图实现车辆定位，主要借助Camera、激光雷达等环境感知传感器检测周围环境，并将其与先验地图进行特征匹配。目前基于激光雷达实现的NDT(NormalDistributionsTransform)定位方法，其精度较高，已经广泛应用于自动驾驶技术。但是在点云稠密的地方，该方法计算量较大，存在系统延迟，自动驾驶对定位的频率要求高，NDT定位频率和雷达输出频率一致，不能满足自动驾驶对定位的频率要求，且存在滞后性，无法满足车辆高速行驶；在点云稀疏地方和因为局部道路环境改变需要更新地图的位置，会存在定位出错情况；而且这个方法对计算平台算力要求较高；鲁棒性较低，一旦计算出错，很难恢复；此外，其需要参考位置作为算法输入。
技术实现思路
专利技术目的：本专利技术针对现有技术存在的问题，提出了一种高频、高精度的用于自动驾驶车辆的定位方法。技术方案：为实现上述目的，本专利技术提供了一种用于自动驾驶车辆的定位方法，包括以下步骤，<br>NDT模块根据激光雷达提供的点云信息获取车辆在点云地图中的位姿信息；其中位姿信息包括观测位姿信息和定位位姿信息，其中观测位姿信息作为观测值无际卡尔曼滤波模块中；车辆速度信息采集模块实时将采集到的车辆速度信息发送到无际卡尔曼滤波模块；无际卡尔曼滤波模块根据接收到的车辆速度信息对车辆的位姿进行预测得到车辆的预测位姿信息；无际卡尔曼滤波模块根据接收到的观测位姿信息与车辆的预测位姿信息对车辆位姿信息进行更新并输出；无际卡尔曼滤波模块输出的车辆位姿信息与NDT模块输出的定位位姿信息融合得到车辆定位信息。其中，所述NDT模块中根据车辆定位信息对NDT模块中获取的观测位姿信息进行时间补偿得到第二观测位姿信息，第二观测位姿信息作为观测值输入到无际卡尔曼滤波模块中，时间补偿的方法为：根据公式：xcps＝xndt+vprecos(yawpre)×(tnow-tndt_time)；ycps＝yndt+vpresin(yawpre)×(tnow-tndt_time)；yawcps＝yawndt+yaw_ratepre×(tnow-tndt_time)；完成时间补偿，其中，第二观测位姿信息包括xcps、ycps、yawcps，xcps、ycps、yawcps分别表示为经过时间补偿后的车辆的横坐标、纵坐标和航向角；观测位姿信息包括xndt、yndt、yawndt，xndt、yndt、yawndt分别表示NDT模块根据激光雷达提供的点云信息获取车辆在点云地图中的横坐标、纵坐标和航向角；vpre为上一时刻车辆定位信息中的车辆速度；yawpre为上一时刻车辆定位信息中的车辆的航向角；yaw_ratepre为上一时刻车辆定位信息中的车辆的角速度；tnow和tndt_time分别表示为当前时间和激光雷达点云输入NDT模块的时间。进一步，根据NDT模块中的迭代次数或者运行系统中中央处理器的占用率来调整NDT模块接收激光雷达发送的点云数据的频率。进一步，设置线速度阈值和角速度阈值，无际卡尔曼滤波模块输出的车辆位姿信息中车辆的线速度阈值和车辆的角速度阈值，当无际卡尔曼滤波模块输出的车辆位姿信息中车辆线速度大于线速度阈值时，则车辆位姿信息中的车辆速度为线速度阈值；当无际卡尔曼滤波模块输出的车辆位姿信息中车辆角速度大于角速度阈值时，则车辆位姿信息中的车辆角速度为角速度阈值。本专利技术还提供了一种用于自动驾驶车辆的定位系统，包括：激光雷达、NDT模块、车辆速度信息采集模块和无际卡尔曼滤波模块；其中，NDT模块根据激光雷达提供的点云信息获取车辆在点云地图中的位姿信息；其中位姿信息包括观测位姿信息和定位位姿信息，其中观测位姿信息作为观测值无际卡尔曼滤波模块中；车辆速度信息采集模块实时将采集到的车辆速度信息发送到无际卡尔曼滤波模块；无际卡尔曼滤波模块根据接收到的车辆速度信息对车辆的位姿进行预测得到车辆的预测位姿信息；无际卡尔曼滤波模块根据接收到的观测位姿信息与车辆的预测位姿信息对车辆位姿信息进行更新并输出；无际卡尔曼滤波模块输出的车辆位姿信息与NDT模块输出的定位位姿信息融合得到车辆定位信息。进一步，还包括频率控制模块，所述频率控制模块根据NDT模块中的迭代次数或者运行系统中中央处理器的占用率来调整NDT模块接收激光雷达发送的点云数据的频率。进一步，NDT模块中还包括时间补偿子模块，所述时间补偿子模块根据车辆定位信息对NDT模块中获取的观测位姿信息进行时间补偿得到第二观测位姿信息，第二观测位姿信息作为观测值输入到无际卡尔曼滤波模块中，其中根据公式：xcps＝xndt+vprecos(yawpre)×(tnow-tndt_time)；ycps＝yndt+vpresin(yawpre)×(tnow-tndt_time)；yawcps＝yawndt+yaw_ratepre×(tnow-tndt_time)；完成时间补偿，其中，第二观测位姿信息包括xcps、ycps、yawcps，xcps、ycps、yawcps分别表示为经过时间补偿后的车辆的横坐标、纵坐标和航向角；观测位姿信息包括xndt、yndt、yawndt，xndt、yndt、yawndt分别表示NDT模块根据激光雷达提供的点云信息获取车辆在点云地图中的横坐标、纵坐标和航向角；vpre为上一时刻车辆定位信息中的车辆速度；yawpre为上一时刻车辆定位信息中的车辆的航向角；yaw_ratepre为上一时刻车辆定位信息中的车辆的角速度；tnow和tndt_time分别表示为当前时间和激光雷达点云输入NDT模块的时间。本专利技术还提供了一种存储软件的计算机可读介质，所述软件包括能通过一个或多个计算机执行的指令，所述指令通过这样的执行使得所述一个或多个计算机执行操作，所述操作包括如权利要求1-4中任意一项所述的用于自动驾驶车辆的定位方法的流程。本专利技术还提供了一种计算机系统，包括：一个或多个处理器；存储器，存储可被操作的指令，所述指令在通过所述一个或多个处理器执行时使得所述一个或多个处理器执行操作，所述操作包括如权利要求1-4中任意一项所述的用于自动驾驶车辆的定位方法的流程。工作原理：本专利技术将NDT模块输出的定位信息、车辆ODOM输出的速度信息在2D空间通过无际卡尔曼滤波(UnscentedKalmanFilter,下文简称UKF)算法进行数据融合,其ODOM输出的数据代入预测模本文档来自技高网...

【技术保护点】
1.一种用于自动驾驶车辆的定位方法，其特征在于：包括以下步骤，/nNDT模块根据激光雷达提供的点云信息获取车辆在点云地图中的位姿信息；其中位姿信息包括观测位姿信息和定位位姿信息，其中观测位姿信息作为观测值无际卡尔曼滤波模块中；/n车辆速度信息采集模块实时将采集到的车辆速度信息发送到无际卡尔曼滤波模块；/n无际卡尔曼滤波模块根据接收到的车辆速度信息对车辆的位姿进行预测得到车辆的预测位姿信息；无际卡尔曼滤波模块根据接收到的观测位姿信息与车辆的预测位姿信息对车辆位姿信息进行更新并输出；无际卡尔曼滤波模块输出的车辆位姿信息与NDT模块输出的定位位姿信息融合得到车辆定位信息。/n

【技术特征摘要】
1.一种用于自动驾驶车辆的定位方法，其特征在于：包括以下步骤，
NDT模块根据激光雷达提供的点云信息获取车辆在点云地图中的位姿信息；其中位姿信息包括观测位姿信息和定位位姿信息，其中观测位姿信息作为观测值无际卡尔曼滤波模块中；
车辆速度信息采集模块实时将采集到的车辆速度信息发送到无际卡尔曼滤波模块；
无际卡尔曼滤波模块根据接收到的车辆速度信息对车辆的位姿进行预测得到车辆的预测位姿信息；无际卡尔曼滤波模块根据接收到的观测位姿信息与车辆的预测位姿信息对车辆位姿信息进行更新并输出；无际卡尔曼滤波模块输出的车辆位姿信息与NDT模块输出的定位位姿信息融合得到车辆定位信息。


2.根据权利要求1所述的用于自动驾驶车辆的定位方法，其特征在于：所述NDT模块中根据车辆定位信息对NDT模块中获取的观测位姿信息进行时间补偿得到第二观测位姿信息，第二观测位姿信息作为观测值输入到无际卡尔曼滤波模块中，时间补偿的方法为：根据公式：
xcps＝xndt+vprecos(yawpre)×(tnow-tndt_time)；
ycps＝yndt+vpresin(yawpre)×(tnow-tndt_time)；
yawcps＝yawndt+yaw_ratepre×(tnow-tndt_time)；
完成时间补偿，其中，第二观测位姿信息包括xcps、ycps、yawcps，xcps、ycps、yawcps分别表示为经过时间补偿后的车辆的横坐标、纵坐标和航向角；观测位姿信息包括xndt、yndt、yawndt，xndt、yndt、yawndt分别表示NDT模块根据激光雷达提供的点云信息获取车辆在点云地图中的横坐标、纵坐标和航向角；vpre为上一时刻车辆定位信息中的车辆速度；yawpre为上一时刻车辆定位信息中的车辆的航向角；yaw_ratepre为上一时刻车辆定位信息中的车辆的角速度；tnow和tndt_time分别表示为当前时间和激光雷达点云输入NDT模块的时间。


3.根据权利要求1所述的用于自动驾驶车辆的定位方法，其特征在于：根据NDT模块中的迭代次数或者运行系统中中央处理器的占用率来调整NDT模块接收激光雷达发送的点云数据的频率。


4.根据权利要求1所述的用于自动驾驶车辆的定位方法，其特征在于：设置线速度阈值和角速度阈值，无际卡尔曼滤波模块输出的车辆位姿信息中车辆的线速度阈值和车辆的角速度阈值，当无际卡尔曼滤波模块输出的车辆位姿信息中车辆线速度大于线速度阈值时，则车辆位姿信息中的车辆速度为线速度阈值；当无际卡尔曼滤波模块输出的车辆位姿信息中车辆角速度大于角速度阈值时，则车辆位姿信息中的车辆角速度为角速度阈值。


5.一种用于自动驾驶车辆的定位系统，其特征在于：包括：激光雷达、NDT模块、车辆速度信息采集模块和无际卡尔...

【专利技术属性】
技术研发人员：李赵，刘心刚，张旸，陈诚，刘洁，
申请(专利权)人：奥特酷智能科技南京有限公司，
类型：发明
国别省市：江苏;32