CN111007863B - 一种机器人航向角的测量方法、装置及存储介质 - Google Patents
一种机器人航向角的测量方法、装置及存储介质 Download PDFInfo
- Publication number
- CN111007863B CN111007863B CN201911254139.8A CN201911254139A CN111007863B CN 111007863 B CN111007863 B CN 111007863B CN 201911254139 A CN201911254139 A CN 201911254139A CN 111007863 B CN111007863 B CN 111007863B
- Authority
- CN
- China
- Prior art keywords
- gyroscope
- robot
- angle
- course angle
- gyroscopes
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000004927 fusion Effects 0.000 claims abstract description 36
- 238000012937 correction Methods 0.000 claims abstract description 33
- 238000005259 measurement Methods 0.000 claims abstract description 33
- 238000011084 recovery Methods 0.000 claims description 23
- 238000012545 processing Methods 0.000 claims description 16
- 238000004590 computer program Methods 0.000 claims description 13
- 238000001914 filtration Methods 0.000 claims description 12
- 230000005484 gravity Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 claims description 3
- 238000000691 measurement method Methods 0.000 abstract description 3
- 230000005856 abnormality Effects 0.000 description 4
- 230000002159 abnormal effect Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000011900 installation process Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种机器人航向角的测量方法、装置及存储介质，所述方法包括：在每个采集间隔获取各个陀螺仪当前时刻的原始航向角、上一时刻的最终航向角以及机器人转向速度；根据转向速度指令采用融合算法计算机器人下一时刻的预测航向角；计算当前时刻的一步协方差；分别计算各个陀螺仪的原始航向角相互之间的最短距离、所有陀螺仪的距离差的总和以及各个陀螺仪的偏差惩罚因子；根据上述各个陀螺仪的测量数据依序对所述预测航向角和当前时刻的一步协方差进行迭代校正，得到机器人在当前时刻进行数据融合后的最终航向角。本发明能够有效解决基于单一陀螺仪的测量方式带来航向角不稳定的问题，提高了航向角测量方法的可靠性以及数据的可恢复性。
Description
技术领域
本发明涉及机器人导航控制技术领域，尤其涉及一种机器人航向角的测量方法、装置及存储介质。
背景技术
当前巡检机器人在进行自主室内导航时，需要测定自身的航向角以确定自身在环境中的位姿，航向角测定的准确与否直接决定机器人的定位准确性，因此需要设计一个高度可靠的航向角测量方法来确定机器人的航向角。
现有技术中机器人航向角的测定方法主要包括两种，第一种方法是基于单个陀螺仪计算航向角，第二种方法是基于单个陀螺仪和电子罗盘的数据融合计算航向角。基于单个陀螺仪计算航向角在开机的时候，静止一段时间进行均值测量，随后把测得的陀螺仪角速度数据减去这个均值，同时对测量的角速度信号进行高通滤波，过滤低频的温漂干扰，对滤波后的角速度数据进行积分从而得出机器人的航向角。第二种方法的开始过程和第一种的原理相似，在过滤低频的温漂干扰后，对角速度数据进行积分，但并不立即将该积分结果作为航向角的最终值，而是把该结果和电子罗盘获得的绝对地磁角度进行互补滤波，通过地磁角来校正航向角。
但是，在对现有技术的研究与实践的过程中，本发明的发明人发现，现有技术存在以下缺陷：由于现有方法均采用单一的航向角测量设备输出唯一的结果，一旦测量设备出现异常，将不可避免导致航向角的测量错误，数据容错性较低；同时，由于仅仅通过单一设备来测量航向角，当该测量设备的测量结果错误时，则会永久丢失当前机器人的航向角，导致由于缺乏航向角的真值无法对航向角进行校正恢复，无法为机器人恢复为正确的航向角。
发明内容
本发明实施例所要解决的技术问题在于，提供一种机器人航向角的测量方法、装置及存储介质，通过采用多个陀螺仪得出多个原始航向角，并提出基于惩罚因子的数据融合算法，对多个原始航向角进行数据融合滤波来获取最终的航向角。
为解决上述问题，本发明的一个实施例提供一种机器人航向角的测量方法，至少包括如下步骤：
在每一个采集间隔内获取各个陀螺仪当前时刻所测量的原始航向角、机器人上一时刻的最终航向角以及机器人当前的转向速度；
根据机器人所接收的转向速度指令，采用融合算法每隔一个预设时间段计算机器人下一时刻的预测航向角；
根据一步协方差公式计算机器人当前时刻的一步协方差；
分别计算各个陀螺仪的原始航向角相互之间的最短距离、所有陀螺仪的距离差的总和以及各个陀螺仪的偏差惩罚因子；
根据各个陀螺仪的测量数据和引入偏差惩罚因子的卡尔曼滤波算法，依序对所述预测航向角和当前时刻的一步协方差进行迭代校正，完成迭代校正后得到机器人在当前时刻进行数据融合后的最终航向角。
进一步的，所述机器人航向角的测量方法，还包括：
根据所述各个陀螺仪的原始航向角相互之间的最短距离判断是否需要陀螺仪自恢复处理。
进一步的，所述机器人航向角的测量方法，还包括：
在机器人底盘平面上竖直安装所述各个陀螺仪，以使各个陀螺仪的重心均在同一条铅垂线上。
进一步的，所述根据所述各个陀螺仪的原始航向角相互之间的最短距离判断是否需要陀螺仪自恢复处理，具体为：
在分别计算各个陀螺仪的原始航向角相互之间的最短距离时，若检测到陀螺仪的原始航向角与其它陀螺仪的最短距离大于预设阈值时，则判定该陀螺仪需要进行数据自恢复处理；
获取当前时刻进行数据融合后的最终航向角，连续10秒测量需要进行数据自恢复处理的陀螺仪当前时刻的原始航向角的平均值；
在下一采集间隔内获取该陀螺仪当前时刻所测量的原始航向角时，根据航向角校正公式计算该陀螺仪的原始航向角。
进一步的，所述陀螺仪的测量数据包括各个陀螺仪的原始航向角相互之间的最短距离、所有陀螺仪的距离差的总和、各个陀螺仪的偏差惩罚因子以及当前时刻的一步协方差。
进一步的，所述机器人当前的转向速度由上位机根据导航算法自动发送至机器人。
进一步的，所述迭代校正，具体为：
采用第一陀螺仪的测量数据对所述预测航向角进行校正，并根据第一陀螺仪的校正结果调整一步协方差；
采用第二陀螺仪的测量数据对第一陀螺仪校正后的预测航向角和一步协方差再次进行迭代校正；
重复上述步骤迭代校正，直至最后一个陀螺仪对其上一个陀螺仪校正后的预测航向角和一部协方差迭代校正完毕，得到当前时刻进行数据融合后的最终航向角和***协方差。
本发明的一个实施例提供了一种机器人航向角的测量装置，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如上述的机器人航向角的测量方法。
本发明的一个实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如上述的机器人航向角的测量方法。
实施本发明实施例，具有如下有益效果：
本发明实施例提供的一种机器人航向角的测量方法、装置及存储介质，所述方法包括如下步骤：在每一个采集间隔内获取各个陀螺仪当前时刻所测量的原始航向角、机器人上一时刻的最终航向角以及机器人当前的转向速度；根据机器人所接收的转向速度指令，采用融合算法每隔一个预设时间段计算机器人下一时刻的预测航向角；根据一步协方差公式计算机器人当前时刻的一步协方差；分别计算各个陀螺仪的原始航向角相互之间的最短距离、所有陀螺仪的距离差的总和以及各个陀螺仪的偏差惩罚因子；根据各个陀螺仪的测量数据和引入偏差惩罚因子的卡尔曼滤波算法，依序对所述预测航向角和当前时刻的一步协方差进行迭代校正，完成迭代校正后得到机器人在当前时刻进行数据融合后的最终航向角。
本发明能够通过采用多个陀螺仪得出多个原始航向角，并提出基于惩罚因子的数据融合算法，对多个原始航向角进行数据融合滤波来获取最终的航向角；通过多陀螺仪数据融合的处理，在其中一个陀螺仪出现问题时，能够通过当前融合结果得出的真值校正该出问题的陀螺仪数据，从而使有问题的陀螺仪的输出数据恢复正常，实现数据恢复的功能，有效地解决了在基于单一陀螺仪的测量方式中，由于陀螺仪数据偶然性异常，带来航向角测量的不稳定的问题，从而提高了航向角测量***的可靠性以及数据的可恢复性。
附图说明
图1是本发明实施例提供的一种机器人航向角的测量方法的流程示意图；
图2是本发明实施例提供的迭代校正的流程示意图；
图3是本发明实施例提供的陀螺仪数据自恢复的流程示意图；
图4是本发明实施例提供的另一种机器人航向角的测量方法的流程示意图；
图5是本发明实施例提供的一种陀螺仪结构的安装示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参阅图1-5。
如图1所示，本实施例提供了一种机器人航向角的测量方法，至少包括如下步骤：
S101、在每一个采集间隔内获取各个陀螺仪当前时刻所测量的原始航向角、机器人上一时刻的最终航向角以及机器人当前的转向速度。
具体的，对于步骤S101，获取当前时刻(k时刻)的三个陀螺仪的原始航向角
以及
获取上一时刻(k-1时刻)的最终航向角
获取当前机器人的转向速度vk。其中vk是由上位机根据导航算法自动发送的，所以在每一次上位机发送转向速度时，把该值记录下来即可。
S102、根据机器人所接收的转向速度指令，采用融合算法每隔一个预设时间段计算机器人下一时刻的预测航向角。
具体的，对于步骤S102，计算根据速度指令进行预测的航向角：
其中，融合算法每隔Δt运行一次，Δt推荐值为50ms。
S103、根据一步协方差公式计算机器人当前时刻的一步协方差。
具体的，对于步骤S103，根据如下公式计算一步协方差：
p1＝pk-1+state_q；
其中，pk-1为上一时刻的***协方差，state_q为***方差，初始值为1，p1为一步协方差。
S104、分别计算各个陀螺仪的原始航向角相互之间的最短距离、所有陀螺仪的距离差的总和以及各个陀螺仪的偏差惩罚因子。
具体的，对于步骤S104，在本实施例中，以计算三个陀螺仪的航向角相互间的最短距离为例，具体各个陀螺仪的航向角相互间的最短距离计算步骤如下：
1号陀螺仪航向角与其余陀螺仪航向角的最短距离：
2号陀螺仪航向角与其余陀螺仪航向角的最短距离：
3号陀螺仪航向角与其余陀螺仪航向角的最短距离：
因此，陀螺仪总的距离差为
在计算各个陀螺仪的航向角相互间的最短距离后，计算各个陀螺仪的偏差惩罚因子，其中，1号陀螺仪惩罚因子：
2号陀螺仪惩罚因子：
3号陀螺仪惩罚因子：
本实施例通过引入了各个陀螺仪的偏差惩罚因子，提高了***对于陀螺仪数据异常时的容错性，即使在陀螺仪数据出现问题时，也不会影响最终融合后的航向角，极大地提高了航向角测量的可靠性。
S105、根据各个陀螺仪的测量数据和引入偏差惩罚因子的卡尔曼滤波算法，依序对所述预测航向角和当前时刻的一步协方差进行迭代校正，完成迭代校正后得到机器人在当前时刻进行数据融合后的最终航向角。
在优选的实施例中，如图2所示，所述迭代校正，具体为：
采用第一陀螺仪的测量数据对所述预测航向角进行校正，并根据第一陀螺仪的校正结果调整一步协方差；
采用第二陀螺仪的测量数据对第一陀螺仪校正后的预测航向角和一步协方差再次进行迭代校正；
重复上述步骤迭代校正，直至最后一个陀螺仪对其上一个陀螺仪校正后的预测航向角和一部协方差迭代校正完毕，得到当前时刻进行数据融合后的最终航向角和***协方差。
具体的，对于步骤S105中的迭代校正，具体步骤如下：
首先采用1号陀螺仪的观测数据对步骤S102预测的航向角进行校正：
para_p＝p1+para_1；
根据1号陀螺仪校正的结果，调整一步协方差：p1＝(1-k)*p1。
接着，采用2号陀螺仪的观测数据对1号陀螺仪校正的航向角进行校正：
para_p＝p1+para_2；
根据2号陀螺仪校正的结果，调整一步协方差：p1＝(1-k)*p1。
最后，采用3号陀螺仪的观测数据对步骤2号陀螺仪校正的航向角进行校正：
para_p＝p1+para_3；
根据2号陀螺仪校正的结果，调整一步协方差：p1＝(1-k)*p1。
而这一步得出的一步协方差p1即为当前时刻***协方差：pk＝p1。
同时，这一步得出的
即为***在当前时刻的数据融合后的航向角，后续的导航则以该值作为机器人的航向角。本实施例提出利用多个陀螺仪测量多个陀螺仪的航向角并将多个航向角进行数据融合，得出可靠的最终航向角。并且在数据融合卡尔曼滤波方法的过程中，对各个陀螺仪的观测数据引入惩罚因子机制，实现对异常陀螺仪数据的干扰排除处理，实现最终的测量结果不会因为单个陀螺仪的异常数据而出现错误，进一步提高航向角测量的精准度和有效性。
在优选的实施例中，所述机器人航向角的测量方法，还包括：
根据所述各个陀螺仪的原始航向角相互之间的最短距离判断是否需要陀螺仪自恢复处理。
具体的，本实施例还提供了陀螺仪自恢复的技术方案，在其中部分陀螺仪数据出现问题的时候，会在该步骤进行对应陀螺仪数据校正，在校正完毕后，则会再次进行数据融合的循环检测流程。
在优选的实施例中，如图3所示，所述根据所述各个陀螺仪的原始航向角相互之间的最短距离判断是否需要陀螺仪自恢复处理，具体为：
在分别计算各个陀螺仪的原始航向角相互之间的最短距离时，若检测到陀螺仪的原始航向角与其它陀螺仪的最短距离大于预设阈值时，则判定该陀螺仪需要进行数据自恢复处理；
获取当前时刻进行数据融合后的最终航向角，连续10秒测量需要进行数据自恢复处理的陀螺仪当前时刻的原始航向角的平均值；
在下一采集间隔内获取该陀螺仪当前时刻所测量的原始航向角时，根据航向角校正公式计算该陀螺仪的原始航向角。
具体的，如图4所示，在数据融合流程的步骤S104(计算各陀螺仪航向角之间的最短距离)中，如果出现一个陀螺仪的原始航向角的最短距离大于一个阈值，则认为该陀螺仪遇到了故障，则需要进行数据自恢复。
在自恢复阶段，获取当前时刻的融合后的航向角wstate，然后连续10秒测量需要校正的陀螺仪当前的原始航向角的平均值werror：，在后续在数据融合流程中的步骤I获取该陀螺仪的原始航向角的时候，该陀螺仪的原始航向角的计算如下：
在记录下werror以及wstate时，自恢复阶段结束，继续进行数据融合的流程。
通过自恢复流程，实现在单个陀螺仪出现问题的时候，实时对有问题的陀螺仪进行修正，使其数据恢复正常，完成设备的自恢复。有效地解决了在基于单一陀螺仪的测量方式中，由于陀螺仪数据偶然性异常，带来航向角不稳定的问题，从而提高了航向角测量***的可靠性以及数据的可恢复性。
在优选的实施例中，所述机器人航向角的测量方法，还包括：
在机器人底盘平面上竖直安装所述各个陀螺仪，以使各个陀螺仪的重心均在同一条铅垂线上。
具体的，如图5所示，以3个陀螺仪为例子。在安装时，为了使每个陀螺仪的航向角一致，采用竖直安装的方式保证几个陀螺仪都在同一条铅垂线上。
在优选的实施例中，所述陀螺仪的测量数据包括各个陀螺仪的原始航向角相互之间的最短距离、所有陀螺仪的距离差的总和、各个陀螺仪的偏差惩罚因子以及当前时刻的一步协方差。
在优选的实施例中，所述机器人当前的转向速度由上位机根据导航算法自动发送至机器人。
本发明实施例提供的一种机器人航向角的测量方法，包括如下步骤：在每一个采集间隔内获取各个陀螺仪当前时刻所测量的原始航向角、机器人上一时刻的最终航向角以及机器人当前的转向速度；根据机器人所接收的转向速度指令，采用融合算法每隔一个预设时间段计算机器人下一时刻的预测航向角；根据一步协方差公式计算机器人当前时刻的一步协方差；分别计算各个陀螺仪的原始航向角相互之间的最短距离、所有陀螺仪的距离差的总和以及各个陀螺仪的偏差惩罚因子；根据各个陀螺仪的测量数据和引入偏差惩罚因子的卡尔曼滤波算法，依序对所述预测航向角和当前时刻的一步协方差进行迭代校正，完成迭代校正后得到机器人在当前时刻进行数据融合后的最终航向角。
本发明能够通过采用多个陀螺仪得出多个原始航向角，并提出基于惩罚因子的数据融合算法，对多个原始航向角进行数据融合滤波来获取最终的航向角；通过多陀螺仪数据融合的处理，在其中一个陀螺仪出现问题时，能够通过当前融合结果得出的真值校正该出问题的陀螺仪数据，从而使有问题的陀螺仪的输出数据恢复正常，实现数据恢复的功能，有效地解决了在基于单一陀螺仪的测量方式中，由于陀螺仪数据偶然性异常，带来航向角测量的不稳定的问题，从而提高了航向角测量***的可靠性以及数据的可恢复性。
本发明的一个实施例提供了一种机器人航向角的测量装置，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如上述的一种机器人航向角的测量方法。
本发明的另一个实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如上述的一种机器人航向角的测量方法。
在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述模块的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个模块或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。
所述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理模块，即可以位于一个地方，或者也可以分布到多个模块上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和变形，这些改进和变形也视为本发明的保护范围。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(Random AccessMemory，RAM)等。
Claims (9)
1.一种机器人航向角的测量方法，其特征在于，至少包括如下步骤：
在每一个采集间隔内获取各个陀螺仪当前时刻所测量的原始航向角、机器人上一时刻的最终航向角以及机器人当前的转向速度；
根据机器人所接收的转向速度指令，采用融合算法每隔一个预设时间段计算机器人下一时刻的预测航向角；
根据一步协方差公式计算机器人当前时刻的一步协方差；
分别计算各个陀螺仪的原始航向角相互之间的最短距离、所有陀螺仪的距离差的总和以及各个陀螺仪的偏差惩罚因子；
根据各个陀螺仪的测量数据和引入偏差惩罚因子的卡尔曼滤波算法，依序对所述预测航向角和当前时刻的一步协方差进行迭代校正，完成迭代校正后得到机器人在当前时刻进行数据融合后的最终航向角；
其中，所述各个陀螺仪的原始航向角相互之间的最短距离的计算如下：
1号陀螺仪航向角与其余陀螺仪航向角的最短距离：
；
2号陀螺仪航向角与其余陀螺仪航向角的最短距离：
；
3号陀螺仪航向角与其余陀螺仪航向角的最短距离：
；
因此，所有陀螺仪的距离差的总和为；
式中，为k时刻的1号陀螺仪的原始航向角，为k时刻的2号陀螺仪的原始航向角，为k时刻的3号陀螺仪的原始航向角；
在计算各个陀螺仪的原始航向角相互之间的最短距离后，计算各个陀螺仪的偏差惩罚因子，其中，1号陀螺仪惩罚因子：；2号陀螺仪惩罚因子：；3号陀螺仪惩罚因子：。
2.根据权利要求1所述的机器人航向角的测量方法，其特征在于，还包括：
根据所述各个陀螺仪的原始航向角相互之间的最短距离判断是否需要陀螺仪自恢复处理。
3.根据权利要求1所述的机器人航向角的测量方法，其特征在于，还包括：
在机器人底盘平面上竖直安装所述各个陀螺仪，以使各个陀螺仪的重心均在同一条铅垂线上。
4.根据权利要求2所述的机器人航向角的测量方法，其特征在于，所述根据所述各个陀螺仪的原始航向角相互之间的最短距离判断是否需要陀螺仪自恢复处理，具体为：
在分别计算各个陀螺仪的原始航向角相互之间的最短距离时，若检测到陀螺仪的原始航向角与其它陀螺仪的最短距离大于预设阈值时，则判定该陀螺仪需要进行数据自恢复处理；
获取当前时刻进行数据融合后的最终航向角，连续10秒测量需要进行数据自恢复处理的陀螺仪当前时刻的原始航向角的平均值；
在下一采集间隔内获取该陀螺仪当前时刻所测量的原始航向角时，根据航向角校正公式计算该陀螺仪的原始航向角。
5.根据权利要求1所述的机器人航向角的测量方法，其特征在于，所述陀螺仪的测量数据包括各个陀螺仪的原始航向角相互之间的最短距离、所有陀螺仪的距离差的总和、各个陀螺仪的偏差惩罚因子以及当前时刻的一步协方差。
6.根据权利要求1所述的机器人航向角的测量方法，其特征在于，所述机器人当前的转向速度由上位机根据导航算法自动发送至机器人。
7.根据权利要求1所述的机器人航向角的测量方法，其特征在于，所述迭代校正，具体为：
采用第一陀螺仪的测量数据对所述预测航向角进行校正，并根据第一陀螺仪的校正结果调整一步协方差；
采用第二陀螺仪的测量数据对第一陀螺仪校正后的预测航向角和一步协方差再次进行迭代校正；
重复上述迭代校正，直至最后一个陀螺仪对其上一个陀螺仪校正后的预测航向角和一部协方差迭代校正完毕，得到当前时刻进行数据融合后的最终航向角和***协方差。
8.一种机器人航向角的测量装置，其特征在于，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至7任一项所述的机器人航向角的测量方法。
9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如权利要求1至7任一项所述的机器人航向角的测量方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911254139.8A CN111007863B (zh) | 2019-12-06 | 2019-12-06 | 一种机器人航向角的测量方法、装置及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911254139.8A CN111007863B (zh) | 2019-12-06 | 2019-12-06 | 一种机器人航向角的测量方法、装置及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111007863A CN111007863A (zh) | 2020-04-14 |
| CN111007863B true CN111007863B (zh) | 2023-05-02 |
Family
ID=70114139
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911254139.8A Active CN111007863B (zh) | 2019-12-06 | 2019-12-06 | 一种机器人航向角的测量方法、装置及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111007863B (zh) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104713554A (zh) * | 2015-02-01 | 2015-06-17 | 北京工业大学 | 一种基于mems惯性器件与安卓智能手机融合的室内定位方法 |
| CN107218917A (zh) * | 2017-06-06 | 2017-09-29 | 吕文君 | 一种移动机器人航向角估计方法 |
| CN107272718A (zh) * | 2017-06-19 | 2017-10-20 | 歌尔科技有限公司 | 基于卡尔曼滤波的姿态控制方法及装置 |
| CN107817821A (zh) * | 2017-10-27 | 2018-03-20 | 成都鼎信精控科技有限公司 | 一种基于mems陀螺仪组合的稳定云台及控制方法 |
| CN108501768A (zh) * | 2018-03-29 | 2018-09-07 | 南京航空航天大学 | 一种基于z轴陀螺仪及轮速差的两轮速度控制方法 |
| CN109001787A (zh) * | 2018-05-25 | 2018-12-14 | 北京大学深圳研究生院 | 一种姿态角解算与定位的方法及其融合传感器 |
| CN109141410A (zh) * | 2018-07-25 | 2019-01-04 | 深圳市集大自动化有限公司 | Agv组合导航的多传感器融合定位方法 |
| CN109443349A (zh) * | 2018-11-14 | 2019-03-08 | 广州中海达定位技术有限公司 | 一种姿态航向测量***及其融合方法、存储介质 |
| CN109883423A (zh) * | 2019-02-25 | 2019-06-14 | 广州市香港科大***研究院 | 基于卡尔曼滤波的定位方法、***、设备及存储介质 |
| CN110231030A (zh) * | 2019-06-28 | 2019-09-13 | 苏州瑞久智能科技有限公司 | 基于陀螺仪的扫地机器人角度最优估算方法 |
| CN110515381A (zh) * | 2019-08-22 | 2019-11-29 | 浙江迈睿机器人有限公司 | 用于定位机器人的多传感器融合算法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6622091B2 (en) * | 2001-05-11 | 2003-09-16 | Fibersense Technology Corporation | Method and system for calibrating an IG/GP navigational system |
-
2019
- 2019-12-06 CN CN201911254139.8A patent/CN111007863B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104713554A (zh) * | 2015-02-01 | 2015-06-17 | 北京工业大学 | 一种基于mems惯性器件与安卓智能手机融合的室内定位方法 |
| CN107218917A (zh) * | 2017-06-06 | 2017-09-29 | 吕文君 | 一种移动机器人航向角估计方法 |
| CN107272718A (zh) * | 2017-06-19 | 2017-10-20 | 歌尔科技有限公司 | 基于卡尔曼滤波的姿态控制方法及装置 |
| CN107817821A (zh) * | 2017-10-27 | 2018-03-20 | 成都鼎信精控科技有限公司 | 一种基于mems陀螺仪组合的稳定云台及控制方法 |
| CN108501768A (zh) * | 2018-03-29 | 2018-09-07 | 南京航空航天大学 | 一种基于z轴陀螺仪及轮速差的两轮速度控制方法 |
| CN109001787A (zh) * | 2018-05-25 | 2018-12-14 | 北京大学深圳研究生院 | 一种姿态角解算与定位的方法及其融合传感器 |
| CN109141410A (zh) * | 2018-07-25 | 2019-01-04 | 深圳市集大自动化有限公司 | Agv组合导航的多传感器融合定位方法 |
| CN109443349A (zh) * | 2018-11-14 | 2019-03-08 | 广州中海达定位技术有限公司 | 一种姿态航向测量***及其融合方法、存储介质 |
| CN109883423A (zh) * | 2019-02-25 | 2019-06-14 | 广州市香港科大***研究院 | 基于卡尔曼滤波的定位方法、***、设备及存储介质 |
| CN110231030A (zh) * | 2019-06-28 | 2019-09-13 | 苏州瑞久智能科技有限公司 | 基于陀螺仪的扫地机器人角度最优估算方法 |
| CN110515381A (zh) * | 2019-08-22 | 2019-11-29 | 浙江迈睿机器人有限公司 | 用于定位机器人的多传感器融合算法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111007863A (zh) | 2020-04-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110806215B (zh) | 车辆定位的方法、装置、设备及存储介质 | |
| CN111721289A (zh) | 车辆定位方法、装置、设备、存储介质及车辆 | |
| CN108981687B (zh) | 一种视觉与惯性融合的室内定位方法 | |
| US20200286245A1 (en) | Posture estimation method, posture estimation apparatus and computer readable storage medium | |
| US7962255B2 (en) | System and method for estimating inertial acceleration bias errors | |
| CN111026081B (zh) | 一种误差计算方法、装置、设备及存储介质 | |
| CN116067370B (zh) | 一种imu姿态解算方法及设备、存储介质 | |
| CN112762944B (zh) | 零速区间检测及零速更新方法 | |
| EP4345421A2 (en) | Method for calibrating sensor parameters based on autonomous driving, apparatus, storage medium, and vehicle | |
| CN111024067B (zh) | 一种信息处理方法、装置、设备及计算机存储介质 | |
| CN107702710B (zh) | 一种多陀螺表头常值漂移实时估计方法 | |
| CN110986928A (zh) | 光电吊舱三轴陀螺仪漂移实时修正方法 | |
| CN111007863B (zh) | 一种机器人航向角的测量方法、装置及存储介质 | |
| CN113009816B (zh) | 时间同步误差的确定方法及装置、存储介质及电子装置 | |
| CN112797979B (zh) | 一种应用于agv的惯性姿态导航*** | |
| CN109917644B (zh) | 一种提高视觉惯导***鲁棒性的方法、装置和机器人设备 | |
| CN110674888B (zh) | 一种基于数据融合的头部姿态识别方法 | |
| CN109631892B (zh) | 一种imu数据中断的组合导航数据处理方法 | |
| CN112330727A (zh) | 图像匹配方法、装置、计算机设备及存储介质 | |
| CN116608853A (zh) | 载体动态姿态估计方法、设备、存储介质 | |
| CN111982179B (zh) | 异常检测设备、异常检测方法以及计算机可读介质 | |
| CN114739416A (zh) | 自动驾驶车辆定位方法、装置及电子设备、存储介质 | |
| CN114812575A (zh) | 一种修正参数的确定方法、装置、电子设备及存储介质 | |
| CN115237105A (zh) | 无人车定位异常检测方法、装置、存储介质及电子设备 | |
| Yun et al. | Reducing the computation time in the state chi-square test for IMU fault detection |
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 | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20240227 Address after: 511400 3rd Floor, No. 7 Guankeng Industrial Second Road, Guankeng Village, Dashi Street, Panyu District, Guangzhou City, Guangdong Province Patentee after: Guangzhou Shendi Intelligent Technology Co.,Ltd. Country or region after: China Address before: 510600 1st floor, No.2, Jinyang 1st Street, Zhongshan 1st Road, Yuexiu District, Guangzhou City, Guangdong Province Patentee before: GUANGZHOU SENDI COMPUTER SYSTEM CO.,LTD. Country or region before: China |