CN109813305A - 基于激光slam的无人叉车 - Google Patents

Abstract

本发明提供了基于激光SLAM的无人叉车，具体的控制方法包括：接通电源；启动激光传感器获取无人叉车当前时刻附近目的物的云点位置信息以及云点角度信息，利用扩展卡尔曼滤波算法对当前时刻云点位置信息校正得到当前时刻云点位置信息的最优值以及位置信息校正方程，利用扩展卡尔曼滤波算法对当前时刻云点角度信息进行校正得到当前时刻云点位置信息的最优值以及角度信息校正方程；根据得到的云点位置信息最优值以及云点角度最优值利用激光SLAM算法得到构建当前时刻环境；进行下一时刻当前时刻环境构建，最终形成地图；确定无人叉车的导航路径，利用地标信息对当前位置信息，地图构建准确，导航误差小。

Description

基于激光SLAM的无人叉车

技术领域

本发明涉及无人叉车技术领域，具体涉及基于激光SLAM的无人叉车的导航方法。

背景技术

SLAM(即simultaneous localization and mapping，同步定位与地图构建)，是指运动物体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程，解决机器人等在未知环境下运动时的定位与地图构建问题。目前，SLAM的主要应用于机器人、无人机、无人驾驶等领域。其用途包括传感器自身的定位，以及后续的路径规划、运动性能、场景理解。

由于传感器种类和安装方式的不同，SLAM的实现方式和难度会有一定的差异。按传感器来分，SLAM主要分为激光SLAM和VSLAM(基于视觉SLAM)两大类。激光SLAM利用激光雷达采集物体信息呈现出的一系列分散的、具有准确角度和距离信息的点(通常被称为点云)，通过对不同时刻两片点云的匹配与比对，计算激光雷达相对运动的距离和姿态的改变，完成室内定位，由于激光SLAM采集云点，无需像VSLAM采集三维深度信息，从而其对计算性能需要大大低于VSLAM，算法简单，从而现在已被广泛应用与无人机控制领域，但是由于激光SLAM进行定位需要首先从起始点采用激光传感器获取目标物的位置以及角度等信息，然后激光SLAM算法构建地图进行定位，然而由于激光传感器容易受到灰尘、雨滴等反射影响，从而使得激光传感器获得一些不准确的云点信息将导致后续通过激光SLAM算法得出的地图存在累计误差，最终导致不准确；另外由于在导航过程中如果由于机器运动偏差使得机器人与导航路径存在偏差这样的话也会导致偏差一致会累计从而导致导航不准确的问题；

现有技术如中国专利申请号CN201810534991.X，申请公布日：2018.12.11，其公开了一种基于激光导航AGV惯导航向角原点变换及误差补偿算法，步骤如下：在有激光slam检测数据时航向角为A，惯导器件航向角为B，那么零度角偏差C为A-B；通过对激光slam航向角变量a与惯导航向角变量b的积分，记录航向角累计旋转角度；得出惯导航向角的误差模型，在机器人惯性导航期间对惯导航向角

进行误差补偿；设定目标航向角dir与当前航向角的偏差值angle，基于零度角统一，惯导航向角误差补偿计算，得出更准确的偏差值，本发明将惯导器件航向角的零度角与其统一，并实时计算出惯导航向角累计误差模型，从而对惯导航向角进行误差补偿；降低当前惯导器件的累计误差，可以将惯导器件的输出数据统一化，但是该文献是采用SLAM检测到的航向角对惯性系统的惯导向器件的航向角进行误差矫正，其并没有考虑激光SLAM自身位置以及角度的偏差从而导致激光SLAM定位地图形成累计误差，从而导致定位不准确的位置。

发明内容

为解决现有技术中存在的问题，本发明提供一种基于激光SLAM的无人叉车，该无人叉车地图构建准确，导航误差小。

为达到上述目的，本发明的技术方案是：基于激光SLAM的无人叉车，具体的控制方法，包括以下步骤：

S1：接通电源；

S2：无人叉车通过激光SLAM算法构建地图步骤，基于激光SLAM算法构建地图的步骤具体包括：

