CN116625360B - 一种基于强跟踪模型的扫地机打滑识别和修正方法 - Google Patents

一种基于强跟踪模型的扫地机打滑识别和修正方法 Download PDF

Info

Publication number
CN116625360B
CN116625360B CN202310882640.9A CN202310882640A CN116625360B CN 116625360 B CN116625360 B CN 116625360B CN 202310882640 A CN202310882640 A CN 202310882640A CN 116625360 B CN116625360 B CN 116625360B
Authority
CN
China
Prior art keywords
matrix
error
value
speed
covariance matrix
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
CN202310882640.9A
Other languages
English (en)
Other versions
CN116625360A (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.)
Suzhou Jingyuanchuang Intelligent Technology Co ltd
Original Assignee
Suzhou Jingyuanchuang Intelligent Technology Co ltd
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 Suzhou Jingyuanchuang Intelligent Technology Co ltd filed Critical Suzhou Jingyuanchuang Intelligent Technology Co ltd
Priority to CN202310882640.9A priority Critical patent/CN116625360B/zh
Publication of CN116625360A publication Critical patent/CN116625360A/zh
Application granted granted Critical
Publication of CN116625360B publication Critical patent/CN116625360B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
    • A47L11/24—Floor-sweeping machines, motor-driven
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
    • A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
    • A47L11/4061—Steering means; Means for avoiding obstacles; Details related to the place where the driver is accommodated
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L2201/00—Robotic cleaning machines, i.e. with automatic control of the travelling movement or the cleaning operation
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L2201/00—Robotic cleaning machines, i.e. with automatic control of the travelling movement or the cleaning operation
    • A47L2201/04—Automatic control of the travelling movement; Automatic obstacle detection

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明提供一种基于强跟踪模型的扫地机打滑识别和修正方法，包括S1、基于18维松组合扩展卡尔曼滤波模型得到一步预测值；通过扫地机内捷联惯性导航系统的速度与里程计提供的速度进行比较，得到两者的差值作为量测量值；S2、将一步预测值、量测量值录入卡尔曼滤波器，进行差值计算，得到新息值；S3、基于新息值计算一步预测协方差矩阵；并根据新息值得到新息参与运算的统计量，并将新息参与运算的统计量与异常阈值作比较，判断扫地机是否打滑；本发明基于传感器误差模型的SINS/OD松组合导航方法，利用强跟踪卡尔曼滤波模型，对新息序列进行野值判别与修正。

Description

