CN106569225A - 一种基于测距传感器的无人车实时避障方法 - Google Patents

Abstract

本发明公开了一种传感器无关的实时避障算法，以来自双目相机、激光雷达、超声波等距离传感器的点云数据为计算数据源，通过拟合无人车底盘参考平面信息而得到障碍信息。提出了一个称为虚拟力的概念，并以虚拟力为计算框架，计算出无人车的安全行驶方向。根据行驶方向改变的剧烈程度，动态自适应地设置无人车的线运动速度。

Description

一种基于测距传感器的无人车实时避障方法

技术领域

本发明属于人工智能领域，涉及传感器无关的实时避障方法。

背景技术

避障行为是移动机器人必要的功能，是其智能性的重要体现之一。即使是一个没有明确任务或目标的移动机器人，也需要可靠的避障行为来保证自身的安全性。对于实时避障问题，给定实时的传感器信息，避障意味着为避免碰撞调整机器人路径规划系统给出的运动轨迹。避障规划算法不同程度上依赖着全局地图的存在性和相对于地图的定位信息。

典型的避障算法有最简单的Bug算法，向量场直方图算法(VFH)及其改进VFH+算法和VFH*算法，气泡带算法，曲率速度算，动态窗口算法，Schlegel避障算法等。Bug算法其思想是在机器人路途中，跟踪障碍物的轮廓，从而绕开障碍物。它简单易用，但没有考虑机器人的动力学，同时因为只使用最近的传感器数据，因此在许多情况下不是鲁棒的。VFH算法通过创建围绕机器人环境的局部地图，克服了Bug算法只使用最近传感信息的限制，可以使机器人具有通过窄通道的能力。在VFH+改进中，机器人运动学的约束被纳入计算过程。气泡带算法的思想是对Khatib和Quinlan提出的弹性带概念中非完备无人车的一种扩展。气泡被定义为围绕一个给定的机器人方位，其自由空间的最大局部子集，允许以任意方向行走于此区域而无碰撞。其优点是允许考虑机器人的实际尺寸，适用性强。但是计算气泡带需要全局地图和全局路径规划器，同时，其只当环境方位事先已很好知道的情况下才最适合于应用。基本的曲率速度方法(CVM)由Simmons提出，其能在避障期间考虑机器人的运动学约束，甚至考虑到部分的动力学约束。其不足则在于将障碍物的形状简化成圆形，在一些环境中，这是不可接受的。此外，由于CVM方法不使用先验知识，所以可能遭受局部极小问题。后来提出的道路曲率方法(LCM)具有比CVM算法更好的性能。Fox，Burgard和Thrun的局部动态窗口法，通过搜索一个离线选择的速度空间，考虑了机器人的运动学。Brock和Khatib的全局动态窗口法在Fox等人的局部动态窗口法上加上了全局思考，其以高速保证实时、动态约束、全局思考和最小无避障。Schlegel避障方法考虑动力学以及机器人的实际形状的方法。其利用笛卡尔栅格表示环境中的障碍物，具有很好的实时性。上述算法多数依赖全局或半全局的环境地图，而未知环境下的路径规划和实时避障，是一项具有挑战性的任务，需要感知系统在线感知的支持，如何在信息不完全的情况下保证机器人系统安全运行，同时探索环境，迭代地优化行走路径，并规避非结构环境下的动态障碍物，是本专利试图解决的问题。

考虑到避障行为是一种反应式行为，因此，本发明的设计思路是采用原始的点云信息作为障碍分布模型，而不是像传统的做法一样建立局部的障碍分布图。

发明内容

本发明所要解决的技术问题是提供一种传感器无关的实时避障算法，以克服传统避障算法对先验环境信息的过度依赖问题。为此，本发明提供以下技术方案：

一种基于测距传感器的无人车实时避障方法，包括以下步骤：

步骤(1)：由无人车搭载的测距传感器采集点云信息或采用双目相机采集点云信息；

