CN113891251B - 一种基于AGV的惯导-WiFi信号融合定位算法 - Google Patents

一种基于AGV的惯导-WiFi信号融合定位算法 Download PDF

Info

Publication number
CN113891251B
CN113891251B CN202111020746.5A CN202111020746A CN113891251B CN 113891251 B CN113891251 B CN 113891251B CN 202111020746 A CN202111020746 A CN 202111020746A CN 113891251 B CN113891251 B CN 113891251B
Authority
CN
China
Prior art keywords
agv
track
curve
points
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111020746.5A
Other languages
English (en)
Other versions
CN113891251A (zh
Inventor
陈凌宇
陈鹏飞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xiamen University
Original Assignee
Xiamen University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Xiamen University filed Critical Xiamen University
Priority to CN202111020746.5A priority Critical patent/CN113891251B/zh
Publication of CN113891251A publication Critical patent/CN113891251A/zh
Application granted granted Critical
Publication of CN113891251B publication Critical patent/CN113891251B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/029—Location-based management or tracking services
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/021—Services related to particular areas, e.g. point of interest [POI] services, venue services or geofences
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/024—Guidance services
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • H04W64/006—Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种基于AGV的惯导‑WiFi信号融合定位算法，包括以下步骤：S1：前端构建节点和边约束；以AGV位姿作为节点，构建节点与节点之间的边约束，实现对AGV轨迹的前端构图；边约束包括以下内容：AGV轨迹导航关系约束；实际轨迹点和特定标定点的回环约束；识别AGV轨迹的回环约束；S2：将前端构图输入后端进行图优化求解，得到当前约束条件下AGV位姿的全局最优解，进而优化AGV的定位轨迹。本发明可以仅采用惯导采集的数据和WiFi信号实现AGV室内轨迹定位，且具有成本低、定位精度高等特点。

Description

一种基于AGV的惯导-WiFi信号融合定位算法
技术领域
本发明涉及智能厂房中无人搬运车(AGV)轨迹的室内定位技术领域，尤其涉及一种基于AGV的惯导-WiFi信号融合定位算法。
背景技术
随着社会的快速发展，以无人搬运车(AGV)为代表的移动机器人在智能厂房中被大量应用。受制于严重的信号衰减及多路径反射，诸如GPS信号等无法很好地穿透室内环境，因此卫星导航定位(GNSS)难以直接应用于工业场域，特别是室内厂区。利用惯性测量单元(IMU)可实现任意环境下的定位和导航，但纯惯性导航面临着众所周知的问题——随着时间的推移，会产生巨大的漂移误差。由于这些漂移误差的存在，在大多数实际应用中无法直接采用纯惯性进行定位和导航。
解决纯惯性导航漂移误差问题的常见方法是用辅助信息源，包括WiFi、视觉、激光雷达、超宽带(UWB)、蓝牙、光、磁场等。
Juntao L等人采用视觉导航和惯性导航系统(INS)融合定位的方式开发了计算机控制的自动导航AGV，通过视觉导航来纠正惯性导航的累积误差，AGV机器人扫描二维码，然后纠正惯性导航系统的初始参数，实验结果显示此方法可以将定位精度控制在±20mm以内。
Yudanto R G等人提出了基于无线超宽带室内定位系统(UWB)和惯性导航系统(INS)融合的方法，用于工厂和仓库中AGV和移动机器人的室内实时导航和跟踪。实验结果表明，所提出的传感器融合方法能够在实时操作中提供1kHz的位置更新率，线性运动的位置误差标准偏差为3.7cm，旋转运动的位置误差标准偏差为1.7度，与独立的UWB室内定位系统或独立的惯性导航系统相比，具有明显的性能改进。
Lu M F等人提出了低成本惯性导航系统(INS)和室内定位系统(IPS)结合的定位技术来代替仓库中工作的高成本的激光导航系统。由红外线构造的IPS用于在特定时间间隔后校准惯性系统的累积误差。测试结果表明，经过长时间和长距离移动后，位置偏差约为5％，证明了该系统的稳定性。
这些方案中除了WiFi外，大多数均需要增加摄像头、激光雷达等额外传感器部件，甚至需要架设基站，搭建系统，这不仅带来了成本上的开销，但是在某些工业场域仍不可行。
发明内容
针对现有技术存在的问题，本发明的目的在于提供一种成本低且定位精度高的基于AGV的惯导-WiFi信号融合定位算法
为实现上述目的，本发明采用的技术方案是：
一种基于AGV的惯导-WiFi信号融合定位算法，其包括以下步骤：
S1：前端构建节点和边约束；以AGV位姿作为节点，构建节点与节点之间的边约束，实现对AGV轨迹的前端构图；边约束包括以下内容：
A、AGV轨迹导航关系约束；
B、实际轨迹点和特定标定点的回环约束；
C、识别AGV轨迹的回环约束；
S2：将前端构图输入后端进行图优化求解，得到当前约束条件下AGV位姿的全局最优解，进而优化AGV的定位轨迹。
优选地，步骤S1中，构件边约束的优化目标函数的一般形式如下式所示，
Figure BDA0003241855900000031
x*＝argminF(x)
其中，F(x)为求解AGV最优轨迹的误差平方函数；xi和xj为顶点参数向量，即AGV的位姿点
Figure BDA0003241855900000032
X为位姿点集合；e(xi,xj,zij)为向量误差函数，代表xi和xj两者的估计关系与测量值或理论值zi j的匹配程度；Ωij为该误差所占权重矩阵；x*为对目标函数求取最小值得到的位姿最优解；误差函数e(xi,xj,zij)具体表示如下式所示，
Figure BDA0003241855900000033
其中
Figure BDA0003241855900000034
表示顶点xi和xj之间位姿相对变换的估计值；
步骤S2中，通过构建全局误差的最小二乘形式，进而求解出AGV位姿的全局最优解。
优选地，构建AGV轨迹导航关系约束为将IMU惯导采集的原始六轴数据进行处理，得到AGV轨迹序列位姿点，得出AGV在相邻两个采样点之间的空间相对位姿转换关系，进而计算得出AGV轨迹导航关系约束的误差项。
优选地，构建AGV轨迹导航关系约束具体如下：
设AGV在相邻两个采样点之间的空间相对位姿转换关系如下式所示，
Figure BDA0003241855900000035
其中Δxt和Δyt为根据导航关系得出的t时刻两个采样点之间位置坐标差的估计值，
Figure BDA0003241855900000036
为航向差的估计值，ut为系统观测值；则轨迹导航关系约束的误差项如下式所示，
Figure BDA0003241855900000041
其中xt、yt、
Figure BDA0003241855900000042
和xt-1、yt-1、
Figure BDA0003241855900000043
分别为t时刻和t-1时刻的位姿Xt和Xt-1的参数。
优选地，构建实际轨迹点和特定标定点的回环约束为将实际轨迹点与标定点进行回环检测；回环检测是将当前时刻的传感器观测值与AGV收集的所有历史传感器数值进行对比，判断AGV是否曾经出现在同一个地点，并将这些出现在同一地方的不同时刻的位姿点之间联系起来，进而建立的一条闭环边约束。
优选地，构建实际轨迹点和特定标定点的回环约束具体如下：
设特定标定点位姿集为
Figure BDA0003241855900000044
其中f表示取了f个特定标定点，集合中元素
Figure BDA0003241855900000045
代表某个特定标定点的平面坐标；
AGV路过这些特定标定点时停止几秒，根据这些停止间隙识别出AGV静止状态，以此得到轨迹点集中AGV对应的静止位姿顶点集
Figure BDA0003241855900000046
其中s表示轨迹中一共有s个实际轨迹点，集合中元素
Figure BDA0003241855900000047
代表AGV轨迹中某个实际轨迹点的平面坐标；
将实际轨迹点与其对应停止地点的特定标定点联系起来，对漂移的轨迹在数学上作逼近特定标定点的回环约束，并且当AGV多次路过同一个特点标定点时，将这些对应的实际轨迹点之间联系起来，构成实际轨迹点之间的回环约束；
设某t时刻AGV静止，此时实际轨迹点坐标为
Figure BDA0003241855900000048
其对应的地面特定标定点坐标为
Figure BDA0003241855900000049
则实际轨迹点和标定点间的回环检测的误差项如下式所示，
Figure BDA0003241855900000051
优选地，利用采集的惯导数据和各AP的WiFi信号强度来构建识别AGV轨迹的回环约束，将AGV每次路过的同一个具有明显的地理位置和形状特征的轨迹识别出来作为回环匹配，包括AGV特定轨迹识别和对特定轨迹进行匹配；
AGV特定轨迹识别为根据轨迹规模设定轨迹序列窗口大小，然后从轨迹起点开始逐个滑动窗口寻找符合条件的轨迹，并得到特定轨迹的序列集合；
对特定轨迹进行匹配为对特定轨迹的序列集合的元素进行两两比较，判断是否为同一地理位置的轨迹，以此进行回环匹配。
优选地，所述具有明显的地理位置和形状特征的轨迹为弯道；AGV特定轨迹识别包括如下步骤：
SA1：输入轨迹位姿顶点集X，根据轨迹规模设定弯道序列窗口大小w，设置角度判定阈值θ，窗口前后采样点数T；
SA2：从轨迹起点开始逐一向后移动滑动窗口，如果当前滑动窗口左侧时间t>T，则直接转到步骤SA3，否则置窗口左边T＝1后转到步骤SA3；
SA3：按照是否符合下式的条件判定所需弯道，
Figure BDA0003241855900000052
其中，
Figure BDA0003241855900000061
为弯道序列窗口前T个采样点航向角的平均值，
Figure BDA0003241855900000062
为弯道序列窗口后T个采样点航向角的平均值；
当
Figure BDA0003241855900000063
大于设定的角度阈值θ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序列
Figure BDA0003241855900000064
若两个弯道的RSS序列的欧氏距离小于设定阈值sth，则认为两个弯道为同一个地点的概率较大；设RSS序列长度为l，则弯道的序列集合cor中第k1和第k2个弯道的RSS序列之间欧氏距离如下式所示，
Figure BDA0003241855900000065
当
Figure BDA0003241855900000071
时可以推测这两个弯道为同一地点弯道的概率较大；
SB22：取各弯道集合中每个弯道序列的中点坐标，构成弯道中点坐标集合
Figure BDA0003241855900000072
N为弯道总个数，当弯道中点坐标集中两个点之间的欧氏距离小于设定阈值dth时，即表示两个弯道在地理空间上处于比较近的一个范围内，为同一地点弯道的概率较大，弯道的序列集合cor中第k1个和第k2个弯道中点的距离如下式所示，
Figure BDA0003241855900000073
SB3：当第k1个和第k2个弯道满足
Figure BDA0003241855900000074
和
Figure BDA0003241855900000075
两个条件时，对两个弯道进行ICP匹配，得到转换角度Δθ、移动距离Δd，并判断Δθ12≤θth,ICP、Δd12≤dth,ICP，如果是则构建该弯道对的回环约束，否则返回步骤SB2，具体如下：
设两个弯道序列cor1和cor2的位姿点集分别
Figure BDA0003241855900000076
和
Figure BDA0003241855900000077
其中
Figure BDA0003241855900000078
表示弯道序列cori中第k个位姿点的坐标向量；将这两个弯道序列输入ICP算法得到两者之间变换的位姿矩阵，如下式所示，
Figure BDA0003241855900000079
其中R12为旋转矩阵，利用反三角函数可求解出二者之间旋转变化的角度Δθ12；Δd12代表两个弯道对应位姿点之间的平均距离向量，Δd12＝[Δx12,Δy12]T；当两个弯道的变换角度Δθ12小于设定阈值θth,ICP且两个弯道中对应点的平均距离小于设定阈值dth,ICP＝[Δxth,ICP,Δyth,ICPβT，即Δθ12≤θth,ICP且Δd12≤dth,ICP时，判定两个弯道为同一地点的弯道，形成一个回环约束；此时两个匹配弯道对中对应第k对位姿点之间的误差项如下式所示，
Figure BDA0003241855900000081
SB4：判断是否还有未匹配的弯道对，如果还有则回到步骤SB2，否则算法结束。
优选地，步骤S2中，将AGV轨迹作为迭代的初始取值，将构建的所有约束项输入图优化后端，采用高斯牛顿迭代算法求解全局误差的最小二乘形式，进而得到全局最优解，优化定位轨迹。
采用上述方案后，通过利用AGV轨迹位姿点结合对应的WiFi信号、以及少量的特定标定点信息，建立全局位姿点间的惯导约束、特定标定点匹配约束、AGV运动轨迹匹配约束，最后进行轨迹位姿点的图优化求解，实现定位精度的提升。相比于现有的大多数室内定位方案均需要增加摄像头、激光雷达等额外传感器部件，甚至需要架设基站，搭建系统，本发明可以仅采用惯导采集的数据和WiFi信号实现AGV室内轨迹定位，且具有成本低、定位精度高等特点。
附图说明
图1为AGV轨迹导航关系约束构建示意图。
图2为实际轨迹点和特定标定点的回环约束构建示意图。
图3为识别AGV轨迹的回环约束构建示意图。
图4为弯道识别流程示意图。
图5为弯道匹配流程示意图。
图6为基于图优化的最终优化轨迹和地面真实轨迹的对比图。
具体实施方式
本发明揭示了一种基于AGV的惯导-WiFi信号融合定位算法，其包括以下步骤：
S1：前端构建节点和边约束；以AGV位姿作为节点，构建节点与节点之间的边约束，实现对AGV轨迹的前端构图；边约束包括以下内容：
A、AGV轨迹导航关系约束。其意义在于通过将IMU惯导采集的原始六轴数据进行处理，得到AGV轨迹序列位姿点，因此可以得出AGV在相邻两个采样点之间的空间相对位姿转换关系，进而计算得出AGV轨迹导航关系约束的误差项。
B、实际轨迹点和特定标定点的回环约束。其意义在于将实际轨迹点与标定点进行回环检测，回环检测是为了将当前时刻的传感器观测值与AGV收集的所有历史传感器数值进行对比，判断AGV是否曾经出现在同一个地点，并将这些出现在同一地方的不同时刻的位姿点之间联系起来，进而建立的一条闭环边约束，进而可以有效地校正轨迹偏差。
C、识别AGV轨迹的回环约束。其意义在于通过将AGV每次路过的同一个具有明显的地理位置和形状特征的轨迹识别出来作为回环匹配，通过AGV运动轨迹识别算法得到识别出的特定轨迹集合，进而采用特定轨迹匹配算法得到轨迹回环匹配对，可大大抑制惯导的漂移误差。
S2：将前端构图输入后端进行图优化求解，得到当前约束条件下AGV位姿的全局最优解，进而优化AGV的定位轨迹。
如图1-3所示，构建边约束的优化目标函数的一般形式如式(1)和式(2)所示，
Figure BDA0003241855900000101
x*＝argminF(x) (2)
其中，F(x)为求解AGV最优轨迹的误差平方函数；xi和xj为顶点参数向量，即AGV的位姿点
Figure BDA0003241855900000102
X为位姿点集合；e(xi,xj,zij)为向量误差函数，代表xi和xj两者的估计关系与测量值(或理论值)zij的匹配程度；Ωij为该误差所占权重矩阵；x*为对目标函数求取最小值得到的位姿最优解。误差函数e(xi,xj,zij)具体表示如式(3)所示，
Figure BDA0003241855900000103
其中
Figure BDA0003241855900000104
表示顶点xi和xj之间位姿相对变换的估计值。
因此，边约束的构建最终是要转换成顶点xi和xj之间的关系与测量值(或理论值)zij之间的误差项，进而通过后端优化构建全局误差的最小二乘形式，实现全局最优姿态的求解。综上，关键问题是找到节点与节点之间的联系，使得这种联系最大程度上与实际测量值(或者理论值)一致。前端构建约束项具体如下：
一、构建AGV轨迹导航关系约束
根据IMU采集的原始六轴数据得到轨迹序列位姿点，因此可以得出AGV在相邻两个采样点之间的空间相对位姿转换关系如式(4)所示，
Figure BDA0003241855900000105
其中Δxt和Δyt为根据导航关系得出的t时刻两个采样点之间位置坐标差的估计值，
Figure BDA0003241855900000106
为航向差的估计值，ut为系统观测值。则轨迹导航关系约束的误差项如式(5)所示，
Figure BDA0003241855900000111
其中xt、yt、
Figure BDA0003241855900000112
和xt-1、yt-1、
Figure BDA0003241855900000113
分别为t时刻和t-1时刻的位姿Xt和Xt-1的参数。式(5)写成目标函数的形式如式(6)所示，
Figure BDA0003241855900000114
AGV轨迹导航关系约束是顺序数据关联的方式，对连续两个采样点的传感器数据进行融合，计算AGV的位姿变换，将空间中AGV位姿点前后时刻之间的空间转换关系联系起来。
二、构建实际轨迹点和特定标定点的回环约束
在实施例中，我们将AGV路过这些特点标定点时随意停止几秒，因此实际轨迹点可以认为是静止点。则约束项具体为静止点和标定点间的回环约束。
设特定标定点位姿集为
Figure BDA0003241855900000115
其中f表示取了f个特定标定点，集合中元素
Figure BDA0003241855900000116
代表某个特定标定点的平面坐标。
AGV路过这些特定标定点时停止几秒，根据这些停止间隙识别出AGV静止状态，以此得到轨迹点集中AGV对应的静止位姿顶点集
Figure BDA0003241855900000117
其中s表示轨迹中一共有s个实际轨迹点，集合中元素
Figure BDA0003241855900000118
代表AGV轨迹中某个实际轨迹点的平面坐标。
将静止点与其对应停止地点的特定标定点联系起来，对漂移的轨迹在数学上作逼近特定标定点的回环约束，并且当AGV多次路过同一个特点标定点时，将这些对应的实际轨迹点之间联系起来，构成实际轨迹点之间的回环约束，将有效地校正轨迹偏差。
理论上，对应的标定点和静止点之间的平面坐标差值应该为0。因此设某t时刻AGV静止，此时实际轨迹点坐标为
Figure BDA0003241855900000121
其对应的地面特定标定点坐标为
Figure BDA0003241855900000122
则静止点和标定点间的回环检测的误差项如式(7)所示，
Figure BDA0003241855900000123
写成目标函数形式则如式(8)所示，
Figure BDA0003241855900000124
该约束即表示AGV轨迹上的静止位姿点的真实坐标应该与对应的标定点坐标一致，即两者之间的坐标差值应该趋于0。
三、构建识别AGV轨迹的回环约束
利用采集的惯导数据和各AP(WiFi的接入点)的WiFi信号强度来构建识别AGV轨迹的回环约束，主要包括AGV特定轨迹识别和对特定轨迹进行匹配两项操作。
AGV特定轨迹识别为根据轨迹规模设定轨迹序列窗口大小，然后从轨迹起点开始逐个滑动窗口寻找符合条件的轨迹，并得到特定轨迹的序列集合。
对特定轨迹进行匹配为对特定轨迹的序列集合的元素进行两两比较，判断是否为同一地理位置的轨迹，以此进行回环匹配。
通常情况下，厂区内轨迹包含各种弯道。弯道在轨迹中具有明显的地理位置和形状特征。本实施例中将弯道作为特定轨迹进行说明。弯道匹配算法采用ICP算法。因此AGV特定轨迹识别和对特定轨迹进行匹配具体可认为是弯道识别与ICP匹配回环检测。
如图4所示，弯道识别包括如下步骤：
SA1：输入轨迹位姿顶点集X，根据轨迹规模设定弯道序列窗口大小w，设置角度判定阈值θ，窗口前后采样点数T。
SA2：从轨迹起点开始逐一向后移动滑动窗口，如果当前滑动窗口左侧时间t>T，则直接转到步骤SA3，否则置窗口左边T＝1后转到步骤SA3。
SA3：按照是否符合式(9)的条件判定是否为所需弯道，
Figure BDA0003241855900000131
其中，
Figure BDA0003241855900000132
为弯道序列窗口前T个采样点航向角的平均值，
Figure BDA0003241855900000133
为弯道序列窗口后T个采样点航向角的平均值。
当
Figure BDA0003241855900000134
大于设定的角度阈值θth,cor时判定为一个弯道，并添加入弯道集合cor中，否则回到步骤SA2。θth,cor设置的不同可以识别出不同拐弯幅度的弯道。通过该方法找到轨迹上弯道的序列集合cor＝{cor1,cor2,...,corN}，其中N表示识别出的弯道总个数。
SA4：判断t+T是否大于总采样点数，如果是则得到识别的弯道的序列集合cor，否则回到步骤SA2。
如图5所示，ICP匹配回环检测包括如下步骤：
SB1：输入弯道的序列集合cor和弯道中点集合P，并设定中点欧式距离阈值dth、弯道RSS序列相似度阈值sth以及ICP弯道匹配阈值θth。
SB2：将识别出来的弯道的序列集合cor进行预分组，将可能是同一地点的弯道序列归为一组，在组内两两进行ICP匹配，并结合弯道RSS序列相似性和弯道中点之间的距离远近这两个条件进行判断，具体如下：
SB21：AGV小车在行进中实时采集各AP的WiFi信号强度，即获取各位置点接收的无线信号强度(RSS)序列；通过WiFi处理模块，对接收到的各AP的RSS时间序列采取近邻填充法和均值填充法相结合的方法对缺值进行补充，并对RSS序列数据进行均值滤波以消除噪声等因素带来的抖动，最终获得经过校正的RSS序列。
取弯道的序列集合cor中每个弯道在对应转弯时间内采集的RSS序列
Figure BDA0003241855900000141
若两个弯道的RSS序列的欧氏距离小于设定阈值sth，则认为两个弯道为同一个地点的概率较大。由上述滑动窗口w可知每个弯道的长度都是等长的，则RSS序列之间也对应为等长的。设RSS序列长度为l，则弯道的序列集合cor中第k1和第k2个弯道的RSS序列之间欧氏距离如式(10)所示，
Figure BDA0003241855900000142
当
Figure BDA0003241855900000143
时可以推测这两个弯道为同一地点弯道的概率较大。
SB22：取各弯道集合中每个弯道序列的中点坐标，构成弯道中点坐标集合
Figure BDA0003241855900000144
N为弯道总个数，当弯道中点坐标集中两个点之间的欧氏距离小于设定阈值dth时，即表示两个弯道在地理空间上处于比较近的一个范围内，为同一地点弯道的概率较大，弯道的序列集合cor中第k1个和第k2个弯道中点的距离如式(11)所示，
Figure BDA0003241855900000145
SB3：当第k1个和第k2个弯道满足
Figure BDA0003241855900000151
和
Figure BDA0003241855900000152
两个条件时，即两个弯道之间的空间距离较近且接收到的RSS序列相似度较高时，将对应的弯道分为同一组进行ICP匹配，这将大大减少计算量，省去不必要的匹配。对两个弯道进行ICP匹配，可以得到转换角度Δθ、移动距离Δd，并判断Δθ12≤θth,ICP、Δd12≤dth,ICP，如果是则构建该弯道对的回环约束，否则返回步骤SB2。ICP是一种点云匹配算法，目的是在两个点云之间找到变换关系，包含旋转关系以及平移关系，以最小化两个点集之间的距离。若Δθ和Δd小于设定阈值时，即表明两个弯道在空间坐标和形态上的相似度较高，则将二者判定为弯道回环对。具体如下：
设两个弯道序列cor1和cor2的位姿点集分别为
Figure BDA0003241855900000153
和
Figure BDA0003241855900000154
其中
Figure BDA0003241855900000155
表示弯道序列cori中第k个位姿点的坐标向量；将这两个弯道序列输入ICP算法得到两者之间变换的位姿矩阵，如式(12)所示，
Figure BDA0003241855900000156
其中R12为旋转矩阵，利用反三角函数可求解出二者之间旋转变化的角度Δθ12；Δd12代表两个弯道对应位姿点之间的平均距离向量，Δd12＝[Δx12,Δy12]T。当两个弯道的变换角度Δθ12小于设定阈值θth,ICP且两个弯道中对应点的平均距离小于设定阈值dth,ICP＝[Δxth,ICP,Δyth,ICP]T，即Δθ12≤θth,ICP且Δd12≤dth,ICP时，判定两个弯道为同一地点的弯道，形成一个回环约束，理论上二个弯道中对应点之间的平面坐标差值为0，且航向角差值为0或π(运动方向相反时为π)。此时两个匹配弯道对中对应第k对位姿点之间的误差项如式(13)所示，
Figure BDA0003241855900000161
写成目标函数形式则如式(14)所示，
Figure BDA0003241855900000162
SB4：判断是否还有未匹配的弯道对，如果还有则回到步骤SB2，否则算法结束。
综上，根据式(6)、式(8)以及式(14)可以建立总体的代价函数，如式(15)所示，
F(all)＝F(IMU)+F(sf)+F(ICP) (15)
其中，F(all)是全局误差函数，具有最小二乘的形式，图优化后端便是对此目标优化函数进行求解，得到全局最优姿态解。
当前端构图完成后，将AGV轨迹(IMU采集到的的小车数据轨迹)作为迭代的初始取值，将构建的所有约束项输入图优化后端，采用高斯牛顿迭代算法求解全局误差的最小二乘形式，进而得到全局最优解，优化定位轨迹。
为了对本发明进行验证，在模拟实验中，实验场景轨迹范围约为43.5m*51.5m，轨迹长度约为1034.8m。实验结果如图6所示，蓝色优化轨迹与红色地面真实轨迹的对比直观地反映了定位精度的提升，可以看到AGV重复行走的轨迹之间基本重叠，整体定位轨迹贴近地面真实轨迹。
这里选取图优化处理前后轨迹上各个弯道中点作为位置误差对比点，弯道是轨迹转折的关键点，在轨迹中具有标志性，能够较好地衡量定位轨迹与真实轨迹之间的误差。误差分析见表1，从表中可以看到AGV轨迹经过本发明图优化处理后，最大误差与真实轨迹相比降到了0.748m，平均误差降到了0.321m，均方根误差降到了0.389m，圆概率误差(75％)降到0.451m，圆概率误差(95％)降到0.663m，轨迹精度得到了提升，证明了算法具备有效性。
表1
Figure BDA0003241855900000171
以上所述，仅是本发明实施例而已，并非对本发明的技术范围作任何限制，故凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (5)

1.一种基于AGV的惯导-WiFi信号融合定位算法，其特征在于，包括以下步骤：
S1：前端构建节点和边约束；以AGV位姿作为节点，构建节点与节点之间的边约束，实现对AGV轨迹的前端构图；边约束包括以下内容：
A、AGV轨迹导航关系约束；
将IMU惯导采集的原始六轴数据进行处理，得到AGV轨迹序列位姿点，得出AGV在相邻两个采样点之间的空间相对位姿转换关系，进而计算得出AGV轨迹导航关系约束的误差项；
B、实际轨迹点和特定标定点的回环约束；
将实际轨迹点与标定点进行回环检测；回环检测是将当前时刻的传感器观测值与AGV收集的所有历史传感器数值进行对比，判断AGV是否曾经出现在同一个地点，并将这些出现在同一地方的不同时刻的位姿点之间联系起来，进而建立的一条闭环边约束；
C、识别AGV轨迹的回环约束；
利用惯导数据和采集的各AP的WiFi信号强度构建识别AGV轨迹的回环约束，将AGV每次路过的同一个具有明显的地理位置和形状特征的轨迹识别出来作为回环匹配，包括AGV特定轨迹识别和对特定轨迹进行匹配；
AGV特定轨迹识别为根据轨迹规模设定轨迹序列窗口大小，然后从轨迹起点开始逐个滑动窗口寻找符合条件的轨迹，并得到特定轨迹的序列集合；
对特定轨迹进行匹配为对特定轨迹的序列集合的元素进行两两比较，判断是否为同一地理位置的轨迹，以此进行回环匹配；
S2：将前端构图输入后端进行图优化求解，得到当前约束条件下AGV位姿的全局最优解，进而优化AGV的定位轨迹；
将AGV轨迹作为迭代的初始取值，将构建的所有约束项输入图优化后端，采用高斯牛顿迭代算法求解全局误差的最小二乘形式，进而得到全局最优解，优化定位轨迹。
2.根据权利要求1所述的一种基于AGV的惯导-WiFi信号融合定位算法，其特征在于：步骤S1中，构件边约束的优化目标函数的一般形式如下式所示，
Figure FDA0003843116360000021
x*＝argminF(x)
其中，F(x)为求解AGV最优轨迹的误差平方函数；xi和xj为顶点参数向量，即AGV的位姿点
Figure FDA0003843116360000022
X为位姿点集合；e(xi,xj,zij)为向量误差函数，代表xi和xj两者的估计关系与测量值或理论值zij的匹配程度；Ωij为该误差所占权重矩阵；x*为对目标函数求取最小值得到的位姿最优解；误差函数e(xi,xj,zij)具体表示如下式所示，
Figure FDA0003843116360000023
其中
Figure FDA0003843116360000024
表示顶点xi和xj之间位姿相对变换的估计值。
3.根据权利要求1所述的一种基于AGV的惯导-WiFi信号融合定位算法，其特征在于，构建AGV轨迹导航关系约束具体如下：
设AGV在相邻两个采样点之间的空间相对位姿转换关系如下式所示，
Figure FDA0003843116360000025
其中Δxt和Δyt为根据导航关系得出的t时刻两个采样点之间位置坐标差的估计值，
Figure FDA0003843116360000031
为航向差的估计值，ut为系统观测值；则轨迹导航关系约束的误差项如下式所示，
Figure FDA0003843116360000032
其中xt、yt、
Figure FDA0003843116360000033
和xt-1、yt-1、
Figure FDA0003843116360000034
分别为t时刻和t-1时刻的位姿Xt和Xt-1的参数。
4.根据权利要求1所述的一种基于AGV的惯导-WiFi信号融合定位算法，其特征在于，构建实际轨迹点和特定标定点的回环约束具体如下：
设特定标定点位姿集为
Figure FDA0003843116360000035
其中f表示取了f个特定标定点，集合中元素
Figure FDA0003843116360000036
代表某个特定标定点的平面坐标；
AGV路过这些特定标定点时停止几秒，根据这些停止间隙识别出AGV静止状态，以此得到轨迹点集中AGV对应的静止位姿顶点集
Figure FDA0003843116360000037
其中s表示轨迹中一共有s个实际轨迹点，集合中元素
Figure FDA0003843116360000038
代表AGV轨迹中某个实际轨迹点的平面坐标；
将实际轨迹点与其对应停止地点的特定标定点联系起来，对漂移的轨迹在数学上作逼近特定标定点的回环约束，并且当AGV多次路过同一个特点标定点时，将这些对应的实际轨迹点之间联系起来，构成实际轨迹点之间的回环约束；
设某t时刻AGV静止，此时实际轨迹点坐标为
Figure FDA0003843116360000039
其对应的地面特定标定点坐标为
Figure FDA00038431163600000310
则实际轨迹点和标定点间的回环检测的误差项如下式所示，
Figure FDA0003843116360000041
5.根据权利要求1所述的一种基于AGV的惯导-WiFi信号融合定位算法，其特征在于，所述具有明显的地理位置和形状特征的轨迹为弯道；AGV特定轨迹识别包括如下步骤：
SA1：输入轨迹位姿顶点集X，根据轨迹规模设定弯道序列窗口大小w，设置角度判定阈值θ，窗口前后采样点数T；
SA2：从轨迹起点开始逐一向后移动滑动窗口，如果当前滑动窗口左侧时间t>T，则直接转到步骤SA3，否则置窗口左边T＝1后转到步骤SA3；
SA3：按照是否符合下式的条件判定所需弯道，
Figure FDA0003843116360000042
其中，
Figure FDA0003843116360000043
为弯道序列窗口前T个采样点航向角的平均值，
Figure FDA0003843116360000044
为弯道序列窗口后T个采样点航向角的平均值；
当
Figure FDA0003843116360000045
大于设定的角度阈值θ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序列
Figure FDA0003843116360000051
若两个弯道的RSS序列的欧氏距离小于设定阈值sth，则认为两个弯道为同一个地点的概率较大；设RSS序列长度为l，则弯道的序列集合cor中第k1和第k2个弯道的RSS序列之间欧氏距离如下式所示，
Figure FDA0003843116360000052
当
Figure FDA0003843116360000053
时可以推测这两个弯道为同一地点弯道的概率较大；
SB22：取各弯道集合中每个弯道序列的中点坐标，构成弯道中点坐标集合
Figure FDA0003843116360000054
N为弯道总个数，当弯道中点坐标集中两个点之间的欧氏距离小于设定阈值dth时，即表示两个弯道在地理空间上处于比较近的一个范围内，为同一地点弯道的概率较大，弯道的序列集合cor中第k1个和第k2个弯道中点的距离如下式所示，
Figure FDA0003843116360000055
SB3：当第k1个和第k2个弯道满足
Figure FDA0003843116360000061
和
Figure FDA0003843116360000062
两个条件时，对两个弯道进行ICP匹配，得到转换角度Δθ、移动距离Δd，并判断Δθ12≤θth,ICP、Δd12≤dth,ICP，如果是则构建该弯道对的回环约束，否则返回步骤SB2，具体如下：
设两个弯道序列cor1和cor2的位姿点集分别为
Figure FDA0003843116360000063
和
Figure FDA0003843116360000064
其中
Figure FDA0003843116360000065
表示弯道序列cori中第k个位姿点的坐标向量；将这两个弯道序列输入ICP算法得到两者之间变换的位姿矩阵，如下式所示，
Figure FDA0003843116360000066
其中R12为旋转矩阵，利用反三角函数可求解出二者之间旋转变化的角度Δθ12；Δd12代表两个弯道对应位姿点之间的平均距离向量，Δd12＝[Δx12,Δy12]T；当两个弯道的变换角度Δθ12小于设定阈值θth,ICP且两个弯道中对应点的平均距离小于设定阈值dth,ICP＝[Δxth,ICP,Δyth,ICP]T，即Δθ12≤θth,ICP且Δd12≤dth,ICP时，判定两个弯道为同一地点的弯道，形成一个回环约束；此时两个匹配弯道对中对应第k对位姿点之间的误差项如下式所示，
Figure FDA0003843116360000067
SB4：判断是否还有未匹配的弯道对，如果还有则回到步骤SB2，否则算法结束。
CN202111020746.5A 2021-09-01 2021-09-01 一种基于AGV的惯导-WiFi信号融合定位算法 Active CN113891251B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111020746.5A CN113891251B (zh) 2021-09-01 2021-09-01 一种基于AGV的惯导-WiFi信号融合定位算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111020746.5A CN113891251B (zh) 2021-09-01 2021-09-01 一种基于AGV的惯导-WiFi信号融合定位算法

Publications (2)

Publication Number Publication Date
CN113891251A CN113891251A (zh) 2022-01-04
CN113891251B true CN113891251B (zh) 2022-12-23

Family

ID=79011605

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111020746.5A Active CN113891251B (zh) 2021-09-01 2021-09-01 一种基于AGV的惯导-WiFi信号融合定位算法

Country Status (1)

Country Link
CN (1) CN113891251B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117889871B (zh) * 2024-03-14 2024-05-10 德博睿宇航科技(北京)有限公司 一种导航路网匹配精准位置梯次迭代搜寻方法和系统

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112052993A (zh) * 2020-08-27 2020-12-08 上海应用技术大学 一种基于改进teb算法的agv局部路径规划方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104374395A (zh) * 2014-03-31 2015-02-25 南京邮电大学 基于图的视觉slam方法
CN110320809B (zh) * 2019-08-19 2022-04-01 杭州电子科技大学 一种基于模型预测控制的agv轨迹修正方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112052993A (zh) * 2020-08-27 2020-12-08 上海应用技术大学 一种基于改进teb算法的agv局部路径规划方法

Also Published As

Publication number Publication date
CN113891251A (zh) 2022-01-04

Similar Documents

Publication Publication Date Title
CN109916410B (zh) 一种基于改进平方根无迹卡尔曼滤波的室内定位方法
CN112584306B (zh) 一种基于卡尔曼滤波的室内机器人定位算法
CN113706612B (zh) 融合uwb和单目视觉slam的煤矿井下车辆定位方法
González et al. Mobile robot localization based on ultra-wide-band ranging: A particle filter approach
Röhrig et al. Localization of an omnidirectional transport robot using IEEE 802.15. 4a ranging and laser range finder
CN103471595A (zh) 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
CN111982102B (zh) 一种复杂环境下基于bp-ekf的uwb-imu定位方法
CN114998276B (zh) 一种基于三维点云的机器人动态障碍物实时检测方法
CN113891251B (zh) 一种基于AGV的惯导-WiFi信号融合定位算法
Gao et al. Localization of mobile robot based on multi-sensor fusion
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
Zheng et al. Mobile robot integrated navigation algorithm based on template matching VO/IMU/UWB
CN118129746A (zh) 一种gnss/惯性/视觉的车辆组合导航方法
Lin et al. Drift-free visual slam for mobile robot localization by integrating uwb technology
Ju et al. Scene-aware error modeling of LiDAR/visual odometry for fusion-based vehicle localization
Santos et al. Localization using ultra wideband and IEEE 802.15. 4 radios with nonlinear bayesian filters: a comparative study
CN115421153B (zh) 一种基于扩展卡尔曼滤波的激光雷达和uwb组合定位方法及系统
Lategahn et al. Global localization of automated guided vehicles in wireless networks
Ulmschneider et al. Cooperative Estimation of Maps of Physical and Virtual Radio Transmitters
CN116399321A (zh) 基于双目视觉、imu和agps融合的户外移动机器人导航方法
Ge et al. Integrating WSN and Laser SLAM for Mobile Robot Indoor Localization.
Luo et al. Accurate localization for indoor and outdoor scenario by GPS and UWB fusion
Piperigkos et al. 5G enabled cooperative localization of connected and semi-autonomous vehicles via sparse Laplacian processing
Su et al. GNSS-Aided Visual-Inertial Odomtry with Failure Mode Recognition
Zeng et al. An introduction for the indoor localization systems and the position estimation algorithms

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant