專利名稱：移動(dòng)體姿態(tài)角檢測(cè)設(shè)備的制作方法
技術(shù)領(lǐng)域：

本發(fā)明涉及移動(dòng)體姿態(tài)角檢測(cè)設(shè)備，更具體地，涉及精確地檢測(cè)例如機(jī)器人的移動(dòng)體的姿態(tài)角的技術(shù)。

背景技術(shù)：

加速度傳感器和角速度傳感器用在例如機(jī)器人的移動(dòng)體的姿態(tài)控制中。當(dāng)三個(gè)正交軸表示為x軸、y軸和z軸，在這三個(gè)軸的方向上的加速度由三個(gè)加速度傳感器檢測(cè)，在這三個(gè)軸的方向上的角速度由三個(gè)角速度傳感器檢測(cè)。圍繞這些軸的角，即姿態(tài)角，通過(guò)對(duì)來(lái)自角速度傳感器的輸出進(jìn)行時(shí)間積分而獲得，從而可以計(jì)算俯仰角(pitch angle)、側(cè)傾角(roll angle)和橫擺角(yaw angle)。

公開(kāi)號(hào)為JP-A-2004-268730的日本專利申請(qǐng)描述了利用從陀螺儀傳感器輸出的姿態(tài)數(shù)據(jù)和加速度數(shù)據(jù)來(lái)執(zhí)行姿態(tài)控制的技術(shù)。

然而，當(dāng)角速度傳感器(橫擺率傳感器)的偏移和漂移變大時(shí)，因?yàn)橥ㄟ^(guò)角速度的積分獲得姿態(tài)角，偏移和漂移值逐漸累積并且最終成為非常大的值。這些值隨著時(shí)間增加并且最終發(fā)散(diverge)。該問(wèn)題可以通過(guò)利用具有小偏移和漂移的角速度傳感器來(lái)避免，但是這樣類型的傳感器大、重并且昂貴。

另一方面，可以通過(guò)用加速度傳感器來(lái)檢測(cè)重力方向和利用此重力方向在合適的時(shí)刻修正由角速度傳感器獲得的姿態(tài)角，來(lái)修正來(lái)自積分的誤差的累積。然而，不幸地，當(dāng)移動(dòng)體的運(yùn)動(dòng)變化快時(shí)以及除了重力之外的加速度頻繁產(chǎn)生時(shí)不能使用該方法。


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

因而本發(fā)明提供了一種能夠檢測(cè)移動(dòng)體的姿態(tài)角的設(shè)備，其通過(guò)簡(jiǎn)單的結(jié)構(gòu)而無(wú)需昂貴的、高精確性的角速度傳感器來(lái)防止積分誤差的累積。

本發(fā)明的一個(gè)方案涉及移動(dòng)體姿態(tài)角檢測(cè)設(shè)備，其包括檢測(cè)移動(dòng)體的角速度的角速度傳感器；檢測(cè)所述移動(dòng)體的加速度的加速度傳感器；角速度姿態(tài)角計(jì)算裝置，用于由所述角速度計(jì)算所述移動(dòng)體的姿態(tài)角，作為角速度姿態(tài)角；加速度姿態(tài)角計(jì)算裝置，用于由所述加速度計(jì)算所述移動(dòng)體的姿態(tài)角，作為加速度姿態(tài)角；角速度姿態(tài)角低范圍分量提取裝置，用于提取所述角速度姿態(tài)角的低范圍分量；加速度姿態(tài)角低范圍分量提取裝置，用于提取所述加速度姿態(tài)角的低范圍分量；差值計(jì)算裝置，用于計(jì)算所述角速度姿態(tài)角的低范圍分量和所述加速度姿態(tài)角的低范圍分量之間的差，作為誤差；誤差去除裝置，用于從所述角速度姿態(tài)角去除所述誤差；輸出裝置，用于輸出由所述誤差去除裝置去除了所述誤差的角速度姿態(tài)角，作為所述移動(dòng)體的姿態(tài)角；及反饋裝置，用于將由所述誤差去除裝置去除了所述誤差的所述角速度姿態(tài)角反饋到所述角速度姿態(tài)角計(jì)算裝置。

在本發(fā)明的此方案中，所述角速度姿態(tài)角低范圍分量提取裝置和所述加速度姿態(tài)角低范圍分量提取裝置包括用于sin轉(zhuǎn)換和cos轉(zhuǎn)換所述角速度姿態(tài)角的裝置；用于提取通過(guò)所述sin轉(zhuǎn)換和所述cos轉(zhuǎn)換獲得的值的低范圍分量的裝置；和用于對(duì)所述提取的低分量的值執(zhí)行atan2計(jì)算或atan計(jì)算的裝置。

本發(fā)明的另一方案涉及移動(dòng)體姿態(tài)角檢測(cè)設(shè)備，其包括檢測(cè)移動(dòng)體的角速度的角速度傳感器；檢測(cè)所述移動(dòng)體的加速度的加速度傳感器；角速度姿態(tài)角計(jì)算裝置，用于從所述角速度計(jì)算所述移動(dòng)體的姿態(tài)角，作為角速度姿態(tài)角；加速度姿態(tài)角計(jì)算裝置，用于從所述加速度計(jì)算所述移動(dòng)體的姿態(tài)角，作為加速度姿態(tài)角；差值計(jì)算裝置，用于計(jì)算所述角速度姿態(tài)角和所述加速度姿態(tài)角之間的差；低范圍分量提取裝置，用于提取所述差的低范圍分量作為誤差；誤差去除裝置，用于從所述角速度姿態(tài)角去除所述誤差；輸出裝置，用于輸出由所述誤差去除裝置去除了所述誤差的角速度姿態(tài)角，作為所述移動(dòng)體的姿態(tài)角；和反饋裝置，用于將由所述誤差去除裝置去除了所述誤差的所述角速度姿態(tài)角反饋到所述角速度姿態(tài)角計(jì)算裝置。

本發(fā)明的另一方案涉及移動(dòng)體姿態(tài)角檢測(cè)設(shè)備，其包括檢測(cè)移動(dòng)體的角速度的角速度傳感器；檢測(cè)所述移動(dòng)體的加速度的加速度傳感器；用于從所述角速度計(jì)算在經(jīng)過(guò)一小段時(shí)間ts之后定義小旋轉(zhuǎn)角的小角度矩陣的裝置；用于從所述小角度矩陣和被反饋回來(lái)的姿態(tài)矩陣計(jì)算新的角速度系統(tǒng)姿態(tài)矩陣的裝置；用于從所述加速度計(jì)算傾斜角的裝置；用于從所述傾斜角計(jì)算加速度系統(tǒng)姿態(tài)矩陣的裝置；角速度系統(tǒng)低范圍分量提取裝置，用于提取所述角速度系統(tǒng)姿態(tài)矩陣的低范圍分量；加速度系統(tǒng)低范圍分量提取裝置，用于提取所述加速度系統(tǒng)姿態(tài)矩陣的低范圍分量；差值計(jì)算裝置，用于計(jì)算所述角速度系統(tǒng)姿態(tài)矩陣的所述低范圍分量和所述加速度系統(tǒng)姿態(tài)矩陣的所述低范圍分量之間的差，作為誤差；誤差去除裝置，用于從所述角速度系統(tǒng)姿態(tài)矩陣去除所述誤差；輸出裝置，用于從由所述誤差去除裝置去除了所述誤差的角速度系統(tǒng)姿態(tài)矩陣計(jì)算姿態(tài)角，并且輸出所述結(jié)果作為所述移動(dòng)體的姿態(tài)角；和反饋裝置，用于將由所述誤差去除裝置去除了所述誤差的所述角速度系統(tǒng)姿態(tài)矩陣反饋到所述用于計(jì)算所述新的角速度系統(tǒng)姿態(tài)矩陣的裝置。

本發(fā)明的又一方案涉及移動(dòng)體姿態(tài)角檢測(cè)設(shè)備，其包括檢測(cè)移動(dòng)體的角速度的角速度傳感器；檢測(cè)所述移動(dòng)體的加速度的加速度傳感器；用于從所述角速度計(jì)算在經(jīng)過(guò)一小段時(shí)間ts之后定義小旋轉(zhuǎn)角的小角度四元數(shù)的裝置；用于從所述小角度四元數(shù)和被反饋回來(lái)的四元數(shù)計(jì)算新的角速度系統(tǒng)四元數(shù)的裝置；用于從所述加速度計(jì)算傾斜角的裝置；用于從所述傾斜角計(jì)算加速度系統(tǒng)四元數(shù)的裝置；角速度系統(tǒng)低范圍分量提取裝置，用于提取所述角速度系統(tǒng)四元數(shù)的低范圍分量；加速度系統(tǒng)低范圍分量提取裝置，用于提取所述加速度系統(tǒng)四元數(shù)的低范圍分量；差值計(jì)算裝置，用于計(jì)算所述角速度系統(tǒng)四元數(shù)的所述低范圍分量與所述加速度系統(tǒng)四元數(shù)的所述低范圍分量之間的差，作為誤差；誤差去除裝置，用于從所述角速度系統(tǒng)四元數(shù)去除所述誤差；輸出裝置，用于從由所述誤差去除裝置去除了所述誤差的角速度系統(tǒng)四元數(shù)計(jì)算姿態(tài)角，并且輸出所述結(jié)果作為所述移動(dòng)體的姿態(tài)角；和反饋裝置，用于將由所述誤差去除裝置去除了所述誤差的所述角速度系統(tǒng)四元數(shù)反饋到所述用于計(jì)算所述新的角速度系統(tǒng)四元數(shù)的裝置。

在本發(fā)明中，角速度姿態(tài)角，其為基于由角速度傳感器檢測(cè)到的角速度計(jì)算出的姿態(tài)角，被提供給低范圍分量提取裝置。角速度姿態(tài)角包括來(lái)自積分誤差的累積的偏移量。因此，當(dāng)姿態(tài)角的真值(truevalue)被分為高范圍分量和低范圍分量時(shí)，角速度姿態(tài)角包括(高范圍姿態(tài)角真值+低范圍姿態(tài)角真值+低范圍偏移量)。低范圍分量提取裝置去除高范圍分量并且提取(低范圍姿態(tài)角真值+低范圍偏移量)。同時(shí)，加速度姿態(tài)角，其為基于由加速度傳感器檢測(cè)到的加速度計(jì)算出的姿態(tài)角，也被提供給低范圍分量提取裝置。加速度姿態(tài)角包括高范圍噪聲分量和低范圍姿態(tài)角真值，但是不包括來(lái)自積分誤差的累積的偏移量。低范圍分量提取裝置去除高范圍分量并且提取低范圍姿態(tài)角真值。因此，通過(guò)將這些低范圍分量提取裝置的輸出提供給差值計(jì)算裝置，并且計(jì)算(低范圍姿態(tài)角真值+低范圍偏移量)-(低范圍姿態(tài)角真值)，可以僅提取出偏移量(即，誤差)。然后通過(guò)從角速度姿態(tài)角去除所提取的偏移量可以檢測(cè)到高精確性的姿態(tài)角。同樣，從角速度姿態(tài)角去除所提取的偏移量并且反饋該結(jié)果防止積分誤差的累積，提高輸出響應(yīng)度，并且使得輸出穩(wěn)定。高范圍噪聲量包括振動(dòng)加速度噪聲和電噪聲分量，以及除伴隨測(cè)量體移動(dòng)的重力之外的加速度分量。特別地，來(lái)自除伴隨測(cè)量體移動(dòng)的重力之外的加速度分量的噪聲在具有大運(yùn)動(dòng)量的小測(cè)量體例如機(jī)器人中變成大的噪聲分量，這在獲得姿態(tài)角中是主要障礙。

同樣在本發(fā)明中，基于由角速度傳感器檢測(cè)到的角速度計(jì)算出的角速度系統(tǒng)姿態(tài)矩陣被提供給低范圍分量提取裝置。角速度系統(tǒng)姿態(tài)矩陣包括來(lái)自積分誤差的累積的偏移量。因此，當(dāng)姿態(tài)的真值被分為高范圍分量和低范圍分量時(shí)，角速度系統(tǒng)姿態(tài)矩陣包括(高范圍姿態(tài)真值+低范圍姿態(tài)真值+低范圍偏移量)。低范圍分量提取裝置去除高范圍分量并且提取(低范圍姿態(tài)真值+低范圍偏移量)。同時(shí)，基于由加速度傳感器檢測(cè)到的加速度計(jì)算出的加速度系統(tǒng)姿態(tài)矩陣也被提供給低范圍分量提取裝置。加速度系統(tǒng)姿態(tài)矩陣包括高范圍噪聲分量、高范圍姿態(tài)真值分量，以及低范圍姿態(tài)真值，但是不包括來(lái)自積分誤差的累積的偏移量。低范圍分量提取裝置去除高范圍分量并且提取低范圍姿態(tài)真值。因此，通過(guò)將這些低范圍分量提取裝置的輸出提供給差值計(jì)算裝置，并且計(jì)算(低范圍姿態(tài)真值+低范圍偏移量)-(低范圍姿態(tài)真值)，可以僅提取出偏移量(即，誤差)。然后通過(guò)從角速度系統(tǒng)姿態(tài)矩陣去除所提取的偏移量可以檢測(cè)到高精確性的姿態(tài)角，以獲得沒(méi)有任何誤差的角速度系統(tǒng)姿態(tài)矩陣，然后從此角速度系統(tǒng)姿態(tài)矩陣計(jì)算姿態(tài)角。同樣，從角速度系統(tǒng)姿態(tài)矩陣去除所提取的偏移量并且反饋該結(jié)果防止積分誤差的累積，提高輸出響應(yīng)度，并且使得輸出穩(wěn)定。此外，可以通過(guò)計(jì)算來(lái)自角速度系統(tǒng)姿態(tài)矩陣的四元數(shù)和計(jì)算來(lái)自加速度系統(tǒng)姿態(tài)矩陣的四元數(shù)，在所述四元數(shù)階段(stage)提取低范圍分量并且計(jì)算這兩者之間的差值，來(lái)相似地提取出偏移量。姿態(tài)矩陣和四元數(shù)本質(zhì)是等價(jià)的，但是利用四元數(shù)減少了計(jì)算次數(shù)，因而縮短了處理時(shí)間。

通過(guò)防止積分誤差的累積而無(wú)需使用大、重并且昂貴的傳感器，本發(fā)明可以精確地檢測(cè)移動(dòng)體的姿態(tài)角。




結(jié)合附圖，通過(guò)閱讀本發(fā)明的優(yōu)選實(shí)施例的如下詳細(xì)描述將更好地理解本發(fā)明的特點(diǎn)、優(yōu)點(diǎn)以及技術(shù)和工業(yè)意義，其中 圖1為本發(fā)明的第一示例性實(shí)施例的框圖； 圖2為本發(fā)明的第二示例性實(shí)施例的框圖； 圖3為本發(fā)明的第三示例性實(shí)施例的框圖； 圖4為本發(fā)明的第四示例性實(shí)施例的框圖； 圖5為本發(fā)明的第五示例性實(shí)施例的框圖； 圖6為本發(fā)明的第六示例性實(shí)施例的框圖； 圖7為圖示了參考坐標(biāo)系(XYZ)和傳感器坐標(biāo)系(xyz)之間的關(guān)系的圖； 圖8為圖示了參考坐標(biāo)系中的姿態(tài)角(側(cè)傾角、俯仰角、橫擺角)的圖； 圖9為圖示了在傳感器坐標(biāo)系中隨著時(shí)間的變化的圖； 圖10為圖示了在傳感器坐標(biāo)系中的小的旋轉(zhuǎn)角的圖； 圖11為圖示了傾斜角的圖； 圖12為本發(fā)明第七示例性實(shí)施例的框圖； 圖13為本發(fā)明第八示例性實(shí)施例的框圖； 圖14為本發(fā)明第九示例性實(shí)施例的框圖； 圖15為本發(fā)明第十示例性實(shí)施例的框圖； 圖16為本發(fā)明第十一示例性實(shí)施例的框圖； 圖17為本發(fā)明第十二示例性實(shí)施例的框圖； 圖18為本發(fā)明第十三示例性實(shí)施例的框圖； 圖19為本發(fā)明第十四示例性實(shí)施例的框圖； 圖20為本發(fā)明第十五示例性實(shí)施例的框圖； 圖21為圖20中的低通濾波器的框圖； 圖22為本發(fā)明第十六示例性實(shí)施例的框圖； 圖23為本發(fā)明第十七示例性實(shí)施例的框圖； 圖24為圖23中的微分器的框圖； 圖25為根據(jù)本發(fā)明的第十七示例性實(shí)施例的流程圖； 圖26為示出了Z軸旋轉(zhuǎn)和姿態(tài)角φ之間的關(guān)系的圖； 圖27為示出了姿態(tài)角φ在Z軸旋轉(zhuǎn)180±20度的圖； 圖28為示出了通過(guò)了LPF(fc＝中)的用于SCA處理、姿態(tài)角φ和旋轉(zhuǎn)角的信號(hào)的圖； 圖29為示出了通過(guò)了LPF(fc＝小)的用于SCA處理、姿態(tài)角φ和旋轉(zhuǎn)角的信號(hào)的圖； 圖30為示出了姿態(tài)角θ和Y軸旋轉(zhuǎn)之間的關(guān)系的圖； 圖31為圖示了姿態(tài)角θ在Y軸旋轉(zhuǎn)90±20度的圖； 圖32為示出了通過(guò)了LPF(fc＝中)的用于SCA處理、姿態(tài)角θ和旋轉(zhuǎn)角的信號(hào)的圖； 圖33為示出了通過(guò)了LPF(fc＝小)的用于sin-cos轉(zhuǎn)換、姿態(tài)角θ和旋轉(zhuǎn)角的信號(hào)的圖。

