CN113848561A - 深度视觉相机与激光雷达融合的导航方法、系统及设备 - Google Patents

深度视觉相机与激光雷达融合的导航方法、系统及设备 Download PDF

Info

Publication number
CN113848561A
CN113848561A CN202111146005.1A CN202111146005A CN113848561A CN 113848561 A CN113848561 A CN 113848561A CN 202111146005 A CN202111146005 A CN 202111146005A CN 113848561 A CN113848561 A CN 113848561A
Authority
CN
China
Prior art keywords
vision camera
depth vision
information
laser radar
particles
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
Application number
CN202111146005.1A
Other languages
English (en)
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.)
China Comservice Enrising Information Technology Co Ltd
Original Assignee
China Comservice Enrising Information Technology 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 China Comservice Enrising Information Technology Co Ltd filed Critical China Comservice Enrising Information Technology Co Ltd
Priority to CN202111146005.1A priority Critical patent/CN113848561A/zh
Publication of CN113848561A publication Critical patent/CN113848561A/zh
Pending legal-status Critical Current

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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

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

Abstract

本发明公开了一种深度视觉相机与激光雷达融合的导航方法，包括：S1利用深度视觉相机和激光雷达获取信息，并根据该信息生成地图；S2在生成的地图中对自身即时位置进行定位；S3规划从自身即时位置到目的地的路径。将两种传感器所得到的信息进行汇总，以激光雷达数据为主，辅助增加深度视觉摄像头提供的轮廓、大小、相对位置等信息后，可以在粒子权重估算方面得到更准确的权重值，从而构建更准确的地图，使得导航更加精确。

Description