4)启动激光传感器获取无人叉车当前时刻附近目的物的云点位置信息以及云点角度信息，利用扩展卡尔曼滤波算法对当前时刻云点位置信息校正得到当前时刻云点位置信息的最优值以及位置信息校正方程，利用扩展卡尔曼滤波算法对当前时刻云点角度信息进行校正得到当前时刻云点位置信息的最优值以及角度信息校正方程；

利用扩展卡尔曼滤波算法进行校正具体步骤包括：

第1步：k时刻的预测值＝k-1时刻的值+变化值*单位时间,即

X(k丨k-1)＝A*X(k-1丨k-1)+B*U(k) (1)

其中，X(k丨k-1)是利用上一状态预测的结果，X(k-1丨k-1)是上一状态最优的结果，U(k)为现在状态的控制量；

第2步：根据上一时刻的状态转移矩阵P，P初始时为单位矩阵，加上预测噪音协方差矩阵Q，Q为预计误差，推算出当前k时刻的误差矩阵，

即P(k丨k-1)＝A*P(k-1丨k-1)*A'T+Q (2)

其中P(k丨k-1)是X(k丨k-1)对应的协方差，P(k-1丨k-1)是X(k-1丨k-1)对应的协方差，A'表示A的转置矩阵，Q是系统过程的协方差；

第3步：根据得到的k时刻误差矩阵和测量噪声协方差矩阵R，R为测量误差，推算出k时刻卡尔曼增益，

即Kg(k)＝P(k丨k-1)*H'/(H*P(k丨k-1))*H'+R) (3)

第4步：k时刻的预测值+k时刻卡尔曼增益*(k时刻测量值-观测矩阵*k时刻的预测值)＝k时刻的值，

即X(k丨k)＝X(k丨k-1)+Kg(k)*(Z(k)-H*X(k丨k-1) (4)

其中X(k丨k)是由k时刻的预算值和k时刻测量值合成的估算值；

第5步：更新状态转移矩阵P，用于下一次迭代，

即P(k丨k)＝(1-Kg(k)H)*P(k丨k-1) (5)

5)根据步骤1)得到的云点位置信息最优值以及云点角度最优值利用激光SLAM算法得到构建当前时刻环境；

6)按照步骤1)和2)进行下一时刻当前时刻环境构建，最终形成地图；

S3：确定无人叉车的导航路径，从起始点开始，每间隔一预设时间通过无人叉车上视觉传感器获取当前位置的地标信息，然后利用当前位置的地标信息对当前位置的位置信息进行矫正。

本发明采用扩展卡尔曼滤波算法通过前一时刻的云点信息对激光传感器获得目标物当前时刻的云点位置信息以及云点角度信息进行校正得到最优的云点信息用于构建地图，从而使得构建地图时所使用的云点比较准确，进而构建地图较准确，防止由于灰尘、雨滴等物存在导致获得云点信息不准确进而导致构建地图不准确的问题，另外在无人叉车导航过程中，由于在导航行走过程中，通过即使获取对应的地标信息作为无人叉车的绝对位置信息对无人叉车实际位置信息进行校正并通过控制无人叉车的两侧轮的速度差将无人叉车引导到正确的导航路径上，防止无人叉车行走系统误差从而导致实际行走的路线与导航路径不相同从而导致导航不准确的问题，导航误差小。

进一步的，步骤1)中包括：利用视觉传感器获取无人叉车当前时刻的地标信息，

步骤2)为：根据步骤1)得到的云点位置信息最优值以及云点角度最优值、当前时刻地标信息利用激光SLAM算法得到构建当前时刻环境。

以上设置，由于在步骤1)中引入检测地标信息，从而使得地标信息也能放入到构建的地图中，从而便于后续地标信息在图中显示，同时也能方便地图构建的验证。

进一步的，所述的地标信息为二维码信息，步骤S3中利用当前位置的地标信息对当前位置的位置信息进行矫正的具体步骤包括：通过二维码信息获得当前位置并确定当前位置的位置误差d和当前位置的角度误差θ，并将两个误差通过比例权重的融合，合成合成误差e，公式如下:

e＝d+L*θ

其中，L为权重系数,

在行进过程中，通过调整无人叉车两侧车轮的速度差进行位置和角度调整，直至行进至导航路径上下一个二维码信息上。

