CN114337605B - 一种旋转角滤波方法、计算机设备及存储介质 - Google Patents

一种旋转角滤波方法、计算机设备及存储介质 Download PDF

Info

Publication number
CN114337605B
CN114337605B CN202210245105.8A CN202210245105A CN114337605B CN 114337605 B CN114337605 B CN 114337605B CN 202210245105 A CN202210245105 A CN 202210245105A CN 114337605 B CN114337605 B CN 114337605B
Authority
CN
China
Prior art keywords
rotation angle
current time
filtering
filtered
converted
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
CN202210245105.8A
Other languages
English (en)
Other versions
CN114337605A (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.)
Xiaoshi Technology (Jiangsu) Co.,Ltd.
Original Assignee
Nanjing Zhenshi 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 Nanjing Zhenshi Intelligent Technology Co Ltd filed Critical Nanjing Zhenshi Intelligent Technology Co Ltd
Priority to CN202210245105.8A priority Critical patent/CN114337605B/zh
Publication of CN114337605A publication Critical patent/CN114337605A/zh
Application granted granted Critical
Publication of CN114337605B publication Critical patent/CN114337605B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Image Processing (AREA)

Abstract

本发明公开了一种旋转角滤波方法、计算机设备及存储介质，其中旋转角滤波方法包括：将当前时刻的旋转角弧度值转换到
Figure 736431DEST_PATH_IMAGE002
范围内，得到转化后的旋转角；根据当前时刻旋转角滤波系数，对转化后的旋转角进行滤波，得到滤波后的旋转角；将滤波后的旋转角
Figure 715889DEST_PATH_IMAGE004
范围内，得到
Figure 557943DEST_PATH_IMAGE006
时刻的滤波结果。本发明提出的旋转角滤波方法，采用了旋转角滤波系数和前一时刻滤波结果对当前时刻的旋转角进行滤波，能够减少旋转角周期变化对滤波的干扰，对所有范围的旋转角都能进行有效滤波；且滤波系数能够根据旋转角变化率进行自适应调节，减少旋转角低速变化时的抖动，降低高速变化时的延迟。

Description

