CN111504254A - 一种基于六轴陀螺仪的扫地机角度测算方法 - Google Patents

一种基于六轴陀螺仪的扫地机角度测算方法 Download PDF

Info

Publication number
CN111504254A
CN111504254A CN202010350412.3A CN202010350412A CN111504254A CN 111504254 A CN111504254 A CN 111504254A CN 202010350412 A CN202010350412 A CN 202010350412A CN 111504254 A CN111504254 A CN 111504254A
Authority
CN
China
Prior art keywords
data
time
zero offset
calculating
updating
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.)
Granted
Application number
CN202010350412.3A
Other languages
English (en)
Other versions
CN111504254B (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.)
Shenzhen Umouse Technology Development Co Ltd
Original Assignee
Shenzhen Umouse Technology Development 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 Shenzhen Umouse Technology Development Co Ltd filed Critical Shenzhen Umouse Technology Development Co Ltd
Priority to CN202010350412.3A priority Critical patent/CN111504254B/zh
Publication of CN111504254A publication Critical patent/CN111504254A/zh
Application granted granted Critical
Publication of CN111504254B publication Critical patent/CN111504254B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • 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/4011—Regulation of the cleaning machine by electric means; Control systems and remote control systems therefor
    • 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/4063—Driving means; Transmission means therefor
    • A47L11/4066—Propulsion of the whole machine
    • 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/4063—Driving means; Transmission means therefor
    • A47L11/4069—Driving or transmission means for the cleaning tools
    • 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

Landscapes

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

Abstract

本发明提供一种基于六轴陀螺仪的扫地机角度测算方法，包括以下步骤，S1：扫地机开启，惯导初始化，任务挂起；预设5MS的任务执行周期，并进行惯导模式判断；所述惯导模式判断包括正常模式、复位角度、惯导初始化以及以及IIC恢复；S2：正常模式下的数据更新包括第一个5MS周期更新以及第二个5MS周期更新；所述第一个5MS周期更新具体包括陀螺仪数据更新以及逻辑计算、零偏更新以及姿态解算；所述第二个5MS周期更新具体包括加速度计数据更新，本发明专利实现了以六轴陀螺仪为信号采集单元通过一系列算法处理，实现精准角度的计算，通过本算法可实现在扫地机执行清扫时1小时内角度误差在5度左右，2小时在10度左右。

Description