一种基于强跟踪模型的扫地机打滑识别和修正方法
技术领域
本发明涉及扫地机器人技术领域，具体为一种基于强跟踪模型的扫地机打滑识别和修正方法。
背景技术
扫地机器人经常会出现打滑问题，无论是纯惯导扫地机还是激光雷达、视觉导航扫地机，打滑的出现都将影响扫地机的导航精度与控制结果。打滑信息将影响两个方面，一方面是激光、视觉导航扫地机需要打滑识别信号作为机器脱困、加减速等策略安排的信息；另一方面是扫地机（尤其是纯惯导扫地机）需要在打滑后对打滑所导致的导航信息错误进行修正。
打滑是由里程计与IMU（6轴传感器）来组合判断的，里程计与IMU可进行组合，它们的组合方式有两种，一种是DR（航位推算）导航，这是扫地机最常用的组合方式；另一种是基于传感器误差分析的SINS/OD组合导航（SINS，捷联惯导系统；OD，里程计），一般采用卡尔曼滤波将里程计与SINS进行松组合。
传统DR算法导航精度有限，其导航精度劣于SINS/OD组合导航方式，DR导航只能简单判别较为简单和连续的打滑，其判别条件有以下：1、扫地机两行进轮的里程信息换算后得到的速度、位置增量信息与SINS所计算得到的速度、位置信息求差，若差值很大，说明某一段时间内发生了打滑；2、扫地机两行进轮的差速可以推算出航向偏移角度，与SINS提供的航向角进行比较，若一小段时间内的差值过大，则表明发生打滑。对以上两点进行分析，条件1所说的打滑只能在一定情况下才能被判别，若发生了打滑而二者差值并没有很大，就无法被判定；条件2所说的打滑判定，只能在单轮打滑时才有效，若双轮打滑或单轮打滑并不明显则无法被判别。
发明内容
针对现有技术存在的不足，本发明目的是提供一种基于强跟踪模型的扫地机打滑识别和修正方法，可以解决现有扫地机打滑无法明显判断的问题。
为了实现上述目的，本发明是技术方案如下：
本发明是通过如下的技术方案来实现：一种基于强跟踪模型的扫地机打滑识别和修正方法，包括以下步骤：
S1、基于18维松组合扩展卡尔曼滤波模型得到一步预测值；通过扫地机内捷联惯性导航系统的速度与里程计提供的速度进行比较，得到两者的差值作为量测量值；
S2、将一步预测值、量测量值/>录入卡尔曼滤波器，求解得到新息值/>；
S3、基于新息值计算一步预测协方差矩阵/>；并根据新息值/>得到新息参与运算的统计量/>，并将新息参与运算的统计量/>与异常阈值/>作比较；
若新息参与运算的统计量大于异常阈值/>，则量测量值/>为野值；则扫地机打滑，此时修正因子/>；
若新息参与运算的统计量不大于异常阈值/>，则量测量值/>非野值；则扫地机不打滑，此时修正因子/>。
进一步的，还包括以下步骤：
S4、基于新息值计算量测噪声误差协方差矩阵/>；基于修正因子/>、量测噪声误差协方差矩阵/>计算修正因子/>修正后的量测噪声误差协方差矩阵/>；
S5、基于修正因子修正后的量测噪声误差协方差矩阵/>计算滤波卡尔曼增益/>；
S6、基于滤波卡尔曼增益对S1得到的一步预测值/>进行后验计算，得到后验一步预测值/>；基于滤波卡尔曼增益/>对S3得到的一步预测协方差矩阵/>进行后验计算，得到后验一步预测协方差矩阵/>；
S7、扫地机器人的捷联惯性导航系统按照、/>进行下一步动作。
进一步的，所述18维包括三维SINS姿态角误差、三维SINS速度误差、三维SINS位置误差、三轴陀螺误差、三轴加速度计误差、三维里程计相关误差；
所述一步预测值的计算公式为：
；
上式中：
为/>时刻后验估计值；
为一步预测值；
为/>到/>时刻的一步转移矩阵；
该式根据系统的卡尔曼滤波状态方程来求算，状态方程为：
则：；
式中：
，
，
，
，，
，
，
；
上式中：
：地球自转角速率；
：角度误差增量与角度误差的转换矩阵；
：角度误差增量与速度误差的转换矩阵；
：角度误差增量与位置误差的转换矩阵；
：速度误差增量与角度误差的转换矩阵；
：速度误差增量与速度误差的转换矩阵；
：速度误差增量与位置误差的转换矩阵；
：位置误差增量与速度误差的转换矩阵；
：位置误差增量与位置误差的转换矩阵；
：状态一步转移矩阵；
：卡尔曼滤波误差状态量；
：误差状态一步增加量；
：角度误差状态一步增加量；
：速度误差状态一步增加量；
：位置误差状态一步增加量；
：陀螺零偏误差状态一步增加量；
：加计零偏误差状态一步增加量；
：里程计误差状态一步增加量；
：系统噪声转移矩阵；
：系统噪声；
：/>为三个姿态角误差，/>分别为俯仰角、横滚角、航向角；
：/>为/>系速度，/>分别为东速、北速、天速；
：/>为/>系三轴速度误差；
：/>为位置，/>分别为纬度、经度、高度；
：/>为位置误差；
：三轴陀螺零偏；
：三轴加速度计零偏；
：里程计输出速度误差；
：/>为地球子午圈曲率半径，/>为海拔高度，/>为子午圈曲率半径与海拔高度的和；
：/>为卯酉圈曲率半径，/>为卯酉圈曲率半径与海拔高度的和；
：加速度计输出在/>系的三轴投影；/>是东向速度，/>是北向速度，/>是天向速度；
：地球转速在载体天向的投影，/>：地球转速在载体北向的投影；
：陀螺的系统白噪声；
：加速度计的系统白噪声；
：姿态转置矩阵；/>(0<i<4,0<j<4)为姿态转置矩阵的i行j列元素。
进一步的，所述量测量值的计算公式为：
；
式中：
：量测过程噪声；
：姿态转置矩阵；
：里程计理想输出速度；
：量测量。
进一步的，所述新息值的计算公式为：
；
式中：
：/>时刻量测值转移矩阵；/>：量测值转移矩阵。
进一步的，所述一步预测协方差矩阵的计算公式为：
；
式中：为系统噪声的误差协方差矩阵，/>为自适应调整渐消因子；/>：时刻误差协方差矩阵；/>为/>到/>时刻的一步转移矩阵；/>为的转置矩阵；/>为/>时刻系统噪声一步转移矩阵；/>为/>的转置矩阵；
且式中：
；
式中：
，
，
；
式中：为遗忘因子，取值范围（0，1]；/>为新息序列的协方差矩阵；/>为量测噪声误差协方差矩阵；/>为/>时刻量测值转移矩阵；/>为/>的转置矩阵；为/>时刻误差协方差矩阵；/>为/>时刻系统噪声的误差协方差矩阵；为/>时刻新息序列的协方差矩阵；/>为/>时刻新息序列的协方差矩阵；/>为/>的转置矩阵；/>为矩阵/>的迹；/>为矩阵/>的迹；/>、/>为计算自适应调整渐消因子/>的中间变量矩阵符号。
进一步的，所述新息运算的统计量的计算公式为：
；
异常阈值的计算式为：
；
上式中a为显著性水平，为显著性水平为a、自由度为3、非中心化参数为0（中心）的卡方，/>为判别新息序列中是否有异常值的阈值；/>为/>时刻新息序列的协方差矩阵；/>为/>的逆矩阵。
进一步的，量测噪声误差协方差矩阵计算公式为：
；
其中，渐消因子，/>为渐消因子的初值，/>为遗忘因子，取值（0.9，0.999），/>为/>时刻的渐消因子；/>为/>到/>时刻的一步预测协方差矩阵；/>为/>时刻量测值转移矩阵；/>为/>的转置矩阵；
；
修正因子修正后的量测噪声误差协方差矩阵/>的计算公式为：
；
式中：为统计量；/>为判别新息序列中是否有异常值的阈值；/>为量测噪声误差协方差矩阵。
进一步的，所述滤波卡尔曼增益的计算式为：
；
式中：为/>时刻修正因子/>修正后的量测噪声误差协方差矩阵；/>为时刻量测值转移矩阵；/>为/>的转置矩阵；/>为/>到/>时刻的一步预测协方差矩阵。
进一步的，所述后验一步预测值和一步预测协方差矩阵/>的计算公式为：
；
式中：为单位矩阵；/>为量测量值。
与现有技术相比，本发明的有益效果包括：
本发明基于传感器误差模型的SINS/OD松组合导航方法，利用强跟踪卡尔曼滤波模型，对新息序列进行野值判别与修正，从而实现打滑判别与误差补偿；采用本发明的判别方法能够对绝大多数情况下的打滑情况进行判别，并针对打滑产生的导航野值进行修正。
附图说明
参照附图来说明本发明的公开内容。应当了解，附图仅仅用于说明目的，而并非意在对本发明的保护范围构成限制。其中：
图1为本发明一种基于强跟踪模型的扫地机打滑识别和修正方法的流程示意图。
具体实施方式
容易理解，根据本发明的技术方案，在不变更本发明实质精神下，本领域的一般技术人员可以提出可相互替换的多种结构方式以及实现方式。因此，以下具体实施方式以及附图仅是对本发明的技术方案的示例性说明，而不应当视为本发明的全部或者视为对本发明技术方案的限定或限制。
本发明提供一种基于强跟踪模型的扫地机打滑识别和修正方法，本发明采用基于传感器误差模型的SINS/OD松组合导航方法，利用强跟踪卡尔曼滤波模型，对新息序列进行野值判别与修正，从而实现打滑判别与误差补偿；
本发明的方法包括以下步骤：如图1所示，
步骤一、基于18维松组合扩展卡尔曼滤波模型得到一步预测值；通过扫地机内捷联惯性导航系统的速度与里程计提供的速度进行比较，得到两者的差值作为量测量值/>；
具体的，基于卡尔曼滤波的18维松组合EKF（扩展卡尔曼滤波）模型，18维分别是三维SINS姿态角误差、三维SINS速度误差、三维SINS位置误差、三轴陀螺误差、三轴加速度计误差、三维里程计相关误差。
基于18维松组合扩展卡尔曼滤波模型得到一步预测值，其中的一步预测值/>的计算公式为：
........（1）
上式中：
为/>时刻后验估计值；
为一步预测值；
为/>到/>时刻的一步转移矩阵；
该式根据系统的卡尔曼滤波状态方程来求算，状态方程为：
则：；.......（2）
式（2）中：
，/>
，
，
，，
，/>
，
；
式（2）中：：角度误差增量与角度误差的转换矩阵；
：角度误差增量与速度误差的转换矩阵；
：角度误差增量与位置误差的转换矩阵；
：速度误差增量与角度误差的转换矩阵；
：速度误差增量与速度误差的转换矩阵；
：速度误差增量与位置误差的转换矩阵；
：位置误差增量与速度误差的转换矩阵；
：位置误差增量与位置误差的转换矩阵；
：状态一步转移矩阵；
：卡尔曼滤波误差状态量；
：误差状态一步增加量；
：角度误差状态一步增加量；
：速度误差状态一步增加量；
：位置误差状态一步增加量；/>
：陀螺零偏误差状态一步增加量；
：加计零偏误差状态一步增加量；
：里程计误差状态一步增加量；
：系统噪声转移矩阵；
：系统噪声；
：地球自转角速率；
：/>为三个姿态角误差，/>分别为俯仰角、横滚角、航向角；
：/>为/>系速度，/>分别为东速、北速、天速；
：/>为/>系三轴速度误差；
：/>为位置，/>分别为纬度、经度、高度；
：/>为位置误差；
：三轴陀螺零偏；
：三轴加速度计零偏；
：里程计输出速度误差；
：/>为地球子午圈曲率半径，/>为海拔高度，/>为子午圈曲率半径与海拔高度的和；
：/>为卯酉圈曲率半径，/>为卯酉圈曲率半径与海拔高度的和；
：加速度计输出在/>系的三轴投影；/>是东向速度，是北向速度，/>是天向速度；
：地球转速在载体天向的投影，/>：地球转速在载体北向的投影；
：陀螺的系统白噪声；
：加速度计的系统白噪声；
：姿态转置矩阵；/>(0<i<4,0<j<4)为姿态转置矩阵的i行j列元素。
SINS（捷联关系导航系统）/OD（里程计）组合过程选择速度差作为量测量，捷联惯性导航系统输出的速度与里程计提供的速度进行比较，并将两者的差值作为量测量进入卡尔曼滤波过程；两者的差值作为量测量值，其中的/>的计算式为：
........（3）
式（3）中：：量测过程噪声；
：姿态转置矩阵；
：里程计理想输出速度；
：量测量。
步骤二、将一步预测值、量测量值/>录入卡尔曼滤波器，求解得到新息值；
具体的，新息值的计算公式为：
........（4）/>
式（4）中：/>时刻量测值转移矩阵；/>：量测值转移矩阵；/>为：
。
基于新息值计算一步预测协方差矩阵/>；具体的，/>的计算公式为：
........（5）
式（5）中：为系统噪声的误差协方差矩阵，/>为自适应调整渐消因子；/>：时刻误差协方差矩阵；/>为/>到/>时刻的一步转移矩阵；/>为的转置矩阵；/>为/>时刻系统噪声一步转移矩阵；/>为/>的转置矩阵；
且式（5）的计算公式为：
........（6）
式（6）中：
，
，
，新息序列的协方差矩阵，/>为遗忘因子，取值范围（0，1]。/>为量测噪声误差协方差矩阵；/>为/>时刻量测值转移矩阵；为/>的转置矩阵；/>为/>时刻误差协方差矩阵；/>为/>时刻系统噪声的误差协方差矩阵；/>为/>时刻新息序列的协方差矩阵；/>为/>时刻新息序列的协方差矩阵；/>为/>的转置矩阵；/>为矩阵/>的迹；/>为矩阵/>的迹；/>、/>为计算自适应调整渐消因子/>的中间变量矩阵符号。
步骤三、根据新息值得到新息参与运算的统计量/>，并将新息参与运算的统计量/>与异常阈值/>作比较；
若新息参与运算的统计量大于异常阈值/>，则量测量值/>为野值；则扫地机打滑，/>；
若新息参与运算的统计量不大于异常阈值/>，则量测量值/>非野值；则扫地机不打滑，/>。
具体的，其中新息参与运算的统计量的计算公式为：
........（7）
异常阈值的计算公式为：
........（8）
式（7）、（8）中：a为显著性水平，为显著性水平为a、自由度为3、非中心化参数为0（中心）的卡方，/>为判别新息序列中是否有异常值的阈值；/>为/>时刻新息序列的协方差矩阵；/>为/>的逆矩阵。
通过上述步骤一至步骤三，即可进行扫地机打滑的判定；但是由于打滑是里程计测量不准确的最主要原因，其表明里程计测得的路程增量与实际路程增量之间有较大差别；打滑分两种，一种是空转打滑，轮子存在一定的空转率，即里程计的路程增量比实际路程增量要大；另一种是滑行打滑，即里程计的路程增量比实际路程增量要大；无论是哪种打滑，都表明里程计测得的速度与实际速度的差值绝对值都很大，判别打滑也需从这里入手；kalman滤波器定义了新息，表示量测值与一步预测值之间的差，当这个差的绝对值小时，表明量测值与预测值接近，量测值可信且正常；当这个差绝对值很大时，表明量测值与预测值很不一致，量测值此时应不可信；我们可以设置阈值来判定这个值，若值大于阈值，则表明该值为野值，说明扫地机打滑。
新息序列理论上是正交序列，而在实际工程上卡尔曼滤波模型容易在量测噪声方差失去正定型情况下导致滤波发散，也就是说经典卡尔曼滤波模型鲁棒性不强，在量测值有突变时无法正常跟踪，且可能会导致新息不再是正交序列；若新息序列不是正交序列，则新息不一定可信，用新息来判别野值就不一定可靠；因此本发明提出利用强跟踪模型下的新息来判别打滑；强跟踪滤波是从扩展卡尔曼滤波发展而来，通过自适应渐消因子来调整一步误差协方差矩阵，强制使新息序列保持正交，突变状态下具有极强的跟踪能力；因此本申请还包括：
步骤四、基于新息值计算量测噪声误差协方差矩阵/>；计算修正因子/>修正后的量测噪声误差协方差矩阵/>；
量测噪声的计算公式为：
........（9）
式（9）中：渐消因子，/>为渐消因子的初值，/>为遗忘因子，取值（0.9，0.999），/>为/>时刻的渐消因子；/>为/>到/>时刻的一步预测协方差矩阵；/>为/>时刻量测值转移矩阵；/>为/>的转置矩阵。基于新息值/>计算量测噪声误差协方差矩阵/>(公式9)，然而由于新息中含有不合理的数据(打滑导致的不可用或不合理的新息数据)，因此此时的/>不能直接使用，我们先把不合理的新息数据剔除，再求新的/>也就是修正后的/>，记为/>，/>，/>是修正因子，它与剔除不合理新息数据有关。剔除不合理的新息数据就用到了新息参与运算的统计量/>和异常阈值/>，也就是说剔除不合理新息是利用设定的异常阈值/>来实现的，所以
........（10）
式（10）中，为修正因子。
进而得到修正因子修正后的量测噪声误差协方差矩阵/>，/>的计算公式为：
........（11）。/>
步骤五、基于修正后的量测噪声误差协方差矩阵计算滤波卡尔曼增益/>；
具体的，滤波卡尔曼增益的计算公式为：
.......（12）
式（12）中为/>时刻修正因子/>修正后的量测噪声误差协方差矩阵；/>为时刻量测值转移矩阵；/>为/>的转置矩阵；/>为/>到/>时刻的一步预测协方差矩阵。
步骤六、基于滤波卡尔曼增益对S1得到的一步预测值/>进行后验计算，得到后验一步预测值/>；基于滤波卡尔曼增益/>对S3得到的一步预测协方差矩阵/>进行后验计算，得到后验一步预测协方差矩阵/>；
具体的，所述后验一步预测值和一步预测协方差矩阵/>的计算公式为：
........（13）
........（14）
式（13）、（14）中：为单位矩阵；/>为量测量值
步骤七、扫地机器人的捷联惯性导航系统按照、/>进行下一步动作。
本发明的技术范围不仅仅局限于上述说明中的内容，本领域技术人员可以在不脱离本发明技术思想的前提下，对上述实施例进行多种变形和修改，而这些变形和修改均应当属于本发明的保护范围内。

