CN110888125B - 一种基于毫米波雷达的自动驾驶车辆定位方法 - Google Patents

一种基于毫米波雷达的自动驾驶车辆定位方法 Download PDF

Info

Publication number
CN110888125B
CN110888125B CN201911237088.8A CN201911237088A CN110888125B CN 110888125 B CN110888125 B CN 110888125B CN 201911237088 A CN201911237088 A CN 201911237088A CN 110888125 B CN110888125 B CN 110888125B
Authority
CN
China
Prior art keywords
grid
sub
particle
gps
vehicle
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.)
Active
Application number
CN201911237088.8A
Other languages
English (en)
Other versions
CN110888125A (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 CN201911237088.8A priority Critical patent/CN110888125B/zh
Publication of CN110888125A publication Critical patent/CN110888125A/zh
Application granted granted Critical
Publication of CN110888125B publication Critical patent/CN110888125B/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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement

Landscapes

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

Abstract

本发明公开了一种基于毫米波雷达的自动驾驶车辆定位方法，主要基于毫米波雷达、GPS等传感器反馈的车辆信息，结合改进后的粒子滤波算法实现车辆车道级定位。本发明将车辆ODOM中的车辆的线速度和角速度输入滤波算法的预测模型，将毫米波雷达预处理后的点云数据和已经构建的占据栅格地图进行数据匹配，计算新粒子权重。此外为提高定位算法的鲁棒性，本专利对传统算法进行改进，根据粒子的权重和的不同计算定位结果以及相应的置信度；在重采样阶段，在粒子群中加入部分根据GPS位置信息计算出的新粒子。本发明在能够保证车辆定位精度的的同时有效降低了整个定位***的成本，更利于自动驾驶技术的普及。

Description

一种基于毫米波雷达的自动驾驶车辆定位方法
技术领域
本发明属于自动驾驶领域，特别涉及一种基于毫米波雷达的自动驾驶车辆定位方法。
背景技术
高精度定位是自动驾驶领域的的重要研究课题，目前常用高精度组合导航、多线激光雷达、Camera等传感器结合高精度地图实现车道级定位，主要采用卡尔曼滤波、粒子滤波、SLAM等算法。其中常规的卡尔曼滤波算法需要价格昂贵的高精度RTK和IMU，在高架桥、隧道等GPS信号不稳定地方，其定位精度较低；SLAM算法在车辆快速运行的高速公路上鲁棒性较低，此外其需要高性能的计算平台。
相比Camera，毫米波雷达能够获取被测物体的位置信息；相比多线激光雷达，毫米波雷达具有价格低廉，安装方便等优势。基于Camera的定位方法目前主要是视觉里程计，其需要IMU补偿修正图像帧之间的相对运动，该方法在夜间无法工作，鲁棒性较低，适用于低速场景。基于多线激光雷达的定位方位目前主要是NDT，其需要借助外部信息提供初始化位置，此外其计算算力较高，对高精度地图的建图精度也有较高的要求。普通的粒子滤波算法直接对所有粒子加权求和，输出单一的定位数据；此外，在结构化道路中，因环境信息的相似性，粒子群容易分散，算法的鲁棒性较低。
发明内容
发明目的：本发明针对现有技术存在的问题，提出了一种基于毫米波雷达的自动驾驶车辆定位方法。
技术方案：为实现上述目的，本发明提供了一种基于毫米波雷达的自动驾驶车辆定位方法，包括以下步骤：
步骤1：设置在车辆上的毫米波雷达、GPS和ODOM传感器实时采集车辆的信息；
步骤2：对毫米波雷达采集到的点云数据进行预处理生成局部栅格图；
步骤3：生成粒子群，将上一时刻粒子群中每个粒子的状态输入到恒定转率和速度模型中，计算粒子群中每个粒子当前时刻的状态；
步骤4：根据公式
Figure GDA0002363188780000011
计算每个粒子的权重；其中wi表示第i个粒子的权重，n表示预处理后状态为占据状态的子栅格的总数，dk表示栅格局部图中第k个占据状态的子栅格在先验占据栅格地图中的最短距离，σ表示先验占据栅格地图的精度，η表示毫米波雷达的调整系数；
步骤5：计算所有粒子的权重和，若粒子的总权重和小于最小权重和时候，车辆位姿状态为所有粒子状态的平均值；否则，根据所有粒子权重加权求和计算车辆的状态；并根据粒子的权重计算当前定位结果的置信度；
步骤6：生成新的粒子群，根据新的粒子群重复步骤3～5。
进一步，所述步骤1中的毫米雷达有3个分别设置在车头和车尾，其中车头设置一个，车尾设置两个，三个毫米波雷达之间的夹角均为120度。
进一步，所述步骤2中队毫米波雷达采集到的点云数据进行预处理并生成局部栅格图的方法包括以下步骤：
步骤201：剔除车辆正前后方安全距离内雷达散射截面积大于第一阈值的点云数据。
步骤201：将剩余的点云数据映射到一个平面空间中，将该平面空间平均分成多个栅格；
步骤203：统计每个栅格中的点云数量，若小于第二阈值，标记该栅格为0，否则，标记该栅格为1；
步骤204：分别在每个栅格平均划分成多个子栅格，子栅格中的标记与所在的栅格的标记一致；从而得到局部栅格图。这样可以有效的解决毫米波雷达输出点云数量少，不稳定、易跳变等数据采集不稳定的问题。
进一步，所述步骤4中栅格局部图中第k个占据状态的子栅格在先验占据栅格地图中的最短距离dk的获得方法为：
步骤401：设最大距离阈值为d_max，以先验占据栅格地图中第l个占据子栅格为中心，计算横向、纵向为d_max范围内第m行n列的未占据子栅格到第l个占据子栅格的距离，记为
Figure GDA0002363188780000021
步骤402：以先验占据栅格地图中第l+1个占据子栅格为中心，计算横向，纵向为d_max范围内第m行n列的未占据子栅格到第l+1个占据子栅格的距离，记为
Figure GDA0002363188780000022
步骤403：若
Figure GDA0002363188780000023
小于
Figure GDA0002363188780000024
先验占据栅格地图中m行n列的未占据子栅格的最小距离记
Figure GDA0002363188780000025
否则仍是
Figure GDA0002363188780000026
步骤404：按照步骤401-403遍历先验占据栅格地图中所有占据子栅格，可获得在先验占据栅格地图中未占据态子栅格到先验占据栅格地图中占据态子栅格的最小距离，其中处于占据态子栅格的最小距离为0；
步骤405：将局部栅格图覆盖到先验占据栅格地图上，根据局部栅格图中占据子栅格在先验占据栅格地图上所以对应的子栅格的状态，得到先验占据栅格地图上对应的子栅格的最小距离，这个即为第k个雷达数据子栅格在先验占据栅格地图中的最短距离dk。这样能够有效提高整个定位方法的效率，在能够保证定位定位精度的同时有效降低了对计算平台的要求。
进一步，所述步骤6中生成新的粒子群的方法为：每个粒子按照权重从大到小进行排序，其中排名前10％的粒子，每个复制3份；排名10％-20％的粒子，每个复制2份；排名20％-60％的粒子，每个复制1份，此外根据GPS的位置信息加入总数10％的新粒子；其中新加入的GPS粒子呈椭圆形，椭圆的长半径与道路垂直；加入的总粒子的10％的新粒子的位置计算公式为：
Figure GDA0002363188780000031
Figure GDA0002363188780000032
公式中
Figure GDA0002363188780000033
和
Figure GDA0002363188780000034
分别表示根据GPS生成的第j个粒子的横坐标和纵坐标，xgps和ygps为当前GPS输出车辆的横坐标和纵坐标，hroad为道路的朝向，
Figure GDA0002363188780000035
和
Figure GDA0002363188780000036
分别为第j个粒子根据高斯分布生成的椭圆形的短半径和长半径，其中j表示根据GPS生成的新粒子的标号。这样能够有效的防止粒子退化，提高定位的鲁棒性。
工作原理：本发明基于毫米波雷达、GPS等传感器反馈的车辆信息，结合改进后的粒子滤波算法实现车辆车道级定位。本发明将车辆ODOM中的车辆的线速度和角速度输入滤波算法的预测模型，将毫米波雷达预处理后的点云数据和已经构建的占据栅格地图进行数据匹配，计算新粒子权重。此外为提高定位算法的鲁棒性，本专利对传统算法进行改进，根据粒子的权重和的不同计算定位结果以及相应的置信度；在重采样阶段，在粒子群中加入部分根据GPS位置信息计算出的新粒子。
有益效果：与现有技术相比，本发明具有以下优点：
1、本发明采用的毫米波雷达和GPS均为低成本传感设备，具有明显价格优势，有利于自动驾驶技术普及。
2、本发明采用的定位算法算力较低，无需高昂计算平台。
3、对毫米波雷达采集的数据进行预处理可以有效的解决毫米波雷达输出点云数量少，不稳定、易跳变等数据采集不稳定的问题。
4、本发明采用改进常规粒子滤波算法，在重采样阶段根据GPS的位置加入椭圆形粒子点，同时算法输出与定位结果相对应的置信度，方便其它模块使用。有效的增加了鲁棒性。
5、在重采样阶段根据GPS信息增加新的粒子能够有效的防止粒子退化，提高定位的鲁棒性。
附图说明
图1为本发明中毫米波雷达在车辆上的设置示意图。
图2为本发明的工作流程图。
图3是本发明定位方法进行车辆定位效果图。
具体实施方式
下面将结合本发明实例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明提供的车辆定位方法中，对于车辆参数的采集主要采用毫米波雷达、GPS和ODOM传感器。其中，毫米波雷达采用的型号为：ARS408-21；GPS采用的型号为：GN200B；ODOM是车辆通过can总线发出的线速度和角速度信息。结合考虑毫米波雷达水平视场角为120°和便于检测两侧的道路边缘，如高度公路上护栏，城市道路路牙石、绿化带等原因。如图1所示，本发明中采用三个毫米波雷达：车头一个，车尾两个，三个雷达之间夹角成120°；通常情况车头车尾各一个毫米波雷达；在车辆较少的结构化道路中只安装车头一个毫米波雷达。
本实施例公开了一种基于毫米波雷达的自动驾驶车辆定位方法，主要包括以下步骤：
步骤1：毫米波雷达采集到的点云数据进行预处理生成局部栅格图；其中预处理主要包括以下步骤：
步骤101：剔除车辆正前后方DIS_SAFT米内雷达散射截面积RCS(雷达散射截面积Radar Cross-Section)大于雷达散射截面积阈值RCS_H的点云数据。其中，DIS_SAFT表示与前方和后方车辆的安全距离，正常情况该值的选取与车辆速度相关，速度越大，安全距离越大，速度越小，安全距离越小。其中，RCS_H的取值范围为20-40dBms。
步骤102：将剩余的点云数据映射到长为M_REC，宽为N_REC的平面空间中，该平面空间中每个栅格标记为GRID_BIG，每个栅格的长为M_GRID，宽为N_GRID。其中，M_REC表示当前需要感知的环境的局部矩形平面的长度，M_REC的优选范围为60～80米。N_REC表示当前需要感知的环境的局部矩形平面的宽度，N_REC的优选范围为10～30米。M_GRID表示栅格的长度，M_GRID的优选范围为0.75-1.25米。N_GRID表示栅格的宽度，N_GRID的优选范围为0.25-0.5米。N_GRID为M_GRID的整数倍，这样可以检测出道路两旁的绿化带、栏杆、栅栏等呈长条形的物体。M_REC为M_GRID的整数倍，N_REC为N_GRID的整数倍。
步骤103：统计每个栅格GRID_BIG中的点云数量，若小于NUM_MAX，标记该栅格为0，即表示该栅格为未占据状态；否则，标记该栅格为1，即表示该栅格为占据状态。其中，NUM_MAX表示每个栅格GRID_BIG中含有的点云个数的阈值。NUM_MAX的优选范围为3～5个。
步骤104：在栅格GRID_BIG中，划分子栅格标记为GRID_SML，若栅格GRID_BIG为0，其中的子栅格GRID_SML全为0，即表示子栅格GRID_SM为未占据状态；若栅格GRID_BIG为1，其中的子栅格GRID_SML全为1，即表示子栅格GRID_SM为占据状态。从而得到局部栅格图，局部栅格图中子栅格的尺寸与先验占据栅格地图中栅格尺寸一致。
步骤2：在先验占据栅格地图上以采集到的车辆所在的经纬度为圆心，GPS定位偏差λ为半径做圆；并在圆内根据高斯分布设置粒子群；根据车辆ODOM输出的线速度和角速度，和上一时刻粒子群中每个粒子的状态，计算粒子群中每个粒子当前时刻的状态。其中每个粒子的状态包括每个粒子的位置坐标，速度信息和航向信息；
其中，计算粒子群中每个粒子当前时刻的状态主要包括以下步骤：将车辆ODOM(里程计)输出的速度信息输入到CTRV(恒定转率和速度模型)运行模型中，CTRV运行模型输出每个粒子的状态信息，CTRV运行模型为：
Figure GDA0002363188780000051
Figure GDA0002363188780000052
Figure GDA0002363188780000053
式中，
Figure GDA0002363188780000061
表示第i个粒子在t时刻的航向角，
Figure GDA0002363188780000062
表示第i个粒子在t-1时刻的航向角，
Figure GDA0002363188780000063
表示在t-1时刻IMU输出的车辆的航向角的角速度加入高斯噪声的值，Δt表示时间差，
Figure GDA0002363188780000064
表示第i个粒子在t时刻在UTM坐标系下的横坐标，
Figure GDA0002363188780000065
表示第i个粒子在t-1时刻在UTM坐标系下的横坐标，vt表示在t时刻车辆ODOM(里程计)输出的车辆的速度加入高斯噪声后的值，
Figure GDA0002363188780000066
表示第i个粒子在t时刻在UTM坐标系下的纵坐标，
Figure GDA0002363188780000067
表示第i个粒子在t-1时刻在UTM坐标系下的纵坐标。
步骤3：根据公式
Figure GDA0002363188780000068
计算每个粒子的权重；其中wi表示第i个粒子的权重，n表示预处理后状态为占据状态的子栅格的总数，dk表示局部栅格图中第k个占据状态的子栅格在先验占据栅格地图中的最短距离，σ表示先验占据栅格地图的精度，η表示毫米波雷达的调整系数。其中，栅格局部图中第k个占据状态的子栅格在先验占据栅格地图中的最短距离dk的计算方法为：
步骤301：设最大距离阈值为d_max，以先验占据栅格地图中第l个占据子栅格为中心，计算横向、纵向为d_max范围内第m行n列的未占据子栅格到第l个占据子栅格的距离，记为
Figure GDA0002363188780000069
步骤302：以先验占据栅格地图中第l+1个占据子栅格为中心，再次计算横向，纵向为d_max范围内第m行n列的未占据子栅格到第l+1个占据子栅格的距离，记为
Figure GDA00023631887800000610
步骤303：若
Figure GDA00023631887800000611
小于
Figure GDA00023631887800000612
先验占据栅格地图中m行n列的未占据子栅格的最小距离记
Figure GDA00023631887800000613
否则仍是
Figure GDA00023631887800000614
步骤304：按照步骤301-303遍历先验占据栅格地图中所有占据子栅格，可获得在先验占据栅格地图中未占据态子栅格到先验占据栅格地图中占据态子栅格的最小距离，其中处于占据态子栅格的最小距离为0。其中d_max设置3-5米。
步骤305：将步骤1生成的局部栅格图覆盖到先验占据栅格地图上，根据局部栅格图中占据子栅格在先验占据栅格地图上所以对应的子栅格的状态，可以得到该子栅格的最小距离，即可知第k个雷达数据子栅格在先验占据栅格地图中的最短距离dk。
步骤4：计算所有粒子的权重和，若粒子的总权重和小于最小权重和W_MIN时候，车辆位姿状态为所有粒子状态的平均值；否则，根据所有粒子权重加权求和计算车辆的状态；并根据粒子的权重计算当前定位结果的置信度。计算公式分别如下：
Figure GDA0002363188780000071
Figure GDA0002363188780000072
Figure GDA0002363188780000073
其中X表示算法输出的车辆位姿状态向量，其中X中包括车辆的位置和航向角，B表示输出位姿状态的置信度，m为粒子总数，Xi为第i个粒子的位姿状态，wi为第i个粒子的权重，wsum为所有粒子的权重和，wfit为预处理毫米波雷达后的所有子栅格和占据栅格地图中栅格位置完全匹配(即步骤3中dk＝0)时的单个粒子的权重，m为总粒子数。最小权重和W_MIN的优选范围为1.5～2.5。
步骤5：在重采样阶段，生成新的粒子群，并根据新的粒子群重复步骤2～4。每个粒子按照权重从大到小进行排序，其中排名前10％的粒子，每个复制3份；排名10％-20％的粒子，每个复制2份；排名20％-60％的粒子，每个复制1份，此外根据GPS的位置信息加入总数10％的新粒子组成新的粒子群。其中新加入的GPS粒子呈椭圆形，椭圆的长半径与道路垂直；加入的总粒子的10％的新粒子的位置计算公式为：
Figure GDA0002363188780000074
Figure GDA0002363188780000075
公式中
Figure GDA0002363188780000076
和
Figure GDA0002363188780000077
分别表示根据GPS生成的第j个粒子的横坐标和纵坐标，xgps和ygps为当前GPS输出车辆的横坐标和纵坐标，hroad为道路的朝向，
Figure GDA0002363188780000078
和
Figure GDA0002363188780000079
分别为第j个粒子根据高斯分布生成的椭圆形的短半径和长半径，其中j表示根据GPS生成的新粒子的标号。
上述步骤中，步骤3中权重计算方法主要约束算法的横向定效果，在结构化道路上，因道路结构的相似性，粒子群容易在纵向位置发散；根据步骤5中GPS位置信息加入的椭圆形粒子主要为提高该算法的纵向定位效果，新加入呈椭圆形的粒群，椭圆的长轴为横向，短轴为纵向，这两部分相结合能够提高算法的定位精度。步骤4中根据粒子权重计算车辆的位姿状态，同时求出其置信度，可解决道路两侧出现道路边缘不完整而导致的定位抖动现象，具有较强的鲁棒性，为其它模块提供丰富的数据借口。
采用本发明提供的定位方法进行车辆定位效果图如图3所示，图中白色点为粒子群，灰色箭头为定位轨迹，灰色点为先验占据栅格地图中的占据子栅格，黑色点为局部栅格图中的占据子栅格。本发明中部分数据结果如表1所示。
表1：
Figure GDA0002363188780000081
从表1中不难看出，本发明提供的定位方法的定位精度为车道级别，满足自动驾驶定位精度。本发明提供的定位方法不受天气、环境的影响，运行于隧道、桥洞等被遮挡地方，鲁棒性强。

Claims (5)

1.一种基于毫米波雷达的自动驾驶车辆定位方法，其特征在于包括以下步骤：
步骤1，设置在车辆上的毫米波雷达、GPS和ODOM传感器实时采集车辆的信息；
步骤2，对毫米波雷达采集到的点云数据进行预处理生成局部栅格图；
步骤3，生成粒子群，将上一时刻粒子群中每个粒子的状态输入到恒定转率和速度模型中，计算粒子群中每个粒子当前时刻的状态；
步骤4，根据公式
Figure DEST_PATH_IMAGE001
计算每个粒子的权重；其中w i 表示第i个粒子的权重，n表示预处理后状态为占据状态的子栅格的总数，d k 表示栅格局部图中第k个占据状态的子栅格在先验占据栅格地图中的最短距离，σ表示先验占据栅格地图的精度，η表示毫米波雷达的调整系数；
步骤5，计算所有粒子的权重和，若粒子的总权重和小于最小权重和时，车辆位姿状态为所有粒子状态的平均值；否则，根据所有粒子权重加权求和计算车辆的状态；并根据粒子的权重计算当前定位结果的置信度；
步骤6，生成新的粒子群，根据新的粒子群重复步骤3～5。
2.根据权利要求1所述的基于毫米波雷达的自动驾驶车辆定位方法，其特征在于：所述步骤1中的毫米雷达有三个，分别设置在车头和车尾，其中车头设置一个，车尾设置两个，三个毫米波雷达之间的夹角均为120度。
3.根据权利要求1所述的基于毫米波雷达的自动驾驶车辆定位方法，其特征在于：所述步骤2中对毫米波雷达采集到的点云数据进行预处理并生成局部栅格图的方法包括以下步骤：
步骤201，剔除车辆正前后方安全距离内雷达散射截面积大于第一阈值的点云数据；
步骤202，将剩余的点云数据映射到一个平面空间中，将该平面空间平均分成多个栅格；
步骤203，统计每个栅格中的点云数量，若小于第二阈值，标记该栅格为0，否则，标记该栅格为1；
步骤204，分别在每个栅格平均划分成多个子栅格，子栅格中的标记与所在的栅格的标记一致；从而得到局部栅格图。
4.根据权利要求1所述的基于毫米波雷达的自动驾驶车辆定位方法，其特征在于：所述步骤4中栅格局部图中第k个占据状态的子栅格在先验占据栅格地图中的最短距离d k 的获得方法为：
步骤401，设最大距离阈值为d_max，以先验占据栅格地图中第l个占据子栅格为中心，计算横向、纵向为d_max范围内第m行n列的未占据子栅格到第l个占据子栅格的距离，记为d l mn ；
步骤402，以先验占据栅格地图中第l+1个占据子栅格为中心，计算横向，纵向为d_max范围内第m行n列的未占据子栅格到第l+1个占据子栅格的距离，记为d l+1 mn ；
步骤403，若d l+1 mn 小于d l mn ，先验占据栅格地图中m行n列的未占据子栅格的最小距离记d l+1 mn ，否则仍是d l mn
步骤404，按照步骤401-403遍历先验占据栅格地图中所有占据子栅格，可获得在先验占据栅格地图中未占据态子栅格到先验占据栅格地图中占据态子栅格的最小距离，其中处于占据态子栅格的最小距离为0；
步骤405，将局部栅格图覆盖到先验占据栅格地图上，根据局部栅格图中占据子栅格在先验占据栅格地图上所以对应的子栅格的状态，得到先验占据栅格地图上对应的子栅格的最小距离，这个即为第k个雷达数据子栅格在先验占据栅格地图中的最短距离d k 。
5.根据权利要求1所述的基于毫米波雷达的自动驾驶车辆定位方法，其特征在于：所述步骤6中生成新的粒子群的方法为：每个粒子按照权重从大到小进行排序，其中排名前10％的粒子，每个复制3份；排名10％-20％的粒子，每个复制2份；排名20％-60％的粒子，每个复制1份，此外根据GPS的位置信息加入总数10％的新粒子形成新的粒子群；其中新加入的GPS粒子呈椭圆形，椭圆的长半径与道路垂直；加入的总粒子的10％的新粒子的位置计算公式为：
x j gps = x gps + r j sht *cos(-h road )+ r j lon *sin(-h road )
y j gps = y gps + r j lon *cos(-h road )- r j sht *sin(-h road )
公式中x j gps 和y j gps 分别表示根据GPS生成的第j个粒子的横坐标和纵坐标，x gps 和y gps 为当前GPS输出车辆的横坐标和纵坐标，h road 为道路的朝向r j sht 和r j lon 分别为第j个粒子根据高斯分布生成的椭圆形的短半径和长半径，其中j表示根据GPS生成的新粒子的标号。
CN201911237088.8A 2019-12-05 2019-12-05 一种基于毫米波雷达的自动驾驶车辆定位方法 Active CN110888125B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911237088.8A CN110888125B (zh) 2019-12-05 2019-12-05 一种基于毫米波雷达的自动驾驶车辆定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911237088.8A CN110888125B (zh) 2019-12-05 2019-12-05 一种基于毫米波雷达的自动驾驶车辆定位方法

Publications (2)

Publication Number Publication Date
CN110888125A CN110888125A (zh) 2020-03-17
CN110888125B true CN110888125B (zh) 2020-06-19

Family

ID=69750662

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911237088.8A Active CN110888125B (zh) 2019-12-05 2019-12-05 一种基于毫米波雷达的自动驾驶车辆定位方法

Country Status (1)

Country Link
CN (1) CN110888125B (zh)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021184320A1 (zh) * 2020-03-19 2021-09-23 华为技术有限公司 车辆定位方法和装置
CN113535863B (zh) * 2020-04-13 2022-06-14 阿里巴巴集团控股有限公司 一种移动轨迹渲染方法、设备及存储介质
CN111487918B (zh) * 2020-04-26 2021-08-20 天地科技股份有限公司 基于多线激光雷达的散料无人装车控制方法、***和装置
CN111562570A (zh) * 2020-04-30 2020-08-21 南京理工大学 基于毫米波雷达的面向自动驾驶的车辆感知方法
CN112034445B (zh) * 2020-08-17 2022-04-12 东南大学 基于毫米波雷达的车辆运动轨迹跟踪方法和***
CN112731450B (zh) * 2020-08-19 2023-06-30 深圳市速腾聚创科技有限公司 点云的运动补偿方法、装置和***
CN112201040B (zh) * 2020-09-29 2022-12-16 同济大学 一种基于毫米波雷达数据的交通数据清洗方法及***
CN112698345B (zh) * 2020-12-04 2024-01-30 江苏科技大学 一种激光雷达的机器人同时定位与建图优化方法
CN112762928B (zh) * 2020-12-23 2022-07-15 重庆邮电大学 含有激光slam的odom与dm地标组合移动机器人及导航方法
CN112505671B (zh) * 2021-02-07 2021-05-25 北方工业大学 Gnss信号缺失环境下毫米波雷达目标定位方法及装置
CN113419235A (zh) * 2021-05-28 2021-09-21 同济大学 一种基于毫米波雷达的无人机定位方法
CN114199251B (zh) * 2021-12-03 2023-09-15 江苏集萃智能制造技术研究所有限公司 一种机器人的防碰撞定位方法
CN115222767B (zh) * 2022-04-12 2024-01-23 广州汽车集团股份有限公司 一种基于空间车位的跟踪方法及***

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105865449A (zh) * 2016-04-01 2016-08-17 深圳杉川科技有限公司 基于激光和视觉的移动机器人的混合定位方法
US9721181B2 (en) * 2015-12-07 2017-08-01 The Climate Corporation Cloud detection on remote sensing imagery
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN109682382A (zh) * 2019-02-28 2019-04-26 电子科技大学 基于自适应蒙特卡洛和特征匹配的全局融合定位方法
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及***

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3239729A1 (en) * 2016-04-25 2017-11-01 Viavi Solutions UK Limited Sensor-based geolocation of a user device

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9721181B2 (en) * 2015-12-07 2017-08-01 The Climate Corporation Cloud detection on remote sensing imagery
CN105865449A (zh) * 2016-04-01 2016-08-17 深圳杉川科技有限公司 基于激光和视觉的移动机器人的混合定位方法
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN109682382A (zh) * 2019-02-28 2019-04-26 电子科技大学 基于自适应蒙特卡洛和特征匹配的全局融合定位方法
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及***

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
"Application of Particle Swarm Optimization with Stochastic Inertia Weight and Adaptive Mutation in Target Localization";Jinjie Yao et al.;《2010 International Conference on Computer Application and System Modeling》;20101231;全文 *
"基于改进粒子群算法的智能机器人路径规划";张万绪 等;《计算机应用》;20140210;第34卷(第2期);全文 *
"基于智能优化算法的移动机器人路径规划与定位方法研究";黄辰;《中国博士学位论文全文数据库 信息科技辑》;20190430(第04期);全文 *

Also Published As

Publication number Publication date
CN110888125A (zh) 2020-03-17

Similar Documents

Publication Publication Date Title
CN110888125B (zh) 一种基于毫米波雷达的自动驾驶车辆定位方法
CN111307162B (zh) 用于自动驾驶场景的多传感器融合定位方法
Wu et al. Automatic background filtering and lane identification with roadside LiDAR data
CN111537990B (zh) 一种车道的标定方法、装置以及电子设备
CN112380312B (zh) 基于栅格检测的激光地图更新方法、终端及计算机设备
CN108961758B (zh) 一种基于梯度提升决策树的路口展宽车道探测方法
CN102208013A (zh) 风景匹配参考数据生成***和位置测量***
CN101975951A (zh) 一种融合距离和图像信息的野外环境障碍检测方法
CN111027430A (zh) 一种面向无人车智能评估的交通场景复杂度计算方法
CN104182957A (zh) 交通视频信息检测方法和装置
CN113888850B (zh) 沙尘气象市政道路拥堵态势多因素预警方法
CN114926984A (zh) 一种实时交通冲突收集与道路安全评价方法
DE102020118318A1 (de) Verfahren und Vorrichtung zur Erkennung eines Verkehrsknotenpunktes auf Basis von Trajektoriendaten
CN104156969A (zh) 基于全景影像景深图的探面方法
Lin et al. Construction of Traffic Moving Object Detection System Based on Improved YOLOv5 Algorithm
CN113848878B (zh) 基于众源数据的室内外三维行人路网构建方法
CN113393676B (zh) 一种基于无人机视觉和毫米波雷达的交通检测方法及装置
Ryan et al. Evaluation of small unmanned aerial system highway volume and speed‐sensing applications
CN107588760A (zh) 适用于固定翼无人机跟踪道路的定高高精度航迹生成方法
CN112905856B (zh) 一种具有时空依赖的高速交通数据集的构建方法
DE102022200936A1 (de) Verfahren und Steuergerät zum Betreiben eines Sensorsystems eines Fahrzeugs
Kang et al. Urban road quality design supported by multi-source data and deep learning
Xie et al. Vehicle Automatic Driving Path Based on AdaBoost Algorithm
Linderoth et al. Map-based localization using LiDAR and deep neural networks
CN115626181A (zh) 自驾车的路径规划***及方法

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, Jiangbei new district, Pukou District, Nanjing City, Jiangsu Province

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