阅读说明：本技术 基于lstm模型的室内自主导航agv运动轨迹融合方法 (Indoor autonomous navigation AGV (automatic guided vehicle) motion track fusion method based on LSTM (least squares metric model) ) 是由 全燕鸣 黄令苇 马磊 何一明 全思博 于 2019-10-28 设计创作，主要内容包括：本发明公开了一种基于LSTM模型的室内自主导航AGV运动轨迹融合方法,该方法首先采集AGV在一次运动的过程中里程计、惯性测量单元、经卡尔曼滤波融合里程计和惯性测量单元数据后的数据,经处理后得到AGV角度数据,将角度数据进行分组得到训练集和测试集,利用训练集对搭建的LSTM网络进行训练,测试集测试训练结果；将经过训练的模型部署上线后,向模型实时输入通过里程计和惯性测量单元得到的角度数据,得到实时的AGV角度,结合该角度和里程计计算AGV的运动轨迹。本发明在保证精度的基础上,不需要确定噪声的统计特性,不易受到外界环境影响,节约了时间成本,且不需要在输入传感器数据时进行传感器数据的同步。(The invention discloses an indoor autonomous navigation AGV movement track fusion method based on an LSTM model, which comprises the steps of firstly collecting data of a mileometer, an inertia measurement unit and a Kalman filtering fusion mileometer and the inertia measurement unit of an AGV in the process of one-time movement, processing the data to obtain AGV angle data, grouping the angle data to obtain a training set and a test set, training a built LSTM network by using the training set, and testing the training result by using the test set; after the trained model is deployed on line, angle data obtained through the odometer and the inertia measurement unit are input into the model in real time to obtain a real-time AGV angle, and the motion trail of the AGV is calculated by combining the angle and the odometer. On the basis of ensuring the precision, the invention does not need to determine the statistical characteristic of the noise, is not easily influenced by the external environment, saves the time cost, and does not need to synchronize the sensor data when inputting the sensor data.)

基于LSTM模型的室内自主导航AGV运动轨迹融合方法

技术领域

本发明涉及室内AGV信息感知与数据融合技术领域，具体涉及一种基于LSTM模型的室内自主导航AGV运动轨迹融合方法。

背景技术

对于能够实现室内自主导航的自动导引运输车，以下简称室内自主导航AGV(Automated Guided Vehicle)，要使它能够保持长期正常工作的状态，最重要的一点是要保证它对自身的定位准确。然而，仅靠其自身内部的里程计很难对AGV的状态进行正确的估计，它是依靠车轮转动圈数来对AGV的位置进行估计的，但由于存在车轮打滑和累计误差的情况，所以里程计会出现一定的偏差。因此需要多种传感器互相协作来完成这一任务，实现AGV对自身位置的准确估计，其中十分常用的传感器是惯性测量单元。

数据融合方法主要包括加权平均法、卡尔曼滤波法、多贝叶斯估计法、D-S证据推理方法、产生式规则法、模糊逻辑推理法、人工神经网络法等。其中，由于卡尔曼滤波方法能够用于任何含有不确定信息的动态系统，对系统下一步的状态做出有根据的预测，所以当前人们经常使用卡尔曼滤波法对从传感器上获取的信息进行数据融合，来对AGV的姿态和位置进行估计。

虽然使用卡尔曼滤波法的融合效果比较精确，但它仍然存在一定的限制。该方法在使用时需要预先确定噪声的统计特性，且在面对复杂多变的环境时，噪声的统计特性容易受到外界环境的影响而改变。因此当有设备更换或者外界环境发生改变时，经常需要重新通过实验来确定噪声的统计特性，这会占用大量的时间成本，对于科学研究或者产品的实际使用都是不利的。

传统的人工神经网络融合方法多是基于BP神经网络、SOFM等浅层网络模型，这些模型易出现过拟合、模型训练陷入局部极小、收敛速度过慢等问题，导致算法效率降低、特征提取分类能力变弱，且无法有效地处理高维数据。但是随着近年来人工智能算法发展迅速，人工神经网络方法又重新进入人们的视野，使用该方法可以不需要预先确定噪声的统计特性，且实现过程简单，提高了系统对不同环境的适应能力。