若采用双目相机采集，需要计算才能得到点云信息，而后，将得到的点云信息缓存为点云向量，所有点云向量的坐标均参考以无人车中心为原点、前方为x轴的正方向、左方为y轴正方向的本体坐标系；

步骤(2)：拟合所述无人车的底盘所在的参考平面信息，视该参考平面为可通行面，从而，高于这个参考平面的点云即为阻碍无人车通行的障碍物；从所获得的点云中拟合得到参考平面方程；

步骤(3)：基于上述的障碍物，采取虚拟力方法计算最优行驶方向，虚拟力的含义是将每个距离点在无人车本体坐标系下的位置矢量虚拟化为一个力矢量，定义如下：

其中，|F_i|表示力矢量的模值，f(·)表示力矢量是位置矢量的函数，R为测距传感器测量得到的距离值；R_thresh为测距传感器最大感知半径；

若某个方向返回的位置矢量半径大于阈值半径R_thresh，那么，该方向的阈值半径更新为此力矢量模值；通过对所有位置矢量求和，可得到一个最优行驶方向，并作为角度控制量，角度控制量的计算步骤如下：

参考步骤(1)中所述的本体坐标系，分别计算x正方向和y正方向的力矢量的和，其中，力矢量的定义为(*)式所示：

其中，N为障碍点云序列的长度，r_i为障碍点云的距离值，θ_i为障碍点云所在方向的角度；

归一化虚拟力：

计算最优行驶方向，并作为角度控制量：

步骤(4)：根据角度控制量，计算当前的平移速度v，即采用如下计算方法：

其中，v_max是允许的最大行驶线速度，k_p为比例系数；当方向偏角dir为零时，线速度达到这个值。

在上述技术方案的基础上，本发明还可以采用一下进一步的技术方案：

其中所述步骤(1)中，包括采用激光雷达作为测距传感器获取点云。

其中所述步骤(1)中，采用双目立体视觉获取点云：计算出双目图像中满足视差范围容许值的绝对差最小匹配点对，作为匹配点，根据双目相机的参数和三角形相似原理，即可计算出匹配点对应的空间点的三维坐标，从而获得点云。

由于采用本发明的技术方案，本发明的有益效果为：本发明计算过程针对的仅仅是点云信息，因此与传感器的种类无关，所以在实际的工程应用中，方法极易封装成为可复用模块，与能提供点云信息的传感器配合使用即可。

本发明提出了虚拟力这一概念，将点云的空间距离值或空间距离值的二次方作为力矢量，吸引无人车向更开阔的地带行驶，从而提供了性能稳定、实时性良好的避障功能，由于避障半径可调，因此，预警性强，适用于各种场景。

附图说明

图1是本发明中激光雷达坐标系统下的无人车系统传感器安装示意图；

O_cX_cY_cZ_c：双目相机坐标系；

O_lX_lY_l：激光雷达坐标系；

O_rX_rY_rZ_r：无人车本体坐标系；

O_wX_wY_wZ_w：世界坐标系。

图2是本发明中激光雷达坐标系统下的标准极坐标系统图；

图3是本发明中激光雷达数据范围示意图；

Deg：激光雷达扫描的角度范围，单位是度；

Step：激光雷达数据的步进范围，无量纲；

Index：激光雷达点云向量的索引范围，无量纲。

图4是本发明中双目相机立体成像过程示意图；

图5是本发明中外极限平面示意图；

x：空间点；

x1：空间点在左图像的投影点；

x2：空间点在右图像的投影点；

c1：左相机的光心；

c2：右相机的光心。

图6是本发明中地面拟合示意图；

图7是本发明中虚拟力算法原理图示意图。

具体实施方式

为了更好地理解本发明的技术方案，以下结合附图作进一步描述。

步骤(1)：由无人车搭载的测距传感器采集点云信息或采用双目相机采集点云信息；

若采用双目相机采集，需要计算才能得到点云信息，计算包括图像校正计算、双目匹配计算、三角距离测量计算，而后，将得到的点云信息缓存为点云向量，所有点云的坐标均参考以无人车中心为原点、前方为x轴、左方为y轴的本体坐标系。

