CN106548486B - 一种基于稀疏视觉特征地图的无人车位置跟踪方法 - Google Patents

Abstract

本发明公开了一种基于稀疏视觉特征地图的无人车位置跟踪方法。在已经探索过的野外环境中，由用户指定环境地图中的某一位置相对于地图的坐标。基于已建立的稀疏视觉特征地图提供的全局定位信息，生成目标位置跟踪控制量，控制无人车自动向目标位置行驶。与此同时，利用无人车搭载的距离传感器实时感知周围的障碍信息，生成局部避障控制量。通过融合目标位置跟踪和避障控制两个模块的控制量，实现无人车安全的位置跟踪控制。

Description

一种基于稀疏视觉特征地图的无人车位置跟踪方法

技术领域

本发明属于移动机器人自主导航技术领域，特别是基于稀疏视觉特征地图的无人车位置跟踪方法。

背景技术

随着移动机器人技术的发展，针对非结构化场景自主建立环境地图，并基于所建立的环境地图实现安全的导航控制，成为越来越迫切的核心需求，是达成移动机器人高层次作业任务的基础支撑。为了提高建图的效率，一般的做法是提取环境的稀疏特征信息进行运算，最终生成的地图也是稀疏的表达形式，从而难以直接用于自主移动平台的路径规划和控制。

发明内容

本发明所要解决的技术问题是提供一种基于稀疏视觉特征地图的无人车位置跟踪方法，以解决稀疏环境地图难以集成到规划控制系统的问题。为此，本发明提供以下技术方案：

一种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，包括以下步骤：

步骤1，启动设有避障探索模块的无人车：设置无人车为随机探索环境的自动运动模式,启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以二进制文件形式将稀疏视觉特征地图数据存储至车载计算机本地磁盘，构成稀疏视觉特征地图数据库；

步骤2，从无人车载计算机的本地磁盘读取地图数据文件至内存，设置为定位跟踪模式；定位成功后，接收用户设置的目标位置坐标信息；按如下步骤，有位置关系变化后，反复尝试重定位：

2.1.当没有成功重定位时，由所述避障探索模块生成随机控制量控制无人车运动；

2.2.运动后采集新的场景图像，与稀疏视觉特征地图数据库中的场景进行匹配；

2.3.匹配成功后进行位姿估计，计算无人车相对于地图的相对位置关系，实现重定位；

步骤3：计算目标位置跟踪控制角：设无人车中心为本体坐标系的原点，前方为z轴的正方向，右方为x轴正方向，建立本体坐标系；设无人车启动时的无人车中心为全局坐标系的原点，前方为z轴的正方向，右方为x轴正方向，建立世界坐标系；根据当前定位信息和目标位置信息，计算目标位置跟踪控制角：设置无人车在世界坐标系下的当前位置为x_curr，z_curr和朝向h_curr，目标位置在世界坐标系下的坐标为x_t和z_t，可得到目标位置跟踪控制角，将目标位置变换至本体坐标系，计算公式如下：

x_{in_cam}＝(x_t-x_curr)·sin(h_curr)-(z_t-z_curr)·cos(h_curr)

z_{in_cam}＝(x_t-x_curr)·cos(h_curr)+(z_t-z_curr)·sin(h_curr)

其中，x_{in_cam}，z_{in_cam}是目标位置在本体坐标系下的坐标。

计算参考向量v_x，v_z，计算公式如下：

v_x＝x_{in_cam}-0

v_z＝z_{in_cam}-0

计算目标位置跟踪控制角θ_ctrl，计算公式如下：

θ_ctrl＝tan^-1(v_x,v_z)；

步骤4：根据车载距离传感器的障碍探测信息，计算局部的避障控制角；

步骤5：加权融合目标位置跟踪控制角和局部的避障控制量，并将融合后的控制量发送给无人车执行，直至最终安全抵达目标位置；

采用如下方式融合两个控制量，实现安全的目标跟踪：

θ＝w₁θ_obs+w₂θ_t；

其中，w₁、w₂为权重系数，且有w₁+w₂＝1.0；θ_obs是局部避障控制角，θ_t是目标位置跟踪控制角。

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

步骤1中，所述随机探索环境包括盲目避障模式，使得无人车随任意时刻尽可能朝向空旷的地带运动，采取如下步骤计算随机游走的运动方向：

