CN112462793A - 基于传感器的设备控制方法、装置、计算机设备 - Google Patents
基于传感器的设备控制方法、装置、计算机设备 Download PDFInfo
- Publication number
- CN112462793A CN112462793A CN201910848344.0A CN201910848344A CN112462793A CN 112462793 A CN112462793 A CN 112462793A CN 201910848344 A CN201910848344 A CN 201910848344A CN 112462793 A CN112462793 A CN 112462793A
- Authority
- CN
- China
- Prior art keywords
- data
- pass filter
- acceleration
- low
- equipment
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 230000001133 acceleration Effects 0.000 claims abstract description 177
- 206010034719 Personality change Diseases 0.000 claims abstract description 84
- 230000004927 fusion Effects 0.000 claims abstract description 78
- 238000001914 filtration Methods 0.000 claims abstract description 50
- 230000000295 complement effect Effects 0.000 claims abstract description 38
- 230000005484 gravity Effects 0.000 claims abstract description 33
- 238000004590 computer program Methods 0.000 claims description 23
- 230000008859 change Effects 0.000 description 15
- 238000010586 diagram Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 239000000126 substance Substances 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000001939 inductive effect Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000004044 response Effects 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
- G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles
-
- 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)
- Aviation & Aerospace Engineering (AREA)
- Gyroscopes (AREA)
Abstract
本申请涉及一种基于传感器的设备控制方法、装置、计算机设备和存储介质。所述方法包括:获取加速度传感器测量的设备的加速度数据及角度传感器测量的设备的角速度数据;根据加速度数据和重力加速度,确定设备的当前姿态变化状态;根据当前姿态变化状态,调整高通滤波器和低通滤波器的比例系数;根据调整比例系数后的高通滤波器和低通滤波器对加速度数据和角速度数据进行互补滤波,得到姿态融合数据;根据姿态融合数据控制所述设备调整姿态。采用本方法能够提高数据的准确性。
Description
技术领域
本申请涉及惯性导航技术领域,特别是涉及一种基于传感器的设备控制方法、装置、计算机设备和存储介质。
背景技术
惯性导航设备是一种使用惯性导航测量姿态的设备,比如外骨骼机器人、无人机以及平衡车等。惯性导航是一种融合了多种传感器的导航方案,是一种不依赖外部信息,也不向外部辐射能量的自助式导航系统,主要通过设置在设备上的传感器测量的数据进行姿态的控制,例如,测量姿态常用的传感器有加速度计和陀螺仪。其中,加速度计是一种受高频噪声影响、极易受到外部干扰的传感器,且只能得到二维的角度关系,但是加速度计的测量值随时间的变化相对较小。而陀螺仪是一种受低频噪声影响、受外部干扰较小的传感器,且可以积分得到三维的角度关系,但是测量值随时间变化比较大。因此,传统为了得到更好的导航效果,利用速度计和陀螺仪的优缺点互补,将二者的数据融合起来,基于融合得到的数据进行姿态的控制。
然而,传统进行数据融合时,所使用的互补滤波算法主要是通过加速度计和陀螺仪之间的差异给出输出结果,没有考虑到设备在运动时所受到的噪声干扰,从而导致输出的结果不准确。
发明内容
基于此,有必要针对上述技术问题,提供一种能够提高准确性的基于传感器的设备控制方法、装置、计算机设备和存储介质。
一种基于传感器的设备控制方法,所述方法包括:
获取加速度传感器测量的设备的加速度数据及角速度传感器测量的设备的角速度数据;
根据所述加速度数据和重力加速度,确定设备的当前姿态变化状态;
根据所述当前姿态变化状态,调整高通滤波器和低通滤波器的比例系数;
根据调整所述比例系数后的高通滤波器和低通滤波器对所述加速度数据和所述角速度数据进行互补滤波,得到姿态融合数据;
根据所述姿态融合数据控制所述设备调整姿态。
在其中一个实施例中,根据所述加速度数据和重力加速度,确定设备的当前姿态变化状态,包括:
当所述加速度数据与所述重力加速度的差异大于或等于阈值时,确定设备处于高频姿态变化状态;
当所述加速度数据与所述重力加速度的差异小于阈值时,确定设备处于低频姿态变化状态。
在其中一个实施例中,根据所述当前姿态变化状态,调整滤波器的比例系数,包括:
当设备处于高频姿态变化状态时,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数大于所述低通滤波器的比例系数。
在其中一个实施例中,根据所述当前姿态变化状态,调整高通滤波器和低通滤波器的比例系数,包括:
当设备处于低频姿态变化状态时,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数小于所述低通滤波器的比例系数。
在其中一个实施例中,根据调整所述比例系数后的高通滤波器和低通滤波器对所述加速度数据和所述角速度数据进行互补滤波,得到姿态融合数据,包括:
通过调整所述比例系数后的低通滤波器对所述加速度数据进行滤波,以及通过调整所述比例系数后的高通滤波器对所述角速度数据进行滤波;
将滤波之后的所述加速度数据和所述角速度数据进行融合,得到姿态融合数据
在其中一个实施例中,根据所述姿态融合数据控制所述设备调整姿态,包括:
对所述姿态融合数据进行拉普拉斯逆变换,得到所述姿态融合数据对应的姿态时域数据;
根据所述姿态时域数据控制所述设备调整姿态。
一种基于传感器的设备控制装置,所述装置包括:
获取模块,用于获取加速度传感器测量的设备的加速度数据及角度传感器测量的设备的角速度数据;
确定模块,用于根据所述加速度数据和重力加速度,确定设备的当前姿态变化状态;
调整模块,用于根据所述当前姿态变化状态,调整高通滤波器和低通滤波器的比例系数;
融合模块,用于根据调整所述比例系数后的高通滤波器和低通滤波器对所述加速度数据和所述角速度数据进行互补滤波,得到姿态融合数据;
控制模块,用于根据所述姿态融合数据控制所述设备调整姿态。
在其中一个实施例中,所述确定模块还用于当所述加速度数据与所述重力加速度的差异大于或等于阈值时,确定设备处于高频姿态变化状态;当所述加速度数据与所述重力加速度的差异小于阈值时,确定设备处于低频姿态变化状态。
一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时实现上述任意一项所述的基于传感器的设备控制方法的步骤。
一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现上述任意一项所述的基于传感器的设备控制方法的步骤。
上述基于传感器的设备控制方法、装置、计算机设备和存储介质,由于设备快速运动时,高频噪声对输出的结果影响较大,因此该方法利用实际获取的加速度传感器测量加速度数据和重力加速度确定设备运动时的当前姿态变化状态,从而基于当前姿态变化状态对用于进行互补滤波的滤波器的比例系数进行调整。该方法由于结合设备实际的运动情况,将数据融合所使用的滤波器的比例系数进行动态的调整,从而在设备姿态快速变化的场合也能更好更快地给出最优的输出,从而保证了输出数据的准确性。
附图说明
图1为一个实施例中基于传感器的设备控制方法的流程示意图;
图2为一个实施例中根据调整比例系数后的高通滤波器和低通滤波器对加速度数据和角速度数据进行互补滤波,得到姿态融合数据步骤的流程示意图;
图3为一个实施例中互补滤波数据流向的示意图;
图4为一个实施例中基于传感器的设备控制装置的结构框图;
图5为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
本申请提供的基于传感器的设备控制方法,可以应用于惯性导航设备中,惯性导航设备是一种具备惯性导航功能的设备,包括用于测量数据的传感器和处理数据的处理器。其中,当惯性导航设备中用于测量设备姿态数据的加速度传感器测量得到加速度数据、以及角速度传感器测量得到角速度数据之后,可以通过惯性导航设备中的处理器,根据测量的加速度数据和角速度数据单独实现基于传感器的设备控制方法。设备姿态是指设备围绕空间坐标系旋转的角度,和设备坐标一起,可以唯一确定设备在坐标系中的状态。以无人机为例,无人机的俯仰角度、倾斜角度等角度的变化都可以决定无人机不同的姿态。即,当惯性导航设备的处理器获取由加速度传感器测量到的加速度数据及角速度传感器测量到的角速度数据之后,根据加速度数据和重力加速度,确定设备的当前姿态变化状态;根据当前姿态变化状态,调整高通滤波器和低通滤波器的比例系数;根据调整比例系数后的高通滤波器和低通滤波器对加速度数据和角速度数据进行互补滤波,得到姿态融合数据;根据姿态融合数据控制设备调整姿态。其中,惯性导航设备110包括但不限于是各种无人机、外骨骼机器人以及平衡车等。
在一个实施例中,如图1所示,提供了一种基于传感器的设备控制方法,以该方法应用于惯性导航设备为例进行说明,包括以下步骤:
步骤S102,获取加速度传感器测量的设备的加速度数据及角速度传感器测量的设备的角速度数据。
其中,加速度传感器是一种能够测量设备加速度的传感器,加速度数据则是加速度传感器测量得到的数据。加速度传感器通常由质量块、阻尼器、弹性元件、敏感元件适调电路等部分组成。加速度传感器在加速过程中,通过对质量块所受惯性力的测量,利用牛顿第二定律获取加速度值。根据传感器敏感元件的不同,常见的加速传感器包括电容式、电感式、应变式、压阻式以及压电式等。在本实施例中,加速度传感器可以理解为惯性导航设备中使用的加速度计。
角速度传感器是一种能够测量角速度的传感器,角速度数据则是角速度传感器测量得到的数据。角速度传感器又称为陀螺仪,与加速度计不同的是,陀螺仪是用高速回转体的动量矩敏感壳体相对惯性空间绕正交与自转轴的一个或二个轴的角运动检测装置。应当理解,利用其它原理制成的角运动检测装置起同样功能的也能称为陀螺仪。
具体地,当惯性导航设备中用于测量姿态数据的加速度传感器测量到加速度数据,角速度传感器测量到角速度数据之后,通过惯性导航设备中的处理器获取加速度传感器所测的加速度数据和角速度传感器测量的角速度数据。例如,在实际应用中,当惯性导航设备运动时,通过加速度传感器和角速度传感器实时测量当前运动状态下的加速度数据和角速度数据,并将测量得到的加速度数据和角速度数据传输给惯性导航设备中的处理器进行处理。应当理解,由于噪声的原因,即使惯性导航设备处于静止状态,加速度传感器和角速度传感器同样能测得一定的数据,即噪声数据。如加速度计的高频噪声影响和陀螺仪的低频噪声。以加速度计和陀螺仪为例,加速度数据和角速度数据数测量数据公式如下:
ya=y+μh
yg=y+μl
其中,ya表示加速度计测量得到的加速度数据,yg表示陀螺仪测量得到的角速度数据。y表示传感器测量的真实数据,真实数据可以理解为没有噪声影响的数据。μh表示高频噪声,μl表示低频噪声。
步骤S104,根据加速度数据和重力加速度,确定设备的当前姿态变化状态。
其中,重力加速度是物理学中的物理量,是一个物体受重力作用的情况下所具有的加速度,也称为自由落体加速度。重力加速度通常用G表示,且为了便于计算,其近似标准值通常区域为9.8米/秒的二次方。姿态变化状态用于表示惯性导航设备当前的运动是否剧烈,可以理解为表示惯性导航设备当前是否处于快速运动的状态。
具体地,当获取到加速度传感器和角速度传感器,即加速度计和陀螺仪测量得到的加速度数据和角速度数据之后,根据加速度数据和重力加速度G之间的差异确定惯性导航设备的当前姿态变化状态。加速度数据和重力加速度G之间的差异越大,则表示设备的运动越剧烈,即姿态变换越剧烈。然后,根据惯性导航设备当前运动的状态,对测量得到的加速度数据和角速度数据进行融合。
步骤S106,根据当前姿态变化状态,调整高通滤波器和低通滤波器的比例系数。
其中,由于加速度计和陀螺仪受不同的频率的噪声影响,加速度计通常受高频噪声影响,而陀螺仪通常受低频噪声影响,因此互补滤波需要用到两个滤波器,即高通滤波器和低通滤波器。高通滤波器又称低截止滤波器或低阻滤波器,是允许高于某一截止频率的频率信号通过,而大大衰减较低频率的一种滤波器。可以理解为,高通滤波器去掉了信号中不必要的低频成分的干扰。低通滤波器则与高通滤波器相反,低通滤波器是容许低于某一截止频率的频率信号通过,而高于截止频率的频率信号不同通过。高通滤波器和低通滤波器的表达形式如下:
其中,Fh(s)表示高通滤波器,Fl(s)表示低通滤波器,s表示频域状态下的自变量,C(s)为滤波器的多项式,滤波器的多项式是决定滤波器阶数的参数。比例系数则是指滤波器中多项式的系数。由于加速度数据和角速度数据进行数据融合时通常采用滤波器进行互补滤波的方式,且滤波器中的参数是数据融合所用到的融合参数,因此,数据融合输出的结果会随着滤波器中参数的变化而变化,即随着融合参数的变化而变化。
具体地,当根据加速度传感器和重力加速度G确定好惯性导航设备的当前姿态变化状态时,根据姿态变化状态对互补滤波所用到滤波器中的比例系数进行调整。也就是说,当得到当前姿态变化状态时,根据姿态变化状态分别对高通滤波器的比例系数、低通滤波器的比例系数进行调整。比例系数调整完成之后,根据已经调整比例系数的高通滤波器和低通滤波器进行互补滤波。即,根据惯性导航设备的当前运动状态,如静止状态、高频姿态变化状态和低频姿态变化状态,实时将比例系数调整到与当前变化状态相匹配的值。当处于静止状态时,将高通滤波器的比例系数调整为0,低通滤波器的比例系数为1。而当设备处于高频姿态变化状态时,将高通滤波器的比例系数调整到大于低通滤波器的比例系数。而当设备处于低频姿态变化状态时,将高通滤波器的比例系数调整到小于低通滤波器的比例系数。其中,高通滤波器的比例系数与低通滤波器的比例系数的和为1。也就是说,若调整后高通滤波器的比例系数为0.6,则低通滤波器的比例系数即为0.4、调整后高通滤波器的比例系数为0.9,则低通滤波器的比例系数即为0.1,具体的值应当根据设备的实际情况确定。依据设备的当前状态调整用于数据融合的高通滤波器和低通滤波器的比例系数,从而改变加速度数据和角速度数据在数据融合中所占的比重,使得数据融合时能够更加符合当前的运行情况,提高数据的准确性和处理速度。
步骤S108,根据调整比例系数后的高通滤波器和低通滤波器对加速度数据和角速度数据进行互补滤波,得到姿态融合数据。
其中,互补滤波是指多组数据结合互补,并进行滤波处理稳定输出,得到姿态的算法。而我们使用的传感器就是加速度计和陀螺仪,由于加速度计的静态稳定性更好,而在运动时数据相对不可靠。陀螺仪的动态稳定性更好,但是静止时的数据相对不可靠,所以通过可以通过加速度计输出的数据来修正陀螺仪的漂移误差,即通过加速度计来修正陀螺仪。简单来说,由于加速度计和陀螺仪正好优缺点互补,因此为了得到更好的输出效果,通常将加速度计输出的数据和陀螺仪输出的数据进行数据融合。姿态融合数据即为进行互补滤波之后得到的数据。
具体地,当将高通滤波器以及低通滤波器的比例系数调整之后,利用已调整比例系数的高通滤波器对加速度数据进行过滤,以及利用已调整比例系数的高通滤波器对陀螺仪所测的角速度数据进行过滤。然后,将过滤后的数据进行融合,得到姿态融合数据。
步骤S110,根据姿态融合数据控制设备调整姿态。
具体地,姿态融合数据可以理解为设备当前的姿态数据,当惯性导航设备得到姿态融合数据后,根据该姿态融合数据即可调整设备的姿态进行导航。例如,通过姿态融合数据可以得到惯性导航设备当前的姿态角度,然后结合遥控指令驱动惯性导航设备的电机调整姿态角,从而实现不同方位的导航。
上述基于传感器的设备控制方法,由于设备快速运动时,高频噪声对输出的结果影响较大,因此该方法利用实际获取的加速度传感器测量加速度数据和重力加速度确定设备运动时的当前姿态变化状态,从而基于当前姿态变化状态对用于进行互补滤波的滤波器的比例系数进行调整。该方法由于结合设备实际的运动情况,将数据融合所使用的滤波器的比例系数进行动态的调整,从而在设备姿态快速变化的场合也能更好更快地给出最优的输出,从而保证了输出数据的准确性。
在一个实施例中,根据加速度数据和重力加速度,确定设备的当前姿态变化状态,具体包括:当加速度数据与重力加速度的差异大于或等于阈值时,确定设备处于高频姿态变化状态;当加速度数据与重力加速度的差异小于阈值时,确定设备处于低频姿态变化状态。
其中,高频姿态变化状态是指设备处于运行比较剧烈,可以理解为处于姿态变化较为快速的状态。而低频姿态变化状态是指设备处于运行较为稳定,可以理解为处于姿态变化较为缓慢的状态。而阈值即是用于衡量当前姿态变化状态是处于高频还是低频的状态的预设值,可以根据惯性导航设备的实际情况进行设定。
具体地,当根据加速度数据和重力加速度,确定设备的当前姿态变化状态时,将加速度数据和重力加速度进行差值计算,得到差值。得到的差值即可表示当前的差异大小。将得到的差值与阈值进行比较,当差值大于等于阈值时,表示当前设备处于一个比较剧烈的运动状态,即可确定设备处于高频姿态变化状态。而当差值小于阈值时,表示当前设备处于一个比较稳定的运动状态,即可确定设备处于低频姿态变化状态。其中,若以Ya(t)表示时域下的加速度数据,G表示重力加速度,D(t)表示当前姿态变化状态,计算公式如下:
D(t)=Ya(t)-G
另外,由于传统互补滤波的输出结果与输入信号(传感器测量得到的数据)之间是线性关系,即传统互补滤波的输出结果与输入信号存在一次方函数关系。简单来说,若将输出结果与输入信号这两个变量分别作为横坐标和纵坐标,其构成的图像是一条直线。因此,传统互补滤波算法在姿态变化较为快速的场景下,无法兼顾数据的准确性和响应速度。所以,在计算加速度数据和重力加速度之间的差异时,可通过非线性函数构造具备非线性关系的差值D(t)。当后续根据姿态变化状态进行参数调整时,使得输出结果根据输入信号的不同有不同的值。在本实施例中,若以自然底数e构造非线性关系式,非线性关系式如下:
其中,t表示当前数据为时域状态下,D(t)表示当前姿态变化状态,e为自然底数,Ya(t)表示时域下的加速度数据,G表示重力加速度。
当加速度计测量得到加速度数据之后,通过非线性关系式计算加速度数据和重力加速度之间的差异,得到能够表达设备当前姿态变化状态的模值D(t),当计算得到的模值D(t)小于1时,模值D(t)取原本计算的值,即取值处于0-1之间。而当计算得到的模值D(t)大于等于1,模值D(t)取值为1,根据该模值D(t)即可调整滤波器中参数的比例系数。其中,当模值D(t)取值为1时,即表示设备当前处于非常剧烈的变化状态中,可以理解为处于高频姿态变化状态。而当模值D(t)取值为0-1之间时,相对取值为1来说,取值0-1表示的则是设备处于低频姿态变化状态。应当理解的是,由于从0到1的过程表示从无到有,所以当取值为0到1时可以表示设备是从静止到运动的状态。
由于惯性导航设备大多数为嵌入式设备,所以计算能力通常有限,因此为了便于简化计算降低设备的计算量,本实施例优选以自然底数e构造非线性关系式。即当通过自然底数e构造得到非线性关系式后,为了减少设备负担,通过简化计算将上述非线性关系式进行泰勒展开并去掉高次项,使得D(t)近似线性化,通过D(t)的大小即可近似判断设备当前姿态变化状态。其中,非线性关系式简化后如下:
在本实施例中,通过加速度数据和重力加速度确定设备的状态,从而能够准确进行当前系统状态的判断。并通过自然底数e构造非线性关系式,从而非线性化输出数据与输入信号之间的关系,保证输出数据的准确性。
在一个实施例中,根据当前姿态变化状态,调整滤波器的比例系数,具体包括:当设备处于高频姿态变化状态时,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数大于低通滤波器的比例系数。当设备处于低频姿态变化状态时,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数小于低通滤波器的比例系数。
具体地,由于加速度计受高频噪声影响,陀螺仪受低频噪声影响。当设备处于高频姿态变化状态时,主要以陀螺仪测量的角速度数据为主。也就是说,要降低高频噪声的影响。因此,通过模值D(t)对高通滤波器和低通滤波器进行参数的比例系数调整后,高通滤波器的比例系数要大于低通滤波器的比例系数。因为高通滤波器是用于过滤陀螺仪的数据,而低通滤波器的是用于过滤加速度计的数据,因此,提高高通滤波器中参数的比例系数,从而提高陀螺仪所测量的角速度数据的比重,使得互补滤波时主要以陀螺仪测量的角速度数据为主。
当设备处于低频姿态变化状态时,主要以加速度计测量的加速度数据为主。也就是说,要降低低频噪声的影响。因此,通过模值D(t)对高通滤波器和低通滤波器进行参数的比例系数调整后,高通滤波器的比例系数要小于低通滤波器的比例系数。同样是因为高通滤波器是用于过滤陀螺仪的数据,而低通滤波器的是用于过滤加速度传计的数据,因此,提高低通滤波器中参数的比例系数,从而提高加速度计测量的加速度数据的比重,使得互补滤波时主要以加速度测量的加速度数据为主。利用模值D(t)对高通滤波器和低通滤波器进行参数的比例系数调整的表达式如下:
C(s)→C(s,1-D(s))
由于传感器测量得到的数据进行处理时均是频域状态下,所以D(s)是D(t)对应的频域状态下的值。在本实施例中,通过用于表示当前姿态变化状态的模值D(t)动态调整高通滤波器和低通滤波器的比例系数,从而根据实际运动情况调整加速度数据和陀螺仪数据的比重,使得数据互补滤波时更加符合实际运动的情况,提高数据的准确性。
在一个实施例中,如图2所示,根据调整比例系数后的高通滤波器和低通滤波器对加速度数据和角速度数据进行互补滤波,得到姿态融合数据,包括以下步骤:
步骤S202,通过调整比例系数后的低通滤波器对加速度数据进行滤波,以及通过调整比例系数后的高通滤波器对角速度数据进行滤波。
步骤S204,将滤波之后的加速度数据和角速度数据进行融合,得到姿态融合数据。
具体地,传统当得到加速度数据和角速度数据之后,同样是通过低通滤波器对加速度数据进行滤波,通过高通滤波器对角速度数据进行滤波。然后,将滤波后的数据进行融合,得到姿态融合数据,即完成互补滤波。姿态融合数据是指数据融合后得到数据,即互补滤波后得到的数据。互补滤波公式如下:
其中,表示互补滤波输出的数据,Ya(s)表示测量得到加速度数据,Fl(s)表示低通滤波器,Yg(s)表示测量得到角速度数据,Fh(s)表示高通滤波器,Y(s)表示真实数据,s表示频域状态下,可以理解为频域下的自变量。如图3所示,提供一种互补滤波数据流向的示意图。即经过高通滤波器Fh(s)和低通滤波器Fl(s)表达形式的简化后,传统互补滤波公式表达如下:
而在本实施例中,由于已经根据模值D(t)对高通滤波器和低通滤波器的参数,即多项式C(s)的比例系数进行调整,则通过调整比例系数后的低通滤波器和高通滤波器进行互补滤波的公式如下:
另外,由于滤波器制造出厂时,通常都预先设定好只会包含比例系数P,即需要调整的比例系数只有P。当滤波器的参数C(s)仅包含比例系数P时,通过调整比例系数后的低通滤波器和高通滤波器进行互补滤波的公式如下:
由此可得,若当设备处于非常剧烈的高频姿态变化状态时,D(s)取值为1,以D(s)=1为例,则通过D(s)=1即可将高通滤波器的比例系数调整为0,则使得受高频噪声影响的加速度数据权重为0。也就是说,由于比例系数调整的原因,在数据融合中加速度数据所占的比重为0,可以理解为已经将加速度的数据忽略不计,全以陀螺仪的数据为主。在本实施例中,通过比例系数调整后的滤波器对测量得到的数据进行数据融合,从而能够得到准确的输出数据,提高数据的准确性。
在一个实施例中,根据姿态融合数据控制设备调整姿态,包括:对姿态融合数据进行拉普拉斯逆变换,得到姿态融合数据对应的姿态时域数据;根据姿态时域数据控制设备调整姿态。
具体地,由于互补滤波的计算为频域s状态下,而惯性导航设备根据数据进行控制时,所需的数据是时域t状态的数据。因为,为便于惯性导航设备进行数据的使用,通常得到姿态融合数据后,通过拉普拉斯逆变换将频域下的姿态融合数据转换为时域下的姿态融合数据,从而惯性导航设备根据时域下的姿态融合数据进行姿态的控制。时域转换表达式如下:
应该理解的是,虽然图1-2的流程图中的各个步骤按照箭头的指示依次显示,但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明,这些步骤的执行并没有严格的顺序限制,这些步骤可以以其它的顺序执行。而且,图1-2中的至少一部分步骤可以包括多个子步骤或者多个阶段,这些子步骤或者阶段并不必然是在同一时刻执行完成,而是可以在不同的时刻执行,这些子步骤或者阶段的执行顺序也不必然是依次进行,而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中,如图4所示,提供了一种基于传感器的设备控制装置,包括:获取模块402、确定模块404、调整模块406、融合模块408和控制模块410,其中:
获取模块402,用于获取加速度传感器测量的设备的加速度数据及角度传感器测量的设备的角速度数据。
确定模块404,用于根据加速度数据和重力加速度,确定设备的当前姿态变化状态。
调整模块406,用于根据当前姿态变化状态,调整高通滤波器和低通滤波器的比例系数。
融合模块408,用于根据调整比例系数后的高通滤波器和低通滤波器对加速度数据和角速度数据进行互补滤波,得到姿态融合数据。
控制模块410,用于根据姿态融合数据控制所述设备调整姿态。
在一个实施例中,确定模块404还用于当加速度数据与重力加速度的差异大于或等于阈值时,确定设备处于高频姿态变化状态;当加速度数据与重力加速度的差异小于阈值时,确定设备处于低频姿态变化状态。
在一个实施例中,调整模块406还用于当设备处于高频姿态变化状态时,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数大于低通滤波器的比例系数。
在一个实施例中,调整模块406还用于当设备处于低频姿态变化状态时,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数小于低通滤波器的比例系数。
在一个实施例中,融合模块408还用于通过调整比例系数后的低通滤波器对加速度数据进行滤波,以及通过调整比例系数后的高通滤波器对角速度数据进行滤波;将滤波之后的加速度数据和角速度数据进行融合,得到姿态融合数据。
在一个实施例中,控制模块410还用于对姿态融合数据进行拉普拉斯逆变换,得到姿态融合数据对应的姿态时域数据;根据姿态时域数据控制设备调整姿态。
关于基于传感器的设备控制装置的具体限定可以参见上文中对于基于传感器的设备控制方法的限定,在此不再赘述。上述基于传感器的设备控制装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中,也可以以软件形式存储于计算机设备中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中,提供了一种计算机设备,该计算机设备可以是惯性导航设备,其内部结构图可以如图5所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和传感器。其中,该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的网络设备通过网络连接通信。该计算机程序被处理器执行时以实现一种基于传感器的设备控制方法。
本领域技术人员可以理解,图5中示出的结构,仅仅是与本申请方案相关的部分结构的框图,并不构成对本申请方案所应用于其上的计算机设备的限定,具体的计算机设备可以包括比图中所示更多或更少的部件,或者组合某些部件,或者具有不同的部件布置。
在一个实施例中,提供了一种计算机设备,包括存储器和处理器,存储器中存储有计算机程序,该处理器执行计算机程序时实现以下步骤:
获取加速度传感器测量的设备的加速度数据及角度传感器测量的设备的角速度数据;
根据加速度数据和重力加速度,确定设备的当前姿态变化状态;
根据当前姿态变化状态,调整高通滤波器和低通滤波器的比例系数;
根据调整比例系数后的高通滤波器和低通滤波器对加速度数据和角速度数据进行互补滤波,得到姿态融合数据;
根据姿态融合数据控制所述设备调整姿态。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:
当加速度数据与重力加速度的差异大于或等于阈值时,确定设备处于高频姿态变化状态;当加速度数据与重力加速度的差异小于阈值时,确定设备处于低频姿态变化状态。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:
当设备处于高频姿态变化状态时,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数大于低通滤波器的比例系数。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:
当设备处于低频姿态变化状态时,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数小于低通滤波器的比例系数。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:
通过调整比例系数后的低通滤波器对加速度数据进行滤波,以及通过调整比例系数后的高通滤波器对角速度数据进行滤波;将滤波之后的加速度数据和角速度数据进行融合,得到姿态融合数据。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:
对姿态融合数据进行拉普拉斯逆变换,得到姿态融合数据对应的时域数据;根据时域数据控制设备调整姿态。
在一个实施例中,提供了一种计算机可读存储介质,其上存储有计算机程序,计算机程序被处理器执行时实现以下步骤:
获取加速度传感器测量的设备的加速度数据及角度传感器测量的设备的角速度数据;
根据加速度数据和重力加速度,确定设备的当前姿态变化状态;
根据当前姿态变化状态,调整高通滤波器和低通滤波器的比例系数;
根据调整比例系数后的高通滤波器和低通滤波器对加速度数据和角速度数据进行互补滤波,得到姿态融合数据;
根据姿态融合数据控制所述设备调整姿态。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:
当加速度数据与重力加速度的差异大于或等于阈值时,确定设备处于高频姿态变化状态;当加速度数据与重力加速度的差异小于阈值时,确定设备处于低频姿态变化状态。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:
当设备处于高频姿态变化状态,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数大于低通滤波器的比例系数。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:
当设备处于低频姿态变化状态,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数小于低通滤波器的比例系数。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:
通过调整比例系数后的低通滤波器对加速度数据进行滤波,以及通过调整比例系数后的高通滤波器对角速度数据进行滤波;将滤波之后的加速度数据和角速度数据进行融合,得到姿态融合数据。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:
对姿态融合数据进行拉普拉斯逆变换,得到姿态融合数据对应的姿态时域数据;根据姿态时域数据控制设备调整姿态。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一非易失性计算机可读取存储介质中,该计算机程序在执行时,可包括如上述各方法的实施例的流程。其中,本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用,均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限,RAM以多种形式可得,诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (10)
1.一种基于传感器的设备控制方法,所述方法包括:
获取加速度传感器测量的设备的加速度数据及角速度传感器测量的设备的角速度数据;
根据所述加速度数据和重力加速度,确定设备的当前姿态变化状态;
根据所述当前姿态变化状态,调整高通滤波器和低通滤波器的比例系数;
根据调整所述比例系数后的高通滤波器和低通滤波器对所述加速度数据和所述角速度数据进行互补滤波,得到融合数据;
根据所述姿态融合数据控制所述设备调整姿态。
2.根据权利要求1所述的方法,其特征在于,根据所述加速度数据和重力加速度,确定设备的当前姿态变化状态,包括:
当所述加速度数据与所述重力加速度的差异大于或等于阈值时,确定设备处于高频姿态变化状态;
当所述加速度数据与所述重力加速度的差异小于阈值时,确定设备处于低频姿态变化状态。
3.根据权利要求2所述的方法,其特征在于,根据所述当前姿态变化状态,调整滤波器的比例系数,包括:
当设备处于高频姿态变化状态时,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数大于所述低通滤波器的比例系数。
4.根据权利要求2所述的方法,其特征在于,根据所述当前姿态变化状态调整高通滤波器和低通滤波器的比例系数,包括:
当设备处于低频姿态变化状态时,分别调整高通滤波器和低通滤波器的比例系数,其中,调整后高通滤波器的比例系数小于所述低通滤波器的比例系数。
5.根据权利要求1所述的方法,其特征在于,根据调整所述比例系数后的高通滤波器和低通滤波器对所述加速度数据和所述角速度数据进行互补滤波,得到姿态融合数据,包括:
通过调整所述比例系数后的低通滤波器对所述加速度数据进行滤波,以及通过调整所述比例系数后的高通滤波器对所述角速度数据进行滤波;
将滤波之后的所述加速度数据和所述角速度数据进行融合,得到姿态融合数据。
6.根据权利要求1所述的方法,其特征在于,根据所述姿态融合数据控制所述设备调整姿态,包括:
对所述姿态融合数据进行拉普拉斯逆变换,得到所述姿态融合数据对应的姿态时域数据;
根据所述姿态时域数据控制所述设备调整姿态。
7.一种基于传感器的设备控制装置,其特征在于,所述装置包括:
获取模块,用于获取加速度传感器测量的设备的加速度数据及角度传感器测量的设备的角速度数据;
确定模块,用于根据所述加速度数据和重力加速度,确定设备的当前姿态变化状态;
调整模块,用于根据所述当前姿态变化状态,调整高通滤波器和低通滤波器的比例系数;
融合模块,用于根据调整所述比例系数后的高通滤波器和低通滤波器对所述加速度数据和所述角速度数据进行互补滤波,得到姿态融合数据;
控制模块,用于根据所述姿态融合数据控制所述设备调整姿态。
8.根据权利要求7所述的方法,其特征在于,所述确定模块还用于当所述加速度数据与所述重力加速度的差异大于或等于阈值时,确定设备处于高频姿态变化状态;当所述加速度数据与所述重力加速度的差异小于阈值时,确定设备处于低频姿态变化状态。
9.一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至6中任一项所述方法的步骤。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至6中任一项所述的方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910848344.0A CN112462793A (zh) | 2019-09-09 | 2019-09-09 | 基于传感器的设备控制方法、装置、计算机设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910848344.0A CN112462793A (zh) | 2019-09-09 | 2019-09-09 | 基于传感器的设备控制方法、装置、计算机设备 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN112462793A true CN112462793A (zh) | 2021-03-09 |
Family
ID=74807269
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910848344.0A Pending CN112462793A (zh) | 2019-09-09 | 2019-09-09 | 基于传感器的设备控制方法、装置、计算机设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112462793A (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113282005A (zh) * | 2021-05-26 | 2021-08-20 | 惠州中国科学院遥感与数字地球研究所空间信息技术研究院 | 无刷云台调控方法、装置、计算机设备和存储介质 |
CN113992846A (zh) * | 2021-10-19 | 2022-01-28 | 上海艾为电子技术股份有限公司 | 姿态角的获取方法、防抖控制方法以及移动终端 |
CN114177017A (zh) * | 2021-12-29 | 2022-03-15 | 东北农业大学 | 一种盲人出行辅助系统 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004268730A (ja) * | 2003-03-07 | 2004-09-30 | Yamaha Motor Co Ltd | 無人ヘリコプタの姿勢制御方法 |
CN105739510A (zh) * | 2016-01-13 | 2016-07-06 | 天津中科智能识别产业技术研究院有限公司 | 一种用于救灾无人机姿态控制的互补滤波方法 |
CN106482734A (zh) * | 2016-09-28 | 2017-03-08 | 湖南优象科技有限公司 | 一种用于imu多传感器数据融合的滤波方法 |
CN106873612A (zh) * | 2017-02-24 | 2017-06-20 | 宁波工程学院 | 电动平衡车姿态快速检测方法 |
CN107830861A (zh) * | 2017-12-07 | 2018-03-23 | 智灵飞(北京)科技有限公司 | 基于自适应增益互补滤波运动物体姿态测量方法及装置 |
-
2019
- 2019-09-09 CN CN201910848344.0A patent/CN112462793A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004268730A (ja) * | 2003-03-07 | 2004-09-30 | Yamaha Motor Co Ltd | 無人ヘリコプタの姿勢制御方法 |
CN105739510A (zh) * | 2016-01-13 | 2016-07-06 | 天津中科智能识别产业技术研究院有限公司 | 一种用于救灾无人机姿态控制的互补滤波方法 |
CN106482734A (zh) * | 2016-09-28 | 2017-03-08 | 湖南优象科技有限公司 | 一种用于imu多传感器数据融合的滤波方法 |
CN106873612A (zh) * | 2017-02-24 | 2017-06-20 | 宁波工程学院 | 电动平衡车姿态快速检测方法 |
CN107830861A (zh) * | 2017-12-07 | 2018-03-23 | 智灵飞(北京)科技有限公司 | 基于自适应增益互补滤波运动物体姿态测量方法及装置 |
Non-Patent Citations (1)
Title |
---|
兰建军;谭力弓;朴亨;王鹏;: "惯性传感器和互补滤波器在姿态估计中的应用", 制造业自动化, vol. 35, no. 9, pages 65 - 67 * |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113282005A (zh) * | 2021-05-26 | 2021-08-20 | 惠州中国科学院遥感与数字地球研究所空间信息技术研究院 | 无刷云台调控方法、装置、计算机设备和存储介质 |
CN113992846A (zh) * | 2021-10-19 | 2022-01-28 | 上海艾为电子技术股份有限公司 | 姿态角的获取方法、防抖控制方法以及移动终端 |
CN114177017A (zh) * | 2021-12-29 | 2022-03-15 | 东北农业大学 | 一种盲人出行辅助系统 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109959381B (zh) | 一种定位方法、装置、机器人及计算机可读存储介质 | |
KR102440358B1 (ko) | 관성 기반 항법 장치 및 상대사전적분에 따른 관성 기반 항법 방법 | |
CN112462793A (zh) | 基于传感器的设备控制方法、装置、计算机设备 | |
KR102017404B1 (ko) | 9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법 | |
JP6104169B2 (ja) | ジャイロセンサ較正のデバイス及び方法 | |
US10627237B2 (en) | Offset correction apparatus for gyro sensor, recording medium storing offset correction program, and pedestrian dead-reckoning apparatus | |
Jin et al. | The adaptive Kalman filter based on fuzzy logic for inertial motion capture system | |
CN108731664B (zh) | 机器人状态估计方法、装置、计算机设备和存储介质 | |
CN107796387B (zh) | 定位方法、定位装置和电子设备 | |
CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及系统 | |
CN109813308A (zh) | 姿态估计方法、装置及计算机可读存储介质 | |
RU2762143C2 (ru) | Система определения курса и углового пространственного положения, выполненная с возможностью функционирования в полярной области | |
WO2020124678A1 (zh) | 一种基于函数迭代积分的惯性导航解算方法及系统 | |
CN112179342B (zh) | 一种估计无人机姿态的方法及系统 | |
KR101584080B1 (ko) | 3차원 회전 모션센서의 가속도 신호처리 방법 | |
CN111989631A (zh) | 自身位置推定方法 | |
WO2022135070A1 (zh) | 惯性导航方法及设备 | |
CN113498505A (zh) | 通过预测传感器数据对被跟踪对象的姿态建模 | |
CN113063416B (zh) | 一种基于自适应参数互补滤波的机器人姿态融合方法 | |
CN109866217B (zh) | 机器人里程定位方法、装置、终端设备及计算机存储介质 | |
KR20170092359A (ko) | 회전 모션센서를 이용한 3축 자세정보 계측 시스템 | |
WO2019127092A1 (en) | State estimatation | |
CN110132271A (zh) | 一种自适应卡尔曼滤波姿态估计算法 | |
CN110954103B (zh) | 基于mems传感器的车体动态姿态估计的方法及系统 | |
CN112284380A (zh) | 一种基于光流和imu融合的非线性估计方法及系统 |
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 |