CN116520302A - 应用于自动驾驶系统的定位方法和构建三维地图的方法 - Google Patents
应用于自动驾驶系统的定位方法和构建三维地图的方法 Download PDFInfo
- Publication number
- CN116520302A CN116520302A CN202310118899.6A CN202310118899A CN116520302A CN 116520302 A CN116520302 A CN 116520302A CN 202310118899 A CN202310118899 A CN 202310118899A CN 116520302 A CN116520302 A CN 116520302A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- frame sequence
- target
- motion
- cloud frame
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 44
- 230000004927 fusion Effects 0.000 claims description 5
- 238000005295 random walk Methods 0.000 claims description 4
- 238000001514 detection method Methods 0.000 description 16
- 238000010586 diagram Methods 0.000 description 7
- 238000013507 mapping Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 230000009471 action Effects 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 238000000605 extraction Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000008447 perception Effects 0.000 description 3
- 229910052698 phosphorus Inorganic materials 0.000 description 3
- 230000001360 synchronised effect Effects 0.000 description 3
- 230000009466 transformation Effects 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 239000013307 optical fiber Substances 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 102000008115 Signaling Lymphocytic Activation Molecule Family Member 1 Human genes 0.000 description 1
- 108010074687 Signaling Lymphocytic Activation Molecule Family Member 1 Proteins 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000013145 classification model Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 229910052757 nitrogen Inorganic materials 0.000 description 1
- 238000011176 pooling Methods 0.000 description 1
- 238000013138 pruning Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
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/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
- G01S13/06—Systems determining position data of a target
-
- 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/66—Radar-tracking systems; Analogous systems
-
- 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/88—Radar or analogous systems specially adapted for specific applications
- G01S13/89—Radar or analogous systems specially adapted for specific applications for mapping or imaging
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
公开了一种应用于自动驾驶系统的定位方法和构建三维地图的方法。该定位方法包括：利用雷达设备采集周边环境的点云帧序列；根据点云帧序列计算出雷达设备的第一位置和姿态；根据第一位置和姿态从点云帧序列去除动态点云；以及根据去除了动态点云的点云帧序列，计算并输出雷达设备的第二位置和姿态。这种定位方法应用于无人驾驶(或自动驾驶)的无人车或移动机器人等领域，能够提高定位的精度。
Description
技术领域
本公开涉及自动驾驶技术领域，尤其涉及一种应用于自动驾驶系统的定位方法和构建三维地图的方法。
背景技术
近年来，伴随着无人驾驶技术的快速发展，无人驾驶技术的应用场景和范围一再扩大，并产生了诸如无人配送车、无人零售车、无人清扫车、无人巡逻车等具有一定商业价值的落地应用。无人驾驶系统按照功能可大致分为：感知层、决策层和执行层，感知层基于各类传感器完成对车内外环境的感知；决策层根据感知层反馈的信息建立相应的模型，通过分析，制定出最适合的控制策略；执行层主要是完成电子制动、电子驱动、电子转向等实际操作。而定位(确定位置和姿态)在感知层和决策层至关重要，是感知、预测、规划、控制等相关技术的基础。
SLAM(Simultaneous Localization and Mapping,同步定位与建图)是最核心的定位技术之一。SLAM从机器人或者无人车携带的激光雷达、摄像头、IMU(Inertial
Measurement Unit,惯性测量单元)等传感器接收实时采集的点云数据，并将点云数据与三维地图相匹配，计算出机器人或无人车的位置和姿态(即定位)并同时构建周围环境的三维地图。相比于基于GNSS(Global Navigation Satellite System,全球导航卫星系统)，SLAM不需要依赖卫星信号，因此可以在任何环境下工作，更重要的是，它还能输出高精度的三维地图。
在SLAM的实现中，假定周围环境是刚体，即工作环境中的所有物体都是静止的且不会发生形变的。然而，机器人或者无人车的工作环境一般又是充满了动态物体的，例如马路上的行人和汽车等。实时采集到的点云数据中如果包含动态点云，那么会干扰点云数据和三维地图的匹配结果，进而干扰定位结果。
发明内容
有鉴于此，本发明实施例提出一种应用于无人驾驶系统的定位方法和构建三维地图的方法，以解决现存的技术问题。
根据本发明的第一方面，提供一种应用于自动驾驶系统的定位方法，包括：
利用雷达设备采集周边环境的点云帧序列；
根据所述点云帧序列计算出所述雷达设备的第一位置和姿态；
根据所述第一位置和姿态从所述点云帧序列去除动态点云；以及
根据所述去除了动态点云的点云帧序列，计算并输出所述雷达设备的第二位置和姿态。
在一些实施例中，所述根据所述第一位置和姿态从所述点云帧序列去除动态点云包括：
从所述点云帧序列的多个点云帧中识别出多个目标，每个目标具备至少一个语义标签；
将所述雷达坐标系下的点云帧序列中的点云转换到世界坐标系下；
在所述世界坐标下对语义标签为具有运动属性的物体的目标进行追踪，以确定该目标是否运动；以及
将确定为运动的目标对应的动态点云从所述点云帧序列中去除。
在一些实施例中，所述在所述世界坐标下对语义标签为具有运动属性的物体的目标进行追踪包括：为第一目标设定多个运动模型，并基于所述第一目标在所述多个运动模型下的上一时刻的多个状态得到所述第一目标在所述多个运动模型下的当前时刻的多个状态估计，然后根据所述第一目标在当前时刻的实际观测更新所述多个状态估计，最终将更新后的多个状态估计进行融合，并将融合结果作为所述第一目标在当前时刻的最优状态估计。
在一些实施例中，所述将更新后的多个状态估计进行融合包括：
基于所述观测对象与所述更新后的多个状态估计之间的距离分别更新所述多个模型的概率；以及
对更新后的多个模型的概率和所述更新后的多个状态估计进行加权求和，并将加权求和结果作为所述第一目标的最优状态估计。
在一些实施例中，所述在所述世界坐标下对语义标签为具有运动属性的物体的目标进行追踪还包括：
基于预先设置的模型之间相互转化的概率以及所述更新后的多个模型的概率进行加权求和，以得到所述多个模型的新概率；
基于所述预先设置的模型之间相互转化的概率和所述更新后的多个状态估计进行加权求和，以得到第一目标在所述多个运动模型下的下一时刻的状态。
在一些实施例中，所述多个运动模型包括：匀速直线运动模型、匀速旋转运动模型和随机游走模型。
在一些实施例中，在所述世界坐标下对语义标签为具有运动属性的物体的目标进行追踪时，采用UKF进行状态估计。
第二方面，本发明实施例提供一种构建三维地图的方法，包括：
利用雷达设备采集周边环境的点云帧序列；
根据所述点云帧序列计算出所述雷达设备的第一位置和姿态；
根据所述第一位置和姿态从所述点云帧序列去除动态点云；以及
根据所述去除了动态点云的点云帧序列，构建所述三维地图。
第三方面，本发明实施例提供一种雷达定位系统，包括：
采集模块，用于利用雷达设备采集周边环境的点云帧序列；
SLAM模块，用于根据所述点云帧序列计算出所述雷达设备的第一位置和姿态，并根据去除了动态点云的点云帧序列，计算并输出所述雷达设备的第二位置和姿态；
点云去除模块，用于根据所述第一位置和姿态从所述点云帧序列去除动态点云。
第四方面，本发明实施例提供一种电子设备，包括存储器和处理器，所述存储器还存储有可由所述处理器执行的计算机指令，所述计算机指令被执行时，实现上述的定位方法或上述的构建三维地图的方法。
第五方面，本发明实施例提供一种计算机可读介质，所述计算机可读介质存储有可由电子设备执行的计算机指令，所述计算机指令被执行时，实现上述的定位方法或上述的构建三维地图的方法。
本发明实施例旨在解决动态环境下通过雷达设备进行定位和建图的问题，先通过点云帧序列与三维地图的匹配得到初步的雷达设备的位置和姿态，然后利用初步的位置和姿态对点云帧序列中的动态点云进行去除，然后再将去除了动态点云的点云帧序列与三维地图进行匹配来确定雷达设备的位置和姿态，并同时可创建不含动态点云的纯净地图。进一步地为了实现稳定而准确的动态点云去除，本发明实施例将目标检测和多目标跟踪相结合，只对具有运动语义属性的目标进行跟踪。为了尽可能准确的对物体的运动进行预测，多目标跟踪中还利用三种运动模型对物体的模型进行描述，并进行了融合,从而提高了定位的精度。
附图说明
通过参照以下附图对本发明实施例的描述，本发明的上述以及其它目的、特征和优点将更为清楚，在附图中：
图1是根据本发明实施例提供的定位方法的流程图；
图2是图1中的步骤S102的一实施方式的流程图；
图3是示例性的Point-pillars模型的框图；
图4是图2的步骤S1023更具体的框图；
图5是根据本发明实施例提供的构建三维地图的方法的流程图；
图6是根据利用现有SLAM构建的同步定位和建图系统的框图；
图7用于实施本发明各个实施例的电子设备的结构示意图。
具体实施方式
以下将参照附图更详细地描述本发明。在各个附图中，相同的元件采用类似的附图标记来表示。为了清楚起见，附图中的各个部分没有按比例绘制。此外，可能未示出某些公知的部分。
图1是根据本发明实施例提供的定位方法的流程图。具体包括步骤S101至S104。
在步骤S101中，利用雷达设备采集周围环境的点云帧序列。
在步骤S102中，根据点云帧序列计算出雷达设备的第一位置和姿态。
在步骤S103中，根据雷达设备的第一位置和姿态从点云帧序列去除动态点云。
在步骤S104中，根据去除了动态点云的点云帧序列，计算出雷达设备的第二位置和姿态。
具体地，雷达设备包括机器人或者无人车携带的激光雷达。雷达设备通常以固定周期扫描周边环境，每次扫描会得到一个点云帧，多次扫描得到多个按照时间排序的点云帧序列。每个点云帧可以表示为三维空间下一组无规则分布的、表达三维物体或三维场景的空间结构及表面属性的离散点集合。每个离散点都可以具备几何信息和属性信息。几何信息也可以称为三维位置信息，任一个点的几何信息可以是指该点的三维坐标(x，y，z)，可以包括该点在三维坐标系统的各个坐标轴中的坐标值，即X轴的坐标值x，Y轴的坐标值y，Z轴的坐标值z。
现有技术中，SLAM的工作方式是：通过点云帧序列计算出雷达设备的位置和姿态(例如旋转或平移)，将雷达设备采集到的离散点转换到同一个坐标系下，然后基于转换后的离散点三维坐标构建新的三维地图或者扩展已有的三维地图。
但是，由于现有技术中的SLAM采用了可能包含动态点云的点云帧计算雷达设备的当前位置和姿态及构建三维地图，因此定位结果和地图精度受到影响。动态点云是指在每个点云帧中，那些代表正在运动的物体的离散点集合，举例说明，如果用雷达扫描大街，会扫描到正在行驶的汽车和正在行走的某人，则代表正在行驶的汽车的离散点集合是动态点云，代表正在行走的某人的点集合同样是动态点云。有鉴于此，在本实施例中，增加了步骤S103，从点云帧序列中去除动态点云，然后根据去除了动态点云的点云帧序列，计算出雷达设备的位置和姿态(例如旋转或平移)，并基于雷达设备的位置和姿态将点云帧中的各个离散点的三维坐标从雷达坐标下转换到世界坐标下，然后根据世界坐标下的各个离散点的三维坐标构建或扩展三维地图。作为可选方式，步骤S104这样实现：将去除了动态点云的点云帧序列输入给现有技术中的SLAM，由SLAM输出雷达设备的位置和姿态以及三维地图。
应该强调的是，由于雷达设备采集到的离散点的三维坐标是在雷达坐标系下，因此后续处理时需要将所有离散点转换到同一个坐标系下才能够进行运算和比较。例如将每个离散点从雷达坐标系转换到世界坐标下，操作时，将变换矩阵与雷达坐标系下的每个离散点(x,y,z)相乘得到世界坐标下的(X,Y,Z)。变换矩阵根据雷达设备的位置和姿态得到，因此在本实施例中，包含步骤S102，通过步骤S102得到雷达设备的位置和姿态，并在步骤S103中使用雷达设备的第一位置和姿态。还应该说明的是，三维地图一般是指世界坐标系下的三维地图，因此制作地图前也需要将雷达坐标下的点云数据转换到世界坐标系下，例如，将去除动态点云的点云帧序列从雷达坐标系转换到世界坐标系，然后再制作三维地图。
进一步地，图2是图1中的步骤S102的一实施方式的流程图。如图2所示，该实施方式包括步骤S1021至S1024。
在步骤S1021中，从点云帧序列识别出多个目标，每个目标具备至少一个语义标签。
在步骤S1022中，将点云帧序列从雷达坐标系转换到世界坐标系。
在步骤S1023中，对语义标签为具有运动属性的物体的目标进行追踪，以确定该目标是否正在运动。
在步骤S1024中，将确定为正在运动的目标对应的动态点云从点云帧序列中去除。
根据本实施例，从点云帧序列中识别出多个目标，所述目标是指具有明确语义的物体，其表现形式为三维矩形的边界框，属性包括语义标签、边界框的中心、长度、宽度、高度、长轴朝向等信息，然后在点云帧序列中语义标签为具有运动属性的物体的目标进行追踪，具有运动属性的物体是具有运动能力的物体但不确定当前是否正在运动，例如大街上的人和汽车，因此通过追踪这类目标确定其是否正在运动，确定为正在运动的目标对应的点云为动态点云，将其从点云帧序列中去除，以便于利用去除动态点云的点云帧序列计算出雷达设备的当前位置和姿态，这样计算的位置和姿态相对而言更加精确。
更具体地，作为一种实施方式，步骤S1021可以由已经训练好的3D点云目标检测模型实现。3D点云目标检测模型可以在原始模型基础上进行剪枝处理得到。原始模型可以由点云特征提取网络，2D CNN网络和多组检测头组合而成。其中，点云特征提取网络用于将输入的点云帧序列通过1D卷积或者全连接方式编码为2D的伪图片；2D CNN网络用于从2D的伪图片进一步提取特征；多组检测头用于完成对最后一个特征图每个像素点所包含的检测框的回归与分类，目标分类确定了目标的语义标签，比如，人、车、房屋等；目标定位确定了目标框中心点位置,目标框的长，宽，高,朝向角。
图3是可以用作原始模型的Point-pillars三维模型的结构图。PointPillars整体网络结构有三个部分构成：点云特征提取网络(PFN，Pillar Feature Net),骨干网(2DCNN)和检测头(SSD)。PFN将输入的点云转换为2D的伪图片，具体实现步骤如下：1)按照点云数据所在的X，Y轴(不考虑Z轴)将点云数据划分为一个个网格，凡是落在一个网格中的点云被视为在一个pillar(支柱)里。每个点云用一个D＝9维的向量表示，分别为(x,y,z,r,xc,yc,zc,xp,yp)。x,y,z,r是该点云的真实坐标信息(三维)和雷达反射强度；xc,yc,zc是该点云所处pillar中所有点的几何中心，xp,yp为x-xc,y-yc,反映了该点云与几何中心的相对位置。假设每个点云帧有P个非空的支柱，每个支柱中有N个点云(大于N下采样至N，小于N则填充0)，则这个点云帧可使用(D,P,N)张量表示。2)然后将支柱所在维度进行最大池化操作，得到(C,P)的特征图；3)然后将(C,P)的特征图转换为(C,H,W)的伪图片，即将P转换为(H,W)。Backbone(2D CNN)通过2D卷积从伪图片实现特征提取目标检测，如图上所示，骨干网(2D CNN)则采用两个网络，其中一个网络采用卷积层不断地缩小特征图的分辨率，同时提升特征图的维度，因此得到三个不同分辨率的特征图，另一个网络通过解卷积(例如上采样)将三个特征图恢复至至同样大小，然后进行拼接。检测头(SSD)用于目标分类与目标定位，目标定位是生成边界框，目标分类是目标进行预测，并最终将目标检测框从特征图维度恢复到三维实际位置与长高宽。检测头(SSD)中的损失函数主要有两部分，定位损失和分类预测损失。
进一步，采用点云聚类算法检测得到各个目标的三维边界框。点云聚类算法指通过搜索迭代的策略对类别上同类且位置上在局部为一簇的点云进行聚合并得到其外接三维边界框，最终得到场景内所有检测目标的三维边界框的信息，即3D聚类边界框的信息。在此过程中，还可以对3D聚类边界框的目标的点数进行判断，其中，当点云数量大于预设阈值时，作为目标框出；当点数小于预设的阈值时，判断检测目标为噪声点或误识别目标。点云聚类算法可以与目标分类模型进行连接，以确定目标类别。此外，还可以综合点云聚类算法和3D点云目标检测模型的检测结果，以产生更优化的结果。
而对于步骤S1022，其输入是步骤S1021输出的多个目标，需从中挑选这些具有运动属性的物体(边界框)作为跟踪的对象，例如行人、汽车、骑行的人，然后对这些物体(边界框)进行跟踪。具体操作是：利用运动模型对已经跟踪上的具有运动属性的物体(边界框)的状态进行估计，然后利用当前帧对物体的观测对已经跟踪上的运动物体的状态进行更新，其状态具体包括位置、姿态、线速度、角速度；如果当前帧观测到的物体没有用于更新任何一个已经跟踪上的物体，那么会初始化一个新的物体；如果跟踪中发现物体的速度超过一定范围，即状态估计x中的线速度和/或角速度大于对应阈值，则认为该物体是运动的，边界框内的点云都属于动态点云，应该被去除，剩下的为静态点云。更具体地，作为一种实施方式，步骤S1022使用UKF(Unscented Kalman Filter,无损卡尔曼滤波)对物体的状态进行估计和更新，相比于EKF(Extended Kalman Filter,扩展卡尔曼滤波)，UKF在预测方程和观测方程非线性化比较强的情况下，依然能够比较好的对状态进行估计，同时效率也比较高。
参考图4，使用了三种运动模型并结合UKF对物体的状态进行估计和更新。这三种运动模型即匀速直线运动、匀速旋转运动和随机运动(模拟静止情况下其他模块带来的噪声)。图4中，UKF预测根据k-1时刻已经跟踪上物体的状态和运动模型，对k时刻物体的状态进行估计；更新则根据当前帧的观测，即利用当前帧分割并坐标变换到世界坐标系下的对该物体当前状态的观测，对该物体的状态进行修正。应该理解，在实际环境中，运动物体的运动模型并不能使用单一的模型来完整的描述，因此，为了提升跟踪的稳定性，对于每个被跟踪物体，都在三个独立的UKF中使用三种运动模型分别对物体的状态进行估计；同时，对于每个运动模型mi都用一个概率值pi来表征该运动物体与该运动模型之间的相符程度。当然，本发明不局限于此，还可以采用更多和更少的运动模型。
为了方便说明，假定匀速直线运动模型、匀速旋转运动模型和随机游走模型分别为m1,m2和m3,每种模型对应的K时刻状态为x1,x2和x3,每种模型的概率为p1,p2和p3。
参考图上，从k-1时刻物体状态出发，分三个分支：分支S11中，基于匀速直线运动模型m1，采用UKF预测得到K时刻的状态估计x1；分支S12中，基于匀速旋转运动模型m2，采用UKF预测得到K时刻的状态估计x2；分支S13中，基于随机游走模型m3，采用UKF预测得到K时刻的状态估计x3。
观测指的是从当前帧分割得到的具有运动属性的物体在世界坐标系下的状态，为了对UKF进行更新需要先进行数据关联。数据关联要解决的是，对于一个状态估计，从所有可能的观测中找到最可能的一个观测，用这个关联度最高的观测对象来进行状态更新。鉴于本实施例使用三种独立的运动模型来独立估计运动物体的状态，因此在步骤S22的数据关联中会选择距离三个估计结果的平均距离(例如马氏距离、欧氏距离)最近的观测对象作为关联对象。
然后，由于三个估计结果与关联对象的距离，也反映了三种运动模型与观测之间相符的程度，因此，在步骤S31至S33中，分别以三个状态估计与关联对象的距离对每个运动模型mi的概率pi进行更新；同时基于关联对象，对三个运动模型的K时刻的状态估计进行独立的更新，得到三个更新后的K时刻的状态估计。
再然后，在状态混合的框图S44中，将步骤S31至S33输出的更新后的x1,x2和x3与更新后的p1,p2和p3进行加权求和，混合加权得到该物体最优状态估计x，最优状态估计x中的线速度和/或角速度大于对应阈值，则认为该物体是运动的。
同时，物体的运动模型也不是一成不变的，例如运动的汽车在等红绿灯的时候会从匀速运动模型转换到随机运动，因此预先设置了模型之间相互转化的概率，对每个模型在特定时刻的概率进行更新。例如，模型m1的概率值为p1,模型m2的概率值为p2,模型m3的概率值为p3,预先设定模型m1保持不变的概率为p11，模型m2转换到模型m1的概率为p21，模型m3转换到模型m1的概率为p31，那么对于模型m1来说，基于模型转化概率更新自身的概率为p1*p11+p2*p21+p3*p31。通过这种方式，在步骤S51至S53中，分别计算出m1、m2和m3更新后的概率。然后通常还会对对三种模型的更新后的概率进行归一化。
然后在步骤S61至S63中，基于每个运动模型对应的更新后的K时刻的状态估计结合模型转化的概率进行状态更新，得到每个运动模型的K时刻的状态。例如，假设之前得到的运动模型m1,m2和m3对应的K时刻的状态估计分别为x1’,x2’和x3’，m1保持的概率为p11,模型m2转换到模型m1的概率为p21，模型m3转换到模型m1的概率为p31，则得到m1更新后的K时刻的状态为：x1’＝x1’*p11+x2’*p21+x3’*p31，其中p11+p21+p31＝1。以此类推。如此每种运动模型mi在k时刻获得了新的概率pi和新的状态xi，当下一时刻点云帧来临时，重复开始上述处理。而根据k时刻的最优状态估计可以确定目标是否处于运动状态。每个时刻的最优状态估计形成了每个具有运动属性的物体的目标的运动轨迹。
图5是根据本发明实施例提供的构建三维地图的方法的流程图。包括以下步骤。
在步骤S501中，利用雷达设备采集周围环境的点云帧序列。
在步骤S502中，根据点云帧序列计算出雷达设备的第一位置和姿态。
在步骤S503中，根据雷达设备的第一位置和姿态从点云帧序列去除动态点云。
在步骤S504中，根据去除了动态点云的点云帧序列构建三维地图。
根据本实施例，先通过点云帧序列与三维地图的匹配得到初步的雷达设备的位置和姿态，然后利用初步的位置和姿态对点云帧序列中的动态点云进行去除，然后再根据去除了动态点云的点云帧序列创建不含动态点云的纯净地图。
综上，本发明实施例旨在解决动态环境下通过雷达设备进行定位和建图的问题，和现有技术相比，能够得到更加精准的定位，并得到相对纯净的地图。进一步地为了实现稳定而准确的动态点云去除，本发明实施例将目标检测和多目标跟踪相结合，只对具有运动语义属性的目标进行跟踪。为了尽可能准确的对物体的运动进行预测，多目标跟踪中还利用三种运动模型对物体的模型进行描述，并进行了融合,从而提高了定位的精度。
图6是根据利用现有SLAM构建的同步定位和建图系统的框图。如图上所示，系统600包括采集模块601、SLAM模块602和动态点云去除模块603。
采集模块601可包括机器人或者无人车携带的激光雷达、摄像头、IMU(IertialMeasurement Unit,惯性测量单元)等传感器，通过这些传感器接收实时采集的点云帧序列，并将这些实时采集的点云帧序列提供给SLAM 602。
SLAM模块602采用现有技术SLAM，用于将点云数据与三维地图相匹配，计算出机器人或无人车的位置和姿态(即定位)。
动态点云去除模块603根据SLAM模块602输出的位置和姿态从点云帧序列去除动态点云，并将去除了动态点云的点云帧序列提供给SLAM模块602。SLAM模块602使用去除了动态点云的点云帧序列重新进行定位，并且可以使用去除了动态点云的点云帧序列产生纯净的三维地图。由于关于动态点云去除模块603的相关细节上文已有详细介绍，这里就不再重复。
参考图7，电子设备700包括通过总线连接的处理器701、存储器702和输入输出设备703。存储器702包括只读存储器(ROM)和随机访问存储器(RAM)，存储器702内存储有执行系统功能所需的各种计算机指令和数据，处理器701从存储器702中读取各种计算机指令以执行各种适当的动作和处理。输入输出设备包括键盘、鼠标等的输入部分；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分；包括硬盘等的存储部分；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分。存储器702还存储有计算机指令，当执行计算机指令时，完成上述实施例中的各个步骤。
相应地，本发明实施例提供一种计算机可读存储介质，该计算机可读存储介质存储有计算机指令，计算机指令被执行时实现实施例中的各个步骤。
计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如但不限于为电、磁、光、电磁、红外线或半导体的系统、装置或器件，或其它任意以上的组合。计算机可读存储介质的更具体的例子包括：具体一个或多个导线的电连接，便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或者闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器、磁存储器或者上述任意合适的组合。在本文中，计算机可读的存储介质可以是任意包含或存储程序的有形介质，该程序可以被处理单元、装置或者器件使用，或者与其结合使用。
计算机可读信号介质可以包括在基带中或者作为截波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或者其它任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质之外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令系统、装置或器件使用或者与其结合使用的程序。
计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、电线、光缆、RF等等，以及上述任意合适的组合。
可以以一种或者多种程序设计语言或者组合来编写用于执行本发明实施例的计算机程序代码。所述程序设计语言包括面向对象的程序设计语言，例如JAVA、C++，还可以包括常规的过程式程序设计语言，例如C。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络包括局域网(LAN)或广域网(WAN)连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
应当说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其它变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其它要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
依照本发明的实施例如上文所述，这些实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施例。显然，根据以上描述，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地利用本发明以及在本发明基础上的修改使用。本发明仅受权利要求书及其全部范围和等效物的限制。
Claims (11)
1.一种应用于自动驾驶系统的定位方法，包括：
利用雷达设备采集周边环境的点云帧序列；
根据所述点云帧序列计算出所述雷达设备的第一位置和姿态；
根据所述第一位置和姿态从所述点云帧序列去除动态点云；以及
根据去除了动态点云的点云帧序列，计算并输出所述雷达设备的第二位置和姿态。
2.根据权利要求1所述的定位方法，其中，所述根据所述第一位置和姿态从所述点云帧序列去除动态点云包括：
从所述点云帧序列的多个点云帧中识别出多个目标，每个目标具备至少一个语义标签；
将雷达坐标系下的点云帧序列中的点云转换到世界坐标系下；
在所述世界坐标下对语义标签为具有运动属性的物体的目标进行追踪，以确定该目标是否正在运动；以及
将确定为正在运动的目标对应的动态点云从所述点云帧序列中去除。
3.根据权利要求2所述的定位方法，其中，所述在所述世界坐标下对语义标签为具有运动属性的物体的目标进行追踪包括：为第一目标设定多个运动模型，并基于所述第一目标在所述多个运动模型下的上一时刻的多个状态得到所述第一目标在所述多个运动模型下的当前时刻的多个状态估计，然后根据所述第一目标在当前时刻的实际观测更新所述多个状态估计，最终将更新后的多个状态估计进行融合，并将融合结果作为所述第一目标在当前时刻的最优状态估计。
4.根据权利要求3所述的定位方法，其中，所述将更新后的多个状态估计进行融合包括：
基于所述观测对象与所述更新后的多个状态估计之间的距离分别更新所述多个模型的概率；以及
对更新后的多个模型的概率和所述更新后的多个状态估计进行加权求和，并将加权求和结果作为所述第一目标的最优状态估计。
5.根据权利要求3所述的定位方法，其中，所述在所述世界坐标下对语义标签为具有运动属性的物体的目标进行追踪还包括：在将所述将融合结果作为所述第一目标在当前时刻的最优状态估计之后，
基于预先设置的模型之间相互转化的概率以及所述更新后的多个模型的概率进行加权求和，以得到所述多个模型的新概率；
基于所述预先设置的模型之间相互转化的概率和所述更新后的多个状态估计进行加权求和，以得到第一目标在所述多个运动模型下的下一时刻的状态。
6.根据权利要求3所述的定位方法，其中，所述多个运动模型包括：匀速直线运动模型、匀速旋转运动模型和随机游走模型。
7.根据权利要求3所述的定位方法，其中，在所述世界坐标下对语义标签为具有运动属性的物体的目标进行追踪时，采用UKF进行状态估计。
8.一种构建三维地图的方法，包括：
利用雷达设备采集周边环境的点云帧序列；
根据所述点云帧序列计算出所述雷达设备的第一位置和姿态；
根据所述第一位置和姿态从所述点云帧序列去除动态点云；以及
根据所述去除了动态点云的点云帧序列，构建所述三维地图。
9.一种雷达定位系统，包括：
采集模块，用于利用雷达设备采集周边环境的点云帧序列；
SLAM模块，用于根据所述点云帧序列计算出所述雷达设备的第一位置和姿态，并根据去除了动态点云的点云帧序列，计算并输出所述雷达设备的第二位置和姿态；
点云去除模块，用于根据所述第一位置和姿态从所述点云帧序列去除动态点云。
10.一种电子设备，包括存储器和处理器，所述存储器还存储有可由所述处理器执行的计算机指令，所述计算机指令被执行时，实现如权利要求1至7任一项所述的定位方法或实现如权利要求8所述的方法。
11.一种计算机可读介质，所述计算机可读介质存储有可由电子设备执行的计算机指令，所述计算机指令被执行时，实现如权利要求1至7任一项所述的定位方法或实现如权利要求8所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310118899.6A CN116520302A (zh) | 2023-01-31 | 2023-01-31 | 应用于自动驾驶系统的定位方法和构建三维地图的方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310118899.6A CN116520302A (zh) | 2023-01-31 | 2023-01-31 | 应用于自动驾驶系统的定位方法和构建三维地图的方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116520302A true CN116520302A (zh) | 2023-08-01 |
Family
ID=87389272
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310118899.6A Pending CN116520302A (zh) | 2023-01-31 | 2023-01-31 | 应用于自动驾驶系统的定位方法和构建三维地图的方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116520302A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117218123A (zh) * | 2023-11-09 | 2023-12-12 | 上海擎刚智能科技有限公司 | 一种基于点云的冷轧带钢飞丝设备故障检测方法及系统 |
-
2023
- 2023-01-31 CN CN202310118899.6A patent/CN116520302A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117218123A (zh) * | 2023-11-09 | 2023-12-12 | 上海擎刚智能科技有限公司 | 一种基于点云的冷轧带钢飞丝设备故障检测方法及系统 |
| CN117218123B (zh) * | 2023-11-09 | 2024-02-02 | 上海擎刚智能科技有限公司 | 一种基于点云的冷轧带钢飞丝设备故障检测方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111429574B (zh) | 基于三维点云和视觉融合的移动机器人定位方法和系统 | |
| US11145073B2 (en) | Computer vision systems and methods for detecting and modeling features of structures in images | |
| CN108319655B (zh) | 用于生成栅格地图的方法和装置 | |
| CN111402339B (zh) | 一种实时定位方法、装置、系统及存储介质 | |
| CN112639502A (zh) | 机器人位姿估计 | |
| WO2021021862A1 (en) | Mapping and localization system for autonomous vehicles | |
| JP2023549036A (ja) | 点群からの効率的な三次元物体検出 | |
| CN113720324A (zh) | 一种八叉树地图构建方法及系统 | |
| JP2007322391A (ja) | 自車両位置推定装置 | |
| CN114066773B (zh) | 一种基于点云特征与蒙特卡洛扩展法的动态物体去除 | |
| CN116520302A (zh) | 应用于自动驾驶系统的定位方法和构建三维地图的方法 | |
| CN114577196B (zh) | 使用光流的激光雷达定位 | |
| CN114842106A (zh) | 栅格地图的构建方法及构建装置、自行走装置、存储介质 | |
| CN112907625A (zh) | 应用于四足仿生机器人的目标跟随方法及系统 | |
| KR102130687B1 (ko) | 다중 센서 플랫폼 간 정보 융합을 위한 시스템 | |
| CN115619954A (zh) | 稀疏语义地图的构建方法、装置、设备及存储介质 | |
| Wu et al. | 2d lidar slam based on Gauss-Newton | |
| CN112612788A (zh) | 一种无导航卫星信号下的自主定位方法 | |
| CN117537803B (zh) | 机器人巡检语义-拓扑地图构建方法、系统、设备及介质 | |
| CN118225078A (zh) | 交通工具的定位方法、装置、交通工具及存储介质 | |
| CN115200601A (zh) | 一种导航方法、装置、轮式机器人及存储介质 | |
| CN115270918A (zh) | 目标检测方法、建立时间关联感知模型的方法及装置 | |
| CN116300922A (zh) | 一种自定义跟随图层的分层代价地图构建方法 | |
| CN117746417A (zh) | 目标检测模型构建方法、目标检测方法及相关装置 | |
| CN117079243A (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 |