CN113776519A - 一种无光动态开放环境下agv车辆建图与自主导航避障方法 - Google Patents
一种无光动态开放环境下agv车辆建图与自主导航避障方法 Download PDFInfo
- Publication number
- CN113776519A CN113776519A CN202111072340.1A CN202111072340A CN113776519A CN 113776519 A CN113776519 A CN 113776519A CN 202111072340 A CN202111072340 A CN 202111072340A CN 113776519 A CN113776519 A CN 113776519A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- dynamic
- map
- agv
- obstacle avoidance
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000013507 mapping Methods 0.000 title claims abstract description 17
- 230000004927 fusion Effects 0.000 claims abstract description 31
- 238000012545 processing Methods 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 8
- 239000000126 substance Substances 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 4
- 239000002131 composite material Substances 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 2
- 238000009499 grossing Methods 0.000 claims description 2
- 230000033001 locomotion Effects 0.000 abstract description 10
- 238000005457 optimization Methods 0.000 abstract description 8
- 230000007547 defect Effects 0.000 abstract description 2
- 238000010276 construction Methods 0.000 abstract 1
- 230000006870 function Effects 0.000 description 25
- 238000010586 diagram Methods 0.000 description 15
- 230000008569 process Effects 0.000 description 10
- 238000005259 measurement Methods 0.000 description 9
- 238000004422 calculation algorithm Methods 0.000 description 7
- 238000009826 distribution Methods 0.000 description 7
- 238000006243 chemical reaction Methods 0.000 description 6
- 230000000007 visual effect Effects 0.000 description 6
- 230000009286 beneficial effect Effects 0.000 description 5
- 238000004590 computer program Methods 0.000 description 5
- 238000001514 detection method Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 238000009795 derivation Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000007499 fusion processing Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 208000037170 Delayed Emergence from Anesthesia Diseases 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000002146 bilateral effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- HDRXZJPWHTXQRI-BHDTVMLSSA-N diltiazem hydrochloride Chemical compound [Cl-].C1=CC(OC)=CC=C1[C@H]1[C@@H](OC(C)=O)C(=O)N(CC[NH+](C)C)C2=CC=CC=C2S1 HDRXZJPWHTXQRI-BHDTVMLSSA-N 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Classifications
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种无光动态开放环境下AGV车辆建图与自主导航避障方法，该方法基于激光雷达、深度相机以及惯导多传感器融合方式进行AGV车辆定位和建图问题的解决，同时通过深度相机弥补二维激光只能单扫安装平面范围的障碍物的缺陷进行AGV车辆移动空间内的三维障碍物避障。通过AGV车辆运动模型对编码器进行移动车辆位置进行解算，通过惯导解算移动车辆位姿解算。通过二维激光匹配求取移动车辆位姿，通过深度相机点云匹配求取移动车辆位姿，同时对AGV车辆运动空间内的障碍物进行伪二维激光的投影弥补二维激光扫描单一平面的不足。最后通过融合、优化进行高精度位姿求解与地图构建。
Description
技术领域
本发明涉及AGV车辆自动控制领域，具体涉及一种无光动态开放环境下AGV车辆建图与自主导航避障方法。
背景技术
目前对于未知环境的移动车辆的导航方式有很多，有惯性导航、视觉导航、卫星导航、等等。他们都不同程度地适用于不同的环境，包括室内和室外环境，结构化环境与非结构化环境。
(1)惯性导航
惯性导航是一种最基本的导航方式。它利用车辆装配的光电编码器和陀螺仪计算里程，从而推知车辆当前的位置和下一步的目的地。但是，随着车辆航程的增长，定位的精度就会下降，而定位误差将会无限地增加。
(2)视觉导航
由于计算机视觉理论及算法的发展，视觉导航成为了导航技术中一个重要发展方向。通常，车辆利用装配的摄像机拍摄周围环境的局部图像，然后通过图像处理技术，如图像特征识别、距离估计等，进行车辆定位及规划下一步动作。但是视觉导航计算量很大，不利于在小平台上运用，并且在计算远距离时精度较低。
(3)激光导航
激光导航通过激光传感测距，因激光信号有能量密度大等特性，所以该技术精度更高，测距更远，速度也更快，但同样受到外部环境影响且成本更高。
(4)卫星导航
卫星导航技术最开始主要被用于军事领域，之后随着民用卫星导航技术精度的提高逐渐被用于服务车辆领域。将卫星信号接收装置安装在服务车辆上，同时利用接收全球卫星定位系统提供的时间、位置等信息以实现车辆定位导航。该技术不受地形的影响，且可以提供全球性的导航，因此在服务车辆领域应用较为广泛。但是卫星导航又具有计算量大，实时性差，定位精度低等缺点，且由于卫星探测信号通常无法穿透建筑物，导致卫星导航技术难以应用到车辆室内导航中去。
综上所述，惯性导航在大范围导航时容易产生漂移；视觉导航必须为白天且对光线变化鲁棒性查、对平台计算性能较高不利于在移动车辆平台进行实现；三维激光导航成本较高，二维激光导航只能扫描同一水平面的障碍，对于移动车辆在三维空间中可能遇到的障碍物无法完整的识别；卫星导航实时性差且信号无法穿透建筑物故在室内或地下场景无法使用。
针对以上存在的问题，对于室内或地下无光场景，本发明将融合轮式编码器、惯导、深度相机与二维激光等多种传感器进行无光环境的三维场景进行定位与地图重建，同时系统融合多种紧急安全监测传感器进行运动安全保障，如超声波传感器、防跌落传感器。
发明内容
针对现有技术中的上述不足，本发明提供了一种无光动态开放环境下AGV车辆建图与自主导航避障方法。
为了达到上述发明目的，本发明采用的技术方案为：
一种无光动态开放环境下AGV车辆建图与自主导航避障方法，包括如下步骤：
S1、获取多传感器参数，对多传感器进行相互标定得到多传感器之间的外参；
S2、利用步骤S1标定的多传感器外参融合2D激光雷达参数求解车辆位置姿态信息；
S3、对步骤S2求解的车辆位置姿态信息进行检测及优化，通过优化后的位姿关系对格栅地图进行调整，得到未知三维场景的全局地图；
S4、扫描构建局部地图融合步骤S3得到的位置三维场景的地图对车辆进行动态环境的三维避障与导航。
上述方案的有益效果是，
(1)低成本传感器实现三维避障，没有使用三维激光雷达实现移动车辆的三维空间避障；
(2)多传感器融合实现高精度定位，构建高精度地图，三层融合方式加回环检测调整优化地图结构；
(3)设计紧急安全模块，对突发环境情况进行移动车辆保护；
(4)通过导航时同时扫描构建局部地图融合全局地图构建改进人工势力场函数对移动车辆进行动态环境的避障与导航。
进一步的，步骤S1具体包括：里程计与激光雷达外参标定、深度相机与激光雷达外参标定以及深度相机与惯性导航的外参标定，其中：
所述里程计与激光雷达外参标定方法为：以激光雷达的扫描-匹配数据作为真值，里程计测量到的数据作为测量值，通过直接线性法求解出两者之间的外参；
所述深度相机与激光雷达外参标定方法为：通过相机捕获的标定板点云拟合出标定板平面的距离，然后将激光线点云通过外参旋转到相机坐标系，再通过旋转后的激光点云到平面的距离进行优化求解相机与激光雷达之间的外参；
所述深度相机与惯性导航的外参标定方法为：通过对标定板进行角点提取计算深度相机的旋转量，通过对陀螺仪数据积分求解惯性导航的旋转量，然后通过最小二乘法求解两者之间的外参。
上述进一步方案的有益效果是，在移动车辆的定位结果最终需要统一到一个坐标系中，而不同传感器在解算位姿时所基于的坐标系均有所不同，故在融合与优化求解移动车辆位姿时需要知道各个传感器之间的相对位姿关系，即是需要对各传感器进行相互标定求取传感器之间的外参。
进一步的，步骤S2具体包括：
S21、通过轮式编码器与惯性导航获取的信息融合求解车辆在不平整路况上的位置姿态；
S22、通过激光雷达与深度相机获取的信息融合求解车辆在未知空间上的位置姿态；
S23、将步骤S21和步骤S22中求解的车辆位置姿态通过UKF融合优化，得到优化后的车辆位置姿态。
上述进一步方案的有益效果是，利用传感器信息融合方式对车辆位置和姿态进行捕捉和计算，同时进行优化。
进一步的，步骤S21中信息融合的方式为：
通过上一时刻的状态对下一时刻的状态进行预测后，采用复合位姿模型的观测量进行校正。
上述进一步方案的有益效果是，车辆运动时在遇到凹凸不平以及车轮打滑时轮式编码器就无法进行准确的解算出位置信息，此时则可以使用IMU进行数据融合提高移动车辆的定位精度。
进一步的，步骤S22具体包括：
S221、分别利用激光雷达与深度相机从位置环境中获取二维激光数据和点云图像；
S222、将步骤S221得到的点云图像进行处理得到虚拟的二维激光数据；
S223、将步骤S222得到的虚拟的二维激光数据和激光雷达测量的数据进行融合得到车辆的位置姿势。
上述进一步方案的有益效果是，通过激光传感器和深度相机对未知环境进行图像模拟，可以得到未知三维环境中的局部地图。
进一步的，步骤S3中使用最小二乘法求解车辆的最优位姿，表示为：
其中，
函数含义为使目标函数达到最小时所取得变量值，ξ为AGV车辆位姿，Tξ为转换矩阵，Tξ表示将地图点hk由当前扫描帧到局部地图中的位姿变换关系，k为地图点序号，K为地图点的个数，Msmooth表示对局部子地图中概率值进行平滑处理函数，。
上述进一步方案的有益效果是，可得到更为准确的AGV车辆定位及建图效果，且定位精度可达±2.78cm，建图精度可达±13.79cm。
进一步的，步骤S4具体包括：
S41、加载全局地图后使用2D匹配车辆的位置，并对车辆位置周围的动态障碍进行检测；
S42、在移动导航的同时进行局部地图的扫描并在局部地图中构建动态人工势力场，对移动车辆进行动态环境的避障与导航。
上述进一步方案的有益效果是，对未知三维场景建图后，利用动态人工势力场对车辆周围的未知动态物体进行检测，达到局部动态场景三维避障与导航。
进一步的，步骤S42具体包括：
S421、在传统人工势力场上加入速度项，在局部地图中构建动态人工势力场模型；
S422、对其中的动态势力场进行计算得到动态障碍物场景的势力函数；
S423、根据步骤S422得到的势力函数对障碍物场景进行势力场中的受力分析，得到车辆再动态障碍物下运动导航的总势力场。
上述进一步方案的有益效果是，
进一步的，步骤S421中构建动态人工势力场模型表示为：
其中，V为AGV速度与障碍物速度之差，krepv为速度引力因子，
为势场模型的计算公式，d0为与障碍物之间距离阈值，p为AGV的当前位置，v为AGV的当前速度，vobs为障碍物的速度，pobs为障碍物位置，
为势力场的位置分量，
为势力场的速度分量，e为表征速度差方向的单位向量。
进一步的，步骤S423中总势力场表示为：
其中，
表示总势力场，
表示斥力场，
表示引力场总和。
附图说明
图1为本发明一种无光动态开放环境下AGV车辆建图与自主导航避障方法流程示意图。
图2为本发明实施例2D激光雷达与深度相机点云融合求取定位信息流程图。
图3为本发明实施例深度图像二维化处理示意图。
图4为本发明实施例深度图像转换模块算法流程图。
图5为本发明实施例二维LiDAR和深度相机相融合的理论模型结构示意图。
图6为本发明实施例后端优化流程示意图。
图7为本发明实施例移动车辆在势力场中的运动示意图，其中a为势力场原理图，b为移动车辆受引力场的牵引示意图。
图8为本发明实施例移动车辆在动态障碍物势力场中的受力分析图。
具体实施方式
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
一种无光动态开放环境下AGV车辆建图与自主导航避障方法，如图1所示，包括如下步骤：
S1、获取多传感器参数，对多传感器进行相互标定得到多传感器之间的外参；
在移动车辆的定位结果最终需要统一到一个坐标系中，而不同传感器在解算位姿时所基于的坐标系均有所不同，故在融合与优化求解移动车辆位姿时需要知道各个传感器之间的相对位姿关系，即是需要对各传感器进行相互标定求取传感器之间的外参。
(1)里程计与激光雷达外参标定
里程计坐标系与车辆坐标系为同一坐标系，在移动车辆中由于在二维平面上运动，移动车辆与二维激光雷达均为二维运动，故两者之间只相差一个SE(2)的旋转与平移。以激光雷达的scan-match数据作为真值，里程计测量到的数据作为测量值，通过直接线性法即可求解出两者之间的外参。
(2)深度相机与激光雷达外参标定
标定时，将相机与雷达视野均指向标定板，标定时以标定板平面为z＝0建立坐标系，通过相机捕获的标定板点云拟合出标定板平面的距离，然后将激光线点云通过外参旋转到相机坐标系。则通过旋转后的激光点云到平面的距离进行优化求解相机与激光雷达之间的外参。
标定板在相机坐标系下的表示方程为：
其中，Pm＝[0 0 1 0]为标定板平面在标定板坐标系下的齐次坐标表示，
为相机与标定板之间的外参，可以通过相机标定获取。同时，激光雷达点通过外参变换到相机坐标系的关系为：
Pc i＝Tcl*Pl i (2)
其中，Pl i为激光点云，Pc i为激光点变换到的相机平面的点，Tcl为激光与相机之间的外参数。此时则可以通过点到平面的之间的距离构建目标函数进行优化求解，如式(3)所示。
(3)深度相机与惯导外参标定
标定时，相机通过对标定板进行角点提取并解算出旋转量
惯导则通过对陀螺仪数据积分解算出旋转量
此时则有：
对式(1)转换为四元数形式有：
其中，q＝[qx qy qz qw]，Q1,2(q)为将旋转矩阵表示为四元数。
为反对称矩阵，
为四元数乘法。标定时测量多组数据进行计算如式(6)。
通过最小二乘即可求解外参旋转部分，平移部分需联立视觉测量特征点深度等求解。
S2、利用步骤S1标定的多传感器外参融合2D激光雷达参数求解车辆位置姿态信息，具体而言，包括如下步骤：
S21、通过轮式编码器与惯性导航获取的信息融合求解车辆在不平整路况上的位置姿态。
车辆运动时在遇到凹凸不平以及车轮打滑时轮式编码器就无法进行准确的解算出位置信息，此时则可以使用IMU进行数据融合提高移动车辆的定位精度。融合过程采用UKF(Unscented Transform Kalman Filter)算法，UKF没有像EKF那样泰勒展开的线性化处理过程，而是通过sigmapoint通过函数投影为高斯分布。设非线性系统为：
Xt＝g(Xt-1,ut-1,wt) (7)
则其投影过程为可分为以下几个步骤：
构造Sigmapoint集合
对于N维高斯分布，需要选2N+1个Sigmapoint，映射后的高斯分布规则为：
其中，μ为均值，Σ为协方差，λ＝α2(n+k)-n，α,k为确定σ分布的相关参数。
Sigmapoint变换
Sigma point通过g(*)变换，来进行映射至高斯分布：
y[i]＝g(χ[i]) (9)
输出均值与方差
映射后的高斯参数为y[i]所决定：
其中，
为协方差权重，
表示均值权重，α,β为sigmapoint高斯分布参数。
SLAM中定位问题可以描述为一个运动方程与一个观测方程：
其中，xk为车辆状态，uk为激励输入，yk为路标点观测，wk,nk为噪声。SLAM问题则是通过uk与yk求取xk的过程，其中f(*),g(*)为非线性系统。在通过UKF变换后满足高斯分布则式(12)变换为线性系统描述：
显然，Qk,Rk为噪声项的协方差，A，表示状态转移矩阵与观测矩阵。对于线性高斯最优无偏估计则可以使用卡尔曼滤波实现：
式(14)中，前两式为预测方程，第三个为卡尔曼增益计算，后两式为校正方程。通过上一时刻的状态进行预测下一时刻的状态后，采用复合位姿模型的观测量即轮式里程计测量模型进行校正，从而达到融合的效果。
S22、通过激光雷达与深度相机获取的信息融合求解车辆在未知空间上的位置姿态；
本环节融合感知大致可以描述为：第一，LiDAR传感器与深度相机分别从未知的环境测量获取二维激光数据以及点云图像；第二，使用深度点云图转换模块将深度相机获取的点云进行处理得到虚拟的二维激光数据；第三，将虚拟的二维激光数据和LiDAR传感器测量的数据进行融合得到系统最终测量值。本环节操作框架如图2所示，具体而言，
S221、分别利用激光雷达与深度相机从位置环境中获取二维激光数据和点云图像。
整个观测系统中，深度图转换模块将深度相机测量的深度图图像指定待转换窗口中像素列深度值的最小像素点距离以及角度分离出来，最后得到的效果可以看作是将图像的部分或所有深度图朝图像中心平面压缩，从而得到虚拟二维LiDAR数据。转换窗口如图3所示。其中，S为转换像素行起始位置，H为窗口高度，窗口高度根据移动车辆的物理尺寸进行设计，从而达到移动车辆识别三维障碍物的目的。o-xy和o-uv分别为图像坐标系和像素坐标系，O(Cx,Cy)代表o-xy的原点在o-uv下的坐标。图4为转换算法流程，通过深度相机获取深度图像后，粗略计算窗口高度H，然后获取深度图像转换窗口的每列像素中深度值最小的像素点，计算该像素点的索引编号及与相机中心的水平距离。最后按图4算法流程进行点云投影。
S222、将步骤S221得到的点云图像进行处理得到虚拟的二维激光数据。
如图2流程所示，伪激光数据还需要与二维激光数据进行融合然后输出栅格地图。该数据融合模块的理论模型如图5所示。设αmax和αmin分别为深度相机测量数据的最大角度和最小角度，βmax和βmin分别为深度相机虚拟二维LiDAR数据测量的最大角度和最小角度，则有：
图5为两个传感器数据融合的理论模型，K与L分别为深度相机和LiDAR中心点，P为深度相机测量未知环境中的障碍物。假设深度相机和LiDAR水平间距为|LK|＝d|，深度相机最大测量距离为D且第i个虚拟二维LiDAR检测到障碍物为P点，此处i的数据范围表示为：
0≤i≤(βmax-βmin)/rk (16)
其中，rk为深度相机虚拟二维数据的角增量。假设∠xLP＝α,∠xOP＝β,dk表示深度相机第i束虚拟激光测量的障碍物距离，dij表示LiDAR的第j束激光测量的障碍物距离，ri表示LiDAR的增量角，dfj表示深度相机LiDAR数据融合后的第j束激光测量的障碍物距离。当P在x轴的右侧或在x轴上时，则有：
β＝βmin+i×rk (17)
|OA|＝dkl×sin|β| (18)
|AP|＝dkl×cos|β| (19)
|PB|＝|AP|-|LK| (20)
tan|α|＝|LP|/|PB| (21)
α＝-arctan(|LB|/|PB|) (22)
假设LiDAR扫描的第j个激光束与射线LP方向相同，符号[]表示对浮点型测量数据进行取整运算，则有：
j＝[α-αmin/rl] (24)
若|LP|≤dLj且dki<D，成立，则有：dfj＝|LP|；反之，则有：dfj＝dlj。
同理可得，当P位于x轴左侧时，有：
|LP|≤dlj (26)
最后，dfi(i＝0,1,2…,N-1)表示深度相机和LiDAR数据融合后的最终测量数据。
S223、将步骤S222得到的虚拟的二维激光数据和激光雷达测量的数据进行融合得到车辆的位置姿势。
点云求取位姿与激光雷达位姿信息融合同样采取步骤二中的UKF方式进行融合，激光点云与相机点云分别匹配求取移动车辆位姿，需要注意的是相机点云数据量庞大，为了减少计算量，匹配前先试用双边滤波进行数据量降维，然后通过UKF进行融合。
S23、将步骤S21和步骤S22中求解的车辆位置姿态通过UKF融合优化，得到优化后的车辆位置姿态，其融合过程同样采用UKF融合。
S3、对步骤S2求解的车辆位置姿态信息进行检测及优化，通过优化后的位姿关系对格栅地图进行调整，得到未知三维场景的全局地图；
回环检测主要通过相机点云进行实现，通过ScanContext算法进行点云帧的描述与回环匹配。同时对前端所得的移动车辆系统因子图通过图优化进行局部地图的调整与优化。
因子图由节点和边组成，节点为待求解的车辆状态量，边为邻近帧或回环检测所提供的约束。然后通过约束构建残差使用非线性优化进行求解。同时通过优化后的位姿关系对栅格地图进行调整。
在本实施例里，局部位姿求解阶段，记当前扫描帧的位姿为ξ，在局部地图中依据匹配关系，使用最小二乘法求解车辆最优位姿，优化项如下：
其中，Tξ为转换矩阵，表示将地图点hk由当前扫描帧到局部地图中的位姿变换关系。函数
的作用是对局部子地图中概率值进行平滑处理。
在闭环检测阶段，对新来的扫描帧搜索其最优匹配帧，若该最优匹配帧符合要求，则认为检测到闭环。并构建非线性最小二乘问题如下。
其中，
表示扫描帧位姿，
为闭环约束信息，这些约束以以相对位姿约束ξij及相关协方差矩阵∑ij的形式给出，E为残差块，ρ为损失参数，本设计使用Huber损失函数。
经由，步骤S3中的一系列优化算法，可得到更为准确的AGV车辆定位及建图效果，且定位精度可达±2.78cm，建图精度可达±13.79cm。
S4、扫描构建局部地图融合步骤S3得到的位置三维场景的地图对车辆进行动态环境的避障与导航：
局部静态场景中的障碍物则可以视为虚拟斥力场，移动车辆在导航运动过程中所需要达到的目的点则为引力场，其沿着势力场梯度下降方向运动达到目的地。如图7所示。
势力场中引力方向为车辆指向目的点，障碍物则表示为斥力如图7(b)所示。势力场函数状态量则为距离，引力场函数如式(27)所示
式(27)中pgoal为目标点，P为移动车辆当前所在位置，m为函数参数，katt为常量，显然p-pgoal为两点之间的距离，式(28)为式(27)求导所得，其物理意义为移动车辆与目标点之间的距离越近引力越小。
斥力场函数如式(29)所示。
式(29)中pobs为障碍物位置，krep常量，d0为障碍物相对移动车辆的影响的半径，显然p-pobs同样为两点之间的距离。同理对斥力场势函数求导如式(30)所示。
其物理意义同理可知，综合式(27)与式(29)可得总的势函数如式(31)所示。
对式(31)求导可得：
局部静态场景中的障碍物视为虚拟势力场可以进行很好的导航，但实际环境中常会出现一些动态物体，此时按照加载的全局地图通过人工势力场进行导航则可能会失败。为解决移动车辆在动态场景中的避障导航，本发明从两个方面入手对动态场景的移动车辆的导航进行改进设计。一是采用紧急安全防范模块进行局部动态障碍物进行检测，二是在移动导航时同时进行局部地图的扫描构建并对传统人工势力场进行改进进而达到在动态场景中安全有效的运动目的。
若局部地图中扫描到障碍物是动态的，则障碍物所构建的斥力场也必须是动态的而引力场可无需改变，考虑进障碍物的移动后构建的人工势力场模型如式(33)所示。
式(33)中，v-vobs即为移动车辆与动态障碍物之间的相对速度，此时的斥力场势力函数状态则为距离与速度两个。此时对动态斥力场进行求导如式(34)所示。
令：
式中，
及
为斥力场函数对函数两个状态量即速度与位置的偏导，通过上几式可得动态障碍物场景的势力函数，如式(36)所示。
最后得到移动车辆在动态障碍物下运动导航的总势力场如式(37)所示。
其中，
表示总势力场，
表示斥力场，
表示引力场总和。
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。
Claims (10)
1.一种无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，包括如下步骤：
S1、获取多传感器参数，对多传感器进行相互标定得到多传感器之间的外参；
S2、利用步骤S1标定的多传感器外参融合2D激光雷达参数求解车辆位置姿态信息；
S3、对步骤S2求解的车辆位置姿态信息进行检测及优化，通过优化后的位姿关系对格栅地图进行调整，得到未知三维场景的全局地图；
S4、扫描构建局部地图融合步骤S3得到的位姿三维场景的地图对车辆进行动态环境的三维避障与导航。
2.根据权利要求1所述的无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S1具体包括：里程计与激光雷达外参标定、深度相机与激光雷达外参标定以及深度相机与惯性导航的外参标定，其中：
所述里程计与激光雷达外参标定方法为：以激光雷达的扫描-匹配数据作为真值，里程计测量到的数据作为测量值，通过直接线性法求解出两者之间的外参；
所述深度相机与激光雷达外参标定方法为：通过相机捕获的标定板点云拟合出标定板平面的距离，然后将激光线点云通过外参旋转到相机坐标系，再通过旋转后的激光点云到平面的距离进行优化求解相机与激光雷达之间的外参；
所述深度相机与惯性导航的外参标定方法为：通过对标定板进行角点提取计算深度相机的旋转量，通过对陀螺仪数据积分求解惯性导航的旋转量，然后通过最小二乘法求解两者之间的外参。
3.根据权利要求2所述的无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S2具体包括：
S21、通过轮式编码器与惯性导航获取的信息融合求解车辆在不平整路况上的位置姿态；
S22、通过激光雷达与深度相机获取的信息融合求解车辆在未知空间上的位置姿态；
S23、将步骤S21和步骤S22中求解的车辆位置姿态通过UKF融合优化，得到优化后的车辆位置姿态。
4.根据权利要求3所述的无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S21中信息融合的方式为：
通过上一时刻的状态对下一时刻的状态进行预测后，采用复合位姿模型的观测量进行校正。
5.根据权利要求4所述的一种无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S22具体包括：
S221、分别利用激光雷达与深度相机从位置环境中获取二维激光数据和点云图像；
S222、将步骤S221得到的点云图像进行处理得到虚拟的二维激光数据；
S223、将步骤S222得到的虚拟的二维激光数据和激光雷达测量的数据进行融合得到车辆的位置姿势。
6.根据权利要求5所述的无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S3中使用最小二乘法求解车辆的最优位姿，表示为：
其中，
函数含义为使目标函数达到最小时所取得变量值，ξ为AGV车辆位姿，Tξ为转换矩阵，Tξ表示将地图点hk由当前扫描帧到局部地图中的位姿变换关系，k为地图点序号，K为地图点的个数，Msmooth表示对局部子地图中概率值进行平滑处理函数。
7.根据权利要求6所述的无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S4具体包括：
S41、加载全局地图后使用2D匹配车辆的位置，并对车辆位置周围的动态障碍进行检测；
S42、在移动导航的同时进行局部地图的扫描并在局部地图中构建动态人工势力场，对移动车辆进行动态环境的避障与导航。
8.根据权利要求7所述的一种无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S42具体包括：
S421、在传统人工势力场上加入速度项，在局部地图中构建动态人工势力场模型；
S422、对其中的动态人工势力场进行计算得到动态障碍物场景的势力函数；
S423、根据步骤S422得到的势力函数对障碍物场景进行势力场中的受力分析，得到车辆在动态障碍物下运动导航的总势力场。
9.根据权利要求8所述的一种无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S421中构建动态人工势力场模型表示为：
其中，V为AGV速度与障碍物速度之差，krepv为速度引力因子，
为势场模型的计算公式，d0为与障碍物之间距离阈值，p为AGV的当前位置，v为AGV的当前速度，vobs为障碍物的速度，pobs为障碍物位置，
为势力场的位置分量，
为势力场的速度分量，e为表征速度差方向的单位向量。
10.根据权利要求9所述的一种无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S423中总势力场表示为：
其中，
表示总势力场，
表示斥力场，
表示引力场总和。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111072340.1A CN113776519B (zh) | 2021-09-14 | 2021-09-14 | 一种无光动态开放环境下agv车辆建图与自主导航避障方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111072340.1A CN113776519B (zh) | 2021-09-14 | 2021-09-14 | 一种无光动态开放环境下agv车辆建图与自主导航避障方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113776519A true CN113776519A (zh) | 2021-12-10 |
| CN113776519B CN113776519B (zh) | 2022-10-21 |
Family
ID=78843284
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111072340.1A Active CN113776519B (zh) | 2021-09-14 | 2021-09-14 | 一种无光动态开放环境下agv车辆建图与自主导航避障方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113776519B (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114199240A (zh) * | 2022-02-18 | 2022-03-18 | 武汉理工大学 | 无gps信号下二维码、激光雷达与imu融合定位系统及方法 |
| CN116339347A (zh) * | 2023-04-24 | 2023-06-27 | 广东工业大学 | 一种无人车运行路径规划方法、装置及设备 |
| CN117232539A (zh) * | 2023-11-16 | 2023-12-15 | 山东新坐标智能装备有限公司 | 基于激光雷达的轻量化环境感知地图构建方法 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101408772A (zh) * | 2008-11-21 | 2009-04-15 | 哈尔滨工程大学 | Auv智能避碰装置及避碰方法 |
| CN110262495A (zh) * | 2019-06-26 | 2019-09-20 | 山东大学 | 可实现移动机器人自主导航与精确定位的控制系统及方法 |
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN111983639A (zh) * | 2020-08-25 | 2020-11-24 | 浙江光珀智能科技有限公司 | 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法 |
| CN112484725A (zh) * | 2020-11-23 | 2021-03-12 | 吉林大学 | 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 |
| DE102019130204A1 (de) * | 2019-11-08 | 2021-05-12 | Automotive Research & Testing Center | Verfahren und System zum Erstellen dynamischer Karteninformation, die zum Bereitstellen von Umgebungsinformationen geeignet ist |
-
2021
- 2021-09-14 CN CN202111072340.1A patent/CN113776519B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101408772A (zh) * | 2008-11-21 | 2009-04-15 | 哈尔滨工程大学 | Auv智能避碰装置及避碰方法 |
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN110262495A (zh) * | 2019-06-26 | 2019-09-20 | 山东大学 | 可实现移动机器人自主导航与精确定位的控制系统及方法 |
| DE102019130204A1 (de) * | 2019-11-08 | 2021-05-12 | Automotive Research & Testing Center | Verfahren und System zum Erstellen dynamischer Karteninformation, die zum Bereitstellen von Umgebungsinformationen geeignet ist |
| CN111983639A (zh) * | 2020-08-25 | 2020-11-24 | 浙江光珀智能科技有限公司 | 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法 |
| CN112484725A (zh) * | 2020-11-23 | 2021-03-12 | 吉林大学 | 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 |
Non-Patent Citations (4)
| Title |
|---|
| DONG WANG等: "Research on SLAM of Unmanned Platform Based on the Fusion of Lidar and Depth Camera", 《2021 THE 3RD WORLD SYMPOSIUM ON ARTIFICIAL INTELLIGENCE》 * |
| 周阳: "基于多传感器融合的移动机器人SLAM算法研究", 《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》 * |
| 孟凡利: "基于视觉与激光融合的SLAM系统的设计与实现", 《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》 * |
| 陈军: "基于视觉_惯性_激光融合感的实时6DOF定位与3D环境建模研究", 《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》 * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114199240A (zh) * | 2022-02-18 | 2022-03-18 | 武汉理工大学 | 无gps信号下二维码、激光雷达与imu融合定位系统及方法 |
| CN116339347A (zh) * | 2023-04-24 | 2023-06-27 | 广东工业大学 | 一种无人车运行路径规划方法、装置及设备 |
| CN116339347B (zh) * | 2023-04-24 | 2023-10-31 | 广东工业大学 | 一种无人车运行路径规划方法、装置及设备 |
| CN117232539A (zh) * | 2023-11-16 | 2023-12-15 | 山东新坐标智能装备有限公司 | 基于激光雷达的轻量化环境感知地图构建方法 |
| CN117232539B (zh) * | 2023-11-16 | 2024-02-27 | 苏州新坐标智能装备有限公司 | 基于激光雷达的轻量化环境感知地图构建方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113776519B (zh) | 2022-10-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113776519B (zh) | 一种无光动态开放环境下agv车辆建图与自主导航避障方法 | |
| US11536572B2 (en) | Method and system for accurate long term simultaneous localization and mapping with absolute orientation sensing | |
| CN111338383B (zh) | 基于gaas的自主飞行方法及系统、存储介质 | |
| KR20200043006A (ko) | 위치 추정 장치 및 방법 | |
| CN113674412B (zh) | 基于位姿融合优化的室内地图构建方法、系统及存储介质 | |
| Dawood et al. | Harris, SIFT and SURF features comparison for vehicle localization based on virtual 3D model and camera | |
| Deng et al. | Global optical flow-based estimation of velocity for multicopters using monocular vision in GPS-denied environments | |
| Niewola et al. | PSD–probabilistic algorithm for mobile robot 6D localization without natural and artificial landmarks based on 2.5 D map and a new type of laser scanner in GPS-denied scenarios | |
| Pang et al. | Low-cost and high-accuracy LiDAR SLAM for large outdoor scenarios | |
| Khoshelham et al. | Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry | |
| Park et al. | Least-square matching for mobile robot SLAM based on line-segment model | |
| Wu et al. | Precise pose graph localization with sparse point and lane features | |
| CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
| Wang et al. | Micro aerial vehicle navigation with visual-inertial integration aided by structured light | |
| Soleimani et al. | A disaster invariant feature for localization | |
| Wei et al. | Horizontal/vertical LRFs and GIS maps aided vehicle localization in urban environment | |
| Aggarwal | Machine vision based SelfPosition estimation of mobile robots | |
| Kim et al. | Vision-based map-referenced navigation using terrain classification of aerial images | |
| Liu et al. | 6-DOF motion estimation using optical flow based on dual cameras | |
| Liu et al. | Laser 3D tightly coupled mapping method based on visual information | |
| Zhu et al. | Terrain‐inclination‐based Three‐dimensional Localization for Mobile Robots in Outdoor Environments | |
| Fitzgibbons et al. | Application of vision in simultaneous localization and mapping | |
| Liu et al. | Error modelling and optimal estimation of laser scanning aided inertial navigation system in GNSS-denied environments | |
| Yang et al. | Laser radar based vehicle localization in GPS signal blocked areas | |
| Andert et al. | A fast and small 3-d obstacle model for autonomous applications |
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 |