一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法

一种基于anfis-eskf的地面移动机器人多传感器融合定位方法
技术领域
1.本发明属于导航技术领域，特别是一种基于anfis-eskf的地面移动机器人多传感器融合定位方法。


背景技术：

2.里程计系统使用本体传感器来估计车辆相对于起始点的瞬时位置和方向，这是自动导航系统中的一个关键模块，可以在外部传感器不可用时提供位置信息。常用的机载传感器包括车轮编码器、惯性测量单元(imu)、摄像机和激光雷达。使用单个传感器的里程计的性能受传感器特性、环境干扰和车辆状态的影响，通常不能提供稳定和满意的定位结果。
3.多传感器融合技术可以融合多个传感器的信息得到稳定的定位结果。应用最广泛的传感器融合方法是卡尔曼滤波器(kf)的变种，如扩展卡尔曼滤波器(ekf)和无迹卡尔曼滤波器(ukf)。如国内专利cn201310455813.5《gps盲区下融合多源信息的车辆高精度定位方法及装置》，该方法使用陀螺仪输出的角速率信息和加速度信息、路面图像信息输入到卡尔曼滤波器得到最终融合信息、再由道路旁边设置的锚节点重新矫正融合算法产生的累计误差，可以提供稳定、可靠、高精度的车辆位置信息。如专利cn201310369354.9《一种面向室内移动机器人的光流场视觉/ins组合导航方法》中所述，利用光流场视觉和imu等信息输入到扩展卡尔曼滤波器进行融合计算，能够提高室内移动机器人的导航精度。
4.尽管卡尔曼滤波器及其变种在某些应用中被证明是有效的，但它们有一个共同的假设，对于实际的工作场景可能是有问题的——不变协方差。在现实中，测量噪声不确定，模型误差存在，车辆经历暂时不可预测的运动变化，环境局部改变。这种变化的条件需要适当处理，否则定位性能将严重降低。以往的误差状态卡尔曼滤波(eskf)方法在预测模型和测量模型中都不能解决具有不同不确定性的传感器融合问题。基于anfis-eskf(adaptive network-based fuzzy inference system
–
error state kalmanfilter，自适应网络模糊推理系统-误差状态卡尔曼滤波)方法可以对系统的瞬时变化做出响应，并且在融合算法中可以独立确定两个协方差矩阵，从而提高定位系统的鲁棒性。


技术实现要素：

