基于双一线激光雷达自导航定位方法及系统与流程

本发明涉及机器视觉领域，尤其涉及一种基于双一线激光雷达的自导航定位系统、方法及存储介质。

背景技术：

自主导航移动机器人在生产、生活中使用范围越来越广泛，自主导航移动机器人是指搭载特定传感器，在没有环境先验证信息的情况下，在运动过程中可以建立环境模型并估计自身运动的机器人。

现有技术中应用较多的是搭载单线激光雷达的导航机器人，此种机器人采用单线激光雷达采集数据方向单一、数量稀疏、建图速度慢、无法实现对周围环境的快速扫描、且自导航、定位精度不高。

技术实现要素：

基于上述存在的问题，本发明提供基于双一线激光雷达的自导航定位系统、方法及存储介质，用以解决现有技术中的采用单线激光雷达采集数据方向单一、数量稀疏、建图速度慢、无法实现对周围环境的快速扫描、且自导航、定位精度不高的技术问题。

本发明实施例提供一种基于双一线激光雷达的自导航定位系统，包括：

数据采集单元1，自导航算法控制单元2，车体3，车体运动控制单元4；

所述数据采集单元1包括：前激光雷达1-1，后激光雷达1-2，第一俯仰电机1-3，第一俯仰电机编码器1-4，第一俯仰电机驱动单元1-5，第二俯仰电机1-6，第二俯仰电机驱动单元1-7，第二俯仰电机编码器1-8，以及惯性测量imu模块1-9；

所述自导航算法控制单元2包括：自导航算法执行计算机2-1；

所述车体运动控制单元4包括：驱动电机驱动单元4-1，转向电机驱动单元4-2，驱动电机4-3，转向电机4-4，驱动电机编码器4-5，转向电机编码器4-6；

所述数据采集单元1的前激光雷达1-1安装于车体3的前部，与第一俯仰电机1-3，第一俯仰电机编码器1-4以及第一俯仰电机驱动单元1-5连接并与自导航算法执行计算机2-1进行数据通信；

所述数据采集单元1的后激光雷达1-2安装于车体3的后部，第二俯仰电机1-6，第二俯仰电机驱动单元1-7，第二俯仰电机编码器1-8连接并与自导航算法执行计算机2-1进行数据通信；

所述数据采集单元1的惯性测量imu模块1-9与自导航算法执行计算机2-1进行数据通信；

所述车体运动控制单元4的驱动电机驱动单元4-1与驱动电机4-3、驱动电机编码器4-5连接，并与自导航算法执行计算机2-1进行数据通信；

所述车体运动控制单元4的转向电机驱动单元4-2与转向电机4-4、转向电机编码器4-6连接，并与自导航算法执行计算机2-1进行数据通信。

进一步地，所述连接，具体为：电性线连接。

进一步地，所述自导航算法控制单元2的自导航算法执行计算机2-1，与数据采集单元1和车体运动控制单元4内设备进行数据通信，根据采集的数据对周围环境进行建图、车体运动轨迹监测以及运动控制。

进一步地，所述前激光雷达1-1、后激光雷达1-2，选用相同设备并对称安装在车体3前后两端。

与现有技术相比，本发明实施例提供的一种基于双一线激光雷达的自导航定位系统，可带来如下有益效果：车体在行进过程中，俯仰电机、俯仰电机编码器、俯仰电机驱动单元，带动一前一后对称设置的前、后激光雷达进行往复俯仰运动，激光雷达以及惯性测量imu模块完成数据采集以及修正工作，自导航算法控制单元的自导航算法执行计算机，获取激光雷达采集的数据，执行双一线激光雷达自导航定位算法，对车体行进轨迹进行评估并下达行进及转向参数给车体运动控制单元的驱动电机及转向电机，进而根据预设轨迹修正车体实际行进轨迹，完成在车体行进过程中对周围环境的多角度扫描、实现快速建图、并提高了自导航车体行进轨迹自导航、定位精度。

本发明实施例还提供一种基于双一线激光雷达自导航定位方法，包括：

系统控制第一俯仰电机1-3、第二俯仰电机1-7，以及各自配合使用的俯仰电机编码器、俯仰电机驱动单元带动前激光雷达和后激光雷达按相同的预设频率及速度往复进行俯仰运动，对环境数据进行采集；

将采集到的环境数据与俯仰电机编码器输出的角度数据进行耦合，得到3d点云数据；

俯仰电机带动激光雷达完成一次往复运动即完成一帧数据的采集，利用orb算法提取3d点云数据的图像特征，并对连续两帧数据的图像特征进行匹配，计算两帧数据相同的特征元素之间的位移关系，得到两帧数据间隔期间的小车位移数据，获取车体实际行进轨迹；

系统采用前后双一线激光雷达，车体在行走超过预设距离进行回环监测，减小累计误差；

将车体实际行进轨迹与预设行进路径进行比对，获得路径误差，通过增量式pid算法按预设行进路径，进行转向和前进方向参数修正，控制车体按预设进行路径行进。

进一步地，所述将采集到的环境数据与俯仰电机编码器输出的角度数据进行耦合，得到3d点云数据，包括：

采集激光雷达测取的距离l，激光雷达测量光线与激光雷达中心线的水平夹角α，激光雷达往复摆动的俯仰角度β；

则激光雷达测得物体的3d点云数据的坐标u＝(x,y,z)，按如下方式进行计算：

进一步地，所述将采集到的环境数据与俯仰电机编码器输出的角度数据进行耦合，得到3d点云数据，还包括：

对车体行进过程中采集的数据进行修正；

根据系统惯性测量imu模块采集车体运动过程中的3d点云数据的u＝(x,y,z)加速度数据，在采样间隔δt内，位移数据δu＝(δx,δy,δz)；

修正后的3d点云数据形式为：u+δu＝(x+δx,y+δy,z+δz)。

进一步地，俯仰电机带动激光雷达完成一次往复运动即完成一帧数据的采集，利用orb算法提取3d点云数据的图像特征，并对连续两帧数据的图像特征进行匹配，计算两帧数据相同的特征元素之间的位移关系，得到两帧数据间隔期间的小车位移数据，获取车体实际行进轨迹，包括：

在相邻的数据帧中提取图像特征，使用orb算法，获取3d点云数据的关键点fast和描述子brief；

采用图像金字塔方法对图像数据进行不同层次的降采样，以实现关键点的尺度变换，采用灰度质心法得到图像特征关键点的方向信息；

在提取出每一帧图像特征后，在帧与帧之间对这些特征进行匹配计算，采用快速近似最近邻flann算法计算出相同特征点；

完成图像特征匹配计算后，可计算出两帧图像间小车移动的距离和方向变化，根据车体出发点位置，采用迭代最近点icp方法进行计算，求解出小车当前的位置和方向，获取车体实际行进轨迹。

进一步地，所述将车体实际行进轨迹与预设行进路径进行比对，获得路径误差，通过增量式pid算法按预设行进路径进行转向和前进方向参数修正，控制车体按预设进行路径行进，具体为：增量式pid控制算法为：

δu(k)＝u(k)-u(k-1)

＝kp[e(k)-e(k-1)]+ki*e(k)+kd[e(k)-2e(k-1)+e(k-2)]

其中，kp，ki，kd为pid控制参数，e(k)为第k次误差值，u(k)为控制量。

本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现前述权利要求5至9中任一项所述的一种基于双一线激光雷达自导航定位方法。

与现有技术相比，一种基于双一线激光雷达的自导航定位方法及存储介质，至少实现了如下的有益效果：本发明提出的一种基于双一线激光雷达的自导航定位方法，前激光雷达和后激光雷达按相同的预设频率及速度往复进行俯仰运动，对环境数据进行多角度采集可以得到比激光雷达自导航定位方法更为密集的3d点云数据，使用orb方法计算速度快并可以实现实时控制小车运动的需求，采用快速近似最近邻flann算法能较快计算出相同特征点，自导航技术方案中遇到最大的问题就是累积误差的问题，本技术方案，车体在行走超过预设距离进行回环监测，有效减小车体行进过程中产生的累计误差，采用前后双一线激光雷达可以实现对周围环境的多角度扫描、快速建图、并提高了自导航车体行进轨迹自导航、定位精度，有效的解决了采用单线激光雷达采集数据方向单一、数量稀疏、建图速度慢、无法实现对周围环境的快速扫描、且自导航、定位精度不高的技术问题。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种基于双一线激光雷达的自导航定位系统结构图；

图2为本发明实施例提供的一种基于双一线激光雷达的自导航定位系统的前、后激光雷达安装位置示意图；

图3为本发明实施例提供的种基于双一线激光雷达的自导航定位方法流程图。

具体实施方式

为了使本发明的目的，技术方案和优点更加清楚，下面结合附图，对本发明实施例提供的一种基于素数预处理的ecc标量乘法器实现的方法的具体实施方式进行详细地说明。应当理解，下面所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。并且在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

本发明实施例提供一种基于双一线激光雷达的自导航定位系统，系统结构如图1和图2所示，包括：

数据采集单元1，自导航算法控制单元2，车体3，车体运动控制单元4；

所述数据采集单元1包括：前激光雷达1-1，后激光雷达1-2，第一俯仰电机1-3，第一俯仰电机编码器1-4，第一俯仰电机驱动单元1-5，第二俯仰电机1-6，第二俯仰电机驱动单元1-7，第二俯仰电机编码器1-8，以及惯性测量imu模块1-9；

所述自导航算法控制单元2包括：自导航算法执行计算机2-1；

所述车体运动控制单元4包括：驱动电机驱动单元4-1，转向电机驱动单元4-2，驱动电机4-3，转向电机4-4，驱动电机编码器4-5，转向电机编码器4-6；

所述数据采集单元1的前激光雷达1-1安装于车体3的前部，与第一俯仰电机1-3，第一俯仰电机编码器1-4以及第一俯仰电机驱动单元1-5连接并与自导航算法执行计算机2-1进行数据通信；

所述数据采集单元1的后激光雷达1-2安装于车体3的后部，第二俯仰电机1-6，第二俯仰电机驱动单元1-7，第二俯仰电机编码器1-8连接并与自导航算法执行计算机2-1进行数据通信；

所述数据采集单元1的惯性测量imu模块1-9与自导航算法执行计算机2-1进行数据通信；

所述车体运动控制单元4的驱动电机驱动单元4-1与驱动电机4-3、驱动电机编码器4-5连接，并与自导航算法执行计算机2-1进行数据通信；

所述车体运动控制单元4的转向电机驱动单元4-2与转向电机4-4、转向电机编码器4-6连接，并与自导航算法执行计算机2-1进行数据通信。

优选地，所述连接，具体为：电性线连接。

优选地，所述自导航算法控制单元2的自导航算法执行计算机2-1，与数据采集单元1和车体运动控制单元4内设备进行数据通信，根据采集的数据对周围环境进行建图、车体运动轨迹监测以及运动控制。

优选地，所述前激光雷达1-1、后激光雷达1-2，选用相同设备并对称安装在车体3前后两端。

与现有技术相比，本发明实施例提供的一种基于双一线激光雷达的自导航定位系统，可带来如下有益效果：车体在行进过程中，俯仰电机、俯仰电机编码器、俯仰电机驱动单元，带动一前一后对称设置的前、后激光雷达进行往复俯仰运动，激光雷达以及惯性测量imu模块完成数据采集以及修正工作，自导航算法控制单元的自导航算法执行计算机，获取激光雷达采集的数据，执行双一线激光雷达自导航定位算法，对车体行进轨迹进行评估并下达行进及转向参数给车体运动控制单元的驱动电机及转向电机，进而根据预设轨迹修正车体实际行进轨迹，完成在车体行进过程中对周围环境的多角度扫描、实现快速建图、并提高了自导航车体行进轨迹控制精度。

本发明实施例还提供一种基于双一线激光雷达自导航定位方法，方法流程图如图3所示，包括：

s301.系统控制第一俯仰电机1-3、第二俯仰电机1-7，以及各自配合使用的俯仰电机编码器、俯仰电机驱动单元带动前激光雷达和后激光雷达按相同的预设频率及速度往复进行俯仰运动，对环境数据进行采集；

s302.将采集到的环境数据与俯仰电机编码器输出的角度数据进行耦合，得到3d点云数据；

s303.俯仰电机带动激光雷达完成一次往复运动即完成一帧数据的采集，利用orb算法提取3d点云数据的图像特征，并对连续两帧数据的图像特征进行匹配，计算两帧数据相同的特征元素之间的位移关系，得到两帧数据间隔期间的小车位移数据，获取车体实际行进轨迹；

