CN112505718A - 用于自动驾驶车辆的定位方法、系统及计算机可读介质 - Google Patents

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

Info

Publication number
CN112505718A
CN112505718A CN202011246324.5A CN202011246324A CN112505718A CN 112505718 A CN112505718 A CN 112505718A CN 202011246324 A CN202011246324 A CN 202011246324A CN 112505718 A CN112505718 A CN 112505718A
Authority
CN
China
Prior art keywords
vehicle
ndt
pose information
information
module
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202011246324.5A
Other languages
English (en)
Other versions
CN112505718B (zh
Inventor
李赵
刘心刚
张旸
陈诚
刘洁
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
AutoCore Intelligence Technology Nanjing Co Ltd
Original Assignee
AutoCore Intelligence Technology Nanjing Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by AutoCore Intelligence Technology Nanjing Co Ltd filed Critical AutoCore Intelligence Technology Nanjing Co Ltd
Priority to CN202011246324.5A priority Critical patent/CN112505718B/zh
Publication of CN112505718A publication Critical patent/CN112505718A/zh
Application granted granted Critical
Publication of CN112505718B publication Critical patent/CN112505718B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Traffic Control Systems (AREA)
  • Navigation (AREA)

Abstract

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

Description

用于自动驾驶车辆的定位方法、系统及计算机可读介质
技术领域
本发明属于自动驾驶控制领域，特别涉及一种用于自动驾驶车辆的定位方法、系统及计算机可读介质。
背景技术
高精度定位是自动驾驶领域的关键技术，目前常用高精度组合导航、多线激光雷达、Camera等传感器结合高精度地图实现车辆定位，主要采用卡尔曼滤波、SLAM等算法。目前主要分成绝对定位和相对定位，其中绝对定位需要依赖价格高昂的GPS和IMU设备，在车辆长期经过隧道等GPS信号、磁场环境不稳定地方，其定位偏差较大。相对定位是在已经构建的先验地图实现车辆定位，主要借助Camera、激光雷达等环境感知传感器检测周围环境，并将其与先验地图进行特征匹配。目前基于激光雷达实现的NDT(Normal DistributionsTransform)定位方法，其精度较高，已经广泛应用于自动驾驶技术。但是在点云稠密的地方，该方法计算量较大，存在系统延迟，自动驾驶对定位的频率要求高，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模块的时间。
进一步，根据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空间通过无际卡尔曼滤波(Unscented Kalman Filter,下文简称UKF)算法进行数据融合,其ODOM输出的数据代入预测模型推算车辆位姿信息，并将其与经过时间补偿后的NDT定位信息进行状态更新，将其结果与NDT的其它位姿信息进行组合得出3D信息，最终实现车辆定位，此外再把UKF输出的定位信息当做NDT的参考位置输入NDT模块。若在算力不高的嵌入式平台，可根据CPU性能，控制NDT的执行频率，将低频NDT结果输入UKF。由于ODOM的频率一般为50HZ，而NDT的频率为1-10HZ，UKF输出频率为50HZ，葱油能够有效的提高绝对定位的频率。
有益效果：与现有技术相比，本发明能够输出高频、高精度的定位信息，具有鲁棒性强，通用型广等优点，适用于车辆自动驾驶技术；同时，整个系统的稳定性高，本发明根据CPU占用率和NDT的迭代次数动态调整Lidar的输入频率，有效降低了嵌入式平台的计算压力。
附图说明
图1为实施例1提供的方法的模型示意图；
图2为实施例2提供的方法的模型示意图。
具体实施方式
下面将结合本发明实例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1：
如图1所示，本实施例提供了一种用于自动驾驶车辆的定位方法，包括以下步骤：
点云地图和激光雷达采集到的点云信息输入到NDT模块，NDT模块根据激光雷达提供的点云信息获取车辆在点云地图中的位姿信息；其中位姿信息为车辆的三维坐标值(x,y,z)、车辆的航向角，车辆的俯仰角和滚转角；观测位姿信息包括车辆的横坐标x、纵坐标y和车辆的航向角；定位位姿信息包括车辆的高度坐标z、车辆的俯仰角和滚转角；观测位姿信息作为观测值无际卡尔曼滤波模块中。
NDT模块中设置了初始化的判断条件，其中包括，NDT模块输出分数score、计算时间time_cost、迭代次数iterations；NDT模块输出分数score正常值0.5～1.2，计算时间time_cost正常值30ms～80ms，迭代次数iterations正常值1-3。若NDT模块输出分数socore>1.5或者计算时间time_cost>100ms或者迭代次数Iterations>5则重新初始化NDT模块。
还包括频率控制模块，其根据NDT模块中的迭代次数或者NDT模块的中央处理器的占用率来调整NDT模块接收激光雷达发送的点云数据的频率。正常情况，激光雷达lidar发送点云的频率为10HZ，NDT模块的CPU占用率60％；若温度较热，或者其它原因导致NDT模块的CPU占用率达到80％，将lidar点云频率调节至5HZ。NDT模块的CPU占用率达90％时，lidar点云频率调节至2HZ。NDT模块的CPU占用率达95％时，lidar点云频率调节至1HZ。这样能够保障嵌入式平台的稳定性。
车辆速度信息采集模块实时将采集到的车辆速度信息发送到无际卡尔曼滤波模块；本实施中车辆速度信息采集模块为车辆的ODOM传感器；车辆速度信息包括车辆的线速度、角速度和数据采集和发送的时间节点。
无际卡尔曼滤波模块中包括预测子模块和更新子模块。
其中，预测子模块的工作方法为：
步骤1：以车辆当前状态为中心设置2n+1个采样点，其中，n为车辆位姿信息中参数的数量总数，本实施例中车辆位姿信息包括车辆的横坐标，纵坐标，航向角，车辆的角速度和车辆的线速度，所以本实施例中n等于5。
步骤2：根据预测模型分别计算每个采样点的位姿信息中的车辆的坐标和航向角，其中预测模型为：
Figure BDA0002770163280000051
Figure BDA0002770163280000052
Figure BDA0002770163280000053
其中，
Figure BDA0002770163280000054
分别表示预测后t时刻第i个采用的横坐标、纵坐标和航向角，
Figure BDA0002770163280000055
分别表示预测前t-1时刻第i个采用的横坐标、纵坐标和航向角；
Figure BDA0002770163280000061
和
Figure BDA0002770163280000062
的初始值由NDT模块提供，vt和yaw_ratet分别为ODOM传感器输出的车辆的线速度和角速度，tt和tt-1为t和t-1时刻的时间。
步骤3：根据每个采样点的位姿信息和其权重进行加权平均得到车辆预测位姿信息，并将获得的预测位姿信息发送到更新子模块。其中车辆当前状态的中心采样点的权重最大，为
Figure BDA0002770163280000063
其它采样点的权重相等，为
Figure BDA0002770163280000064
λ表示比例因子。
更新子模块根据观测位姿信息和车辆预测位姿信息的置信度计算卡尔曼增益，根据计算得到的卡尔曼增益更新车辆位姿信息，并输出完成车辆的定位。由于车辆的线速度和角速度是车辆运动状态的控制量，该值异常会导致定位结果极不稳定。为保障算法健壮性，本实施例中对输出的车辆位姿信息中车辆的线速度和角速度进行限幅控制，其计算公式如下：
Figure BDA0002770163280000065
其中，vmax为线速度阈值，yaw_ratemax为角速度阈值，本实施例中vmax优选为40
其中，vmax为线速度阈值，
米/秒，yaw_ratemax优选为50度/秒，v为车辆位姿信息中车辆的线速度，yaw_rate为车辆位姿信息中车辆的角速度。
无际卡尔曼滤波模块输出的车辆位姿信息与NDT模块输出的定位位姿信息融合得到车辆定位信息，车辆定位信息包括车辆的三维坐标、航向角，线速度、角速度、车辆的俯仰角和滚转角。
实施例2：
如图2所示，本实施例提供了一种用于自动驾驶车辆的定位方法，包括以下步骤：
点云地图和激光雷达采集到的点云信息输入到NDT模块，NDT模块根据激光雷达提供的点云信息获取车辆在点云地图中的位姿信息；其中位姿信息为车辆的三维坐标值(x,y,z)、车辆的航向角，车辆的俯仰角和滚转角；观测位姿信息包括车辆的横坐标x、纵坐标y和车辆的航向角；定位位姿信息包括车辆的高度坐标z、车辆的俯仰角和滚转角；观测位姿信息作为观测值无际卡尔曼滤波模块中。本实施例中，NDT模块输出的位姿信息为：车辆的横坐标x为1266.65米，车辆的纵坐标y为-814.83米，车辆的高度坐标z为-1.86米，车辆的滚转角roll为0.0090弧度，车辆的俯仰角pitch为0.00099弧度，车辆的航向角yaw:0.2191弧度。
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模块的时间。本实施例中经过时间补偿后得到的第二观测位姿信息为：xcps为1267.20米，ycps为-814.71米，yawcps为0.22弧度。
NDT模块中设置了初始化的判断条件，其中包括，NDT模块输出分数score、计算时间time_cost、迭代次数iterations；NDT模块输出分数score正常值0.5～1.2，计算时间time_cost正常值30ms～80ms，迭代次数iterations正常值1-3。若NDT模块输出分数socore>1.5或者计算时间time_cost>100ms或者迭代次数Iterations>5则重新初始化NDT模块。
还包括频率控制模块，其根据NDT模块中的迭代次数或者NDT模块的中央处理器的占用率来调整NDT模块接收激光雷达发送的点云数据的频率。正常情况，激光雷达lidar发送点云的频率为10HZ，NDT模块的CPU占用率60％；若温度较热，或者其它原因导致NDT模块的CPU占用率达到80％，将lidar点云频率调节至5HZ。NDT模块的CPU占用率达90％时，lidar点云频率调节至2HZ。NDT模块的CPU占用率达95％时，lidar点云频率调节至1HZ。这样能够保障嵌入式平台的稳定性。
车辆速度信息采集模块实时将采集到的车辆速度信息发送到无际卡尔曼滤波模块；本实施中车辆速度信息采集模块为车辆的ODOM传感器；车辆速度信息包括车辆的线速度、角速度和数据采集和发送的时间节点。
无际卡尔曼滤波模块中包括预测子模块和更新子模块。
其中，预测子模块的工作方法为：
步骤1：以车辆当前状态为中心设置2n+1个采样点，其中，n为车辆位姿信息中参数的数量总数，本实施例中车辆位姿信息包括车辆的横坐标，纵坐标，航向角，车辆的角速度和车辆的线速度，所以本实施例中n等于5。
步骤2：根据预测模型分别计算每个采样点的位姿信息中的车辆的坐标和航向角，其中预测模型为：
Figure BDA0002770163280000081
Figure BDA0002770163280000082
Figure BDA0002770163280000083
其中，
Figure BDA0002770163280000084
分别表示预测后t时刻第i个采用的横坐标、纵坐标和航向角，
Figure BDA0002770163280000085
分别表示预测前t-1时刻第i个采用的横坐标、纵坐标和航向角；
Figure BDA0002770163280000086
和
Figure BDA0002770163280000087
的初始值由NDT模块提供，vt和yaw_ratet分别为ODOM传感器输出的车辆的线速度和角速度，tt和tt-1为t和t-1时刻的时间。本实施例中，此刻vt为5.65米/秒，yaw_ratet为0.085弧度/秒。
步骤3：根据每个采样点的位姿信息和其权重进行加权求和得到车辆预测位姿信息，并将获得的预测位姿信息发送到更新子模块。其中车辆当前状态的中心采样点的权重最大，为
Figure BDA0002770163280000088
其它采样点的权重相等，为
Figure BDA0002770163280000089
λ表示比例因子。经过预测后得到车辆的预测位姿信息为：车辆的横坐标为1267.44米，车辆的纵坐标为-814.48米，车辆的航向角为0.22弧度。
更新子模块根据观测位姿信息和车辆预测位姿信息的置信度计算卡尔曼增益，根据计算得到的卡尔曼增益更新车辆位姿信息，并输出。由于车辆的线速度和角速度是车辆运动状态的控制量，该值异常会导致定位结果极不稳定。为保障算法健壮性，本实施例中对输出的车辆位姿信息中车辆的线速度和角速度进行限幅控制，其计算公式如下：
Figure BDA0002770163280000091
其中，vmax为线速度阈值，yaw_ratemax为角速度阈值，本实施例中vmax优选为40米/秒，yaw_ratemax优选为50度/秒，v为车辆位姿信息中车辆的线速度，v为车辆位姿信息中车辆的线速度，yaw_rate为车辆位姿信息中车辆的角速度。本实施例中无际卡尔曼滤波模块输出的车辆位姿信息包括：车辆的横坐标：1267.17米，车辆的纵坐标：-814.54米，车辆的航向角：0.22弧度，车辆的线速度：5.66米/秒，车辆的角速度：0.086弧度/秒。
无际卡尔曼滤波模块输出的车辆位姿信息与NDT模块输出的定位位姿信息融合得到车辆定位信息，车辆定位信息包括车辆的三维坐标、航向角，线速度、角速度、车辆的俯仰角和滚转角。本实施例最后获得的车辆定位信息为：车辆的横坐标：1267.17米，车辆的纵坐标：-814.54米，车辆的高度坐标：-1.86米，车辆的滚转角：0.0090弧度，车辆的俯仰角：0.00099弧度，车辆的航向角：0.22弧度。
本发明还提供了一种存储软件的计算机可读介质，所述软件包括能通过一个或多个计算机执行的指令，所述指令通过这样的执行使得所述一个或多个计算机执行操作，所述操作包括如前述用于自动驾驶车辆的定位流程。
本发明还提供了一种计算机系统，包括：一个或多个处理器；存储器，存储可被操作的指令，所述指令在通过所述一个或多个处理器执行时使得所述一个或多个处理器执行操作，所述操作包括如前述用于自动驾驶车辆的定位方法的流程。
应当理解，本发明的前述用于自动驾驶车辆的定位方法的示例可以在任何包含具有数据存储和数据处理的计算机系统中，前述的计算机系统可以是至少一个包括处理器和存储器的电子处理系统或者电子设备，例如PC电脑，不论是个人用PC电脑、商用PC电脑，或者图形处理用PC电脑、服务器级PC电脑。这些PC电脑通过具有数据接口和/或网络接口，实现有线和/或无线的数据传输，尤其是车辆的各种数据。
在另一些实施例，该计算机系统还可以是服务器，尤其是云服务器，具有数据存储、处理以及网络通讯功能。
作为示例的计算机系统通常包括由系统总线连接的至少一个处理器、存储器和网络接口。网络接口用于与其他设备/系统进行通信。
处理器用于提供系统的计算和控制。
存储器包括非易失性存储器和缓存。
非易失性存储器通常具有海量存储能力，可以存储操作系统以及计算机程序，这些计算机程序可以包括可被操作的指令，这些指令在通过一个或多个处理器执行时使得一个或多个处理器能够执行本发明前述实施例的车辆定位的过程。
在需要或者合理的实现方式中，前述计算机系统，不论是PC设备或者服务器，还可以包括比图示中更多或者更少的部件，或者组合，或者采用不同的硬件、软件等不同部件或者不同的部署方式。

Claims (9)

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模块、车辆速度信息采集模块和无际卡尔曼滤波模块；其中，
NDT模块根据激光雷达提供的点云信息获取车辆在点云地图中的位姿信息；其中位姿信息包括观测位姿信息和定位位姿信息，其中观测位姿信息作为观测值无际卡尔曼滤波模块中；
车辆速度信息采集模块实时将采集到的车辆速度信息发送到无际卡尔曼滤波模块；
无际卡尔曼滤波模块根据接收到的车辆速度信息对车辆的位姿进行预测得到车辆的预测位姿信息；无际卡尔曼滤波模块根据接收到的观测位姿信息与车辆的预测位姿信息对车辆位姿信息进行更新并输出；无际卡尔曼滤波模块输出的车辆位姿信息与NDT模块输出的定位位姿信息融合得到车辆定位信息。
6.根据权利要求5所述的用于自动驾驶车辆的定位系统，其特征在于：还包括频率控制模块，所述频率控制模块根据NDT模块中的迭代次数或者运行系统中中央处理器的占用率来调整NDT模块接收激光雷达发送的点云数据的频率。
7.根据权利要求5所述的用于自动驾驶车辆的定位系统，其特征在于：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模块的时间。
8.一种存储软件的计算机可读介质，其特征在于，所述软件包括能通过一个或多个计算机执行的指令，所述指令通过这样的执行使得所述一个或多个计算机执行操作，所述操作包括如权利要求1-4中任意一项所述的用于自动驾驶车辆的定位方法的流程。
9.一种计算机系统，其特征在于，包括：
一个或多个处理器；
存储器，存储可被操作的指令，所述指令在通过所述一个或多个处理器执行时使得所述一个或多个处理器执行操作，所述操作包括如权利要求1-4中任意一项所述的用于自动驾驶车辆的定位方法的流程。
CN202011246324.5A 2020-11-10 2020-11-10 用于自动驾驶车辆的定位方法、系统及计算机可读介质 Active CN112505718B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011246324.5A CN112505718B (zh) 2020-11-10 2020-11-10 用于自动驾驶车辆的定位方法、系统及计算机可读介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011246324.5A CN112505718B (zh) 2020-11-10 2020-11-10 用于自动驾驶车辆的定位方法、系统及计算机可读介质

Publications (2)

Publication Number Publication Date
CN112505718A true CN112505718A (zh) 2021-03-16
CN112505718B CN112505718B (zh) 2022-03-01

Family

ID=74957099

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011246324.5A Active CN112505718B (zh) 2020-11-10 2020-11-10 用于自动驾驶车辆的定位方法、系统及计算机可读介质

Country Status (1)

Country Link
CN (1) CN112505718B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113895446A (zh) * 2021-12-13 2022-01-07 智道网联科技(北京)有限公司 自动驾驶车辆速度确定方法、装置、设备及存储介质
CN114217555A (zh) * 2021-12-09 2022-03-22 浙江大学 基于数字孪生场景的低延时远程遥控方法和系统

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107369181A (zh) * 2017-06-13 2017-11-21 华南理工大学 基于双处理器结构的点云数据采集及处理方法
WO2018128669A1 (en) * 2017-01-04 2018-07-12 Qualcomm Incorporated Systems and methods for using a sliding window of global positioning epochs in visual-inertial odometry
CN110132304A (zh) * 2019-04-26 2019-08-16 苏州感测通信息科技有限公司 一种车载数据采集装置及数据采集方法
CN110631593A (zh) * 2019-11-25 2019-12-31 奥特酷智能科技(南京)有限公司 一种用于自动驾驶场景的多传感器融合定位方法
JP2020038362A (ja) * 2018-08-31 2020-03-12 株式会社デンソー 車両側装置、サーバ、方法および記憶媒体
CN110906923A (zh) * 2019-11-28 2020-03-24 重庆长安汽车股份有限公司 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆
CN110926483A (zh) * 2019-11-25 2020-03-27 奥特酷智能科技(南京)有限公司 一种用于自动驾驶的低成本传感器组合定位系统及方法
CN110940344A (zh) * 2019-11-25 2020-03-31 奥特酷智能科技(南京)有限公司 一种用于自动驾驶的低成本传感器组合定位方法
WO2020132305A1 (en) * 2018-12-19 2020-06-25 Zoox, Inc. Safe system operation using latency determinations and cpu usage determinations
CN111780756A (zh) * 2020-07-20 2020-10-16 北京百度网讯科技有限公司 车辆航位推算方法、装置、设备以及存储介质

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018128669A1 (en) * 2017-01-04 2018-07-12 Qualcomm Incorporated Systems and methods for using a sliding window of global positioning epochs in visual-inertial odometry
CN110100190A (zh) * 2017-01-04 2019-08-06 高通股份有限公司 用于在视觉惯性测距中使用全球定位历元的滑动窗口的系统和方法
CN107369181A (zh) * 2017-06-13 2017-11-21 华南理工大学 基于双处理器结构的点云数据采集及处理方法
JP2020038362A (ja) * 2018-08-31 2020-03-12 株式会社デンソー 車両側装置、サーバ、方法および記憶媒体
WO2020132305A1 (en) * 2018-12-19 2020-06-25 Zoox, Inc. Safe system operation using latency determinations and cpu usage determinations
CN110132304A (zh) * 2019-04-26 2019-08-16 苏州感测通信息科技有限公司 一种车载数据采集装置及数据采集方法
CN110631593A (zh) * 2019-11-25 2019-12-31 奥特酷智能科技(南京)有限公司 一种用于自动驾驶场景的多传感器融合定位方法
CN110926483A (zh) * 2019-11-25 2020-03-27 奥特酷智能科技(南京)有限公司 一种用于自动驾驶的低成本传感器组合定位系统及方法
CN110940344A (zh) * 2019-11-25 2020-03-31 奥特酷智能科技(南京)有限公司 一种用于自动驾驶的低成本传感器组合定位方法
CN110906923A (zh) * 2019-11-28 2020-03-24 重庆长安汽车股份有限公司 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆
CN111780756A (zh) * 2020-07-20 2020-10-16 北京百度网讯科技有限公司 车辆航位推算方法、装置、设备以及存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
DOSSAY ORYSPAYEV 等: "LiDAR data reduction using vertex decimation and processing with GPGPU and multicore CPU technology", 《COMPUTERS & GEOSCIENCES》 *
刘汝卿 等: "激光雷达实时数据传输存储系统设计与实现", 《电子测量技术》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114217555A (zh) * 2021-12-09 2022-03-22 浙江大学 基于数字孪生场景的低延时远程遥控方法和系统
CN113895446A (zh) * 2021-12-13 2022-01-07 智道网联科技(北京)有限公司 自动驾驶车辆速度确定方法、装置、设备及存储介质
CN113895446B (zh) * 2021-12-13 2022-02-22 智道网联科技(北京)有限公司 自动驾驶车辆速度确定方法、装置、设备及存储介质

Also Published As

Publication number Publication date
CN112505718B (zh) 2022-03-01

Similar Documents

Publication Publication Date Title
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN108759838A (zh) 基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法
CN111721289A (zh) 车辆定位方法、装置、设备、存储介质及车辆
CN108645413A (zh) 一种移动机器人的同时定位与地图创建的动态纠正方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN111288989B (zh) 一种小型无人机视觉定位方法
CN112505718B (zh) 用于自动驾驶车辆的定位方法、系统及计算机可读介质
Petrich et al. On-board wind speed estimation for uavs
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN112985391B (zh) 一种基于惯性和双目视觉的多无人机协同导航方法和装置
CN108534783A (zh) 一种基于北斗导航技术的飞行器导航方法
CN112229405A (zh) 一种基于图像跟踪与激光测距的无人机目标运动估计方法
Suwandi et al. Low-cost IMU and GPS fusion strategy for apron vehicle positioning
CN115855048A (zh) 一种多传感器融合位姿估计方法
CN113218389B (zh) 一种车辆定位方法、装置、存储介质及计算机程序产品
CN113076634B (zh) 一种多机协同无源定位方法、装置及系统
CN113359167A (zh) 一种通过惯性测量参数将gps与激光雷达融合定位的方法
CN111708008B (zh) 一种基于imu和tof的水下机器人单信标导航方法
CN116222541A (zh) 利用因子图的智能多源组合导航方法及装置
CN116482735A (zh) 一种受限空间内外高精度定位方法
CN114088104B (zh) 一种自动驾驶场景下的地图生成方法
CN114723902A (zh) 基于概率膜计算的煤矿井下无人直升机slam
CN113483769A (zh) 基于粒子滤波器的车辆自定位方法、系统、设备和介质
CN114660641A (zh) 一种自适应gps融合定位系统、方法及介质
Čvek et al. Impact of Different Noise Distributions in the Application of Kalman Filter in Sensor Fusion

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CP02 Change in the address of a patent holder
CP02 Change in the address of a patent holder

Address after: 210012 room 401-404, building 5, chuqiaocheng, No. 57, Andemen street, Yuhuatai District, Nanjing, Jiangsu Province

Patentee after: AUTOCORE INTELLIGENT TECHNOLOGY (NANJING) Co.,Ltd.

Address before: 211800 building 12-289, 29 buyue Road, Qiaolin street, Pukou District, Nanjing City, Jiangsu Province

Patentee before: AUTOCORE INTELLIGENT TECHNOLOGY (NANJING) Co.,Ltd.

CP03 Change of name, title or address
CP03 Change of name, title or address

Address after: 12th Floor, Building 5, Jieyuan Financial City, No. 55 Andemen Street, Yuhuatai District, Nanjing City, Jiangsu Province, China 210012

Patentee after: AUTOCORE INTELLIGENT TECHNOLOGY (NANJING) Co.,Ltd.

Country or region after: China

Address before: 210012 room 401-404, building 5, chuqiaocheng, No. 57, Andemen street, Yuhuatai District, Nanjing, Jiangsu Province

Patentee before: AUTOCORE INTELLIGENT TECHNOLOGY (NANJING) Co.,Ltd.

Country or region before: China