一种室内主动消防机器人融合定位方法和系统

1.本技术涉及消防设备技术领域，特别涉及一种室内主动消防机器人融合定位方法和系统。


背景技术：

2.随着社会经济的迅猛发展，建筑和企业生成的特殊性，导致化学危险品和放射性物质泄露以及燃烧、爆炸、坍塌的事故隐患增加，事故发生的概率也相应提高。一旦发生火灾事故，消防员面对高温、黑暗、有毒和浓烟等危害环境时，若没有相应的设备贸然冲进现场，不仅不能完成任务，还会徒增人员伤亡。
3.而消防机器人作为特种机器人的一种，在灭火救援中具有举足轻重的作用，随着消防机器人技术也得到快速的发展，消防机器人逐渐在灭火救灾领域得到广泛的应用，能够代替消防救援人员进入易燃易爆、有毒、缺氧、浓烟等危险灾害事故现场进行数据采集、处理、反馈，有效地解决消防人员在危险灾害事故现场的人身安全、数据信息采集不足等问题，而且现场指挥人员还可以根据其反馈结果，及时对灾情做出科学判断，并对灾害事故现场工作做出正确、合理的决策。因而，亟需提供一种针对上述现有技术不足的技术方案


技术实现要素：

4.本技术的目的在于提供一种室内主动消防机器人融合定位方法和系统，以解决或缓解上述现有技术中存在的问题。
5.为了实现上述目的，本技术提供如下技术方案：
6.本技术提供一种室内主动消防机器人融合定位方法，包括：通过所述消防机器人的编码器，确定所述消防机器人的第一位姿；根据所述消防机器人的uwb定位坐标和惯性导航数据，确定所述消防机器人的第二位姿；基于自适应无迹卡尔曼滤波算法，对所述第一位姿和所述第二位姿进行融合，确定所述消防机器人的准确位姿。
7.优选的，所述根据所述消防机器人的uwb定位坐标和惯性导航数据，确定所述消防机器人的第二位姿，包括：
8.根据所述消防机器人uwb定位坐标，确定所述消防机器人的绝对位置；
9.将所述消防机器人的绝对位置与所述惯性导航数据进行融合，确定所述消防机器人的第二位姿。
10.优选的，所述根据所述消防机器人uwb定位坐标，确定所述消防机器人的绝对位置，具体为：
11.基于所述双阈值拟合滑动窗口滤波算法，对所述消防机器人uwb定位坐标进行双阈值拟合滑动窗口滤波；
12.响应于所述消防机器人uwb定位坐标与上一时刻滑动窗口滤波位值的差小于等于所述双阈值拟合滑动窗口滤波算法中滑动窗口的一级预设阈值，将所述消防机器人uwb定位坐标作为所述消防机器人的绝对位置；
13.响应于所述消防机器人uwb定位坐标与上一时刻滑动窗口滤波位值的差大于所述一级预设阈值且小于所述双阈值拟合滑动窗口滤波算法中滑动窗口的二级预设阈值，则对所述消防机器人uwb定位坐标进行二次双阈值拟合滑动窗口滤波，得到所述消防机器人的绝对位置。
14.优选的，按照公式：
[0015][0016]
对所述消防机器人uwb定位坐标进行双阈值拟合滑动窗口滤波；
[0017]
其中，n1表示所述滑动窗口的长度，n1为自然数；表示所述消防机器人的当前时刻的uwb定位坐标；表示滑动窗口上一时刻的滤波值，为根据所述消防机器人的前一时刻的位置值与当前时刻的uwb定位置的差，通过双阈值拟合滑动窗口滤波法预测得的当前坐标值；ed为所述窗口预测值与真实坐标的欧式距离；l1为所述一级预设阈值；l2为所述二级预设阈值。
[0018]
优选的，还包括：响应于所述消防机器人uwb定位坐标与上一时刻滑动窗口滤波位值的差大于所述二级预设阈值，基于改进的chan算法，根据小于等于所述一级预设阈值的所述消防机器人uwb定位坐标，得到所述消防机器人的绝对位置。
[0019]
优选的，按照：
[0020][0021]
确定所述消防机器人t时刻的绝对位置(x
t
，y
t
)；
[0022]
其中，n2表示地理坐标系；表示所述消防机器人在地理坐标系下的实时速度；为所述消防机器人的四元数的姿态矩阵；a
bs
表示所述消防机器人在地理坐标系下的南向加速度；g表示当前坐标系的重力加速度；表示地球的向心加速度；
[0023]
表示所述消防机器人在地理坐标下的初始速度；a
be
表示所述消防机器人在地理坐标系下的东向加速度；(x0，y0)表示所述消防机器人的起始绝对坐标。
[0024]
优选的，在所述基于自适应无迹卡尔曼滤波算法，对所述第一位姿和所述第二位姿进行融合中，包括：将所述消防机器人的第一位姿与所述消防机器人的第二位姿的差值作为跟随矫正系数，对所述无迹卡尔曼滤波算法的预测值的误差进行调节。
[0025]
优选的，按照：
[0026][0027]
对k时刻所述无迹卡尔曼滤波算法的预测值的误差进行调节；
[0028]
其中，k为自然数，为(k-1)时刻所述无迹卡尔曼滤波算法的预测值；kk为所述无迹卡尔曼滤波算法中的五级卡尔曼增益；zk表示k时刻所述消防机器人的第一位姿；为所述消防机器人的第二位姿。
[0029]
本技术实施例还提供一种室内主动消防机器人融合定位系统，包括：推演位姿单元，配置为通过所述消防机器人的编码器，确定所述消防机器人的第一位姿；
[0030]
第二位姿单元，配置为根据所述消防机器人的uwb定位坐标和惯性导航数据，确定所述消防机器人的第二位姿；
[0031]
定位融合单元，配置为基于自适应无迹卡尔曼滤波算法，对所述第一位姿和所述第二位姿进行融合，确定所述消防机器人的准确位姿。
[0032]
有益效果：
[0033]
本技术提供的室内主动消防机器人融合定位技术中，首先，通过消防机器人的编码器，确定消防机器人的第一位姿；以及，根据消防机器人的uwb定位坐标和惯性导航数据，确定消防机器人的第二位姿；然后，基于自适应无迹卡尔曼滤波算法，对第一位姿和第二位姿进行融合，确定消防机器人的准确位姿。籍此，使消防机器人在移动过程中能够获得稳定的、高精度的位置信息，解决了消防机器人单一定位传感器的缺陷，满足了消防机器人远距离定位需求，提高了消防机器人定位的准确度。
附图说明
[0034]
构成本技术的一部分的说明书附图用来提供对本技术的进一步理解，本技术的示意性实施例及其说明用于解释本技术，并不构成对本技术的不当限定。其中：
[0035]
图1为根据本技术的一些实施例提供的一种室内主动消防机器人融合定位方法的流程示意图；
[0036]
图2为根据本技术的一些实施例提供的一种室内主动消防机器人融合定位方法的逻辑框图；
[0037]
图3为根据本技术的一些实施例提供的室内主动消防机器人的差速模型示意图；
[0038]
图4为根据本技术的一些实施例提供的室内主动消防机器人的航迹模型示意图；
[0039]
图5为根据本技术的一些实施例提供的一种室内主动消防机器人融合定位系统的结构示意图。
具体实施方式
[0040]
下面将参考附图并结合实施例来详细说明本技术。各个示例通过本技术的解释的方式提供而非限制本技术。实际上，本领域的技术人员将清楚，在不脱离本技术的范围或精神的情况下，可在本技术中进行修改和变型。例如，示为或描述为一个实施例的一部分的特征可用于另一个实施例，以产生又一个实施例。因此，所期望的是，本技术包含归入所附权利要求及其等同物的范围内的此类修改和变型。
[0041]
在复杂场景中，机器人依靠单一定位方法很难得到准确定位信息，多定位技术融合是把场景中不同类型传感器得到的定位信息加以优化融合，实现各定位方法的取长补短，从而提高机器人系统定位的准确度。
[0042]
目前，消防机器人普遍采用遥操作与局部自主的方式，对消防机器人进行控制，这种方式需要操作人员根据红外摄像头传回的图像对机器人的运动状态和所处的环境进行判断，在火灾现场外调整机器人的偏航角和俯仰角，从而实施灭火措施。这种依靠单一传感器的定位方法，极大的影响的机器人的定位精度，且由于依靠远距离人员操作，不可避免的会出现定位滞后，难以满足复杂的室内结构空间的消防需求。
[0043]
室内自主消防机器人功能的实现依赖于准确的定位导航技术，而现有的室内自主消防机器人主要通过编码器、imu(inertial measurement unit)惯导定位、超宽带(uwb)技术实现消防机器人的定位。
[0044]
但是，申请人研究发现，在消防机器人的定位过程中，单独通过自身编码器进行航迹推演的航迹推演定位技术受编码器分辨率的影响很大，通过其实现定位和航向角计算在短距离、短时间内会比较准确。但是消防机器人的行进距离是不可控的，一般会达到几十米甚至数百米。这必然造成编码器的误差随时间增加而不断累积。同时因为机器人行走场地的摩擦力、机械结构误差等原因，仅通过该技术无法推演出消防机器人的准确位置和航向角姿态。因此传统航迹推演算法并不适合远距离、长航时的工况。
[0045]
imu(inertial measurement unit)惯导定位技术是经过对加速度的二次积分获得位置信息，通过对角速度的积分获得机器人航向角信息。由于定位采用了双重积分，因此定位误差必然随时间延长而增大，该方法长期导航时精度较差。常规的惯性导航更新算法是以理想状态下无误差为假定工况，此时在计算过程中只会引入微小的数值误差作为模拟。但是，在实际使用过程中，惯导传感器不可避免地会存在各种误差和噪声，并且导航参数(姿态、速度和位置)的初始值设置也很难完全准确，这些误差都会通过导航更新迭代计算进行传导，使后续的定位误差不断增大。惯性导航由于累计误差的存在并且没有自我纠正的方法，因此不能将其单独应用于定位当中。
[0046]
超宽带(uwb)虽然具有许多优点，包括出色的穿透力和高精度测距。并且由于采用绝对定位的方式，使得该技术不存在累积误差。uwb在视距情况下，依靠极高的通信速率和带宽可以获得精度很高的定位解算结果。但在室内复杂环境中存在遮挡、多径、散射、衍射等因素，因此无线定位容易受到nlos(non-line of sight)环境影响,使得uwb信号得到的测距值出现较大波动和随机误差，导致定位精度大大降低。
[0047]
基于此，申请人提出了一种室内主动消防机器人融合定位技术，通过消防机器人上的编码器获得消防机器人的第一位姿，并根据防机器人的uwb定位坐标和惯性导航数据获得消防机器人的第二位姿，最后，基于自适应无迹卡尔曼滤波算法，对第一位姿和第二位姿进行融合，得到符合机器人快速移动所需的定位精度信息。整个定位过程不需要人员操作，完全由消防机器人自主实现，并且将编码器、uwb和惯性导航定位进行了额有机融合，从而得到机器人自主移动所需要的坐标信息和航向角信息。解决了消防机器人单一定位传感器的缺陷，有效消除了消防机器人定位时的累积误差和顺序误差，满足了消防机器人远距离定位需求，使消防机器人在移动过程中能够获得稳定的、高精度的位置信息，提高了消防机器人定位的准确度。
[0048]
首先，本技术中，定义消防机器人地理坐标系下，y轴对应地理坐标系的东向，x轴对应地理坐标系的南向；消防机器人的南向加速度即为地理坐标系下x轴方向的加速度；消防机器人的东向加速度即为地理坐标系下y轴方向的加速度。
[0049]
如图1、图2所示，该室内主动消防机器人融合定位方法包括：
[0050]
步骤s101、通过消防机器人的编码器，确定消防机器人的第一位姿。
[0051]
本技术中，利用机器人在室内结构空间中运动时，自身编码器输出的值，根据航迹推演模型确定消防机器人在室内结构空间中的相对坐标和航向角。具体的，在t时刻由机器人的陀螺仪输出的位姿信息(第一位姿，即位置1)的状态为：
[0052]
(u(t)
t
＝(xe、ye、θe))
[0053]
其中，(xe、ye)为消防机器人在室内结构空间中的相对坐标，θe为消防机器人在室内结构空间中的航向角。
[0054]
本技术中，消防机器人的移动底盘采用两轮差速自平衡控制，由双pid控制实现底盘的动态自平衡。在行驶过程中，通过驱动器控制两个主动轮以不同的速度运转，当两个主动轮同向同速转动时刻控制消防机器人直线运动，当两个主动轮以不同的速度运转时刻实现消防机器人的转动控制。
[0055]
本技术中，消防机器人的空间模型如图3所示，在平面空间中构建以x轴(南向)为横坐标，以y轴(东向)为纵坐标，以o为原点的坐标系。原点处的速度为零，根据消防机器人自平衡双差速轮设计原理分析可得消防机器人中间c点处的速度vc为两轮速度的平均值，即：
[0056][0057]
其中，vr、v
l
分别为消防机器人的左、右轮的速度。
[0058]
消防机器人在图3中沿着当前方向顺时针转动，可知：
[0059][0060]
其中，v
x
、vy分别表示防机器人中间c点速度在x轴、y轴上的分量。
[0061]
本技术中，消防机器人的左、右轮的速度，以及消防机器人两轮之间的间距(l)均可以由消防机器人获取。因而，消防机器人的角速度为：
[0062][0063]
有消防机器人的差速运动模型为：
[0064]
xk＝f(x
k-1
,uk)
[0065]
其中，f(x
k-1
,uk)表示消防机器人的状态转移方程。
[0066]
进而，由图4消防机器人底盘航迹模型，可得消防机器人t时刻的空间模型(第一位姿)为：
[0067][0068]
其中，(xc，yc)为消防机器人的航迹推演坐标，αc为消防机器人的航迹推演航向角，即(xe、ye、θe)＝(xc、yc、αc)。
[0069]
步骤s102、根据消防机器人的uwb定位坐标和惯性导航数据，确定消防机器人的第二位姿。
[0070]
本技术中，通过消防机器人的uwb定位传感器获取消防机器人的uwb定位坐标，通过机器人的惯性导航系统获取的惯性导航数据；并将uwb定位坐标和惯性导航数据进行融合，确定消防机器人的第二位姿，即确定消防机器人的绝对位置和航向角。具体的，根据消防机器人的uwb定位坐标，确定消防机器人的绝对位置；将消防机器人的绝对位置与惯性导航数据进行融合，确定消防机器人的第二位姿。
[0071]
本技术中，消防机器人在复杂的室内结构空间中运动，由于受到外界干扰及噪声的多样性、传感器本身的测量误差等因素的影响，在非视觉情况下,uwb定位坐标中包含大量的测量噪声，且uwb定位坐标存在较强的非线性特质。因而，采用双阈值拟合滑动窗口滤波算法对uwb定位坐标进行优化，降低噪声对消防机器人定位精度的影响。
[0072]
在利用滑动窗口滤波过程中，消防机器人的uwb定位坐标存在巨大抖动，如果采用阈值判定直接丢弃，则会造成大量的空白定位信息，因此，本技术中采用双阈值拟合滑动窗口滤波对噪声数据进行处理，结合消防机器人的最大移动速度和uwb数据更新速率，设定滑动窗口的一级阈值和二级阈值。一级阈值之内的定位信息直接进入滑动窗口处理，一级阈值和二级阈值之间的数据在拟合后进入滑动窗口。
[0073]
具体的，基于双阈值拟合滑动窗口滤波算法，对消防机器人uwb定位坐标进行双阈值拟合滑动窗口滤波；响应于消防机器人uwb定位坐标与上一时刻滑动窗口滤波位值的差小于等于双阈值拟合滑动窗口滤波算法中滑动窗口的一级预设阈值，将消防机器人uwb定位坐标作为消防机器人的绝对位置；响应于消防机器人uwb定位坐标与上一时刻滑动窗口滤波位值的差大于一级预设阈值且小于双阈值拟合滑动窗口滤波算法中滑动窗口的二级预设阈值，则对消防机器人uwb定位坐标进行二次双阈值拟合滑动窗口滤波，得到消防机器人的绝对位置。
[0074]
本技术中，按照公式：
[0075][0076]
对消防机器人uwb定位坐标进行双阈值拟合滑动窗口滤波；
[0077]
其中，n1表示滑动窗口的长度，n1为自然数；表示消防机器人的当前时刻的uwb定位坐标；表示滑动窗口上一时刻的滤波值，为根据消防机器人的前一时刻的位置
值与当前时刻的uwb定位置的差，通过双阈值拟合滑动窗口滤波法预测得到当前坐标值；ed为窗口预测值与uwb定位置的欧式距离；l1为一级预设阈值；l2为二级预设阈值。
[0078]
其中，一级预设阈值l1由第n时刻与第n-1时刻的第一位姿的差得到；二级预设阈值根据消防机器人的最高速度与uwb数据更新速率(uwb传感器采集uwb定位坐标的采集周期)得到。在此，需要说明的时，消防机器人的最高速度为汇总性能参数得到的消防机器人的最快速度，uwb数据更新速率即为uwb传感器1秒钟采集多少次数据，比如，机器人的最高速度为2m/s,数据采集频率为20hz,则uwb传感器本次采样与上次采样间隔，消防机器人最远移动距离为0.1米(即2m/s/20hz)，那么二级预设阈值即为0.1。
[0079]
对于大于二级预设阈值的uwb定位坐标，直接丢弃，会造成大量的空白定位信息，本技术中，响应于消防机器人uwb定位坐标与上一时刻滑动窗口滤波位值的差大于二级预设阈值，基于改进的chan算法，根据小于等于一级预设阈值的消防机器人uwb定位坐标，得到消防机器人的绝对位置。也就是说，采用改进的chan算法，根据“爆窗”(uwb定位坐标与上一时刻滑动窗口滤波位值的差小于等于滑动窗口的一级预设阈值)前的uwb定位坐标，结合机器人的自平衡(加速度计、陀螺仪)，对消防机器人的速度、加速度进行拟合，确定消防机器人的绝对位置。
[0080]
具体的，由欧拉公式可以得到消防机器人的陀螺仪的旋转矩阵：
[0081][0082]
其中，r
x
、ry、rz分别表示绕坐标系的x、y、z轴的旋转矩阵，θ
x
、θy、θz分别表示消防机器人的航向角绕坐标系的x、y、z轴的分量。
[0083]
在陀螺仪姿态中，沿z轴旋转，可得消防机器人的姿态矩阵：
[0084][0085][0086]
其中，β，α，γ分别表示消防机器人的姿态角，具体的，α表示消防机器人的俯仰角，
β表示消防机器人的翻滚角，γ表示消防机器人的航向角；n2表示地理坐标系。
[0087]
根据四元数的空间旋转的姿态转换原理，消防机器人的陀螺仪的四元数的姿态矩阵方程为：
[0088][0089]
式中，q0、q1、q2、q3分别表示坐标旋转的四元数；b、n2分别表示载体坐标系(陀螺仪)和地理坐标系。
[0090]
按照z-y-x的顺序旋转至最终姿态，消防机器人的最终姿态矩阵，如下所示：
[0091]
m(β，α，γ)＝rz(β)ry(α)r
x
(γ)
ꢀꢀ
(2-10)
[0092]
由旋转矩阵可得消防机器人的姿态角：
[0093][0094]
其中，α为消防机器人的俯仰角，用于自平衡消防机器人的底盘平衡控制，γ为消防机器人的航向角，用于消防机器人的运动控制。
[0095]
在通过陀螺仪获取消防机器人的位置时，在消防机器人的地理坐标系中y轴对应地球坐标系的东向，x轴对应地球坐标系的南向。在t时刻载体在机器人坐标系(地理坐标系)得到的加速度为转换到地理坐标系后表达式为：
[0096][0097]
其中，为t时刻消防机器人载体坐标系下的加速度。
[0098]
在机器人载体坐标系下加速度映射到地理坐标系时含有重力加速度分量，该分量作为误差引入模型，可以得出地理坐标系中消防机器人的速度方程，如下：
[0099][0100]
其中，n2表示地理坐标系，为消防机器人在地理坐标系下的加速度，g为当行坐标系的重力加速度，为地球的向心加速度。则消防机器人在t时刻的速度表示为：
[0101][0102]
进而，可得消防机器人在x轴(地球坐标系的南向)、y轴(地球坐标系的东向)的实时坐标(第二位姿)为：
[0103][0104]
其中，其中，n2表示地理坐标系；表示所述消防机器人在地理坐标系下的实时速度；为所述消防机器人的四元数的姿态矩阵；a
bs
表示所述消防机器人在地理坐标系下的南向加速度；g表示当前坐标系的重力加速度；表示地球的向心加速度；
[0105]
表示所述消防机器人在地理坐标系下的初始速度；a
be
表示所述消防机器人在地理坐标系下的东向加速度；(x0，y0)表示所述消防机器人的起始绝对坐标。
[0106]
步骤s103、基于自适应无迹卡尔曼滤波算法，对第一位姿和第二位姿进行融合，确定消防机器人移动所需的定位信息。
[0107]
本技术中，由于自适应无迹卡尔曼滤波算法(aukf)融合前的定位数据为非线性，在实际环境下，可以将高斯分布的数据近似补偿为满足高斯分布。由现有数据可知，通过编码器获得的第一位姿(位置1)的信息包含了累积误差；通过惯性导航系统和陀螺仪设备获取的第二位姿(位置2)出的信息虽然累积误差不大，但是包含了一定的瞬时误差。第一位姿(位置1)的数据的误差近似服从高斯分布，因此第一位姿(位置1)和第二位姿(位置2)的数据都可以通过aukf算法进行融合优化，从而减小累积误差和瞬时误差。最终通过滤波可以得到更为精确到的估计位置，实现消防机器人在室内结构空间的高精度定位。
[0108]
现有的消防机器人定位融合方法只关注了坐标信息，并未对航向角进行准确融合，因而，造成机器人无法真正意义上的自主移动。本技术中，在基于自适应无迹卡尔曼滤波算法对第一位姿和第二位姿进行融合时，首先，在t时刻由陀螺仪输出的位姿信息(第一位姿，即位置1)的状态(u(t)
t
＝(xe、ye、θe))为预测量(预测值)，消防机器人的当前位置信息与上一时刻位姿信息的差值为δu(t)。
[0109]
在t时刻由uwb定位传感器和imu(惯性导航)传感器确定的位姿信息(第二位姿，即位置2)的状态为观测量(观测值)。其中，在t时刻由uwb定位传感器和imu(惯性导航)传感器确定的位姿信息(第二位姿，即位置2)的状态为：
[0110]
z(t)
t
＝(xu、yu、θu)
[0111]
式中，(xu、yu)为由uwb定位传感器和imu(惯性导航)传感器确定的消防机器人的绝对位置，θu为由uwb定位传感器和imu(惯性导航)传感器确定的消防机器人的航向角。
[0112]
本技术中，定义第一位姿(即位置1)的误差为w(t)，该误差的噪声方差为q；第二位姿(即位置2)的误差为v(t)，该误差的噪声方差为r；消防机器人在每个n
×
δt时间内的融合位置信息x(t)的预测误差为p；定义kk为滤波器的增益矩阵。其中，u(t)、z(t)、w(t)、v(t)、q、r、p均为三维矩阵。
[0113]
基于aukf算法，将作为预测量(预测估计值)的第一位姿，作为观测量(观测估计值)的第二位姿进行融合，具体步骤如下：
[0114]
首先，令：
[0115][0116][0117]
其中，为消防机器人的位置1的初始状态x0的预期均值；p0为位置1的初始状态估计误差的预期协方差矩阵。
[0118]
然后，计算k时刻的sigma点：
[0119][0120]
式中，sigma点集x
k-1
的分段表达式定位为：
[0121][0122]
其中，λ＝α2(n+κ)-n，为复合缩放参数，n为状态向量的维数，本技术中，n＝3；α为控制sigma点扩散的正参数，α＝1；κ为次要缩放参数，κ＝0。
[0123]
接着，对消防机器人的的位姿信息(第一位姿，即位置1)进行预测，有：
[0124]
x
k∣k-1
＝f(x
k-1
)
[0125][0126][0127]
式中，x
k∣k-1
为状态传递函数在k时刻的线性状态向量(预测估计值)，为平均权重，为协方差权重；它们的分段表达式定义如下：
[0128][0129]
其中，α为控制sigma点扩散的正参数，β是一个非负参数，用于合并部分先验分布知识，本技术中，β＝2。
[0130]
最后，对消防机器人的位置状态进行更新修正，具体的，
[0131]zk∣k-1
＝h(x
k∣k-1
)
[0132][0133]
式中，z
k∣k-1
为消防机器人在k时刻的由uwb定位传感器和imu(惯性导航)传感器确定观测估计值。
[0134]
本技术中，在基于自适应无迹卡尔曼滤波算法对第一位姿和第二位姿进行融合时，将消防机器人的第一位姿(观测估计均值)与消防机器人的第二位姿(预测估计均值)的差值作为跟随矫正系数(roth)对无迹卡尔曼滤波算法的预测值得误差进行调节。具体的，如下公式所示：
[0135][0136][0137]
其中，p
zz,k∣k-1
是预期的测量误差协方差矩阵，p
xz,k∣k-1
是预期的测量误差互协方差矩阵。
[0138]
传统的aukf的sage-husa噪声滤波器在更新随机变量系统(陀螺仪)和观测变量系统(uwb定位传感器和imu(惯性导航)传感器)白噪声协方差时，当二者中非零元素的绝对值大于某特定阈值，或子协方差元素出现负值时，容易导致随机变量系统白噪声协方差失去非负性，或者，观测变量系统白噪声协方差失去正定型，从而使得aukf滤波发散，精度降低。为此，在本技术中，将消防机器人的观测估计均值(zpred)与消防机器人的预测估计均值(xpred)的差值作为跟随矫正系数(roth)引入到测量误差协方差矩阵中，以防止出现白噪声协方差无法正定问题，且提高了算法拟合的准确度。其中，
[0139]
roth＝(zpred-xpred)r
[0140]
式中，zpred和xpred观测估计均值与预测估计均值。
[0141]
本技术中，按照：
[0142][0143]
对k时刻无迹卡尔曼滤波算法的预测值的误差进行调节；
[0144]
其中，k为自然数，为(k-1)时刻无迹卡尔曼滤波算法的预测值；kk为无迹卡尔曼滤波算法中的五级卡尔曼增益；zk表示k时刻消防机器人的第一位姿；为消防机器人的第二位姿。
[0145]
在此，将消防机器人的观测估计均值与消防机器人的预测估计均值的差值作为跟随矫正系数，通过该参数的变化影响无迹卡尔曼增益kk变化，从而达到消防机器人定位过程更新的自适应。具体如下：
[0146][0147][0148][0149]
可见，当在(k-1)时刻存在随机误差或者机器人异常抖动时，状态向量估计(预测估计值)也会出现相应误差，此时，通过协方差矫正因子，即跟随矫正系数(roth)，对误差进行调节。
[0150]
式中，p
k-1
为一步预测估计偏差协方差矩阵，p
k∣k-1
为量测修正估计偏差协方差矩阵。p的初值设定为[1,1,1]，满足非零即可。噪声方差q和r的初值分别为：q＝[0.001,1,1]；r＝[200,200,1].
[0151]
本技术提供的室内主动消防机器人融合定位方法中，第一位姿、第二位姿均为实时位姿，第一位姿通过消防机器人的轮体编码器的航迹推演得到；第二位姿通过消防机器人的uwb定位传感器和惯性导航系统得到；基于改进的自适应无迹卡尔曼滤波算法，对第一位姿和第二位姿进行融合，确定消防机器人移动所需的定位信息。籍此，解决了消防机器人单一定位传感器的缺陷，有效消除了消防机器人定位时的累积误差和顺序误差，使消防机器人在移动过程中能够获得稳定的、高精度的位置信息，满足了消防机器人远距离定位需求，提高了消防机器人定位的准确度。
[0152]
本技术实施例还提供一种室内主动消防机器人融合定位系统，如图5所示，该室内主动消防机器人融合定位系统包括：
[0153]
推演位姿单元501，配置为通过消防机器人的编码器，确定消防机器人的第一位姿；第二位姿单元502，配置为根据消防机器人的uwb定位坐标和惯性导航数据，确定消防机器人的第二位姿；定位融合单元503，配置为基于自适应无迹卡尔曼滤波算法，对第一位姿和第二位姿进行融合，确定消防机器人的准确位姿。
[0154]
本技术实施例提供的室内主动消防机器人融合定位系统能够实现上述任一室内主动消防机器人融合定位方法实施例的步骤、流程，并达到相同的技术效果，在此不在一一赘述。
[0155]
在本技术的室内主动消防机器人融合定位系统中，从消防机器人上安装的编码器和uwb+imu系统分别获得消防机器人的第一位姿(位置1)和第二位姿(位置2)，并基于无迹卡尔曼滤波算法进行融合，得到消防机器人的融合位置信息。进一步的，将位置信息1(陀螺仪确定的第一位姿)、位置信息2(uwb+imu系统确定的第二位姿)、融合位置信息以及实际位置信息(通过场地实际位置测量和角度测量得到)进行比价，可以得到该消防机器人融合定位系统的定位精准度ε。具体的，如下公式所示：
[0156][0157]
其中，h为融合位置信息；l
′
为实际位置信息。
[0158]
对于现有技术中单一传感器定位技术由于器自身原理缺陷难以满足机器人定位需求，本技术提出了基于自适应无迹卡尔曼滤波算法，结合消防机器人自身的编码器，以及uwb定位传感器和惯性导航系统，获取消防机器人的融合定位信息，提升消防机器人的定位
精准度，有效提高了消防机器人的抗干扰能力，满足了消防机器人在自主移动过程中的定位和位姿感知需求。
[0159]
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及方法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和涉及约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本技术实施例的范围。
[0160]
需要说明的是，本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其它实施例的不同之处。尤其，对于设备及系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。
[0161]
以上所描述得设备及系统实施例仅仅是示意性的，其中作为分离不见说明的单元可以使或者也可以不是物理上分开的，作为单元提示的不见可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
[0162]
以上所述仅为本技术的优选实施例，并不用于限制本技术，对于本领域的技术人员来说，本技术可以有各种更改和变化。凡在本技术的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本技术的保护范围之内。