[发明专利]一种基于AGV的惯导-WiFi信号融合定位算法

权利要求书

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中，构件边约束的优化目标函数的一般形式如下式所示，

x^*＝argminF(x)

其中，F(x)为求解AGV最优轨迹的误差平方函数；x_i和x_j为顶点参数向量，即AGV的位姿点X为位姿点集合；e(x_i,x_j,z_ij)为向量误差函数，代表x_i和x_j两者的估计关系与测量值或理论值z_ij的匹配程度；Ω_ij为该误差所占权重矩阵；x^*为对目标函数求取最小值得到的位姿最优解；误差函数e(x_i,x_j,z_ij)具体表示如下式所示，

其中表示顶点x_i和x_j之间位姿相对变换的估计值。

3.根据权利要求1所述的一种基于AGV的惯导-WiFi信号融合定位算法，其特征在于，构建AGV轨迹导航关系约束具体如下：

设AGV在相邻两个采样点之间的空间相对位姿转换关系如下式所示，

其中Δx_t和Δy_t为根据导航关系得出的t时刻两个采样点之间位置坐标差的估计值，为航向差的估计值，u_t为系统观测值；则轨迹导航关系约束的误差项如下式所示，

其中x_t、y_t、和x_t-1、y_t-1、分别为t时刻和t-1时刻的位姿X_t和X_t-1的参数。