1.1在无人车上水平安装的2D激光雷达获取点云序列{(θ_i,d_i)|i＝1,2,…,M}；当光线照射至物体时，即可返回该处的角度和距离信息，这种角度和距离信息即构成一个点云，如此持续扫描，就可获得连续的二维点云信息；

1.2定义安全行驶方向矢量；

1.3计算安全行驶方向。

步骤1中，建立稀疏视觉特征地图的步骤如下：

首先，从双目图像中提取FAST角点，然后，利用灰度质心法为FAST角点添加方向信息，在提取了有方向信息的FAST角点后，对每个特征点计算ORB描述子，采用BRIEF算法进行描述；将所提取的ORB描述子及对应的三维坐标与所属双目图像的位置进行关联，建立稀疏视觉特征地图。

步骤1中，对稀疏视觉特征地图采用二进制文件的形式进行序列化和反序列化操作使其能够重复使用、扩充及更新。

步骤1中，对稀疏视觉特征地图采用二进制文件的形式进行序列化和反序列化操作使其能够重复使用、扩充及更新。以便每次重新运动无人车时不必重新建图，或者，为了在原有地图的基础上扩充新区域的地图，必须对内存中的地图数据进行序列化和反序列化。序列化的对象是关键帧数据及关键帧对应的特征点云数据。关键帧是指包含相机位置信息的提取自双目图像的特征点。

由于采用本发明的技术方案，本发明的有益效果为：本发明采取了直接基于稀疏视觉特征地图提供的定位信息进行导航控制量运算的直观有效方法，并在目标位置跟踪控制的同时，实时规避无人车运动过程中周围可能随时出现的动态障碍物体，从而实现无人车地图构建和自主导航的无缝集成，完成安全的位置跟踪控制。本发明稀疏视觉特征地图可重用，可扩充，可更新。

附图说明

图1是本发明中基于稀疏视觉特征地图位姿信息的控制示意图；

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

图3是本发明中FAST角点检测原理示意图；

图4-1是本发明k时刻和k-1时刻的场景；

图4-2是本发明前后时刻的三对特征点对应关系；

图5是本发明三点透视算法计算原理图；

图6是本发明目标位置跟踪控制量计算原理图；

图7是本发明踪控制量和避障控制量融合示意图。

具体实施方式

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

步骤1，启动设有避障探索模块的无人车：设置无人车为随机探索环境的自动运动模式,启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以二进制文件形式将稀疏视觉特征地图数据存储至车载计算机本地磁盘，构成稀疏视觉特征地图数据库；

步骤2，从无人车载计算机的本地磁盘读取地图数据文件至内存，设置为定位跟踪模式；定位成功后，接收用户设置的目标位置坐标信息；按如下步骤，有位置关系变化后，反复尝试重定位：

2.1.当没有成功重定位时，由所述避障探索模块生成随机控制量控制无人车运动；

2.2.运动后采集新的场景图像，与稀疏视觉特征地图数据库中的场景进行匹配；

2.3.匹配成功后进行位姿估计，采用三点透视算法(P3P)，计算无人车相对于地图的相对位置关系，实现重定位；

步骤(3)：如图6所示，设无人车中心为本体坐标系的原点，正前方为z轴，右方为x轴，建立本体坐标系；设无人车启动时的车中心为全局坐标系的原点，正前方为z轴，右方为x轴，建立世界坐标系。根据当前定位信息和目标位置信息，计算目标位置控制角：设置无人车在世界坐标系下的当前位置为x_curr，z_curr和朝向h_curr，目标位置在世界坐标系下的坐标为x_t和z_t，可得到目标位置跟踪控制角，将目标位置变换至本体坐标系，计算公式如下：

x_{in_cam}＝(x_t-x_curr)·sin(h_curr)-(z_t-z_curr)·cos(h_curr)

z_{in_cam}＝(x_t-x_curr)·cos(h_curr)+(z_t-z_curr)·sin(h_curr)

其中，x_{in_cam}，z_{in_cam}是目标位置在本体坐标系下的坐标。

计算参考向量v_x，v_z，计算公式如下：

v_x＝x_{in_cam}-0；

v_z＝z_{in_cam}-0；

计算目标位置控制角θ_ctrl，计算公式如下：

θ_ctrl＝tan^-1(v_x,v_z)；

步骤4：根据车载距离传感器的障碍探测信息，计算局部的避障控制角；

步骤5：加权融合目标位置跟踪控制角和局部的避障控制量，并将融合后的控制量发送给无人车执行，直至最终安全抵达目标位置，可实现的位置控制精度为0.2米；