具體實(shí)施例方式
本發(fā)明的示例性實(shí)施例將參照附圖，使用諸如機(jī)器人作為移動(dòng)體和傳感器單元作為姿態(tài)角檢測(cè)設(shè)備，來(lái)進(jìn)行描述。

下面描述本發(fā)明的第一示例性實(shí)施例。圖1為示出了根據(jù)本發(fā)明的第一示例性實(shí)施例的移動(dòng)體姿態(tài)角檢測(cè)設(shè)備的結(jié)構(gòu)的框圖。包括角速度傳感器10和加速度傳感器20的傳感器單元1設(shè)置在機(jī)器人上的預(yù)定位置。

角速度傳感器10檢測(cè)在傳感器坐標(biāo)系(即，傳感器單元的坐標(biāo)系)上的角速度。圍繞軸的角速度被表示為ωx、ωy和ωz。角速度傳感器10將檢測(cè)到的角速度輸出到小角度矩陣計(jì)算器12。

此小角度矩陣計(jì)算器12計(jì)算坐標(biāo)轉(zhuǎn)換矩陣，用于計(jì)算當(dāng)在參考坐標(biāo)系XYZ中觀察時(shí)的傳感器坐標(biāo)系xyz。小角度矩陣在下面描述。小角度矩陣計(jì)算器12將計(jì)算出的小角度矩陣輸出到矩陣加法計(jì)算器14。

矩陣加法計(jì)算器14通過(guò)將從小角度矩陣計(jì)算器12輸入的由角速度傳感器10檢測(cè)到的當(dāng)前小角度矩陣，與從輸出側(cè)反饋回來(lái)的姿態(tài)角矩陣相加來(lái)執(zhí)行積分計(jì)算。然后矩陣加法計(jì)算器14將積分后的矩陣輸出到矩陣姿態(tài)角計(jì)算器16。

矩陣姿態(tài)角計(jì)算器16從輸入的積分后的矩陣來(lái)計(jì)算姿態(tài)角，并且將結(jié)果作為角速度姿態(tài)角SYA輸出到低通濾波器LPF18和減法器32。

低通濾波器(即，角速度系統(tǒng)低通濾波器)18從角速度姿態(tài)角SYA提取低范圍分量并且將其輸出到微分器30。角速度姿態(tài)角SYA包括真值的姿態(tài)角(姿態(tài)角真值)和來(lái)自積分誤差的偏移量。低通濾波器18從角速度姿態(tài)角SYA去除高范圍姿態(tài)角真值，并且提取低范圍姿態(tài)角真值和偏移量。

同時(shí)，加速度傳感器20檢測(cè)在傳感器坐標(biāo)系xyz中的各軸方向上的加速度。檢測(cè)到的加速度被表示為Gx、Gy和Gz。然后加速度傳感器20將檢測(cè)到的加速度輸出到傾斜角計(jì)算器22。

傾斜角計(jì)算器22計(jì)算參考坐標(biāo)系XYZ的Z軸和傳感器坐標(biāo)系xyz的各軸之間的角度。所計(jì)算出的傾斜角被表示為λx、λy和λz。然后傾斜角計(jì)算器22將所計(jì)算出的傾斜角輸出到加速度矩陣計(jì)算器24。

加速度矩陣計(jì)算器24利用輸入的傾斜角來(lái)計(jì)算當(dāng)在參考坐標(biāo)系觀察時(shí)關(guān)于傳感器坐標(biāo)系的姿態(tài)矩陣，并且將所計(jì)算的姿態(tài)矩陣輸出到矩陣姿態(tài)角計(jì)算器26。

然后，矩陣姿態(tài)角計(jì)算器26從輸入的矩陣來(lái)計(jì)算姿態(tài)角，并且將計(jì)算出的姿態(tài)角作為加速度姿態(tài)角SGA輸出到低通濾波器28，與矩陣姿態(tài)角計(jì)算器16相似。

低通濾波器(即，加速度系統(tǒng)低通濾波器)28為一種濾波器，其中的截止頻率和衰減因子與它們?cè)诘屯V波器18中設(shè)置得相同。低通濾波器28從加速度姿態(tài)角SGA提取低范圍分量并且將其輸出到微分器30。加速度姿態(tài)角SGA包括高范圍分量中的高范圍姿態(tài)角偽值和低范圍分量中的低范圍姿態(tài)角真值。在加速度姿態(tài)角中沒(méi)有發(fā)生來(lái)自積分誤差的偏移。因?yàn)榈屯V波器28的截止頻率和衰減因子設(shè)置得與它們?cè)诘屯V波器18中相同，所以角速度系統(tǒng)的低范圍姿態(tài)角真值與加速度系統(tǒng)的低范圍姿態(tài)角真值相同。角速度系統(tǒng)的頻率特性也與加速度系統(tǒng)的頻率特性相同。因而，通過(guò)兩個(gè)低范圍姿態(tài)角真值的減法計(jì)算只有偏移分量被準(zhǔn)確地輸出。然而，當(dāng)與角速度系統(tǒng)和加速度系統(tǒng)產(chǎn)生的姿態(tài)角相對(duì)應(yīng)的分量的響應(yīng)度預(yù)先已知時(shí)，低通濾波器的截止頻率和衰減因子可以調(diào)整為與它們的特性相匹配。該方法在適應(yīng)范圍窄而移動(dòng)特性預(yù)先已知時(shí)是有效的。

如上所述，來(lái)自低通濾波器18的(低范圍姿態(tài)角真值加上偏移量)，以及來(lái)自低通濾波器28的(低范圍姿態(tài)角真值)被輸入到微分器30。然后微分器30計(jì)算這兩者之間的差值并且提取偏移量(即，偏移量＝(低范圍姿態(tài)角真值+偏移量)-(低范圍姿態(tài)角真值))。然后微分器30將通過(guò)微分計(jì)算獲得的偏移量輸出到減法器32。

減法器32計(jì)算從矩陣姿態(tài)角計(jì)算器16輸入的角速度姿態(tài)角SYA和從微分器30輸入的偏移量之間的差值，然后去除包括在來(lái)自矩陣姿態(tài)角計(jì)算器16的角速度姿態(tài)角SYA中的偏移量。如上所述，來(lái)自矩陣姿態(tài)角計(jì)算器16的角速度姿態(tài)角SYA包括姿態(tài)角真值(高范圍分量+低范圍分量)和偏移量。然后減法器32通過(guò)減法(即，低范圍姿態(tài)角真值＝(低范圍姿態(tài)角真值+偏移量)-(偏移量))提取低范圍姿態(tài)角真值。然后減法器32將通過(guò)減法獲得的姿態(tài)角真值輸出到輸出裝置34。減法器32還將姿態(tài)角真值輸出到姿態(tài)角矩陣計(jì)算器36。

姿態(tài)角矩陣計(jì)算器36從輸入的姿態(tài)角來(lái)計(jì)算姿態(tài)角矩陣，并且將其反饋到矩陣加法計(jì)算器14。然后姿態(tài)角矩陣計(jì)算器36執(zhí)行矩陣姿態(tài)角計(jì)算器16的反運(yùn)算(inverse operation)，并且從姿態(tài)角生成姿態(tài)角矩陣。然后矩陣加法計(jì)算器14通過(guò)將反饋回來(lái)的姿態(tài)角矩陣加到基于角速度傳感器10新檢測(cè)到的角速度計(jì)算出的小角度矩陣來(lái)執(zhí)行積分計(jì)算，然后從角速度計(jì)算姿態(tài)角。

輸出裝置34將已經(jīng)由減法器32去除了偏移量的姿態(tài)角輸出到外部部件。在機(jī)器人的例子中，主處理器(或主處理機(jī))輸入由傳感器單元檢測(cè)到的姿態(tài)角等，并且對(duì)機(jī)器人的姿態(tài)執(zhí)行反饋控制。輸出裝置34響應(yīng)來(lái)自主處理器的命令在預(yù)定的時(shí)刻將姿態(tài)角輸出到主處理器。由角速度傳感器10檢測(cè)到的角速度或由加速度傳感器檢測(cè)到的加速度可以提供到輸出裝置34，并且除了姿態(tài)角之外，輸出裝置34還可以輸出角速度或加速度到主處理器。這些要被輸出的是由來(lái)自主處理器的命令或設(shè)置的參數(shù)來(lái)確定。輸出裝置34在給定的時(shí)刻將所有的角速度、加速度和姿態(tài)角輸出到主處理器，并且在另一個(gè)時(shí)刻選擇性地輸出其中的一個(gè)。盡管圖中未示出，通過(guò)姿態(tài)角矩陣計(jì)算器36獲得的姿態(tài)矩陣也可以經(jīng)由輸出裝置34輸出到機(jī)器人。這是當(dāng)在機(jī)器人內(nèi)執(zhí)行矩陣計(jì)算時(shí)的有效方法。

如上所述，因而去除了偏移量的準(zhǔn)確的姿態(tài)角被檢測(cè)和輸出。因?yàn)槠屏渴腔谟杉铀俣葌鞲衅?0檢測(cè)到的在各軸向上的加速度提取出來(lái)的，然后從角速度姿態(tài)角去除偏移量并且結(jié)果被反饋，角速度傳感器10不需要是昂貴的、重的并且極其精確的傳感器。

此后，將描述在小角度矩陣計(jì)算器12、矩陣姿態(tài)角計(jì)算器16、傾斜角計(jì)算器22和矩陣姿態(tài)角計(jì)算器26中執(zhí)行的計(jì)算。

首先，將描述姿態(tài)矩陣。在離散時(shí)間n中的姿態(tài)矩陣T(n)用于表示參考坐標(biāo)系XYZ中的傳感器坐標(biāo)系。姿態(tài)矩陣T(n)由4×4元素組成，如下面的表達(dá)式1所示。
[表達(dá)式1]
按照矩陣T(n)的含義，第一列(a，b，c)、第二列(d，e，f)和第三列(g，h，i)分別指的是當(dāng)從參考坐標(biāo)系中看時(shí)傳感器坐標(biāo)系n的x軸、y軸和z軸的方向矢量。第四列表示參考坐標(biāo)系中的傳感器坐標(biāo)系n的原點(diǎn)(即，典型地，當(dāng)有平移時(shí)，平移量在第四列中表示)。當(dāng)原點(diǎn)固定時(shí)，表示位置轉(zhuǎn)換的第四列中的第一到第三行的元素設(shè)置為0。如圖7中所示，傳感器坐標(biāo)系的原點(diǎn)O位于參考坐標(biāo)系的(0，0，0)，x軸矢量在參考坐標(biāo)系上具有分量(a，b，c)，y軸矢量在參考坐標(biāo)系上具有分量(d，e，f)，z軸矢量在參考坐標(biāo)系上具有分量(g，h，i)。

下面描述用于從姿態(tài)角的側(cè)傾角、俯仰角和橫擺角獲得姿態(tài)矩陣T(n)的方法。該方法基于來(lái)自減法器32的姿態(tài)角通過(guò)姿態(tài)角矩陣計(jì)算器36來(lái)計(jì)算姿態(tài)矩陣。為了表示姿態(tài)矩陣T(n)，來(lái)自矩陣的旋轉(zhuǎn)變換必須要考慮到旋轉(zhuǎn)軸的順序。如圖8中所示，當(dāng)使用典型地用于機(jī)器人的側(cè)傾角、俯仰角和橫擺角時(shí)，它們被定義為發(fā)生的三種旋轉(zhuǎn)，即，首先是圍繞z軸的旋轉(zhuǎn)φ，接下來(lái)是在所述旋轉(zhuǎn)之后圍繞y軸的旋轉(zhuǎn)θ，最后是在所述旋轉(zhuǎn)之后圍繞x軸的旋轉(zhuǎn)ψ(應(yīng)注意軸的旋轉(zhuǎn)順序是特定的)。

傾角、俯仰角和橫擺角的轉(zhuǎn)換矩陣表示為RPY(φ，θ，ψ)。RPY(φ，θ，ψ)是從左到右相乘的旋轉(zhuǎn)變換矩陣的乘積，如下面的表達(dá)式2所示。
[表達(dá)式2] RPY(φ，θ，ψ)＝Rot(z，φ)Rot(y，θ)Rot(x，ψ) (2)
表達(dá)式3更詳細(xì)地示出了表達(dá)式2。
[表達(dá)式3]
當(dāng)全部寫(xiě)出表達(dá)式3時(shí)，可以表示為表達(dá)式4。
[表達(dá)式4]
歐拉角可以用于代替?zhèn)葍A角、俯仰角和橫擺角作為姿態(tài)角。用歐拉角，當(dāng)首先圍繞z軸發(fā)生旋轉(zhuǎn)φ，接下來(lái)是在所述旋轉(zhuǎn)之后圍繞y軸發(fā)生旋轉(zhuǎn)θ，最后是在所述旋轉(zhuǎn)之后圍繞x軸發(fā)生旋轉(zhuǎn)ψ時(shí)，轉(zhuǎn)換矩陣表示為Euler(Eφ，Eθ，Eψ)，并如表達(dá)式5所示。
[表達(dá)式5] Euler(Eφ，Eθ，Eψ)＝Rot(z，Eφ)Rot(y，Eθ)Rot(x，Eψ) (5)
表達(dá)式5在下面的表達(dá)式6中詳細(xì)示出。
[表達(dá)式6]
當(dāng)全部寫(xiě)出表達(dá)式6時(shí)，可以表示為表達(dá)式7。
[表達(dá)式7]
參考坐標(biāo)系被表示為O-XYZ，并且初始傳感器坐標(biāo)系被表示為O0-x0y0z0。參考坐標(biāo)系和坐標(biāo)系O0-x0y0z0在時(shí)刻t＝0通過(guò)坐標(biāo)轉(zhuǎn)換A(0)相關(guān)聯(lián)。坐標(biāo)系在時(shí)刻t＝tn被表示為On-xnynzn。每一個(gè)坐標(biāo)系的原點(diǎn)O、O0、On是相同的并且位置沒(méi)有移動(dòng)。如圖9中所示，當(dāng)從坐標(biāo)系O(n-1)-x(n-1)y(n-1)z(n-1)變到On-xnynzn時(shí)，由于移動(dòng)體的姿態(tài)的變化，O(n-1)-x(n-1)y(n-1)z(n-1)和On-xnynzn通過(guò)從傳感器輸出獲得的矩陣A(n)相關(guān)聯(lián)。當(dāng)從參考坐標(biāo)系中看時(shí)，通過(guò)在表達(dá)式8中將轉(zhuǎn)換A(n)從右相乘獲得傳感器坐標(biāo)系T(n)。當(dāng)傳感器坐標(biāo)系的原點(diǎn)隨著時(shí)間移動(dòng)時(shí)，將已經(jīng)隨著時(shí)間移動(dòng)的坐標(biāo)連續(xù)地輸入，作為矩陣A的第四列的第一到第三個(gè)元素。矩陣A中的第四列不受傳感器坐標(biāo)系的旋轉(zhuǎn)的影響，所以在此不作特別的描述。
[表達(dá)式8] A(n)＝A(0)A(1)............A(n-1)A(n) (8)
接下來(lái)，描述用于引入(introduce)來(lái)自傳感器輸出的小旋轉(zhuǎn)矩陣A(n)的矩陣的方法。此方法利用小角度矩陣計(jì)算器12，基于來(lái)自角速度傳感器10的角速度來(lái)計(jì)算小角度矩陣。在傳感器坐標(biāo)系的每個(gè)軸上設(shè)置三個(gè)角速度傳感器中的一個(gè)。如圖10中所示，三個(gè)角速度傳感器測(cè)量圍繞傳感器的軸x、y和z的角速度。當(dāng)在表達(dá)式4中旋轉(zhuǎn)角Δφ、Δθ、Δψ足夠小時(shí)，可得到如下表達(dá)式。
[表達(dá)式9] sinΔφ≈Δφ，sinΔθ≈Δθ，sinΔψ≈Δψ(9) [表達(dá)式10] cosΔφ≈1，cosΔθ≈1，cosΔψ≈1 (10) 因此，這可以利用圍繞傳感器x軸的小旋轉(zhuǎn)角Δψ、圍繞傳感器y軸的小旋轉(zhuǎn)角Δθ以及圍繞傳感器z軸的小旋轉(zhuǎn)角Δφ通過(guò)表達(dá)式11來(lái)表示。表達(dá)式11的結(jié)果類似于不取決于旋轉(zhuǎn)順序的情況，因?yàn)榫仃囍械拿恳粋€(gè)元素由小旋轉(zhuǎn)角獨(dú)立的表示。
[表達(dá)式11]
如表達(dá)式12和13所示，小角度和傳感器輸出通過(guò)小旋轉(zhuǎn)角Δψ、Δθ、Δφ，角速度傳感器輸出ωx、ωy、ωz，以及采樣周期ts相關(guān)聯(lián)。采樣周期ts相對(duì)于旋轉(zhuǎn)運(yùn)動(dòng)足夠早，所以在采樣周期ts的時(shí)間內(nèi)的旋轉(zhuǎn)足夠小，因此被認(rèn)為是小旋轉(zhuǎn)角。
[表達(dá)式12] Δψ＝ωxts(12) [表達(dá)式13] Δθ＝ωyts(13) [表達(dá)式14] Δφ＝ωzts(14)
因此，A(n)矩陣可以通過(guò)表達(dá)式15來(lái)表示。
[表達(dá)式15]
接下來(lái)，描述用于從姿態(tài)矩陣獲得姿態(tài)角的方法。該方法利用矩陣姿態(tài)角計(jì)算器16，基于來(lái)自矩陣加法計(jì)算器14的姿態(tài)矩陣(即，積分后的姿態(tài)矩陣)來(lái)計(jì)算姿態(tài)角。該方法還利用矩陣姿態(tài)角計(jì)算器26，基于來(lái)自加速度矩陣計(jì)算器24的姿態(tài)矩陣來(lái)計(jì)算姿態(tài)角。

