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

本申请提供了一种室内主动消防机器人融合定位方法和系统。该方法包括：通过所述消防机器人的轮体编码器确定所述消防机器人的第一位姿；根据所述消防机器人的UWB定位坐标和惯性导航数据，确定所述消防机器人的第二位姿；基于改进自适应无迹卡尔曼滤波算法，对所述第一位姿和所述第二位姿进行融合，确定所述消防机器人的准确位姿。籍此，使消防机器人在移动过程中能够获得稳定的、高精度的位置信息，解决了消防机器人单一定位传感器的物理缺陷，满足了消防机器人远距离定位需求，提高了消防机器人定位的准确度。消防机器人定位的准确度。消防机器人定位的准确度。

【技术实现步骤摘要】
一种室内主动消防机器人融合定位方法和系统


[0001]本申请涉及消防设备
，特别涉及一种室内主动消防机器人融合定位方法和系统。

技术介绍

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

技术实现思路

[0004]本申请的目的在于提供一种室内主动消防机器人融合定位方法和系统，以解决或缓解上述现有技术中存在的问题。
[0005]为了实现上述目的，本申请提供如下技术方案：
[0006]本申请提供一种室内主动消防机器人融合定位方法，包括：通过所述消防机器人的编码器，确定所述消防机器人的第一位姿；根据所述消防机器人的UWB定位坐标和惯性导航数据，确定所述消防机器人的第二位姿；基于自适应无迹卡尔曼滤波算法，对所述第一位姿和所述第二位姿进行融合，确定所述消防机器人的准确位姿。
[0007]优选的，所述根据所述消防机器人的UWB定位坐标和惯性导航数据，确定所述消防机器人的第二位姿，包括：
[0008]根据所述消防机器人UWB定位坐标，确定所述消防机器人的绝对位置；
[0009]将所述消防机器人的绝对位置与所述惯性导航数据进行融合，确定所述消防机器人的第二位姿。
[0010]优选的，所述根据所述消防机器人UWB定位坐标，确定所述消防机器人的绝对位置，具体为：
[0011]基于所述双阈值拟合滑动窗口滤波算法，对所述消防机器人UWB定位坐标进行双阈值拟合滑动窗口滤波；
[0012]响应于所述消防机器人UWB定位坐标与上一时刻滑动窗口滤波位值的差小于等于所述双阈值拟合滑动窗口滤波算法中滑动窗口的一级预设阈值，将所述消防机器人UWB定位坐标作为所述消防机器人的绝对位置；
[0013]响应于所述消防机器人UWB定位坐标与上一时刻滑动窗口滤波位值的差大于所述一级预设阈值且小于所述双阈值拟合滑动窗口滤波算法中滑动窗口的二级预设阈值，则对所述消防机器人UWB定位坐标进行二次双阈值拟合滑动窗口滤波，得到所述消防机器人的绝对位置。
[0014]优选的，按照公式：
[0015][0016]对所述消防机器人UWB定位坐标进行双阈值拟合滑动窗口滤波；
[0017]其中，n1表示所述滑动窗口的长度，n1为自然数；表示所述消防机器人的当前时刻的UWB定位坐标；表示滑动窗口上一时刻的滤波值，为根据所述消防机器人的前一时刻的位置值与当前时刻的UWB定位置的差，通过双阈值拟合滑动窗口滤波法预测得的当前坐标值；E
d
为所述窗口预测值与真实坐标的欧式距离；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)时刻所述无迹卡尔曼滤波算法的预测值；K
k
为所述无迹卡尔曼滤波算法中的五级卡尔曼增益；z
k
表示k时刻所述消防机器人的第一位姿；为所述消防机器人的第二位姿。
[0029]本申请实施例还提供一种室内主动消防机器人融合定位系统，包括：推演位姿单元，配置为通过所述消防机器人的编码器，确定所述消防机器人的第一位姿；
[0030]第二位姿单元，配置为根据所述消防机器人的UWB定位坐标和惯性导航数据，确定所述消防机器人的第二位姿；
[0031]定位融合单元，配置为基于自适应无迹卡尔曼滤波算法，对所述第一位姿和所述第二位姿进行融合，确定所述消防机器人的准确位姿。
[0032]有益效果：
[0033]本申请提供的室内主动消防机器人融合定位技术中，首先，通过消防机器人的编码器，确定消防机器人的第一位姿；以及，根据消防机器人的UWB定位坐标和惯性导航数据，确定消防机器人的第二位姿；然后，基于自适应无迹卡尔曼滤波算法，对第一位姿和第二位姿进行融合，确定消防机器人的准确位姿。籍此，使消防机器人在移动过程中能够获得稳定的、高精度的位置信息，解决了消防机器人单一定位传感器的缺陷，满足了消防机器人远距离定位需求，提高了消防机器人定位的准确度。
附图说明
[0034]构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。其中：
[0035]图1为根据本申请的一些实施例提供的一种室内主动消防机器人融合定位方法的流程示意图；
[0036]图2为根据本申请的一些实施例提供的一种室内主动消防机器人融合定位方法的逻辑框图；
[0037]图3为根据本申请的一些实施例提供的室内主动消防机器人的差速模型示意图；
[0038]图4为根据本申请的一些实施例提供的室内主动消防机器人的航迹模型示意图；
[0039]图5为根据本申请的一些实施例提供的一种室内主动消防机器人融合本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种室内主动消防机器人融合定位方法，其特征在于，包括：通过所述消防机器人的编码器，确定所述消防机器人的第一位姿；根据所述消防机器人的UWB定位坐标和惯性导航数据，确定所述消防机器人的第二位姿；基于自适应无迹卡尔曼滤波算法，对所述第一位姿和所述第二位姿进行融合，确定所述消防机器人的准确位姿。2.根据权利要求1所述的室内主动消防机器人融合定位方法，其特征在于，所述根据所述消防机器人的UWB定位坐标和惯性导航数据，确定所述消防机器人的第二位姿，包括：根据所述消防机器人UWB定位坐标，确定所述消防机器人的绝对位置；将所述消防机器人的绝对位置与所述惯性导航数据进行融合，确定所述消防机器人的第二位姿。3.根据权利要求2所述的室内主动消防机器人融合定位方法，其特征在于，所述根据所述消防机器人UWB定位坐标，确定所述消防机器人的绝对位置，具体为：基于所述双阈值拟合滑动窗口滤波算法，对所述消防机器人UWB定位坐标进行双阈值拟合滑动窗口滤波；响应于所述消防机器人UWB定位坐标与上一时刻滑动窗口滤波位值的差小于等于所述双阈值拟合滑动窗口滤波算法中滑动窗口的一级预设阈值，将所述消防机器人UWB定位坐标作为所述消防机器人的绝对位置；响应于所述消防机器人UWB定位坐标与上一时刻滑动窗口滤波位值的差大于所述一级预设阈值且小于所述双阈值拟合滑动窗口滤波算法中滑动窗口的二级预设阈值，则对所述消防机器人UWB定位坐标进行二次双阈值拟合滑动窗口滤波，得到所述消防机器人的绝对位置。4.根据权利要求3所述的室内主动消防机器人融合定位方法，其特征在于，按照公式：对所述消防机器人UWB定位坐标进行双阈值拟合滑动窗口滤波；其中，n1表示所述滑动窗口的长度，n1为自然数；表示所述消防机器人的当前时刻的UWB定位坐标；表示滑动窗口上一时刻的滤波值，为根据所述消防机器人的前一时刻的位置值与当前时刻UWB定位置的差，通过双阈值拟合滑动窗口滤波法预测得到的当前坐标值；E
d
为所述窗口预测值与所述UWB定位坐标的欧式距离；L1为所述一级预设阈值；L2为所述二级预设阈值。5.根据权利要求3所述的室内主动消防机器人融合定位方法，其特征在于，还...

【专利技术属性】
技术研发人员：刘永涛，李秋瑜，杜虎彬，刘佳，
申请(专利权)人：华北科技学院，
类型：发明
国别省市：