发明内容

本发明的目的是为了能够降低时间成本，提高系统对环境的适应性，提供一种基于LSTM模型的室内自主导航AGV运动轨迹融合方法，该方法采集各种传感器数据并进行预处理，得到能输入到LSTM模型中的数据格式，将这些数据输入LSTM模型，对该模型进行训练，在使用时通过直接将训练完毕的模型部署在待使用的软件平台上来完成对多传感器数据的实时融合。

本发明的目的可以通过采取如下技术方案达到：

一种基于LSTM模型的室内自主导航AGV运动轨迹融合方法，所述的运动轨迹融合方法包括以下步骤：

S1、采集数据：

首先设置参考坐标系，以AGV启动位置为原点，启动位置的车头方向为x轴正向，启动位置车头朝向的左方向为y轴正向，垂直地面向上的方向为z轴正向，AGV逆时针旋转为正。控制AGV在一空旷范围内以一定的线速度和角速度***，保证AGV在结束运动时，其至少绕z轴旋转了360度。采集通过里程计(以下简称odom)测量得到的AGV偏航角(即AGV绕z轴旋转的角度，以下简称yaw)，把通过odom测量得到的yaw称为odom_yaw；采集通过惯性测量单元(以下简称imu)测量得到的yaw，把通过imu测量得到的yaw称为imu_yaw；同时通过卡尔曼滤波融合方法融合odom与imu，得到融合里程计(以下简称ekf_combined)，采集通过ekf_combined测量得到的yaw，并将其作为融合结果的期望值，把通过ekf_combined测量得到的yaw称为ekf_combined_yaw；

S2、制作数据集并划分训练集与测试集：

将odom_yaw、imu_yaw、ekf_combined_yaw分别保存到3个数组中，得到3组数据，第1组为odom_yaw；第2组为imu_yaw，第3组为ekf_combined_yaw。按照时间顺序，取第1、2组的同一位置的数据和第3组对应的下一个位置的数据为1个批次，取总批次的前70％-85％为训练集，其余为测试集；

S3、建立LSTM模型：

所采用的LSTM模型包含1个输入层，2个隐藏层，1个Dropout层和1个全连接层，训练数据按顺序通过输入层、第1个隐藏层、第2个隐藏层、Dropout层、全连接层，最后输出得到融合数据，称LSTM模型输出的融合数据为lstm_combined_yaw；

S4、训练并保存LSTM模型：

每个隐藏层采用的激活函数为Relu，优化器采用Adam方法，损失函数采用均方差MSE；初始学习率为0.001，输入训练数据对LSTM模型进行训练，训练完成后保存到指定路径；

S5、部署模型上线，实现实时融合数据：

利用gRPC框架，分别建立服务端和客户端，服务端将已训练的模型部署至线上，客户端接收待融合的odom_yaw与imu_yaw作为其输入，输出lstm_combined_yaw；

S6、根据odom实时测量得到的AGV位移和LSTM模型实时输出的lstm_combined_yaw计算AGV的运动轨迹。

进一步地，所述的步骤S1的实现过程如下：

为保证数据集质量，需要采集同一时刻的odom_yaw、imu_yaw、ekf_combined_yaw，由于odom、imu、ekf_combined直接输出的数据是四元数，所以需要将四元数转换为yaw，四元数是表示物体在三维空间中旋转的一种方法，它包含四个参数：x，y，z，w；x表示物体绕x轴的旋转，y表示物体绕y轴的旋转，z表示物体绕z轴的旋转，且有约束如下式：

x²+y²+z²+w²＝1

根据下式计算把四元数转换为yaw:

yaw＝arctan{2(w×z+x×y)÷[1-2(y²+z²)]}

其中，x、y、z、w为传感器测得的四元数的4个参数，即x表示AGV绕x轴的旋转，y表示AGV绕y轴的旋转，z表示AGV绕z轴的旋转。

进一步地，所述的步骤S2中一个批次的具体组成描述为：

一个批次的数据包含有1个odom_yaw值，1个imu_yaw值，以及1个ekf_combined_yaw值，表示为：

ODOM_YAW_i＝{odom_yaw_t}

IMU_YAW_i＝{imu_yaw_t}