姿態(tài)矩陣T(n)如表達(dá)式16所示。
[表達(dá)式16]
橫擺角φ為 [表達(dá)式17] φ＝atan2(b，a) (17) 并且姿態(tài)角φ的變化范圍為-πφ≤π。

俯仰角θ為 [表達(dá)式18] θ＝atan2(-c，cosφ·a+sinφb) (18) 并且姿態(tài)角θ的變化范圍為-π/2＜θ＜π/2。

側(cè)傾角ψ為 [表達(dá)式19] ψ＝atan2(sinφ·g-cosφ·h，-sinφ·d+cosφ·e)(19) 并且姿態(tài)角ψ的變化范圍為-π＜ψ≤π。

如果使用歐拉角，則使用表達(dá)式21到23。
[表達(dá)式20] Eφ為 [表達(dá)式21] Eφ＝atan2(b，a) (21) Eθ為 [表達(dá)式22] Eθ＝atan2(cosφ·g+sinφ·h，i) (22) 并且Eψ為 [表達(dá)式23] Eψ＝atan2(-sinφ·a+cosφ·b，-sinφ·d+cosφ·e)(23)
接下來(lái)，描述矩陣的正規(guī)化(normalization)。
[表達(dá)式24]
用姿態(tài)矩陣T(n)，在計(jì)算后，姿態(tài)矩陣中的每一列可能不是單位矢量，所以通過(guò)表達(dá)式25執(zhí)行正規(guī)化以使得表達(dá)式24中的每一列矢量的大小成為1。這是通過(guò)圖5中的正規(guī)器(normalizer)/正交器(orthogonalizer)54和56執(zhí)行的計(jì)算，在后面進(jìn)行描述。
[表達(dá)式25]
這里，p1和p2由表達(dá)式26和27提供。
[表達(dá)式26] [表達(dá)式27]
然后，正規(guī)化后的每一個(gè)元素被再次重置為 [表達(dá)式28]
此外，下面描述矩陣的正交化(orthogonalization)。
[表達(dá)式29]
對(duì)于姿態(tài)矩陣T(n)，在計(jì)算后，姿態(tài)矩陣中的列可能不是正交軸，所以執(zhí)行使得表達(dá)式29中的列矢量正交化的正交化過(guò)程(在該例子中，z軸用作參考)。為了獲得與z軸和y軸正交的新的x’軸，要獲得a’、b’和c’。
[表達(dá)式30] a’＝ei-fh(30) [表達(dá)式31] b’＝fg-di(31) [表達(dá)式32] c’＝dh-eg(32)
接下來(lái)，為了獲得與z軸和x’軸正交的新的y’軸，要獲得d’、e’和f’。
[表達(dá)式33] d’＝hc’-ib’(33) [表達(dá)式34] e’＝ia’-gc’(34) [表達(dá)式35] f’＝gb’-ha’ (35)
然后從獲得的a’到f’，來(lái)獲得已經(jīng)正交化的姿態(tài)矩陣T(n)。
[表達(dá)式36]
這里，描述atan2。atan2(y，x)是具有兩個(gè)變量x和y的計(jì)算器函數(shù)。atan2的應(yīng)用范圍比通常使用的atan函數(shù)廣。
[表達(dá)式37] ξ＝atan2(y，x)(37) 當(dāng)x＞0并且y＞0時(shí)，-π＜ξ≤π [表達(dá)式38] ξ＝tan-1(y/x) (38) 以及當(dāng)x＞0并且y＜0時(shí)， [表達(dá)式39] ξ＝tan-1(y/x) (39) 以相同方式， 當(dāng)x＜0并且y＞0時(shí)，則ξ＝π+tan-1(y/x)， 當(dāng)x＜0并且y＜0時(shí)，則ξ＝-π+tan-1(y/x)， 當(dāng)x＝0并且y＞0時(shí)，則ξ＝π/2， 當(dāng)x＝0并且y＜0時(shí)，則ξ＝-π/2， 以及當(dāng)x＝0并且y＝0時(shí)，則ξ＝0。

接下來(lái)，描述對(duì)傾斜角的計(jì)算。該方法利用傾斜角計(jì)算器22，基于通過(guò)加速度傳感器20獲得的加速度來(lái)計(jì)算傾斜角。傾斜角指的是在傳感器x、y、z軸和參考Z軸之間的角λx、λy、λz。即，λx是x軸和Z軸之間的角，λy是y軸和Z軸之間的角，λz是z軸和Z軸之間的角。λx、λy和λz的范圍為0≤(λx，λy，λz)≤π。圖11示出了傾斜角和重力矢量。

傾斜角如下面所示從設(shè)置在傳感器坐標(biāo)上的加速度傳感器獲得。利用表達(dá)式40到42來(lái)正規(guī)化加速度Gx、Gy和Gz，并且獲得正規(guī)化后的加速度Gx’、Gy’和Gz’。
[表達(dá)式40] [表達(dá)式41] [表達(dá)式42]
利用表達(dá)式43到45從加速度Gx’、Gy’和Gz’獲得傾斜角λx、λy和λz。
[表達(dá)式43] λx＝acos(-Gx’)(43) [表達(dá)式44] λy＝acos(-Gy’)(44) [表達(dá)式45] λz＝acos(-Gz’)(45)
接下來(lái)，描述從傾斜角λx、λy和λz獲得姿態(tài)矩陣T(n)的過(guò)程。這是利用加速度矩陣計(jì)算器24，基于來(lái)自傾斜角計(jì)算器22的傾斜角來(lái)獲得姿態(tài)矩陣的計(jì)算。
[表達(dá)式46] c＝cos(λx) (46) [表達(dá)式47] [表達(dá)式48] b＝0(48) [表達(dá)式49] f＝cos(λy) (49) [表達(dá)式50] d＝-cf/a (50) [表達(dá)式51]當(dāng)0≤λz＜π/2時(shí)(51) [表達(dá)式52]當(dāng)π/2＜λz≤π時(shí) (52) [表達(dá)式53] e＝0當(dāng)λz＝π/2時(shí)(53) [表達(dá)式54] g＝-ce (54) [表達(dá)式55] h＝cd-af (55) [表達(dá)式56] i＝ae(56) 從上述結(jié)果獲得姿態(tài)矩陣T(n)。

接下來(lái)，描述用于從姿態(tài)矩陣T(n)獲得傾斜角λx、λy和λz的過(guò)程。
[表達(dá)式57] [表達(dá)式58] λx＝acos(c)(58) [表達(dá)式59] λy＝acos(f)(59) [表達(dá)式60] λz＝acos(i)(60)
下面繼續(xù)描述第二示例性實(shí)施例。圖2為本發(fā)明的第二示例性實(shí)施例的框圖。此示例性實(shí)施例與第一示例性實(shí)施例的不同在于，微分器30計(jì)算低通濾波器18的輸出和低通濾波器28的輸出之間的差值，并且將其輸出到乘法器38、積分器40和微分器42。乘法器38、積分器40和微分器42執(zhí)行PID(即，比例積分微分)控制。即，乘法器38執(zhí)行將來(lái)自微分器30的偏移量與預(yù)先設(shè)置的系數(shù)相乘的計(jì)算，并且將計(jì)算值(比例項(xiàng))輸出到減法器32。積分器40執(zhí)行將來(lái)自微分器30的偏移量隨著時(shí)間累積的值與預(yù)先設(shè)置的系數(shù)相乘的計(jì)算，并且將計(jì)算值(積分項(xiàng))輸出到減法器32。微分器42執(zhí)行來(lái)自微分器30的偏移量隨著時(shí)間微分的值與預(yù)先設(shè)置的系數(shù)相乘的計(jì)算，并且將計(jì)算值(微分項(xiàng))輸出到減法器32。所述系數(shù)可以是固定的或者它們也可以如下所述是可變的。減法器32從由矩陣姿態(tài)角計(jì)算器16輸入的角速度姿態(tài)角減去這些項(xiàng)(即，比例項(xiàng)+積分項(xiàng)+微分項(xiàng))的和，并且將結(jié)果作為減去偏移量的移動(dòng)體的姿態(tài)角輸出到輸出裝置34和姿態(tài)角矩陣計(jì)算器36。減法器32的輸出由姿態(tài)角矩陣計(jì)算器36轉(zhuǎn)換為姿態(tài)矩陣然后被反饋。乘法器38主要確定反饋程度，積分器40累積長(zhǎng)期偏離的量并且確定反饋量，微分器42確定有關(guān)短期變化的反饋量。利用此類型的PID控制短期和長(zhǎng)期特性均得以改善。

接著，描述第三示例性實(shí)施例。圖3為本發(fā)明第三示例性實(shí)施例的框圖。此示例性實(shí)施例與第二示例性實(shí)施例的不同在于，來(lái)自外部姿態(tài)角44的繞Z軸的姿態(tài)角，即橫擺角，被提供到矩陣姿態(tài)角計(jì)算器26。加速度傳感器20檢測(cè)在軸x、y和z方向上的加速度。然而，當(dāng)使用了G傳感器時(shí)，橫擺角φ，即繞Z軸的姿態(tài)角不確定，所以需要將橫擺角φ固定為恒定值。在此示例性實(shí)施例中，橫擺角被單獨(dú)檢測(cè)并且被提供給矩陣姿態(tài)角計(jì)算器26，因此使得也可以準(zhǔn)確地計(jì)算橫擺角φ。更具體地，根據(jù)如上所述的計(jì)算，矩陣姿態(tài)角計(jì)算器26計(jì)算橫擺角φ、側(cè)傾角θ和俯仰角ψ，作為來(lái)自姿態(tài)矩陣T(n)的姿態(tài)角。然而，這些中的橫擺角φ用從外部部件輸入的橫擺角φ代替，這防止了橫擺角φ的發(fā)散。

接著，下面描述第四示例性實(shí)施例。圖4為本發(fā)明第四示例性實(shí)施例的框圖。在前述的各示例性實(shí)施例中，來(lái)自矩陣姿態(tài)角計(jì)算器16的角速度姿態(tài)角輸出到低通濾波器18，來(lái)自矩陣姿態(tài)角計(jì)算器26的加速度姿態(tài)角輸出到低通濾波器28。然而，在第四示例性實(shí)施例中，來(lái)自矩陣加法計(jì)算器14的姿態(tài)矩陣輸出到低通濾波器(ALPF)46，來(lái)自加速度矩陣計(jì)算器24的姿態(tài)矩陣輸出到低通濾波器(ALPF)48，并且低范圍分量在矩陣階段而不是在姿態(tài)角階段提取。低通濾波器46和48適合4×4姿態(tài)矩陣中的每個(gè)矩陣分量并且提取相應(yīng)的低范圍分量。低通濾波器46和48的截止頻率設(shè)置為相同，衰減因子也相同。在從姿態(tài)矩陣提取低范圍分量之后，低通濾波器(角速度系統(tǒng))46將姿態(tài)矩陣輸出到矩陣姿態(tài)角計(jì)算器50。與矩陣姿態(tài)角計(jì)算器16相似，矩陣姿態(tài)角計(jì)算器50從姿態(tài)矩陣計(jì)算角速度姿態(tài)角并且將其輸出到微分器30。低通濾波器46和48應(yīng)用到表示旋轉(zhuǎn)的分量，即4×4姿態(tài)矩陣中的3×3矩陣分量的第一列(a，b，c)、第二列(e，d，f)和第三列(g，h，i)，因此使得容易提取相應(yīng)的低范圍分量。還有，低通濾波器可以應(yīng)用到全部六個(gè)分量，即從3×3矩陣分量中，在一側(cè)上的三個(gè)相對(duì)的角分量，以及在另一側(cè)的三個(gè)非對(duì)角線分量，并且可以產(chǎn)生等價(jià)的矩陣濾波器。

同樣，在從姿態(tài)矩陣提取低范圍分量之后，低通濾波器(加速度系統(tǒng))48將姿態(tài)矩陣輸出到矩陣姿態(tài)角計(jì)算器26。與第三示例性實(shí)施例相似，來(lái)自外部姿態(tài)角44的繞Z軸的姿態(tài)角，即橫擺角，可被提供到加速度系統(tǒng)。然而，在第三示例性實(shí)施例中，由外部傳感器檢測(cè)到的橫擺角φ被提供到矩陣姿態(tài)角計(jì)算器26，但是在第四示例性實(shí)施例中，橫擺角φ被提供到加速度矩陣計(jì)算器24。當(dāng)然這樣做是為了使用低通濾波器48來(lái)提取姿態(tài)矩陣的低范圍分量，也是因?yàn)樾枰ㄓ赏獠總鞲衅鳈z測(cè)到的橫擺角φ作為姿態(tài)矩陣的分量。

接著，下面描述第五示例性實(shí)施例。圖5為本發(fā)明第五示例性實(shí)施例的框圖。第五示例性實(shí)施例與第四示例性實(shí)施例的不同在于，在低通濾波器46和48之后增加正規(guī)器/正交器54和56。當(dāng)通過(guò)低通濾波器46和48從姿態(tài)矩陣提取低范圍分量時(shí)，姿態(tài)矩陣變形使得正規(guī)化和正交化不再能被保證。因此利用正規(guī)器/正交器54和56通過(guò)正規(guī)化和正交化來(lái)修正該變形。正規(guī)化和正交化的方法如前所述。

接著，下面描述第六示例性實(shí)施例。圖6為本發(fā)明第六示例性實(shí)施例的框圖。該示例性實(shí)施例與第四示例性實(shí)施例的不同在于，設(shè)置了運(yùn)動(dòng)頻率傳感器52，其輸入來(lái)自角速度傳感器10的角速度和來(lái)自加速度傳感器20的加速度，并且檢測(cè)在機(jī)器人，即移動(dòng)物體的運(yùn)動(dòng)中的特定頻率。運(yùn)動(dòng)頻率傳感器52從角速度和加速度中提取經(jīng)常出現(xiàn)的頻率分量，并且將其輸出到低通濾波器46和48。一個(gè)例子為當(dāng)機(jī)器人，即移動(dòng)物體用兩條腿行走時(shí)的兩足移動(dòng)中的特定頻率。低通濾波器46和48根據(jù)從運(yùn)動(dòng)頻率傳感器52輸入的頻率來(lái)適應(yīng)性地調(diào)整截止頻率。運(yùn)動(dòng)頻率越大，低通濾波器46和48越要增大截止頻率。此外，運(yùn)動(dòng)頻率傳感器52將運(yùn)動(dòng)頻率輸出到執(zhí)行PID控制的乘法器38、積分器40和微分器42。乘法器38、積分器40和微分器42根據(jù)輸入的運(yùn)動(dòng)頻率調(diào)整乘法器系數(shù)。根據(jù)運(yùn)動(dòng)中的特定頻率來(lái)適應(yīng)性地調(diào)整截止頻率和PID控制的系數(shù)，以此方式提高響應(yīng)度，以及跟隨和適應(yīng)的能力。

接著，描述第七示例性實(shí)施例。圖12為根據(jù)本發(fā)明第七示例性實(shí)施例的移動(dòng)體姿態(tài)角檢測(cè)設(shè)備的框圖。

角速度傳感器10檢測(cè)傳感器坐標(biāo)系(即，傳感器單元的坐標(biāo)系)中的角速度。圍繞軸的角速度被表示為ωx、ωy和ωz。角速度傳感器10將檢測(cè)到的角速度輸出到小角度矩陣計(jì)算器12。

小角度矩陣計(jì)算器12計(jì)算用于計(jì)算當(dāng)從參考坐標(biāo)系XYZ中看時(shí)的傳感器坐標(biāo)系xyz的坐標(biāo)轉(zhuǎn)換矩陣。小角度矩陣在下面描述。小角度矩陣計(jì)算器12將計(jì)算出的小角度矩陣輸出到矩陣加法計(jì)算器14。