采用如下方式融合两个控制量，如图7所示，实现安全的目标跟踪：

θ＝w₁θ_obs+w₂θ_t

其中，w₁、w₂为权重系数，且有w₁+w₂＝1.0；

θ_obs是避障控制量，θ_t是目标跟踪控制量。

步骤(1)中，所述随机探索环境包括盲目避障模式，使得无人车随任意时刻尽可能朝向空旷的地带运动，这一方面保证了车辆自动运动的安全，同时，也能采集到足够多的环境特征点。基于此策略进行探索建图。

具体而言，采取如下步骤计算随机游走的运动方向：

1.1激光雷达获取点云：

从水平安装的2D激光雷达按照固定的角度分辨率扫描激光雷达周围270°角度范围内的空间，获取点云序列{(θ_i,d_i)|i＝1,2,…,M}。

扫描角度步长为0.25°，点云的数据范围如图1所示。

当光线照射至物体时，即可返回该处的角度和距离信息，这种角度和距离信息即构成一个点云，如此持续扫描，就可获得连续的二维点云信息；

1.2定义安全行驶方向矢量；

安全行驶方向是激光雷达探测距离的函数，其一般形式如下式：

其中，r_thresh是预设的避障半径。

1.3计算安全行驶方向；

考虑到引力场的强度通常与作用距离的二次方相关，因此，将上述的行驶方向矢量函数具体化为激光雷达探测距离的二次方形式，即有：

计算x方向的矢量和：

计算y放向的矢量和：

x方向矢量和归一化：

y方向矢量和归一化：

计算行驶方向角：

从而，得到安全行驶方向θ_steer，以此作为角度操纵控制量发送给执行器，即可驱动无人车驶向安全方向。

步骤1中，建立稀疏视觉特征地图的步骤如下，如图3所示：

特征点的计算和位姿跟踪,

1.4从图像中抽取ORB特征点：

1.4.1首先，按如下步骤提取FAST角点：在图像中选取像素p，假设其亮度为I_p；

1.4.2然后，设置阈值T(例如I_p的20％)；

1.4.3再以像素p为中心，选取半径为3的圆上的16个像素点；

1.4.4最后，假如所选取的圆上，有连续N个点的亮度大于I_p+T或小于I_p-T，那么像素p可以被认为是特征点，一般取N为12；

循环以上4步(1.4.1～1.4.4)，对每一个像素执行相同的操作。

1.5利用灰度质心法为FAST角点添加方向信息，操作如下：

1.5.1在FAST角点邻域图像块中，按如下公式计算邻域的矩：

1.5.2按如下公式计算邻域的质心：

1.5.3连接角点(即邻域的几何中心)与质心C，得到方向向量OC，按如下公式计算特征点的方向：

θ＝arctan2(m₀₁,m₁₀)

1.6在提取了有方向信息的FAST角点后，对每个特征点计算ORB描述子，采用BRIEF算法进行描述。

操作如下：

1.6.1在FAST角点邻域中随机选取n_d对点；

1.6.2比较每对点的大小，给出0或1；

1.6.3得到n_d维的二进制串，作为此角点的描述子，即ORB特征点描述子；匹配前后帧的特征点，如图4-1和图4-2所示：

在图像I_t中提取到特征点m＝1,2,...,M，在图像I_t+1中提取到特征点/>n＝1,2,...,N，采用快速近似最近邻(FLANN)算法进行匹配，由于BRIEF是二进制的描述子，采用汉明距离作为度量距离的方式，即两个二进制串之间不同位的个数。根据这些匹配对计算两帧间的位姿变换矩阵。

由于双目相机已事先标定好，并且，系统采用双目相机，那么利用三角化计算可获得3D点和其在相机的投影位置，故计算位姿变换矩阵的方式是P3P算法。P3P的输入数据为三对3D-2D匹配点，3D点表示为A，B，C，2D点表示为a，b，c，如图5所示。

P3P问题的主要方程为：

(1-u)y²-ux²-cos<b,c>y+2uxycos<a,b>+1＝0

(1-w)x²-wy²-cos<a,c>x+2wxycos<a,b>+1＝0

其中，

v＝AB²/OC²,uv＝BC²/OC²,wv＝AC²/OC²。

基于词包模型实现闭环检测：

