申请/专利权人：浙江大学

申请日：2016-11-01

公开（公告）日：2024-02-27

公开（公告）号：CN106548486B

主分类号：G06T7/246

分类号：G06T7/246;G06T7/73;G06F16/29;G05D1/43;G05D1/249

优先权：

专利状态码：有效-授权

法律状态：2024.02.27#授权;2019.08.30#实质审查的生效;2017.03.29#公开

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

主权项：1.一种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，包括以下步骤：步骤1，启动设有避障探索模块的无人车：设置无人车为随机探索环境的自动运动模式,启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以二进制文件形式将稀疏视觉特征地图数据存储至车载计算机本地磁盘，构成稀疏视觉特征地图数据库；步骤2，从无人车载计算机的本地磁盘读取地图数据文件至内存，设置为定位跟踪模式；定位成功后，接收用户设置的目标位置坐标信息；按如下步骤，有位置关系变化后，反复尝试重定位：2.1.当没有成功重定位时，由所述避障探索模块生成随机控制量控制无人车运动；2.2.运动后采集新的场景图像，与稀疏视觉特征地图数据库中的场景进行匹配；2.3.匹配成功后进行位姿估计，计算无人车相对于地图的相对位置关系，实现重定位；步骤3：计算目标位置跟踪控制角：设无人车中心为本体坐标系的原点，前方为z轴的正方向，右方为x轴正方向，建立本体坐标系；设无人车启动时的无人车中心为全局坐标系的原点，前方为z轴的正方向，右方为x轴正方向，建立世界坐标系；根据当前定位信息和目标位置信息，计算目标位置跟踪控制角：设置无人车在世界坐标系下的当前位置为xcurr，zcurr和朝向hcurr，目标位置在世界坐标系下的坐标为xt和zt，可得到目标位置跟踪控制角，将目标位置变换至本体坐标系，计算公式如下：xin_cam＝xt-xcurr·sinhcurr-zt-zcurr·coshcurrzin_cam＝xt-xcurr·coshcurr+zt-zcurr·sinhcurr其中，xin_cam，zin_cam是目标位置在本体坐标系下的坐标；计算参考向量vx，vz，计算公式如下：vx＝xin_cam-0vz＝zin_cam-0计算目标位置跟踪控制角θctrl，计算公式如下：θctrl＝tan-1vx,vz；步骤4：根据车载距离传感器的障碍探测信息，计算局部的避障控制角；步骤5：加权融合目标位置跟踪控制角和局部的避障控制量，并将融合后的控制量发送给无人车执行，直至最终安全抵达目标位置；采用如下方式融合两个控制量，实现安全的目标跟踪：θ＝w1θobs+w2θt；其中，w1、w2为权重系数，且有w1+w2＝1.0；θobs是局部避障控制角，θt是目标位置跟踪控制角。