矩陣加法計(jì)算器14通過(guò)將從小角度矩陣計(jì)算器12輸入的由角速度傳感器10檢測(cè)到的當(dāng)前小角度矩陣加到從輸出側(cè)反饋回來(lái)的姿態(tài)矩陣來(lái)執(zhí)行積分計(jì)算。然后矩陣加法計(jì)算器14將積分后的矩陣(即，角速度系統(tǒng)姿態(tài)矩陣)輸出到低通濾波器(ALPF)46。矩陣加法計(jì)算器14還將積分后的矩陣輸出到減法器32。

低通濾波器(即，角速度系統(tǒng)低通濾波器)46從角速度系統(tǒng)姿態(tài)矩陣的分量中提取低范圍分量并且將其輸出到微分器30。角速度系統(tǒng)姿態(tài)矩陣包括真姿態(tài)(即，姿態(tài)真值)和來(lái)自積分誤差的偏移量。低通濾波器46從角速度系統(tǒng)姿態(tài)矩陣中去除高范圍姿態(tài)真值，并且提取低范圍姿態(tài)真值和偏移量。

同時(shí)，加速度傳感器20檢測(cè)在傳感器坐標(biāo)系xyz中的每個(gè)軸的方向上的加速度。檢測(cè)到的加速度被表示為Gx、Gy和Gz。加速度傳感器20將檢測(cè)到的加速度輸出到傾斜角計(jì)算器22。

傾斜角計(jì)算器22計(jì)算在傳感器坐標(biāo)系xyz的每個(gè)軸和參考坐標(biāo)系XYZ中的Z軸之間的角度。計(jì)算出的傾斜角度被表示為λx、λy和λz。然后傾斜角計(jì)算器22將計(jì)算出的傾斜角輸出到加速度矩陣計(jì)算器24。

加速度矩陣計(jì)算器24利用輸入的傾斜角，來(lái)計(jì)算當(dāng)在參考坐標(biāo)系中看時(shí)有關(guān)傳感器坐標(biāo)系的姿態(tài)矩陣，并且將此結(jié)果作為加速度系統(tǒng)姿態(tài)矩陣輸出到低通濾波器(ALPF)48。

低通濾波器(即，加速度系統(tǒng)低通濾波器)48為截止頻率和衰減因子設(shè)置得與其在低通濾波器46中相同的濾波器。低通濾波器48從加速度系統(tǒng)姿態(tài)矩陣中提取低范圍分量并且將其輸出到微分器30。加速度系統(tǒng)姿態(tài)矩陣包括產(chǎn)生在高范圍中的噪聲、高范圍姿態(tài)真值和低范圍姿態(tài)真值。在加速度系統(tǒng)姿態(tài)矩陣中沒(méi)有發(fā)生來(lái)自積分誤差的漂移。因?yàn)榈屯V波器48的截止頻率和衰減因子設(shè)置得與其在低通濾波器46中相同，角速度系統(tǒng)的低范圍姿態(tài)角真值與加速度系統(tǒng)的低范圍姿態(tài)角真值相同。角速度系統(tǒng)的頻率特性也與加速度系統(tǒng)的頻率特性相同。因此，通過(guò)兩個(gè)低范圍姿態(tài)角度真值的減法運(yùn)算，只有漂移分量被準(zhǔn)確地輸出。然而，當(dāng)與由角速度系統(tǒng)和加速度系統(tǒng)產(chǎn)生的姿態(tài)相對(duì)應(yīng)的分量的響應(yīng)預(yù)先已知時(shí)，可以調(diào)整低通濾波器的截止頻率和衰減因子來(lái)匹配它們的特性。當(dāng)適應(yīng)范圍窄但是移動(dòng)特性預(yù)先已知時(shí)此方法有效。

如上所述，來(lái)自低通濾波器46的(低范圍姿態(tài)真值加上漂移分量)，以及來(lái)自低通濾波器48的(低范圍姿態(tài)真值)輸入進(jìn)微分器30。然后微分器30計(jì)算這兩者之間的差值，并且提取偏移量(即，偏移量＝(低范圍姿態(tài)真值+偏移量)-(低范圍姿態(tài)真值))。然后微分器30將通過(guò)微分計(jì)算獲得的偏移量輸出到減法器32。

減法器32計(jì)算從矩陣加法計(jì)算器14輸入的角速度系統(tǒng)姿態(tài)和從微分器30輸入的偏移量之間的差值(即，矩陣差值)，然后去除包括在來(lái)自矩陣加法計(jì)算器14的角速度系統(tǒng)姿態(tài)矩陣中的偏移量。如上所述，來(lái)自矩陣加法計(jì)算器14的角速度系統(tǒng)姿態(tài)矩陣包括姿態(tài)真值和偏移量。然后減法器32通過(guò)減法來(lái)提取姿態(tài)真值(即，姿態(tài)真值＝(姿態(tài)真值+偏移量)-(偏移量))。然后減法器32將用減法獲得的姿態(tài)真值的角速度系統(tǒng)姿態(tài)矩陣輸出到矩陣姿態(tài)角計(jì)算器37。矩陣姿態(tài)角計(jì)算器37計(jì)算來(lái)自姿態(tài)矩陣的姿態(tài)角并且將其作為移動(dòng)體的姿態(tài)角輸出到輸出裝置34。減法器32還將減去偏移量的姿態(tài)矩陣反饋到矩陣加法計(jì)算器14。然后矩陣加法計(jì)算器14通過(guò)將反饋的姿態(tài)矩陣加到基于由角速度傳感器10新檢測(cè)到的角速度而計(jì)算出的小角度矩陣，來(lái)執(zhí)行積分計(jì)算，并且從角速度計(jì)算新的角速度系統(tǒng)姿態(tài)矩陣。

輸出裝置34將來(lái)自矩陣姿態(tài)角計(jì)算器37的減去偏移量的姿態(tài)角輸出到外部部件。在機(jī)器人的例子中，主處理器(主機(jī)處理器)輸入由傳感器單元檢測(cè)到的姿態(tài)角等，并且對(duì)機(jī)器人的姿態(tài)執(zhí)行反饋控制。輸出裝置34響應(yīng)來(lái)自主處理器的命令在預(yù)定時(shí)間將姿態(tài)角輸出到主處理器。由角速度傳感器10檢測(cè)到的角速度或由加速度傳感器檢測(cè)到的加速度可以被提供到輸出裝置34，并且除了姿態(tài)角之外，輸出裝置34還可以將角速度或加速度輸出到主處理器。這些中哪個(gè)將被輸出是由來(lái)自主處理器的命令或設(shè)置的參數(shù)確定的。輸出裝置34在給定的時(shí)間將所有角速度、加速度和姿態(tài)角輸出到主處理器，并且在其他時(shí)刻選擇性地輸出這些中的一個(gè)。

如上所述，因此檢測(cè)并且輸出了去除了偏移量的準(zhǔn)確的姿態(tài)角。因?yàn)榛谟杉铀俣葌鞲衅?0檢測(cè)到的每個(gè)軸的方向上的加速度來(lái)提取偏移量，然后從角速度姿態(tài)中去除偏移量并且反饋結(jié)果，角速度傳感器10不需要是很貴、很重和特別精確的傳感器。

接著，下面描述第八示例性實(shí)施例。圖13是本發(fā)明的第八示例性實(shí)施例的框圖。此示例性實(shí)施例與第七示例性實(shí)施例的不同在于，微分器30計(jì)算低通濾波器46的輸出和低通濾波器48的輸出之間的差值并且將結(jié)果輸出到乘法器38、積分器40和微分器42。乘法器38、積分器40和微分器42執(zhí)行PID控制。即，乘法器38執(zhí)行將來(lái)自微分器30的偏移量與預(yù)先設(shè)置的系數(shù)相乘的計(jì)算，并將計(jì)算的值(比例項(xiàng))輸出到減法器32。積分器40執(zhí)行將來(lái)自微分器30的偏移量隨著時(shí)間累積的值與預(yù)先設(shè)置的系數(shù)相乘的計(jì)算，并將計(jì)算的值(積分項(xiàng))輸出到減法器32。微分器42執(zhí)行將來(lái)自微分器30的偏移量隨著時(shí)間的微分的值與預(yù)先設(shè)定的系數(shù)相乘的計(jì)算，并且將計(jì)算的值(微分項(xiàng))輸出到減法器32。對(duì)偏移量的矩陣的每個(gè)分量執(zhí)行乘法計(jì)算、積分計(jì)算和微分計(jì)算。系數(shù)可以是固定的或者它們可以如下面所述是可變的。減法器32從由矩陣加法計(jì)算器14輸入的角速度系統(tǒng)姿態(tài)矩陣中減去這些項(xiàng)的和(即，比例項(xiàng)+積分項(xiàng)+微分項(xiàng))，來(lái)計(jì)算減去偏移量的角速度系統(tǒng)姿態(tài)矩陣，并且將其輸出到矩陣姿態(tài)角計(jì)算器37。來(lái)自減法器32的輸出被反饋到矩陣加法計(jì)算器14。乘法器38主要確定反饋度，積分器40累積長(zhǎng)期偏移的量并且確定反饋量，微分器42確定有關(guān)短期變化的反饋量。利用此類型的PID控制短期和長(zhǎng)期特性均得以改善。

此外，在圖13中，繞Z軸的姿態(tài)角，即橫擺角φ，從外部姿態(tài)角44提供到加速度矩陣計(jì)算器24。加速度傳感器20檢測(cè)在每個(gè)軸x、y和z方向上的加速度。然而，當(dāng)使用了G傳感器時(shí)，橫擺角φ，即繞Z軸的姿態(tài)角不確定，所以需要將橫擺角φ固定為恒定值(例如0)。在此示例性實(shí)施例中，橫擺角被單獨(dú)檢測(cè)并且被提供到加速度矩陣計(jì)算器24，這還防止了橫擺角φ的發(fā)散。

接著，描述第九示例性實(shí)施例。圖14為本發(fā)明第九示例性實(shí)施例的框圖。第九示例性實(shí)施例與第八示例性實(shí)施例的不同在于，在低通濾波器46和48之后增加正規(guī)器/正交器54和56。當(dāng)通過(guò)低通濾波器46和48從姿態(tài)矩陣提取低范圍分量時(shí)，姿態(tài)矩陣變形使得正規(guī)化和正交化不再能被保證。因此利用正規(guī)器/正交器54和56通過(guò)正規(guī)化和正交化來(lái)修正該變形。正規(guī)化和正交化的方法如前所述。低通濾波器46和48被應(yīng)用到表示旋轉(zhuǎn)的分量，即4×4姿態(tài)矩陣中的3×3矩陣分量的第一列(a，b，c)、第二列(d，e，f)和第三列(g，h，i)，因此使得容易提取相應(yīng)的低范圍分量。還有，低通濾波器可以應(yīng)用到全部六個(gè)分量，即從3×3矩陣分量中，在一側(cè)上的三個(gè)相對(duì)的角分量，以及在另一側(cè)的三個(gè)非對(duì)角線分量，并且可以產(chǎn)生等價(jià)的矩陣濾波器。

接著，描述第十示例性實(shí)施例。圖15為本發(fā)明第十示例性實(shí)施例的框圖。第十實(shí)施例與第八實(shí)施例的不同在于，設(shè)置了運(yùn)動(dòng)頻率傳感器52，其輸入來(lái)自角速度傳感器10的角速度和來(lái)自加速度傳感器20的加速度，并且檢測(cè)在機(jī)器人，即移動(dòng)物體的運(yùn)動(dòng)中的特定頻率。運(yùn)動(dòng)頻率傳感器52從角速度和加速度中提取經(jīng)常出現(xiàn)的頻率分量，并且將其輸出到低通濾波器46和48。一個(gè)例子為當(dāng)機(jī)器人，即移動(dòng)物體用兩條腿行走時(shí)的兩足移動(dòng)中的特定頻率。低通濾波器46和48根據(jù)從運(yùn)動(dòng)頻率傳感器52輸入的頻率來(lái)適應(yīng)性地調(diào)整截止頻率。運(yùn)動(dòng)頻率越大，低通濾波器46和48越要增大截止頻率。此外，運(yùn)動(dòng)頻率傳感器52將運(yùn)動(dòng)頻率輸出到執(zhí)行PID控制的乘法器38、積分器40和微分器42。乘法器38、積分器40和微分器42根據(jù)輸入的運(yùn)動(dòng)頻率調(diào)整乘法器系數(shù)。根據(jù)運(yùn)動(dòng)中的特定頻率來(lái)適應(yīng)性地調(diào)整截止頻率和PID控制的系數(shù)，以此方式提高響應(yīng)度，以及跟隨和適應(yīng)的能力。

接著，描述第十一示例性實(shí)施例。圖16是本發(fā)明的第十一示例性實(shí)施例的框圖。前述示例性實(shí)施例的每個(gè)都利用了姿態(tài)矩陣。然而，此示例性實(shí)施例利用四元數(shù)代替姿態(tài)矩陣。小角度四元數(shù)計(jì)算器13利用從角速度傳感器10獲得的小角度來(lái)產(chǎn)生小角度四元數(shù)。利用此小角度四元數(shù)，小旋轉(zhuǎn)被應(yīng)用到反饋回來(lái)的四元數(shù)，而且通過(guò)四元數(shù)積分計(jì)算器15獲得此旋轉(zhuǎn)后的四元數(shù)。然后此積分的四元數(shù)被輸出到減法器32。輸出到減法器32的相同的輸出還被輸入到四元數(shù)低通濾波器63。此四元數(shù)低通濾波器63提取姿態(tài)的低范圍分量。在傾斜角計(jì)算器22獲得此傾斜角后，加速度四元數(shù)計(jì)算器25利用來(lái)自加速度傳感器20的輸出來(lái)獲得加速度四元數(shù)。然后此加速度四元數(shù)被輸入到四元數(shù)低通濾波器67。四元數(shù)低通濾波器67提取姿態(tài)的低范圍分量。微分器30計(jì)算角速度系統(tǒng)四元數(shù)的低范圍分量和加速度系統(tǒng)四元數(shù)的低范圍分量之間的差值，并且提取包括在角速度四元數(shù)中的偏移量。然后微分器30將該偏移量輸出到減法器32。

減法器32計(jì)算從四元數(shù)積分計(jì)算器15輸入的角速度系統(tǒng)四元數(shù)和來(lái)自從微分器30的偏移量之間的差值，并且從角速度系統(tǒng)四元數(shù)中去除偏移量。然后減法器32將去除了偏移量的角速度系統(tǒng)四元數(shù)輸出到四元數(shù)積分計(jì)算器15和四元數(shù)姿態(tài)角計(jì)算器70。

因?yàn)榉答伒乃脑獢?shù)為去除了偏移量的四元數(shù)，所以可以防止積分誤差的累積。四元數(shù)姿態(tài)角計(jì)算器70從輸入的四元數(shù)來(lái)計(jì)算姿態(tài)角，并且將結(jié)果輸出到輸出裝置34。

以下，將描述計(jì)算四元數(shù)的方法和從所述四元數(shù)計(jì)算姿態(tài)角的方法。

首先，描述四元數(shù)。四元數(shù)是具有二維自由度的多個(gè)素?cái)?shù)被擴(kuò)展為具有四維自由度。四元數(shù)典型地表示為 q＝a+bi+cj+dk 這里，a、b、c和d為實(shí)數(shù)，i、j和k為基(大?。?)?；某朔e具有如下關(guān)系。
i×l＝l×i＝i j×l＝l×j＝j(luò) k×l＝l×k＝k i×j＝-j×i＝k j×k＝-k×j＝i k×i＝-i×k＝j(luò)
此外，關(guān)于二個(gè)四元數(shù)q1＝a1+b1i+c1j+d1k和q2＝a2+b2i+c2j+d2k，和與差為 q1+q2＝a1+a2+(b1+b2)i+(c1+c2)j+(d1+d2)k q1-q2＝a1-a2+(b1-b2)i+(c1-c2)j+(d1-d2)k 乘積為 q1q2＝(a1+b1i+c1j+d1k)×(a2+b2i+c2j+d2k)＝(a1a2-b1b2-c1c2-d1d2)+(a1b2+b1a2+c1d2-d1c2)i+(a1c2-b1d2+c1a2+d1b2)j+(a1d2+b1c2-c1b2+d1a2)k
利用虛數(shù)單元(unit)i、j和k來(lái)如下表示四元數(shù)。
P＝(a；xi，yj，zk)
利用四元數(shù)Q，當(dāng)在參考坐標(biāo)XYZ中看時(shí)傳感器坐標(biāo)系xyz的關(guān)系，即，姿態(tài)為 Q＝(cosη/2；αsinη/2，βsinη/2，γsinη/2) 這示出了一坐標(biāo)系(傳感器坐標(biāo)系)，其中參考坐標(biāo)系圍繞由參考坐標(biāo)系表示的旋轉(zhuǎn)軸(α，β，γ)旋轉(zhuǎn)角度η，并且表示了傳感器坐標(biāo)系的姿態(tài)。