深度视觉相机与激光雷达融合的导航方法、系统及设备
技术领域
本发明涉及一种将深度视觉相机与激光雷达相融合的导航方法、实现该方法的系统、装配该系统的设备。
背景技术
目前各类工业机器人、服务型机器人已经广泛应用于社会生产、生活的各个方面，为生产生活带来了极大的便捷。其中应用最广泛的是应用于各类搬运、巡检、清扫等应用场景的机器人设备。
现有的机器人通过搭载大量激光传感器、红外传感器、超声波传感器、碰撞传感器等辅助设备进行目标定位、物体识别、距离感知等功能，造成机器人价格高昂，体积庞大等缺点。另外现有机器人的导航算法在在某些场景会发生偏差，导致导航不准确。
发明内容
有鉴于此，本发明提供一种深度视觉相机与激光雷达融合的导航方法、系统及设备，采用深度视觉相机和激光雷达作为导航传感器并融合二者的导航算法，减小传感器数量和体积，提高导航准确率。
为解决以上技术问题，本发明提供一种深度视觉相机与激光雷达融合的导航方法，包括：S1利用深度视觉相机和激光雷达获取信息，并根据该信息生成地图；S2在生成的地图中对自身即时位置进行定位；S3规划从自身即时位置到目的地的路径。
作为一种改进，步骤S1包括：
S11设定粒子数量，并通过激光雷达将粒子平均分布在规划区域内；S12根据状态转移方程对每个粒子进行状态估计，从而产生与之对应的预测粒子；S13将深度视觉相机采集到的信息与激光雷达采集到的信息进行叠加获得更精确的预测粒子，并对预测粒子进行评价并赋予相应的权重，权重大小和其与真实值的差距成反比，即与真实值的差距越小权重越大；S14对预测粒子进行筛选，保留权重大于设定值的预测粒子，部分保留权重小于设定值的预测粒子，并添加新的预测粒子使得预测粒子的总量不变；S16选取最优预测粒子，利用激光雷达采集的信息构建2D地图，并在按照2D地图运动的过程中利用深度视觉相机采集的信息构建3D地图。
作为一种进一步的改进，所述步骤S12中状态转移方程为：
P(:,i)＝P(:,i)+distance*[-cos(k*theta)；sin(k*theta)]+wgn(2,1,10*log10(Q)) I
dist＝norm(P(:,i)-Z(:,k)) II
w(i)＝(1/sqrt(R)/sqrt(2*pi))*exp(-(dist)^2/2/R) III
P(:,i)为粒子群；theta＝pi/T表示旋转角度(T为测量时间)；k为当前时间；Q为过程噪声；Wgn为高斯白噪声计算函数；Norm函数，在向量空间里对向量赋予长度和大小；R为测量噪声；Sqrt求平方根函数；Exp指数函数。
作为另一种更进一步的改进，步骤S13中，将深度视觉相机采集到的信息与激光雷达采集到的信息进行叠加包括：S131对激光雷达采集的数据进行分析，得到规划区域中障碍物的距离信息；S132对深度视觉相机采集的数据进行分析，得到规划区域中障碍物的旋转角度信息；S133融合距离信息和旋转角度信息。
作为一种改进，所述步骤S133包括：
设深度视觉相机获取的旋转角度信息为ZV，激光雷达获取的距离信息为ZL；若(Zi L,Zj V)可配对，则
Figure BDA0003285448140000031
式中：
Figure BDA0003285448140000032
从公式IX中可以获得其协方差矩阵：
Eij＝BijEi LBT ij+CijEjVCT ij V
式中：Ei L,Ej V分别是Zi L,Zj V的协方差矩阵；
根据Zi L,Zj V之间的马氏距离的X2验证进行相容性判断：
Figure BDA0003285448140000033
式中：d＝rank(hij),α为可信度；
当出现Li t与Vt中有多个特征相匹配时，则选取与其距离最近的特征作为最佳匹配；
通过基本的扩展卡尔曼滤波进行信息融合：
Figure BDA0003285448140000034
Figure BDA0003285448140000035
Figure BDA0003285448140000036
作为一种改进，还包括滤波步骤S15，所述滤波步骤S15包括：循环执行步骤S12、步骤S13、步骤S14。
作为一种改进，所述步骤S2采用AMCL来实现。
作为一种改进，所述步骤S3采用move_base来实现。
本发明还提供一种导航系统，包括深度视觉相机和激光雷达，并采用上述导航方法进行导航。
本发明还提供一种智能设备，装配有上述导航系统。
本发明的有益之处在于：深度视觉可以比激光雷达更准确的得到物体轮廓、大小、与其他物体的相对位置等信息，但无法精确得到距离信息。激光雷达可以精确得到距离信息，但无法精确得到物体轮廓、与其他物体相对位置等信息。将两种传感器所得到的信息进行汇总，以激光雷达数据为主，辅助增加深度视觉摄像头提供的轮廓、大小、相对位置等信息后，可以在粒子权重估算方面得到更准确的权重值。
在综合建图方面，最终在服务器端呈现的地图样本将为叠加了深度视觉数据与激光雷达点云数据的地图，可更直观的用于其他应用。
附图说明
图1为发明中导航方法的流程图。
图2为导航方法中生成地图的流程图。
图3为粒子示意图。
具体实施方式
为了使本领域的技术人员更好地理解本发明的技术方案，下面结合具体实施方式对本发明作进一步的详细说明。
如图1所示，本发明提供一种深度视觉相机与激光雷达融合的导航方法，包括以下步骤：
S1利用深度视觉相机和激光雷达获取信息，并根据该信息生成地图；
S2在生成的地图中对自身即时位置进行定位；机器人在导航的过程中需要时刻确定自身当前的位置，本实施例中采用的AMCL(自适应蒙特卡洛定位)包。AMCL是一种概率定位系统，以2D方式对移动机器人定位，它实现了自适应(或者KLD-采样)蒙特卡洛定位法，使用粒子滤波跟踪机器人在已知地图中的位姿。下面的图片显示用里程计和AMCL定位的不同之处，AMCL估计base结构(机器人)相当于global结构(世界地图)TF转换(ROS中的坐标系转换)。从本质上，这种转换利用航位推算来处理漂移，所发布的转换是远期的。
S3规划从自身即时位置到目的地的路径。本实施例中，路径导航部分使用move_base包，move_base能够获取机器人周围信息(如激光雷达扫描结果)并生成全局与局部的代价地图，根据这些代价地图可以使机器人绕开障碍物安全到达指定的位置。move_base的路径规划主要分为全局规划和局部规划，分别采用的是A*算法和DWA(Dynamic WindowApproach))算法。move_base包是ROS机器人操作系统环境下关于机器人路径规划的中心枢纽。move_base订阅了激光雷达、地图、amcl(自适应蒙特卡洛定位)的定位等数据，然后规划出全局和局部路径，再将路径转化为机器人的速度信息，最终实现机器人自主导航。move_base提供了ROS导航的配置、运行、交互接口，它主要包括两个部分：(1)全局路径规划(global planner)：根据给定的目标位置进行总体路径的规划；(2)本地实时规划(localplanner)：根据附近的障碍物进行躲避路线规划。
其中，如图2所示，步骤S1又包括：
S11设定粒子数量，并通过激光雷达将粒子平均分布在规划区域内；本发明中所述的“粒子”见步骤S12中解释。
S12根据状态转移方程对每个粒子进行状态估计，从而产生与之对应的预测粒子；状态转移方程为：
P(:,i)＝P(:,i)+distance*[-cos(k*theta)；sin(k*theta)]+wgn(2,1,10*log10(Q)) I
dist＝norm(P(:,i)-Z(:,k)) II
w(i)＝(1/sqrt(R)/sqrt(2*pi))*exp(-(dist)^2/2/R) III
P(:,i)为粒子群；theta＝pi/T表示旋转角度(T为测量时间)；k为当前时间；Q为过程噪声；Wgn为高斯白噪声计算函数；Norm函数，在向量空间里对向量赋予长度和大小；R为测量噪声；Sqrt求平方根函数；Exp指数函数。
如图3所示，实心圆表示移动机器人真实的信息情况及粒子,空心圆表示估计信息情况及预测粒子。箭头表示估计信息情况；xt表示t时刻的实际位姿向量,则x1:t表示机器人的轨迹；zt,k表示t时刻对第k个路标的观测,则z1:t表示观测信息；ut表示移动机器人的控制信息,使其从xt-1到达位姿xt,则u1:t表示控制的历史信息；mk表示第k个路标的位置状态向量,则m表示地图。
S13将深度视觉相机采集到的信息与激光雷达采集到的信息进行叠加获得更精确的预测粒子，并对预测粒子进行评价并赋予相应的权重，权重大小和其与真实值的差距成反比，即与真实值的差距越小权重越大；在这步骤中，需要对预测粒子进行评价，越接近于真实状态的粒子，其权重越大，反之，与真实值相差较大的粒子，其权重越小。此步骤是为下一步骤重采样做准备。激光雷达和深度视觉相机会返回周围位置信息，如果这些信息与期望值相差较大，亦或者在运动中某些粒子本应该没有碰到障碍或者边界，然而在运算中却到达甚至穿过了障碍点或边界，那么这种粒子就是坏点粒子，这样的粒子权重也就比较低一些。将上一步中上一时刻每个粒子的预测状态估计与该时刻实际观测的粒子状态进行比对，如果差值越小，说明上一时刻的预测越准确，权重越大，反之差值越大，说明上一时刻的预测约不精准，权重越小。
S14对预测粒子进行筛选，保留权重大于设定值的预测粒子，部分保留权重小于设定值的预测粒子，并添加新的预测粒子使得预测粒子的总量不变；权重小粒子存在两种情况，一种是坏点，就是采集错误；另一种是不重要点，比如对机器人运动轨迹不会造成很大影响的点。但由于无法分辨哪些是坏点哪些是不重要点，所以采取的措施就是删除一部分权重小的点，最终通过下一步滤波将坏点尽可能的清除。
S15滤波：循环执行步骤S12、步骤S13、步骤S14。将步骤S14中重采样后的粒子带入状态转移方程得到新的预测粒子，然后将它们继续进行上述步骤S13、步骤S14过程，经过这种循环迭代，最终绝大部分粒子会聚集在与真实值最接近的区域内，从而得到机器人准确的位置。
S16选取最优预测粒子，利用激光雷达采集的信息构建2D地图，并在按照2D地图运动的过程中利用深度视觉相机采集的信息构建3D地图。
步骤S13中将深度视觉相机采集到的信息与激光雷达采集到的信息进行叠加的步骤具体包括：
S131对激光雷达采集的数据进行分析，得到规划区域中障碍物的距离信息；
S132对深度视觉相机采集的数据进行分析，得到规划区域中障碍物的旋转角度信息；
S133融合距离信息和旋转角度信息。设深度视觉相机获取的旋转角度信息为ZV，激光雷达获取的距离信息为ZL；若(Zi L,Zj V)可配对，则
Figure BDA0003285448140000071
式中：
Figure BDA0003285448140000072
从公式IX中可以获得其协方差矩阵：
Eij＝BijEi LBT ij+CijEjVCT ij V
式中：Ei L,Ej V分别是Zi L,Zj V的协方差矩阵；
根据Zi L,Zj V之间的马氏距离的X2验证进行相容性判断：
Figure BDA0003285448140000081
式中：d＝rank(hij),α为可信度；
当出现Li t与Vt中有多个特征相匹配时，则选取与其距离最近的特征作为最佳匹配；
通过基本的扩展卡尔曼滤波进行信息融合：
Figure BDA0003285448140000082
Figure BDA0003285448140000083
Figure BDA0003285448140000084
本发明还提供一种导航系统，包括深度视觉相机和激光雷达，并采用上述导航方法进行导航。
本发明还提供一种智能设备，装配有上述导航系统。
深度视觉相机主要作用是通过红外结构光或Time-of-Flight原理，直接测出图像中各像素离相机的距离，但深度视觉相机对光线的依赖程度较高，所以在该机器人产品开发过程中，使用深度视觉相机+激光雷达的两种传感器同时获得图像位置、距离信息，将两个传感器获得的位置信息进行融合比对，后端根据参照物和预先配置的配置文件情况，对目标物体位置进行精确测距和定位，保障导航路径规划和行驶路径更加精确。
以上仅是本发明的优选实施方式，应当指出的是，上述优选实施方式不应视为对本发明的限制，本发明的保护范围应当以权利要求所限定的范围为准。对于本技术领域的普通技术人员来说，在不脱离本发明的精神和范围内，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种深度视觉相机与激光雷达融合的导航方法，其特征在于包括：
S1利用深度视觉相机和激光雷达获取信息，并根据该信息生成地图；
S2在生成的地图中对自身即时位置进行定位；
S3规划从自身即时位置到目的地的路径。
2.根据权利要求1所述的一种深度视觉相机与激光雷达融合的导航方法，其特征在于步骤S1包括：
S11设定粒子数量，并通过激光雷达将粒子平均分布在规划区域内；
S12根据状态转移方程对每个粒子进行状态估计，从而产生与之对应的预测粒子；
S13将深度视觉相机采集到的信息与激光雷达采集到的信息进行叠加获得更精确的预测粒子，并对预测粒子进行评价并赋予相应的权重，权重大小和其与真实值的差距成反比，即与真实值的差距越小权重越大；
S14对预测粒子进行筛选，保留权重大于设定值的预测粒子，部分保留权重小于设定值的预测粒子，并添加新的预测粒子使得预测粒子的总量不变；
S16选取最优预测粒子，利用激光雷达采集的信息构建2D地图，并在按照2D地图运动的过程中利用深度视觉相机采集的信息构建3D地图。
3.根据权利要求2所述的一种深度视觉相机与激光雷达融合的导航方法，其特征在于所述步骤S12中状态转移方程为：
P(:,i)＝P(:,i)+distance*[-cos(k*theta)；sin(k*theta)]+wgn(2,1,10*log10(Q))Idist＝norm(P(:,i)-Z(:,k))IIw(i)＝(1/sqrt(R)/sqrt(2*pi))*exp(-(dist)^2/2/R)III
P(:,i)为粒子群；
theta＝pi/T表示旋转角度(T为测量时间)；
k为当前时间；
Q为过程噪声；
Wgn为高斯白噪声计算函数；
Norm函数，在向量空间里对向量赋予长度和大小；
R为测量噪声；
Sqrt求平方根函数；
Exp指数函数。
4.根据权利要求2所述的一种深度视觉相机与激光雷达融合的导航方法，其特征在于步骤S13中，将深度视觉相机采集到的信息与激光雷达采集到的信息进行叠加包括：
S131对激光雷达采集的数据进行分析，得到规划区域中障碍物的距离信息；
S132对深度视觉相机采集的数据进行分析，得到规划区域中障碍物的旋转角度信息；
S133融合距离信息和旋转角度信息。
5.根据权利要求4所述的一种深度视觉相机与激光雷达融合的导航方法，其特征在于所述步骤S133包括：
设深度视觉相机获取的旋转角度信息为ZV，激光雷达获取的距离信息为ZL；若
Figure FDA0003285448130000021
可配对，则
Figure FDA0003285448130000022
式中：
Figure FDA0003285448130000023
从公式IX中可以获得其协方差矩阵：
Figure FDA0003285448130000031
式中：Ei L,Ej V分别是Zi L,Zj V的协方差矩阵；
根据Zi L,Zj V之间的马氏距离的X2验证进行相容性判断：
Figure FDA0003285448130000032
式中：d＝rank(hij),α为可信度；
当出现Li t与Vt中有多个特征相匹配时，则选取与其距离最近的特征作为最佳匹配；
通过基本的扩展卡尔曼滤波进行信息融合：
Figure FDA0003285448130000033
Figure FDA0003285448130000034
Figure FDA0003285448130000035
6.根据权利要求2所述的一种深度视觉相机与激光雷达融合的导航方法，其特征在于还包括滤波步骤S15，所述滤波步骤S15包括：循环执行步骤S12、步骤S13、步骤S14。
7.根据权利要求1所述的一种深度视觉相机与激光雷达融合的导航方法，其特征在于：所述步骤S2采用AMCL来实现。
8.根据权利要求1所述的一种深度视觉相机与激光雷达融合的导航方法，其特征在于：所述步骤S3采用move_base来实现。
9.一种导航系统，其特征在于：包括深度视觉相机和激光雷达，并采用权利要求1～8中任意一项所述的导航方法进行导航。
10.一种智能设备，其特征在于：装配有权利要求9所述的导航系统。
CN202111146005.1A 2021-09-28 2021-09-28 深度视觉相机与激光雷达融合的导航方法、系统及设备 Pending CN113848561A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111146005.1A CN113848561A (zh) 2021-09-28 2021-09-28 深度视觉相机与激光雷达融合的导航方法、系统及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111146005.1A CN113848561A (zh) 2021-09-28 2021-09-28 深度视觉相机与激光雷达融合的导航方法、系统及设备