EKF_COMBINED_YAW_i＝{ekf_combined_yaw_t+Δt}

其中，t为该组数据中第一个数据开始采集的时间，Δt为采集间隔，ODOM_YAW_i为第i个批次中的odom_yaw，IMU_YAW_i为第i个批次中的imu_yaw，EKF_COMBINED_YAW_i为第i个批次中的ekf_combined_yaw。

进一步地，所述的步骤S3中所建立的LSTM模型，输入层包含2个神经元，分别输入前一个时刻的odom_yaw和imu_yaw；输出层只包含1个神经元，输出lstm_combined_yaw；每个隐藏层包含128个LSTM单元，LSTM单元除神经元外，还包括遗忘门、输入门、输出门，遗忘门决定从神经元中舍弃的信息量，输入门决定向神经元中保存的信息量，输出门决定从神经元中输出的信息量；

LSTM单元的关键公式如下：

f_t＝σ(W_f·[h_t-1,x_t]+b_f)

i_t＝σ(W_i·[h_t-1,x_t]+b_i)

C_t0＝tanh(W_c·[h_t-1,x_t]+b_c)

C_t＝f_t×C_t-1+i_t×C_t0

o_t＝σ(W_o·[h_t-1,x_t]+b_o)

h_t＝o_t×tanh(C_t)

其中，f_t，i_t，C_t0，C_t，o_t，h_t，h_t-1分别表示遗忘门限，输入门限，前一时刻神经元状态，当前神经元状态，输出门限，当前神经元的输出，前一时刻神经元的输出；W_f，W_i，W_c，W_o分别表示的是遗忘门、输入门、神经元、输出门的权重矩阵；b_f，b_i，b_c，b_o分别表示的是遗忘门、输入门、神经元、输出门的偏置向量；tanh为激活函数；σ为sigmod函数。

进一步地，所述的步骤S4中隐藏层激活函数Relu公式：

Relu(x)＝max(0,x)

损失函数为均方差，其计算公式:

其中，y_i是输入的训练值，y’_i是LSTM输出的预测值，n为数据总批次数，共训练100轮，每次训练包含72个批次，训练采用Google开源的keras深度学习框架，训练完成后保存模型到指定路径。

进一步地，所述的步骤S5实现过程如下：

服务端将保存的LSTM模型部署上线，等待客户端向服务端发送请求，控制AGV行走一个圆形，同时客户端实时处理odom与imu测得的数据，获得odom_yaw和imu_yaw后将其通过客户端发送给服务端；服务端接收到客户端请求后给予响应，返回输出结果lstm_combined_yaw。

进一步地，所述的步骤S6中计算AGV运动轨迹的过程如下：

首先计算odom测得的位移量，计算公式为：

Δx＝odom_X1-odom_X0

Δy＝odom_Y1-odom_Y0

其中，odom_X1、odom_Y1表示当前时刻odom测得的小车位置，odom_X0、odom_Y0表示前一时刻odom测得的小车位置，Δx、Δy、Δs分别表示odom测得的AGV在x方向的位移量、AGV在y方向的位移量、两个时刻之间的直线距离；

结合lstm_combined_yaw，得到融合后AGV在坐标系中的位置，计算方法如下：

ΔX＝Δs×cos(lstm_combined_yaw)

ΔY＝Δs×sin(lstm_combined_yaw)

lstm_combined_x＝lstm_combined_x+ΔX

lstm_combined_y＝lstm_combined_y+ΔY

其中ΔX、ΔY分别表示AGV按lstm_combined_yaw这个角度运动时在x方向和y方向的位移量，lstm_combined_x、lstm_combined_y分别表示融合后AGV在坐标系中的x、y坐标，lstm_combined_x和lstm_combined_y在AGV启动时初始化为0。

本发明相对于现有技术具有如下的优点及效果：

(1)对比卡尔曼滤波融合方法，本发明不需要预先确定噪声的统计特性，节约了进行数据融合的时间成本；

(2)对比卡尔曼滤波融合方法，本发明在处理传感器数据时，不要求对传感器的输入数据进行时间同步，简化了计算步骤；

(3)对比卡尔曼滤波融合方法，本发明不受外部环境的影响，具有更好的鲁棒性；