以上控制方法，利用地标信息获得的位置信息以及角度信息合成误差值进行综合校正，防止仅仅校正位置不校正角度信息导致后续继续出现偏差的问题存在，该误差的合成方式简单且可靠，同时利用在角度信息前设置权重系数，从而可以通过调整权重系数控制合成误差中位置误差和角度误差的影响比重，当需要更加注重角度精准度时，只需将对应的权重系数调节大一点即可，当需要更加注重位置的精准度时，只需要将权重系数调节小一点即可，另外通过调整两侧车轮的速度差调整当前时刻的位置和角度，从而使得无人叉车回到导航路径上，防止由于无人叉车行驶过程中出现偏离导航路径时利用二维码信息对无人叉车行驶进行校正从而使得无人叉车回到正确的导航路径上。

进一步的，步骤S3中预设时间大小等于步骤S2中下一时刻与当前时刻之间的差值，

以上设置，将导航步骤中预设的采集时间设置为与S2步骤中云点信息获取时间间隔值相等，从而使得采用与云点信息获取时间间隔去进行导航行走过程中实时位置信息的获取，从而能使得导航过程与地图构建过程中无人叉车的行进方式相同，能确保导航更加准确。

进一步的，在步骤S3之前包括：S21：地图构建验证，具体步骤包括：将照步骤S2中无人叉车的路径设置导航路径，无人叉车以步骤S2中无人叉车行走方式按照导航路径行走，在采集时间内获取地标信息，并将获得的地标信息与导航路径当前位置信息进行比较，若地标信息中位置信息与导航路径当前位置信息偏差超过一定距离，则以当前位置点作为起始点按照步骤S2方式对当前位置点到目的点之间环境进行重新构建，最后将从起始点到当前位置点之间已存储的环境信息、从当前位置点到目的点重新构建环境信息构建成新的地图。

以上设置，地图构建与无人叉车导航步骤之间增设地图构建验证步骤，以地图构建时无人叉车相同的速度以及转向等方式进行行走并采集地标信息，如果获得的地标信息与导航路径信息存在偏差较大，则以当前地标信息为分界点将已构建的地图分成两个子空间，对从分界点开始到目的点之间的环境信息进行重新构建，然后与已存在的从起始点到分界点之间的环境信息整合构建成新的地图，通过增加验证步骤，利用地标信息获取的绝对位置信息对已构建的地图进行进一步核对，进一步提高地图构建的准确性，防止由于在行进过程中机械操作偏差导致位置误差进而影响地图构建准确性的发生。

进一步的，若地标信息中位置信息与导航路径当前位置信息偏差未超过一定距离，则无人叉车沿着导航路径继续行走，

以上设置，若地标信息中位置信息与导航路径当前位置信息偏差不大，无人叉车无需进行地图调整，进行下一位置点地图验证。

进一步的，控制方法包括：在合成误差之后利用PID算法对权重系数K进行修正得到合成误差最优值，

以上设置，为了防止合成误差没有及时消除从而导致合成误差累计叠加，引入PID算法进行修正，消除累计误差。

进一步的，步骤S3中“在行进过程中，通过调整无人叉车两侧车轮的速度差进行位置和角度调整，直至行进至下一个地标信息上”具体包括：根据合成误差值按照预设的误差值与两侧车轮之间的对应关系表调整两侧车轮的速度差从而达到位置和角度调整，

以上设置，根据合成误差与两侧车轮速度差实现调整无人叉车的位置和角度，从而使得无人叉车能准确地切换到下一个正确的地标信息上。

附图说明

图1为本发明基于激光SLAM控制方法一种实施例的流程图。

图2为本发明无人叉车在二维码信息的直角坐标系中位置示意图。

图3为本发明模拟PID控制系统原理框图。

具体实施方式

下面结合附图和具体实施方式对本发明做进一步详细说明。

如图1～图3所示，基于激光SLAM的无人叉车，包括车体，在车体的左右两侧分别设有车轮，无人叉车行走的地面上设有地标信息，地标信息可以为二位码信息等，本实施例中，所述的地标信息为二维码信息，该无人叉车的控制方法，包括以下步骤：

S1：接通电源；

S2：无人叉车通过激光SLAM算法构建地图步骤，基于激光SLAM算法构建地图的步骤具体包括：