Publications (1)

Publication Number Publication Date
CN113848561A true CN113848561A (zh) 2021-12-28

Family

ID=78980532

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111146005.1A Pending CN113848561A (zh) 2021-09-28 2021-09-28 深度视觉相机与激光雷达融合的导航方法、系统及设备

Country Status (1)

Country Link
CN (1) CN113848561A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114236563A (zh) * 2022-02-23 2022-03-25 博学宽行(成都)科技有限公司 一种基于激光与视觉识别导航的机器人导航方法
CN114265417A (zh) * 2022-03-01 2022-04-01 博学宽行(成都)科技有限公司 一种基于激光与视觉识别导航的机器人控制系统

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114236563A (zh) * 2022-02-23 2022-03-25 博学宽行(成都)科技有限公司 一种基于激光与视觉识别导航的机器人导航方法
CN114265417A (zh) * 2022-03-01 2022-04-01 博学宽行(成都)科技有限公司 一种基于激光与视觉识别导航的机器人控制系统

Similar Documents

Publication Publication Date Title
CN109211251B (zh) 一种基于激光和二维码融合的即时定位与地图构建方法
WO2020258721A1 (zh) 智能巡航车导航方法及系统
US20220392235A1 (en) Automated road edge boundary detection
Kümmerle et al. A navigation system for robots operating in crowded urban environments
Badino et al. Free space computation using stochastic occupancy grids and dynamic programming
CN111539994B (zh) 一种基于语义似然估计的粒子滤波重定位方法
US8024072B2 (en) Method for self-localization of robot based on object recognition and environment information around recognized object
Lee et al. Assistive delivery robot application for real-world postal services
EP2863177A1 (en) Method of calculation a path for use in a vehicle
CN113848561A (zh) 深度视觉相机与激光雷达融合的导航方法、系统及设备
US20220363263A1 (en) Automated bump and/or depression detection in a roadway
RU2740229C1 (ru) Способ локализации и построения навигационных карт мобильного сервисного робота
CN112461249A (zh) 根据外部源数据进行的传感器定位
CN113433937A (zh) 基于启发式探索的分层导航避障系统、分层导航避障方法
CN114077249B (zh) 一种作业方法、作业设备、装置、存储介质
Tsiakas et al. Autonomous vehicle navigation in semi-structured environments based on sparse waypoints and LiDAR road-tracking
Valente et al. Fusing laser scanner and stereo camera in evidential grid maps
Badino et al. Stereo-based free space computation in complex traffic scenarios
Meis et al. A new method for robust far-distance road course estimation in advanced driver assistance systems
Valente et al. Evidential SLAM fusing 2D laser scanner and stereo camera
Hong et al. An intelligent world model for autonomous off-road driving
CN116629106A (zh) 移动机器人运行场景的准数字孪生方法、系统、设备及介质
Azri et al. Visual analytics of 3D LiDAR point clouds in robotics operating systems
JP7455044B2 (ja) 飛行体の着陸制御装置
CN114740869A (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