步骤(1)中：包括采用激光雷达获取点云：如图1所示，虚线部分为水平安装在无人车前部的平面激光雷达的参考坐标系O_lX_lY_l。采用标准的如图2所示的标准极坐标系统，即正前方为x轴正方向，左侧为y轴正方向。激光雷达以某一固定角度步长每次扫描周围一定角度范围(如270°)、一定距离范围(如30米以内)，当光线照射至物体时，即可返回该处的角度和距离信息。如此持续扫描，就可获得连续的二维点云信息。

这里，点云的步进范围为[-540,540]，即每一帧距离-角度点云数据的容量是1080，对应的角度范围为[-135°,135°]。如图3所示。

步骤(1)中：双目立体视觉获取点云；双目立体视觉三维测量基于视差原理。如图4所示。基线长度B是左右相机投影中心连线的距离。双目相机的焦距为f。左右相机同一时刻拍摄空间物体同一点P(x_p,y_p,z_p)，并分别在左右图像上生成像点P_l(X_l,Y_l)和P_r(X_r,Y_r)。其中，x_p、y_p、z_p是空间点在步骤(1)中所述本体坐标系下的三维坐标；X_l、Y_l、X_r、Y_r分别是空间点的左右图像坐标。

经过双目校正的两摄像机的图像可认为位于同一平面(外极线平面)，如图5所示。从而，点P的图像坐标Y相同，即有Y_l＝Y_r＝Y。则由三角形相似原理有如下关系：

从而，可以计算视差Disp：

Disp＝X_l-X_r。

由此，可计算点P在相机坐标系下的三维坐标为：

因此，只要能得到左右相机图像平面上的匹配点，即可计算出匹配点对应的空间点的三维坐标。

为了保证实时控制系统的计算性能，图像对应点的匹配采用快速的绝对差相关算法(SSD)计算图相对的匹配点，计算公式如下：

其中，

d_max和d_min分别时最小和最大视差；

m是方形模板的尺寸，单位是像素；

I_l和I_r分别是双目相机的左右图像。

从而，只要计算出满足视差范围容许值的绝对差最小的匹配对，就可以认为匹配成功。

步骤(2)拟合所述无人车底盘所在的参考平面信息，视该参考面为可通行面，从而，高于这个参考面的点云序列即为阻碍无人车通行的障碍物；采用基于随机采样的RANSAC算法，从所获得的点云向量中拟合得到地平面方程。

步骤如下：

(2.a)随机选取3个三维点；

(2.b)检测3点是否共线，若共线，则重新选取；

(2.c)估计出一个平面方程

(2.d)统计点云序列中有多少三维点符合这一平面方程，记这个数量为K；

(2.e)若K足够大，那么，接受这个平面方程作为地面模型，并退出本算法；

(2.f)重复执行以上步骤L次；

(2.g)若运行至此处，那么，当前点云序列中无法拟合出一个合适的平面模型Π。

利用上述的平面方程，判断每个点云是否属于可通行区域，即若一个点云距离地平面足够近，那么，认定这个点不属于障碍物体。计算点到平面的距离采用如下公式：

依次评估每个点云，即得到障碍物点云簇。

步骤(3)：基于上述的障碍物，采取虚拟力方法计算最优行驶方向，虚拟力的含义是将每个距离点在无人车本体坐标系下的位置矢量虚拟化为一个力矢量，定义如下：

其中，|F_i|表示力矢量的模值，f(·)表示力矢量是位置矢量的函数，R为测距传感器测量得到的距离值；R_thresh为测距传感器最大感知半径；

若某个方向返回的位置矢量半径大于阈值半径R_thresh，那么，该方向的阈值半径更新为此力矢量模值；通过对所有位置矢量求和，可得到一个最优行驶方向，并作为角度控制量，角度控制量的计算步骤如下：