1)启动激光传感器获取无人叉车当前时刻附近目的物的云点位置信息以及云点角度信息，利用扩展卡尔曼滤波算法对当前时刻云点位置信息校正得到当前时刻云点位置信息的最优值以及位置信息校正值，利用扩展卡尔曼滤波算法对当前时刻云点角度信息进行校正得到当前时刻云点位置信息的最优值以及角度信息校正值；启动视觉传感器获取无人叉车当前位置的地标信息，地标信息可以设置在无人叉车所行走路面上的多个二维码信息；利用视觉传感器获取无人叉车当前时刻的地标信息，

利用扩展卡尔曼滤波算法进行校正具体步骤包括利用扩展卡尔曼滤波算法对当前时刻云点位置信息进行校正步骤、利用扩展卡尔曼滤波算法对当前时刻云点角度信息进行校正步骤，

其中利用扩展卡尔曼滤波算法对当前时刻云点位置信息进行校正步骤包括：

第1.1步：k时刻的预测值＝k-1时刻的值+变化值*单位时间,

即X(k丨k-1)＝A*X(k-1丨k-1)+B*U(k) (1)

其中，X(k丨k-1)是利用上一状态预测的当前时刻k的云点位置信息，X(k-1丨k-1)是上一状态云点位置信息最优值，U(k)为现在状态的控制量；

第1.2步：根据上一时刻的状态转移矩阵P，P初始时为单位矩阵，加上预测噪音协方差矩阵Q，Q为预计误差，推算出当前时刻k的误差矩阵，

即P(k丨k-1)＝A*P(k-1丨k-1)*A'T+Q (2)

其中P(k丨k-1)是X(k丨k-1)对应的协方差，P(k-1丨k-1)是X(k-1丨k-1)对应的协方差，A'表示A的转置矩阵，Q是系统过程的协方差；

第1.3步：根据得到的k时刻误差矩阵和测量噪声协方差矩阵R，R为测量误差，推算出k时刻云点位置信息卡尔曼增益，

即Kg(k)＝P(k丨k-1)*H'/(H*P(k丨k-1))*H'+R) (3)

第4步：k时刻的预测值+k时刻卡尔曼增益*(k时刻测量值-观测矩阵1.*k时刻的预测值)＝k时刻的值，

即X(k丨k)＝X(k丨k-1)+Kg(k)*(Z(k)-H*X(k丨k-1)) (4)

其中X(k丨k)是由k时刻的云点位置信息预算值和k时刻云点位置信息测量值(该测量值通过激光传感器测得)合成的估算值，该估算值即为当前时刻云点位置信息的最优值；

第1.5步：对式(4)求协方差并更新状态转移矩阵P，用于下一次迭代，

即P(k丨k)＝(1-Kg(k)H)*P(k丨k-1) (5)

建立模型的单位时间、预测噪音协方差矩阵Q为预设定值，状态转移矩阵P初始时为单位矩阵；

当前时刻云点位置信息校正值包括云点位置信息卡尔曼增益Kg(k)以及当前时刻状态转移矩阵P。

其中利用扩展卡尔曼滤波算法对当前时刻云点角度信息进行校正步骤包括：

第2.1步：k时刻的预测值＝k-1时刻的值+变化值*单位时间,

即X(k丨k-1)＝A*X(k-1丨k-1)+B*U(k) (1)

其中，X(k丨k-1)是利用上一状态预测的当前时刻k的云点角度信息，X(k-1丨k-1)是上一状态云点角度信息最优值，U(k)为现在状态的控制量；

第2.2步：根据上一时刻的状态转移矩阵P，P初始时为单位矩阵，加上预测噪音协方差矩阵Q，Q为预计误差，推算出当前时刻k的误差矩阵，

即P(k丨k-1)＝A*P(k-1丨k-1)*A'T+Q (2)

其中P(k丨k-1)是X(k丨k-1)对应的协方差，P(k-1丨k-1)是X(k-1丨k-1)对应的协方差，A'表示A的转置矩阵，Q是系统过程的协方差；

第2.3步：根据得到的k时刻误差矩阵和测量噪声协方差矩阵R，R为测量误差，推算出k时刻云点角度信息卡尔曼增益，

即Kg(k)＝P(k丨k-1)*H'/(H*P(k丨k-1))*H'+R) (3)

第2.4步：k时刻的预测值+k时刻卡尔曼增益*(k时刻测量值-观测矩阵*k时刻的预测值)＝k时刻的值，

即X(k丨k)＝X(k丨k-1)+Kg(k)*(Z(k)-H*X(k丨k-1)) (4)

其中X(k丨k)是由k时刻的云点角度信息预算值和k时刻云点角度信息测量值(该测量值通过激光传感器测得)合成的估算值；

第2.5步：对式(4)求协方差并更新状态转移矩阵P，用于下一次迭代，

即P(k丨k)＝(1-Kg(k)H)*P(k丨k-1) (5)

预测噪音协方差矩阵Q为预设定值，状态转移矩阵P初始时为单位矩阵；

当前时刻云点角度信息校正值包括云点角度信息卡尔曼增益Kg(k)以及当前时刻状态转移矩阵P。

2)根据步骤1)得到的云点位置信息最优值以及云点角度最优值、当前时刻地标信息利用激光SLAM算法得到构建当前时刻环境；环境信息指由多个云点信息形成的物体的形状和结构，激光SLAM算法为现有算法，包括但是不限于Gmapping算法等。

3)按照步骤1)和2)进行下一时刻当前时刻环境构建，最终形成地图；

S21：地图构建验证，具体步骤包括：将照步骤S2中无人叉车的路径设置导航路径，无人叉车以步骤S2中无人叉车行走方式按照导航路径行走，在采集时间内获取地标信息，并将获得的地标信息与导航路径位置信息进行比较，

21-1、若地标信息中位置信息与导航路径当前位置信息偏差超过一定距离(该距离为系统内预设值)，则以当前地标位置作为分界点，然后以分界点作为起始点按照步骤S2方式对分界点到目的点(该目的点为首次构建地图时目的点)之间环境进行重新构建，将从起始点到分界点之间已存储的环境信息(该环境信息为首次构建时存储的云点位置信息最优值和云点角度信息最优值形成的环境信息)、从分界点到目的点重新构建环境信息替换已存储的从分界点到目的地构建的环境信息，然后构建成新地图并进行存储。

从分界点到目的点重新构建环境信息的步骤如下：

21.1)启动激光传感器获取无人叉车当前时刻附近目的物的云点位置信息以及云点角度信息，利用扩展卡尔曼滤波算法对当前时刻云点位置信息校正得到当前时刻云点位置信息的最优值以及位置信息校正值，利用扩展卡尔曼滤波算法对当前时刻云点角度信息进行校正得到当前时刻云点位置信息的最优值以及角度信息校正值；启动视觉传感器获取无人叉车当前位置的地标信息，地标信息可以设置在无人叉车所行走路面上的多个二维码信息；利用视觉传感器获取无人叉车当前时刻的地标信息，

21.2)根据步骤21.1)得到的云点位置信息最优值以及云点角度最优值、当前时刻地标信息利用激光SLAM算法得到构建当前时刻环境信息；

由云点信息构建环境信息并最终形成地图采用的算法为步骤S2中使用的激光SLAM算法，为现有算法在此不再累述。

21-2、若地标信息中位置信息与导航路径当前位置信息偏差未超过一定距离，则无人叉车沿着导航路径继续行走，并确定校正后的地图为原始构建形成的地图。

S3：确定无人叉车的导航路径，从起始点开始，每间隔一预设时间通过无人叉车上视觉传感器获取当前位置的地标信息，然后利用当前位置的地标信息对当前位置的位置信息进行校正，所述地标信息为二维码信息，利用当前位置的地标信息对当前位置信息进行校正具体步骤包括：通过二维码信息获得当前位置并确定当前位置的位置误差d和角度误差θ，并将两个误差通过比例权重的融合，合成合成误差e，公式如下:

e＝d+L*θ

其中，L为权重系数,

如图2所示，L1为实际位置轨迹，L2为导航路径，无人叉车在二维码信息13a上通过视觉传感器获得当前位置的位置信息，然后将该当前位置的位置信息与导航路径上设定的位置信息利用两点距离公式求出距离d，通过视觉传感器获得无人叉车当前位置的角度信息，然后将当前位置的角度信息与导航路径得到的角度信息相差得到角度误差θ，以及得到合成误差e后，