全文数据：一种基于稀疏视觉特征地图的无人车位置跟踪方法技术领域[0001]本发明属于移动机器人自主导航技术领域，特别是基于稀疏视觉特征地图的无人车位置跟踪方法。背景技术[0002]随着移动机器人技术的发展，针对非结构化场景自主建立环境地图，并基于所建立的环境地图实现安全的导航控制，成为越来越迫切的核心需求，是达成移动机器人高层次作业任务的基础支撑。为了提高建图的效率，一般的做法是提取环境的稀疏特征信息进行运算，最终生成的地图也是稀疏的表达形式，从而难以直接用于自主移动平台的路径规划和控制。发明内容[0003]本发明所要解决的技术问题是提供一种基于稀疏视觉特征地图的无人车位置跟踪方法，以解决稀疏环境地图难以集成到规划控制系统的问题。为此，本发明提供以下技术方案：[0004]-种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，包括以下步骤：[0005]步骤1，启动设有避障探索模块的无人车:设置无人车为随机探索环境的自动运动模式，启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以二进制文件形式将稀疏视觉特征地图数据存储至车载计算机本地磁盘，构成稀疏视觉特征地图数据库；[0006]步骤2，从无人车载计算机的本地磁盘读取地图数据文件至内存，设置为定位跟踪模式;定位成功后，接收用户设置的目标位置坐标信息;按如下步骤，有位置关系变化后，反复尝试重定位：[0007]2.1.当没有成功重定位时，由所述避障探索模块生成随机控制量控制无人车运动；[0008]2.2.运动后采集新的场景图像，与稀疏视觉特征地图数据库中的场景进行匹配；[0009]2.3.匹配成功后进行位姿估计，计算无人车相对于地图的相对位置关系，实现重定位；[0010]步骤3:计算目标位置跟踪控制角:设无人车中心为本体坐标系的原点，前方为z轴的正方向，右方为x轴正方向，建立本体坐标系;设无人车启动时的无人车中心为全局坐标系的原点，前方为z轴的正方向，右方为x轴正方向，建立世界坐标系;根据当前定位信息和目标位置信息，计算目标位置跟踪控制角:设置无人车在世界坐标系下的当前位置为x。^，zcurr和朝向h™rr，目标位置在世界坐标系下的坐标为Xt和zt，可得到目标位置跟踪控制角，将目标位置变换至本体坐标系，计算公式如下：[0011]Xin-cam-xt-Xcurr*sillhcurr-zt-Zcurr*COShcurr[0012]Zincam一xt-Xcurr*COShcurrzt-Zcurr*sillhcurr[0013]其中，乂^+^^:^+^是目标位置在本体坐标系下的坐标。[0014]计算参考向量vx，vz，计算公式如下：[0015]Vx=Xin_cain-〇[0016]Vz=Zin_cam-〇[0017]计算目标位置跟踪控制角0rtri，计算公式如下：[0018]9ctri=tan-1Vx，vz;[0019]步骤4:根据车载距离传感器的障碍探测信息，计算局部的避障控制角；[0020]步骤5:加权融合目标位置跟踪控制角和局部的避障控制量，并将融合后的控制量发送给无人车执行，直至最终安全抵达目标位置；[0021]采用如下方式融合两个控制量，实现安全的目标跟踪：[0022]0=wi0〇bs+W20t;[0023]其中，W1、W2为权重系数，且有W1+W2=1.〇;0Qbs是局部避障控制角，0t是目标位置跟踪控制角。[0024]在上述技术方案的基础上，本发明还可以采用一下进一步的技术方案：[0025]步骤1中，所述随机探索环境包括盲目避障模式，使得无人车随任意时刻尽可能朝向空旷的地带运动，采取如下步骤计算随机游走的运动方向：[0026]1.1在无人车上水平安装的2D激光雷达获取点云序列{0i，di|i=l，2,…，M};当光线照射至物体时，即可返回该处的角度和距离信息，这种角度和距离信息即构成一个点云，如此持续扫描，就可获得连续的二维点云信息；[0027]1.2定义安全行驶方向矢量；[0028]1.3计算安全行驶方向。[0029]步骤1中，建立稀疏视觉特征地图的步骤如下：[0030]首先，从双目图像中提取FAST角点，然后，利用灰度质心法为FAST角点添加方向信息，在提取了有方向信息的FAST角点后，对每个特征点计算0RB描述子，采用BRIEF算法进行描述;将所提取的0RB描述子及对应的三维坐标与所属双目图像的位置进行关联，建立稀疏视觉特征地图。[0031]步骤1中，对稀疏视觉特征地图采用二进制文件的形式进行序列化和反序列化操作使其能够重复使用、扩充及更新。[0032]步骤1中，对稀疏视觉特征地图采用二进制文件的形式进行序列化和反序列化操作使其能够重复使用、扩充及更新。以便每次重新运动无人车时不必重新建图，或者，为了在原有地图的基础上扩充新区域的地图，必须对内存中的地图数据进行序列化和反序列化。序列化的对象是关键帧数据及关键帧对应的特征点云数据。关键帧是指包含相机位置信息的提取自双目图像的特征点。[0033]由于采用本发明的技术方案，本发明的有益效果为:本发明采取了直接基于稀疏视觉特征地图提供的定位信息进行导航控制量运算的直观有效方法，并在目标位置跟踪控制的同时，实时规避无人车运动过程中周围可能随时出现的动态障碍物体，从而实现无人车地图构建和自主导航的无缝集成，完成安全的位置跟踪控制。本发明稀疏视觉特征地图可重用，可扩充，可更新。附图说明[0034]图1是本发明中基于稀疏视觉特征地图位姿信息的控制示意图；[0035]图2是本发明中激光雷达数据范围示意图；[0036]图3是本发明中FAST角点检测原理示意图；[0037]图4-1是本发明k时刻和k-1时刻的场景；[0038]图4-2是本发明前后时刻的三对特征点对应关系；[0039]图5是本发明三点透视算法计算原理图；[0040]图6是本发明目标位置跟踪控制量计算原理图；[0041]图7是本发明踪控制量和避障控制量融合示意图。具体实施方式[0042]为了更好地理解本发明的技术方案，以下结合附图作进一步描述。[0043]步骤1，启动设有避障探索模块的无人车:设置无人车为随机探索环境的自动运动模式，启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以二进制文件形式将稀疏视觉特征地图数据存储至车载计算机本地磁盘，构成稀疏视觉特征地图数据库；[0044]步骤2，从无人车载计算机的本地磁盘读取地图数据文件至内存，设置为定位跟踪模式;定位成功后，接收用户设置的目标位置坐标信息;按如下步骤，有位置关系变化后，反复尝试重定位：[0045]2.1.当没有成功重定位时，由所述避障探索模块生成随机控制量控制无人车运动；[0046]2.2.运动后采集新的场景图像，与稀疏视觉特征地图数据库中的场景进行匹配；[0047]2.3.匹配成功后进行位姿估计，采用三点透视算法P3P，计算无人车相对于地图的相对位置关系，实现重定位；[0048]步骤3:如图6所示，设无人车中心为本体坐标系的原点，正前方为z轴，右方为x轴，建立本体坐标系;设无人车启动时的车中心为全局坐标系的原点，正前方为z轴，右方为x轴，建立世界坐标系。根据当前定位信息和目标位置信息，计算目标位置控制角:设置无人车在世界坐标系下的当前位置为x〇jrr，z™rr和朝向h™rr，目标位置在世界坐标系下的坐标为xt和zt，可得到目标位置跟踪控制角，将目标位置变换至本体坐标系，计算公式如下：[0049]Xin-cam-xt-Xcurr*sillhcurr-zt-Zcurr*COShcurr[0050]Zincam-xt-Xcurr*COShcurrzt-Zcurr*sillhcurr[0051]其中，^"__，2y+2uxycos+l=0[0110]1-wx2-wy2-cosx+2wxycos+l=0[0111]其中，[0112]v=AB2OC2，uv=BC2OC2，wv=AC2OC2。[0113]基于词包模型实现闭环检测：[0114]后台要做闭环检测，以校正累积误差，这里，采用基于表面特征的场景匹配算法，即词包模型。词包模型的原理是视觉词典和词汇统计直方图向量的余弦距离比较，余弦距离公式如下：[0115][0116]地图数据的序列化和反序列化：[0117]为实现地图重用，以便每次重新运动无人车时不必重新建图，或者，为了在原有地图的基础上扩充新区域的地图，必须对内存中的地图数据进行序列化和反序列化。采用boost函数库的serialization类库进行序列化和反序列化操作。序列化的对象是关键帧数据及关键帧对应的特征点云数据。