一种基于六轴陀螺仪的扫地机角度测算方法
[技术领域]
本发明涉及扫地机控制处理方法技术领域，尤其涉及一种可实现在扫地机执行清扫时1小时内角度误差在5度左右，2小时在10度左右的基于六轴陀螺仪的扫地机角度测算方法。
[背景技术]
近些年，扫地机器人产品得到了快速的普及和应用，为人们的日常生活带来了很大的便利。
常规的扫地机器人智能化程度不够高，遇到一般的障碍之后就会影响正常的运行或者产生报警音，且功能多样性不够高，制约着产品的更好的推广，随着科技的进步和性能优化，扫地机器人产品的智能化程度、安全性能也得到了大幅度的提升，人性化程度也更高。
扫地机器人运行过程中的清扫角度控制是影响扫地机器人实际使用的重要部分，且目前的处理方式还较难显著降低角度计算误差度，上述问题也是目前制约产品更好的推广与应用的关键因素。
综上，本领域技术人员有针对性的进行了大量的研发与实验，从扫地机清扫角度处理的具体方式方面入手进行改进和改善，并取得了较好的成绩。
[发明内容]
为克服现有技术所存在的问题，本发明提供一种可实现在扫地机执行清扫时1小时内角度误差在5度左右，2小时在10度左右的基于六轴陀螺仪的扫地机角度测算方法。
本发明解决技术问题的方案是提供一种基于六轴陀螺仪的扫地机角度测算方法，包括以下步骤，
S1：扫地机开启，惯导初始化，任务挂起；预设5MS的任务执行周期，并进行惯导模式判断；所述惯导模式判断包括正常模式、复位角度、惯导初始化以及以及IIC恢复；
S2：正常模式下的数据更新包括第一个5MS周期更新以及第二个5MS周期更新；所述第一个5MS周期更新具体包括陀螺仪数据更新以及逻辑计算、零偏更新以及姿态解算；所述第二个5MS周期更新具体包括加速度计数据更新；
S3：所述逻辑计算、零偏更新以及姿态解算具体分为开机初始零偏计算Z轴零偏作Rz、计算随机零偏、六轴数据去零偏以及姿态解算；
S4：开机初始零偏计算Z轴零偏作Rz具体包括判断机器是否震动过大、计算六轴零偏、记录实时温度以及计算数据方差即Z轴方差记作COV(0)；计算随机零偏包括温度检测和判断机器是否符合计算随机零偏条件；姿态解算包括加速计、陀螺仪、PI互补滤波计算四元数以及四元数转换位欧拉角；
S5：数据测算以及更新输出完成。
优选地，所述步骤S4中温度检测过程，若温度值大于设定阈值则申请停机计算零偏。
优选地，所述步骤S4中判断机器是否符合计算随机零偏条件包括计算约150组Z轴数据的平均值Rk方差COV(k)、数据离散程度判断(使用数据方差判断)、温度变化是否超过设定阈值以及零偏校准。
优选地，所述零偏校准具体通过卡尔曼滤波动态更新陀螺仪零偏方法，包括以下步骤，
B1：步骤S1：开机静止4s，计算400组陀螺仪数据的平均值作为初始offset，记作E(0),此时该400组数据测量值的方差记作COV(0)，并记录此时温度；
B2：通过IMU六轴数据判断是否超过阈值从而判断机器是否为静态；
B3：机器处于静止状态，计算此时陀螺仪数据的平均值，记作E(k),并计算其方差，记作COV(k),k表示第k次成功获取静态数据，若之后能继续成功获取静态载体姿态数据，则k＝k+1,利用卡尔曼算法计算，得到新的陀螺仪offset,之后跳转到所述步骤S2，形成循环；
B4：温度超过设定设定阈值，则申请停机，重新采集数据，复位卡尔曼滤波的参数；初值赋值如下：其中P(0|0)＝COV(0)，X(0|0)＝E(0),Q(k)＝0.0001(Q(k)可根据具体情况设置，R(k)＝COV(k)，通过迭代计算，可以得到不同k时刻的补偿值X(k|k)；X(k|k-1)＝X(k-1|k-1)其中k表示第k次测量满足静态条件的判定(此后称之为k时刻)，此时X(k-1|k-1)为k-1时刻Rz的最优补偿值，即Offset，X(k|k-1)为通过k-1时刻的Rz最优补偿值得到的k时刻Rz的估计补偿值；P(k|k-1)＝P(k-1|k-1)+Q(k)P(k-1|k-1)为k-1时刻的Rz的最优补偿值的协方差；P(k|k-1)为k时刻的估计补偿值的协方差，Q(k)为k时刻估计噪声的方差；X(k|k)＝X(k|k-1)+Kg(k)(E(k)–X(k|k-1))；Kg(k)＝P(k|k-1)/(P(k|k-1)+R(k))；P(k|k)＝(1–Kg(k))P(k|k-1)；Offset＝X(k|k)；其中R(k)为k时刻测量噪声的方差，Kg(k)为k时刻的卡尔曼增益，P(k|k)为k时刻最优补偿值的协方差，X(k|k)表示对k时刻Rz的测量平均值E(k)与估计值X(k|k-1)通过卡尔曼增益值分配权重获得的k时刻Rz的最优补偿值，最后将X(k|k)赋值给Offset，完成Rz补偿值的更新，此时的Offset为获取的最终补偿值。
优选地，所述步骤S1中的扫地机包括主机体以及设置于所述主机体内部的电源、主控制器、数据存储器、用于进行信号采集的六轴陀螺仪；还包括与外部移动终端进行通讯连接的无线通讯传输单元、储水水箱以及安设于主机体下部的边刷组件以及左右驱动轮组件；在所述主机体内部分别设置有用于对左右驱动轮组件的运行进行控制、接收来自控制器的PWM控制信号的左右轮控制驱动电路单元、用于对左右驱动轮组件的运行速率进行实时检测的左右轮速度检测电路单元以及用于对边刷组件的运行进行控制的边刷驱动电路单元；该无线通讯传输单元、左右轮控制驱动电路单元、左右轮速度检测电路单元、边刷驱动电路单元、六轴陀螺仪与主控制器电性连接。
与现有技术相比，本发明一种基于六轴陀螺仪的扫地机角度测算方法，本发明专利实现了以六轴陀螺仪为信号采集单元通过一系列算法处理，实现精准角度的计算，通过本算法可实现在扫地机执行清扫时1小时内角度误差在5度左右，2小时在10度左右。
[附图说明]
图1是本发明一种基于六轴陀螺仪的扫地机角度测算方法的流程示意图。
[具体实施方式]
为使本发明的目的，技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定此发明。
请参阅图1，本发明一种基于六轴陀螺仪的扫地机角度测算方法1包括以下步骤，
S1：扫地机开启，惯导初始化，任务挂起；预设5MS的任务执行周期，并进行惯导模式判断；所述惯导模式判断包括正常模式、复位角度、惯导初始化以及以及IIC恢复；
S2：正常模式下的数据更新包括第一个5MS周期更新以及第二个5MS周期更新；所述第一个5MS周期更新具体包括陀螺仪数据更新以及逻辑计算、零偏更新以及姿态解算；所述第二个5MS周期更新具体包括加速度计数据更新；
S3：所述逻辑计算、零偏更新以及姿态解算具体分为开机初始零偏计算Z轴零偏作Rz、计算随机零偏、六轴数据去零偏以及姿态解算；
S4：开机初始零偏计算Z轴零偏作Rz具体包括判断机器是否震动过大、计算六轴零偏、记录实时温度以及计算数据方差即Z轴方差记作COV(0)；计算随机零偏包括温度检测和判断机器是否符合计算随机零偏条件；姿态解算包括加速计、陀螺仪、PI互补滤波计算四元数以及四元数转换位欧拉角；
S5：数据测算以及更新输出完成。
本发明专利实现了以六轴陀螺仪为信号采集单元通过一系列算法处理，实现精准角度的计算，通过本算法可实现在扫地机执行清扫时1小时内角度误差在5度左右，2小时在10度左右。
优选地，所述步骤S4中温度检测过程，若温度值大于设定阈值则申请停机计算零偏。
优选地，所述步骤S4中判断机器是否符合计算随机零偏条件包括计算约150组Z轴数据的平均值Rk方差COV(k)、数据离散程度判断(使用数据方差判断)、温度变化是否超过设定阈值以及零偏校准。
优选地，所述零偏校准具体通过卡尔曼滤波动态更新陀螺仪零偏方法，包括以下步骤，
B1：步骤S1：开机静止4s，计算400组陀螺仪数据的平均值作为初始offset，记作E(0),此时该400组数据测量值的方差记作COV(0)，并记录此时温度；
B2：通过IMU六轴数据判断是否超过阈值从而判断机器是否为静态；
B3：机器处于静止状态，计算此时陀螺仪数据的平均值，记作E(k),并计算其方差，记作COV(k),k表示第k次成功获取静态数据，若之后能继续成功获取静态载体姿态数据，则k＝k+1,利用卡尔曼算法计算，得到新的陀螺仪offset,之后跳转到所述步骤S2，形成循环；
B4：温度超过设定设定阈值，则申请停机，重新采集数据，复位卡尔曼滤波的参数；初值赋值如下：其中P(0|0)＝COV(0)，X(0|0)＝E(0),Q(k)＝0.0001(Q(k)可根据具体情况设置，R(k)＝COV(k)，通过迭代计算，可以得到不同k时刻的补偿值X(k|k)；X(k|k-1)＝X(k-1|k-1)其中k表示第k次测量满足静态条件的判定(此后称之为k时刻)，此时X(k-1|k-1)为k-1时刻Rz的最优补偿值，即Offset，X(k|k-1)为通过k-1时刻的Rz最优补偿值得到的k时刻Rz的估计补偿值；P(k|k-1)＝P(k-1|k-1)+Q(k)P(k-1|k-1)为k-1时刻的Rz的最优补偿值的协方差；
P(k|k-1)为k时刻的估计补偿值的协方差，Q(k)为k时刻估计噪声的方差；X(k|k)＝X(k|k-1)+Kg(k)(E(k)–X(k|k-1))；Kg(k)＝P(k|k-1)/(P(k|k-1)+R(k))；P(k|k)＝(1–Kg(k))P(k|k-1)；Offset＝X(k|k)；其中R(k)为k时刻测量噪声的方差，Kg(k)为k时刻的卡尔曼增益，P(k|k)为k时刻最优补偿值的协方差，X(k|k)表示对k时刻Rz的测量平均值E(k)与估计值X(k|k-1)通过卡尔曼增益值分配权重获得的k时刻Rz的最优补偿值，最后将X(k|k)赋值给Offset，完成Rz补偿值的更新，此时的Offset为获取的最终补偿值。
优选地，所述步骤S1中的扫地机包括主机体以及设置于所述主机体内部的电源、主控制器、数据存储器、用于进行信号采集的六轴陀螺仪；还包括与外部移动终端进行通讯连接的无线通讯传输单元、储水水箱以及安设于主机体下部的边刷组件以及左右驱动轮组件；在所述主机体内部分别设置有用于对左右驱动轮组件的运行进行控制、接收来自控制器的PWM控制信号的左右轮控制驱动电路单元、用于对左右驱动轮组件的运行速率进行实时检测的左右轮速度检测电路单元以及用于对边刷组件的运行进行控制的边刷驱动电路单元；该无线通讯传输单元、左右轮控制驱动电路单元、左右轮速度检测电路单元、边刷驱动电路单元、六轴陀螺仪与主控制器电性连接。
步骤S4中用四元数法进行姿态解算，其步骤如下：
1.四元素初始化(第一次解算时计算)
①静止状态下由加速度求取初始rollγ、pitchθ、hdgψ：
γ＝arctan-ay-az；
θ＝arctanax-ay；
ψ＝arctan-magymagx；
②初始化四元数
q0＝cosγ2cosθ2cosψ2+sinγ2sinθ2sinψ；
q1＝2sinγ2cosθ2cosψ2-cosγ2sinθ2sinψ；
q2＝2cosγ2sinθ2cosψ2+sinγ2cosθ2sinψ；
q3＝2cosγ2cosθ2sinψ2-sinγ2sinθ2cosψ2；
2.加速度归一化：
Figure BDA0002471626160000071
Ax＝axnorm；
Ay＝aynorm；
Az＝aznorm；
3.提取四元素等效余弦矩阵中的重力分量：
四元素等效矩阵
Figure BDA0002471626160000081
重力矩阵[001]左乘上面矩阵M即可提取出重力分量
vx＝2(q1q3-q0q2)；
vy＝2(q0q1+q2q3)；
vz＝1-2(q21+q22)＝q20-q21-q22+q23；
4.向量叉积求取姿态误差：
Figure BDA0002471626160000085
向量的叉积可以用来判断两个向量是否平行，当两个向量都为单位向量的时候，它们之间的叉积就代表了它们之间的平行度，若平行则叉积为0，若垂直则叉积为1，两向量的方向差越小，叉积也越小，因此可以用叉积来表示两归一化向量的方向误差。
根据向量的叉乘规则：若
Figure BDA0002471626160000082
Figure BDA0002471626160000083
Figure BDA0002471626160000084
a＝b×c
则
ax＝bycz-bzcy；
ay＝bzcx-bxcz；
az＝bxcy-bycx；
可得
ex＝vy＊az-vz＊ay；
ey＝vz＊ax-vx＊az；
ez＝vx＊ay-vy＊ax；
5.对误差进行比例积分运算，更新陀螺仪角速度值：
exlnt＝exlnt+ex*ki
eylnt＝eylnt+ey*ki
ezlnt＝ezlnt+ez*ki
gx＝gx+ex*kp+exlnt
gy＝gy+ey*kp+eylnt
gz＝gz+ez*kp+ezlnt
6.一阶龙格库塔法求解四元素：
Figure BDA0002471626160000091
ωx、ωy、ωz为经过数据融合后的三轴角速度，本例中即为上一步计算出的gx、gy、gz因为引入了误差，使得变换出来的四元素模不再等于1，即失去了规范性，因此在下一步计算前要进行规范化。
7.四元素反正切求取欧拉角：
反正切解算出rollγ、pitchθ、hdgψ：
γ＝arctan2(q0q1+q2q3)q0q0-q1q1-q2q2+q3q3；
θ＝arcsin[2(q0q2-q1q3)]；
ψ＝arctan2(q0q3+q1q2)q0q0+q1q1-q2q2-q3q3；
与现有技术相比，本发明一种基于六轴陀螺仪的扫地机角度测算方法1，本发明专利实现了以六轴陀螺仪为信号采集单元通过一系列算法处理，实现精准角度的计算，通过本算法可实现在扫地机执行清扫时1小时内角度误差在5度左右，2小时在10度左右。
以上所述的本发明实施方式，并不构成对本发明保护范围的限定。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明的权利要求保护范围之内。

Claims (5)

1.一种基于六轴陀螺仪的扫地机角度测算方法，其特征在于：包括以下步骤，
S1：扫地机开启，惯导初始化，任务挂起；预设5MS的任务执行周期，并进行惯导模式判断；所述惯导模式判断包括正常模式、复位角度、惯导初始化以及以及IIC恢复；
S2：正常模式下的数据更新包括第一个5MS周期更新以及第二个5MS周期更新；所述第一个5MS周期更新具体包括陀螺仪数据更新以及逻辑计算、零偏更新以及姿态解算；所述第二个5MS周期更新具体包括加速度计数据更新；
S3：所述逻辑计算、零偏更新以及姿态解算具体分为开机初始零偏计算Z轴零偏作Rz、计算随机零偏、六轴数据去零偏以及姿态解算；
S4：开机初始零偏计算Z轴零偏作Rz具体包括判断机器是否震动过大、计算六轴零偏、记录实时温度以及计算数据方差即Z轴方差记作COV(0)；计算随机零偏包括温度检测和判断机器是否符合计算随机零偏条件；姿态解算包括加速计、陀螺仪、PI互补滤波计算四元数以及四元数转换位欧拉角；
S5：数据测算以及更新输出完成。
2.如权利要求1所述的一种基于六轴陀螺仪的扫地机角度测算方法，其特征在于：所述步骤S4中温度检测过程，若温度值大于设定阈值则申请停机计算零偏。
3.如权利要求1所述的一种基于六轴陀螺仪的扫地机角度测算方法，其特征在于：所述步骤S4中判断机器是否符合计算随机零偏条件包括计算约150组Z轴数据的平均值Rk方差COV(k)、数据离散程度判断(使用数据方差判断)、温度变化是否超过设定阈值以及零偏校准。
4.如权利要求3所述的一种基于六轴陀螺仪的扫地机角度测算方法，其特征在于：所述零偏校准具体通过卡尔曼滤波动态更新陀螺仪零偏方法，包括以下步骤，
B1：步骤S1：开机静止4s，计算400组陀螺仪数据的平均值作为初始offset，记作E(0),此时该400组数据测量值的方差记作COV(0)，并记录此时温度；
B2：通过IMU六轴数据判断是否超过阈值从而判断机器是否为静态；
B3：机器处于静止状态，计算此时陀螺仪数据的平均值，记作E(k),并计算其方差，记作COV(k),k表示第k次成功获取静态数据，若之后能继续成功获取静态载体姿态数据，则k＝k+1,利用卡尔曼算法计算，得到新的陀螺仪offset,之后跳转到所述步骤S2，形成循环；
B4：温度超过设定设定阈值，则申请停机，重新采集数据，复位卡尔曼滤波的参数；初值赋值如下：其中P(0|0)＝COV(0)，X(0|0)＝E(0),Q(k)＝0.0001(Q(k)可根据具体情况设置，R(k)＝COV(k)，通过迭代计算，可以得到不同k时刻的补偿值X(k|k)；X(k|k-1)＝X(k-1|k-1)其中k表示第k次测量满足静态条件的判定(此后称之为k时刻)，此时X(k-1|k-1)为k-1时刻Rz的最优补偿值，即Offset，X(k|k-1)为通过k-1时刻的Rz最优补偿值得到的k时刻Rz的估计补偿值；P(k|k-1)＝P(k-1|k-1)+Q(k)P(k-1|k-1)为k-1时刻的Rz的最优补偿值的协方差；P(k|k-1)为k时刻的估计补偿值的协方差，Q(k)为k时刻估计噪声的方差；X(k|k)＝X(k|k-1)+Kg(k)(E(k)–X(k|k-1))；Kg(k)＝P(k|k-1)/(P(k|k-1)+R(k))；P(k|k)＝(1–Kg(k))P(k|k-1)；Offset＝X(k|k)；其中R(k)为k时刻测量噪声的方差，Kg(k)为k时刻的卡尔曼增益，P(k|k)为k时刻最优补偿值的协方差，X(k|k)表示对k时刻Rz的测量平均值E(k)与估计值X(k|k-1)通过卡尔曼增益值分配权重获得的k时刻Rz的最优补偿值，最后将X(k|k)赋值给Offset，完成Rz补偿值的更新，此时的Offset为获取的最终补偿值。
5.如权利要求1所述的一种基于六轴陀螺仪的扫地机角度测算方法，其特征在于：所述步骤S1中的扫地机包括主机体以及设置于所述主机体内部的电源、主控制器、数据存储器、用于进行信号采集的六轴陀螺仪；还包括与外部移动终端进行通讯连接的无线通讯传输单元、储水水箱以及安设于主机体下部的边刷组件以及左右驱动轮组件；在所述主机体内部分别设置有用于对左右驱动轮组件的运行进行控制、接收来自控制器的PWM控制信号的左右轮控制驱动电路单元、用于对左右驱动轮组件的运行速率进行实时检测的左右轮速度检测电路单元以及用于对边刷组件的运行进行控制的边刷驱动电路单元；该无线通讯传输单元、左右轮控制驱动电路单元、左右轮速度检测电路单元、边刷驱动电路单元、六轴陀螺仪与主控制器电性连接。
CN202010350412.3A 2020-04-28 2020-04-28 一种基于六轴陀螺仪的扫地机角度测算方法 Active CN111504254B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010350412.3A CN111504254B (zh) 2020-04-28 2020-04-28 一种基于六轴陀螺仪的扫地机角度测算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010350412.3A CN111504254B (zh) 2020-04-28 2020-04-28 一种基于六轴陀螺仪的扫地机角度测算方法

Publications (2)

Publication Number Publication Date
CN111504254A true CN111504254A (zh) 2020-08-07
CN111504254B CN111504254B (zh) 2021-04-20

Family

ID=71874883

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010350412.3A Active CN111504254B (zh) 2020-04-28 2020-04-28 一种基于六轴陀螺仪的扫地机角度测算方法

Country Status (1)

Country Link
CN (1) CN111504254B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112535434A (zh) * 2020-10-23 2021-03-23 湖南新视电子技术有限公司 一种无尘室智能扫地机器人
CN112690710A (zh) * 2020-12-29 2021-04-23 深圳市云视机器人有限公司 障碍物通过性判断方法、装置、计算机设备及存储介质
CN114001751A (zh) * 2021-10-08 2022-02-01 深圳市无限动力发展有限公司 扫地机陀螺仪的校准方法、装置、存储介质及计算机设备
CN114947627A (zh) * 2022-08-01 2022-08-30 深圳市云鼠科技开发有限公司 扫地机imu初始化的判定方法、装置、设备及存储介质

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9423250B1 (en) * 2009-12-17 2016-08-23 The Boeing Company Position measurement correction using loop-closure and movement data
CN107560613A (zh) * 2017-08-15 2018-01-09 江苏科技大学 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法
CN108534772A (zh) * 2018-06-24 2018-09-14 西宁泰里霍利智能科技有限公司 姿态角获取方法及装置
CN108638053A (zh) * 2018-04-03 2018-10-12 珠海市微半导体有限公司 一种机器人打滑的检测方法及其矫正方法
CN108784545A (zh) * 2018-07-25 2018-11-13 苏州精源创智能科技有限公司 一种应用于扫地机器人的惯性导航系统及方法
CN108784530A (zh) * 2018-06-05 2018-11-13 江苏美的清洁电器股份有限公司 扫地机及其行进角度的测量方法、装置
CN109443342A (zh) * 2018-09-05 2019-03-08 中原工学院 新型自适应卡尔曼无人机姿态解算方法
CN109512342A (zh) * 2018-12-25 2019-03-26 深圳市云鼠科技开发有限公司 一种基于陀螺仪的扫地机器人及姿态检测与脱困方法
US20190120633A1 (en) * 2017-10-17 2019-04-25 AI Incorporated Discovering and plotting the boundary of an enclosure
CN109674404A (zh) * 2019-01-26 2019-04-26 深圳市云鼠科技开发有限公司 一种基于free move技术的扫地机器人避障处理方式
CN109765402A (zh) * 2019-03-06 2019-05-17 上海理工大学 一种基于双加速度计的加速度测量装置和卡尔曼滤波算法
CN110693396A (zh) * 2019-10-21 2020-01-17 深圳市云鼠科技开发有限公司 一种基于free move技术的扫地机避障处理方式
CN110864684A (zh) * 2019-11-29 2020-03-06 中国科学院电子学研究所 用户姿态测算方法
CN110906922A (zh) * 2019-11-08 2020-03-24 沈阳无距科技有限公司 无人机位姿信息的确定方法及装置、存储介质、终端

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9423250B1 (en) * 2009-12-17 2016-08-23 The Boeing Company Position measurement correction using loop-closure and movement data
CN107560613A (zh) * 2017-08-15 2018-01-09 江苏科技大学 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法
US20190120633A1 (en) * 2017-10-17 2019-04-25 AI Incorporated Discovering and plotting the boundary of an enclosure
CN108638053A (zh) * 2018-04-03 2018-10-12 珠海市微半导体有限公司 一种机器人打滑的检测方法及其矫正方法
CN108784530A (zh) * 2018-06-05 2018-11-13 江苏美的清洁电器股份有限公司 扫地机及其行进角度的测量方法、装置
CN108534772A (zh) * 2018-06-24 2018-09-14 西宁泰里霍利智能科技有限公司 姿态角获取方法及装置
CN108784545A (zh) * 2018-07-25 2018-11-13 苏州精源创智能科技有限公司 一种应用于扫地机器人的惯性导航系统及方法
CN109443342A (zh) * 2018-09-05 2019-03-08 中原工学院 新型自适应卡尔曼无人机姿态解算方法
CN109512342A (zh) * 2018-12-25 2019-03-26 深圳市云鼠科技开发有限公司 一种基于陀螺仪的扫地机器人及姿态检测与脱困方法
CN109674404A (zh) * 2019-01-26 2019-04-26 深圳市云鼠科技开发有限公司 一种基于free move技术的扫地机器人避障处理方式
CN109765402A (zh) * 2019-03-06 2019-05-17 上海理工大学 一种基于双加速度计的加速度测量装置和卡尔曼滤波算法
CN110693396A (zh) * 2019-10-21 2020-01-17 深圳市云鼠科技开发有限公司 一种基于free move技术的扫地机避障处理方式
CN110906922A (zh) * 2019-11-08 2020-03-24 沈阳无距科技有限公司 无人机位姿信息的确定方法及装置、存储介质、终端
CN110864684A (zh) * 2019-11-29 2020-03-06 中国科学院电子学研究所 用户姿态测算方法

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112535434A (zh) * 2020-10-23 2021-03-23 湖南新视电子技术有限公司 一种无尘室智能扫地机器人
CN112690710A (zh) * 2020-12-29 2021-04-23 深圳市云视机器人有限公司 障碍物通过性判断方法、装置、计算机设备及存储介质
CN114001751A (zh) * 2021-10-08 2022-02-01 深圳市无限动力发展有限公司 扫地机陀螺仪的校准方法、装置、存储介质及计算机设备
CN114947627A (zh) * 2022-08-01 2022-08-30 深圳市云鼠科技开发有限公司 扫地机imu初始化的判定方法、装置、设备及存储介质
CN114947627B (zh) * 2022-08-01 2022-11-22 深圳市云鼠科技开发有限公司 扫地机imu初始化的判定方法、装置、设备及存储介质

Also Published As

Publication number Publication date
CN111504254B (zh) 2021-04-20

Similar Documents

Publication Publication Date Title
CN111504254B (zh) 一种基于六轴陀螺仪的扫地机角度测算方法
WO2020253854A1 (zh) 移动机器人姿态角解算方法
CN103017763B (zh) 状态估计设备和偏移更新方法
CN109099913B (zh) 一种基于mems惯性器件的穿戴式导航装置和方法
CN108731700B (zh) 一种视觉惯性里程计中的加权欧拉预积分方法
EP2807452A1 (en) In-use automatic calibration methodology for sensors in mobile devices
CN110543184A (zh) 一种刚性飞行器的固定时间神经网络控制方法
CN111572818B (zh) 一种磁悬浮控制力矩陀螺框架速率伺服系统及控制方法
CN104298113A (zh) 两轮机器人自适应模糊平衡控制器
CN110543183A (zh) 一种考虑执行器受限问题的刚体飞行器固定时间姿态跟踪控制方法
Hasan et al. Model-based fault diagnosis algorithms for robotic systems
CN104819717B (zh) 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法
CN116147624A (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
WO2022222938A1 (zh) 一种基于运动状态监测的自适应水平姿态测量方法
CN108507567B (zh) 姿态四元数确定方法、装置及用户朝向确定方法、装置
CN112070170A (zh) 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN115248553A (zh) 欠驱动船舶的事件触发自适应pid航迹跟踪容错控制方法
CN112729348A (zh) 一种用于imu系统的姿态自适应校正方法
CN117029810A (zh) 一种基于迭代EKF的Mahony姿态测量方法
CN109947099B (zh) 一种基于事件触发机制的机器人控制方法及装置
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems
CN114061571B (zh) 一种自适应梯度下降惯性测量单元的姿态解算方法及系统
CN111189443A (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