在本实施例中每个二维码信息13a都为一个4×4的方形二维码阵列，阵列中包含16个小二维码，以二维码信息13a中心整个构建地图中坐标为二维码信息13a的绝对位置坐标，以二维码信息的中心点为原点，以直角坐标系的x、y方向为基准，建立一个小直角坐标系，如图2所示，每个小二维码中记录有小二维码在小直角坐标中的坐标即小二维码相对于二维码信息13a的相对坐标信息，无人叉车到达小二维码上方，视觉传感器获得小二维码的位置信息，从而得到无人叉车相对于二维码信息13a中心点的位置误差d和角度误差θ，将两个误差通过比例权重的融合，合成融合误差e，在无人叉车向下一个二维码信息13a的行进路途中，按照目标路径，以下一个二维码信息13a中心为目标，进行位置和角度的校正；利用上述合成合成误差e的公式对位置误差d和角度误差θ进行融合时，调整权重系数k的值，使融合误差e越小越好，e越小，无人叉车行走越平稳，同时，L值越大，对角度误差θ的调节越精确，角度误差越小，与工作台的对接越好，L值越小，对位置误差d的调节越精确，位置误差越小，因此根据实际使用无人叉车情况，兼顾位置误差和角度误差，调出最优L值，得到合成误差e后，利用PID算法，对合成误差进行校正，

PID调节器是一种线型调节器，PID控制也称比例-积分-微分控制，模拟PID控制系统的原理如下：如图3所示，其中比例环节是用来放大误差，来纠正偏差的；积分环节用来消除系统的稳态误差；微分环节用来提高系统的快速性，积分项和微分项要用计算机实现，就要将这一的系统转化为差分方程，将给定值r(t)与实际输出值c(t)的偏差的比例(P)、积分(I)、微分(D)通过线性组合构成控制量，对控制对象进行控制，得到PID控制器的基本算法：

其中，e(t)＝r(t)-c(t)，其中，TD为微分时间，T_i为积分时间，K_p为调节器的放大系数。

在无人叉车行进过程中，根据预设的合成误差e与对应无人叉车两轮之间速度差之间的对应关系表，确定无人叉车两轮调节的速度差并以该速度差进行行走到导航路径上下一个二维码信息上，继续导航；当无人叉车行进过程中，合成误差偏差过大(即合成误差超过一预设值)，脱离导航路径一定距离(系统预设的距离)，无法到达下一个二维码信息13a上时，控制调度下发指令寻回，使其回到指定目标路径上。

本发明采用扩展卡尔曼滤波算法通过前一时刻的云点信息对激光传感器获得目标物当前时刻的云点位置信息以及云点角度信息进行校正得到最优的云点信息用于构建地图，从而使得构建地图时所使用的云点比较准确，进而构建地图较准确，防止由于灰尘、雨滴等物存在导致获得云点信息不准确进而导致构建地图不准确的问题，另外在无人叉车导航过程中，由于在导航行走过程中，通过即使获取对应的地标信息作为无人叉车的绝对位置信息对无人叉车实际位置信息进行校正并通过控制无人叉车的两侧轮的速度差将无人叉车引导到正确的导航路径上，防止无人叉车行走系统误差从而导致实际行走的路线与导航路径不相同从而导致导航不准确的问题，导航更准确。

Claims (8)

1.基于激光SLAM的无人叉车，其特征在于：具体的控制方法，包括以下步骤：

S1：接通电源；

S2：无人叉车通过激光SLAM算法构建地图步骤，基于激光SLAM算法构建地图的步骤具体包括：

1)启动激光传感器获取无人叉车当前时刻附近目的物的云点位置信息以及云点角度信息，利用扩展卡尔曼滤波算法对当前时刻云点位置信息校正得到当前时刻云点位置信息的最优值以及位置信息校正方程，利用扩展卡尔曼滤波算法对当前时刻云点角度信息进行校正得到当前时刻云点位置信息的最优值以及角度信息校正方程；

利用扩展卡尔曼滤波算法进行校正具体步骤包括：

第1步：k时刻的预测值＝k-1时刻的值+变化值*单位时间,

即X(k丨k-1)＝A*X(k-1丨k-1)+B*U(k) (1)

其中，X(k丨k-1)是利用上一状态预测的结果，X(k-1丨k-1)是上一状态最优的结果，U(k)为现在状态的控制量；