后台要做闭环检测，以校正累积误差，这里，采用基于表面特征的场景匹配算法，即词包模型。词包模型的原理是视觉词典和词汇统计直方图向量的余弦距离比较，余弦距离公式如下：

地图数据的序列化和反序列化：

为实现地图重用，以便每次重新运动无人车时不必重新建图，或者，为了在原有地图的基础上扩充新区域的地图，必须对内存中的地图数据进行序列化和反序列化。采用boost函数库的serialization类库进行序列化和反序列化操作。序列化的对象是关键帧数据及关键帧对应的特征点云数据。

Claims (4)

1.一种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，包括以下步骤：

步骤1，启动设有避障探索模块的无人车：设置无人车为随机探索环境的自动运动模式,启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以二进制文件形式将稀疏视觉特征地图数据存储至车载计算机本地磁盘，构成稀疏视觉特征地图数据库；

步骤2，从无人车载计算机的本地磁盘读取地图数据文件至内存，设置为定位跟踪模式；定位成功后，接收用户设置的目标位置坐标信息；按如下步骤，有位置关系变化后，反复尝试重定位：

2.1.当没有成功重定位时，由所述避障探索模块生成随机控制量控制无人车运动；

2.2.运动后采集新的场景图像，与稀疏视觉特征地图数据库中的场景进行匹配；

2.3.匹配成功后进行位姿估计，计算无人车相对于地图的相对位置关系，实现重定位；

步骤3：计算目标位置跟踪控制角：设无人车中心为本体坐标系的原点，前方为z轴的正方向，右方为x轴正方向，建立本体坐标系；设无人车启动时的无人车中心为全局坐标系的原点，前方为z轴的正方向，右方为x轴正方向，建立世界坐标系；根据当前定位信息和目标位置信息，计算目标位置跟踪控制角：设置无人车在世界坐标系下的当前位置为x_curr，z_curr和朝向h_curr，目标位置在世界坐标系下的坐标为x_t和z_t，可得到目标位置跟踪控制角，将目标位置变换至本体坐标系，计算公式如下：

x_{in_cam}＝(x_t-x_curr)·sin(h_curr)-(z_t-z_curr)·cos(h_curr)

z_{in_cam}＝(x_t-x_curr)·cos(h_curr)+(z_t-z_curr)·sin(h_curr)

其中，x_{in_cam}，z_{in_cam}是目标位置在本体坐标系下的坐标；

计算参考向量v_x，v_z，计算公式如下：

v_x＝x_{in_cam}-0

v_z＝z_{in_cam}-0

计算目标位置跟踪控制角θ_ctrl，计算公式如下：

θ_ctrl＝tan^-1(v_x,v_z)；

步骤4：根据车载距离传感器的障碍探测信息，计算局部的避障控制角；

步骤5：加权融合目标位置跟踪控制角和局部的避障控制量，并将融合后的控制量发送给无人车执行，直至最终安全抵达目标位置；

采用如下方式融合两个控制量，实现安全的目标跟踪：

θ＝w₁θ_obs+w₂θ_t；

其中，w₁、w₂为权重系数，且有w₁+w₂＝1.0；θ_obs是局部避障控制角，θ_t是目标位置跟踪控制角。

2.如权利要求1所述的一种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，步骤1中，所述随机探索环境包括盲目避障模式，使得无人车随任意时刻尽可能朝向空旷的地带运动，采取如下步骤计算随机游走的运动方向：

1.1在无人车上水平安装的2D激光雷达获取点云序列{(θ_i,d_i)|i＝1,2,…,M}；当光线照射至物体时，即可返回该处的角度和距离信息，这种角度和距离信息即构成一个点云，如此持续扫描，就可获得连续的二维点云信息；

1.2定义安全行驶方向矢量；

1.3计算安全行驶方向。

3.如权利要求1所述的一种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，步骤1中，建立稀疏视觉特征地图的步骤如下：

首先，从双目图像中提取FAST角点，并定义该FAST角点为特征点，然后，利用灰度质心法为FAST角点添加方向信息，在提取了有方向信息的FAST角点后，对每个特征点计算ORB描述子，采用BRIEF算法进行描述；将所提取的ORB描述子及对应的三维坐标与所属双目图像的位置进行关联，建立稀疏视觉特征地图。

4.如权利要求1所述的一种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，步骤1中，对稀疏视觉特征地图采用二进制文件的形式进行序列化和反序列化操作使其能够重复使用、扩充及更新。