[發明專利]角度傳感器和旋轉編碼器的前向插值方法

有效

• 申請號: 201810568513.0
• 申請日: 2018-06-05
• 公開（公告）號: CN108959183B
• 公開（公告）日: 2022-07-12
• 發明（設計）人: 朱劍宇;孫毅
• 申請（專利權）人: 上海麥歌恩微電子股份有限公司
• 主分類號: G06F17/17
• 分類號: G06F17/17
• 代理公司: 上海金盛協力知識產權代理有限公司 31242
• 代理人: 王松

1.一種角度傳感器和旋轉編碼器的前向插值方法，其特征在于，所述方法用于實現利用已知的時刻物體所處的角度信息來預測物體在之后的一段時間內的角度變化；所述方法包括：

按照剛體運動的慣性定理，假設在較短的時間內剛體轉動遵循勻加速運動原則，計算出角度位置和角速度以及角加速度之間的關系；

在最新的幾個采樣時刻所獲得的角度位置信息的基礎上進行計算，得到預測起始位置的角速度和角加速度值；

依據起始位置的角度信息和角速度信息以及角加速度信息，在輸出頻率的要求下進行前向插值預測計算；

所述方法的具體步驟實現方案如下：

步驟S1、按照剛體轉動的慣性原理，在臨近的很短的一段時間內，剛體的運動遵循勻角加速度法則進行；給定角速度β和角加速度α以及角度位置θ都是以時間t變化的函數，那么勻角加速運動的公式可表示為：

β(t)＝β(0)+α*t 公式(1)

那么，在時間t之內角度的變化量是能夠計算的：

θ(t)＝θ(0)+β(0)*t+α*t²/2 公式(3)

在高速數據刷新率的場景下，以此為預測的公式進行前向插值計算，增加數據的刷新頻率且保持較高的精度；

在采樣感應器件輸出有效數據更新的時刻，為避免由于插值計算和真實的感應結果之間的誤差導致的瞬時角度突變，采用先輸出插值計算結果再在隨后的一個采樣感應周期內疊加補償的方式濾除高頻噪聲；

步驟S2、根據公式(3)的原理，在下一個傳感器采樣輸出之前，預判當前的機械結構所處角度；根據不同的應用場景和預測精度的需求，選用不同數量的采樣感應器的輸出作為插值算法的輸入條件；這個數量最少是兩個點，最多是任意多個點；當采用兩個采樣輸出作為插值基值的時候，勻加速運動簡化為勻速運動；

選用三個采樣輸出數據來進行插值運算，在前三個采樣時刻所采樣到的角度位置分別為：θ₀,θ₁和θ₂；

并且假設從位置0到位置2之間剛體的旋轉遵循慣性原則即勻角加速度運動，且位置0的初始角速度為β₀那么得到如下公式：

θ₁-θ₀＝β₀*t+α*t²/2 公式(4)

θ₂-θ₁＝β₁*t+α*t²/2＝β₀*t+3α*t²/2 公式(5)

根據公式(4)和公式(5)不難推到出初始角速度和角加速度的值與采樣到的角度之間的關系：

根據初始角度和角加速度以及預測的初始角度位置θ₂，對當前的角度位置進行預估；由于采用了離散化的數據輸出，當前位置較θ₂位置的時刻延遲了nt/m，所以代入公式計算后得到nt/m時刻的角度位置為：

在實現這個算法的數字系統中，需要保存3組最新的采樣數據以供計算所用；并且在每個新的采樣數據到來的時候，及時刷新保持數據寄存器以產生新的前向插值輸出；

利用高頻時鐘計數，運用計時溢出中斷累加產生計算所需的信號n；而信號m來自于傳感采樣輸出頻率與數字信號高頻信號頻率的倍數關系；為了適應不同應用場景和不同的傳感采樣速率，在實現中采用寄存器配置的方法來產生不同的數據刷新周期和傳感采樣頻率，進而產生不同的m值；

當新的角度信號傳送進來的這個周期，對數據緩存寄存器組進行刷新；將最新的角度存入第二緩存數據寄存器，而原來第二緩存數據寄存器存儲的第二緩存數據存入第一緩存數據寄存器；同理，將原來第一緩存數據寄存器存儲的第一緩存數據存入第零緩存數據寄存器完成對三個寄存器的同步刷新；

步驟S3、在插值判斷輸入數據點數大于等于4的情況下，采用均值濾波算法來實現濾除由于信號自身的噪聲所引入的誤差，所述均值濾波算法為加權平均均值濾波算法；每相鄰的三個時間節點數據進行計算，得到角速度；隨后乘以權重，考慮到越近的數據逼真性越高，所以得到最大的權重，而越早的數據獲得的權重越小；最后對加權之后的所有角速度進行相加，得到最后的插值計算所用的角速度；對于角加速度的加權均值濾波方法與角速度濾波方式完全一致；

步驟S4、當前向插值達到一個信號采樣周期的最后一個數據刷新節點的時刻,如果采用采樣得到的信號直接輸出會導致因為插值預測偏差而引起的信號瞬時突變，從而引入高頻噪聲；而在本算法體系中，為了避免這種噪聲給系統帶來的角度輸出突變問題，采用了如下的方法：

首先，在這個時刻將插值計算預測的值作為有效的值輸出；

然后，比較插值預測的結果和實際信號采樣的結果，得到一個差值；

最后，把這個差值平均分配到隨后的一個采樣周期內的m個點遞進累加；

這樣，就順利的濾除了高頻噪聲，并且最終把插值預測的誤差補償到了角度輸出值上；

在時刻1，采樣傳感單元有輸出的數據θ₁，而在插值輸出計算中根據時刻0的數據θ₀以及之前的數據得到時刻1的預測值為θ_{1_pre}；那么，按照本算法，在時刻1實際輸出的有效數據是θ_{1_pre}；并且，對時刻1之后的數據前向插值計算的基點也是θ_{1_pre}；同時，為了補償時刻1的預測誤差，在時刻1之后的第n個插值點上疊加補償一個值：

D_comp(n)＝n*(θ₁–θ_{1_pre})/m

那么最終的插值產生的信號為：