权利要求：1.一种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，包括以下步骤：步骤1，启动设有避障探索模块的无人车：设置无人车为随机探索环境的自动运动模式，启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以二进制文件形式将稀疏视觉特征地图数据存储至车载计算机本地磁盘，构成稀疏视觉特征地图数据库；步骤2,从无人车载计算机的本地磁盘读取地图数据文件至内存，设置为定位跟踪模式;定位成功后，接收用户设置的目标位置坐标信息;按如下步骤，有位置关系变化后，反复尝试重定位：2.1.当没有成功重定位时，由所述避障探索模块生成随机控制量控制无人车运动；2.2.运动后采集新的场景图像，与稀疏视觉特征地图数据库中的场景进行匹配；2.3.匹配成功后进行位姿估计，计算无人车相对于地图的相对位置关系，实现重定位；步骤3:计算目标位置跟踪控制角:设无人车中心为本体坐标系的原点，前方为z轴的正方向，右方为X轴正方向，建立本体坐标系;设无人车启动时的无人车中心为全局坐标系的原点，前方为z轴的正方向，右方为X轴正方向，建立世界坐标系;根据当前定位信息和目标位置信息，计算目标位置跟踪控制角:设置无人车在世界坐标系下的当前位置为xm和朝向h™rr，目标位置在世界坐标系下的坐标为Xt和zt，可得到目标位置跟踪控制角，将目标位置变换至本体坐标系，计算公式如下：Xin_cam=xt~Xcurr·sinhcurr-zt~zcurr·coshcurrZincam-xt-Xcurr*COShcurr+zt-zcurr·sinhcurr其中，1^_。31!1，2：111_。£1111是目标位置在本体坐标系下的坐标。计算参考向量VX，VZ，计算公式如下：Vx-Xincam-0Vz-Zincam-0计算目标位置跟踪控制角θ。^，计算公式如下：9ctri-tanvx,Vz；步骤4:根据车载距离传感器的障碍探测信息，计算局部的避障控制角；步骤5:加权融合目标位置跟踪控制角和局部的避障控制量，并将融合后的控制量发送给无人车执行，直至最终安全抵达目标位置；采用如下方式融合两个控制量，实现安全的目标跟踪：Θ-Wl9〇bs+W29t;其中，W1、W2为权重系数，且有Wl+W2=l.0;θ。^是局部避障控制角，9t是目标位置跟踪控制角。2.如权利要求1所述的一种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，步骤1中，所述随机探索环境包括盲目避障模式，使得无人车随任意时刻尽可能朝向空旷的地带运动，采取如下步骤计算随机游走的运动方向：1.1在无人车上水平安装的2D激光雷达获取点云序列{0i，di|i=l，2,···，};当光线照射至物体时，即可返回该处的角度和距离信息，这种角度和距离信息即构成一个点云，如此持续扫描，就可获得连续的二维点云信息；1.2定义安全行驶方向矢量；1.3计算安全行驶方向。3.如权利要求1所述的一种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，步骤1中，建立稀疏视觉特征地图的步骤如下：首先，从双目图像中提取FAST角点，然后，利用灰度质心法为FAST角点添加方向信息，在提取了有方向信息的FAST角点后，对每个特征点计算ORB描述子，采用BRIEF算法进行描述;将所提取的ORB描述子及对应的三维坐标与所属双目图像的位置进行关联，建立稀疏视觉特征地图。4.如权利要求1所述的一种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，步骤1中，对稀疏视觉特征地图采用二进制文件的形式进行序列化和反序列化操作使其能够重复使用、扩充及更新。

百度查询： 浙江大学 一种基于稀疏视觉特征地图的无人车位置跟踪方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD