基于Kinect3和激光雷达的SLAM陪跑小车

摘要

本发明公开了一种基于Ki nect3和激光雷达的SLAM陪跑小车，包括覆带小车，安装在覆带小车上的Ki nect3摄像头、RPLIDAR A3，内载于覆带小车内的Jetson TX2模块以及安装在覆带小车上的机械手；Ki nect3摄像头用于实现视觉SLAM，通过扫描覆带小车周围环境并实现覆带小车自身和使用者的定位，RPLI DAR A3用于实现激光SLAM，消除视觉SLAM的累积误差，Ki nect3摄像头和RPLI DAR A3将采集到的信息发送至Jetson TX2模块进行处理，经树莓派实现覆带小车和机械手工作状态的调控。本发明不需要连接网络就可以实现自身和使用者的高准确度定位，并保持距离紧密跟随。

说明书

技术领域

本发明涉及视觉和雷达SLAM定位技术领域，具体涉及一种基于Kinect3 和激光雷达的SLAM陪跑小车。

背景技术

随着SLAM(同步定位与建图)技术兴起及其在机器人室外定位问题上的重要性愈发凸显，越来越多的企业和科研机构纷纷投入到SLAM技术的研发中，如今，SLAM技术是业界公认的前沿定位技术方向，目前SLAM分为视觉SLAM 和激光SLAM，两者各有相应的优缺点。

发明内容

本发明的目的在于提供一种基于Kinect3和激光雷达的SLAM陪跑小车，通过Kinect3来定位目标，再由激光雷达来校正消除视觉SLAM的累积误差，由此实现高准确度的SLAM定位，能够实时定位运动员，并保持距离紧密跟随。

为实现上述目的，本发明采取的技术方案为：

基于Kinect3和激光雷达的SLAM陪跑小车，包括覆带小车，安装在覆带小车上的Kinect3摄像头、RPLIDAR A3，内载于覆带小车内的Jetson TX2模块以及安装在覆带小车上的机械手；所述Kinect3摄像头用于实现视觉SLAM，通过扫描覆带小车周围环境并实现覆带小车自身和使用者的定位，所述 RPLIDAR A3用于实现激光SLAM，消除视觉SLAM的累积误差，Kinect3摄像头和RPLIDAR A3将采集到的信息发送至Jetson TX2模块进行处理，经树莓派实现覆带小车和机械手工作状态的调控。

进一步地，覆带小车工作时首先依靠激光SLAM确认自身位置，使用 RPLIDAR A3扫描采集周围环境数据，初步建立地图，定位覆带小车自身位置；

之后启用视觉SLAM，使用Kinect现场训练使用者的模型并初步定位使用者位置，并测量覆带小车和使用者之间的距离。

之后，将使用者的位置信息、机器人自身的位置信息与测量到的距离信息将传输到树莓派，由树莓派控制履带车追踪使用者并保持一定距离。

进一步地，机械臂用于抓取使用者递过来的水杯等物体，当使用者递过来的物体被抓住后，Kinect3会采集物品特征数据并传输至Jetson TX2进行分类和训练，最后得出物品的特征模型。

进一步地，使用者取用物品时，Kinect3会根据前一步训练好的物品模型，对被取用的物品进行实时识别和跟踪，并将物品位置信息传输到树莓派，当使用者放下物品后，树莓派会根据机器人自身位置和物品位置控制机械手去抓取物品。

进一步地，所述Jetson TX2模块采用安装Python 3.6的PC或其他安装 Python3.6环境的处理器或嵌入式系统。

本发明具有以下有益效果：

基于SLAM的实时定位系统，结合了两大SLAM定位方法的优点，利用激光 SLAM的精确性来验证校准视觉SLAM累积误差的算法，实现了长时间精准地定位。

利用履带车作为机器人地盘可以适应各种复杂地形，顺利追踪使用者，利用机械臂抓取用户使用后随意放置的物品并携带跟随用户，这样用户不需要随身携带水瓶、手机等物品也可以随时随地使用它们。

不需要连接网络就可以实现自身和使用者的定位，采用先进的SLAM建图技术，并且将两种SLAM技术的主要实现方案的优点结合起来，实现大范围精确定位建图。

附图说明

图1为本发明实施例一种基于Kinect3和激光雷达的SLAM陪跑小车的工作原理图。

图2为本发明实施例中树莓派与Jetson TX2的数据传输电路。

图3为基于极限学习机修正卡尔曼滤波的原理图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

如图1-图2所示，本发明实施例提供了一种基于Kinect3和激光雷达的SLAM陪跑小车，包括覆带小车，安装在覆带小车上的Kinect3摄像头、RPLIDAR A3，内载于覆带小车内的Jetson TX2模块以及安装在覆带小车上的机械手；所述Kinect3摄像头用于实现视觉SLAM，通过扫描覆带小车周围环境并实现覆带小车自身和使用者的定位以及物体的检测，所述Kinect3摄像头除了常规 RGB摄像头外还有100万像素的深度摄像头和电子加速度计和电子陀螺仪IMU，可以产生可直接由程序读取的距离数据，大大简化了程序复杂度和缩短了建图定位时间；所述RPLIDAR A3用于实现激光SLAM，消除视觉SLAM的累积误差，测量测量半径为25米，在系统持续运行过程中，视觉SLAM逐渐产生累积误差，可能会对计算使用者位置造成影响，为了消除这种误差，需要利用激光SLAM 确定机器人位置的地图对视觉SLAM进行一次校对，选用激光SLAM去矫正视觉 SLAM的原因是Lidar A3在构建地图时相比Kinect3而言距离很短(25米)，但是精确度极高，不会产生严重影响精度的误差；Kinect3摄像头和RPLIDAR A3 将采集到的信息发送至Jetson TX2模块进行处理，经树莓派实现覆带小车和机械手工作状态的调控。

本实施例中，所述Jetson TX2模块是NVIDIA公司发布的一款可嵌入式 AI计算设备，采用安装Python 3.6的PC或其他安装Python 3.6环境的处理器或嵌入式系统。

视觉SLAM：SLAM是“Simultaneous Localization And Mapping”的缩写，可译为同步定位与建图，只利用相机作为外部感知传感器的SLAM称为视觉 SLAM，是指仅仅利用图片这一外部信息来确定环境中机器人、车辆或者移动相机的位置，同时能够建立已探索区域的一种表示法(地图)。视觉SLAM现在多应用于汽车的无人驾驶，无人机的定位等。

激光SLAM：就是以激光数据作为传感器输入的SLAM方案。相比视觉SLAM 其计算量更小，建图更“平面”，现在多用于家用扫地机器人对自身定位。

技术关键在于结合了两种SLAM定位方法的优点来实现长时间精准地定位，建立了利用了激光SLAM的精确性来验证校准视觉SLAM累积误差的算法，利用履带车作为机器人地盘可以适应各种复杂地形，顺利追踪使用者，利用机械臂抓取用户使用后随意放置的物品并携带跟随用户，这样用户不需要随身携带水瓶、手机等物品也可以随时随地使用它们。

视觉SLAM的累计误差：视觉SLAM对于运动的判断是基于一种视觉里程计的算法，这种算法是通过相邻两帧图像计算两帧之间的相机运动，这样的计算得到的相机位姿会有一定误差，一次两次的误差不会有很大影响，但是这种误差多次累积后会导致严重的建图偏差。

SLAM是移动机器人实现自主化的一项基本任务。目前，在小范围，静态的场景中，认为使用距离传感器(比如激光、声呐)来建立二维地图，能够很好的解决SLAM问题。然而，在动态，复杂，大场景下，仅使用视觉作为唯一的外部传感器来进行SLAM定位跟踪累积误差大，故提出了使用激光SLAM校正视觉SLAM累积误差，保证系统准确性，提高鲁棒性。

误差消除算法是利用卡尔曼滤波算法改进的，卡尔曼滤波是一种递推线性最小方差估计，这种滤波算法无需保存过去的历史数据，只要新数据测得后，结合前一时刻已求出的估计值及系统本身的状态方程，按一定的递推方式即可求得新的估计值。在基于视觉的定位过程中，由于摄像头的突然抖动、传感器故障等会产生突发性的野点噪声，这种噪声服从拉普拉斯分布，虽然不常发生，但是对定位精度影响极大。当用传统卡尔曼方法处理野点噪声时，由于卡尔曼滤波器会将稀疏噪声看作高斯噪声来分析，导致目标跟踪精度严重下降，甚至使滤波结果严重偏离，导致滤波器发散。为克服这一问题，本发明利用基于凸优化的改进卡尔曼滤波方法，来预测视觉SLAM即将产生的误差，从而消除预测到的误差即可避免累积误差，包含两个步骤：第一步是通过改进型卡尔曼滤波算法对目标位置信息进行估计；第二步是利用改进型极限学习机算法对卡尔曼滤波得到的结果进行二次修正。

在第一步中，卡尔曼滤波的目标跟踪系统模型可表示为：

x

y

其中，x

使用凸优化技术，基于最大后验估计理论和贝叶斯理论的基础上，采用状态空间表达式呈现传统卡尔曼理论。

其中，λ是一个控制z

对优化公式(2)的形式进行进一步推导，就能发现改进型卡尔曼算法和传统卡尔曼算法的联系和区别。首先，定义v

对公式(3)中x求偏导，可以得到

令(4)为零，可以进一步得到

其中L＝∑C

min(e

其中，Q

此外，为了进一步提高定位精度，本专利采用改进型极限学习机方法来修正卡尔曼滤波结果。

ELM是一类基于前馈神经网络构建的机器学习方法，具有较好的训练效率和效果，是一种简单、高效的无监督学习算法。ELM在训练前，不需要对网络参数进行设置，仅对激励函数、隐层节点个数进行设置。

传统ELM框架采用伪逆算法估计输出权重，随机设定输入和隐层参数。故其存在容易过度学习、泛化能力低的问题。本专利使用一种基于L1/L2范数正则化约束的极限学习机算法(L1/L2-ELM)来修正卡尔曼滤波算法的结果。一个有T个隐层节点的ELM数学模型可以表示为：

其中，w

Φβ＝t (8)

式中β＝[β

在传统ELM算法中，输出权重β

式中:

因此，在式(10)中加入L1范数约束，借助L1范数的稀疏能力使模型简单。L1范数约束可以提高算法的泛化能力，得到式(11)，即

在式(10)中加入L2范数约束，使输出权重幅值在保持稀疏特性(L1 范数约束)的同时，更有利于在ELM网络中学习到符合实际情况的有效特征，可以得到式(12)，即

为了方便求解公式(12)，定义如下矩阵，即

令

式(12)对β

综上所述，基于L1/L2-ELM算法的步骤可以概括为以下三步。

(1)随机产生隐层节点参数w

(2)计算隐层输出矩阵Φ.

(3)根据式(15)计算输出权重

L1/L2-ELM无需调整输入权值和隐层参数，训练过程一次完成，模型训练速度提升，网络的泛化能力表现上升。

利用L1/L2-ELM与改进型卡尔曼滤波结合提高室内定位滤波算法的精度，在定位阶段使用改进型卡尔曼滤波对测量值进行过滤。在进行卡尔曼滤波过程中，要求系统模型和噪声模型已知，并且滤波过程中不允许改变。将估计误差和测量误差以及卡尔曼增益作为L1/L2-ELM的输入，经过L1/L2-ELM的运算，输出真实值与估计值之间的差。结合L1/L2-ELM与改进型卡尔曼滤波算法可以大大提高室内定位系统的精度。鉴于神经网络的自适应能力，大大提高系统的鲁棒性，泛化能力表现上升。使用L1/L2-ELM修正卡尔曼滤波的整体原理框图，如图3所示。

由于室内定位有着先天的复杂性，单一的标准卡尔曼滤波不可能很好地解决定位问题，为此引入L1/L2-ELM。使用ELM修正卡尔曼滤波的流程图，如图 2所示。其中，改进卡尔曼滤波的估计结果与预测状态同期望预测状态的差

本具体实施使用时，Kinect3摄像头在系统启动后，首先进行使用者的特征模型训练，提取使用者的外貌特征，如衣着、饰品、发型等，将这些特征数据传输至Jetson TX2，由TX2进行分类和训练。

其次，系统会执行激光SLAM程序确认自身位置，由RPLIDAR A3扫描并采集周围地图数据和环境深度信息，用以构建初步地图，这里将采集到的地图数据传输至Jetson TX2，并由TX2来处理数据并构建地图，定位覆带小车自身位置；

之后，启用视觉SLAM，Kinect3将根据前面训练好的使用者特征模型来实时识别使用者并确定使用者位置数据，并测量覆带小车和使用者之间的距离。

之后，将使用者的位置信息、机器人自身的位置信息与测量到的距离信息将传输到树莓派，由树莓派控制履带车追踪使用者并保持一定距离，这个过程中，Kinect3和LidarA3依然工作并实时更新使用者位置和机器人位置。

另外的机械手部分，机械臂用于抓取使用者递过来的水杯等物体，当使用者递过来的物体被抓住后，Kinect3会采集物品特征数据并传输至Jetson TX2 进行分类和训练，最后得出物品的特征模型。

使用者取用物品时，Kinect3会根据前一步训练好的物品模型，对被取用的物品进行实时识别和跟踪，并将物品位置信息传输到树莓派，当使用者放下物品后，树莓派会根据机器人自身位置和物品位置控制机械手去抓取物品。

本发明可以帮助爱好慢跑健身人士携带水杯和手机等慢跑时不便携带的物品，令使用者在顺利跑步健身过程中又可以随时补充水分和查阅手机等。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。