下面，描述角速度傳感器輸出的四元數(shù)的表示。根據(jù)小旋轉(zhuǎn)角Δψ、Δθ和Δφ，角速度傳感器輸出ωx、ωy和ωz，以及采樣周期ts，旋轉(zhuǎn)角和傳感器輸出之間的關(guān)系如下。
Δψ＝ωx×ts Δθ＝ωy×ts Δφ＝ωz×ts 當(dāng)用四元數(shù)表示此時(shí)的旋轉(zhuǎn)變換時(shí)，我們得到 qx＝(cosΔψ/2；sinΔψ/2i，0j，0k) qy＝(cosΔθ/2；0i，sinΔθ/2j，0k) qz＝(cosΔφ/2；0i，0j，sinΔφ/2k) 繞x軸、y軸和z軸的旋轉(zhuǎn)變換一起被表示。當(dāng)旋轉(zhuǎn)角α足夠小時(shí)，我們得到 qxqyqz＝(1+ΔψΔθΔφ/8；(Δψ/2-ΔθΔφ/4)i，(Δθ/2+ΔψΔφ/4)j，(Δφ/2-ΔψΔθ/4)k)
下面，描述來(lái)自四元數(shù)的旋轉(zhuǎn)變換的積分。當(dāng)旋轉(zhuǎn)變換q1為 q1＝(cosη1/2；αsinη1/2，βsinη1/2，γsinη1/2)時(shí)，則旋轉(zhuǎn)變換q2為 q2＝(cosη2/2；αsinη2/2，βsinη2/2，γsinη2/2)。
首先，旋轉(zhuǎn)變換q1執(zhí)行于參考坐標(biāo)系，然后旋轉(zhuǎn)變換q2執(zhí)行于該旋轉(zhuǎn)之后的坐標(biāo)系。來(lái)自兩個(gè)旋轉(zhuǎn)變換的變換q3為 q3＝q1×q2 ＝(cosη1/2；αsinη1/2，βsinη1/2，γsinη1/2)×(cosη2/2；αsinη2/2，βsinη2/2，γsinη2/2)
下面，描述四元數(shù)的正規(guī)化。用表示旋轉(zhuǎn)的四元數(shù)，在計(jì)算后，執(zhí)行正規(guī)化來(lái)使得變換q的大小(范數(shù))等于1。當(dāng)表示旋轉(zhuǎn)的四元數(shù)用q來(lái)表示時(shí)，獲得q的大小然后通過(guò)將q除以其大小來(lái)執(zhí)行正規(guī)化，因而獲得q’。
q＝(ε0；ε1，ε2，ε3) |q|＝(ε02+ε12+ε22+ε32)0.5 q’＝q/|q| 四元數(shù)是保持正交條件的表示方法，所以不需要進(jìn)一步的正交化。

下面，描述從四元數(shù)到姿態(tài)角的變換。當(dāng) a＝ε02+ε12-ε22-ε32 b＝2(ε1ε2+ε0ε3) c＝2(ε1ε3-ε0ε2) d＝2(ε1ε2-ε0ε3) e＝ε02-ε12+ε22-ε32 f＝2(ε2ε3+ε0ε1) g＝2(ε1ε3+ε0ε2) h＝2(ε2ε3-ε0ε1) i＝ε02-ε12-ε22+ε32 在用四元數(shù)表示的變換q＝(ε0；ε1，ε2，ε3)中，通過(guò)如下表達(dá)式獲得姿態(tài)角。
橫擺角φ＝atan2(b，a) 俯仰角θ＝atan2(-c，cosφ·a+sinφ·b) 側(cè)傾角ψ＝atan2(sinφ·g-cosφ·h，-sinφ·d+cosφ·e)
下面，描述從姿態(tài)角(側(cè)傾角、俯仰角、橫擺角)到四元數(shù)的變換。從姿態(tài)角(ψ，θ，φ)的旋轉(zhuǎn)變換用四元數(shù)表示。
繞x軸的旋轉(zhuǎn)為 R(ψ；i)＝cosψ/2+isinψ/2 繞y軸的旋轉(zhuǎn)為 R(θ；j)＝cosθ/2+jsinθ/2 繞z軸的旋轉(zhuǎn)為 R(φ；k)＝cosφ/2+ksinφ/2 當(dāng)把它們乘到一起，我們得到 S＝R(φ；k)×R(θ；j)×R(ψ；i) ＝(cosφ/2+ksinφ/2)×cosθ/2+jsinθ/2)×(cosψ/2+isinψ/2)
如果用四元數(shù)表示的變換q在此時(shí)表示為q＝(ε0；ε1，ε2，ε3)，則旋轉(zhuǎn)角成為S的計(jì)算結(jié)果的實(shí)部，而旋轉(zhuǎn)軸成為S的虛部。
ε0＝cosφ/2·cosθ/2·cosψ/2+sinφ/2×sinθ/2·sinψ/2 ε1＝-sinφ/2·sinθ/2·cosψ/2+cosφ/2·cosθ/2·sinψ/2 ε2＝sinφ/2·cosθ/2·sinψ/2+cosφ/2·sinθ/2·cosψ/2 ε3＝sinφ/2·cosθ/2·cosψ/2-cosφ/2·sinθ/2·sinψ/2
此時(shí)，從cosα/2＝ε0獲得旋轉(zhuǎn)角α。此外，旋轉(zhuǎn)角為(ε1，ε2，ε3)。

接著，下面描述本發(fā)明的第十二示例性實(shí)施例。圖17為本發(fā)明的第十二示例性實(shí)施例的框圖。此示例性實(shí)施例與第十一示例性實(shí)施例的不同在于，設(shè)置了乘法器38、積分器40和微分器42用于執(zhí)行PID控制，并且由外部姿態(tài)角44檢測(cè)到的橫擺角被提供到加速度四元數(shù)計(jì)算器25。所述結(jié)構(gòu)與圖13中所示的相對(duì)應(yīng)。乘法器38執(zhí)行將來(lái)自微分器30的偏移量(四元數(shù))與預(yù)先設(shè)置的系數(shù)相乘的計(jì)算，并將計(jì)算的值(比例項(xiàng))輸出到減法器32。積分器40執(zhí)行將來(lái)自微分器30的偏移量(四元數(shù))隨著時(shí)間積累的值與預(yù)先設(shè)置的系數(shù)相乘的計(jì)算，并將計(jì)算的值(積分項(xiàng))輸出到減法器32。微分器42執(zhí)行將來(lái)自微分器30的偏移量(四元數(shù))隨著時(shí)間微分的值與預(yù)先設(shè)定的系數(shù)相乘的計(jì)算，并且將計(jì)算的值(微分項(xiàng))輸出到減法器32。系數(shù)可以是固定的或者它們可以是可變的。減法器32計(jì)算輸入的角速度系統(tǒng)四元數(shù)與這些項(xiàng)的和(即，比例項(xiàng)+積分項(xiàng)+微分項(xiàng))之間的差值，計(jì)算去除了偏移量的角速度系統(tǒng)四元數(shù)，并且將結(jié)果輸出到四元數(shù)姿態(tài)角計(jì)算器70。

接著，描述第十三示例性實(shí)施例。圖18為本發(fā)明的第十三示例性實(shí)施例的框圖。此示例性實(shí)施例與第十二示例性實(shí)施例的不同在于，在低通濾波器63和67之后設(shè)置正規(guī)器72和74。此結(jié)構(gòu)與圖14中所示的相對(duì)應(yīng)。角速度系統(tǒng)四元數(shù)和加速度系統(tǒng)四元數(shù)分別用低通濾波器63和67處理。然而，此處理引起變形以致它們可能變得反正規(guī)化(denormalized)。因此姿態(tài)角的檢測(cè)精確性可以通過(guò)利用正規(guī)器72和74修正該變形來(lái)維持。

接著，描述第十四示例性實(shí)施例。圖19為本發(fā)明的第十四示例性實(shí)施例的框圖。此示例性實(shí)施例與第十二示例性實(shí)施例的不同在于，設(shè)置了運(yùn)動(dòng)頻率傳感器52，其輸入來(lái)自角速度傳感器10的角速度和來(lái)自加速度傳感器20的加速度，并且檢測(cè)在機(jī)器人，即移動(dòng)物體的運(yùn)動(dòng)中的特定頻率。此結(jié)構(gòu)與圖15中所示的相對(duì)應(yīng)。運(yùn)動(dòng)頻率傳感器52從角速度和加速度中提取經(jīng)常出現(xiàn)的頻率分量，并且將它們輸出到低通濾波器63和67。低通濾波器63和67根據(jù)從運(yùn)動(dòng)頻率傳感器52輸入的頻率來(lái)適應(yīng)性地調(diào)整截止頻率。運(yùn)動(dòng)頻率越大，低通濾波器63和67越要增大截止頻率。此外，運(yùn)動(dòng)頻率傳感器52將運(yùn)動(dòng)頻率輸出到執(zhí)行PID控制的乘法器38、積分器40和微分器42。乘法器38、積分器40和微分器42根據(jù)輸入的運(yùn)動(dòng)頻率調(diào)整乘法器系數(shù)。根據(jù)運(yùn)動(dòng)中的特定頻率來(lái)適應(yīng)性地調(diào)整截止頻率和PID控制的系數(shù)，以此方式提高響應(yīng)度，以及跟隨和適應(yīng)的能力
接著，描述第十五示例性實(shí)施例。圖20為根據(jù)本發(fā)明的第十五示例性實(shí)施例的移動(dòng)體姿態(tài)角檢測(cè)設(shè)備的框圖。此示例性實(shí)施例與第一示例性實(shí)施例的不同在于，低通濾波器18和28執(zhí)行sin轉(zhuǎn)換，cos轉(zhuǎn)換，以及atan2轉(zhuǎn)換(即，它們均執(zhí)行SCA轉(zhuǎn)換)。

角速度傳感器10檢測(cè)傳感器坐標(biāo)系(即，傳感器單元的坐標(biāo)系)中的角速度。繞軸的角速度被表示為ωx、ωy和ωz。角速度傳感器10將檢測(cè)到的角速度輸出到小角度矩陣計(jì)算器12。

小角度矩陣計(jì)算器12計(jì)算用于計(jì)算當(dāng)在參考坐標(biāo)系XYZ中看時(shí)的傳感器坐標(biāo)系xyz的坐標(biāo)轉(zhuǎn)換矩陣。小角度矩陣在下面描述。小角度矩陣計(jì)算器12將計(jì)算的小角度矩陣輸出到矩陣加法計(jì)算器14。

矩陣加法計(jì)算器14通過(guò)將從小角度矩陣計(jì)算器12輸入的由角速度傳感器10檢測(cè)到的當(dāng)前小角度矩陣加到從輸出側(cè)反饋回來(lái)的姿態(tài)角矩陣來(lái)執(zhí)行積分計(jì)算。然后矩陣加法計(jì)算器14將積分后的矩陣輸出到矩陣姿態(tài)角計(jì)算器16。

矩陣姿態(tài)角計(jì)算器16從輸入的積分后的矩陣計(jì)算姿態(tài)角，并且將此結(jié)果作為角速度姿態(tài)角SYA輸出到低通濾波器LPF18和減法器32。

低通濾波器(即，角速度系統(tǒng)低通濾波器)18從角速度姿態(tài)角SYA提取低范圍分量并且將其輸出到微分器30。角速度姿態(tài)角SYA包括真值的姿態(tài)角(姿態(tài)角真值)和來(lái)自積分誤差的偏移量。低通濾波器18將高范圍姿態(tài)角真值從角速度姿態(tài)角SYA中去除，并且提取低范圍姿態(tài)角真值和偏移量。

同時(shí)，加速度傳感器20檢測(cè)在傳感器坐標(biāo)系xyz中的每個(gè)軸的方向上的加速度。檢測(cè)到的加速度被表示為Gx、Gy和Gz。然后加速度傳感器20將檢測(cè)到的加速度輸出到傾斜角計(jì)算器22。

傾斜角計(jì)算器22計(jì)算參考坐標(biāo)系XYZ的Z軸和傳感器坐標(biāo)系xyz的每個(gè)軸之間的角度。計(jì)算出的傾斜角被表示為λx、λy和λz。然后傾斜角計(jì)算器22將計(jì)算出的傾斜角輸出到加速度矩陣計(jì)算器24。

加速度矩陣計(jì)算器24利用輸入的傾斜角，來(lái)計(jì)算當(dāng)在參考坐標(biāo)系中看時(shí)的傳感器坐標(biāo)系，并且將結(jié)果輸出到矩陣姿態(tài)角計(jì)算器26。

與矩陣姿態(tài)角計(jì)算器16相似，矩陣姿態(tài)角計(jì)算器26然后從輸入的矩陣來(lái)計(jì)算姿態(tài)角，并且將計(jì)算出的姿態(tài)角作為加速度姿態(tài)角SGA輸出到低通濾波器28。

低通濾波器(即，加速度系統(tǒng)低通濾波器)28為截止頻率和衰減因子設(shè)置得與它們?cè)诘屯V波器18中相同的濾波器。低通濾波器28從加速度姿態(tài)角SGA中提取低范圍分量并且將其輸出到微分器30。加速度姿態(tài)角SGA包括在高范圍分量中的高范圍姿態(tài)角偽值和在低范圍分量中的低范圍姿態(tài)角真值。在加速度姿態(tài)角中沒(méi)有發(fā)生來(lái)自積分誤差的偏移。因?yàn)榈屯V波器28中的截止頻率和衰減因子設(shè)置得與它們?cè)诘屯V波器18中相同，角速度系統(tǒng)的低范圍姿態(tài)角真值與加速度系統(tǒng)的低范圍姿態(tài)角真值相同。角速度系統(tǒng)的頻率特性也與加速度系統(tǒng)的頻率特性相同。因此，通過(guò)兩個(gè)低范圍姿態(tài)角真值的相減計(jì)算，只有偏移分量被準(zhǔn)確地輸出。然而，當(dāng)與由角速度系統(tǒng)和加速度系統(tǒng)產(chǎn)生的姿態(tài)角相對(duì)應(yīng)的分量的響應(yīng)預(yù)先已知時(shí)，可以調(diào)整低通濾波器的截止頻率和衰減因子來(lái)匹配它們的特性。當(dāng)適應(yīng)范圍窄但是移動(dòng)特性預(yù)先已知時(shí)此方法有效。

如上所述，來(lái)自低通濾波器18的(低范圍姿態(tài)角真值加上偏移分量)，以及來(lái)自低通濾波器28的(低范圍姿態(tài)角真值)被輸入到微分器30。然后微分器30計(jì)算這兩者之間的差值并且提取偏移量(即，偏移量＝(低范圍姿態(tài)角真值+偏移量)-(低范圍姿態(tài)角真值))。然后微分器30將通過(guò)微分計(jì)算獲得的偏移量輸出到減法器32。

減法器32計(jì)算從矩陣姿態(tài)角計(jì)算器16輸入的角速度姿態(tài)角SYA和從微分器30輸入的偏移量之間的差值，然后去除包括在來(lái)自矩陣姿態(tài)角計(jì)算器16的角速度姿態(tài)角SYA中的偏移量。如上所述，來(lái)自矩陣姿態(tài)角計(jì)算器16的角速度姿態(tài)角SYA包括姿態(tài)角真值(高范圍分量+低范圍分量)和偏移量。然后減法器32通過(guò)減法來(lái)提取低范圍姿態(tài)角真值(即，低范圍姿態(tài)角真值＝(低范圍姿態(tài)角真值+偏移量)-(偏移量))。然后減法器32將通過(guò)減法獲得的姿態(tài)角真值輸出到輸出裝置34。減法器32還將姿態(tài)角真值輸出到姿態(tài)角矩陣計(jì)算器36。

姿態(tài)角矩陣計(jì)算器36從輸入的姿態(tài)角來(lái)計(jì)算姿態(tài)角矩陣，并且將其反饋到矩陣加法計(jì)算器14。然后姿態(tài)角矩陣計(jì)算器36執(zhí)行矩陣姿態(tài)角計(jì)算器16的反運(yùn)算，并且從姿態(tài)角產(chǎn)生姿態(tài)角矩陣。然后矩陣加法計(jì)算器14通過(guò)將反饋的姿態(tài)角矩陣加到基于由角速度傳感器10新檢測(cè)到的角速度而計(jì)算出的小角度矩陣，來(lái)執(zhí)行積分計(jì)算，然后從角速度來(lái)計(jì)算姿態(tài)角。

輸出裝置34將通過(guò)減法器32去除了偏移量的姿態(tài)角輸出到外部部件。在機(jī)器人的例子中，主處理器(主機(jī)處理器)輸入由傳感器單元檢測(cè)到的姿態(tài)角等，并且對(duì)機(jī)器人的姿態(tài)執(zhí)行反饋控制。輸出裝置34響應(yīng)來(lái)自主處理器的命令在預(yù)定時(shí)間將姿態(tài)角輸出到主處理器。由角速度傳感器10檢測(cè)到的角速度或由加速度傳感器檢測(cè)到的加速度可以被提供到輸出裝置34，并且除了姿態(tài)角，輸出裝置34還可以將角速度或加速度輸出到主處理器。這些中的哪個(gè)將被輸出是由來(lái)自主處理器的命令或設(shè)置的參數(shù)來(lái)確定的。輸出裝置34在給定的時(shí)刻將所有角速度、加速度和姿態(tài)角輸出到主處理器，并且在其他時(shí)刻選擇性地輸出它們其中一個(gè)。

如上所述，因此檢測(cè)并且輸出了去除了偏移量的準(zhǔn)確的姿態(tài)角。因?yàn)榛谟杉铀俣葌鞲衅?0檢測(cè)到的每個(gè)軸的方向上的加速度來(lái)提取偏移量，然后從角速度姿態(tài)角中去除偏移量并且反饋結(jié)果，角速度傳感器10不需要是很貴、很重和特別精確的傳感器。

同時(shí)，矩陣姿態(tài)角計(jì)算器16和26從姿態(tài)矩陣計(jì)算姿態(tài)角，它們利用函數(shù)atan2執(zhí)行計(jì)算。結(jié)果，盡管實(shí)事上初始的姿態(tài)角連續(xù)地變化，但是姿態(tài)角在計(jì)算中可能不連續(xù)地變化。這是姿態(tài)角表達(dá)中的不連續(xù)性。因此，來(lái)自矩陣姿態(tài)角計(jì)算器16和26的姿態(tài)角使用為，如同它們不具有由低通濾波器提取的低范圍分量。而是，低范圍分量是在轉(zhuǎn)換到連續(xù)姿態(tài)角之后提取的。

圖21為低通濾波器18的框圖。低通濾波器18具有對(duì)姿態(tài)角進(jìn)行sin轉(zhuǎn)換和cos轉(zhuǎn)換的轉(zhuǎn)換器，以及atan2計(jì)算器或atan計(jì)算器。低通濾波器28具有與低通濾波器18相同的結(jié)構(gòu)，所以省略其描述。

sin-cos轉(zhuǎn)換器18a、18b和18c對(duì)三個(gè)軸的每個(gè)的姿態(tài)角執(zhí)行sin和cos轉(zhuǎn)換并且輸出結(jié)果。例如，如果角速度姿態(tài)角為S1ψ、S1θ和S1φ，sin-cos轉(zhuǎn)換器18a對(duì)S1ψ計(jì)算sin(S1ψ)和cos(S1ψ)并且輸出結(jié)果。然后sin和cos轉(zhuǎn)換后的值被分別輸出到低通濾波器18d和18e。然后低通濾波器18d和18e從sin和cos轉(zhuǎn)換后的值提取低范圍分量并且將結(jié)果輸出到atan2計(jì)算器18j。atan2計(jì)算器18j利用sin和cos轉(zhuǎn)換后的值計(jì)算atan2并且將結(jié)果作為姿態(tài)角S2ψ輸出。此外，sin-cos轉(zhuǎn)換器18b對(duì)S1θ計(jì)算sin(S1θ)和cos(S1θ)并且輸出結(jié)果。然后sin和cos轉(zhuǎn)換后的值被分別輸出到低通濾波器18f和18g。然后低通濾波器18f和18g從sin和cos轉(zhuǎn)換后的值提取低范圍分量并且將結(jié)果輸出到atan計(jì)算器18k。然后atan計(jì)算器18k利用sin和cos轉(zhuǎn)換后的值計(jì)算atan，并且將結(jié)果作為姿態(tài)角S2θ輸出。此外，sin-cos轉(zhuǎn)換器18c對(duì)S1φ計(jì)算sin(S1φ)和cos(S1φ)并且輸出結(jié)果。sin和cos轉(zhuǎn)換后的值被分別輸出到低通濾波器18h和18i。然后低通濾波器18h和18i從sin和cos轉(zhuǎn)換后的值提取低范圍分量并且將結(jié)果輸出到atan2計(jì)算器18m。atan2計(jì)算器18m利用sin和cos轉(zhuǎn)換后的值計(jì)算atan2，并且將結(jié)果作為姿態(tài)角S2φ輸出。

以此方式結(jié)合的sin-cos轉(zhuǎn)換以及atan2計(jì)算或atan計(jì)算，可以消除由于姿態(tài)角的不連續(xù)造成的低通濾波器的不良操作，從而穩(wěn)定地提取低范圍分量。在sin和cos轉(zhuǎn)換之后的值在±1的范圍內(nèi)連續(xù)并且平滑地變化，所以在濾波器處理中沒(méi)有產(chǎn)生離散(dispersion)或非正常值。通過(guò)利用在濾波器處理后使用sin和cos轉(zhuǎn)換后的值來(lái)執(zhí)行atan2計(jì)算過(guò)程，值被再轉(zhuǎn)換到姿態(tài)角分量，使得姿態(tài)角分量的濾波器處理可以被實(shí)現(xiàn)而值并未超過(guò)姿態(tài)角表達(dá)范圍。

接著，描述第十六示例性實(shí)施例。圖22為本發(fā)明的第十六示例性實(shí)施例的框圖。此示例性實(shí)施例與第十五示例性實(shí)施例的不同在于，微分器30計(jì)算低通濾波器18的輸出和低通濾波器28的輸出之間的差值，并且將結(jié)果輸出到乘法器38、積分器40和微分器42。乘法器38、積分器40和微分器42執(zhí)行PID控制。即，乘法器38執(zhí)行將來(lái)自微分器30的偏移量與預(yù)先設(shè)置的系數(shù)相乘，并且將計(jì)算的值(比例項(xiàng))輸出到減法器32。積分器40執(zhí)行將來(lái)自微分器30的偏移量隨著時(shí)間累積的值與預(yù)先設(shè)置的系數(shù)相乘的計(jì)算，并將計(jì)算的值(積分項(xiàng))輸出到減法器32。微分器42執(zhí)行將來(lái)自微分器30的偏移量隨著時(shí)間微分的值與預(yù)先設(shè)定的系數(shù)相乘的計(jì)算，并且將計(jì)算的值(微分項(xiàng))輸出到減法器32。系數(shù)可以是固定的或者它們可以如下所述是可變的。減法器32從由矩陣姿態(tài)角計(jì)算器16輸入的角速度姿態(tài)角中減去這些項(xiàng)的和(即，比例項(xiàng)+積分項(xiàng)+微分項(xiàng))，并且將結(jié)果作為減去偏移量的移動(dòng)體的姿態(tài)角輸出到輸出裝置34和姿態(tài)角矩陣計(jì)算器36。來(lái)自減法器32的輸出被姿態(tài)角矩陣計(jì)算器36轉(zhuǎn)換為姿態(tài)矩陣，并且反饋結(jié)果。乘法器38主要確定反饋度，積分器40累積長(zhǎng)期的偏移量并且確定反饋量，微分器42確定關(guān)于短期變化的反饋量。利用此類PID控制短期和長(zhǎng)期特性均得以改善。

接著，描述第十七示例性實(shí)施例。圖23為本發(fā)明的第十七示例性實(shí)施例的框圖。此示例性實(shí)施例與第十六示例性實(shí)施例的不同在于，來(lái)自外部姿態(tài)角44的繞Z軸的姿態(tài)角，即橫擺角，被提供到矩陣姿態(tài)角計(jì)算器26。加速度傳感器20檢測(cè)在每個(gè)軸x、y和z方向上的加速度。然而，當(dāng)使用了G傳感器時(shí)，橫擺角φ，即繞Z軸的姿態(tài)角不確定，所以需要將橫擺角φ固定為恒定值。在此示例性實(shí)施例中，橫擺角被單獨(dú)檢測(cè)并且被提供到矩陣姿態(tài)角計(jì)算器26，因此還使橫擺角φ能被準(zhǔn)確計(jì)算。更具體地，矩陣姿態(tài)角計(jì)算器26根據(jù)如上所述的計(jì)算，來(lái)計(jì)算橫擺角φ、側(cè)傾角θ和俯仰角ψ，作為來(lái)自姿態(tài)矩陣T(n)的姿態(tài)角。然而，這些中的橫擺角φ用從外部部件輸入的橫擺角φ代替，這防止了橫擺角φ的發(fā)散。

此外，在此示例性實(shí)施例中，設(shè)置了微分器31代替微分器30。微分器31包括sin-cos轉(zhuǎn)換器和atan2計(jì)算器或atan計(jì)算器，就像低通濾波器18和28。

圖24為微分器31的框圖。來(lái)自低通濾波器18的角速度姿態(tài)角和來(lái)自低通濾波器28的加速度姿態(tài)角均被提供到微分器31。三個(gè)軸的角速度姿態(tài)角被分別表示為ξn0ψ、ξn0θ和ξn0φ，并且三個(gè)軸的加速度姿態(tài)角被分別表示為ξm0ψ、ξm0θ和ξm0φ。

sin-cos轉(zhuǎn)換器31a對(duì)ξn0ψ執(zhí)行sin和cos轉(zhuǎn)換，并且將相應(yīng)的結(jié)果輸出到atan2計(jì)算器31g。atan2計(jì)算器31g利用sin和cos轉(zhuǎn)換后的值執(zhí)行atan2計(jì)算，并且將結(jié)果輸出到微分器31n。同時(shí)，sin-cos轉(zhuǎn)換器31b對(duì)ξm0ψ執(zhí)行sin和cos轉(zhuǎn)換，并且將相應(yīng)的值輸出到atan2計(jì)算器31h。atan2計(jì)算器31h利用sin和cos轉(zhuǎn)換后的值執(zhí)行atan2計(jì)算，并且將結(jié)果輸出到微分器31n。微分器31n計(jì)算這兩者之間的差值并且提取偏移量，然后將偏移量輸出到sin-cos轉(zhuǎn)換器31r。sin-cos轉(zhuǎn)換器31r和atan2計(jì)算器31u執(zhí)行相同的計(jì)算并且輸出ξd1ψ作為偏移量。sin-cos轉(zhuǎn)換器31c對(duì)ξn0θ執(zhí)行sin和cos轉(zhuǎn)換，并且將相應(yīng)的值輸出到atan計(jì)算器31i。atan計(jì)算器31i利用sin和cos轉(zhuǎn)換后的值執(zhí)行atan計(jì)算，并且將結(jié)果輸出到微分器31p。同時(shí)，sin-cos轉(zhuǎn)換器31d對(duì)ξm0θ執(zhí)行sin和cos轉(zhuǎn)換，并且將相應(yīng)的值輸出到atan計(jì)算器31j。atan計(jì)算器31j利用sin和cos轉(zhuǎn)換后的值執(zhí)行atan計(jì)算，并且將結(jié)果輸出到微分器31p。微分器31p計(jì)算這兩者之間的差值并且提取偏移量，然后將偏移量輸出到sin-cos轉(zhuǎn)換器31s。sin-cos轉(zhuǎn)換器31s和atan計(jì)算器31v執(zhí)行相同的運(yùn)算并且輸出ξd1θ作為偏移量。sin-cos轉(zhuǎn)換器31e對(duì)ξn0φ?qǐng)?zhí)行sin和cos轉(zhuǎn)換，并且將相應(yīng)的值輸出到atan2計(jì)算器31k。atan2計(jì)算器31k利用sin和cos轉(zhuǎn)換后的值執(zhí)行atan2計(jì)算，并且將結(jié)果輸出到微分器31q。同時(shí)，sin-cos轉(zhuǎn)換器31f對(duì)ξm0φ?qǐng)?zhí)行sin和cos轉(zhuǎn)換，并且將相應(yīng)的值輸出到atan2計(jì)算器31m。atan2計(jì)算器31m利用sin和cos轉(zhuǎn)換后的值執(zhí)行atan2計(jì)算，并且將結(jié)果輸出到微分器31q。微分器31q計(jì)算這兩者之間的差值并且提取偏移量。然后微分器31q將偏移量輸出到sin-cos轉(zhuǎn)換器31t。sin-cos轉(zhuǎn)換器31t和atan2計(jì)算器31w執(zhí)行相同的運(yùn)算并且輸出ξd1φ作為偏移量。

以此方式，可以防止發(fā)生由于角速度姿態(tài)角和加速度姿態(tài)角中的不連續(xù)造成的非正常計(jì)算。還防止發(fā)生由于當(dāng)計(jì)算姿態(tài)角差值時(shí)姿態(tài)角的不連續(xù)造成的非正常計(jì)算。此外，還消除了偏移量的姿態(tài)角中的不連續(xù)，所以可以防止PID控制的不良操作。

圖25為根據(jù)此示例性實(shí)施例的流程圖。首先，對(duì)角速度姿態(tài)角ξn1和加速度姿態(tài)角ξm1執(zhí)行sin和cos轉(zhuǎn)換，并且執(zhí)行atan2或atan計(jì)算(S101)。然后，計(jì)算在所述計(jì)算之后的值的差值(S102)。通過(guò)該計(jì)算獲得的值，即ξd0＝ξm1-ξn1，是偏移量。然后執(zhí)行sin-cos轉(zhuǎn)換和atan2或atan計(jì)算以獲得ξd1(S103)。在計(jì)算出偏移量ξd1之后，對(duì)該偏移量執(zhí)行PID控制。即，計(jì)算比例項(xiàng)、積分項(xiàng)和微分項(xiàng) ξd1p＝P×ξd1 ξd1i＝I×ξd1 ξd1d＝D×ξd1 然后將它們相加到一起來(lái)獲得ξs0(即，ξs0＝ξd1p+ξd1i+ξd1d)(S104和S105)。

然后，可以計(jì)算角速度姿態(tài)角ξn1和ξs0之間的差值。然而，用在ξm＝-π和ξp＝+π之間的值ψ和φ，以及在ξm＝-π/2和ξp＝+π/2之間的值θ來(lái)限定姿態(tài)角，所以如果ξs0超過(guò)-π和+π之間的值，用ψ和φ控制變得不穩(wěn)定。因此，如果ξs0超過(guò)此范圍，需要用該范圍內(nèi)的最近的值來(lái)代替。因此，首先確定ξs0是否小于ξm＝-π(S106)。如果ξs0小于ξm＝-π，那么用ξm＝-π來(lái)代替ξs0并且形成ξs1(S108)。同樣，如果ξs0大于ξp＝π(S107)，那么用ξp＝π來(lái)代替ξs0并且形成ξs1(S109)。當(dāng)ξs0在ξm＝-π和ξp＝π之間時(shí)沒(méi)有問(wèn)題出現(xiàn)，所以該值形成ξs1并且計(jì)算其與ξn1之間的差值(S111)。另一方面，如果ξs0的值超過(guò)ξm＝-π/2和ξp＝π/2之間的值，則用θ控制變得不穩(wěn)定。因此，如果ξs0超過(guò)此范圍，需要用在該范圍內(nèi)的最接近的值來(lái)代替。因此，首先確定ξs0是否小于ξm＝-π/2(S106)。如果ξs0小于ξm＝-π/2，那么用ξm＝-π/2+δ來(lái)代替ξs0并且形成ξs1(S108)。同樣，如果ξs0大于ξp＝π/2(S107)，那么用ξp＝π/2-δ來(lái)代替ξs0并且形成ξs1(S109)。這里，δ為實(shí)數(shù)并且表示計(jì)算用的最小有效數(shù)。當(dāng)ξs0在ξm＝-π/2和ξp＝π/2之間時(shí)沒(méi)有問(wèn)題出現(xiàn)，所以該值形成ξs1并且計(jì)算其與ξn1之間的差值(S111)。然而，當(dāng)ξs0變成-π/2時(shí)，使其成為-π/2+δ，并且當(dāng)ξs0變成π/2時(shí)，使其成為π/2-δ。上述流程使得能夠輸出穩(wěn)定的姿態(tài)角。

以此方式，根據(jù)此示例性實(shí)施例，通過(guò)執(zhí)行sin、cos和atan2轉(zhuǎn)換(即，SCA轉(zhuǎn)換)，參數(shù)中的不連續(xù)得到消除。姿態(tài)角的不連續(xù)的補(bǔ)充說(shuō)明以及微分器31的操作如下。

下面，說(shuō)明姿態(tài)角的不連續(xù)性。姿態(tài)角用繞X軸的ψ、繞Y軸的θ和繞Z軸的φ通過(guò)(ψ，θ，φ)來(lái)表示。各自的范圍為 -π＜ψ≤π(弧度)或-180＜ψ≤180(度) -π/2＜θ＜π/2(弧度)或-90＜θ＜90(度) -π＜φ≤π(弧度)或-180＜φ≤180(度)
圖26示出了姿態(tài)角φ和當(dāng)有繞Z軸的旋轉(zhuǎn)時(shí)的旋轉(zhuǎn)角之間的關(guān)系。當(dāng)Z軸從-720旋轉(zhuǎn)變化到720度時(shí)，φ在±180度的范圍中振蕩。當(dāng)φ達(dá)到180度時(shí)，其突然翻轉(zhuǎn)并且開(kāi)始接近-180度。這是產(chǎn)生不連續(xù)的機(jī)理。盡管移動(dòng)體隨著時(shí)間連續(xù)地旋轉(zhuǎn)(如圖中通過(guò)Z軸旋轉(zhuǎn)所示)，但是姿態(tài)角φ是不連續(xù)的。

圖27更具體地示出了當(dāng)±20度的sin波類型連續(xù)旋轉(zhuǎn)振蕩在φ＝180度附近應(yīng)用。在圖中，Z軸旋轉(zhuǎn)以在180度的±20度的sin波類型振蕩連續(xù)地變化，但是φ在±180度之間非連續(xù)地變化。

圖28和29同時(shí)示出了來(lái)自作為SCA處理的本發(fā)明的結(jié)果。根據(jù)所示結(jié)果，來(lái)自SCA處理的波形與圖27中所示的φ相似。從圖中明顯看出，解決了波形變形或與在現(xiàn)有技術(shù)中的波形Mφ完全不相似的問(wèn)題。這對(duì)于ψ也一樣。結(jié)果是，可以準(zhǔn)確地檢測(cè)到在大操作范圍上的姿態(tài)角。