Claims (8)

1.一种基于强跟踪模型的扫地机打滑识别和修正方法，其特征在于，包括以下步骤：
S1、基于18维松组合扩展卡尔曼滤波模型得到一步预测值；通过扫地机内捷联惯性导航系统的速度与里程计提供的速度进行比较，得到两者的差值作为量测量值/>；
S2、将一步预测值、量测量值/>录入卡尔曼滤波器，求解得到新息值/>；
S3、基于新息值计算一步预测协方差矩阵/>；并根据新息值/>得到新息参与运算的统计量/>，并将新息参与运算的统计量/>与异常阈值/>作比较；
若新息参与运算的统计量大于异常阈值/>，则量测量值/>为野值；则扫地机打滑，此时修正因子/>；
若新息参与运算的统计量不大于异常阈值/>，则量测量值/>为非野值；则扫地机不打滑，此时修正因子/>；
其中，一步预测协方差矩阵的计算公式为：
；
式中：为系统噪声的误差协方差矩阵，/>为自适应调整渐消因子；/>：/>时刻误差协方差矩阵；/>为/>到/>时刻的一步转移矩阵；/>为/>的转置矩阵；/>为/>时刻系统噪声一步转移矩阵；/>为/>的转置矩阵；
且式中：
；
式中：
，
，
；
式中：为遗忘因子，取值范围（0，1]；/>为量测噪声误差协方差矩阵；/>为/>时刻量测值转移矩阵；/>为/>的转置矩阵；/>为/>时刻误差协方差矩阵；/>为时刻系统噪声的误差协方差矩阵；/>为/>时刻新息序列的协方差矩阵；/>为时刻新息序列的协方差矩阵；/>为/>的转置矩阵；/>为矩阵/>的迹；为矩阵/>的迹；/>、/>为计算自适应调整渐消因子/>的中间变量矩阵符号；
新息参与运算的统计量的计算公式为：
；
异常阈值的计算式为：
；
上式中为显著性水平，/>为显著性水平为/>、自由度为3、非中心化参数为0的卡方，非中心化参数为0的卡方中的0表示中心；/>为判别新息序列中是否有异常值的阈值；/>为/>的逆矩阵。
2.根据权利要求1所述的一种基于强跟踪模型的扫地机打滑识别和修正方法，其特征在于，还包括以下步骤：
S4、基于新息值计算量测噪声误差协方差矩阵/>；基于修正因子/>、量测噪声误差协方差矩阵/>计算修正因子/>修正后的量测噪声误差协方差矩阵/>；
S5、基于修正因子修正后的量测噪声误差协方差矩阵/>计算滤波卡尔曼增益/>；
S6、基于滤波卡尔曼增益对S1得到的一步预测值/>进行后验计算，得到后验一步预测值/>；基于滤波卡尔曼增益/>对S3得到的一步预测协方差矩阵/>进行后验计算，得到后验一步预测协方差矩阵/>；
S7、扫地机的捷联惯性导航系统按照、/>进行下一步动作。
3.根据权利要求2所述的一种基于强跟踪模型的扫地机打滑识别和修正方法，其特征在于，所述18维包括三维SINS姿态角误差、三维SINS速度误差、三维SINS位置误差、三轴陀螺误差、三轴加速度计误差、三维里程计相关误差；
所述一步预测值的计算公式为：
；
上式中：
为/>时刻后验估计值；
为一步预测值；
为/>到/>时刻的一步转移矩阵；
该式根据系统的卡尔曼滤波状态方程来求解，状态方程为：
则：；
式中：
，
，
，
，，
，
，
；
上式中：
：地球自转角速率；
：角度误差增量与角度误差的转换矩阵；
：角度误差增量与速度误差的转换矩阵；
：角度误差增量与位置误差的转换矩阵；
：速度误差增量与角度误差的转换矩阵；
：速度误差增量与速度误差的转换矩阵；
：速度误差增量与位置误差的转换矩阵；
：位置误差增量与速度误差的转换矩阵；
：位置误差增量与位置误差的转换矩阵；
：状态一步转移矩阵；
：卡尔曼滤波误差状态量；
：误差状态一步增加量；
：角度误差状态一步增加量；
：速度误差状态一步增加量；
：位置误差状态一步增加量；
：陀螺零偏误差状态一步增加量；
：加计零偏误差状态一步增加量；
：里程计误差状态一步增加量；
：系统噪声转移矩阵；
：系统噪声；
：/>为三个姿态角误差，/>分别为俯仰角、横滚角、航向角；
：/>为/>系速度，/>分别为东速、北速、天速；
：/>为/>系三轴速度误差；
：/>为位置，/>分别为纬度、经度、高度；
：/>为位置误差；
：三轴陀螺零偏；
：三轴加速度计零偏；
：里程计输出速度误差；
：/>为地球子午圈曲率半径，/>为海拔高度，/>为子午圈曲率半径与海拔高度的和；
：/>为卯酉圈曲率半径，/>为卯酉圈曲率半径与海拔高度的和；
：加速度计输出在/>系的三轴投影；/>是东向速度，/>是北向速度，/>是天向速度；
：地球转速在载体天向的投影，/>：地球转速在载体北向的投影；
：陀螺的系统白噪声；
：加速度计的系统白噪声；
：姿态转置矩阵；/>为姿态转置矩阵的i行j列元素，其中，0<i<4，0<j<4。
4.根据权利要求3所述的一种基于强跟踪模型的扫地机打滑识别和修正方法，其特征在于，所述量测量值的计算公式为：
；
式中：
：量测过程噪声；
：姿态转置矩阵；
：里程计理想输出速度；
：量测量。
5.根据权利要求2所述的一种基于强跟踪模型的扫地机打滑识别和修正方法，其特征在于，所述新息值的计算公式为：
；
式中：
：/>时刻量测值转移矩阵。
6.根据权利要求5所述的一种基于强跟踪模型的扫地机打滑识别和修正方法，其特征在于，量测噪声误差协方差矩阵计算公式为：
；
其中，渐消因子，/>=0，/>为渐消因子的初值，/>为遗忘因子，取值（0.9，0.999），/>为/>时刻的渐消因子；/>为/>到/>时刻的一步预测协方差矩阵；/>为/>时刻量测值转移矩阵；/>为/>的转置矩阵；
；
修正因子修正后的量测噪声误差协方差矩阵/>的计算公式为：
；
式中：为统计量；/>为判别新息序列中是否有异常值的阈值；/>为量测噪声误差协方差矩阵。
7.根据权利要求6所述的一种基于强跟踪模型的扫地机打滑识别和修正方法，其特征在于，所述滤波卡尔曼增益的计算式为：
；
式中：为/>时刻修正因子/>修正后的量测噪声误差协方差矩阵；/>为/>时刻量测值转移矩阵；/>为/>的转置矩阵；/>为/>到/>时刻的一步预测协方差矩阵。
8.根据权利要求7所述的一种基于强跟踪模型的扫地机打滑识别和修正方法，其特征在于，所述后验一步预测值和一步预测协方差矩阵/>的计算公式为：
,/>；
式中：为单位矩阵；/>为量测量值。
CN202310882640.9A 2023-07-19 2023-07-19 一种基于强跟踪模型的扫地机打滑识别和修正方法 Active CN116625360B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310882640.9A CN116625360B (zh) 2023-07-19 2023-07-19 一种基于强跟踪模型的扫地机打滑识别和修正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310882640.9A CN116625360B (zh) 2023-07-19 2023-07-19 一种基于强跟踪模型的扫地机打滑识别和修正方法

