專(zhuān)利名稱(chēng)：：用于改善物體取向估計(jì)的方法和實(shí)施所述方法的姿態(tài)控制系統(tǒng)的制作方法用于改善物體取向估計(jì)的方法和實(shí)施所述方法的姿態(tài)控制系統(tǒng)本發(fā)明涉及一種估計(jì)有或無(wú)固有加速度以及有或無(wú)磁性擾動(dòng)的物體在空間中的取向的方法，還涉及一種適于實(shí)施所述方法以能夠估計(jì)取向的裝置。獲得取向一般涉及使用若干傳感器，形成被指定為運(yùn)動(dòng)俘獲裝置，也被指定為姿態(tài)控制單元的組件的一部分。MEMS(微機(jī)電系統(tǒng))傳感器可用于構(gòu)造該控制單元，所述MEMS傳感器具有小巧廉價(jià)的優(yōu)點(diǎn)。使用這樣的MEMS傳感器使得能夠考慮在各種應(yīng)用領(lǐng)域中使用姿態(tài)控制單元，尤其是在生物醫(yī)學(xué)領(lǐng)域中用于監(jiān)測(cè)居家老人、功能性再教育，在運(yùn)動(dòng)領(lǐng)域中用于分析體育運(yùn)動(dòng)，用于汽車(chē)、機(jī)器人、虛擬現(xiàn)實(shí)和三維動(dòng)畫(huà)領(lǐng)域，更一般地用于需要確定或觀察運(yùn)動(dòng)的任何領(lǐng)域中。然而，與非MEMS傳感器(例如用于導(dǎo)航領(lǐng)域中)相比，這些MEMS傳感器的缺點(diǎn)是較易受噪聲和偏差影響。此外，眾所周知，有些姿態(tài)控制單元既使用加速度計(jì)又使用磁強(qiáng)計(jì)，從而使其能夠重構(gòu)三個(gè)自由度的運(yùn)動(dòng)，即與地球重力場(chǎng)和地球磁場(chǎng)相比固有加速度和磁性擾動(dòng)分別可以忽略的運(yùn)動(dòng)。然而，在未觀察到這種假設(shè)情況，即不能忽略固有加速度或磁性擾動(dòng)時(shí)，所述運(yùn)動(dòng)表現(xiàn)出六個(gè)或九個(gè)自由度。此時(shí)不可能使用僅使用加速度計(jì)和磁強(qiáng)計(jì)的姿態(tài)控制單元來(lái)估計(jì)活動(dòng)物體的取向。亦即，運(yùn)動(dòng)俘獲應(yīng)用的多樣化使得必須要克服這些限制。因此考慮使用額外的傳感器，更具體而言是組合使用速率陀螺儀、加速度計(jì)和磁強(qiáng)計(jì)。從這些傳感器獲得的測(cè)量結(jié)果由兩個(gè)部分構(gòu)成與活動(dòng)物體的取向直接相關(guān)的信息部分和擾動(dòng)部分，擾動(dòng)部分的性質(zhì)取決于相關(guān)傳感器。首先，這些是針對(duì)加速度計(jì)提供的測(cè)量結(jié)果的固有加速度，對(duì)磁強(qiáng)計(jì)提供的測(cè)量結(jié)果的磁性擾動(dòng)以及速率陀螺儀的偏差(bias)。這些擾動(dòng)導(dǎo)致不正確的取向估計(jì)。當(dāng)前，有很多方法用于從加速度計(jì)、磁強(qiáng)計(jì)和速率陀螺儀提供的測(cè)量結(jié)果獲得對(duì)物體取向的估計(jì)。有一些實(shí)施一種或多種優(yōu)化標(biāo)準(zhǔn)的所謂優(yōu)化方法，但這些方法在計(jì)算時(shí)間方面成本較高。此外，在問(wèn)題變得復(fù)雜時(shí)，定義優(yōu)化標(biāo)準(zhǔn)是困難的。還有些實(shí)施神經(jīng)網(wǎng)絡(luò)的方法，神經(jīng)網(wǎng)絡(luò)需要后續(xù)學(xué)習(xí)階段，尤其是關(guān)于數(shù)據(jù)庫(kù)大小和計(jì)算時(shí)間的學(xué)習(xí)階段，以獲得精確的估計(jì)。此外，對(duì)于優(yōu)化方法和那些實(shí)施神經(jīng)網(wǎng)絡(luò)的方法而言，難以考慮到狀態(tài)隨時(shí)間的變化趨勢(shì)的概念，這使得它們魯棒性不夠好。還有些實(shí)現(xiàn)觀察器的方法，與上述方法不同的是，這些方法使得能夠合并來(lái)自?xún)蓚€(gè)源的信息源自傳感器提供的測(cè)量結(jié)果的信息和源自趨勢(shì)模型的信息，并且在這樣做的同時(shí)保持與實(shí)時(shí)實(shí)施兼容的計(jì)算時(shí)間。使用觀察器的已知方法主要依賴(lài)于卡爾曼濾波器的使用。這項(xiàng)技術(shù)的優(yōu)點(diǎn)是允許合并數(shù)據(jù)，同時(shí)考慮到傳感器提供的測(cè)量結(jié)果所提供的信息的質(zhì)量和模型的質(zhì)量。本領(lǐng)域技術(shù)人員都知道，有很多種卡爾曼濾波器-擴(kuò)展卡爾曼濾波器(或EKF)；這種卡爾曼濾波器實(shí)施起來(lái)迅速且容易，其應(yīng)用之一是運(yùn)動(dòng)俘獲，在如下文獻(xiàn)中特別地描述了這種應(yīng)用“Quaternion-basedextendedKalmanfilterfordeterminationorientationbyinertialandmagneticsensing，，，SABATINIΑ.Μ.,IEEETransactionsonBiomedicalEngineering,2006,53(7)。-專(zhuān)用于強(qiáng)非線性問(wèn)題的UKF卡爾曼濾波器(無(wú)氣味卡爾曼濾波器)。碰巧，在運(yùn)動(dòng)俘獲的背景下，遇到的問(wèn)題是弱非線性的；因此，它并不是很適于估計(jì)取向。此外，與EKF濾波器相比計(jì)算成本大大增加。因此人們對(duì)它的興趣不如對(duì)EKF濾波器多。例如，如下文獻(xiàn)描述了一種便攜式取向估計(jì)裝置，其中利用加速度計(jì)和磁強(qiáng)計(jì)合并絕對(duì)取向并利用UKF濾波器合并轉(zhuǎn)速”P(pán)ortabIeorientationestimationdevicebasedonaccelerometers，magnetometersandgyroscopesensorsforsensornetwork”，HARADAT.，UCHINOH.，MORIΤ.，SATOΤ.，IEEEConferenceonMultisensorFusionandIntegrationforIntelligentSystems，2003。-互補(bǔ)卡爾曼濾波器在這種相關(guān)情況下，目的是估計(jì)狀態(tài)的誤差而不是狀態(tài)自身，其實(shí)施起來(lái)非常復(fù)雜。除了濾波器的選擇之外，注入濾波器中的測(cè)量結(jié)果的質(zhì)量，尤其是它們值的可信性也是非常重要的。在實(shí)踐中，如前所述，測(cè)量結(jié)果包括與活動(dòng)的物體的取向直接相關(guān)的信息部分和擾動(dòng)部分，擾動(dòng)部分的性質(zhì)取決于相關(guān)傳感器。首先，這些是針對(duì)加速度計(jì)提供的測(cè)量結(jié)果的固有加速度，對(duì)磁強(qiáng)計(jì)提供的測(cè)量結(jié)果的磁性擾動(dòng)以及速率陀螺儀的偏差。還有必要考慮到測(cè)量噪聲，但所述噪聲常規(guī)是在濾波器中處理的。當(dāng)前有很多用于處理擾動(dòng)的方法。所述方法之一包括認(rèn)為擾動(dòng)是可以忽略的，并為濾波器提供由傳感器提供的測(cè)量結(jié)果，在如下文獻(xiàn)中就是這種情況“Design，implementationandexperimentalresultsofaquaternion-basedKalmanfilterforhumanbodymotiontracking",YUNX.,BACHMANNE.R.IEEETransactionsOnRobotics，2006，22(6)，以及"ApplicationofMIMU/MagnetometerintegratedsystemOntheattitudedeterminationofmicrosatellite，，，SUK.,REND.H.,YOUΖ.,ZHOUQ.,InternationalConferenceonIntelligentMechatronicsandAutomation，2004年8月，中國(guó)成都。結(jié)果，在傳感器之一上實(shí)際發(fā)生了擾動(dòng)的情況下，濾波器提供的測(cè)量結(jié)果是有誤差的，但濾波器將認(rèn)為它是正確的。此時(shí)取向的估計(jì)變得不正確。因此不論自由度的數(shù)目為幾個(gè)，都不可能忽略擾動(dòng)并以期望精度獲得估計(jì)。因此大大劣化了性能指標(biāo)，因?yàn)槭軘_動(dòng)的測(cè)量結(jié)果是原樣地提供給觀察器的。因此估計(jì)方法使得能夠通過(guò)檢測(cè)擾動(dòng)的存在并通過(guò)更新這些測(cè)量結(jié)果的可信性來(lái)將一次性缺點(diǎn)考慮在內(nèi)，例如，如以下文獻(xiàn)中所述“Portableorientationestimationdevicebasedonaccelerometers,magnetometersandgyroscopesensorsforsensornetwork",HARADAT.，UCHINOH.，MORIΤ.，SATOΤ.，IEEEConferenceonMultisensorFusionandIntegrationforIntelligentSystems,2003。這種方法提供了用于檢測(cè)測(cè)量結(jié)果中的擾動(dòng)的額外步驟。在檢測(cè)到擾動(dòng)時(shí)，對(duì)應(yīng)測(cè)量結(jié)果中的信任度最小化。因此在估計(jì)取向時(shí)不考慮包括擾動(dòng)的測(cè)量結(jié)果提供的信息。因此取向的估計(jì)僅依賴(lài)于由其他傳感器提供的測(cè)量結(jié)果?，F(xiàn)在，在來(lái)自若干傳感器的測(cè)量結(jié)果同時(shí)表現(xiàn)出擾動(dòng)的情況下，觀察器不再有充分多信息來(lái)提供正確的取向估計(jì)。最后，在如下文獻(xiàn)中描述了一種方法^nertialandmagneticsensingofhumanmotion，，,ROETENBERGD.,doctoralthesis,Twenteuniversity,Netherlands,2006以及“Measuringorientationofhumanbodysegmentusingminiaturegyroscopesandaccelerometers，，，PhDThesis,Inertialsensingofhumanmovement,LUINGEH.J.n，2002b，其中，借助于觀察器檢測(cè)擾動(dòng)的存在并對(duì)它們進(jìn)行估計(jì)。為此，放大狀態(tài)矢量，且測(cè)量模型中發(fā)生擾動(dòng)，這變得更接近事實(shí)。原則上，這種技術(shù)似乎適于具有六或九個(gè)自由度的運(yùn)動(dòng)情形。然而，因?yàn)槿狈捎^察性，對(duì)擾動(dòng)和取向的組合估計(jì)是困難的。還需要大量的待設(shè)置參數(shù)，這增大了其實(shí)施的復(fù)雜性。因此本發(fā)明的一個(gè)目的是提供一種取向估計(jì)方法，不論是否存在固有加速度和磁性擾動(dòng)，這種方法都提供精確的取向估計(jì)，并且與現(xiàn)有方法相比以簡(jiǎn)化的方式這樣做。前述目的是通過(guò)一種基于加速度、磁場(chǎng)和轉(zhuǎn)速沿三個(gè)空間軸的測(cè)量結(jié)果估計(jì)取向的方法實(shí)現(xiàn)的，所述方法包括-用于預(yù)處理這些測(cè)量結(jié)果以檢測(cè)擾動(dòng)的存在并估計(jì)無(wú)擾動(dòng)的測(cè)量結(jié)果的步驟，以及-用于基于從預(yù)處理步驟獲得的測(cè)量值估計(jì)取向的步驟。該方法未忽略擾動(dòng)，這意味著估計(jì)是無(wú)誤差的；該方法恒定地估計(jì)它們。如果它們存在，它并不像其他估計(jì)方法那樣拒絕相關(guān)的一個(gè)或多個(gè)測(cè)量結(jié)果。此外，不在狀態(tài)矢量或測(cè)量模型中并入它們，這簡(jiǎn)化了模型并且不會(huì)導(dǎo)致估計(jì)變得不可能的情況。因此在兩個(gè)相繼步驟中估計(jì)取向，并可能估計(jì)擾動(dòng)。因此為觀察器提供來(lái)自加速度計(jì)、磁強(qiáng)計(jì)和速率陀螺儀的測(cè)量結(jié)果，測(cè)量結(jié)果盡可能接近估計(jì)取向的理想條件即，分別沒(méi)有固有加速度、沒(méi)有磁性擾動(dòng)且沒(méi)有偏差。為此，使用在前一時(shí)刻估計(jì)的取向作為執(zhí)行對(duì)測(cè)量結(jié)果的預(yù)處理的額外信息。因此根據(jù)本發(fā)明的估計(jì)方法使得能夠以最佳方式從傳感器的測(cè)量結(jié)果提取物體的取向而不論相關(guān)的運(yùn)動(dòng)如何。此外，這種方法實(shí)施起來(lái)簡(jiǎn)單，且僅包括少量設(shè)置參數(shù)。有利地，所述觀察器為擴(kuò)展卡爾曼濾波器。可以估計(jì)擾動(dòng)，尤其是固有加速度，這使得能夠通過(guò)積分和二次積分分別回溯到物體的速度和位置。因此本發(fā)明的主題是一種利用物體的沿三個(gè)空間軸的總加速度、磁場(chǎng)和轉(zhuǎn)速的測(cè)量結(jié)果估計(jì)所述物體在時(shí)間k在空間中的取向的方法，包括以下步驟A-預(yù)處理時(shí)刻k的所述測(cè)量結(jié)果，以檢測(cè)所述測(cè)量結(jié)果中擾動(dòng)的存在，所述擾動(dòng)屬于包括所述物體的固有加速度、增加到地球磁場(chǎng)的磁場(chǎng)以及所述轉(zhuǎn)速的測(cè)量結(jié)果的偏差的組，并估計(jì)時(shí)間k時(shí)的無(wú)擾動(dòng)測(cè)量結(jié)果，B-由觀察器從在步驟A獲得的時(shí)間k時(shí)的估計(jì)的無(wú)擾動(dòng)測(cè)量結(jié)果估計(jì)時(shí)間k時(shí)的取向。步驟A有利地包括Al-預(yù)處理所述轉(zhuǎn)速測(cè)量結(jié)果，A2-檢測(cè)所述總加速度和磁場(chǎng)的所述測(cè)量結(jié)果中存在還是不存在擾動(dòng)，A3-在時(shí)間k沒(méi)有擾動(dòng)的情況下，估計(jì)的時(shí)間k時(shí)的無(wú)擾動(dòng)測(cè)量結(jié)果等于時(shí)間k時(shí)的測(cè)量結(jié)果，在有擾動(dòng)的情況下，在時(shí)間k估計(jì)的無(wú)擾動(dòng)測(cè)量結(jié)果是基于在前一時(shí)間k-Ι時(shí)估計(jì)的取向計(jì)算的。步驟Al包括從轉(zhuǎn)速測(cè)量結(jié)果減去在預(yù)備初始化步驟期間確定的平均偏差?？梢酝ㄟ^(guò)在給定時(shí)間期間固定提供轉(zhuǎn)速測(cè)量結(jié)果的模塊并計(jì)算每個(gè)軸上轉(zhuǎn)速測(cè)量結(jié)果值的平均值獲得這一平均偏差。在姿態(tài)控制單元由人佩戴的情況下，這種固定需要從人身上去除控制單元以消除人不可避免的震顫。用于預(yù)處理加速度和磁場(chǎng)測(cè)量結(jié)果的步驟A2可以包括-步驟A2.1，包括將所述總加速度測(cè)量結(jié)果的范數(shù)與重力場(chǎng)的范數(shù)進(jìn)行比較的測(cè)試，如果時(shí)間k時(shí)的加速度測(cè)量結(jié)果的范數(shù)與重力場(chǎng)的范數(shù)之間的差異的絕對(duì)值低于預(yù)定閾值，則假設(shè)加速度擾動(dòng)為零，否則假設(shè)有擾動(dòng)，在每個(gè)軸上所述擾動(dòng)等于時(shí)間k時(shí)所述總加速度的測(cè)量結(jié)果和在時(shí)間k時(shí)估計(jì)的無(wú)擾動(dòng)加速度測(cè)量結(jié)果之間的差異，-步驟A2.2，包括將所述磁場(chǎng)的測(cè)量結(jié)果的范數(shù)與地球磁場(chǎng)的范數(shù)進(jìn)行比較的測(cè)試，如果所述磁場(chǎng)測(cè)量結(jié)果的范數(shù)與地球磁場(chǎng)的范數(shù)之間的差異的絕對(duì)值低于預(yù)定閾值，則假設(shè)磁性擾動(dòng)為零，否則假設(shè)每個(gè)軸上所述磁性擾動(dòng)等于時(shí)間k時(shí)所述磁場(chǎng)測(cè)量結(jié)果和在時(shí)間k時(shí)在每個(gè)軸上估計(jì)的無(wú)擾動(dòng)磁場(chǎng)測(cè)量結(jié)果之間的差異。在步驟A2.1中，有利地對(duì)時(shí)間k-Ι時(shí)估計(jì)的擾動(dòng)進(jìn)行額外測(cè)試如果時(shí)間k時(shí)總加速度的測(cè)量結(jié)果的范數(shù)與重力場(chǎng)的范數(shù)之間的差異的絕對(duì)值低于預(yù)定閾值，則執(zhí)行檢查，以確定在時(shí)間k-Ι時(shí)估計(jì)的加速度擾動(dòng)的范數(shù)是否低于預(yù)定閾值，如果該測(cè)試是肯定的，則假設(shè)加速度擾動(dòng)在時(shí)間k實(shí)際為零，和/或在步驟A2.2中進(jìn)行對(duì)時(shí)間k-Ι時(shí)估計(jì)的磁性擾動(dòng)的額外測(cè)試如果磁場(chǎng)測(cè)量結(jié)果的范數(shù)與地球磁場(chǎng)的范數(shù)之間的差異的絕對(duì)值低于預(yù)定閾值，則執(zhí)行檢查，以確定在時(shí)間k-Ι時(shí)估計(jì)的磁性擾動(dòng)的絕對(duì)值是否低于預(yù)定閾值，如果本測(cè)試是肯定的，則假設(shè)磁性擾動(dòng)在時(shí)間k實(shí)際為零。該額外步驟使得能夠改善估計(jì)方法的精確度。不過(guò)，在已知估計(jì)取向可能漂移的不利的使用情況下，僅在有限的時(shí)間窗口上確保了步驟A2.1和A2.2的這些比較測(cè)試的魯棒性，所述時(shí)間窗口根據(jù)環(huán)境而不同。因此，有利地在說(shuō)明書(shū)中稍后指出的時(shí)間帶上執(zhí)行比較測(cè)試。于是，使用K，、知,*表示完成預(yù)處理步驟時(shí)獲得的測(cè)量結(jié)果，它們被稱(chēng)為估計(jì)的無(wú)擾動(dòng)的測(cè)量結(jié)果-僅利用加速度測(cè)量結(jié)果的范數(shù)執(zhí)行固有加速度的檢測(cè)；如果在滑動(dòng)時(shí)間窗口Ta的測(cè)量結(jié)果的至少一個(gè)上該范數(shù)與(^(地球重力)的范數(shù)不同，那么認(rèn)為當(dāng)前時(shí)刻的測(cè)量結(jié)果受到擾動(dòng)；-通過(guò)類(lèi)似方式執(zhí)行磁性擾動(dòng)的檢測(cè)〇如果在滑動(dòng)時(shí)間窗口Tm的測(cè)量結(jié)果的至少一個(gè)上磁性測(cè)量結(jié)果的范數(shù)與Htl(地球磁場(chǎng))的范數(shù)不同〇或者，如果磁性測(cè)量結(jié)果和無(wú)擾動(dòng)加速度測(cè)量結(jié)果的相反數(shù)-兄之間的角度與矢量和Htl之間的角度不同，此時(shí)，當(dāng)前時(shí)刻的測(cè)量結(jié)果受到磁性擾動(dòng)。Ta可以是恒定值的參數(shù)，而Tm的值可以與運(yùn)動(dòng)速度相關(guān)聯(lián)。利用可能位于同一設(shè)備上并由用戶(hù)激活的該變量，不需要查找時(shí)間k-Ι時(shí)的測(cè)量結(jié)果的值，因此消除了漂移。步驟B中使用的觀察器優(yōu)選為迅速而容易的擴(kuò)展卡爾曼濾波器。用于從在時(shí)間k時(shí)估計(jì)的測(cè)量結(jié)果估計(jì)取向的步驟B可以包括-從在時(shí)間k-Ι后驗(yàn)估計(jì)的狀態(tài)矢量估計(jì)時(shí)間k的先驗(yàn)狀態(tài)矢量，-從時(shí)間k的先驗(yàn)狀態(tài)矢量的估計(jì)(稱(chēng)為測(cè)量結(jié)果的先驗(yàn)估計(jì))來(lái)估計(jì)時(shí)間k的先驗(yàn)測(cè)量結(jié)果，-通過(guò)計(jì)算在時(shí)間k估計(jì)的無(wú)擾動(dòng)測(cè)量結(jié)果和先驗(yàn)估計(jì)測(cè)量結(jié)果之間的差異來(lái)計(jì)算擴(kuò)展卡爾曼濾波器的增益并計(jì)算創(chuàng)新(innovation)，-通過(guò)用所述增益和所述創(chuàng)新校正在時(shí)間k先驗(yàn)估計(jì)的狀態(tài)矢量來(lái)計(jì)算時(shí)間k時(shí)的估計(jì)取向。所述擴(kuò)展卡爾曼濾波器中使用的狀態(tài)矢量可以包含角速度和取向四元數(shù)的元。擴(kuò)展卡爾曼濾波器中使用的狀態(tài)矢量有利地僅包含取向四元數(shù)的元，這樣能夠簡(jiǎn)化狀態(tài)結(jié)構(gòu)和測(cè)量模型。本發(fā)明的另一主題是一種姿態(tài)控制單元，包括適于提供沿三個(gè)空間軸的加速度測(cè)量結(jié)果的模塊，測(cè)量沿三個(gè)空間軸的磁場(chǎng)的模塊和測(cè)量沿三個(gè)空間軸的轉(zhuǎn)速的模塊(所述模塊意在加入物體的運(yùn)動(dòng)中)，以及基于所述測(cè)量模塊提供的測(cè)量結(jié)果來(lái)估計(jì)時(shí)間k時(shí)的取向的模塊，所述估計(jì)模塊包括-預(yù)處理所述加速度、磁場(chǎng)和轉(zhuǎn)速測(cè)量結(jié)果的模塊，所述預(yù)處理模塊適于檢測(cè)所述測(cè)量結(jié)果中擾動(dòng)的存在并提供估計(jì)的無(wú)擾動(dòng)加速度測(cè)量結(jié)果、估計(jì)的無(wú)擾動(dòng)磁性測(cè)量結(jié)果和無(wú)偏差轉(zhuǎn)速，-由觀察器從預(yù)處理模塊提供的測(cè)量結(jié)果估計(jì)時(shí)刻k時(shí)的取向的模塊。本觀察器可以是擴(kuò)展卡爾曼濾波器。根據(jù)本發(fā)明的姿態(tài)控制單元還可以包括計(jì)算控制中心的初始化期間轉(zhuǎn)速測(cè)量模塊的平均偏差的模塊。預(yù)處理模塊包括用于檢測(cè)所述加速度測(cè)量結(jié)果中固有加速度的存在的模塊和用于檢測(cè)所述磁場(chǎng)測(cè)量結(jié)果中磁性擾動(dòng)的存在的模塊。根據(jù)本發(fā)明的姿態(tài)控制單元還可以包括估計(jì)固有加速度以及計(jì)算物體的速度和位置的模塊。有利地，適于提供沿三個(gè)空間軸的總加速度測(cè)量結(jié)果、磁場(chǎng)測(cè)量結(jié)果和轉(zhuǎn)速測(cè)量結(jié)果的模塊為MEMS傳感器。從以下描述和附圖中將更好地理解本發(fā)明，其中-圖1是時(shí)間k時(shí)的根據(jù)本發(fā)明的方法的流程圖，-圖2A到圖2C示出了根據(jù)本發(fā)明用于預(yù)處理分別來(lái)自加速度計(jì)、速率陀螺儀和磁強(qiáng)計(jì)的測(cè)量結(jié)果的步驟的詳細(xì)流程圖。目的是獲得在空間中活動(dòng)的物體的取向，例如人的取向。為此，使用姿態(tài)控制單元，其包括適于提供沿三個(gè)空間軸的總加速度、磁場(chǎng)和轉(zhuǎn)速的測(cè)量結(jié)果的傳感器。有利地，傳感器是MEMS傳感器，其具有降低的成本價(jià)格和有限的覆蓋面積。對(duì)于加速度測(cè)量而言，例如，可以是提供每個(gè)軸上的測(cè)量結(jié)果的三軸加速度計(jì)或三個(gè)單軸加速度計(jì)。類(lèi)似地，對(duì)于磁場(chǎng)測(cè)量而言，例如，可以是三軸磁強(qiáng)計(jì)或三個(gè)單軸磁強(qiáng)計(jì)。對(duì)于轉(zhuǎn)速測(cè)量而言，例如，可以是三個(gè)單軸速率陀螺儀，或有利地，是兩個(gè)雙軸速率陀螺儀。三個(gè)軸可以對(duì)準(zhǔn)或不對(duì)準(zhǔn)，但在后一種情況下，軸間的相對(duì)取向必須是已知的。在下文中，在描述中為了簡(jiǎn)單起見(jiàn)，將把一個(gè)或多個(gè)加速度計(jì)稱(chēng)為加速度計(jì)，把一個(gè)或多個(gè)磁強(qiáng)計(jì)稱(chēng)為磁強(qiáng)計(jì)，把一個(gè)或多個(gè)速率陀螺儀稱(chēng)為速率陀螺儀。這些傳感器附著于取向已知的物體。我們僅有測(cè)量結(jié)果y，將通過(guò)下式進(jìn)行建模'yA=~R.G0+a+vA權(quán)利要求1.一種利用物體沿三個(gè)空間軸的總加速度(yA)、磁場(chǎng)(yM)和轉(zhuǎn)速(ye)的測(cè)量結(jié)果來(lái)估計(jì)所述物體在時(shí)間k時(shí)在空間中的取向的方法，所述方法包括A-對(duì)時(shí)刻k時(shí)的所述測(cè)量結(jié)果(yA，yM，yG)進(jìn)行預(yù)處理，以檢測(cè)所述測(cè)量結(jié)果中擾動(dòng)的存在，并估計(jì)時(shí)間k時(shí)的無(wú)擾動(dòng)測(cè)量結(jié)果，其中所述擾動(dòng)來(lái)自包括以下各項(xiàng)的組所述物體的固有加速度、增加到地球磁場(chǎng)的磁場(chǎng)以及所述轉(zhuǎn)速的測(cè)量結(jié)果的偏差，B-由觀察器利用在步驟A獲得的時(shí)間k時(shí)的估計(jì)的無(wú)擾動(dòng)測(cè)量結(jié)果^m,k,來(lái)估計(jì)時(shí)間k時(shí)的取向。2.根據(jù)權(quán)利要求1所述的方法，其特征在于，估計(jì)物體在時(shí)間k時(shí)在空間中的取向僅利用所述物體沿三個(gè)空間軸的所述總加速度(yA)、磁場(chǎng)(y)和轉(zhuǎn)速(ye)的測(cè)量結(jié)果。3.根據(jù)權(quán)利要求1所述的方法，其特征在于，步驟A包括Al-對(duì)所述轉(zhuǎn)速(ye)的測(cè)量結(jié)果進(jìn)行預(yù)處理，A2-檢測(cè)所述總加速度和所述磁場(chǎng)的所述測(cè)量結(jié)果(yA，yM)中是否存在擾動(dòng)，A3-在時(shí)間k時(shí)不存在擾動(dòng)的情況下，時(shí)間k時(shí)的估計(jì)的無(wú)擾動(dòng)測(cè)量結(jié)果(3^，*，yM,k)等于時(shí)間k時(shí)的測(cè)量結(jié)果，而在存在擾動(dòng)的情況下，在時(shí)間k時(shí)估計(jì)的無(wú)擾動(dòng)測(cè)量結(jié)果(y^k，)是基于在時(shí)間k-i時(shí)估計(jì)的取向計(jì)算的。4.根據(jù)權(quán)利要求3所述的方法，其特征在于，步驟Al包括從轉(zhuǎn)速的測(cè)量結(jié)果減去在預(yù)備初始化步驟期間確定的平均偏差(Swerage)。5.根據(jù)權(quán)利要求4所述的方法，其特征在于，通過(guò)在給定時(shí)間期間固定提供轉(zhuǎn)速的測(cè)量結(jié)果的模塊并計(jì)算每個(gè)軸上所述轉(zhuǎn)速的測(cè)量結(jié)果的值的平均值來(lái)獲得所述平均偏差八(baverage)。6.根據(jù)權(quán)利要求3所述的方法，其特征在于，所述步驟A2包括A2.1-將所述總加速度的測(cè)量結(jié)果的范數(shù)與重力場(chǎng)()的范數(shù)進(jìn)行比較的測(cè)試，其特征在于，如果時(shí)間k時(shí)的所述加速度的測(cè)量結(jié)果的范數(shù)與重力場(chǎng)(Gtl)的范數(shù)之間的差異的絕對(duì)值低于預(yù)定閾值(αA)，則假設(shè)加速度擾動(dòng)為零，否則假設(shè)存在擾動(dòng)，在每個(gè)軸上所述擾動(dòng)等于時(shí)間k時(shí)的所述總加速度的測(cè)量結(jié)果和在時(shí)間k時(shí)估計(jì)的無(wú)擾動(dòng)加速度第測(cè)量結(jié)果之間的差異，A2.2-將所述磁場(chǎng)的測(cè)量結(jié)果的范數(shù)與所述地球磁場(chǎng)(Htl)的范數(shù)進(jìn)行比較的測(cè)試，其特征在于，如果所述磁場(chǎng)的測(cè)量結(jié)果的范數(shù)與所述地球磁場(chǎng)的范數(shù)之間的差異的絕對(duì)值低于預(yù)定閾值(αM)，則假設(shè)磁性擾動(dòng)為零，否則假設(shè)每個(gè)軸上的所述磁性擾動(dòng)等于時(shí)間k時(shí)的所述磁場(chǎng)的測(cè)量結(jié)果和在時(shí)間k時(shí)估計(jì)的無(wú)擾動(dòng)磁場(chǎng)的測(cè)量結(jié)果之間的差異。7.根據(jù)權(quán)利要求6所述的方法，其特征在于，在步驟A2.1中對(duì)時(shí)間k-Ι時(shí)的估計(jì)的擾動(dòng)1)進(jìn)行額外的測(cè)試如果時(shí)間k時(shí)的所述總加速度的測(cè)量結(jié)果的范數(shù)與所述重力場(chǎng)(G0)的范數(shù)之間的差異的絕對(duì)值低于預(yù)定閾值(αΑ)，則執(zhí)行檢查，以確定時(shí)間k-Ι時(shí)的估計(jì)的加速度擾動(dòng)的范數(shù)是否低于預(yù)定閾值(βA)，如果該測(cè)試是肯定的，則假設(shè)加速度擾動(dòng)在時(shí)間k時(shí)實(shí)際為零，和/或在步驟A2.2中對(duì)時(shí)間k-Ι時(shí)的估計(jì)的磁性擾動(dòng)(i-i)進(jìn)行額外的測(cè)試，其特征在于，如果所述磁場(chǎng)的測(cè)量結(jié)果的范數(shù)與所述地球磁場(chǎng)(Htl)的范數(shù)之間的差異的絕對(duì)值低于預(yù)定閾值(αM)，則檢查時(shí)間k-Ι時(shí)的估計(jì)的磁性擾動(dòng)的絕對(duì)值是否低于預(yù)定閾值(βM)，其特征在于，如果該測(cè)試是肯定的，則假設(shè)所述磁性擾動(dòng)在時(shí)間k時(shí)實(shí)際為零。8.根據(jù)權(quán)利要求3所述的方法，其特征在于，在用戶(hù)設(shè)置的時(shí)間窗口(TA，TM)上執(zhí)行步驟A2中的至少一個(gè)檢測(cè)。9.根據(jù)權(quán)利要求8所述的方法，其特征在于，通過(guò)以下形式的處理進(jìn)行對(duì)所述固有加速度的檢測(cè)10.根據(jù)權(quán)利要求8所述的方法，其特征在于，通過(guò)以下形式的處理執(zhí)行對(duì)磁性擾動(dòng)的檢測(cè)11.根據(jù)權(quán)利要求9所述的方法，其特征在于，還在固有加速度的檢測(cè)的輸出處計(jì)算角度叩/e(-h，fc，;v，t)，然后通過(guò)以下形式的處理進(jìn)行對(duì)磁性擾動(dòng)的檢測(cè)If存在固有加速度T=TIf對(duì)于至少一個(gè)值tk而言IIlyM,kIl-IlHJII>aM或|uk-uQ|>au，使得tke[tk-TM；12.根據(jù)權(quán)利要求1到11中的任一項(xiàng)所述的方法，其特征在于，步驟B中使用的觀察器為擴(kuò)展卡爾曼濾波器。13.根據(jù)權(quán)利要求12所述的方法，其特征在于，用于利用在時(shí)間k時(shí)估計(jì)的無(wú)擾動(dòng)測(cè)量結(jié)果來(lái)估計(jì)所述取向的步驟B包括-利用在時(shí)間k-Ι時(shí)后驗(yàn)估計(jì)的狀態(tài)矢量(^w)來(lái)估計(jì)時(shí)間k時(shí)的先驗(yàn)狀態(tài)矢量(《)，-利用時(shí)間k時(shí)的先驗(yàn)狀態(tài)矢量的估計(jì)來(lái)估計(jì)時(shí)間k時(shí)的先驗(yàn)測(cè)量結(jié)果(K)，-通過(guò)計(jì)算在時(shí)間k時(shí)估計(jì)的所述無(wú)擾動(dòng)測(cè)量結(jié)果和估計(jì)的所述先驗(yàn)測(cè)量結(jié)果之間的差異來(lái)計(jì)算所述擴(kuò)展卡爾曼濾波器的增益(Kk)和創(chuàng)新(Ik)，-通過(guò)用所述增益和所述創(chuàng)新對(duì)在時(shí)間k時(shí)先驗(yàn)估計(jì)的狀態(tài)矢量進(jìn)行校正來(lái)計(jì)算時(shí)間k時(shí)的估計(jì)取向(也)。14.根據(jù)權(quán)利要求13所述的方法，其特征在于，所述擴(kuò)展卡爾曼濾波器中使用的狀態(tài)矢量包含角速度的元和取向四元數(shù)的元。15.根據(jù)權(quán)利要求14所述的方法，其特征在于，所述擴(kuò)展卡爾曼濾波器中使用的狀態(tài)矢量?jī)H包含取向四元數(shù)的元。16.一種姿態(tài)控制系統(tǒng)，至少包括用于提供沿三個(gè)空間軸的加速度的測(cè)量結(jié)果(yA)的感測(cè)單元、用于測(cè)量沿三個(gè)空間軸的磁場(chǎng)(yM)的感測(cè)單元、用于測(cè)量沿三個(gè)空間軸的轉(zhuǎn)速(yG)的感測(cè)單元、以及用于基于所述感測(cè)單元提供的測(cè)量結(jié)果來(lái)估計(jì)時(shí)間k的取向的處理單元，所述控制系統(tǒng)包括-用于對(duì)所述加速度(yA)、磁場(chǎng)(y)和轉(zhuǎn)速(ye)的測(cè)量結(jié)果進(jìn)行預(yù)處理以及用于提供估計(jì)的無(wú)擾動(dòng)加速度測(cè)量結(jié)果(％，”、估計(jì)的無(wú)擾動(dòng)磁性測(cè)量結(jié)果(”和無(wú)偏差的轉(zhuǎn)速(3^)的子單元，其中所述預(yù)處理子單元適于檢測(cè)所述測(cè)量結(jié)果中擾動(dòng)的存在，所述擾動(dòng)來(lái)自包括以下各項(xiàng)的組所述物體的固有加速度、增加到所述地球磁場(chǎng)的磁場(chǎng)以及所述轉(zhuǎn)速的測(cè)量結(jié)果的偏差，-用于由觀察器利用所述預(yù)處理模塊提供的估計(jì)的無(wú)擾動(dòng)加速度測(cè)量結(jié)果、估計(jì)的無(wú)擾動(dòng)磁性測(cè)量結(jié)果和無(wú)偏差的轉(zhuǎn)速測(cè)量結(jié)果來(lái)估計(jì)時(shí)刻k的取向的子單元。17.根據(jù)權(quán)利要求16所述的姿態(tài)控制系統(tǒng)，還包括用于計(jì)算在控制系統(tǒng)的初始化步驟期間轉(zhuǎn)速測(cè)量模塊的平均偏差(^werage)的模塊。18.根據(jù)權(quán)利要求16所述的姿態(tài)控制系統(tǒng)，其特征在于，預(yù)處理子單元包括用于檢測(cè)所述加速度的測(cè)量結(jié)果中固有加速度的存在的模塊和用于檢測(cè)所述磁場(chǎng)的測(cè)量結(jié)果中磁性擾動(dòng)的存在的模塊。19.根據(jù)權(quán)利要求18所述的姿態(tài)控制系統(tǒng)，其特征在于，所述用于檢測(cè)所述加速度的測(cè)量結(jié)果中固有加速度的存在的模塊和所述用于檢測(cè)所述磁場(chǎng)的測(cè)量結(jié)果中磁性擾動(dòng)的存在的模塊在一個(gè)或多個(gè)時(shí)間窗口內(nèi)執(zhí)行這些檢測(cè)。20.根據(jù)權(quán)利要求16所述的姿態(tài)控制系統(tǒng)，還包括用于估計(jì)所述固有加速度和所述磁性擾動(dòng)并用于計(jì)算所述物體的速度和位置的模塊。21.根據(jù)權(quán)利要求16所述的姿態(tài)控制系統(tǒng)，其特征在于，所述觀察器是擴(kuò)展卡爾曼濾波器。22.根據(jù)權(quán)利要求16所述的姿態(tài)控制單元，其特征在于，用于提供沿三個(gè)空間軸的總加速度的測(cè)量結(jié)果(yA)、磁場(chǎng)的測(cè)量結(jié)果(yM)和轉(zhuǎn)速的測(cè)量結(jié)果(ye)的感測(cè)單元是MEMS傳感器。全文摘要本發(fā)明涉及一種利用物體沿三個(gè)空間軸的總加速度(yA)、磁場(chǎng)(yM)和轉(zhuǎn)速(yG)的測(cè)量結(jié)果來(lái)估計(jì)所述物體在時(shí)間k時(shí)在空間中的取向的方法，所述方法包括A-預(yù)處理(200)時(shí)刻k時(shí)的所述測(cè)量結(jié)果(yA，yM＞yG)，以檢測(cè)所述測(cè)量結(jié)果中擾動(dòng)的存在并估計(jì)時(shí)間k時(shí)的無(wú)擾動(dòng)測(cè)量結(jié)果，B-由觀察器從在步驟A獲得的時(shí)間k時(shí)的無(wú)擾動(dòng)測(cè)量結(jié)果(公式(II))估計(jì)時(shí)間k時(shí)的取向(公式(I))。文檔編號(hào)G01C17/38GK102308183SQ200980132305公開(kāi)日2012年1月4日申請(qǐng)日期2009年7月17日優(yōu)先權(quán)日2008年7月18日發(fā)明者A·瓦西列夫,C·巴松皮埃爾申請(qǐng)人:原子能和輔助替代能源委員會(huì),莫韋公司