第2步：根据上一时刻的状态转移矩阵P，P初始时为单位矩阵，加上预测噪音协方差矩阵Q，Q为预计误差，推算出当前k时刻的误差矩阵，

即P(k丨k-1)＝A*P(k-1丨k-1)*A'T+Q (2)

其中P(k丨k-1)是X(k丨k-1)对应的协方差，P(k-1丨k-1)是X(k-1丨k-1)对应的协方差，A'表示A的转置矩阵，Q是系统过程的协方差；

第3步：根据得到的k时刻误差矩阵和测量噪声协方差矩阵R，R为测量误差，推算出k时刻卡尔曼增益，

即Kg(k)＝P(k丨k-1)*H'/(H*P(k丨k-1))*H'+R) (3)

第4步：k时刻的预测值+k时刻卡尔曼增益*(k时刻测量值-观测矩阵*k时刻的预测值)＝k时刻的值，

即X(k丨k)＝X(k丨k-1)+Kg(k)*(Z(k)-H*X(k丨k-1)) (4)

其中X(k丨k)是由k时刻的预算值和k时刻测量值合成的估算值；

第5步：根据更新状态转移矩阵P，用于下一次迭代，

即P(k丨k)＝(1-Kg(k)H)*P(k丨k-1) (5)；

2)根据步骤1)得到的云点位置信息最优值以及云点角度最优值利用激光SLAM算法得到构建当前时刻环境；

3)按照步骤1)和2)进行下一时刻当前时刻环境构建，最终形成地图；

S3：确定无人叉车的导航路径，从起始点开始，每间隔一预设时间通过无人叉车上视觉传感器获取当前位置的地标信息，然后利用当前位置的地标信息对当前位置信息进行矫正。

2.根据权利要求1所述的基于激光SLAM的无人叉车，其特征在于：步骤1)中还包括：利用视觉传感器获取无人叉车当前时刻的地标信息，

步骤2)为：根据步骤1)得到的云点位置信息最优值以及云点角度最优值、当前时刻地标信息利用激光SLAM算法得到构建当前时刻环境。

3.根据权利要求2所述的基于激光SLAM的无人叉车，其特征在于：所述的地标信息为二维码信息，步骤S3中利用当前位置的地标信息对当前位置的位置信息进行校正的具体步骤包括：通过二维码信息获得当前位置并确定当前位置的位置误差D和当前位置的角度误差θ，并将两个误差通过比例权重的融合，合成合成误差e，公式如下:

e＝d+L*θ

其中，L为权重系数,

在行进过程中，通过调整无人叉车两侧车轮的速度差进行位置和角度调整，直至行进至导航路径上下一个二维码信息上。

4.根据权利要求1所述的基于激光SLAM的无人叉车，其特征在于：步骤S3中预设时间大小等于步骤S2中下一时刻与当前时刻之间的差值。

5.根据权利要求2所述的基于激光SLAM的无人叉车，其特征在于：在步骤S3之前包括：S21：地图构建验证，具体步骤包括：将照步骤S2中无人叉车的路径设置导航路径，无人叉车以步骤S2中无人叉车行走方式按照导航路径行走，在采集时间内获取地标信息，并将获得的地标信息与导航路径当前位置信息进行比较，若地标信息中位置信息与导航路径当前位置信息偏差超过一定距离，则以当前位置点作为起始点按照步骤S2方式对当前位置点到目的点之间环境进行重新构建，最后将从起始点到当前位置点之间已存储的环境信息、从当前位置点到目的点重新构建环境信息构建成新的地图。

6.根据权利要求5所述的基于激光SLAM的无人叉车，其特征在于：若地标信息中位置信息与导航路径当前位置信息偏差未超过一定距离，则无人叉车沿着导航路径继续行走。

7.根据权利要求3所述的基于激光SLAM的无人叉车，其特征在于：控制方法还包括：在合成误差之后利用PID算法对权重系数K进行修正得到合成误差最优值。

8.根据权利要求7所述的基于激光SLAM的无人叉车，其特征在于：步骤S3中“在行进过程中，通过调整无人叉车两侧车轮的速度差进行位置和角度调整，直至行进至下一个地标信息上”具体包括：根据得到的合成误差最优值按照预设的合成误差最优值与两侧车轮之间的对应关系表调整两侧车轮的速度差从而达到位置和角度调整。