(4)对比传统的神经网络数据融合方法，本发明使用LSTM模型，有助于利用传感器数据的时序性，提高数据融合精度；

附图说明

图1是本发明中公开的基于LSTM模型的室内自主导航AGV运动轨迹融合方法流程图；

图2是LSTM模型结构示意图

图3是本发明实施例中采集训练集时AGV的行走轨迹示意图；

图4是LSTM单元结构示意图；

图5是本发明实施例中采用融合算法前后AGV的轨迹对比图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一

如图1所示，本实施例提供一种基于LSTM模型的室内自主导航AGV运动轨迹融合方法，包括以下6个步骤：

S1、采集数据：

首先设置参考坐标系，以AGV启动位置为原点，启动位置的车头方向为x轴正向，启动位置车头朝向的左方向为y轴正向，垂直地面向上的方向为z轴正向，AGV逆时针旋转为正。控制AGV在一空旷范围内以一定的线速度和角速度***，保证AGV在结束运动时，其至少绕z轴旋转了360度。采集通过里程计(以下简称odom)测量得到的AGV偏航角(即AGV绕z轴旋转的角度，以下简称yaw)，把通过odom测量得到的yaw称为odom_yaw；采集通过惯性测量单元(以下简称imu)测量得到的yaw，把通过imu测量得到的yaw称为imu_yaw；同时通过卡尔曼滤波融合方法融合odom与imu，得到融合里程计(以下简称ekf_combined)，采集通过ekf_combined测量得到的yaw，并将其作为融合结果的期望值，把通过ekf_combined测量得到的yaw称为ekf_combined_yaw；

S2、制作数据集并划分训练集与测试集：

将odom_yaw、imu_yaw、ekf_combined_yaw分别保存到3个数组中，得到3组数据，第1组为odom_yaw；第2组为imu_yaw，第3组为ekf_combined_yaw。按照时间顺序，取第1、2组的同一位置的数据和第3组对应的下一个位置的数据为1个批次，取总批次的前70％-85％为训练集，其余为测试集；

S3、建立LSTM模型：

所采用的LSTM模型包含1个输入层，2个隐藏层，1个Dropout层和1个全连接层，训练数据按顺序通过输入层、第1个隐藏层、第2个隐藏层、Dropout层、全连接层，最后输出得到融合数据，称LSTM模型输出的融合数据为lstm_combined_yaw；

S4、训练并保存LSTM模型：

每个隐藏层采用的激活函数为Relu，优化器采用Adam方法，损失函数采用均方差MSE；初始学习率为0.001，输入训练数据对LSTM模型进行训练，训练完成后保存到指定路径；

S5、部署模型上线，实现实时融合数据：

利用gRPC框架，分别建立服务端和客户端，服务端将已训练的模型部署至线上，客户端接收待融合的odom_yaw与imu_yaw作为其输入，输出lstm_combined_yaw；

S6、根据odom实时测量得到的AGV位移和LSTM模型实时输出的lstm_combined_yaw计算AGV的运动轨迹。

实施例二

本实施例中，步骤S1的具体实施方式如下：

控制AGV在室内较空旷场地中行走轨迹如图3所示，并在期间包含AGV的旋转运动。为保证数据集质量，需要采集同一时刻的odom_yaw、imu_yaw、ekf_combined_yaw。由于odom、imu、ekf_combined直接输出的数据是四元数，所以需要将四元数转换为yaw。四元数是表示物体在三维空间中旋转的一种方法。它包含四个参数：x，y，z，w。x表示物体绕x轴的旋转，y表示物体绕y轴的旋转，z表示物体绕z轴的旋转，且有约束如下式：

x²+y²+z²+w²＝1

根据下式可计算把四元数转换为yaw:

yaw＝arctan{2(w×z+x×y)÷[1-2(y²+z²)]}

其中，x、y、z、w为传感器测得的四元数的4个参数，即x表示AGV绕x轴的旋转，y表示AGV绕y轴的旋转，z表示AGV绕z轴的旋转；

步骤S2的具体实施方式如下：

保存采集到的odom_yaw、imu_yaw、ekf_combined_yaw，得到3列数组。接下来结合表1来做进一步说明。