s304.系统采用前后双一线激光雷达，车体在行走超过预设距离进行回环监测，减小累计误差；

s305.将车体实际行进轨迹与预设行进路径进行比对，获得路径误差，通过增量式pid算法按预设行进路径，进行转向和前进方向参数修正，控制车体按预设进行路径行进。

优选地，所述将采集到的环境数据与俯仰电机编码器输出的角度数据进行耦合，得到3d点云数据，包括：

采集激光雷达测取的距离l，激光雷达测量光线与激光雷达中心线的水平夹角α，激光雷达往复摆动的俯仰角度β；

则激光雷达测得物体的3d点云数据的坐标u＝(x,y,z)，按如下方式进行计算：

优选地，所述将采集到的环境数据与俯仰电机编码器输出的角度数据进行耦合，得到3d点云数据，还包括：

对车体行进过程中采集的数据进行修正；

根据系统惯性测量imu模块采集车体运动过程中的3d点云数据的u＝(x,y,z)加速度数据，在采样间隔δt内，位移数据δu＝(δx,δy,δz)；

修正后的3d点云数据形式为：u+δu＝(x+δx,y+δy,z+δz)。

优选地，俯仰电机带动激光雷达完成一次往复运动即完成一帧数据的采集，利用orb算法提取3d点云数据的图像特征，并对连续两帧数据的图像特征进行匹配，计算两帧数据相同的特征元素之间的位移关系，得到两帧数据间隔期间的小车位移数据，获取车体实际行进轨迹，包括：

在相邻的数据帧中提取图像特征，使用orb算法，获取3d点云数据的关键点fast和描述子brief；

采用图像金字塔方法对图像数据进行不同层次的降采样，以实现关键点的尺度变换，采用灰度质心法得到图像特征关键点的方向信息；

在提取出每一帧图像特征后，在帧与帧之间对这些特征进行匹配计算，采用快速近似最近邻flann算法计算出相同特征点；

完成图像特征匹配计算后，可计算出两帧图像间小车移动的距离和方向变化，根据车体出发点位置，采用迭代最近点icp方法进行计算，求解出小车当前的位置和方向，获取车体实际行进轨迹。

优选地，所述将车体实际行进轨迹与预设行进路径进行比对，获得路径误差，通过增量式pid算法按预设行进路径进行转向和前进方向参数修正，控制车体按预设进行路径行进，具体为：增量式pid控制算法为：

δu(k)＝u(k)-u(k-1)

＝kp[e(k)-e(k-1)]+ki*e(k)+kd[e(k)-2e(k-1)+e(k-2)]

其中，kp，ki，kd为pid控制参数，e(k)为第k次误差值，u(k)为控制量。

本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现前述权利要求5至9中任一项所述的一种基于双一线激光雷达自导航定位方法。

与现有技术相比，一种基于双一线激光雷达的自导航定位方法及存储介质，至少实现了如下的有益效果：本发明提出的一种基于双一线激光雷达的自导航定位方法，前激光雷达和后激光雷达按相同的预设频率及速度往复进行俯仰运动，对环境数据进行多角度采集可以得到比激光雷达自导航定位方法更为密集的3d点云数据，使用orb方法计算速度快并可以实现实时控制小车运动的需求，采用快速近似最近邻flann算法能较快计算出相同特征点，自导航技术方案中遇到最大的问题就是累积误差的问题，本技术方案，车体在行走超过预设距离进行回环监测，有效减小车体行进过程中产生的累计误差，采用前后双一线激光雷达可以实现对周围环境的多角度扫描、快速建图、并提高了自导航车体行进轨迹自导航、定位精度，有效的解决了采用单线激光雷达采集数据方向单一、数量稀疏、建图速度慢、无法实现对周围环境的快速扫描、且自导航、定位精度不高的技术问题。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到本发明实施例可以通过硬件实现，也可以借助软件和必要的通用硬件平台结合的方式来实现。基于这样的理解，本发明实施例的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是cd-rom，u盘，移动硬盘等)中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

本领域技术人员可以理解附图只是一个优选实施例的示意图，附图中的模块或流程并不一定是实施本发明所必须的。

本领域技术人员可以理解实施例中的装置中的模块可以按照实施例描述进行分布于实施例的装置中，也可以进行相应变化位于不同于本实施例的一个或多个装置中。上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。