(3.a)计算x方向和y方向的虚拟力。

其中，L为障碍点云序列的长度。

(3.b)归一化虚拟力。

计算最优行驶方向，并作为角度控制量。

步骤(4)：根据避障角度，计算当前的平移速度，转弯时慢转，直行时快速行驶，即采用如下计算方法：

其中，v_max是允许的最大行驶线速度，当方向偏角Δθ为零时，线速度达到这个值，为了保证无人车系统的安全，当障碍物的距离小于某一阈值d_min时，行驶线速度将为零。

Claims (3)

1.一种基于测距传感器的无人车实时避障方法，其特征在于，包括以下步骤：

步骤(1)：由无人车搭载的测距传感器采集点云信息或采用双目相机采集点云信息；

若采用双目相机采集，需要计算才能得到点云信息，而后，将得到的点云信息缓存为点云向量，所有点云向量的坐标均参考以无人车中心为原点、前方为x轴的正方向、左方为y轴正方向的本体坐标系；

步骤(2)：拟合所述无人车的底盘所在的参考平面信息，视该参考平面为可通行面，从而，高于这个参考平面的点云即为阻碍无人车通行的障碍物；从所获得的点云中拟合得到参考平面方程；

步骤(3)：基于上述的障碍物，采取虚拟力方法计算最优行驶方向，虚拟力的含义是将每个距离点在无人车本体坐标系下的位置矢量虚拟化为一个力矢量，定义如下：

$\left|F_i\right|=\left\{\begin{array}{c}f\left(R\right),R\≤R_{thresh}\\ f\left(R_{thresh}\right),R>R_{thresh}\end{array}\right.---(*)$

其中，|F_i|表示力矢量的模值，f(·)表示力矢量是位置矢量的函数，R为测距传感器测量得到的距离值；R_thresh为测距传感器最大感知半径；

若某个方向返回的位置矢量半径大于阈值半径R_thresh，那么，该方向的阈值半径更新为此力矢量模值；通过对所有位置矢量求和，可得到一个最优行驶方向，并作为角度控制量，角度控制量的计算步骤如下：

参考步骤(1)中所述的本体坐标系，分别计算x正方向和y正方向的力矢量的和，其中，力矢量的定义为(*)式所示：

$F_x=\underset{i=1}{\overset{N}{\Σ}}{r_i}^{2}cos\left({\mathrm{\θ}}_i\right)$

$F_y=\underset{i=1}{\overset{N}{\Σ}}{r_i}^{2}sin\left({\mathrm{\θ}}_i\right)$

其中，N为障碍点云序列的长度，r_i为障碍点云的距离值，θ_i为障碍点云所在方向的角度；

归一化虚拟力：

${F_x}^n=\frac{F_x}{\sqrt{F_x^2+F_y^2}}$

${F_y}^n=\frac{F_y}{\sqrt{F_x^2+F_y^2}}$

计算最优行驶方向，并作为角度控制量：

$dir={\tan }^{-1}(F_x^n,F_y^n);$

步骤(4)：根据角度控制量，计算当前的平移速度v，即采用如下计算方法：

$v=\left\{\begin{array}{c}v=\frac{v\_max}{\sqrt{\left|k_P\·dir\right|+1}},d>dmin\\ 0,d\≤d\_min\end{array}\right.;$

其中，v_max是允许的最大行驶线速度，k_p为比例系数；当方向偏角dir为零时，线速度达到这个值。

2.如权利要求1所述的一种基于测距传感器的无人车实时避障方法，其特征在于，其中所述步骤(1)中，包括采用激光雷达作为测距传感器获取点云。

3.如权利要求1所述的一种基于测距传感器的无人车实时避障方法，其特征在于，其中所述步骤(1)中，采用双目立体视觉获取点云：计算出双目图像中满足视差范围容许值的绝对差最小匹配点对，作为匹配点，根据双目相机的参数和三角形相似原理，即可计算出匹配点对应的空间点的三维坐标，从而获得点云。