5.为了解决现有技术问题，本发明的目的在于克服已有技术存在的卡尔曼滤波及其变种模型在多传感器融合技术中不足，提供一种基于anfis-eskf的地面移动机器人多传感器融合定位方法，能够在线检测异常工况并且调整融合增益的基于anfis-eskf的地面移动机器人多传感器融合定位，能够解决现有卡尔曼滤波技术的弊端，从而实现对多传感器融合的瞬时变化做出响应，并确定独立确定两个协方差矩阵，提高定位系统的鲁棒性。
6.为达到上述发明创造目的，本发明采用如下技术方案：
7.一种基于anfis-eskf的地面移动机器人多传感器融合定位方法，包括模块1、模块2、模块3和模块4共四个计算模块，其特征在于：
8.所述模块1，建立轮式-imu里程计(wio)的测量模型，推导系统的状态方程；
9.所述模块2，建立视觉里程计(vo)的测量模型，推导系统的观测方程；
10.所述模块3，用误差状态卡尔曼滤波(eskf)对wio和vo的运动估计信息进行融合；
11.所述模块4，建立自适应模糊神经推理系统(anfis)，根据wio和vo的直接信息，推断里程计的运动状态，实时调整eskf中预测模型和测量模型的协方差矩阵；该模块在使用前需要根据gps数据对anfis进行训练；
12.所述模块1的输入是轮式里程计数据和imu数据，输出是相邻时刻imu坐标系的位姿估计；模块2的输入是双目相机拍摄的照片，输出是相邻时刻相机坐标系的位姿估计；模块3的输入是模块1和模块2的输出，输出是相邻时刻imu坐标系的位姿估计；模块4的输入模块1中wio和模块2中vo的中间数据，输出是模块3中需要使用的wio和vo的协方差矩阵；
13.整个定位程序的输入是imu数据、轮式里程计数据、相机数据，输出是imu坐标系的位姿变化。
14.优选地，所述模块1中建立wio测量模型，具体如下：
15.imu包括三轴陀螺仪和三轴加速度计，考虑到测试系统加速度计中的噪声极大地影响了位移估计，选择仅使用陀螺仪来估计方向变化；作为补偿，引入车轮编码器进行平移运动计算，并与陀螺仪集成，融合为轮式-imu里程计wio，提供完整的六自由度运动估计；wio为eskf系统提供移动机器人运动状态的预测；
16.下面的式子(1)展示了i+1时刻wio测得的imu坐标系{i}的位置变化：
17.其中是i+1时刻测得的imu坐标系的姿态变化，是轮式里程计坐标系{w}相对于imu坐标系的位置标定关系，是轮式里程计坐标系相对于imu坐标系的姿态标定关系，是i+1时刻测得的轮式里程计坐标系的位置变化，i是单位矩阵。
18.优选地，所述模块1中建立系统状态方程，具体如下：
19.定义系统的状态为：
20.其中pi为i时刻imu坐标系相对于初始时刻的位置，qi为i时刻imu坐标系相对于初始时刻的姿态，为i时刻imu角速度偏移；
21.那么根据wio测量模型得到系统的名义状态方程为：
22.其中是i时刻轮式里程计的测量值，是i时刻imu坐标系相对于0时刻的姿
态，是i时刻imu测得的角速度，是i时刻imu的角速度偏移，δt是imu的采样周期，[γi]
×
指的是γi对应的反对称矩阵；p
i+1
表示i+1时刻imu坐标系相对于初始时刻imu坐标系的位置，表示i+1时刻imu坐标系相对于初始时刻imu坐标系的姿态对时间的导数，表示i+1时刻imu角速度偏移对时间的导数；
[0023]
误差状态方程为：
[0024][0025]
其中θi是qi对应的李代数，是imu的测量噪声，是imu角速度偏移的导数，表示i时刻轮式里程计的测量噪声；ω
ni
表示i时刻imu的角速度测量噪声；δt表示imu采样周期。
[0026]
优选地，所述模块2中建立视觉里程计(vo)的测量模型，推导系统的观测方程，具体如下：
[0027]
视觉里程计可估计移动机器人在两个序列帧之间的姿态变化和位置变化观测模型为：
[0028][0029][0030]
是相机观测到的从第j-1帧到第j帧的位移，它和相等；是的从时间j-1到时间j的旋转向量，和等价；δp
c，n
是随机位移误差，服从高斯分布δp
c，n
～n(0，v1)；δθ
c，n
是随机的姿态误差，服从高斯分布δθ
c，n
～n(0，v2)；表示第j-1帧到第j帧的相机位移变化的测量值；表示从时间j-1到时间j的相机姿态变化的测量值；
[0031]
根据imu坐标系{i}和相机坐标系{c}的位姿关系和vo可提供相机观测到的相对于imu坐标系{i
j-1
}的实时位姿变化，如下式(7)所示：
[0032]
进一步地，所述模块2中建立系统的观测方程，具体如下：
[0033][0034]
上式中，表示相机观测到的相机坐标系的位移变化；表示相机观测到的imu坐标系的位移变化；表示相机观测到的imu坐标系的姿态变化。
[0035]
优选地，在所述模块4中，建立自适应模糊神经推理系统anfis，根据wio和vo的直接信息，推断里程计的运动状态，实时调整eskf中预测模型和测量模型的协方差矩阵，具体步骤如下：
[0036]
采用两个anfis，第一个anfis记作wio-anfis，其输入的是移动机器人的角速度ω和左右车轮的转速差ρ，输出是wio的协方差矩阵q1；第二个anfis记作vo-anfis，其输入的是视觉里程计的特征点个数nu和重投影误差er，输出是vo的协方差矩阵v；
[0037]
定义wio-anfis的输入为xw＝(ω，ρ)
t
，定义它的模糊集为a；为a中的每一个成员aa设计一个隶属度函数，并且记作a
mf-a
(xw)∈[0,1]；a
mf-a
(xw)越接近1，xw属于aa的可能性越大；vo-anfis的输入是特征点数量nu和重投影误差er记作xv＝(nu，er)
t
，定义它的模糊集为b；构建隶属度函数b
mf-b
(x
ν
)的方法与a
mf-a
(xw)类似；
[0038]
在设计隶属函数时考虑了模糊集合中成员之间的平滑过渡；曲线形状通过改变函数中的自适应参数进行调整；选择合适的自适应参数的原则是保证函数的灵活性和保持有限的变量数量；
[0039]
在a中为ρ定义了四个成员，它们分别趋向于+∞(a1)，趋向于1(a2)，趋向于-1(a3)，趋向于-∞(a4)；它们分别对应于移动机器人滑转、直走、旋转和滑转；a中的ω有两个成员：小(a5)和大(a6)；它们分别对应着慢转弯和快转弯的运动情况；同样，b中er的模糊集分别为小(b1)和大(b2)，nu的模糊集分别为小的(b3)和大的(b4)；
[0040]
隶属度函数定义了输入与其对应的模糊成员之间的关系，在每个隶属函数中，有一两个变量使其可调整，以更好地适应真实的系统情况；表示a和b中的自适应变量分别为aw和bv；a
w1
指定了ρ完全属于a1时初始值；a
w2
指定了ρ属于a2的终点；a
w3
指定了ρ开始属于a3的点，a
w4
指定了ρ完全属于a4时的最大值；函数的另一端固定在临界点
±
1，简化了复杂度，降低了anfis训练中需要优化的变量的维数；a
w5
,a
w6
,a
w7
,a
w8
,指定了ω的隶属度函数的临界点；b
v1
,b
v2
,b
v3
,b
v4
指定了er的隶属度函数的临界点；b
v5
,b
v6
,b
v7
,b
v8
指定了nu的隶属度函数的临界点；曲线形状的控制方式同a
w1
,a
w2
,a
w3
,a
w4
；
[0041]
模糊规则库定义了模糊集与输出之间的逻辑关系；根据不同的输入条件，对wio和vo的测量方差进行了推理；
[0042]
对于wio-anfis，当输入组合为(a3,a6)和(a5,a2)时，方差为低，用w
l
表示；物理的解释是，当车辆正在急转弯(a3)，并且imu测量表明同样的情况(a6)，或移动机器人运动中没有大旋转(a2)并且imu测量表明同样的情况(a5)，移动机器人运动时可能没有打滑，从而wio的方差应该比较小；否则，imu的方差应该比较大，用wh表示；
[0043]
对于每一条规则，例如π
w7
(a3,a6)，它的输出定义如式子(9)所示；根据同样的规则，得到π
wa
的输出，用g
aw
表示；
[0044]g7w
＝min(a
mf-6
(xw),a
mf-3
(xw))
ꢀꢀꢀꢀꢀꢀꢀꢀ
(9)类似地，也为vo-anfis建立了规则库；为视觉的运动估计定义了高中低三个方差，
分别用vh,vm,v
l
,表示，相应的模糊规则的输出用g
bc
表示；
[0045]
根据式子(10)用g
aw
来计算wio-anfis的归一化推理结果类似地，得到vo-anfis的a表示wio-anfis中模糊规则的编号，b表示vo-anfis中模糊规则的编号，na表示wio-anfis中的模糊规则总数；
[0046]
根据各种工况的具体测量方差vh,vm,v
l
,w
h w
l
,则wio的方差q1和vo的方差v的最终结果由式子(11)得到：
[0047][0048]
上式中w
πwa
表示规则π
wa
对应的权重，v
πvb
表示规则π
vb
对应的权重，a表示wio-anfis中模糊规则的编号，b表示vo-anfis中模糊规则的编号。
[0049]
进一步优选地，在所述模块4中，模糊集与相应系统情况的关系如下表所示：
[0050][0051]
在本发明中，模糊推理中隶属函数的参数也可根据经验和直觉来选择；而为了实现anfis系统的适应性，需要对里程计的测量有一个真值；它是为车辆提供无漂移定位的gps单元；利用gps测量值作为训练集的真值的难点在于gps与imu的位置不一致；这种偏差很大，特别是在车辆急转弯时，会引入训练数据的误差，导致参数优化错误；因此，将gps的直接测量转化到imu坐标系{i}中，以提供准确的训练数据；gps接收机在imu坐标系{i}中的坐标可从系统设计参数中导出，但对于训练目的来说不够精确，因此在使用它进一步训练anfis参数之前有必要对其进行优化。
[0052]
优选地，在所述模块4中，建立自适应模糊神经推理系统(anfis)的训练方法，具体步骤如下：
[0053]
将gps的直接测量转化到imu坐标系{i}中，以提供准确的训练数据；gps接收机在
坐标系{i0}中的位置可表示为：
[0054]
其中，ixg是gps接收机在imu坐标系{i}中的坐标，是在时间i时，坐标系{i}相对于坐标系{i0}的旋转矩阵；是时间i时gps路径点在gps测量坐标系{gm}中的表示，是坐标系{gm}和坐标系{i0}之间的旋转矩阵；
[0055]
相应地，定义相对于gps测量的估计轨迹误差为:
[0056]
其中为里程计的估计轨迹；
[0057]ixg从系统设计参数中导出，在使用它进一步训练anfis参数之前，有必要对其进行优化；对于优化后的gps相对于坐标系{i}的位置估计，可根据求式子(14)的解得到；
[0058]
其中i表示时间戳的索引，ni表示时间戳的总数，s表示训练轨迹的索引，ns表示训练轨迹的总数；
[0059]
需要优化的参数包括aw，bv和方差wh，w
l
，vh，vm，v
l
，用式(15)表示：
[0060]
x
anfis
＝[aw，bv，wh，w
l
，vh，vm，v
l
]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)已知ixg和的情况下，式子(15)中的误差用来构造如下代价优化；x
anfis
的解是模糊网络的优化后的参数集合，用来拟合训练数据集；
[0061]
其中是根据参数x
anfis
利用anfis-eskf估计的gps接收机在时间i时相对于坐标系{i0}的位置。
[0062]
本发明与现有技术相比较，具有如下显而易见的突出实质性特点和显著优点：
[0063]
1.本发明经过训练的anfis系统能够根据地面移动机器人系统的状态，比如机器人是否打滑或者环境中的纹理信息是否足够多等，来实时调整eskf的参数，与传统的固定增益滤波器相比，大大减少了内部故障和外部干扰造成的误差，使得地面移动机器人在各种复杂工况下都能够保持较高的定位精度，鲁棒性更强；
[0064]
2.采用本发明方法，在移动机器人上对算法进行验证，结果表明，与固定增益eskf相比，基于anfis的eskf的平均轨迹误差降低了80.6％，即使有意引入外部干扰，其平均位置漂移也在1％以下。
附图说明
[0065]
图1为本发明提供的基于anfis-eskf的移动机器人多传感器融合定位方法的原理图。
[0066]
图2为本发明优选实施例提供的anfis框架，其中(a)为wio的协方差推算的anfis
框架，(b)为vo的协方差推算的anfis框架。
[0067]
图3为本发明优选实施例提供的车轮里程计在一个采样周期内的航位推算方法。
[0068]
图4为本发明优选实施例的anfis的隶属度函数。
具体实施方式
[0069]
以下结合具体的实施例子对上述方案做进一步说明，本发明的优选实施例详述如下：
[0070]
实施例一：
[0071]
在本实施例中，参见图1，一种基于anfis-eskf的地面移动机器人多传感器融合定位方法，包括模块1、模块2、模块3和模块4共四个计算模块；
[0072]
所述模块1，建立轮式-imu里程计(wio)的测量模型，推导系统的状态方程；
[0073]
所述模块2，建立视觉里程计(vo)的测量模型，推导系统的观测方程；
[0074]
所述模块3，用误差状态卡尔曼滤波(eskf)对wio和vo的运动估计信息进行融合；
[0075]
所述模块4，建立自适应模糊神经推理系统(anfis)，根据wio和vo的直接信息，推断里程计的运动状态，实时调整eskf中预测模型和测量模型的协方差矩阵；该模块在使用前需要根据gps数据对anfis进行训练；
[0076]
所述模块1的输入是轮式里程计数据和imu数据，输出是相邻时刻imu坐标系的位姿估计；模块2的输入是双目相机拍摄的照片，输出是相邻时刻相机坐标系的位姿估计；模块3的输入是模块1和模块2的输出，输出是相邻时刻imu坐标系的位姿估计；模块4的输入是模块1中wio和模块2中vo的中间数据，输出是模块3中需要使用的wio和vo的协方差矩阵；
[0077]
整个定位程序的输入是imu数据、轮式里程计数据、相机数据，输出是imu坐标系的位姿变化。
[0078]
本实施例能够在线检测异常工况并且调整融合增益实现基于anfis-eskf的地面移动机器人多传感器融合定位，能够实现对多传感器融合的瞬时变化做出响应。
[0079]
实施例二：
[0080]
本实施例与实施例一基本相同，特别之处在于：
[0081]
在本实施例中，一种基于anfis-eskf的移动机器人多传感器融合定位方法，包括以下模块：
[0082]
模块1，建立轮式-imu里程计(wio)的测量模型，推导系统的状态方程；
[0083]
模块2，建立视觉里程计(vo)的测量模型，推导系统的观测方程；
[0084]
模块3，用eskf对wio和vo的运动估计信息进行融合；
[0085]
模块4，建立anfis系统，根据wio和vo的直接信息，推断里程计的运动状态，实时调整eskf中预测模型和测量模型的协方差矩阵；
[0086]
所述模块1中建立wio测量模型，具体如下：
[0087]
imu包括三轴陀螺仪和三轴加速度计，考虑到测试系统加速度计中的噪声极大地影响了位移估计，选择仅使用陀螺仪来估计方向变化；作为补偿，引入车轮编码器进行平移运动计算，并与陀螺仪集成，融合为轮式-imu里程计wio，提供完整的六自由度运动估计；wio为eskf系统提供移动机器人运动状态的预测；
[0088]
下面的式子(1)展示了i+1时刻wio测得的imu坐标系{i}的位置变化：
[0089]
其中是i+1时刻测得的imu坐标系的姿态变化，是轮式里程计坐标系{w}相对于imu坐标系的位置标定关系，是轮式里程计坐标系相对于imu坐标系的姿态标定关系，是i+1时刻测得的轮式里程计坐标系的位置变化，i是单位矩阵。
[0090]
优选地，所述模块1中建立系统状态方程，具体如下：
[0091]
定义系统的状态为：
[0092]
其中pi为i时刻imu坐标系相对于初始时刻的位置，qi为i时刻imu坐标系相对于初始时刻的姿态，为i时刻imu角速度偏移；
[0093]
那么根据wio测量模型得到系统的名义状态方程为：
[0094]
其中是i时刻轮式里程计的测量值，是i时刻imu坐标系相对于0时刻的姿态，是i时刻imu测得的角速度，是i时刻imu的角速度偏移，δt是imu的采样周期，[γi]
×
指的是γi对应的反对称矩阵；p
i+1
表示i+1时刻imu坐标系相对于初始时刻imu坐标系的位置，表示i+1时刻imu坐标系相对于初始时刻imu坐标系的姿态对时间的导数，表示i+1时刻imu角速度偏移对时间的导数；
[0095]
误差状态方程为：
[0096][0097]
其中θi是qi对应的李代数，ω
ni
是imu的测量噪声，是imu角速度偏移的导数，表示i时刻轮式里程计的测量噪声；ω
ni
表示i时刻imu的角速度测量噪声；δt表示imu采样
周期。
[0098]
在本实施例中，所述模块2中建立视觉里程计(vo)的测量模型，推导系统的观测方程，具体如下：
[0099]
视觉里程计可估计移动机器人在两个序列帧之间的姿态变化和位置变化观测模型为：
[0100][0101][0102]
是相机观测到的从第j-1帧到第j帧的位移，它和相等；是的从时间j-1到时间j的旋转向量，和等价；δp
c，n
是随机位移误差，服从高斯分布δp
c，n
～n(0，v1)；δθ
c，n
是随机的姿态误差，服从高斯分布δθ
c，n
～n(0，v2)；表示第j-1帧到第j帧的相机位移变化的测量值；表示从时间j-1到时间j的相机姿态变化的测量值；
[0103]
根据imu坐标系{i}和相机坐标系{c}的位姿关系和vo可提供相机观测到的相对于imu坐标系{i
j-1
}的实时位姿变化，如下式(7)所示：
[0104]
在本实施例中，所述模块2中建立系统的观测方程，具体如下：
[0105][0106]
上式中，表示相机观测到的相机坐标系的位移变化；表示相机观测到的imu坐标系的位移变化；表示相机观测到的imu坐标系的姿态变化。
[0107]
优选地，在所述模块4中，建立自适应模糊神经推理系统anfis，根据wio和vo的直接信息，推断里程计的运动状态，实时调整eskf中预测模型和测量模型的协方差矩阵，具体步骤如下：
[0108]
采用两个anfis，第一个anfis记作wio-anfis，其输入的是移动机器人的角速度ω和左右车轮的转速差ρ，输出是wio的协方差矩阵q1；第二个anfis记作vo-anfis，其输入的是视觉里程计的特征点个数nu和重投影误差er，输出是vo的协方差矩阵v；
[0109]
定义wio-anfis的输入为xw＝(ω，ρ)
t
，定义它的模糊集为a；为a中的每一个成员aa设计一个隶属度函数，并且记作a
mf-a
(xw)∈[0,1]；a
mf-a
(xw)越接近1，xw属于aa的可能性越大；vo-anfis的输入是特征点数量nu和重投影误差er记作xv＝(nu，er)
t
，定义它的模糊集为b；构建隶属度函数b
mf-b
(x
ν
)的方法与a
mf-a
(xw)类似；
[0110]
在设计隶属函数时考虑了模糊集合中成员之间的平滑过渡；曲线形状通过改变函数中的自适应参数进行调整；选择合适的自适应参数的原则是保证函数的灵活性和保持有限的变量数量；
[0111]
在a中为ρ定义了四个成员，它们分别趋向于+∞(a1)，趋向于1(a2)，趋向于-1(a3)，
趋向于-∞(a4)；它们分别对应于移动机器人滑转、直走、旋转和滑转；a中的ω有两个成员：小(a5)和大(a6)；它们分别对应着慢转弯和快转弯的运动情况；同样，b中er的模糊集分别为小(b1)和大(b2)，nu的模糊集分别为小的(b3)和大的(b4)；
[0112]
隶属度函数定义了输入与其对应的模糊成员之间的关系，在每个隶属函数中，有一两个变量使其可调整，以更好地适应真实的系统情况；表示a和b中的自适应变量分别为aw和bv；a
w1
指定了ρ完全属于a1时初始值；a
w2
指定了ρ属于a2的终点；a
w3
指定了ρ开始属于a3的点，a
w4
指定了ρ完全属于a4时的最大值；函数的另一端固定在临界点
±
1，简化了复杂度，降低了anfis训练中需要优化的变量的维数；a
w5
,a
w6
,a
w7
,a
w8
,指定了ω的隶属度函数的临界点；b
v1
,b
v2
,b
v3
,b
v4
指定了er的隶属度函数的临界点；b
v5
,b
v6
,b
v7
,b
v8
指定了nu的隶属度函数的临界点；曲线形状的控制方式同a
w1
,a
w2
,a
w3
,a
w4
；
[0113]
模糊规则库定义了模糊集与输出之间的逻辑关系；根据不同的输入条件，对wio和vo的测量方差进行了推理；
[0114]
对于wio-anfis，当输入组合为(a3,a6)和(a5,a2)时，方差为低，用w
l
表示；物理的解释是，当车辆正在急转弯(a3)，并且imu测量表明同样的情况(a6)，或移动机器人运动中没有大旋转(a2)并且imu测量表明同样的情况(a5)，移动机器人运动时可能没有打滑，从而wio的方差应该比较小；否则，imu的方差应该比较大，用wh表示；
[0115]
对于每一条规则，例如π
w7
(a3,a6)，它的输出定义如式子(9)所示；根据同样的规则，得到π
wa
的输出，用g
aw
表示；
[0116]g7w
＝min(a
mf-6
(xw),a
mf-3
(xw))
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)类似地，也为vo-anfis建立了规则库；为视觉的运动估计定义了高中低三个方差，分别用vh,vm,v
l
,表示，相应的模糊规则的输出用g
bc
表示；
[0117]
根据式子(10)用g
aw
来计算wio-anfis的归一化推理结果类似地，得到vo-anfis的a表示wio-anfis中模糊规则的编号，b表示vo-anfis中模糊规则的编号，na表示wio-anfis中的模糊规则总数；
[0118]
根据各种工况的具体测量方差vh,vm,v
l
,w
h w
l
,则wio的方差q1和vo的方差v的最终结果由式子(11)得到：
[0119][0120]
上式中，w
πwa
表示规则π
wa
对应的权重，v
πvb
表示规则π
vb
对应的权重，a表示wio-anfis中模糊规则的编号，b表示vo-anfis中模糊规则的编号。
[0121]
在本实施例中，在所述模块4中，模糊集与相应系统情况的关系如下表所示：
[0122][0123]
在本实施例中，模糊推理中隶属函数的参数也可根据经验和直觉来选择；而为了实现anfis系统的适应性，需要对里程计的测量有一个真值；它是为车辆提供无漂移定位的gps单元；利用gps测量值作为训练集的真值的难点在于gps与imu的位置不一致；这种偏差很大，特别是在车辆急转弯时，会引入训练数据的误差，导致参数优化错误；因此，将gps的直接测量转化到imu坐标系{i}中，以提供准确的训练数据；gps接收机在imu坐标系{i}中的坐标可从系统设计参数中导出，但对于训练目的来说不够精确，因此在使用它进一步训练anfis参数之前有必要对其进行优化。
[0124]
在本实施例中，在所述模块4中，建立自适应模糊神经推理系统(anfis)的训练方法，具体步骤如下：
[0125]
将gps的直接测量转化到imu坐标系{i}中，以提供准确的训练数据；gps接收机在坐标系{i0}中的位置可表示为：
[0126]
其中，ixg是gps接收机在imu坐标系{i}中的坐标，是在时间i时，坐标系{i}相对于坐标系{i0}的旋转矩阵；是时间i时gps路径点在gps测量坐标系{gm}中的表示，是坐标系{gm}和坐标系{i0}之间的旋转矩阵；
[0127]
相应地，定义相对于gps测量的估计轨迹误差为:
[0128]
其中为里程计的估计轨迹；
[0129]ixg从系统设计参数中导出，在使用它进一步训练anfis参数之前，有必要对其进行优化；对于优化后的gps相对于坐标系{i}的位置估计，可根据求式子(14)的解得到；
[0130]
其中i表示时间戳的索引，ni表示时间戳的总数，s表示训练轨迹的索引，ns表示训练轨迹的总数；
[0131]
需要优化的参数包括aw，bv和方差wh，w
l
，vh，vm，v
l
，用式(15)表示：
[0132]
x
anfis
＝[aw，bv，wh，w
l
，vh，vm，v
l
]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)已知ixg和的情况下，式子(15)中的误差用来构造如下代价优化；x
anfis
的解是模糊网络的优化后的参数集合，用来拟合训练数据集；
[0133]
其中是根据参数x
anfis
利用anfis-eskf估计的gps接收机在时间i时相对于坐标系{i0}的位置。
[0134]
本实施例基于anfis-eskf的地面移动机器人多传感器融合定位方法建立轮式-imu里程计(wio)的测量模型，推导系统的状态方程；建立视觉里程计(vo)的测量模型，推导系统的观测方程；用eskf对wio和vo的运动估计信息进行融合；建立anfis(adaptive neural fuzzy inference system)，根据wio和vo的直接信息，推断里程计的运动状态，实时调整eskf(error state kalman filter)中预测模型和测量模型的协方差矩阵。本实施例方法经过训练的anfis系统能够根据地面移动机器人系统的状态，比如机器人是否打滑或者环境中的纹理信息是否足够多等，来实时调整eskf的参数，与传统的固定增益滤波器相比，能够根据系统的状态实时调整eskf参数，大大减少了内部故障和外部干扰造成的误差，使得地面移动机器人在各种复杂工况下都能够保持较高的定位精度，定位精度更高，鲁棒性更强。
[0135]
实施例三：
[0136]
本实施例与实施例一基本相同，特别之处在于：
[0137]
参见图1-图4，在本实施例中，所述模块1中建立wio测量模型，具体如下：
[0138]
imu包括三轴陀螺仪和三轴加速度计，考虑到测试系统加速度计中的噪声极大地影响了位移估计，选择仅使用陀螺仪来估计方向变化；作为补偿，引入车轮编码器进行平移运动计算，并与陀螺仪集成，融合为轮式-imu里程计wio，提供完整的六自由度运动估计；wio为eskf系统提供移动机器人运动状态的预测；
[0139]
图3展示了四轮移动机器人运动学模型的推导，假设左侧车轮速度相同，右侧车轮的速度也相同，α
l
和αr表示在一个采样周期内，左右车轮的旋转角度；假设路面平滑，车轮不打滑，机器人在采样周期内以圆心o1为中心从点o移动到点m；左右车轮对应的行驶距离分别为δs
l
和δsr；式子(17)中δs
l
和δsr的原始测量值中的噪声δsn服从高斯分布轮式里程计坐标系{w}的原点位于车轮的几何中心，因此机器人在此采样时间内的行走距离δs可以由式子(17)得到；
[0140]
其中rw是车轮的半径；
[0141]
δs＝(δsr+δs
l
)/2
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)imu的三轴陀螺仪测量移动机器人的角速度；为了实现eskf，采用了姿态积分误差测量模型；
[0142]
其中ω
t
是由三个轴合成的真实角速度，ωm是测量的角速度，ωn是服从高斯分布的随机游走，ω
bt
是角速度偏移的真实值，ωb是角速度偏移的名义值，δωb是角速度偏移的误差，ω
bt
的导数服从高斯分布ωw表示角速度偏移的噪声，表示角速度偏移的误差的导数；
[0143]
在imu坐标系{i}中测得的角速度矢量可以表示为：
[0144]
定义imu在第i个采样时间内相对imu初始坐标系{i0}测得的四元数表示的姿态变化为相对坐标系{i0}的瞬时旋转速度可以由下式计算：
[0145]
其中是四元数的乘法运算，是的四元数表示；
[0146]
imu在第i+1个采样周期内的姿态可以由角速度的迭代积分得到；
[0147]
wio相对于{i0}的姿态由式子(22)计算得到，位置的输出需要更多的推断；定义和的偏差为另外，也可用旋转矩阵来表示；车轮坐标系的相应的姿态变化可通过下式来获得：
[0148]
wio相对于轮式里程计坐标系{w}的瞬时位移为：
[0149]
它可通过式子(1)转换到坐标系{i}中表示，下面的式子(1)展示了i+1时刻wio测得的imu坐标系{i}的位置变化：
[0150][0151]
在第i+1个采样时间处移动机器人相对于初始坐标系{i0}的位置可以通过式子(25)计算：
[0152]
其中是对应于的旋转矩阵，式子(22)和式子(22)是六自由度wio的位姿估计输出的迭代形式；其中是i+1时刻测得的imu坐标系的姿态变化，是轮式里程计
坐标系{w}相对于imu坐标系的位置标定关系，是轮式里程计坐标系相对于imu坐标系的姿态标定关系，是i+1时刻测得的轮式里程计坐标系的位置变化，i是单位矩阵。
[0153]
在本实施例中，所述模块1中建立系统状态方程，具体如下：
[0154]
定义系统的状态为：
[0155]
其中pi为i时刻imu坐标系相对于初始时刻的位置，qi为i时刻imu坐标系相对于初始时刻的姿态，为i时刻imu角速度偏移；
[0156]
那么根据wio测量模型得到系统的名义状态方程为：
[0157]
其中是i时刻轮式里程计的测量值，是i时刻imu坐标系相对于0时刻的姿态，是i时刻imu测得的角速度，是i时刻imu的角速度偏移，δt是imu的采样周期，[γi]
×
指的是γi对应的反对称矩阵；p
i+1
表示i+1时刻imu坐标系相对于初始时刻imu坐标系的位置，表示i+1时刻imu坐标系相对于初始时刻imu坐标系的姿态对时间的导数，表示i+1时刻imu角速度偏移对时间的导数；
[0158]
误差状态方程为：
[0159][0160]
其中θi是qi对应的李代数，ω
ni
是imu的测量噪声，是imu角速度偏移的倒数，表示i时刻轮式里程计的测量噪声；ω
ni
表示i时刻imu的角速度测量噪声；δt表示imu采样周期。
[0161]
在本实施例中，所述模块2中建立视觉里程计(vo)的测量模型，推导系统的观测方程，具体如下：
[0162]
视觉里程计可估计移动机器人在两个序列帧之间的姿态变化和位置变化观测模型为：
[0163][0164][0165]
是相机观测到的从第j-1帧到第j帧的位移，它和相等；是的从时间j-1到时间j的旋转向量，和等价；δp
c，n
是随机位移误差，服从高斯分布δp
c，n
～n(0，v1)；δθ
c，n
是随机的姿态误差，服从高斯分布δθ
c，n
～n(0，v2)；表示第j-1帧到第j帧的相机位移变化的测量值；表示从时间j-1到时间j的相机姿态变化的测量值；
[0166]
根据imu坐标系{i}和相机坐标系{c}的位姿关系和vo可提供相机观测到的相对于imu坐标系{i
j-1
}的实时位姿变化，如下式(7)所示：
[0167]
在本实施例中，所述模块2中建立系统的观测方程，具体如下：
[0168][0169]
上式中，表示相机观测到的相机坐标系的位移变化；表示相机观测到的imu坐标系的位移变化；表示相机观测到的imu坐标系的姿态变化。
[0170]
进一步地，模块3所述用eskf对wio和vo的运动估计信息进行融合，具体如下：
[0171]
eskf分为预测和校正两个阶段；
[0172]
eskf的预测过程具体如下：
[0173]
根据卡尔曼滤波模型，误差状态δx
i+1
和误差协方差矩阵p由下列式子更新：
[0174][0175]
上式中，表示的扰动；fi表示误差状态方程相对于扰动的雅可比矩阵；f
x
表示误差状态方程相对于误差状态的雅可比矩阵；f
it
表示fi的转置；qi表示扰动的协方差矩阵；f
xt
表示f
x
的转置；pi表示i时刻误差状态的协方差矩阵，p
i+1
表示i+1时刻误差状态的协方差矩阵；
[0176]
式子(4)的结果给出了误差状态模型的线性表示，式子(26)中的矩阵f
x
和矩阵fi为：
[0177]
上式中，i表示单位矩阵；δt表示imu采样周期；
[0178]
另外，随机误差的协方差矩阵q被定义为：
[0179]
q1,q2,q3是相应的三个误差状态的测量噪声的方差，它们被定义为：
[0180]
其中分别是sn、ωn、ωw的方差；
[0181]
eskf的校正过程具体如下：
[0182]
其中，是输出矩阵；是视觉测量的误差协方差矩阵；δxj表示系统误差状态的预测值；表示系统状态的观测值与预测值之间的误差；h
t
表示h的转置；sj是卡尔曼增益的分母；pj表示系统误差状态的协方差的预测值；表示卡尔曼增益；表示系统误差状态的修正值；
[0183]
定义名义模型在第j帧输出的机器人位姿为：
[0184][0185]
其中，qj表示j时刻imu坐标系相对于0时刻imu坐标系的姿态的名义值；xj表示j时刻imu坐标系相对于0时刻imu坐标系的位置的名义值；
[0186]
用修正的误差状态补偿xj可得到eskf估计的位姿：
[0187]
其中，和分别是中的位移和四元数形式的旋转。
[0188]
在本实施例中，在所述模块4中，建立自适应模糊神经推理系统anfis，根据wio和vo的直接信息，推断里程计的运动状态，实时调整eskf中预测模型和测量模型的协方差矩阵，具体步骤如下：
[0189]
采用两个anfis，第一个anfis记作wio-anfis，结构如图2(a)所示，其输入的是移动机器人的角速度ω和左右车轮的转速差ρ，输出是wio的协方差矩阵q1；第二个anfis记作vo-anfis，结构如图2(b)所示，其输入的是视觉里程计的特征点个数nu和重投影误差er，输出是vo的协方差矩阵v；
[0190]
定义wio-anfis的输入为xw＝(ω，ρ)
t
，定义它的模糊集为a；为a中的每一个成员aa设计一个隶属度函数，并且记作a
mf-a
(xw)∈[0,1]；a
mf-a
(xw)越接近1，xw属于aa的可能性越大；vo-anfis的输入是特征点数量nu和重投影误差er记作xv＝(nu，er)
t
，定义它的模糊集为b；构建隶属度函数b
mf-b
(x
ν
)的方法与a
mf-a
(xw)类似；
[0191]
在设计隶属函数时考虑了模糊集合中成员之间的平滑过渡，其数学表达式和曲线形状见图4；曲线形状通过改变函数中的自适应参数进行调整；选择合适的自适应参数的原则是保证函数的灵活性和保持有限的变量数量；
[0192]
在a中为ρ定义了四个成员，它们分别趋向于+∞(a1)，趋向于1(a2)，趋向于-1(a3)，趋向于-∞(a4)；它们分别对应于移动机器人滑转、直走、旋转和滑转；a中的ω有两个成员：小(a5)和大(a6)；它们分别对应着慢转弯和快转弯的运动情况；同样，b中er的模糊集分别为小(b1)和大(b2)，nu的模糊集分别为小的(b3)和大的(b4)；
[0193]
在本实施例中，在所述模块4中，模糊集与相应系统情况的关系如表1所示：
[0194]
表1.模糊集与工作情况的关系表
[0195][0196]
隶属度函数定义了输入与其对应的模糊成员之间的关系，如图4所示；在每个隶属函数中，有一两个变量使其可调整，以更好地适应真实的系统情况；表示a和b中的自适应变量分别为aw和bv；a
w1
指定了ρ完全属于a1时初始值；a
w2
指定了ρ属于a2的终点；a
w3
指定了ρ开始属于a3的点，a
w4
指定了ρ完全属于a4时的最大值；函数的另一端固定在临界点
±
1，简化了
复杂度，降低了anfis训练中需要优化的变量的维数；a
w5
，a
w6
，a
w7
，a
w8
，指定了ω的隶属度函数的临界点；b
v1
，b
v2
，b
v3
，b
v4
指定了er的隶属度函数的临界点；b
v5
，b
v6
，b
v7
，b
v8
指定了nu的隶属度函数的临界点；曲线形状的控制方式同a
w1
，a
w2
，a
w3
，a
w4
；具体情况见图4；
[0197]
模糊规则库定义了模糊集与输出之间的逻辑关系；根据不同的输入条件，对wio和vo的测量方差进行了推理；建立的规则库以及每条规则对应的测量方差列于表2和表3中；
[0198]
表2.wio-anfis的规则库
[0199]
ωρ规则库方差a6a1π
w5
wha6a2π
w6
wha6a4π
w8
wha5a1π
w1
wha5a4π
w4
wha6a3π
w7wl
a5a2π
w2wl
a5a3π
w8
wh[0200]
表3.vo-anfis的规则库
[0201]nuer
规则库方差b2b3π
v2vl
b2b4π
v4vh
b1b3π
v1vm
b1b4π
v3vh
[0202]
对于wio-anfis，当输入组合为(a3，a6)和(a5，a2)时，方差为低，用w
l
表示；物理的解释是，当车辆正在急转弯(a3)，并且imu测量表明同样的情况(a6)，或移动机器人运动中没有大旋转(a2)并且imu测量表明同样的情况(a5)，移动机器人运动时可能没有打滑，从而wio的方差应该比较小；否则，imu的方差应该比较大，用wh表示；
[0203]
对于每一条规则，例如π
w7
(a3，a6)，它的输出定义如式子(9)所示；根据同样的规则，得到π
wa
的输出，用g
aw
表示；
[0204]g7w
＝min(a
mf-6
(xw)，a
mf-3
(xw))
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)类似地，也为vo-anfis建立了规则库；为视觉的运动估计定义了高中低三个方差，分别用vh，vm，v
l
，表示，相应的模糊规则的输出用g
bc
表示；
[0205]
根据式子(10)用g
aw
来计算wio-anfis的归一化推理结果类似地，得到vo-anfis的a表示wio-anfis中模糊规则的编号，b表示vo-anfis中模糊规则的编号，na表示w1o-anfis中的模糊规则总数；
[0206]
根据各种工况的具体测量方差vh，vm，v
l
，w
h w
l
，则wio的方差q1和vo的方差v的最终结果由式子(11)得到：
[0207][0208]
上式中，w
πwa
表示规则π
wa
对应的权重，v
πvb
表示规则π
vb
对应的权重，a表示wio-anfis中模糊规则的编号，b表示vo-anfis中模糊规则的编号。
[0209]
在本实施例中，模糊推理中隶属函数的参数也可根据经验和直觉来选择；而为了实现anfis系统的适应性，需要对里程计的测量有一个真值；它是为车辆提供无漂移定位的gps单元；利用gps测量值作为训练集的真值的难点在于gps与imu的位置不一致；这种偏差很大，特别是在车辆急转弯时，会引入训练数据的误差，导致参数优化错误；因此，将gps的直接测量转化到imu坐标系{i}中，以提供准确的训练数据；gps接收机在imu坐标系{i}中的坐标可从系统设计参数中导出，但对于训练目的来说不够精确，因此在使用它进一步训练anfis参数之前有必要对其进行优化。
[0210]
在本实施例中，在所述模块4中，建立自适应模糊神经推理系统(anfis)的训练方法，具体步骤如下：
[0211]
将gps的直接测量转化到imu坐标系{i}中，以提供准确的训练数据；gps接收机在坐标系{i0}中的位置可表示为：
[0212]
其中，ixg是gps接收机在imu坐标系{i}中的坐标，是在时间i时，坐标系{i}相对于坐标系{i0}的旋转矩阵；是时间i时gps路径点在gps测量坐标系{gm}中的表示，是坐标系{gm}和坐标系{i0}之间的旋转矩阵；
[0213]
相应地，定义相对于gps测量的估计轨迹误差为:
[0214]
其中为里程计的估计轨迹；
[0215]ixg从系统设计参数中导出，在使用它进一步训练anfis参数之前，有必要对其进行优化；对于优化后的gps相对于坐标系{i}的位置估计，可根据求式子(14)的解得到；
[0216]
其中i表示时间戳的索引，ni表示时间戳的总数，s表示训练轨迹的索引，ns表示训练轨迹的总数；
[0217]
需要优化的参数包括aw，bv和方差wh，w
l
，vh，vm，v
l
，用式(15)表示：
[0218]
x
anfis
＝[aw，bv，wh，w
l
，vh，vm，v
l
]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)已知ixg和的情况下，式子(15)中的误差用来构造如下代价优化；x
anfis
的解是模糊网络的优化后的参数集合，用来拟合训练数据集；
[0219]
其中是根据参数x
anfis
利用anfis-eskf估计的gps接收机在时间i时相对于坐标系{i0}的位置。
[0220]
本实施例方法经过训练的anfis系统能够根据地面移动机器人系统的状态，比如机器人是否打滑或者环境中的纹理信息是否足够多等，来实时调整eskf的参数，与传统的固定增益滤波器相比，大大减少了内部故障和外部干扰造成的误差，使得地面移动机器人在各种复杂工况下都能够保持较高的定位精度，鲁棒性更强。采用本实施例方法，在移动机器人上对算法进行验证，结果表明，与固定增益eskf相比，基于anfis的eskf的平均轨迹误差降低了80.6％，即使有意引入外部干扰，其平均位置漂移也在1％以下。
[0221]
本发明上述实施例基于anfis-eskf的地面移动机器人多传感器融合定位方法。anfis分别推断wio和vo的方差，并将结果用于在线自适应调整eskf的融合增益。由于模糊网络能够根据系统当前状态识别系统异常，因此估计算法能够即时调整融合参数，与传统的固定增益滤波器相比，上述实施例方法大大减少了内部故障和外部干扰造成的误差。与固定增益eskf相比，基于anfis的eskf平均轨迹误差要小很多，即使有意引入外部干扰，其平均位置漂移也能维持在很小的范围内。
[0222]
上面对本发明实施例结合附图进行了说明，但本发明不限于上述实施例，还可以根据本发明的发明创造的目的做出多种变化，凡依据本发明技术方案的精神实质和原理下做的改变、修饰、替代、组合或简化，均应为等效的置换方式，只要符合本发明的发明目的，只要不背离本发明的技术原理和发明构思，都属于本发明的保护范围。