Publications (2)

Publication Number Publication Date
CN116625360A CN116625360A (zh) 2023-08-22
CN116625360B true CN116625360B (zh) 2023-09-29

Family

ID=87602815

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310882640.9A Active CN116625360B (zh) 2023-07-19 2023-07-19 一种基于强跟踪模型的扫地机打滑识别和修正方法

Country Status (1)

Country Link
CN (1) CN116625360B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103033186A (zh) * 2012-12-30 2013-04-10 东南大学 一种用于水下滑翔器的高精度组合导航定位方法
CN114396941A (zh) * 2021-12-20 2022-04-26 东南大学 一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法
CN115979253A (zh) * 2022-11-30 2023-04-18 河海大学 一种基于抗差滤波的水下机器人多传感器紧组合导航方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012138828A2 (en) * 2011-04-08 2012-10-11 The Trustees Of Columbia University In The City Of New York Kalman filter approach to augment object tracking

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103033186A (zh) * 2012-12-30 2013-04-10 东南大学 一种用于水下滑翔器的高精度组合导航定位方法
CN114396941A (zh) * 2021-12-20 2022-04-26 东南大学 一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法
CN115979253A (zh) * 2022-11-30 2023-04-18 河海大学 一种基于抗差滤波的水下机器人多传感器紧组合导航方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
A Robust Single GPS Navigation and Positioning Algorithm Based on Strong Tracking Filtering.IEEE SENSORS JOURNAL.2018,第18卷(第1期),正文第290-297页. *
贾文哲.两类改进非线性滤波器UKF算法综述.化工自动化及仪表.2016,(第10期),正文第1011-1014页. *

Also Published As

Publication number Publication date
CN116625360A (zh) 2023-08-22

Similar Documents

Publication Publication Date Title
CN102096086B (zh) 一种基于gps/ins组合导航系统不同测量特性的自适应滤波方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&amp;gnss松组合导航方法
CN102508278B (zh) 一种基于观测噪声方差阵估计的自适应滤波方法
CN104061899B (zh) 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法
CN101900573B (zh) 一种实现陆用惯性导航系统运动对准的方法
CN110530361B (zh) 基于农业机械双天线gnss自动导航系统的转向角度估计器
CN112762961B (zh) 一种车载惯性里程计组合导航在线标定方法
CN109945859B (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN107607113A (zh) 一种两轴姿态倾角测量方法
CN110341714B (zh) 一种同时估计车辆质心侧偏角和扰动的方法
CN112050809A (zh) 轮式里程计与陀螺仪信息融合的无人车定向定位方法
CN112284415B (zh) 里程计标度误差标定方法、系统及计算机存储介质
CN111024074A (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN115060257B (zh) 一种基于民用级惯性测量单元的车辆变道检测方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
US11731596B2 (en) Method for the traction control of a single-track motor vehicle taking the slip angle of the rear wheel into consideration
CN113771857A (zh) 一种用于车辆控制的纵向车速估计方法和系统
CN114935345A (zh) 一种基于模式识别的车载惯导安装角误差补偿方法
CN109033017B (zh) 一种丢包环境下的车辆侧倾角与俯仰角估计方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN110082561A (zh) 一种agv车体打滑的检测及其校正方法
CN116625360B (zh) 一种基于强跟踪模型的扫地机打滑识别和修正方法
CN114674313B (zh) 一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法
CN116338719A (zh) 基于b样条函数的激光雷达-惯性-车辆融合定位方法
CN114932909A (zh) 一种基于互补滤波实现加速度校正的坡度估算方法

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