一種改善相對(duì)位置傳感器性能的方法及裝置的制造方法
【專利摘要】本發(fā)明公開(kāi)了一種改善相對(duì)位置傳感器性能的方法，應(yīng)用于測(cè)量裝置中，測(cè)量裝置用于測(cè)量被測(cè)對(duì)象與測(cè)量裝置之間的相對(duì)位置，測(cè)量裝置上設(shè)置有相對(duì)位置傳感器和第一輔助傳感器，被測(cè)對(duì)象上設(shè)置有第二輔助傳感器，所述方法包括：獲取由相對(duì)位置傳感器測(cè)得的第一測(cè)量數(shù)據(jù)，以及獲取由第一輔助傳感器和第二輔助傳感器測(cè)得的第二測(cè)量數(shù)據(jù)；基于第一測(cè)量數(shù)據(jù)和第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器；利用擴(kuò)展卡爾曼濾波器，對(duì)第一測(cè)量數(shù)據(jù)進(jìn)行修正。本發(fā)明實(shí)現(xiàn)了提高相對(duì)位置傳感器的測(cè)量數(shù)據(jù)的準(zhǔn)確性，從而改善相對(duì)位置傳感器的動(dòng)態(tài)性能的技術(shù)效果。同時(shí)，本發(fā)明還公開(kāi)了一種測(cè)量裝置。
【專利說(shuō)明】
一種改善相對(duì)位置傳感器性能的方法及裝置
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及電子技術(shù)領(lǐng)域，尤其涉一種改善相對(duì)位置傳感器性能的方法及裝置。
【背景技術(shù)】
[0002] 相對(duì)位置傳感器，用于測(cè)量?jī)蓚€(gè)物體之間的相對(duì)位置（例如：相對(duì)夾角和相對(duì)距 離），在機(jī)器人領(lǐng)域應(yīng)用廣泛，可以用于對(duì)被測(cè)對(duì)象進(jìn)行跟蹤。
[0003] 但是，目前的相對(duì)位置傳感器普遍性能較差，測(cè)量數(shù)據(jù)容易受到干擾，產(chǎn)生跳動(dòng)， 導(dǎo)致測(cè)量數(shù)據(jù)不準(zhǔn)確，動(dòng)態(tài)性能較差，從而嚴(yán)重影響了相對(duì)位置測(cè)量傳感器的應(yīng)用。
[0004] 綜上所述，如何改善相對(duì)位置傳感器性能，已成為現(xiàn)階段亟待解決的問(wèn)題。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明實(shí)施例通過(guò)提供一種改善相對(duì)位置傳感器性能的方法及裝置，解決了現(xiàn)有 技術(shù)中的相對(duì)位置傳感器存在測(cè)量數(shù)據(jù)容易受到干擾，產(chǎn)生跳動(dòng)，導(dǎo)致測(cè)量數(shù)據(jù)不準(zhǔn)確，動(dòng) 態(tài)性能較差的技術(shù)問(wèn)題。
[0006] -方面，本發(fā)明通過(guò)一實(shí)施例提供如下技術(shù)方案：
[0007] -種改善相對(duì)位置傳感器性能的方法，應(yīng)用于測(cè)量裝置中，所述測(cè)量裝置用于測(cè) 量被測(cè)對(duì)象與所述測(cè)量裝置之間的相對(duì)位置，所述測(cè)量裝置上設(shè)置有相對(duì)位置傳感器和第 一輔助傳感器，所述被測(cè)對(duì)象上設(shè)置有第二輔助傳感器，所述方法包括：
[0008] 獲取由所述相對(duì)位置傳感器測(cè)得的第一測(cè)量數(shù)據(jù)，以及獲取由所述第一輔助傳感 器和所述第二輔助傳感器測(cè)得的第二測(cè)量數(shù)據(jù)；
[0009] 基于所述第一測(cè)量數(shù)據(jù)和所述第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器；
[0010] 利用所述擴(kuò)展卡爾曼濾波器，對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正。
[0011] 可選地，所述第一測(cè)量數(shù)據(jù)，包括：
[0012] 所述測(cè)量裝置與所述被測(cè)對(duì)象之間的相對(duì)夾角續(xù)；^以及所述測(cè)量裝置與所述被 測(cè)對(duì)象之間的相對(duì)距離4
[0013] 可選地，所述第二測(cè)量數(shù)據(jù)，包括：
[0014] 所述被測(cè)對(duì)象的加速度在X軸上的分量以及所述被測(cè)對(duì)象的加速度在γ軸上 的分量
[0015] 所述測(cè)量裝置的前進(jìn)速度咬1，以及所述測(cè)量裝置的旋轉(zhuǎn)速度
[0016] 所述測(cè)量裝置的正方向與地球磁北極的夾角03，以及所述被測(cè)對(duì)象的正方向與地 球磁北極的夾角9t。
[0017] 可選地，所述基于所述第一測(cè)量數(shù)據(jù)和所述第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波 器之前，還包括：
[0018] 對(duì)由所述測(cè)量裝置和所述被測(cè)對(duì)象所組成的系統(tǒng)進(jìn)行數(shù)學(xué)建模，獲得數(shù)學(xué)模型：
[0020] 其中，為所述被測(cè)對(duì)象的加速度在X軸上的零偏，bf為所述被測(cè)對(duì)象的加速度在 Y軸上的零偏；
[0021] 亡為由所述第一輔助傳感器測(cè)得的所述測(cè)量裝置的前進(jìn)速度與所述測(cè)量裝置的 真實(shí)前進(jìn)速度的比值，為由所述第一輔助傳感器測(cè)得的所述測(cè)量裝置的旋轉(zhuǎn)速度與所述 測(cè)量裝置的真實(shí)旋轉(zhuǎn)速度的比值；
[0022] 為所述被測(cè)對(duì)象沿所述第二輔助傳感器指向方向上的速度在X軸上的分量，< 為所述被測(cè)對(duì)象沿所述第二輔助傳感器指向方向上的速度在Y軸上的分量；
[0023] 為所述測(cè)量裝置的前進(jìn)速度矢量向所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑方向 的投影的大小，為所述測(cè)量裝置的前進(jìn)速度矢量向所述測(cè)量裝置和所述被測(cè)對(duì)象的矢 徑方向的法向的投影的大小；
[0024] 為所述被測(cè)對(duì)象的速度矢量向連接所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑方向 的投影的大小，為所述被測(cè)對(duì)象的速度矢量向連接所述測(cè)量裝置和所述被測(cè)對(duì)象的矢 徑方向的法向的投影的大??；
[0025] T( ·)為二維空間中的旋轉(zhuǎn)變換矩陣，用于表示將其所后乘的向量逆時(shí)針旋轉(zhuǎn) (· )〇
[0026] 可選地，所述基于所述第一測(cè)量數(shù)據(jù)和所述第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波 器，包括：
[0027] 構(gòu)造狀態(tài)變量:X =[碎,1；匕S' bf, W Of]7*;
[0028]構(gòu)造輸入變量= [af_rn, 為]Γ;
[0029]基于所述狀態(tài)變量和所述輸入變量，將所述數(shù)學(xué)模型調(diào)整為如下表達(dá)式，得到擴(kuò) 展卡爾曼濾波器：
[0031] 其中，v為測(cè)量噪聲向量，服從均值為0、協(xié)方差為R的高斯分布，ti為時(shí)間， ± = 為系統(tǒng)微分方程模型，y = 為系統(tǒng)輸出量，h為測(cè)量矩陣。
[0032] 可選地，所述利用所述擴(kuò)展卡爾曼濾波器，對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正，包括：
[0033] 對(duì)所述擴(kuò)展卡爾曼濾波器進(jìn)行初始化；
[0034] 執(zhí)行遞推算法對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正，從而獲得到5§和< 的最優(yōu)狀態(tài)估 計(jì)。
[0035] 可選地，基于如下等式，對(duì)于所述擴(kuò)展卡爾曼濾波器進(jìn)行初始化：
[0036] x(0) = ￡[^(0)]
[0037] P(0) = ii f (x(0) - χ(0))(χ(0) - x(0))r]
[0038] 其中，到：〇)為系統(tǒng)初始狀態(tài)x(0)的估計(jì)值，P(0)為系統(tǒng)狀態(tài)轉(zhuǎn)移概率矩陣，E[ ·] 代表對(duì)?的期望。
[0039] 可選地，基于如下等式，執(zhí)行遞推算法對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正：
[0040] 5c = f(x,u, ti) + K[ym - h{x, vQ, ti)]
[0041 ]其中，K按照如下等式不斷更新：
[0049] I為系統(tǒng)狀態(tài)變量估計(jì)值的導(dǎo)數(shù);K為根據(jù)模擬系統(tǒng)和實(shí)際系統(tǒng)的誤差修正模擬系 統(tǒng)狀態(tài)的修正幅度;y m為含有噪聲的測(cè)量結(jié)果，其中包含相對(duì)位置傳感器輸出的相對(duì)距離 名和相對(duì)夾角j為中間變量;P為傳播概率矩陣;戶為傳播概率矩陣的導(dǎo)數(shù)。
[0050] 另一方面，本發(fā)明通過(guò)另一實(shí)施例，提供如下技術(shù)方案：
[0051] -種測(cè)量裝置，所述測(cè)量裝置用于測(cè)量被測(cè)對(duì)象與所述測(cè)量裝置之間的相對(duì)位 置，所述測(cè)量裝置上設(shè)置有相對(duì)位置傳感器和第一輔助傳感器，所述被測(cè)對(duì)象上設(shè)置有第 二輔助傳感器，所述測(cè)量裝置包括：
[0052] 獲取單元，用于獲取由所述相對(duì)位置傳感器測(cè)得的第一測(cè)量數(shù)據(jù)，以及獲取由所 述第一輔助傳感器和所述第二輔助傳感器測(cè)得的第二測(cè)量數(shù)據(jù)；
[0053]構(gòu)造單元，用于基于所述第一測(cè)量數(shù)據(jù)和所述第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾 波器；
[0054]修正單元，用于利用所述擴(kuò)展卡爾曼濾波器，對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正。
[0055] 可選地，所述第一測(cè)量數(shù)據(jù)，包括：
[0056] 所述測(cè)量裝置與所述被測(cè)對(duì)象之間的相對(duì)夾角^以及所述測(cè)量裝置與所述被 測(cè)對(duì)象之間的相對(duì)距離4。
[0057]可選地，所述第二測(cè)量數(shù)據(jù)，包括：
[0058] 所述被測(cè)對(duì)象的加速度在X軸上的分量，以及所述被測(cè)對(duì)象的加速度在Y軸上 的分量
[0059] 所述測(cè)量裝置的前進(jìn)速度，以及所述測(cè)量裝置的旋轉(zhuǎn)速度
[0060] 所述測(cè)量裝置的正方向與地球磁北極的夾角08，以及所述被測(cè)對(duì)象的正方向與地 球磁北極的夾角0t。
[0061 ] 可選地，所述的測(cè)量裝置，還包括：
[0062]建模單元，用于所述基于所述第一測(cè)量數(shù)據(jù)和所述第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾 曼濾波器之前，對(duì)由所述測(cè)量裝置和所述被測(cè)對(duì)象所組成的系統(tǒng)進(jìn)行數(shù)學(xué)建模，獲得數(shù)學(xué) 模型：
[0064] 其中，拉為所述被測(cè)對(duì)象的加速度在X軸上的零偏，為所述被測(cè)對(duì)象的加速度在 Y軸上的零偏；
[0065] #為由所述第一輔助傳感器測(cè)得的所述測(cè)量裝置的前進(jìn)速度與所述測(cè)量裝置的 真實(shí)前進(jìn)速度的比值，rf為由所述第一輔助傳感器測(cè)得的所述測(cè)量裝置的旋轉(zhuǎn)速度與所 述測(cè)量裝置的真實(shí)旋轉(zhuǎn)速度的比值；
[0066] 焯為所述被測(cè)對(duì)象沿所述第二輔助傳感器指向方向上的速度在X軸上的分量，if 為所述被測(cè)對(duì)象沿所述第二輔助傳感器指向方向上的速度在Y軸上的分量；
[0067] 為所述測(cè)量裝置的前進(jìn)速度矢量向所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑方向 的投影的大小，為所述測(cè)量裝置的前進(jìn)速度矢量向所述測(cè)量裝置和所述被測(cè)對(duì)象的矢 徑方向的法向的投影的大?。?br>[0068] 為所述被測(cè)對(duì)象的速度矢量向連接所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑方向 的投影的大小，》?ρ為所述被測(cè)對(duì)象的速度矢量向連接所述測(cè)量裝置和所述被測(cè)對(duì)象的矢 徑方向的法向的投影的大?。?br>[0069] Τ( ·)為二維空間中的旋轉(zhuǎn)變換矩陣，用于表示將其所后乘的向量逆時(shí)針旋轉(zhuǎn) (* )〇
[0070] 可選地，所述構(gòu)造單元，具體用于：
[0071 ]構(gòu)造狀態(tài)變量:Λ：=丨碎,W, χ為37, r；/, r/0]7 :
[0072] 構(gòu)造輸入變量:if =
[0073] 基于所述狀態(tài)變量和所述輸入變量，將所述數(shù)學(xué)模型調(diào)整為如下表達(dá)式，得到擴(kuò) 展卡爾曼濾波器：
[0075]其中，ν為測(cè)量噪聲向量，服從均值為0、協(xié)方差為R的高斯分布，ti為時(shí)間， i = 為系統(tǒng)微分方程模型，y = 孟,為系統(tǒng)輸出量，h為測(cè)量矩陣。
[0076] 可選地，所述修正單元，具體用于：
[0077]對(duì)所述擴(kuò)展卡爾曼濾波器進(jìn)行初始化;執(zhí)行遞推算法對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修 正，從而獲得到#和名的最優(yōu)狀態(tài)估計(jì)。
[0078] 可選地，所述修正單元，具體用于基于如下等式，對(duì)于所述擴(kuò)展卡爾曼濾波器進(jìn)行 初始化：
[0079] X(0) = i?[x(0)]
[0080] P(0) = ￡[0(0)- f (0))0(0)- f(0))r]
[0081] 其中，￡(〇)為系統(tǒng)初始狀態(tài)x(0)的估計(jì)值，P(0)為系統(tǒng)狀態(tài)轉(zhuǎn)移概率矩陣，E[ ·] 代表對(duì)?的期望。
[0082] 可選地，所述修正單元，具體用于基于如下等式，執(zhí)行遞推算法對(duì)所述第一測(cè)量數(shù) 據(jù)進(jìn)行修正：
[0083] 2 = f(x,u, ti) + K[ym - h(x, v0, i0]
[0084] 其中，K按照如下等式不斷更新：
[0085] R = M(x)RMT(x)
[0086] κ - PCT(x)R~1
[0087] p = A(x)P + Fi4r(f) ~ P^i^R-^CP
[0092] ?為系統(tǒng)狀態(tài)變量估計(jì)值的導(dǎo)數(shù);K為根據(jù)模擬系統(tǒng)和實(shí)際系統(tǒng)的誤差修正模擬系 統(tǒng)狀態(tài)的修正幅度;y m為含有噪聲的測(cè)量結(jié)果，其中包含相對(duì)位置傳感器輸出的相對(duì)距離 和相對(duì)夾角s孟:β為中間變量;P為傳播概率矩陣;戶為傳播概率矩陣的導(dǎo)數(shù)。
[0093] 本發(fā)明實(shí)施例中提供的一個(gè)或多個(gè)技術(shù)方案，至少具有如下技術(shù)效果或優(yōu)點(diǎn)：
[0094] 在本發(fā)明實(shí)施例中，公開(kāi)了一種改善相對(duì)位置傳感器性能的方法，應(yīng)用于測(cè)量裝 置中，測(cè)量裝置用于測(cè)量被測(cè)對(duì)象與測(cè)量裝置之間的相對(duì)位置，測(cè)量裝置上設(shè)置有相對(duì)位 置傳感器和第一輔助傳感器，被測(cè)對(duì)象上設(shè)置有第二輔助傳感器，所述方法包括:獲取由相 對(duì)位置傳感器測(cè)得的第一測(cè)量數(shù)據(jù)，以及獲取由第一輔助傳感器和第二輔助傳感器測(cè)得的 第二測(cè)量數(shù)據(jù);基于第一測(cè)量數(shù)據(jù)和第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器;利用擴(kuò)展卡爾 曼濾波器，對(duì)第一測(cè)量數(shù)據(jù)進(jìn)行修正。由于利用擴(kuò)展卡爾曼濾波器，對(duì)相對(duì)位置傳感器測(cè)得 的第一測(cè)量數(shù)據(jù)進(jìn)行修正，所以，有效解決了現(xiàn)有技術(shù)中的相對(duì)位置傳感器存在測(cè)量數(shù)據(jù) 容易受到干擾，產(chǎn)生跳動(dòng)，導(dǎo)致測(cè)量數(shù)據(jù)不準(zhǔn)確，動(dòng)態(tài)性能較差的技術(shù)問(wèn)題;進(jìn)而實(shí)現(xiàn)了提 高相對(duì)位置傳感器的測(cè)量數(shù)據(jù)的準(zhǔn)確性，從而改善相對(duì)位置傳感器的動(dòng)態(tài)性能的技術(shù)效 果。
【附圖說(shuō)明】
[0095] 為了更清楚地說(shuō)明本發(fā)明實(shí)施例中的技術(shù)方案，下面將對(duì)實(shí)施例描述中所需要使 用的附圖作一簡(jiǎn)單地介紹，顯而易見(jiàn)地，下面描述中的附圖是本發(fā)明的一些實(shí)施例，對(duì)于本 領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)這些附圖獲得其他的 附圖。
[0096] 圖1為本發(fā)明實(shí)施例中一種改善相對(duì)位置傳感器性能的方法的流程圖；
[0097] 圖2-圖3為本發(fā)明實(shí)施例中由測(cè)量裝置和被測(cè)對(duì)象組成的系統(tǒng)的模型的示意圖；
[0098] 圖4為本發(fā)明實(shí)施例中一種測(cè)量裝置的結(jié)構(gòu)示意圖。
【具體實(shí)施方式】
[0099] 本發(fā)明實(shí)施例通過(guò)提供一種改善相對(duì)位置傳感器性能的方法及裝置，解決了現(xiàn)有 技術(shù)中的相對(duì)位置傳感器存在測(cè)量數(shù)據(jù)容易受到干擾，產(chǎn)生跳動(dòng)，導(dǎo)致測(cè)量數(shù)據(jù)不準(zhǔn)確，動(dòng) 態(tài)性能較差的技術(shù)問(wèn)題。
[0100] 本發(fā)明實(shí)施例的技術(shù)方案為解決上述技術(shù)問(wèn)題，總體思路如下：
[0101] -種改善相對(duì)位置傳感器性能的方法，應(yīng)用于測(cè)量裝置中，測(cè)量裝置用于測(cè)量被 測(cè)對(duì)象與測(cè)量裝置之間的相對(duì)位置，測(cè)量裝置上設(shè)置有相對(duì)位置傳感器和第一輔助傳感 器，被測(cè)對(duì)象上設(shè)置有第二輔助傳感器，所述方法包括:獲取由相對(duì)位置傳感器測(cè)得的第一 測(cè)量數(shù)據(jù)，以及獲取由第一輔助傳感器和第二輔助傳感器測(cè)得的第二測(cè)量數(shù)據(jù);基于第一 測(cè)量數(shù)據(jù)和第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器;利用擴(kuò)展卡爾曼濾波器，對(duì)第一測(cè)量數(shù) 據(jù)進(jìn)行修正。
[0102] 為了更好的理解上述技術(shù)方案，下面將結(jié)合說(shuō)明書(shū)附圖以及具體的實(shí)施方式對(duì)上 述技術(shù)方案進(jìn)行詳細(xì)的說(shuō)明。
[0103] 首先說(shuō)明，本文中出現(xiàn)的術(shù)語(yǔ)"和/或"，僅僅是一種描述關(guān)聯(lián)對(duì)象的關(guān)聯(lián)關(guān)系，表 示可以存在三種關(guān)系，例如，A和/或B，可以表示:單獨(dú)存在A，同時(shí)存在A和B，單獨(dú)存在B這三 種情況。另外，本文中字符7"，一般表示前后關(guān)聯(lián)對(duì)象是一種"或"的關(guān)系。
[0104] 實(shí)施例一
[0105] 本實(shí)施例提供了一種改善相對(duì)位置傳感器性能的方法，應(yīng)用于測(cè)量裝置中，測(cè)量 裝置用于測(cè)量被測(cè)對(duì)象與測(cè)量裝置之間的相對(duì)位置，其中，在測(cè)量裝置上設(shè)置有相對(duì)位置 傳感器和第一輔助傳感器，在被測(cè)對(duì)象上設(shè)置有第二輔助傳感器。
[0106] 在具體實(shí)施過(guò)程中，測(cè)量裝置可以是:地面機(jī)器人、或自平衡車(chē)、或無(wú)人機(jī)、或電動(dòng) 汽車(chē)等設(shè)備，此處，對(duì)于所述測(cè)量裝置具體是何種設(shè)備，本實(shí)施例不做具體限定。
[0107] 在具體實(shí)施過(guò)程中，被測(cè)對(duì)象可以為靜止或移動(dòng)的人或物體。
[0108] 在具體實(shí)施過(guò)程中，可以在被測(cè)對(duì)象上設(shè)置一定位裝置(或被測(cè)對(duì)象攜帶有該定 位裝置），第二輔助傳感器設(shè)置在所述定位裝置內(nèi)，所述測(cè)量裝置實(shí)際上是測(cè)量定位裝置與 測(cè)量裝置之間的相對(duì)位置。其中，定位裝置可以是：智能手機(jī)、或平板電腦、或遙控鑰匙、或 健身設(shè)備、或個(gè)人數(shù)字助理、或游戲控制臺(tái)，等等。
[0109] 在具體實(shí)施過(guò)程中，慣性測(cè)量單元(英文全稱：Inertial Measurement Unit，英文 縮寫(xiě)：MU)，是由單個(gè)或多個(gè)加速度傳感器和角速度傳感器、微處理器和外圍電路組成的電 子系統(tǒng)。
[0110] 在本發(fā)明實(shí)施例中，第一輔助傳感器或第二輔助傳感器，包括:慣性測(cè)量單元(其 集成有陀螺儀、加速度計(jì)、以及電子羅盤(pán)等器件），測(cè)速傳感器(例如:碼盤(pán)、或光流傳感器）， 等等。一般，陀螺儀、加速度計(jì)、以及電子羅盤(pán)都可以看作是慣性測(cè)量單元。
[0111] 如圖1所示，所述改善相對(duì)位置傳感器性能的方法，包括：
[0112] 步驟S101:獲取由相對(duì)位置傳感器測(cè)得的第一測(cè)量數(shù)據(jù)，以及獲取由第一輔助傳 感器和第二輔助傳感器測(cè)得的第二測(cè)量數(shù)據(jù)。
[0113] 在具體實(shí)施過(guò)程中，第一測(cè)量數(shù)據(jù)，包括：
[0114] 測(cè)量裝置與被測(cè)對(duì)象之間的相對(duì)夾角S孟，以及測(cè)量裝置與被測(cè)對(duì)象之間的相對(duì)距 離此處，S孟和#是由設(shè)置在測(cè)量裝置上的相對(duì)位置傳感器測(cè)得的數(shù)據(jù)，其中含有噪 聲；
[0115] 在具體實(shí)施過(guò)程中，如圖2-圖3所示，第二測(cè)量數(shù)據(jù)，包括：
[0116] 被測(cè)對(duì)象的加速度在X軸上的分量，以及被測(cè)對(duì)象的加速度在Y軸上的分量 ，此處是由設(shè)置在被測(cè)對(duì)象上的第二輔助傳感器(例如：陀螺儀和/或加速 度計(jì))測(cè)量的數(shù)據(jù)，其中含有噪聲；
[0117] 測(cè)量裝置的前進(jìn)速度咬_m，以及測(cè)量裝置的旋轉(zhuǎn)速度此處，和！^m是由 設(shè)置在測(cè)量裝置上的第一輔助傳感器(例如:碼盤(pán))測(cè)得的數(shù)據(jù)，其中含有噪聲；
[0118] 測(cè)量裝置的正方向與地球磁北極的夾角9S，以及被測(cè)對(duì)象的正方向與地球磁北極 的夾角9t，其中，0 S是由設(shè)置在測(cè)量裝置上的第一輔助傳感器(例如：電子羅盤(pán))測(cè)得的數(shù)據(jù)， 是由設(shè)置在被測(cè)對(duì)象上的第二輔助傳感器(例如：電子羅盤(pán))測(cè)得的數(shù)據(jù)。
[0119] 在具體實(shí)施過(guò)程中，測(cè)量裝置可以通過(guò)無(wú)線通信技術(shù)與被測(cè)對(duì)象上的定位裝置進(jìn) 行通信，從而獲取第二輔助傳感器測(cè)量的數(shù)據(jù)。其中，所述線通信技術(shù)，可以為:UWB(Ultra Wideband，超寬帶)技術(shù)、或 WiFi (Wire less Fidelity，無(wú)線保真)技術(shù)、或藍(lán)牙(Bluetooth) 技術(shù)，等等。
[0120] 步驟S102:基于第一測(cè)量數(shù)據(jù)和第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器。
[0121] 擴(kuò)展卡爾曼濾波器(英文全稱:Extended Kalman Filter，英文縮寫(xiě):EKF)，是一種 非線性系統(tǒng)最優(yōu)狀態(tài)估計(jì)器，通過(guò)融合多個(gè)具有線性或非線性關(guān)系的、且不夠準(zhǔn)確的傳感 器的測(cè)量數(shù)據(jù)，得到比單一傳感器精度更高的測(cè)量結(jié)果。本質(zhì)上是遞推最小二乘法。
[0122] 在本發(fā)明實(shí)施例中，在通過(guò)相對(duì)位置傳感器測(cè)量被測(cè)對(duì)象與測(cè)量裝置之間的相對(duì) 位置時(shí)，利用擴(kuò)展卡爾曼濾波器將第一測(cè)量數(shù)據(jù)和第二測(cè)量進(jìn)行數(shù)據(jù)融合，以對(duì)第一測(cè)量 數(shù)據(jù)進(jìn)行修正，從而提高相對(duì)位置傳感器的準(zhǔn)確性、精確性、以及動(dòng)態(tài)性能。
[0123] 作為一種可選的實(shí)施方式，在步驟S102之前，還包括：
[0124] 對(duì)由測(cè)量裝置和被測(cè)對(duì)象組成的系統(tǒng)進(jìn)行數(shù)學(xué)建模，獲得數(shù)學(xué)模型，如下：
[0126] 其中，If為被測(cè)對(duì)象的加速度在X軸上的零偏，為被測(cè)對(duì)象的加速度在Y軸上的 零偏；
[0127] r/為由第一輔助傳感器(例如:碼盤(pán)、或光流傳感器)測(cè)得的測(cè)量裝置的前進(jìn)速度 與測(cè)量裝置的真實(shí)前進(jìn)速度的比值，rf為由第一輔助傳感器(例如:碼盤(pán)、或光流傳感器)測(cè) 得的測(cè)量裝置的旋轉(zhuǎn)速度與測(cè)量裝置的真實(shí)旋轉(zhuǎn)速度的比值;此處，< 和if 一般不為1，且 與測(cè)量裝置的車(chē)輪直徑和間距相關(guān)，其可以反映測(cè)量裝置的幾何尺寸的變化。
[0128] Vf為被測(cè)對(duì)象沿第二輔助傳感器(例如:設(shè)置在被測(cè)對(duì)象上的慣性測(cè)量單元)指向 方向上的速度在X軸上的分量，if為被測(cè)對(duì)象沿第二輔助傳感器(例如:設(shè)置在被測(cè)對(duì)象上 的慣性測(cè)量單元）指向方向上的速度在Y軸上的分量；（甿和圖2中重合，<和圖2中 Gf_講重合）；
[0129] vfp為測(cè)量裝置的前進(jìn)速度矢量向測(cè)量裝置和被測(cè)對(duì)象的矢徑方向的投影的大 小，仏為測(cè)量裝置的前進(jìn)速度矢量向測(cè)量裝置和被測(cè)對(duì)象的矢徑方向的法向的投影的大 ??；
[0130] 為被測(cè)對(duì)象的速度矢量向連接測(cè)量裝置和被測(cè)對(duì)象的矢徑方向的投影的大 小，為被測(cè)對(duì)象的速度矢量向連接測(cè)量裝置和被測(cè)對(duì)象的矢徑方向的法向的投影的大 ??；
[0131] T( ·)為二維空間中的旋轉(zhuǎn)變換矩陣，用于表示將其所后乘的向量逆時(shí)針旋轉(zhuǎn) (* )〇
[0132] 在具體實(shí)施過(guò)程中，擴(kuò)展卡爾曼濾波器大體上可以分為兩步實(shí)現(xiàn)：
[0133] 第一步，預(yù)測(cè)。
[0134] 建立的基于系統(tǒng)差分方程/微分方程的模型相當(dāng)于對(duì)現(xiàn)實(shí)世界中對(duì)應(yīng)的系統(tǒng)的一 種"模擬"，給定此模擬系統(tǒng)一系列輸入量(或稱"驅(qū)動(dòng)力"），則模擬系統(tǒng)的狀態(tài)會(huì)不斷變化。 由于模擬系統(tǒng)已經(jīng)對(duì)實(shí)際系統(tǒng)進(jìn)行了較為完善的建模，且驅(qū)動(dòng)力在"驅(qū)動(dòng)"模擬系統(tǒng)的狀態(tài) 更新時(shí)也在同時(shí)"驅(qū)動(dòng)"實(shí)際系統(tǒng)運(yùn)行，因此模型和實(shí)際系統(tǒng)的狀態(tài)變化幾乎是同時(shí)、同樣 的。因此在通過(guò)各種傳感器(即：相對(duì)位置傳感器、第一輔助傳感器和第二輔助傳感器)采集 實(shí)際系統(tǒng)的狀態(tài)之前，已經(jīng)可以從模擬系統(tǒng)中獲取系統(tǒng)狀態(tài)了，這稱為"狀態(tài)預(yù)測(cè)"。傳統(tǒng) 上，系統(tǒng)狀態(tài)方程是線性的。而擴(kuò)展卡爾曼濾波器應(yīng)用于非線性系統(tǒng)狀態(tài)預(yù)測(cè)時(shí)，將非線性 系統(tǒng)在當(dāng)前狀態(tài)處進(jìn)行一階泰勒展開(kāi)，得到近似的系統(tǒng)狀態(tài)更新微分方程。
[0135] 第二步，修正。
[0136] 在理想情況下，理應(yīng)可以直接使用第一步的預(yù)測(cè)值做狀態(tài)反饋控制。然而，在現(xiàn)實(shí) 中會(huì)存在兩大問(wèn)題：1、不確定初值的問(wèn)題;2、建模不準(zhǔn)確的問(wèn)題。這兩個(gè)問(wèn)題會(huì)使得模擬系 統(tǒng)的狀態(tài)逐漸偏離實(shí)際狀態(tài)。為了解決這個(gè)問(wèn)題，在本實(shí)施例中，擴(kuò)展卡爾曼濾波器通過(guò)相 對(duì)位置傳感器采集到的實(shí)際系統(tǒng)輸出和模擬系統(tǒng)的輸出之間的差值不斷修正模擬系統(tǒng)的 狀態(tài)。由于相對(duì)位置傳感器采集到的數(shù)據(jù)存在誤差，因此修正的幅度滿足須經(jīng)過(guò)計(jì)算使之 最優(yōu)估計(jì)準(zhǔn)則，即使得模擬系統(tǒng)的輸出中受修正量噪聲引起的噪聲的方差最小。
[0137] 從以上兩步算法可知，擴(kuò)展卡爾曼濾波器是依賴于系統(tǒng)建模的、利用第二測(cè)量數(shù) 據(jù)間接地估計(jì)系統(tǒng)狀態(tài)的算法。由于系統(tǒng)模型的存在和最優(yōu)估計(jì)方法，擴(kuò)展卡爾曼濾波器 相比 FIR(Finite Impulse Response，有限脈沖響應(yīng)）濾波器、IIR(Infinite Impulse Response，無(wú)限沖擊響應(yīng))濾波器等直接對(duì)相對(duì)位置傳感器數(shù)據(jù)濾波的方法，有響應(yīng)實(shí)時(shí)、 精度高的優(yōu)點(diǎn)，而且可以通過(guò)融合多個(gè)傳感器的數(shù)據(jù)進(jìn)一步提高精度。
[0138] 作為一種可選的實(shí)施方式，基于第一測(cè)量數(shù)據(jù)和第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼 濾波器，包括：
[0139]首先，構(gòu)造狀態(tài)變量:χ = [vf,《,s'rf,;
[0140]在這里，不僅將被測(cè)目標(biāo)真實(shí)速度（即：Ft%、if)和被測(cè)目標(biāo)真實(shí)相對(duì)位置(測(cè)量 裝置與被測(cè)對(duì)象之間的真實(shí)的相對(duì)夾角sa、測(cè)量裝置與被測(cè)對(duì)象之間的真實(shí)的相對(duì)距離sp) 作為需要觀測(cè)的狀態(tài)變量，也將會(huì)隨時(shí)間變化的慣性傳感器零點(diǎn)偏移、測(cè)量裝置幾何尺寸 變化(例如：自平衡車(chē)的車(chē)身幾何尺寸的變化)作為觀測(cè)量。這樣完整的建?？梢詷O大地提 高測(cè)量的準(zhǔn)確性。
[0141 ]然后，構(gòu)造輸入變量:= 為為]7;
[0142]最后，基于狀態(tài)變量和輸入變量，將數(shù)學(xué)模型調(diào)整為如下表達(dá)式，得到擴(kuò)展卡爾曼 濾波器：
[0144]其中，v為測(cè)量噪聲向量，服從均值為0、協(xié)方差為R的高斯分布，ti為時(shí)間， i = /'(x.u, t〇為系統(tǒng)微分方程模型（即：數(shù)學(xué)模型的等式1)，y =[成,4]為系統(tǒng)輸出量，h為測(cè) 量矩陣：
[0146] 步驟S103:利用擴(kuò)展卡爾曼濾波器，對(duì)第一測(cè)量數(shù)據(jù)進(jìn)行修正。
[0147] 作為一種可選的實(shí)施方式，步驟S103，包括：
[0148] 首先，對(duì)擴(kuò)展卡爾曼濾波器進(jìn)行初始化；
[0149] 然后，執(zhí)行遞推算法對(duì)第一測(cè)量數(shù)據(jù)進(jìn)行修正，從而獲得到難和'的最優(yōu)狀態(tài)估 計(jì)。
[0150] 作為一種可選的實(shí)施方式，可以基于如下等式，對(duì)于擴(kuò)展卡爾曼濾波器進(jìn)行初始 化：
[0151] χ(0) = Ε[χ(0)]
[0152] P(0) = ￡[0(0)- f (0))0(0)- f(0))r]
[0153] 其中，f(:〇:)為系統(tǒng)初始狀態(tài)x(0)的估計(jì)值，P(0)為系統(tǒng)狀態(tài)轉(zhuǎn)移概率矩陣，E[ ·] 代表對(duì)?的期望。
[0154] 作為一種可選的實(shí)施方式，可以基于如下等式，執(zhí)行遞推算法對(duì)第一測(cè)量數(shù)據(jù)進(jìn) 行修正：
[0155] π = f{x,u, ti) + K[ym - h(x, v0, ??)]
[0156] 上式中，第一項(xiàng)/(兄部分是"預(yù)測(cè)"，即通過(guò)等式（1)作為模擬系統(tǒng)"驅(qū)動(dòng)力" 部分的傳感器數(shù)據(jù)更新模擬系統(tǒng)的狀態(tài)，而第二項(xiàng)耵7^11(1^()，^)]為"修正"。1(代表了根 據(jù)模擬系統(tǒng)和實(shí)際系統(tǒng)的誤差修正模擬系統(tǒng)狀態(tài)的修正幅度，這一修正要滿足最優(yōu)估計(jì)準(zhǔn) 則，則K需要按照以下方式不斷更新：
[0157] I = M(x)RMr(x)
[0158] K .= PCT(β)鉸-1
[0159] p = A{x)P + PAT(x) - PC^X^R^CP
[0160] 其中，全為系統(tǒng)狀態(tài)變量估計(jì)值的導(dǎo)數(shù);K為根據(jù)模擬系統(tǒng)和實(shí)際系統(tǒng)的誤差修正 模擬系統(tǒng)狀態(tài)的修正幅度;ym為含有噪聲的測(cè)量結(jié)果，其中包含相對(duì)位置傳感器輸出的相 對(duì)距離和相對(duì)夾角請(qǐng)為中間變量;P為傳播概率矩陣;聲為傳播概率矩陣的導(dǎo)數(shù)；
[0164] 以上一階偏導(dǎo)數(shù)矩陣即為系統(tǒng)在當(dāng)前狀態(tài)下進(jìn)行局部線性化的矩陣，其中，A是狀 態(tài)轉(zhuǎn)移函數(shù)f對(duì)狀態(tài)變量X的偏導(dǎo)數(shù)矩陣，C是測(cè)量矩陣h對(duì)狀態(tài)變量X的偏導(dǎo)數(shù)矩陣，Μ是測(cè) 量矩陣h對(duì)測(cè)量噪聲ν的偏導(dǎo)數(shù)矩陣。
[0165] 上式中，ym*含有噪聲的測(cè)量結(jié)果，包含相對(duì)位置傳感器輸出的相對(duì)距離和相對(duì) 角度。類似于傳統(tǒng)的狀態(tài)觀測(cè)器，擴(kuò)展卡爾曼濾波器通過(guò)狀態(tài)空間方程的輸入量u更新?tīng)?態(tài)，并通過(guò)估計(jì)輸出結(jié)果和實(shí)際輸出的偏差不斷修正擴(kuò)展卡爾曼濾波器的狀態(tài)變量。從直 觀上來(lái)講，其修正狀態(tài)的"速率"相對(duì)于迅速跳動(dòng)的測(cè)量噪聲來(lái)說(shuō)足夠緩慢，而對(duì)于真實(shí)的 狀態(tài)變化又足夠迅速，因此可以從被噪聲污染的測(cè)量數(shù)據(jù)中幾乎無(wú)滯后地提取真實(shí)值。
[0166] 上述本發(fā)明實(shí)施例中的技術(shù)方案，至少具有如下的技術(shù)效果或優(yōu)點(diǎn)：
[0167] 在本發(fā)明實(shí)施例中，公開(kāi)了一種改善相對(duì)位置傳感器性能的方法，應(yīng)用于測(cè)量裝 置中，測(cè)量裝置用于測(cè)量被測(cè)對(duì)象與測(cè)量裝置之間的相對(duì)位置，測(cè)量裝置上設(shè)置有相對(duì)位 置傳感器和第一輔助傳感器，被測(cè)對(duì)象上設(shè)置有第二輔助傳感器，方法包括:獲取由相對(duì)位 置傳感器測(cè)得的第一測(cè)量數(shù)據(jù)，以及獲取由第一輔助傳感器和第二輔助傳感器測(cè)得的第二 測(cè)量數(shù)據(jù);基于第一測(cè)量數(shù)據(jù)和第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器;利用擴(kuò)展卡爾曼濾 波器，對(duì)第一測(cè)量數(shù)據(jù)進(jìn)行修正。由于采用了利用擴(kuò)展卡爾曼濾波器，對(duì)相對(duì)位置傳感器測(cè) 得的第一測(cè)量數(shù)據(jù)進(jìn)行修正，所以，有效解決了現(xiàn)有技術(shù)中的相對(duì)位置傳感器存在測(cè)量數(shù) 據(jù)容易受到干擾，產(chǎn)生跳動(dòng)，導(dǎo)致測(cè)量數(shù)據(jù)不準(zhǔn)確，動(dòng)態(tài)性能較差的技術(shù)問(wèn)題。進(jìn)而實(shí)現(xiàn)了 提高相對(duì)位置傳感器的測(cè)量數(shù)據(jù)的準(zhǔn)確性，從而改善相對(duì)位置傳感器的動(dòng)態(tài)性能的技術(shù)效 果。
[0168] 實(shí)施例二
[0169] 基于同一發(fā)明構(gòu)思，本發(fā)明另一實(shí)施例提供一種實(shí)施本發(fā)明實(shí)施例中改善相對(duì)位 置傳感器性能的方法的測(cè)量裝置。
[0170]如圖4所示，一種測(cè)量裝置，測(cè)量裝置用于測(cè)量被測(cè)對(duì)象與測(cè)量裝置之間的相對(duì)位 置，其特征在于，測(cè)量裝置上設(shè)置有相對(duì)位置傳感器和第一輔助傳感器，被測(cè)對(duì)象上設(shè)置有 第二輔助傳感器，測(cè)量裝置包括：
[0171]獲取單元401，用于獲取由相對(duì)位置傳感器測(cè)得的第一測(cè)量數(shù)據(jù)，以及獲取由第一 輔助傳感器和第二輔助傳感器測(cè)得的第二測(cè)量數(shù)據(jù)；
[0172]構(gòu)造單元402，用于基于第一測(cè)量數(shù)據(jù)和第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器； [0173]修正單元403,用于利用擴(kuò)展卡爾曼濾波器，對(duì)第一測(cè)量數(shù)據(jù)進(jìn)行修正。
[0174] 作為一種可選的實(shí)施方式，第一測(cè)量數(shù)據(jù)，包括：
[0175] 測(cè)量裝置與被測(cè)對(duì)象之間的相對(duì)夾角，以及測(cè)量裝置與被測(cè)對(duì)象之間的相對(duì) 距離4。
[0176] 作為一種可選的實(shí)施方式，第二測(cè)量數(shù)據(jù)，包括：
[0177] 被測(cè)對(duì)象的加速度在X軸上的分量，以及被測(cè)對(duì)象的加速度在γ軸上的分量
[0178] 測(cè)量裝置的前進(jìn)速度P/m，以及測(cè)量裝置的旋轉(zhuǎn)速度;
[0179] 測(cè)量裝置的正方向與地球磁北極的夾角9S，以及被測(cè)對(duì)象的正方向與地球磁北極 的夾角9t。
[0180] 作為一種可選的實(shí)施方式，測(cè)量裝置，還包括：
[0181 ]建模單元，用于基于第一測(cè)量數(shù)據(jù)和第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器之前， 對(duì)由測(cè)量裝置和被測(cè)對(duì)象所組成的系統(tǒng)進(jìn)行數(shù)學(xué)建模，獲得數(shù)學(xué)模型：
[0183] 其中，掉為被測(cè)對(duì)象的加速度在X軸上的零偏，&『為被測(cè)對(duì)象的加速度在Y軸上的 零偏；
[0184] rf為由第一輔助傳感器測(cè)得的測(cè)量裝置的前進(jìn)速度與測(cè)量裝置的真實(shí)前進(jìn)速度 的比值，rf為由第一輔助傳感器測(cè)得的測(cè)量裝置的旋轉(zhuǎn)速度與測(cè)量裝置的真實(shí)旋轉(zhuǎn)速度 的比值；
[0185] 磅為被測(cè)對(duì)象沿第二輔助傳感器指向方向上的速度在X軸上的分量，if為被測(cè)對(duì) 象沿第二輔助傳感器指向方向上的速度在Y軸上的分量；
[0186] 咬p為測(cè)量裝置的前進(jìn)速度矢量向測(cè)量裝置和被測(cè)對(duì)象的矢徑方向的投影的大 小，< P為測(cè)量裝置的前進(jìn)速度矢量向測(cè)量裝置和被測(cè)對(duì)象的矢徑方向的法向的投影的大 ?。?br>[0187] 為被測(cè)對(duì)象的速度矢量向連接測(cè)量裝置和被測(cè)對(duì)象的矢徑方向的投影的大 小，為被測(cè)對(duì)象的速度矢量向連接測(cè)量裝置和被測(cè)對(duì)象的矢徑方向的法向的投影的大 ??；
[0188] Τ( ·)為二維空間中的旋轉(zhuǎn)變換矩陣，用于表示將其所后乘的向量逆時(shí)針旋轉(zhuǎn) (* )〇
[0189] 作為一種可選的實(shí)施方式，構(gòu)造單元402，具體用于：
[0190] 構(gòu)造狀態(tài)變量:X: = , sK 時(shí)',r/, rsw]7
[0191 ]構(gòu)造輸入變量：w = <饑，?文0S為]\
[0192]基于狀態(tài)變量和輸入變量，將數(shù)學(xué)模型調(diào)整為如下表達(dá)式，得到擴(kuò)展卡爾曼濾波 器：
[0194] 其中，v為測(cè)量噪聲向量，服從均值為0、協(xié)方差為R的高斯分布，ti為時(shí)間， 戈=/'(X,LUi)為系統(tǒng)微分方程模型，y = 為系統(tǒng)輸出量，h為測(cè)量矩陣。
[0195] 作為一種可選的實(shí)施方式，修正單元403，具體用于：
[0196] 對(duì)擴(kuò)展卡爾曼濾波器進(jìn)行初始化;執(zhí)行遞推算法對(duì)第一測(cè)量數(shù)據(jù)進(jìn)行修正，從而 獲得到和的最優(yōu)狀態(tài)估計(jì)。
[0197] 作為一種可選的實(shí)施方式，修正單元403,具體用于基于如下等式，對(duì)于擴(kuò)展卡爾 曼濾波器進(jìn)行初始化：
[0198] Λ：(0) = ii[x(0)]
[0199] />(0) = Ε[(χ(0) - f (0))(χ(0) - f (0))η
[0200] 其中，f(〇)為系統(tǒng)初始狀態(tài)x(〇)的估計(jì)值，P(〇)為系統(tǒng)狀態(tài)轉(zhuǎn)移概率矩陣，E[ ·] 代表對(duì)?的期望。
[0201 ]作為一種可選的實(shí)施方式，修正單元403，具體用于基于如下等式，執(zhí)行遞推算法 對(duì)第一測(cè)量數(shù)據(jù)進(jìn)行修正：
[0202] jc = f (x; u, ti) + K[ym - h{x, v0, ti)]
[0203] 其中，K按照如下等式不斷更新：
[0204] R = M(x)RMT(x)
[0205] K = Ρ?；Γ(χ)^"1
[0206] Ρ = Α(χ)Ρ + ΡΑτ(χ) - PC^Cx)/?-^；?
[0207] 其中，I為系統(tǒng)狀態(tài)變量估計(jì)值的導(dǎo)數(shù);Κ為根據(jù)模擬系統(tǒng)和實(shí)際系統(tǒng)的誤差修正 模擬系統(tǒng)狀態(tài)的修正幅度;ym為含有噪聲的測(cè)量結(jié)果，其中包含相對(duì)位置傳感器輸出的相 對(duì)距離和相對(duì)夾角S洽d為中間變量;ρ為傳播概率矩陣汴為傳播概率矩陣的導(dǎo)數(shù)；
[0211] 由于本實(shí)施例所介紹的測(cè)量裝置為實(shí)施本發(fā)明實(shí)施例中改善相對(duì)位置傳感器性 能的方法所采用的裝置，故而基于本發(fā)明實(shí)施例中所介紹的改善相對(duì)位置傳感器性能的方 法，本領(lǐng)域所屬技術(shù)人員能夠了解本實(shí)施例的測(cè)量裝置的【具體實(shí)施方式】以及其各種變化形 式，所以在此對(duì)于該測(cè)量裝置如何實(shí)現(xiàn)本發(fā)明實(shí)施例中的方法不再詳細(xì)介紹。只要本領(lǐng)域 所屬技術(shù)人員實(shí)施本發(fā)明實(shí)施例中改善相對(duì)位置傳感器性能的方法所采用的裝置，都屬于 本發(fā)明所欲保護(hù)的范圍。
[0212] 上述本發(fā)明實(shí)施例中的技術(shù)方案，至少具有如下的技術(shù)效果或優(yōu)點(diǎn)：
[0213] 在本發(fā)明實(shí)施例中，公開(kāi)了一種測(cè)量裝置，測(cè)量裝置用于測(cè)量被測(cè)對(duì)象與測(cè)量裝 置之間的相對(duì)位置，測(cè)量裝置上設(shè)置有相對(duì)位置傳感器和第一輔助傳感器，被測(cè)對(duì)象上設(shè) 置有第二輔助傳感器，測(cè)量裝置，包括:獲取單元，用于獲取由相對(duì)位置傳感器測(cè)得的第一 測(cè)量數(shù)據(jù)，以及獲取由第一輔助傳感器和第二輔助傳感器測(cè)得的第二測(cè)量數(shù)據(jù);構(gòu)造單元， 用于基于第一測(cè)量數(shù)據(jù)和第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器;修正單元，用于利用擴(kuò)展 卡爾曼濾波器，對(duì)第一測(cè)量數(shù)據(jù)進(jìn)行修正。由于采用了利用擴(kuò)展卡爾曼濾波器，對(duì)相對(duì)位置 傳感器測(cè)得的第一測(cè)量數(shù)據(jù)進(jìn)行修正，所以，有效解決了現(xiàn)有技術(shù)中的相對(duì)位置傳感器存 在測(cè)量數(shù)據(jù)容易受到干擾，產(chǎn)生跳動(dòng)，導(dǎo)致測(cè)量數(shù)據(jù)不準(zhǔn)確，動(dòng)態(tài)性能較差的技術(shù)問(wèn)題。進(jìn) 而實(shí)現(xiàn)了提高相對(duì)位置傳感器的測(cè)量數(shù)據(jù)的準(zhǔn)確性，從而改善相對(duì)位置傳感器的動(dòng)態(tài)性能 的技術(shù)效果。
[0214] 本領(lǐng)域內(nèi)的技術(shù)人員應(yīng)明白，本發(fā)明的實(shí)施例可提供為方法、系統(tǒng)、或計(jì)算機(jī)程序 產(chǎn)品。因此，本發(fā)明可采用完全硬件實(shí)施例、完全軟件實(shí)施例、或結(jié)合軟件和硬件方面的實(shí) 施例的形式。而且，本發(fā)明可采用在一個(gè)或多個(gè)其中包含有計(jì)算機(jī)可用程序代碼的計(jì)算機(jī) 可用存儲(chǔ)介質(zhì)（包括但不限于磁盤(pán)存儲(chǔ)器、CD-ROM、光學(xué)存儲(chǔ)器等）上實(shí)施的計(jì)算機(jī)程序產(chǎn) 品的形式。
[0215] 本發(fā)明是參照根據(jù)本發(fā)明實(shí)施例的方法、設(shè)備（系統(tǒng)）、和計(jì)算機(jī)程序產(chǎn)品的流程 圖和/或方框圖來(lái)描述的。應(yīng)理解可由計(jì)算機(jī)程序指令實(shí)現(xiàn)流程圖和/或方框圖中的每一流 程和/或方框、以及流程圖和/或方框圖中的流程和/或方框的結(jié)合。可提供這些計(jì)算機(jī)程序 指令到通用計(jì)算機(jī)、專用計(jì)算機(jī)、嵌入式處理機(jī)或其他可編程數(shù)據(jù)處理設(shè)備的處理器以產(chǎn) 生一個(gè)機(jī)器，使得通過(guò)計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備的處理器執(zhí)行的指令產(chǎn)生用于實(shí) 現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一個(gè)方框或多個(gè)方框中指定的功能的裝置。
[0216] 這些計(jì)算機(jī)程序指令也可存儲(chǔ)在能引導(dǎo)計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備以特 定方式工作的計(jì)算機(jī)可讀存儲(chǔ)器中，使得存儲(chǔ)在該計(jì)算機(jī)可讀存儲(chǔ)器中的指令產(chǎn)生包括指 令裝置的制造品，該指令裝置實(shí)現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一個(gè)方框或 多個(gè)方框中指定的功能。
[0217] 這些計(jì)算機(jī)程序指令也可裝載到計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備上，使得在計(jì) 算機(jī)或其他可編程設(shè)備上執(zhí)行一系列操作步驟以產(chǎn)生計(jì)算機(jī)實(shí)現(xiàn)的處理，從而在計(jì)算機(jī)或 其他可編程設(shè)備上執(zhí)行的指令提供用于實(shí)現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一 個(gè)方框或多個(gè)方框中指定的功能的步驟。
[0218] 盡管已描述了本發(fā)明的優(yōu)選實(shí)施例，但本領(lǐng)域內(nèi)的技術(shù)人員一旦得知了基本創(chuàng)造 性概念，則可對(duì)這些實(shí)施例作出另外的變更和修改。所以，所附權(quán)利要求意欲解釋為包括優(yōu) 選實(shí)施例以及落入本發(fā)明范圍的所有變更和修改。
[0219] 顯然，本領(lǐng)域的技術(shù)人員可以對(duì)本發(fā)明進(jìn)行各種改動(dòng)和變型而不脫離本發(fā)明的精 神和范圍。這樣，倘若本發(fā)明的這些修改和變型屬于本發(fā)明權(quán)利要求及其等同技術(shù)的范圍 之內(nèi)，則本發(fā)明也意圖包含這些改動(dòng)和變型在內(nèi)。
【主權(quán)項(xiàng)】
1. 一種改善相對(duì)位置傳感器性能的方法，應(yīng)用于測(cè)量裝置中，所述測(cè)量裝置用于測(cè)量 被測(cè)對(duì)象與所述測(cè)量裝置之間的相對(duì)位置，其特征在于，所述測(cè)量裝置上設(shè)置有相對(duì)位置 傳感器和第一輔助傳感器，所述被測(cè)對(duì)象上設(shè)置有第二輔助傳感器，所述方法包括： 獲取由所述相對(duì)位置傳感器測(cè)得的第一測(cè)量數(shù)據(jù)，W及獲取由所述第一輔助傳感器和 所述第二輔助傳感器測(cè)得的第二測(cè)量數(shù)據(jù)； 基于所述第一測(cè)量數(shù)據(jù)和所述第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器； 利用所述擴(kuò)展卡爾曼濾波器，對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正。2. 如權(quán)利要求1所述的改善相對(duì)位置傳感器性能的方法，其特征在于，所述第一測(cè)量數(shù) 據(jù)，包括： 所述測(cè)量裝置與所述被測(cè)對(duì)象之間的相對(duì)夾角S及,W及所述測(cè)量裝置與所述被測(cè)對(duì)象 之間的相對(duì)距離S寡。3. 如權(quán)利要求2所述的改善相對(duì)位置傳感器性能的方法，其特征在于，所述第二測(cè)量數(shù) 據(jù)，包括： 所述被測(cè)對(duì)象的加速度在X軸上的分量口是及所述被測(cè)對(duì)象的加速度在Y軸上的分 量也1; 所述測(cè)量裝置的前進(jìn)速度及所述測(cè)量裝置的旋轉(zhuǎn)速度 所述測(cè)量裝置的正方向與地球磁北極的夾角9s，W及所述被測(cè)對(duì)象的正方向與地球磁 北極的夾角9t。4. 如權(quán)利要求3所述的改善相對(duì)位置傳感器性能的方法，其特征在于，在所述基于所述 第一測(cè)量數(shù)據(jù)和所述第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器之前，還包括： 對(duì)由所述測(cè)量裝置和所述被測(cè)對(duì)象所組成的系統(tǒng)進(jìn)行數(shù)學(xué)建模，獲得數(shù)學(xué)模型：其中，為所述被測(cè)對(duì)象的加速度在X軸上的零偏，磚為所述被測(cè)對(duì)象的加速度在Y軸 上的零偏； 詩(shī)為由所述第一輔助傳感器測(cè)得的所述測(cè)量裝置的前進(jìn)速度與所述測(cè)量裝置的真實(shí) 前進(jìn)速度的比值，η;"為由所述第一輔助傳感器測(cè)得的所述測(cè)量裝置的旋轉(zhuǎn)速度與所述測(cè) 量裝置的真實(shí)旋轉(zhuǎn)速度的比值； Vt3'為所述被測(cè)對(duì)象沿所述第二輔助傳感器指向方向上的速度在X軸上的分量，uf為所 述被測(cè)對(duì)象沿所述第二輔助傳感器指向方向上的速度在Y軸上的分量； 每?ρ為所述測(cè)量裝置的前進(jìn)速度矢量向所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑方向的投 影的大小，為所述測(cè)量裝置的前進(jìn)速度矢量向所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑方 向的法向的投影的大?。? ufp為所述被測(cè)對(duì)象的速度矢量向連接所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑方向的 投影的大小，為所述被測(cè)對(duì)象的速度矢量向連接所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑 方向的法向的投影的大?。? Τ( ·)為二維空間中的旋轉(zhuǎn)變換矩陣。5. 如權(quán)利要求4所述的改善相對(duì)位置傳感器性能的方法，其特征在于，所述基于所述第 一測(cè)量數(shù)據(jù)和所述第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器，包括：基于所述狀態(tài)變量和所述輸入變量，將所述數(shù)學(xué)模型調(diào)整為如下表達(dá)式，得到擴(kuò)展卡 爾曼濾波器：其中，V為測(cè)量噪聲向量，服從均值為0、協(xié)方差為R的高斯分布，t i為時(shí)間， 文二/0,斯扣)為系統(tǒng)微分方程模型：為系統(tǒng)輸出量，h為測(cè)量矩陣。6. 如權(quán)利要求5所述的改善相對(duì)位置傳感器性能的方法，其特征在于，所述利用所述擴(kuò) 展卡爾曼濾波器，對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正，包括： 對(duì)所述擴(kuò)展卡爾曼濾波器進(jìn)行初始化； 執(zhí)行遞推算法對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正，從而獲得到S寡和苗> 的最優(yōu)狀態(tài)估計(jì)。7. 如權(quán)利要求6所述的改善相對(duì)位置傳感器性能的方法，其特征在于，基于如下等式， 對(duì)于所述擴(kuò)展卡爾曼濾波器進(jìn)行初始化：其中，克C0；)為系統(tǒng)初始狀態(tài)x(〇)的估計(jì)值，P(〇)為系統(tǒng)狀態(tài)轉(zhuǎn)移概率矩陣，E[ ·]代表 對(duì)?的期望。8. 如權(quán)利要求7所述的改善相對(duì)位置傳感器性能的方法，其特征在于，基于如下等式， 執(zhí)行遞推算法對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正：條為系統(tǒng)狀態(tài)變量估計(jì)值的導(dǎo)數(shù);K為根據(jù)模擬系統(tǒng)和實(shí)際系統(tǒng)的誤差修正模擬系統(tǒng)狀 態(tài)的修正幅度;ym為含有噪聲的測(cè)量結(jié)果，其中包含相對(duì)位置傳感器輸出的相對(duì)距離.讀和 相對(duì)夾角S爲(wèi)；β為中間變量;P為傳播概率矩陣;P為傳播概率矩陣的導(dǎo)數(shù)。9. 一種測(cè)量裝置，所述測(cè)量裝置用于測(cè)量被測(cè)對(duì)象與所述測(cè)量裝置之間的相對(duì)位置， 其特征在于，所述測(cè)量裝置上設(shè)置有相對(duì)位置傳感器和第一輔助傳感器，所述被測(cè)對(duì)象上 設(shè)置有第二輔助傳感器，所述測(cè)量裝置包括： 獲取單元，用于獲取由所述相對(duì)位置傳感器測(cè)得的第一測(cè)量數(shù)據(jù)，W及獲取由所述第 一輔助傳感器和所述第二輔助傳感器測(cè)得的第二測(cè)量數(shù)據(jù)； 構(gòu)造單元，用于基于所述第一測(cè)量數(shù)據(jù)和所述第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾波器； 修正單元，用于利用所述擴(kuò)展卡爾曼濾波器，對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正。10. 如權(quán)利要求9所述的測(cè)量裝置，其特征在于，所述第一測(cè)量數(shù)據(jù)，包括： 所述測(cè)量裝置與所述被測(cè)對(duì)象之間的相對(duì)夾角S惡，W及所述測(cè)量裝置與所述被測(cè)對(duì) 象之間的相對(duì)距離端。11. 如權(quán)利要求10所述的測(cè)量裝置，其特征在于，所述第二測(cè)量數(shù)據(jù)，包括： 所述被測(cè)對(duì)象的加速度在X軸上的分量W及所述被測(cè)對(duì)象的加速度在Υ軸上的分 量α[機(jī)； 所述測(cè)量裝置的前進(jìn)速度哦及所述測(cè)量裝置的旋轉(zhuǎn)速度增m.; 所述測(cè)量裝置的正方向與地球磁北極的夾角9s，W及所述被測(cè)對(duì)象的正方向與地球磁 北極的夾角9t。12. 如權(quán)利要求11所述的測(cè)量裝置，其特征在于，還包括： 建模單元，用于所述基于所述第一測(cè)量數(shù)據(jù)和所述第二測(cè)量數(shù)據(jù)，構(gòu)造擴(kuò)展卡爾曼濾 波器之前，對(duì)由所述測(cè)量裝置和所述被測(cè)對(duì)象所組成的系統(tǒng)進(jìn)行數(shù)學(xué)建模，獲得數(shù)學(xué)模型：其中，壞為所述被測(cè)對(duì)象的加速度在X軸上的零偏，蠟為所述被測(cè)對(duì)象的加速度在巧由 上的零偏； 《為由所述第一輔助傳感器測(cè)得的所述測(cè)量裝置的前進(jìn)速度與所述測(cè)量裝置的真實(shí) 前進(jìn)速度的比值，皆為由所述第一輔助傳感器測(cè)得的所述測(cè)量裝置的旋轉(zhuǎn)速度與所述測(cè)量 裝置的真實(shí)旋轉(zhuǎn)速度的比值； 為所述被測(cè)對(duì)象沿所述第二輔助傳感器指向方向上的速度在X軸上的分量，磚為所 述被測(cè)對(duì)象沿所述第二輔助傳感器指向方向上的速度在Y軸上的分量； 為所述測(cè)量裝置的前進(jìn)速度矢量向所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑方向的投 影的大小，語(yǔ)P為所述測(cè)量裝置的前進(jìn)速度矢量向所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑方 向的法向的投影的大?。? 磚_9為所述被測(cè)對(duì)象的速度矢量向連接所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑方向的投 影的大小，為所述被測(cè)對(duì)象的速度矢量向連接所述測(cè)量裝置和所述被測(cè)對(duì)象的矢徑方 向的法向的投影的大??； τ( ·)為二維空間中的旋轉(zhuǎn)變換矩陣。13.如權(quán)利要求12所述的測(cè)量裝置，其特征在于，所述構(gòu)造單元，具體用于：基于所述狀態(tài)變量和所述輸入變量，將所述數(shù)學(xué)模型調(diào)整為如下表達(dá)式，得到擴(kuò)展卡 爾曼濾波器：其中，V為測(cè)量噪聲向量，服從均值為ο、協(xié)方差為R的高斯分布，ti為時(shí)間， 龍二為系統(tǒng)微分方程模型，y = 品為系統(tǒng)輸出量，h為測(cè)量矩陣。14. 如權(quán)利要求13所述的測(cè)量裝置，其特征在于，所述修正單元，具體用于： 對(duì)所述擴(kuò)展卡爾曼濾波器進(jìn)行初始化;執(zhí)行遞推算法對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正， 從而獲得到端和讀的最優(yōu)狀態(tài)估計(jì)。15. 如權(quán)利要求14所述的測(cè)量裝置，其特征在于，所述修正單元，具體用于基于如下等 式，對(duì)于所述擴(kuò)展卡爾曼濾波器進(jìn)行初始化：其中，.·?(0)為系統(tǒng)初始狀態(tài)x(〇)的估計(jì)值，P(〇)為系統(tǒng)狀態(tài)轉(zhuǎn)移概率矩陣，E[ ·]代表 對(duì)?的期望。16. 如權(quán)利要求15所述的測(cè)量裝置，其特征在于，所述修正單元，具體用于基于如下等 式，執(zhí)行遞推算法對(duì)所述第一測(cè)量數(shù)據(jù)進(jìn)行修正：襲為系統(tǒng)狀態(tài)變量估計(jì)值的導(dǎo)數(shù);K為根據(jù)模擬系統(tǒng)和實(shí)際系統(tǒng)的誤差修正模擬系統(tǒng)狀 態(tài)的修正幅度;ym為含有噪聲的測(cè)量結(jié)果，其中包含相對(duì)位置傳感器輸出的相對(duì)距離S為和 相對(duì)夾角S品;巧為中間變量;P為傳播概率矩陣;為傳播概率矩陣的導(dǎo)數(shù)。
【文檔編號(hào)】G01C25/00GK106092141SQ201610569714
【公開(kāi)日】2016年11月9日
【申請(qǐng)日】2016年7月19日
【發(fā)明人】董世謙, 任冠佼
【申請(qǐng)人】納恩博（北京）科技有限公司