圖30示出了θ和當(dāng)有繞Y軸的旋轉(zhuǎn)時(shí)的旋轉(zhuǎn)角之間的關(guān)系。當(dāng)Y軸從-720旋轉(zhuǎn)變化到720度時(shí)，θ在±90度的范圍中振蕩。當(dāng)θ達(dá)到90度時(shí)，其突然翻轉(zhuǎn)并且開(kāi)始接近-90度。這是產(chǎn)生不連續(xù)的機(jī)理。盡管移動(dòng)體隨著時(shí)間連續(xù)地旋轉(zhuǎn)，但是姿態(tài)角θ是不連續(xù)的。

圖31具體地示出了當(dāng)±20度的sin波類型連續(xù)旋轉(zhuǎn)振蕩在θ＝90度附近應(yīng)用。Y軸旋轉(zhuǎn)連續(xù)變化，但是θ在±90度之間非連續(xù)地變化。

圖32和33同時(shí)示出了使用SCA處理的本發(fā)明的結(jié)果。根據(jù)所示結(jié)果，來(lái)自SCA處理的波形與圖31中所示的θ相似。從圖中明顯看出，解決了波形變形或與在現(xiàn)有技術(shù)中的波形Mθ完全不相似的問(wèn)題。結(jié)果是，可以準(zhǔn)確地檢測(cè)到在大操作范圍上的姿態(tài)角。

下面，說(shuō)明SCA微分器。微分器31計(jì)算關(guān)于姿態(tài)角(ψ，θ，φ)的每個(gè)分量的差值。例如，關(guān)于φ，我們說(shuō)在圖26中角速度姿態(tài)角ξnoφ為175度，加速度姿態(tài)角ξmo為-25度。當(dāng)執(zhí)行差值計(jì)算時(shí)，ξno-ξmo＝200度。因?yàn)檫@超過(guò)了φ的±180度的限定范圍，出現(xiàn)不合理性。以此方式，計(jì)算操作導(dǎo)致φ趨向于超過(guò)限定的范圍。因此，微分器31執(zhí)行操作來(lái)將φ帶入限定范圍內(nèi)。此操作為sin、cos和atan2轉(zhuǎn)換(即，SCA轉(zhuǎn)換)。更具體地，獲得ξnoφ-ξmoφ的sin值和cos值。然后利用sin值和cos值通過(guò)atan2計(jì)算獲得ξd1φ。ξd1φ在此時(shí)落入限定的范圍內(nèi)。根據(jù)條件，還可以利用使得200-360＝-160度的方法。然而，很多其大大超過(guò)±180度，例如±360度、±540度、±720度等的情況是可能的。根據(jù)條件，有必要描述極大的，本質(zhì)上為無(wú)窮數(shù)的組合。同樣，根據(jù)條件，數(shù)字值可能在計(jì)算中劇烈地變化，因此導(dǎo)致不連續(xù)，這將使得控制極其困難。實(shí)際上，對(duì)象為疊加了噪聲的信號(hào)。因此，當(dāng)噪聲在±180度附近疊加時(shí)，產(chǎn)生了極大的、不連續(xù)的振蕩。結(jié)果是，由于其與計(jì)算準(zhǔn)確性的關(guān)系，造成其在計(jì)算中容易變得不穩(wěn)定，使得不能再獲得高準(zhǔn)確性和穩(wěn)定的輸出。SCA轉(zhuǎn)換過(guò)程是用于數(shù)學(xué)地平滑地執(zhí)行此的方法。其是使超過(guò)限定范圍的數(shù)據(jù)通過(guò)計(jì)算處理適合于限定的范圍的過(guò)程。并且因此類似正規(guī)化。

ξnoφ其本身也可能在處理的計(jì)算中的一個(gè)超過(guò)限定范圍。因此，通過(guò)用sin和cos表示ξnoφ以及計(jì)算atan2，首先使得ξnoφ適合在±180度范圍內(nèi)。相似地，ξmoφ也正規(guī)化到限定范圍內(nèi)。然后，微分器31執(zhí)行計(jì)算ξnoφ-ξmoφ。根據(jù)狀態(tài)，此差值的輸出可以超過(guò)φ的限定范圍。因此，通過(guò)sin-cos-atan2轉(zhuǎn)換使輸出回到限定范圍內(nèi)來(lái)執(zhí)行正規(guī)化。在姿態(tài)角ψ、θ和φ中，ψ和φ的限定區(qū)域?yàn)?π到π，其與atan2處理的輸出范圍相同，所以執(zhí)行atan2計(jì)算。同時(shí)，θ的限定區(qū)域?yàn)?π/2到π/2，其是atan2處理的輸出范圍的一半。因此，atan計(jì)算使用θ。atan的輸出范圍在-π/2到π/2與θ匹配。atan(y，x)＝tan-1(x，y)是使得在小的x區(qū)域，分母tan(y/x)小以便誤差變大。因此，當(dāng)與y相比x小時(shí)，使用cot-1(y，x)。

盡管迄今描述了本發(fā)明示例性實(shí)施例，本發(fā)明不限于這些示例性實(shí)施例。相反，各種修改是可能的。

例如，以圖6中所示的結(jié)構(gòu)，運(yùn)動(dòng)頻率傳感器52檢測(cè)在例如兩足行走的機(jī)器人移動(dòng)中的特定頻率，并且將其輸出到低通濾波器46和48。然而，另一種選擇是，以圖1到圖3中的結(jié)構(gòu)，運(yùn)動(dòng)頻率傳感器52可以將此特定頻率輸出到低通濾波器18和28，并且低通濾波器18和28的截止頻率可以設(shè)置為適應(yīng)性地改變。

另外，以圖1的結(jié)構(gòu)，來(lái)自矩陣姿態(tài)角計(jì)算器(角速度系統(tǒng))16的角速度系統(tǒng)姿態(tài)角被輸出到低通濾波器18，并且來(lái)自矩陣姿態(tài)角計(jì)算器(加速度系統(tǒng))26的加速度姿態(tài)角被輸出到低通濾波器28。然而，另一種選擇是，來(lái)自矩陣姿態(tài)角計(jì)算器16的角速度系統(tǒng)姿態(tài)角和來(lái)自矩陣姿態(tài)角計(jì)算器26的加速度姿態(tài)角也均可以直接輸出到微分器30，然后來(lái)自微分器30的差值輸入到單個(gè)低通濾波器，并且低范圍分量作為偏移量輸出到減法器32。這有利于減少LPF的數(shù)量。這同樣也適于圖2和3中的結(jié)構(gòu)。

此外，模擬濾波器或數(shù)字濾波器也可以用作圖1到圖6中的低通濾波器。數(shù)字濾波器可以是FIR(有限時(shí)間沖激回應(yīng))濾波器或IIR(無(wú)限時(shí)間沖激回應(yīng))濾波器。

而且，在每個(gè)結(jié)構(gòu)中，姿態(tài)角、角速度和加速度中的至少一個(gè)從輸出裝置34輸出到機(jī)器人的主處理器。然而，另外，由矩陣加法計(jì)算器14計(jì)算出的姿態(tài)矩陣、由加速度矩陣計(jì)算器24計(jì)算出的姿態(tài)矩陣、由微分器30計(jì)算出的偏移量以及由四元數(shù)LPFs 63和67提取的四元數(shù)等，可以輸出到輸出裝置34，然后從輸出裝置34輸出到主處理器。

此外，圖20、22和23中所示的結(jié)構(gòu)也可以設(shè)置有運(yùn)動(dòng)頻率傳感器52，其輸入來(lái)自角速度傳感器10的角速度和來(lái)自加速度傳感器20的加速度，并且檢測(cè)在機(jī)器人，即移動(dòng)體的運(yùn)動(dòng)中的特定頻率。運(yùn)動(dòng)頻率傳感器從角速度和加速度中提取頻繁出現(xiàn)的頻率分量，并且將其輸出到低通濾波器18和28。一個(gè)例子為當(dāng)機(jī)器人，即移動(dòng)體用兩條腿行走時(shí)的兩足移動(dòng)中的特定頻率。低通濾波器18和28根據(jù)從運(yùn)動(dòng)頻率傳感器輸入的頻率適應(yīng)性地調(diào)整截止頻率。運(yùn)動(dòng)頻率越大，低通濾波器18和26越要增大截止頻率。此外，運(yùn)動(dòng)頻率傳感器將運(yùn)動(dòng)頻率輸出到執(zhí)行PID控制的乘法器38、積分器40和微分器42。乘法器38、積分器40和微分器42根據(jù)輸入的運(yùn)動(dòng)頻率調(diào)整乘法器系數(shù)。

迄今已經(jīng)描述了本發(fā)明的示例性實(shí)施例。這些示例性實(shí)施例的每個(gè)的形式展示了如下獨(dú)一無(wú)二的效果。即，根據(jù)利用姿態(tài)角的形式，輸出為用在與機(jī)器人、車輛、船舶和飛機(jī)有關(guān)的控制等中的姿態(tài)角。姿態(tài)角本身為設(shè)備中的校正循環(huán)控制的對(duì)象，所以能夠獲得用于真實(shí)表現(xiàn)(actual phenomenon)的穩(wěn)定的輸出。此外，當(dāng)通過(guò)合適的測(cè)量和控制來(lái)優(yōu)化設(shè)備中的控制參數(shù)時(shí)，各種參數(shù)可以得到有效地優(yōu)化以便與來(lái)自真實(shí)運(yùn)動(dòng)的姿態(tài)角具有良好的兼容性。因此，需要更少的時(shí)間和工作用于優(yōu)化，而且可以獲得適用于應(yīng)用的特性。

因而，此形式適用于利用姿態(tài)角作為主要對(duì)象的測(cè)量和控制，并且能夠進(jìn)行過(guò)去不能或不足的車輛或機(jī)器人的姿態(tài)測(cè)量和控制。此外，以利用姿態(tài)矩陣的形式，輸出為用于機(jī)器人、車輛、船舶和飛機(jī)有關(guān)的測(cè)量和控制等的姿態(tài)角。通過(guò)內(nèi)部計(jì)算，姿態(tài)矩陣為校正循環(huán)的對(duì)象。因此，為避免特殊性等的特殊處理不是在內(nèi)部計(jì)算中執(zhí)行，這比較容易理解。進(jìn)一步，姿態(tài)矩陣本身可以輸出到外部部件。因此，此形式適于使用姿態(tài)矩陣的測(cè)量和控制。通過(guò)姿態(tài)矩陣，部件的個(gè)數(shù)比姿態(tài)角的個(gè)數(shù)多，所以有大量的數(shù)據(jù)。這不幸地意味著通信需要時(shí)間并且數(shù)據(jù)處理麻煩且復(fù)雜。然而，盡管有此缺點(diǎn)，大量的數(shù)據(jù)使得能夠避免特殊的問(wèn)題，并且能夠擴(kuò)展兼容性、壓縮和修改坐標(biāo)系，因而提供大的自由度。當(dāng)有足夠的通信速度、足夠的測(cè)量和控制的計(jì)算速度、以及執(zhí)行計(jì)算所需的大區(qū)域的存儲(chǔ)器時(shí)可以容易使用此形式。因此，當(dāng)有快速度通信網(wǎng)絡(luò)時(shí)此形式特別有效，此形式應(yīng)用于具有快速計(jì)算速度的高性能測(cè)量/控制處理系統(tǒng)，例如，在用于諸如核能設(shè)備和工業(yè)設(shè)備的空間和高水平測(cè)量/控制設(shè)備中使用的設(shè)備。