一种旋转角滤波方法、计算机设备及存储介质
技术领域
本发明涉及信号处理技术领域，具体是一种旋转角滤波方法。
背景技术
旋转角滤波是指根据之前状态的旋转角对当前状态旋转角进行修正，使角度变化更平滑。旋转角滤波在无人机、机器人等控制工程中有着广泛应用。一方面由于旋转角呈周期性变化，若直接对未处理的角度进行滤波，滤波结果容易在旋转角周期变化处发生跳变。如
Figure 784053DEST_PATH_IMAGE002
和
Figure 977137DEST_PATH_IMAGE004
虽然代表同一角度，但基于二者进行滤波的结果会有较大差异。因此有些技术将使用场景约束在一定范围内，如限定旋转角仅在0到
Figure 178311DEST_PATH_IMAGE006
的之间，这样大大限制了应用领域。另一方面旋转角的变化速度不确定，当旋转角低速变化时，抖动问题更易凸显，当旋转角高速变化时，延迟问题更易凸显，在使用均值滤波、指数滤波等方法时，无法自适应地权衡抖动和延迟。
发明内容
对本发明所要解决的技术问题是针对上述现有技术存在的不足，而提供一种自适应地权衡抖动和延迟的旋转角滤波方法、计算机设备及存储介质。
为解决上述技术问题，本发明采用的技术方案是：
本发明提供一种旋转角滤波方法，包括：
将当前时刻的旋转角弧度值转换到
Figure 824056DEST_PATH_IMAGE008
范围内，得到转化后的旋转角；
若当前时刻=0时，直接输出转化后的旋转角作为滤波结果；
若当前时刻≠0时，根据当前时刻旋转角滤波系数，对转化后的旋转角进行滤波，得到滤波后的旋转角；
将滤波后的旋转角转化到
Figure 73772DEST_PATH_IMAGE010
范围内，得到当前时刻的滤波结果。
本发明还提供一种计算机设备，其特征在于，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实 现上述旋转角滤波方法。
本发明还提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现上述旋转角滤波方法。
与现有技术相比，本发明的有益效果是：
本发明提出的旋转角滤波方法，采用了旋转角滤波系和前一时刻滤波结果对当前时刻的旋转角进行滤波，能够减少旋转角周期变化对滤波的干扰，对所有范围的旋转角都能进行有效滤波；且滤波系数能够根据旋转角变化率进行自适应调节，减少旋转角低速变化时的抖动，降低高速变化时的延迟。
附图说明
图1是本发明方法的流程图。
图2是是本发明方法与传统方法的效果对比，a图为本发明方法滤波效果，b图为传统的滑动平均滤波效果。
具体实施方式
下面结合附图，对本发明作详细说明：
本实施例提供一种旋转角滤波方法，步骤如图1所述，包括：
将当前时刻的旋转角弧度值转换到
Figure 121362DEST_PATH_IMAGE012
范围内，得到转化后的旋转角；转化后的旋转角为：
Figure 493438DEST_PATH_IMAGE014
其中，
Figure 629409DEST_PATH_IMAGE016
为当前时刻
Figure 745132DEST_PATH_IMAGE018
规范化后的旋转角；
Figure 584912DEST_PATH_IMAGE020
为当前时刻
Figure DEST_PATH_IMAGE021
的旋转角弧度值；
Figure DEST_PATH_IMAGE023
为向下取整运算。
根据当前时刻旋转角滤波系数，对转化后的旋转角进行滤波，得到滤波后的旋转角；滤波后的旋转角为：
Figure DEST_PATH_IMAGE025
其中，
Figure DEST_PATH_IMAGE027
为当前时刻
Figure 100002_DEST_PATH_IMAGE029
滤波后的旋转角；
Figure 100002_DEST_PATH_IMAGE031
为当前时刻
Figure 100002_DEST_PATH_IMAGE033
旋转角滤波系数；
Figure 100002_DEST_PATH_IMAGE035
为当前时刻
Figure 100002_DEST_PATH_IMAGE037
转化后的旋转角；
Figure DEST_PATH_IMAGE039
为与当前时刻
Figure DEST_PATH_IMAGE041
旋转角最近的弧度值；
与当前时刻
Figure DEST_PATH_IMAGE043
旋转角最近的弧度值
Figure 691671DEST_PATH_IMAGE045
为：
Figure 984112DEST_PATH_IMAGE047
其中，
Figure 903526DEST_PATH_IMAGE049
为前一时刻的滤波后的旋转角；
当前时刻
Figure 660130DEST_PATH_IMAGE051
的旋转角滤波系数
Figure 374008DEST_PATH_IMAGE053
为：
Figure 419324DEST_PATH_IMAGE054
其中，
Figure 876850DEST_PATH_IMAGE056
为当前时刻
Figure 490890DEST_PATH_IMAGE058
的截止频率；
Figure 578932DEST_PATH_IMAGE060
为当前时刻
Figure 908282DEST_PATH_IMAGE062
的采样间隔；
在一个实施例中，当前时刻
Figure 903920DEST_PATH_IMAGE063
的截止频率
Figure 369536DEST_PATH_IMAGE065
为：
Figure 956375DEST_PATH_IMAGE067
其中，
Figure 179546DEST_PATH_IMAGE069
为最小截止频率；
Figure 103509DEST_PATH_IMAGE071
为速度因子；
Figure 626894DEST_PATH_IMAGE073
为当前时刻
Figure 994421DEST_PATH_IMAGE074
的旋转角变化率。
将当前时刻
Figure 298364DEST_PATH_IMAGE076
滤波后的旋转角
Figure 839067DEST_PATH_IMAGE078
转换到
Figure 129541DEST_PATH_IMAGE080
范围内，得到当前时刻
Figure 933549DEST_PATH_IMAGE082
的滤波结果：
Figure 318262DEST_PATH_IMAGE084
。
本发明滤波方法中旋转角弧度值转换和滤波后旋转角
Figure 662656DEST_PATH_IMAGE086
转化均可表述为规范化处理，通过计算机进行滤波操作的详细步骤为：
1.旋转角规范化。
将当前时刻
Figure DEST_PATH_IMAGE088
（
Figure DEST_PATH_IMAGE090
且
Figure DEST_PATH_IMAGE092
为整数）的旋转角弧度值
Figure 347584DEST_PATH_IMAGE093
转换到
Figure 56914DEST_PATH_IMAGE095
范围内：
Figure 335449DEST_PATH_IMAGE097
其中，
Figure 217954DEST_PATH_IMAGE099
为当前时刻
Figure DEST_PATH_IMAGE100
规范化后的旋转角；
Figure DEST_PATH_IMAGE102
为向下取整运算。
2.初始化。
当
Figure DEST_PATH_IMAGE104
时，此时为初始时刻，直接输出
Figure 619373DEST_PATH_IMAGE106
作为滤波结果。
3.前一时刻旋转角预处理。
为了减少旋转角周期性变化对滤波的干扰，需要将前一时刻的滤波后的旋转角
Figure DEST_PATH_IMAGE108
转换到与当前时刻旋转角最近的弧度值
Figure DEST_PATH_IMAGE110
。当
Figure DEST_PATH_IMAGE112
时，若
Figure DEST_PATH_IMAGE114
，反之
Figure DEST_PATH_IMAGE116
。
4.旋转角变化率滤波。
为了更好地权衡抖动与延迟，根据旋转角的变化率来自适应调节滤波系数。首先对旋转角变化率进行滤波。当
Figure DEST_PATH_IMAGE118
时，滤波后的当前时刻
Figure DEST_PATH_IMAGE120
的旋转角变化率
Figure 673173DEST_PATH_IMAGE122
设为0。当
Figure 314370DEST_PATH_IMAGE124
时，使用指数滤波器，滤波后的当前时刻
Figure 566DEST_PATH_IMAGE120
的旋转角变化率为
Figure 863349DEST_PATH_IMAGE126
，其中当前时刻
Figure 39115DEST_PATH_IMAGE127
的旋转角变化率为
Figure 26663DEST_PATH_IMAGE129
，
Figure 250971DEST_PATH_IMAGE131
当前时刻
Figure DEST_PATH_IMAGE132
的采样间隔
Figure DEST_PATH_IMAGE134
；当前时刻
Figure 702681DEST_PATH_IMAGE075
的旋转角变化率滤波系数为
Figure DEST_PATH_IMAGE136
，
Figure 114595DEST_PATH_IMAGE137
为固定截止频率
Figure 323860DEST_PATH_IMAGE139
为前一时刻滤波后的旋转角变化率。
5.旋转角滤波。
根据旋转角变化率计算自适应的当前时刻
Figure 351858DEST_PATH_IMAGE141
的旋转角滤波系数
Figure 595758DEST_PATH_IMAGE143
：
Figure 113327DEST_PATH_IMAGE145
，
其中，
Figure 75467DEST_PATH_IMAGE147
为当前时刻
Figure 641577DEST_PATH_IMAGE149
的截止频率；
Figure 739983DEST_PATH_IMAGE151
为最小截止频率
Figure 428454DEST_PATH_IMAGE153
，
Figure 549993DEST_PATH_IMAGE155
为速度因子
Figure 809260DEST_PATH_IMAGE157
。
使用指数滤波器，滤波后的当前时刻
Figure 27752DEST_PATH_IMAGE159
的旋转角
Figure 887123DEST_PATH_IMAGE161
。
6. 滤波后的旋转角进行规范化。
再对滤波后的旋转角进行规范化，得到当前时刻
Figure DEST_PATH_IMAGE162
的滤波结果
Figure DEST_PATH_IMAGE164
：
Figure DEST_PATH_IMAGE166
。
分别使用本发明方法和传统的滑动平均滤波对加入高斯噪声的旋转角序列进行滤波，对比效果如图2所示。其中a图为本发明方法滤波效果，b图为滑动平均滤波效果，图中“叉”为原始旋转角，“点”为滤波后的旋转角。可以看出本发明方法能够在平滑数据的同时，在周期变化处不会出现跳变，且延迟较低。
本实施例提供一种计算机设备，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实 现实施例旋转角滤波方法中的步骤。
本实施例提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现实施例旋转角滤波方法中的步骤。

Claims (6)

1.一种旋转角滤波方法，其特征在于，包括：
将当前时刻的旋转角弧度值转换到
Figure 302867DEST_PATH_IMAGE002
范围内，得到转化后的旋转角；
若当前时刻＝0时，直接输出转化后的旋转角作为滤波结果；
若当前时刻≠0时，根据当前时刻旋转角滤波系数，对转化后的旋转角进行滤波，得到滤波后的旋转角；
将滤波后的旋转角转化到
Figure 767346DEST_PATH_IMAGE004
范围内，得到当前时刻的滤波结果；
当前时刻的旋转角滤波系数为：
Figure 764121DEST_PATH_IMAGE006
其中，
Figure 819801DEST_PATH_IMAGE008
为当前时刻
Figure 967274DEST_PATH_IMAGE010
的旋转角滤波系数；
Figure 32182DEST_PATH_IMAGE012
为当前时刻
Figure DEST_PATH_IMAGE014
的截止频率；
Figure 883463DEST_PATH_IMAGE016
为当前时刻
Figure 375624DEST_PATH_IMAGE018
的采样间隔；
当前时刻i的截至频率
Figure 945146DEST_PATH_IMAGE020
为
Figure 282586DEST_PATH_IMAGE022
其中，
Figure 988374DEST_PATH_IMAGE024
为最小截止频率；
Figure 710824DEST_PATH_IMAGE026
为速度因子；
Figure 767641DEST_PATH_IMAGE027
为当前时刻
Figure DEST_PATH_IMAGE029
滤波后的旋转角变化率；当前时刻
Figure DEST_PATH_IMAGE031
滤波后的旋转角变化率
Figure DEST_PATH_IMAGE033
：
Figure DEST_PATH_IMAGE035
其中，
Figure DEST_PATH_IMAGE037
为前时刻
Figure 285604DEST_PATH_IMAGE039
旋转角变化率；
Figure 845898DEST_PATH_IMAGE041
为前时刻
Figure 679862DEST_PATH_IMAGE042
旋转角变化率滤波系数；
Figure 692817DEST_PATH_IMAGE044
为前一时刻滤波后的旋转角变化率；
当前时刻
Figure 840902DEST_PATH_IMAGE045
旋转角变化率
Figure DEST_PATH_IMAGE047
和当前时刻
Figure DEST_PATH_IMAGE049
旋转角变化率滤波系数
Figure 728740DEST_PATH_IMAGE050
分别为：
Figure 733606DEST_PATH_IMAGE051
Figure DEST_PATH_IMAGE052
其中，
Figure DEST_PATH_IMAGE054
为固定截止频率；
得到滤波后的旋转角为：
Figure 92912DEST_PATH_IMAGE056
其中，
Figure 575845DEST_PATH_IMAGE058
为当前时刻
Figure 110732DEST_PATH_IMAGE060
滤波后的旋转角；
Figure DEST_PATH_IMAGE062
为当前时刻
Figure 945220DEST_PATH_IMAGE063
转化后的旋转角；
Figure DEST_PATH_IMAGE065
为将前一时刻旋转角转化到与当前时刻i旋转角最近的弧度值。
2.根据权利要求1所述的旋转角滤波方法，其特征在于，当
Figure 995085DEST_PATH_IMAGE067
时，
Figure DEST_PATH_IMAGE069
；
当
Figure 750551DEST_PATH_IMAGE070
时，将前一时刻旋转角转化到与当前时刻
Figure 874365DEST_PATH_IMAGE072
旋转角最近的弧度值
Figure 17770DEST_PATH_IMAGE074
：
Figure DEST_PATH_IMAGE075
其中，
Figure 552001DEST_PATH_IMAGE077
为前一时刻的滤波后的旋转角。
3.根据权利要求2所述的旋转角滤波方法，其特征在于，将当前时刻的旋转角弧度值转换到
Figure 845579DEST_PATH_IMAGE079
范围内，得到转化后的旋转角为：
Figure 89479DEST_PATH_IMAGE080
其中，
Figure 341469DEST_PATH_IMAGE082
为当前时刻
Figure 303608DEST_PATH_IMAGE084
规范化后的旋转角；
Figure DEST_PATH_IMAGE085
为当前时刻
Figure DEST_PATH_IMAGE087
的旋转角弧度值；
Figure DEST_PATH_IMAGE089
为向下取整运算。
4.根据权利要求3所述的旋转角滤波方法，其特征在于，将滤波后的旋转角
Figure DEST_PATH_IMAGE091
转化到
Figure 121916DEST_PATH_IMAGE079
范围内，得到
Figure DEST_PATH_IMAGE093
的滤波结果为：
Figure DEST_PATH_IMAGE095
式中，
Figure DEST_PATH_IMAGE097
当前时刻
Figure DEST_PATH_IMAGE099
的滤波结果。
5.一种计算机设备，其特征在于，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-4中任一所述的旋转角滤波方法。
6.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-4中任一所述的旋转角滤波方法。
CN202210245105.8A 2022-03-14 2022-03-14 一种旋转角滤波方法、计算机设备及存储介质 Active CN114337605B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210245105.8A CN114337605B (zh) 2022-03-14 2022-03-14 一种旋转角滤波方法、计算机设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210245105.8A CN114337605B (zh) 2022-03-14 2022-03-14 一种旋转角滤波方法、计算机设备及存储介质

Publications (2)

Publication Number Publication Date
CN114337605A CN114337605A (zh) 2022-04-12
CN114337605B true CN114337605B (zh) 2022-05-31

Family

ID=81033562

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210245105.8A Active CN114337605B (zh) 2022-03-14 2022-03-14 一种旋转角滤波方法、计算机设备及存储介质

Country Status (1)

Country Link
CN (1) CN114337605B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102768043A (zh) * 2012-06-14 2012-11-07 辽宁工程技术大学 一种无外观测量的调制型捷联系统组合姿态确定方法
CN103873015A (zh) * 2014-01-24 2014-06-18 中国人民解放军国防科学技术大学 面向连续转角信号的平滑数字滤波方法
CN110830003A (zh) * 2019-11-03 2020-02-21 北京宏德信智源信息技术有限公司 一种基于α-β滤波器的滤波方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102768043A (zh) * 2012-06-14 2012-11-07 辽宁工程技术大学 一种无外观测量的调制型捷联系统组合姿态确定方法
CN103873015A (zh) * 2014-01-24 2014-06-18 中国人民解放军国防科学技术大学 面向连续转角信号的平滑数字滤波方法
CN110830003A (zh) * 2019-11-03 2020-02-21 北京宏德信智源信息技术有限公司 一种基于α-β滤波器的滤波方法

Also Published As

Publication number Publication date
CN114337605A (zh) 2022-04-12

Similar Documents

Publication Publication Date Title
CN110492868B (zh) 一种多参数变步长lms自适应滤波方法
Vainio et al. Digital filtering for robust 50/60 Hz zero-crossing detectors
US10437664B2 (en) Interference suppression using repeated reduced rank adaptive filtering in fractional fourier transform (FrFT) domains
CN114337605B (zh) 一种旋转角滤波方法、计算机设备及存储介质
CN106603036A (zh) 一种基于低阶内插滤波器的自适应时延估计方法
CN106153029B (zh) 二频机抖激光陀螺抖动信号抵消装置
US20240187113A1 (en) Receiving Apparatus
JP5029357B2 (ja) 適応ディジタルフィルタ、信号処理方法、ｆｍ受信機およびプログラム
Bolic et al. Performance and complexity analysis of adaptive particle filtering for tracking applications
CN108667538B (zh) 复杂噪声环境下时延和幅度衰减的联合估计方法
JP5029356B2 (ja) 適応ディジタルフィルタ、信号処理方法、ｆｍ受信機およびプログラム
WO2020037506A1 (zh) 确定cvsd编解码中参数的方法和设备
JP2000332574A (ja) 適応フィルタ、適応フィルタの制御方法及びプログラムを記憶した記憶媒体
CN108510996B (zh) 一种快速迭代自适应滤波方法
CN107800405B (zh) 基于fpga设计的径向基函数神经网络自适应增强器电路
US10135548B2 (en) System, apparatus, and method for at least mitigating a signal reflection
CN111010144B (zh) 改进的两通道iir的qmfb设计方法
JP7102907B2 (ja) 合成開口ソーナー及びその信号処理方法、並びに信号処理プログラム
Kay Comments on" Frequency estimation by linear prediction"
Apolinário Jr et al. Introduction to adaptive filters
Patki et al. Low complexity, low latency resampling of asynchronously sampled signals
Treichler γ-LMS and its use in a noise-compensating adaptive spectral analysis technique
JPH11340877A (ja) 適応型ｄｃ補償
Clifford et al. Centre-frequency adaptive IIR temporal filters for phase-based image velocity estimation
CN112737461B (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
CP01 Change in the name or title of a patent holder
CP01 Change in the name or title of a patent holder

Address after: 210000 Longmian Avenue 568, High-tech Park, Jiangning District, Nanjing City, Jiangsu Province

Patentee after: Xiaoshi Technology (Jiangsu) Co.,Ltd.

Address before: 210000 Longmian Avenue 568, High-tech Park, Jiangning District, Nanjing City, Jiangsu Province

Patentee before: NANJING ZHENSHI INTELLIGENT TECHNOLOGY Co.,Ltd.