本发明涉及一种基于AGV的惯导‑WiFi信号融合定位算法，包括以下步骤：S1：前端构建节点和边约束；以AGV位姿作为节点，构建节点与节点之间的边约束，实现对AGV轨迹的前端构图；边约束包括以下内容：AGV轨迹导航关系约束；实际轨迹点和特定标定点的回环约束；识别AGV轨迹的回环约束；S2：将前端构图输入后端进行图优化求解，得到当前约束条件下AGV位姿的全局最优解，进而优化AGV的定位轨迹。本发明可以仅采用惯导采集的数据和WiFi信号实现AGV室内轨迹定位，且具有成本低、定位精度高等特点。
1.一种基于AGV的惯导‑WiFi信号融合定位算法，其特征在于，包括以下步骤：S1：前端构建节点和边约束；以AGV位姿作为节点，构建节点与节点之间的边约束，实现对AGV轨迹的前端构图；边约束包括以下内容：A、AGV轨迹导航关系约束；
将IMU惯导采集的原始六轴数据进行处理，得到AGV轨迹序列位姿点，得出AGV在相邻两个采样点之间的空间相对位姿转换关系，进而计算得出AGV轨迹导航关系约束的误差项；
将实际轨迹点与标定点进行回环检测；回环检测是将当前时刻的传感器观测值与AGV收集的所有历史传感器数值进行对比，判断AGV是否曾经出现在同一个地点，并将这些出现在同一地方的不同时刻的位姿点之间联系起来，进而建立的一条闭环边约束；
利用惯导数据和采集的各AP的WiFi信号强度构建识别AGV轨迹的回环约束，将AGV每次路过的同一个具有明显的地理位置和形状特征的轨迹识别出来作为回环匹配，包括AGV特定轨迹识别和对特定轨迹进行匹配；
AGV特定轨迹识别为根据轨迹规模设定轨迹序列窗口大小，然后从轨迹起点开始逐个滑动窗口寻找符合条件的轨迹，并得到特定轨迹的序列集合；
对特定轨迹进行匹配为对特定轨迹的序列集合的元素进行两两比较，判断是否为同一地理位置的轨迹，以此进行回环匹配；
S2：将前端构图输入后端进行图优化求解，得到当前约束条件下AGV位姿的全局最优解，进而优化AGV的定位轨迹；
将AGV轨迹作为迭代的初始取值，将构建的所有约束项输入图优化后端，采用高斯牛顿迭代算法求解全局误差的最小二乘形式，进而得到全局最优解，优化定位轨迹。
2.根据权利要求1所述的一种基于AGV的惯导‑WiFi信号融合定位算法，其特征在于：步骤S1中，构件边约束的优化目标函数的一般形式如下式所示，*
其中，F(x)为求解AGV最优轨迹的误差平方函数；xi和xj为顶点参数向量，即AGV的位姿点 X为位姿点集合；e(xi,xj,zij)为向量误差函数，代表xi和xj两者的估计关系*
与测量值或理论值zij的匹配程度；Ωij为该误差所占权重矩阵；x为对目标函数求取最小值得到的位姿最优解；误差函数e(xi,xj,zij)具体表示如下式所示，其中 表示顶点xi和xj之间位姿相对变换的估计值。
3.根据权利要求1所述的一种基于AGV的惯导‑WiFi信号融合定位算法，其特征在于，构建AGV轨迹导航关系约束具体如下：设AGV在相邻两个采样点之间的空间相对位姿转换关系如下式所示，其中Δxt和Δyt为根据导航关系得出的t时刻两个采样点之间位置坐标差的估计值，为航向差的估计值，ut为系统观测值；则轨迹导航关系约束的误差项如下式所示，其中xt、yt、 和xt‑1、yt‑1、 分别为t时刻和t‑1时刻的位姿Xt和Xt‑1的参数。
4.根据权利要求1所述的一种基于AGV的惯导‑WiFi信号融合定位算法，其特征在于，构建实际轨迹点和特定标定点的回环约束具体如下：设特定标定点位姿集为 其中f表示取了f个特定标定点，集
AGV路过这些特定标定点时停止几秒，根据这些停止间隙识别出AGV静止状态，以此得到轨迹点集中AGV对应的静止位姿顶点集 其中s表示轨迹中一共有s个实际轨迹点，集合中元素 代表AGV轨迹中某个实际轨迹点的平面坐标；
将实际轨迹点与其对应停止地点的特定标定点联系起来，对漂移的轨迹在数学上作逼近特定标定点的回环约束，并且当AGV多次路过同一个特点标定点时，将这些对应的实际轨迹点之间联系起来，构成实际轨迹点之间的回环约束；
设某t时刻AGV静止，此时实际轨迹点坐标为 其对应的地面特定标定点坐标为 则实际轨迹点和标定点间的回环检测的误差项如下式所示，
5.根据权利要求1所述的一种基于AGV的惯导‑WiFi信号融合定位算法，其特征在于，所述具有明显的地理位置和形状特征的轨迹为弯道；AGV特定轨迹识别包括如下步骤：SA1：输入轨迹位姿顶点集X，根据轨迹规模设定弯道序列窗口大小w，设置角度判定阈值θ，窗口前后采样点数T；
SA2：从轨迹起点开始逐一向后移动滑动窗口，如果当前滑动窗口左侧时间t>T，则直接转到步骤SA3，否则置窗口左边T＝1后转到步骤SA3；
其中， 为弯道序列窗口前T个采样点航向角的平均值， 为弯道序列窗口后T个采样点航向角的平均值；
当 大于设定的角度阈值θth,cor时判定为一个弯道，并添加入弯道集合cor中；否则回到步骤SA2；得到轨迹上弯道的序列集合cor＝{cor1,cor2,...,corN}，其中N表示识别出的弯道总个数；
SA4：判断t+T是否大于总采样点数，如果是则得到识别的弯道的序列集合cor，否则回到步骤SA2；
SB1：输入弯道的序列集合cor和弯道中点集合P，并设定中点欧式距离阈值dth、弯道RSS序列相似度阈值sth以及ICP弯道匹配阈值θth；
SB2：将识别出来的弯道的序列集合cor进行预分组，将可能是同一地点的弯道序列归为一组，在组内两两进行ICP匹配，并通过弯道RSS序列相似性和弯道中点之间的距离远近这两个条件进行判断，具体如下：SB21：AGV在行进中实时采集各AP的WiFi信号强度，获取各位置点接收的无线信号强度序列，即RSS序列；通过WiFi处理模块，对接收到的各AP的RSS序列缺失值进行填充并滤波，最终获得经过校正的RSS序列；取弯道的序列集合cor中每个弯道在对应转弯时间内采集的RSS序列 若两个弯道的RSS序列的欧氏距离小于设定阈值sth，则认为两个弯道为同一个地点的概率较大；设RSS序列长度为l，则弯道的序列集合cor中第k1和第k2个弯道的RSS序列之间欧氏距离如下式所示，当 时可以推测这两个弯道为同一地点弯道的概率较大；
SB22：取各弯道集合中每个弯道序列的中点坐标，构成弯道中点坐标集合N为弯道总个数，当弯道中点坐标集中两个点之间的欧氏距离
小于设定阈值dth时，即表示两个弯道在地理空间上处于比较近的一个范围内，为同一地点弯道的概率较大，弯道的序列集合cor中第k1个和第k2个弯道中点的距离如下式所示，SB3：当第k1个和第k2个弯道满足 和 两个条件时，对两个弯道进行ICP匹配，得到转换角度Δθ、移动距离Δd，并判断Δθ12≤θth,ICP、Δd12≤dth,ICP，如果是则构建该弯道对的回环约束，否则返回步骤SB2，具体如下：设两个弯道序列cor1和cor2的位姿点集分别为 和
点的坐标向量；将这两个弯道序列输入ICP算法得到两者之间变换的位姿矩阵，如下式所示，其中R12为旋转矩阵，利用反三角函数可求解出二者之间旋转变化的角度Δθ12；Δd12代T表两个弯道对应位姿点之间的平均距离向量，Δd12＝[Δx12,Δy12] ；当两个弯道的变换角度Δθ12小于设定阈值θth,ICP且两个弯道中对应点的平均距离小于设定阈值dth,ICP＝[ΔTxth,ICP,Δyth,ICP] ，即Δθ12≤θth,ICP且Δd12≤dth,ICP时，判定两个弯道为同一地点的弯道，形成一个回环约束；此时两个匹配弯道对中对应第k对位姿点之间的误差项如下式所示，SB4：判断是否还有未匹配的弯道对，如果还有则回到步骤SB2，否则算法结束。
一种基于AGV的惯导‑WiFi信号融合定位算法
技术领域
[0001] 本发明涉及智能厂房中无人搬运车(AGV)轨迹的室内定位技术领域，尤其涉及一种基于AGV的惯导‑WiFi信号融合定位算法。
背景技术
[0002] 随着社会的快速发展，以无人搬运车(AGV)为代表的移动机器人在智能厂房中被大量应用。受制于严重的信号衰减及多路径反射，诸如GPS信号等无法很好地穿透室内环境，因此卫星导航定位(GNSS)难以直接应用于工业场域，特别是室内厂区。利用惯性测量单元(IMU)可实现任意环境下的定位和导航，但纯惯性导航面临着众所周知的问题——随着时间的推移，会产生巨大的漂移误差。由于这些漂移误差的存在，在大多数实际应用中无法直接采用纯惯性进行定位和导航。
[0003] 解决纯惯性导航漂移误差问题的常见方法是用辅助信息源，包括WiFi、视觉、激光雷达、超宽带(UWB)、蓝牙、光、磁场等。
[0004] Juntao L等人采用视觉导航和惯性导航系统(INS)融合定位的方式开发了计算机控制的自动导航AGV，通过视觉导航来纠正惯性导航的累积误差，AGV机器人扫描二维码，然后纠正惯性导航系统的初始参数，实验结果显示此方法可以将定位精度控制在±20mm以内。
[0005] Yudanto R G等人提出了基于无线超宽带室内定位系统(UWB)和惯性导航系统(INS)融合的方法，用于工厂和仓库中AGV和移动机器人的室内实时导航和跟踪。实验结果表明，所提出的传感器融合方法能够在实时操作中提供1kHz的位置更新率，线性运动的位置误差标准偏差为3.7cm，旋转运动的位置误差标准偏差为1.7度，与独立的UWB室内定位系统或独立的惯性导航系统相比，具有明显的性能改进。
[0006] Lu M F等人提出了低成本惯性导航系统(INS)和室内定位系统(IPS)结合的定位技术来代替仓库中工作的高成本的激光导航系统。由红外线构造的IPS用于在特定时间间隔后校准惯性系统的累积误差。测试结果表明，经过长时间和长距离移动后，位置偏差约为5％，证明了该系统的稳定性。
[0007] 这些方案中除了WiFi外，大多数均需要增加摄像头、激光雷达等额外传感器部件，甚至需要架设基站，搭建系统，这不仅带来了成本上的开销，但是在某些工业场域仍不可行。
发明内容
[0008] 针对现有技术存在的问题，本发明的目的在于提供一种成本低且定位精度高的基于AGV的惯导‑WiFi信号融合定位算法
[0009] 为实现上述目的，本发明采用的技术方案是：
[0010] 一种基于AGV的惯导‑WiFi信号融合定位算法，其包括以下步骤：
[0011] S1：前端构建节点和边约束；以AGV位姿作为节点，构建节点与节点之间的边约束，实现对AGV轨迹的前端构图；边约束包括以下内容：
[0012] A、AGV轨迹导航关系约束；
[0013] B、实际轨迹点和特定标定点的回环约束；
[0014] C、识别AGV轨迹的回环约束；
[0015] S2：将前端构图输入后端进行图优化求解，得到当前约束条件下AGV位姿的全局最优解，进而优化AGV的定位轨迹。
[0016] 优选地，步骤S1中，构件边约束的优化目标函数的一般形式如下式所示，[0017]
[0018] x*＝argminF(x)
[0019] 其中，F(x)为求解AGV最优轨迹的误差平方函数；xi和xj为顶点参数向量，即AGV的位姿点 X为位姿点集合；e(xi,xj,zij)为向量误差函数，代表xi和xj两者的估计i *
关系与测量值或理论值zj的匹配程度；Ωij为该误差所占权重矩阵；x为对目标函数求取最小值得到的位姿最优解；误差函数e(xi,xj,zij)具体表示如下式所示，
[0020]
[0021] 其中 表示顶点xi和xj之间位姿相对变换的估计值；
[0022] 步骤S2中，通过构建全局误差的最小二乘形式，进而求解出AGV位姿的全局最优解。
[0023] 优选地，构建AGV轨迹导航关系约束为将IMU惯导采集的原始六轴数据进行处理，得到AGV轨迹序列位姿点，得出AGV在相邻两个采样点之间的空间相对位姿转换关系，进而计算得出AGV轨迹导航关系约束的误差项。
[0024] 优选地，构建AGV轨迹导航关系约束具体如下：
[0025] 设AGV在相邻两个采样点之间的空间相对位姿转换关系如下式所示，
[0026]
[0027] 其中Δxt和Δyt为根据导航关系得出的t时刻两个采样点之间位置坐标差的估计t值， 为航向差的估计值，u为系统观测值；则轨迹导航关系约束的误差项如下式所示，[0028]
[0029] 其中xt、yt、 和xt‑1、yt‑1、 分别为t时刻和t‑1时刻的位姿Xt和Xt‑1的参数。
[0030] 优选地，构建实际轨迹点和特定标定点的回环约束为将实际轨迹点与标定点进行回环检测；回环检测是将当前时刻的传感器观测值与AGV收集的所有历史传感器数值进行对比，判断AGV是否曾经出现在同一个地点，并将这些出现在同一地方的不同时刻的位姿点之间联系起来，进而建立的一条闭环边约束。
[0031] 优选地，构建实际轨迹点和特定标定点的回环约束具体如下：
[0032] 设特定标定点位姿集为 其中f表示取了f个特定标定点，集合中元素 代表某个特定标定点的平面坐标；
[0033] AGV路过这些特定标定点时停止几秒，根据这些停止间隙识别出AGV静止状态，以此得到轨迹点集中AGV对应的静止位姿顶点集 其中s表示轨迹中一共有s个实际轨迹点，集合中元素 代表AGV轨迹中某个实际轨迹点的平面
坐标；
[0034] 将实际轨迹点与其对应停止地点的特定标定点联系起来，对漂移的轨迹在数学上作逼近特定标定点的回环约束，并且当AGV多次路过同一个特点标定点时，将这些对应的实际轨迹点之间联系起来，构成实际轨迹点之间的回环约束；
[0035] 设某t时刻AGV静止，此时实际轨迹点坐标为 其对应的地面特定标定点坐标为 则实际轨迹点和标定点间的回环检测的误差项如下式所示，
[0036]
[0037] 优选地，利用采集的惯导数据和各AP的WiFi信号强度来构建识别AGV轨迹的回环约束，将AGV每次路过的同一个具有明显的地理位置和形状特征的轨迹识别出来作为回环匹配，包括AGV特定轨迹识别和对特定轨迹进行匹配；
[0038] AGV特定轨迹识别为根据轨迹规模设定轨迹序列窗口大小，然后从轨迹起点开始逐个滑动窗口寻找符合条件的轨迹，并得到特定轨迹的序列集合；
[0039] 对特定轨迹进行匹配为对特定轨迹的序列集合的元素进行两两比较，判断是否为同一地理位置的轨迹，以此进行回环匹配。
[0040] 优选地，所述具有明显的地理位置和形状特征的轨迹为弯道；AGV特定轨迹识别包括如下步骤：
[0041] SA1：输入轨迹位姿顶点集X，根据轨迹规模设定弯道序列窗口大小w，设置角度判定阈值θ，窗口前后采样点数T；
[0042] SA2：从轨迹起点开始逐一向后移动滑动窗口，如果当前滑动窗口左侧时间t>T，则直接转到步骤SA3，否则置窗口左边T＝1后转到步骤SA3；
[0043] SA3：按照是否符合下式的条件判定所需弯道，
[0044]
[0045] 其中， 为弯道序列窗口前T个采样点航向角的平均值， 为弯道序列窗口后T个采样点航向角的平均值；
[0046] 当 大于设定的角度阈值θth,cor时判定为一个弯道，并添加入弯道集合cor中；否则回到步骤SA2；得到轨迹上弯道的序列集合cor＝{cor1,cor2,...,corN}，其中N表示识别出的弯道总个数；
[0047] SA4：判断t+T是否大于总采样点数，如果是则得到识别的弯道的序列集合cor，否则回到步骤SA2；
[0048] 对特定轨迹进行匹配包括如下步骤：
[0049] SB1：输入弯道的序列集合cor和弯道中点集合P，并设定中点欧式距离阈值dth、弯道RSS序列相似度阈值sth以及ICP弯道匹配阈值θth；
[0050] SB2：将识别出来的弯道的序列集合cor进行预分组，将可能是同一地点的弯道序列归为一组，在组内两两进行ICP匹配，并通过弯道RSS序列相似性和弯道中点之间的距离远近这两个条件进行判断，具体如下：
[0051] SB21：AGV在行进中实时采集各AP的WiFi信号强度，获取各位置点接收的无线信号强度序列，即RSS序列；通过WiFi处理模块，对接收到的各AP的RSS序列缺失值进行填充并滤波，最终获得经过校正的RSS序列；取弯道的序列集合cor中每个弯道在对应转弯时间内采集的RSS序列 若两个弯道的RSS序列的欧氏距离小于设定阈值sth，则认为两个弯道为同一个地点的概率较大；设RSS序列长度为l，则弯道的序列集合cor中第k1和第k2个弯道的RSS序列之间欧氏距离如下式所示，
[0052]
[0053] 当 时可以推测这两个弯道为同一地点弯道的概率较大；
[0054] SB22：取各弯道集合中每个弯道序列的中点坐标，构成弯道中点坐标集合N为弯道总个数，当弯道中点坐标集中两个点之间的欧氏距离小于设定阈值dth时，即表示两个弯道在地理空间上处于比较近的一个范围内，为同一地点弯道的概率较大，弯道的序列集合cor中第k1个和第k2个弯道中点的距离如下式所示，[0055]
[0056] SB3：当第k1个和第k2个弯道满足 和两个条件时，对两个弯道进行ICP匹配，得到转换角度Δθ、移动距离Δd，并判断Δθ12≤θth,ICP、Δd12≤dth,ICP，如果是则构建该弯道对的回环约束，否则返回步骤SB2，具体如下：
[0057] 设两个弯道序列cor1和cor2的位姿点集分别 和其中 表示弯道序列cori中第k个位姿点
的坐标向量；将这两个弯道序列输入ICP算法得到两者之间变换的位姿矩阵，如下式所示，[0058]
[0059] 其中R12为旋转矩阵，利用反三角函数可求解出二者之间旋转变化的角度Δθ12；ΔTd12代表两个弯道对应位姿点之间的平均距离向量，Δd12＝[Δx12,Δy12] ；当两个弯道的变换角度Δθ12小于设定阈值θth,ICP且两个弯道中对应点的平均距离小于设定阈值dth,ICP＝[ΔT
xth,ICP,Δyth,ICPβ，即Δθ12≤θth,ICP且Δd12≤dth,ICP时，判定两个弯道为同一地点的弯道，形成一个回环约束；此时两个匹配弯道对中对应第k对位姿点之间的误差项如下式所示，[0060]
[0061] SB4：判断是否还有未匹配的弯道对，如果还有则回到步骤SB2，否则算法结束。
[0062] 优选地，步骤S2中，将AGV轨迹作为迭代的初始取值，将构建的所有约束项输入图优化后端，采用高斯牛顿迭代算法求解全局误差的最小二乘形式，进而得到全局最优解，优化定位轨迹。
[0063] 采用上述方案后，通过利用AGV轨迹位姿点结合对应的WiFi信号、以及少量的特定标定点信息，建立全局位姿点间的惯导约束、特定标定点匹配约束、AGV运动轨迹匹配约束，最后进行轨迹位姿点的图优化求解，实现定位精度的提升。相比于现有的大多数室内定位方案均需要增加摄像头、激光雷达等额外传感器部件，甚至需要架设基站，搭建系统，本发明可以仅采用惯导采集的数据和WiFi信号实现AGV室内轨迹定位，且具有成本低、定位精度高等特点。
附图说明
[0064] 图1为AGV轨迹导航关系约束构建示意图。
[0065] 图2为实际轨迹点和特定标定点的回环约束构建示意图。
[0066] 图3为识别AGV轨迹的回环约束构建示意图。
[0067] 图4为弯道识别流程示意图。
[0068] 图5为弯道匹配流程示意图。
[0069] 图6为基于图优化的最终优化轨迹和地面真实轨迹的对比图。
具体实施方式
[0070] 本发明揭示了一种基于AGV的惯导‑WiFi信号融合定位算法，其包括以下步骤：
[0071] S1：前端构建节点和边约束；以AGV位姿作为节点，构建节点与节点之间的边约束，实现对AGV轨迹的前端构图；边约束包括以下内容：
[0072] A、AGV轨迹导航关系约束。其意义在于通过将IMU惯导采集的原始六轴数据进行处理，得到AGV轨迹序列位姿点，因此可以得出AGV在相邻两个采样点之间的空间相对位姿转换关系，进而计算得出AGV轨迹导航关系约束的误差项。
[0073] B、实际轨迹点和特定标定点的回环约束。其意义在于将实际轨迹点与标定点进行回环检测，回环检测是为了将当前时刻的传感器观测值与AGV收集的所有历史传感器数值进行对比，判断AGV是否曾经出现在同一个地点，并将这些出现在同一地方的不同时刻的位姿点之间联系起来，进而建立的一条闭环边约束，进而可以有效地校正轨迹偏差。
[0074] C、识别AGV轨迹的回环约束。其意义在于通过将AGV每次路过的同一个具有明显的地理位置和形状特征的轨迹识别出来作为回环匹配，通过AGV运动轨迹识别算法得到识别出的特定轨迹集合，进而采用特定轨迹匹配算法得到轨迹回环匹配对，可大大抑制惯导的漂移误差。
[0075] S2：将前端构图输入后端进行图优化求解，得到当前约束条件下AGV位姿的全局最优解，进而优化AGV的定位轨迹。
[0076] 如图1‑3所示，构建边约束的优化目标函数的一般形式如式(1)和式(2)所示，[0077]
[0078] x*＝argminF(x) (2)
[0079] 其中，F(x)为求解AGV最优轨迹的误差平方函数；xi和xj为顶点参数向量，即AGV的位姿点 X为位姿点集合；e(xi,xj,zij)为向量误差函数，代表xi和xj两者的估计*
关系与测量值(或理论值)zij的匹配程度；Ωij为该误差所占权重矩阵；x为对目标函数求取最小值得到的位姿最优解。误差函数e(xi,xj,zij)具体表示如式(3)所示，
[0080]
[0081] 其中 表示顶点xi和xj之间位姿相对变换的估计值。
[0082] 因此，边约束的构建最终是要转换成顶点xi和xj之间的关系与测量值(或理论值)zij之间的误差项，进而通过后端优化构建全局误差的最小二乘形式，实现全局最优姿态的求解。综上，关键问题是找到节点与节点之间的联系，使得这种联系最大程度上与实际测量值(或者理论值)一致。前端构建约束项具体如下：
[0083] 一、构建AGV轨迹导航关系约束
[0084] 根据IMU采集的原始六轴数据得到轨迹序列位姿点，因此可以得出AGV在相邻两个采样点之间的空间相对位姿转换关系如式(4)所示，
[0085]
[0086] 其中Δxt和Δyt为根据导航关系得出的t时刻两个采样点之间位置坐标差的估计值， 为航向差的估计值，ut为系统观测值。则轨迹导航关系约束的误差项如式(5)所示，[0087]
[0088] 其中xt、yt、 和xt‑1、yt‑1、 分别为t时刻和t‑1时刻的位姿Xt和Xt‑1的参数。式(5)写成目标函数的形式如式(6)所示，
[0089]
[0090] AGV轨迹导航关系约束是顺序数据关联的方式，对连续两个采样点的传感器数据进行融合，计算AGV的位姿变换，将空间中AGV位姿点前后时刻之间的空间转换关系联系起来。
[0091] 二、构建实际轨迹点和特定标定点的回环约束
[0092] 在实施例中，我们将AGV路过这些特点标定点时随意停止几秒，因此实际轨迹点可以认为是静止点。则约束项具体为静止点和标定点间的回环约束。
[0093] 设特定标定点位姿集为 其中f表示取了f个特定标定点，集合中元素 代表某个特定标定点的平面坐标。
[0094] AGV路过这些特定标定点时停止几秒，根据这些停止间隙识别出AGV静止状态，以此得到轨迹点集中AGV对应的静止位姿顶点集 其中s表示轨迹中一共有s个实际轨迹点，集合中元素 代表AGV轨迹中某个实际轨迹点的平面
坐标。
[0095] 将静止点与其对应停止地点的特定标定点联系起来，对漂移的轨迹在数学上作逼近特定标定点的回环约束，并且当AGV多次路过同一个特点标定点时，将这些对应的实际轨迹点之间联系起来，构成实际轨迹点之间的回环约束，将有效地校正轨迹偏差。
[0096] 理论上，对应的标定点和静止点之间的平面坐标差值应该为0。因此设某t时刻AGV静止，此时实际轨迹点坐标为 其对应的地面特定标定点坐标为 则静止点和标定点间的回环检测的误差项如式(7)所示，
[0097]
[0098] 写成目标函数形式则如式(8)所示，
[0099]
[0100] 该约束即表示AGV轨迹上的静止位姿点的真实坐标应该与对应的标定点坐标一致，即两者之间的坐标差值应该趋于0。
[0101] 三、构建识别AGV轨迹的回环约束
[0102] 利用采集的惯导数据和各AP(WiFi的接入点)的WiFi信号强度来构建识别AGV轨迹的回环约束，主要包括AGV特定轨迹识别和对特定轨迹进行匹配两项操作。
[0103] AGV特定轨迹识别为根据轨迹规模设定轨迹序列窗口大小，然后从轨迹起点开始逐个滑动窗口寻找符合条件的轨迹，并得到特定轨迹的序列集合。
[0104] 对特定轨迹进行匹配为对特定轨迹的序列集合的元素进行两两比较，判断是否为同一地理位置的轨迹，以此进行回环匹配。
[0105] 通常情况下，厂区内轨迹包含各种弯道。弯道在轨迹中具有明显的地理位置和形状特征。本实施例中将弯道作为特定轨迹进行说明。弯道匹配算法采用ICP算法。因此AGV特定轨迹识别和对特定轨迹进行匹配具体可认为是弯道识别与ICP匹配回环检测。
[0106] 如图4所示，弯道识别包括如下步骤：
[0107] SA1：输入轨迹位姿顶点集X，根据轨迹规模设定弯道序列窗口大小w，设置角度判定阈值θ，窗口前后采样点数T。
[0108] SA2：从轨迹起点开始逐一向后移动滑动窗口，如果当前滑动窗口左侧时间t>T，则直接转到步骤SA3，否则置窗口左边T＝1后转到步骤SA3。
[0109] SA3：按照是否符合式(9)的条件判定是否为所需弯道，
[0110]
[0111] 其中， 为弯道序列窗口前T个采样点航向角的平均值， 为弯道序列窗口后T个采样点航向角的平均值。
[0112] 当 大于设定的角度阈值θth,cor时判定为一个弯道，并添加入弯道集合cor中，否则回到步骤SA2。θth,cor设置的不同可以识别出不同拐弯幅度的弯道。通过该方法找到轨迹上弯道的序列集合cor＝{cor1,cor2,...,corN}，其中N表示识别出的弯道总个数。
[0113] SA4：判断t+T是否大于总采样点数，如果是则得到识别的弯道的序列集合cor，否则回到步骤SA2。
[0114] 如图5所示，ICP匹配回环检测包括如下步骤：
[0115] SB1：输入弯道的序列集合cor和弯道中点集合P，并设定中点欧式距离阈值dth、弯道RSS序列相似度阈值sth以及ICP弯道匹配阈值θth。
[0116] SB2：将识别出来的弯道的序列集合cor进行预分组，将可能是同一地点的弯道序列归为一组，在组内两两进行ICP匹配，并结合弯道RSS序列相似性和弯道中点之间的距离远近这两个条件进行判断，具体如下：
[0117] SB21：AGV小车在行进中实时采集各AP的WiFi信号强度，即获取各位置点接收的无线信号强度(RSS)序列；通过WiFi处理模块，对接收到的各AP的RSS时间序列采取近邻填充法和均值填充法相结合的方法对缺值进行补充，并对RSS序列数据进行均值滤波以消除噪声等因素带来的抖动，最终获得经过校正的RSS序列。
[0118] 取弯道的序列集合cor中每个弯道在对应转弯时间内采集的RSS序列若两个弯道的RSS序列的欧氏距离小于设定阈值
sth，则认为两个弯道为同一个地点的概率较大。由上述滑动窗口w可知每个弯道的长度都是等长的，则RSS序列之间也对应为等长的。设RSS序列长度为l，则弯道的序列集合cor中第k1和第k2个弯道的RSS序列之间欧氏距离如式(10)所示，
[0119]
[0120] 当 时可以推测这两个弯道为同一地点弯道的概率较大。
[0121] SB22：取各弯道集合中每个弯道序列的中点坐标，构成弯道中点坐标集合N为弯道总个数，当弯道中点坐标集中两个点之间的欧氏距离小于设定阈值dth时，即表示两个弯道在地理空间上处于比较近的一个范围内，为同一地点弯道的概率较大，弯道的序列集合cor中第k1个和第k2个弯道中点的距离如式(11)所示，[0122]
[0123] SB3：当第k1个和第k2个弯道满足 和两个条件时，即两个弯道之间的空间距离较近且接收到的RSS序列相似度较高时，将对应的弯道分为同一组进行ICP匹配，这将大大减少计算量，省去不必要的匹配。对两个弯道进行ICP匹配，可以得到转换角度Δθ、移动距离Δd，并判断Δθ12≤θth,ICP、Δd12≤dth,ICP，如果是则构建该弯道对的回环约束，否则返回步骤SB2。ICP是一种点云匹配算法，目的是在两个点云之间找到变换关系，包含旋转关系以及平移关系，以最小化两个点集之间的距离。若Δθ和Δd小于设定阈值时，即表明两个弯道在空间坐标和形态上的相似度较高，则将二者判定为弯道回环对。具体如下：
[0124] 设两个弯道序列cor1和cor2的位姿点集分别为 和其中 表示弯道序列cori中第k个位
姿点的坐标向量；将这两个弯道序列输入ICP算法得到两者之间变换的位姿矩阵，如式(12)所示，
[0125]
[0126] 其中R12为旋转矩阵，利用反三角函数可求解出二者之间旋转变化的角度Δθ12；ΔTd12代表两个弯道对应位姿点之间的平均距离向量，Δd12＝[Δx12,Δy12]。当两个弯道的变换角度Δθ12小于设定阈值θth,ICP且两个弯道中对应点的平均距离小于设定阈值dth,ICP＝[ΔT
xth,ICP,Δyth,ICP] ，即Δθ12≤θth,ICP且Δd12≤dth,ICP时，判定两个弯道为同一地点的弯道，形成一个回环约束，理论上二个弯道中对应点之间的平面坐标差值为0，且航向角差值为0或π(运动方向相反时为π)。此时两个匹配弯道对中对应第k对位姿点之间的误差项如式(13)所示，
[0127]
[0128] 写成目标函数形式则如式(14)所示，
[0129]
[0130] SB4：判断是否还有未匹配的弯道对，如果还有则回到步骤SB2，否则算法结束。
[0131] 综上，根据式(6)、式(8)以及式(14)可以建立总体的代价函数，如式(15)所示，[0132] F(all)＝F(IMU)+F(sf)+F(ICP) (15)
[0133] 其中，F(all)是全局误差函数，具有最小二乘的形式，图优化后端便是对此目标优化函数进行求解，得到全局最优姿态解。
[0134] 当前端构图完成后，将AGV轨迹(IMU采集到的的小车数据轨迹)作为迭代的初始取值，将构建的所有约束项输入图优化后端，采用高斯牛顿迭代算法求解全局误差的最小二乘形式，进而得到全局最优解，优化定位轨迹。
[0135] 为了对本发明进行验证，在模拟实验中，实验场景轨迹范围约为43.5m*51.5m，轨迹长度约为1034.8m。实验结果如图6所示，蓝色优化轨迹与红色地面真实轨迹的对比直观地反映了定位精度的提升，可以看到AGV重复行走的轨迹之间基本重叠，整体定位轨迹贴近地面真实轨迹。
[0136] 这里选取图优化处理前后轨迹上各个弯道中点作为位置误差对比点，弯道是轨迹转折的关键点，在轨迹中具有标志性，能够较好地衡量定位轨迹与真实轨迹之间的误差。误差分析见表1，从表中可以看到AGV轨迹经过本发明图优化处理后，最大误差与真实轨迹相比降到了0.748m，平均误差降到了0.321m，均方根误差降到了0.389m，圆概率误差(75％)降到0.451m，圆概率误差(95％)降到0.663m，轨迹精度得到了提升，证明了算法具备有效性。
[0137] 表1
[0138]
[0139] 以上所述，仅是本发明实施例而已，并非对本发明的技术范围作任何限制，故凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同变化与修饰，均仍属于本发明技术方案的范围内。