表1.数据分组方式表

如表1，假设每个数组均采集到15个数据，将表1中被粗边框包围的部分设为一个批次，下一个批次则将粗边框向下移动1个单元格，所以表中共有14个批次，取总批次的前80％组成训练集，后20％组成测试集，则有前11个批次组成训练集，后3个批次组成测试集。训练集和测试集又各自人为的分为两部分，训练集包含train_X和train_Y,测试集包括test_X和test_Y,其中train_X和test_X包含odom_yaw和imu_yaw，train_Y和test_Y包含ekf_combined_yaw。

步骤S3的具体实施方式如下：

所建立的LSTM模型，输入层包含2个神经元，分别输入前一个时刻的odom_yaw和imu_yaw；输出层只包含1个神经元，输出lstm_combined_yaw；每个隐藏层包含128个LSTM单元，LSTM单元除神经元外，还包括遗忘门、输入门、输出门，遗忘门决定从神经元中舍弃的信息量，输入门决定向神经元中保存的信息量，输出门决定从神经元中输出的信息量。LSTM单元的结构如图4所示。

LSTM单元的关键公式如下：

f_t＝σ(W_f·[h_t-1,x_t]+b_f)

i_t＝σ(W_i·[h_t-1,x_t]+b_i)

C_t0＝tanh(W_c·[h_t-1,x_t]+b_c)

C_t＝f_t×C_t-1+i_t×C_t0

o_t＝σ(W_o·[h_t-1,x_t]+b_o)

h_t＝o_t×tanh(C_t)

其中，f_t，i_t，C_t0，C_t，o_t，h_t，h_t-1分别表示遗忘门限，输入门限，前一时刻神经元状态，当前神经元状态，输出门限，当前神经元的输出，前一时刻神经元的输出；W_f，W_i，W_c，W_o分别表示的是遗忘门、输入门、神经元、输出门的权重矩阵；b_f，b_i，b_c，b_o分别表示的是遗忘门、输入门、神经元、输出门的偏置向量；tanh为激活函数；σ为sigmod函数；

步骤S4的具体实施方式如下：

输入数据对已建立的LSTM模型进行训练，共训练100轮，每次训练包含72个批次。训练采用Google开源的keras深度学习框架。训练完成后保存模型到指定路径。

步骤S5的具体实施方式如下：

服务端将保存的LSTM模型部署上线，等待客户端向服务端发送请求。控制小车行走一个圆形，同时客户端实时处理odom与imu测得的数据，获得odom_yaw和imu_yaw，并把它们通过客户端发送给服务端。服务端接收到客户端请求后给予响应，返回输出结果lstm_combined_yaw。

步骤S6的具体实施方法如下：

计算AGV运动轨迹的方法如下：

首先计算odom测得的位移量，计算公式为：

Δx＝odom_X1-odom_X0

Δy＝odom_Y1-odom_Y0

其中，odom_X1、odom_Y1表示当前时刻odom测得的小车位置，odom_X0、odom_Y0表示前一时刻odom测得的小车位置，Δx、Δy、Δs分别表示odom测得的AGV在x方向的位移量、AGV在y方向的位移量、两个时刻之间的直线距离；

结合lstm_combined_yaw，得到融合后AGV在坐标系中的位置，计算方法如下：

ΔX＝Δs×cos(lstm_combined_yaw)

ΔY＝Δs×sin(lstm_combined_yaw)

lstm_combined_x＝lstm_combined_x+ΔX

lstm_combined_y＝lstm_combined_y+ΔY

其中ΔX、ΔY分别表示AGV按lstm_combined_yaw这个角度运动时在x方向和y方向的位移量，lstm_combined_x、lstm_combined_y分别表示融合后AGV在坐标系中的x、y坐标，lstm_combined_x和lstm_combined_y在AGV启动时初始化为0。

如图5，图5中虚线部分为根据odom实时测量得到的AGV位移数据绘制的AGV运动轨迹，实线部分为根据odom实时测量得到的AGV位移数据与LSTM模型实时输出的lstm_combined_yaw计算得到的轨迹。可以肯定的是，该方法在一定精度上起到了融合的作用，且该模型一旦训练完成，就可以不用确定噪声的统计特性。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。