此外，利用四元數(shù)的形式，輸出為用在與機(jī)器人、車輛、船舶和飛機(jī)有關(guān)的測(cè)量和控制等中的姿態(tài)角。對(duì)于內(nèi)部計(jì)算，四元數(shù)為校正循環(huán)的對(duì)象。利用四元數(shù)的優(yōu)點(diǎn)在于，其減少了計(jì)算所需的數(shù)據(jù)量，并且減少了計(jì)算步驟數(shù)。因此，可以利用設(shè)備中的具有中等計(jì)算速度和小存儲(chǔ)器的處理器來(lái)實(shí)現(xiàn)此功能，因此可以減少成本。然而，因?yàn)樗脑獢?shù)是其特征在實(shí)際空間中難于理解的表示方法，其在轉(zhuǎn)換為姿態(tài)角后使用。因此，這是用于測(cè)量和控制的特性清楚的形式，并且適用于當(dāng)有多個(gè)相似移動(dòng)。因此，此形式適用于小尺寸工業(yè)設(shè)備、多用途機(jī)器人和批量生產(chǎn)的車輛的測(cè)量和控制。
權(quán)利要求
1.一種移動(dòng)體姿態(tài)角檢測(cè)設(shè)備，其特征在于，包括
檢測(cè)移動(dòng)體的角速度的角速度傳感器；
檢測(cè)所述移動(dòng)體的加速度的加速度傳感器；
角速度姿態(tài)角計(jì)算裝置，用于由所述角速度計(jì)算所述移動(dòng)體的姿態(tài)角，作為角速度姿態(tài)角；
加速度姿態(tài)角計(jì)算裝置，用于由所述加速度計(jì)算所述移動(dòng)體的姿態(tài)角，作為加速度姿態(tài)角；
角速度姿態(tài)角低范圍分量提取裝置，用于提取所述角速度姿態(tài)角的低范圍分量；
加速度姿態(tài)角低范圍分量提取裝置，用于提取所述加速度姿態(tài)角的低范圍分量；
差值計(jì)算裝置，用于計(jì)算所述角速度姿態(tài)角的低范圍分量和所述加速度姿態(tài)角的低范圍分量之間的差，作為誤差；
誤差去除裝置，用于從所述角速度姿態(tài)角去除所述誤差；
輸出裝置，用于輸出由所述誤差去除裝置去除了所述誤差的角速度姿態(tài)角，作為所述移動(dòng)體的姿態(tài)角；及
反饋裝置，用于將由所述誤差去除裝置去除了所述誤差的所述角速度姿態(tài)角反饋到所述角速度姿態(tài)角計(jì)算裝置。
2.如權(quán)利要求1所述的設(shè)備，其特征在于
所述誤差去除裝置包括
按比例地計(jì)算由所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差的值的乘法器，
對(duì)由所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差的值進(jìn)行積分計(jì)算的積分器，及
對(duì)由所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差的值進(jìn)行微分計(jì)算的微分器；
及所述誤差去除裝置通過(guò)從所述角速度姿態(tài)角減去所述乘法器、所述積分器和所述微分器得到的所述計(jì)算值的和來(lái)去除所述誤差。
3.如權(quán)利要求1或2所述的設(shè)備，其特征在于，進(jìn)一步包括
用于獨(dú)立于所述加速度傳感器而檢測(cè)繞所述移動(dòng)體的垂直軸的姿態(tài)角并且將所述結(jié)果輸出到所述加速度姿態(tài)角計(jì)算裝置的裝置。
4.如權(quán)利要求1-3中任意一項(xiàng)所述的設(shè)備，其特征在于
所述角速度姿態(tài)角計(jì)算裝置包括
用于從所述角速度計(jì)算在經(jīng)過(guò)一小段時(shí)間Δt之后定義小旋轉(zhuǎn)角的小角度矩陣的裝置；
用于從所述小角度矩陣和對(duì)應(yīng)于從所述反饋裝置反饋回來(lái)的所述角速度姿態(tài)角的姿態(tài)矩陣，計(jì)算新的角速度系統(tǒng)姿態(tài)矩陣的裝置；
用于從所述新的角速度系統(tǒng)姿態(tài)矩陣計(jì)算所述角速度姿態(tài)角的裝置；
及所述加速度姿態(tài)角計(jì)算裝置包括
用于從所述加速度計(jì)算傾斜角的裝置；
用于從所述傾斜角計(jì)算加速度系統(tǒng)姿態(tài)矩陣的裝置；和
用于從所述加速度系統(tǒng)姿態(tài)矩陣計(jì)算所述加速度姿態(tài)角的裝置。
5.如權(quán)利要求4所述的設(shè)備，其特征在于
所述角速度姿態(tài)角低范圍分量提取裝置具有由所述計(jì)算所述角速度姿態(tài)角的裝置提取所述角速度姿態(tài)角的所述低范圍分量的角速度系統(tǒng)低通濾波器；
所述加速度姿態(tài)角低范圍分量提取裝置具有由所述計(jì)算所述加速度姿態(tài)角的裝置提取所述加速度姿態(tài)角的所述低范圍分量的加速度系統(tǒng)低通濾波器；及
所述角速度系統(tǒng)低通濾波器和所述加速度系統(tǒng)低通濾波器具有相同的截止頻率和相同的衰減因子。
6.如權(quán)利要求4所述的設(shè)備，其特征在于
所述角速度姿態(tài)角低范圍分量提取裝置具有由所述計(jì)算所述新的角速度系統(tǒng)姿態(tài)矩陣的裝置提取所述新的角速度系統(tǒng)姿態(tài)矩陣的低范圍分量的角速度系統(tǒng)低通濾波器；
所述加速度姿態(tài)角低范圍分量提取裝置具有由所述計(jì)算所述加速度系統(tǒng)姿態(tài)矩陣的裝置提取所述加速度系統(tǒng)姿態(tài)矩陣的低范圍分量的加速度系統(tǒng)低通濾波器；及
所述角速度系統(tǒng)低通濾波器和所述加速度系統(tǒng)低通濾波器具有相同的截止頻率和相同的衰減因子。
7.如權(quán)利要求6所述的設(shè)備，其特征在于，進(jìn)一步包括
用于正規(guī)化和正交化來(lái)自所述角速度系統(tǒng)低通濾波器的所述角速度系統(tǒng)姿態(tài)矩陣的裝置；及
用于正規(guī)化和正交化來(lái)自所述加速度系統(tǒng)低通濾波器的所述加速度系統(tǒng)姿態(tài)矩陣的裝置。
8.如權(quán)利要求5-7中任意一項(xiàng)所述的設(shè)備，其特征在于，進(jìn)一步包括
用于從所述角速度和所述加速度檢測(cè)對(duì)于所述移動(dòng)體特定的運(yùn)動(dòng)頻率的裝置，
其中所述角速度系統(tǒng)低通濾波器和所述加速度系統(tǒng)低通濾波器的所述截止頻率根據(jù)所述運(yùn)動(dòng)頻率可變地設(shè)置。
9.如權(quán)利要求2所述的設(shè)備，其特征在于，進(jìn)一步包括
用于從所述角速度和所述加速度檢測(cè)對(duì)于所述移動(dòng)體特定的運(yùn)動(dòng)頻率的裝置，
其中所述乘法器、所述積分器和所述微分器的系數(shù)根據(jù)所述運(yùn)動(dòng)頻率可變地設(shè)置。
10.一種移動(dòng)體姿態(tài)角檢測(cè)設(shè)備，其特征在于，包括
檢測(cè)移動(dòng)體的角速度的角速度傳感器；
檢測(cè)所述移動(dòng)體的加速度的加速度傳感器；
角速度姿態(tài)角計(jì)算裝置，用于從所述角速度計(jì)算所述移動(dòng)體的姿態(tài)角，作為角速度姿態(tài)角；
加速度姿態(tài)角計(jì)算裝置，用于從所述加速度計(jì)算所述移動(dòng)體的姿態(tài)角，作為加速度姿態(tài)角；
差值計(jì)算裝置，用于計(jì)算所述角速度姿態(tài)角和所述加速度姿態(tài)角之間的差；
低范圍分量提取裝置，用于提取所述差的低范圍分量作為誤差；
誤差去除裝置，用于從所述角速度姿態(tài)角去除所述誤差；
輸出裝置，用于輸出由所述誤差去除裝置去除了所述誤差的角速度姿態(tài)角，作為所述移動(dòng)體的姿態(tài)角；和
反饋裝置，用于將由所述誤差去除裝置去除了所述誤差的所述角速度姿態(tài)角反饋到所述角速度姿態(tài)角計(jì)算裝置。
11.一種移動(dòng)體姿態(tài)角檢測(cè)設(shè)備，其特征在于，包括
檢測(cè)移動(dòng)體的角速度的角速度傳感器；
檢測(cè)所述移動(dòng)體的加速度的加速度傳感器；
用于從所述角速度計(jì)算在經(jīng)過(guò)一小段時(shí)間ts之后定義小旋轉(zhuǎn)角的小角度矩陣的裝置；
用于從所述小角度矩陣和被反饋回來(lái)的姿態(tài)矩陣計(jì)算新的角速度系統(tǒng)姿態(tài)矩陣的裝置；
用于從所述加速度計(jì)算傾斜角的裝置；
用于從所述傾斜角計(jì)算加速度系統(tǒng)姿態(tài)矩陣的裝置；
角速度系統(tǒng)低范圍分量提取裝置，用于提取所述角速度系統(tǒng)姿態(tài)矩陣的低范圍分量；
加速度系統(tǒng)低范圍分量提取裝置，用于提取所述加速度系統(tǒng)姿態(tài)矩陣的低范圍分量；
差值計(jì)算裝置，用于計(jì)算所述角速度系統(tǒng)姿態(tài)矩陣的所述低范圍分量和所述加速度系統(tǒng)姿態(tài)矩陣的所述低范圍分量之間的差，作為誤差；
誤差去除裝置，用于從所述角速度系統(tǒng)姿態(tài)矩陣去除所述誤差；
輸出裝置，用于從由所述誤差去除裝置去除了所述誤差的角速度系統(tǒng)姿態(tài)矩陣計(jì)算姿態(tài)角，并且輸出所述結(jié)果作為所述移動(dòng)體的姿態(tài)角；和
反饋裝置，用于將由所述誤差去除裝置去除了所述誤差的所述角速度系統(tǒng)姿態(tài)矩陣反饋到所述用于計(jì)算所述新的角速度系統(tǒng)姿態(tài)矩陣的裝置。
12.如權(quán)利要求11所述的設(shè)備，其特征在于，所述誤差去除裝置包括
按比例地計(jì)算所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差值的乘法器，
對(duì)由所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差值進(jìn)行積分計(jì)算的積分器，和
對(duì)由所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差值進(jìn)行微分計(jì)算的微分器；
及所述誤差去除裝置通過(guò)從所述角速度系統(tǒng)姿態(tài)矩陣減去通過(guò)將來(lái)自所述乘法器、所述積分器和所述微分器的所述計(jì)算的值相加獲得的矩陣，來(lái)去除所述誤差。
13.一種移動(dòng)體姿態(tài)角檢測(cè)設(shè)備，其特征在于，包括
檢測(cè)移動(dòng)體的角速度的角速度傳感器；
檢測(cè)所述移動(dòng)體的加速度的加速度傳感器；
用于從所述角速度計(jì)算在經(jīng)過(guò)一小段時(shí)間ts之后定義小旋轉(zhuǎn)角的小角度四元數(shù)的裝置；
用于從所述小角度四元數(shù)和被反饋回來(lái)的四元數(shù)計(jì)算新的角速度系統(tǒng)四元數(shù)的裝置；
用于從所述加速度計(jì)算傾斜角的裝置；
用于從所述傾斜角計(jì)算加速度系統(tǒng)四元數(shù)的裝置；
角速度系統(tǒng)低范圍分量提取裝置，用于提取所述角速度系統(tǒng)四元數(shù)的低范圍分量；
加速度系統(tǒng)低范圍分量提取裝置，用于提取所述加速度系統(tǒng)四元數(shù)的低范圍分量；
差值計(jì)算裝置，用于計(jì)算所述角速度系統(tǒng)四元數(shù)的所述低范圍分量與所述加速度系統(tǒng)四元數(shù)的所述低范圍分量之間的差，作為誤差；
誤差去除裝置，用于從所述角速度系統(tǒng)四元數(shù)去除所述誤差；
輸出裝置，用于從由所述誤差去除裝置去除了所述誤差的角速度系統(tǒng)四元數(shù)計(jì)算姿態(tài)角，并且輸出所述結(jié)果作為所述移動(dòng)體的姿態(tài)角；知
反饋裝置，用于將由所述誤差去除裝置去除了所述誤差的所述角速度系統(tǒng)四元數(shù)反饋到所述用于計(jì)算所述新的角速度系統(tǒng)四元數(shù)的裝置。
14.如權(quán)利要求13所述的設(shè)備，其特征在于，所述誤差去除裝置包括
按比例地計(jì)算由所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差值的乘法器，
對(duì)由所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差值進(jìn)行積分計(jì)算的積分器，及
對(duì)由所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差值進(jìn)行微分計(jì)算的微分器；
并且所述誤差去除裝置通過(guò)從所述角速度系統(tǒng)四元數(shù)減去通過(guò)將來(lái)自所述乘法器、所述積分器和所述微分器的所述計(jì)算的值相加獲得的四元數(shù)來(lái)去除所述誤差。
15.如權(quán)利要求11-14中任意一項(xiàng)所述的設(shè)備，其特征在于，進(jìn)一步包括
用于獨(dú)立于所述加速度傳感器檢測(cè)繞所述移動(dòng)體的垂直軸的傾斜角并將所述結(jié)果輸出到所述用于計(jì)算所述加速度系統(tǒng)姿態(tài)矩陣的裝置的裝置。
16.如權(quán)利要求11-15中任意一項(xiàng)所述的設(shè)備，其特征在于
所述角速度系統(tǒng)低范圍分量提取裝置具有角速度系統(tǒng)低通濾波器；
所述加速度系統(tǒng)低范圍分量提取裝置具有加速度系統(tǒng)低通濾波器；及
所述角速度系統(tǒng)低通濾波器和所述加速度系統(tǒng)低通濾波器具有相同的截止頻率和相同的衰減因子。
17.如權(quán)利要求16所述的設(shè)備，其特征在于，進(jìn)一步包括
用于正規(guī)化所述角速度系統(tǒng)低通濾波器的輸出的裝置；和
用于正規(guī)化所述加速度系統(tǒng)低通濾波器的輸出的裝置。
18.如權(quán)利要求16或17所述的設(shè)備，其特征在于，進(jìn)一步包括
用于從所述角速度和所述加速度檢測(cè)對(duì)于所述移動(dòng)體特定的運(yùn)動(dòng)頻率的裝置，
其中所述角速度系統(tǒng)低通濾波器和所述加速度系統(tǒng)低通濾波器的截止頻率根據(jù)所述運(yùn)動(dòng)頻率可變地設(shè)置。
19.如權(quán)利要求12或14所述的設(shè)備，其特征在于，進(jìn)一步包括
用于從所述角速度和所述加速度檢測(cè)對(duì)于所述移動(dòng)體特定的運(yùn)動(dòng)頻率的裝置，
其中所述乘法器、所述積分器和所述微分器的系數(shù)根據(jù)所述運(yùn)動(dòng)頻率可變地設(shè)置。
20.如權(quán)利要求1所述的設(shè)備，其特征在于
所述角速度姿態(tài)角低范圍分量提取裝置和所述加速度姿態(tài)角低范圍分量提取裝置包括
用于sin轉(zhuǎn)換和cos轉(zhuǎn)換所述角速度姿態(tài)角的裝置；
用于提取通過(guò)所述sin轉(zhuǎn)換和所述cos轉(zhuǎn)換獲得的值的低范圍分量的裝置；和
用于對(duì)所述提取的低范圍分量的值執(zhí)行atan2計(jì)算或atan計(jì)算的裝置。
21.如權(quán)利要求20所述的設(shè)備，其特征在于，所述誤差去除裝置包括
按比例地計(jì)算由所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差值的乘法器，
對(duì)由所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差值進(jìn)行積分計(jì)算的積分器，及
對(duì)由所述差值計(jì)算裝置計(jì)算的所述差得到的所述誤差值進(jìn)行微分計(jì)算的微分器；
及所述誤差去除裝置通過(guò)從所述角速度姿態(tài)角減去來(lái)自所述乘法器、所述積分器和所述微分器的所述計(jì)算值的和而去除所述誤差。
22.如權(quán)利要求20或21所述的設(shè)備，其特征在于，進(jìn)一步包括
用于獨(dú)立于所述加速度傳感器檢測(cè)繞所述移動(dòng)體的垂直軸的傾斜角并將所述結(jié)果輸出到所述加速度姿態(tài)角計(jì)算裝置的裝置。
23.如權(quán)利要求20-22中任意一項(xiàng)所述的設(shè)備，其特征在于
所述角速度姿態(tài)角計(jì)算裝置包括
用于從所述角速度計(jì)算在經(jīng)過(guò)一小段時(shí)間ts之后定義小旋轉(zhuǎn)角的小角度矩陣的裝置；
用于從所述小角度矩陣和對(duì)應(yīng)于從所述反饋裝置反饋回來(lái)的所述角速度姿態(tài)角的姿態(tài)矩陣，計(jì)算新的角速度系統(tǒng)姿態(tài)矩陣的裝置；和
用于從所述新的角速度系統(tǒng)姿態(tài)矩陣計(jì)算所述角速度姿態(tài)角的裝置；
及所述加速度姿態(tài)角計(jì)算裝置包括
用于從所述加速度計(jì)算傾斜角的裝置；
用于從所述傾斜角計(jì)算加速度系統(tǒng)姿態(tài)矩陣的裝置；和
用于從所述加速度系統(tǒng)姿態(tài)矩陣計(jì)算所述加速度姿態(tài)角的裝置。
24.如權(quán)利要求20-23中任意一項(xiàng)所述的設(shè)備，其特征在于
所述角速度姿態(tài)角低范圍分量提取裝置具有提取所述sin轉(zhuǎn)換和cos轉(zhuǎn)換的值的所述低范圍分量的角速度系統(tǒng)低通濾波器；
所述加速度姿態(tài)角低范圍分量提取裝置具有提取所述sin轉(zhuǎn)換和cos轉(zhuǎn)換的值的所述低范圍分量的加速度系統(tǒng)低通濾波器；及
所述角速度系統(tǒng)低通濾波器和所述加速度系統(tǒng)低通濾波器具有相同的截止頻率和相同的衰減因子。
25.如權(quán)利要求20-24中任意一項(xiàng)所述的設(shè)備，其特征在于
所述差值計(jì)算裝置包括用于計(jì)算所述角速度姿態(tài)角的低范圍分量在被sin轉(zhuǎn)換和cos轉(zhuǎn)換后，其結(jié)果再經(jīng)atan2計(jì)算或atan計(jì)算而得到的結(jié)果的值，與所述加速度姿態(tài)角的低范圍分量在被sin轉(zhuǎn)換和cos轉(zhuǎn)換后，其結(jié)果再經(jīng)atan2計(jì)算或atan計(jì)算而得到的結(jié)果的值之間的差的裝置。
26.如權(quán)利要求25所述的設(shè)備，其特征在于，進(jìn)一步包括用于sin轉(zhuǎn)換和cos轉(zhuǎn)換通過(guò)計(jì)算所述差獲得的值、并atan2計(jì)算或atan計(jì)算所述結(jié)果的裝置。
27.如權(quán)利要求20-26中任意一項(xiàng)所述的設(shè)備，其特征在于
所述角速度姿態(tài)角和所述加速度姿態(tài)角的ψ或φ被定義為在-π和π之間，包括-π和π，的角度；
所述誤差去除裝置通過(guò)在所述誤差小于-π時(shí)將所述誤差替換為-π，以及在所述誤差大于π時(shí)將所述誤差替換為π，而從所述角速度姿態(tài)角去除所述誤差；
所述角速度姿態(tài)角和所述加速度姿態(tài)角的θ被定義為在-π/2和π/2之間，包括-π/2和π/2，的角度；及
所述誤差去除裝置通過(guò)在所述誤差小于-π/2時(shí)將所述誤差替換為-π/2+δ(δ為實(shí)數(shù))，以及在所述誤差大于π/2時(shí)將所述誤差替換為π/2-δ，而從所述角速度姿態(tài)角去除所述誤差。
28.如權(quán)利要求24所述的設(shè)備，其特征在于，進(jìn)一步包括
用于從所述角速度和所述加速度檢測(cè)對(duì)于所述移動(dòng)體特定的運(yùn)動(dòng)頻率的裝置，
其中所述角速度系統(tǒng)低通濾波器和所述加速度系統(tǒng)低通濾波器的截止頻率根據(jù)所述運(yùn)動(dòng)頻率可變地設(shè)置。
29.如權(quán)利要求21所述的設(shè)備，其特征在于，進(jìn)一步包括
用于從所述角速度和所述加速度檢測(cè)對(duì)于所述移動(dòng)體特定的運(yùn)動(dòng)頻率的裝置，
其中所述乘法器、所述積分器和所述微分器的系數(shù)根據(jù)所述運(yùn)動(dòng)頻率可變地設(shè)置。
全文摘要
由角速度傳感器(10)檢測(cè)到的角速度由小角度矩陣計(jì)算器(12)和矩陣加法計(jì)算器(14)積分，然后由矩陣姿態(tài)角計(jì)算器(16)還原為姿態(tài)角(角速度姿態(tài)角)。通過(guò)傾斜角計(jì)算器(22)和加速度矩陣計(jì)算器(24)計(jì)算姿態(tài)矩陣，并且由加速度傳感器(20)檢測(cè)到的加速度由矩陣姿態(tài)角計(jì)算器(26)還原為姿態(tài)角(加速度姿態(tài)角)。低通濾波器(18，28)的每個(gè)提取低范圍成分，由微分器(30)計(jì)算這兩者之間的差值，并且僅僅提取偏移量。減法器(32)將偏移量從角速度姿態(tài)角中去除，結(jié)果由輸出裝置(34)輸出。另外，姿態(tài)角矩陣計(jì)算器(36)將所述結(jié)果轉(zhuǎn)換為反饋到矩陣加法計(jì)算器(14)的姿態(tài)矩陣。
文檔編號(hào)G01C15/00GK101232979SQ200680027981
公開(kāi)日2008年7月30日 申請(qǐng)日期2006年8月1日 優(yōu)先權(quán)日2005年8月1日
發(fā)明者杉原久義, 野野村裕, 藤吉基弘 申請(qǐng)人:豐田自動(dòng)車株式會(huì)社