。
[0064] 處理部100進(jìn)一步具有坐標(biāo)轉(zhuǎn)換部125。坐標(biāo)轉(zhuǎn)換部125根據(jù)按照第二頻率f2而 由姿勢(shì)更新部124更新的移動(dòng)體的姿勢(shì)和以第二頻率f2輸入的加速度，計(jì)算從第一坐標(biāo)系 向第二坐標(biāo)系進(jìn)行坐標(biāo)轉(zhuǎn)換的移動(dòng)體的加速度（以下，稱為"第二坐標(biāo)系加速度"）。如圖 4所示，處理部100從求得的姿勢(shì)更新移動(dòng)體的加速度的方向（ST25)。
[0065] 處理部100進(jìn)一步具有速度/位置計(jì)算部126。如圖4所示，速度/位置計(jì)算部 126將由坐標(biāo)轉(zhuǎn)換部125輸出的第二坐標(biāo)系加速度進(jìn)行積分，并計(jì)算第二坐標(biāo)系中的移動(dòng) 體的速度和位置（ST26、ST 27)。
[0066] 如上所述，位置計(jì)算裝置1計(jì)算步行運(yùn)動(dòng)中的移動(dòng)體的第二坐標(biāo)系（絕對(duì)坐標(biāo)系、 移動(dòng)體坐標(biāo)系等）中的位置。從第一坐標(biāo)系向第二坐標(biāo)系進(jìn)行坐標(biāo)轉(zhuǎn)換時(shí)，使用通過姿勢(shì) 更新部124更新的移動(dòng)體的姿勢(shì)。移動(dòng)體的姿勢(shì)（角度）通過使用姿勢(shì)變化對(duì)前次的姿勢(shì) 進(jìn)行更新而得到，其中，所述姿勢(shì)變化通過姿勢(shì)變化計(jì)算部113對(duì)陀螺儀傳感器13所檢測(cè) 的角速度進(jìn)行積分而得到。積分期間內(nèi)的角速度的數(shù)據(jù)量（采樣數(shù)）越多，即向姿勢(shì)變化計(jì) 算部113輸入的角速度的頻率（輸入頻率）越高，算出的姿勢(shì)的精度越高。在本實(shí)施方式 中，向姿勢(shì)變化計(jì)算部113輸入的角速度的頻率為第一頻率Π ，高于向姿勢(shì)更新部124輸入 的姿勢(shì)變化頻率（第二頻率f2)。因此，即使在例如進(jìn)行步行運(yùn)動(dòng)的移動(dòng)體等的角速度變化 比較大的情況下，與以第二頻率f2進(jìn)行積分的情況比較，也能夠抑制積分精度的下降。另 一方面，輸入姿勢(shì)更新部124的姿勢(shì)變化頻率為低于第一頻率Π 的第二頻率f2。因此，姿 勢(shì)更新以后的運(yùn)算中的CPU的負(fù)荷，與以第一頻率Π 將姿勢(shì)變化輸入到姿勢(shì)更新部124的 情況相比得以降低。
[0067] 處理部100進(jìn)一步具有擴(kuò)張卡爾曼濾波器運(yùn)算部127及錯(cuò)誤校正部128。如圖4 所示，擴(kuò)張卡爾曼濾波器運(yùn)算部127使用由姿勢(shì)更新部124輸出的姿勢(shì)數(shù)據(jù)、由速度/位置 計(jì)算部126輸出的位置數(shù)據(jù)及速度數(shù)據(jù)、GPS傳感器15的測(cè)位數(shù)據(jù)（位置、速度、方位）、以 及由加速度偏差校正部122對(duì)加速度偏差進(jìn)行校正后的加速度，通過擴(kuò)張卡爾曼濾波器， 推定位置、速度、方位的誤差（ST28)。另外，擴(kuò)張卡爾曼濾波器運(yùn)算部127推定陀螺偏差及 加速度偏差，輸出至加速度偏差校正部122及陀螺偏差校正部113A。使用通過擴(kuò)張卡爾曼 濾波器運(yùn)算部127推定的位置、速度、姿勢(shì)的誤差，通過錯(cuò)誤校正部128進(jìn)行移動(dòng)體的位置、 速度、姿勢(shì)的校正。來自錯(cuò)誤校正部128的信息是處理部100的輸出。
[0068] 3陀螺儀傳感器IC的具體例
[0069] 圖5示出陀螺儀傳感器IC14的具體例。在圖5中，在圖3所示的構(gòu)成的基礎(chǔ)上， 還設(shè)置有模擬電路110、MPU114及串并接口（SPI) 115。
[0070] 模擬電路110,對(duì)于陀螺儀傳感器13的輸出進(jìn)行Q-V轉(zhuǎn)換并放大。MPU114,根據(jù)從 SPI115輸入的命令/數(shù)據(jù)控制模擬電路110、A/D轉(zhuǎn)換器11UDSP112、姿勢(shì)變化計(jì)算部113 及SPI115。因此，MPU114,具有命令解碼器114A和寄存器114B。根據(jù)通過命令解碼器114A 解碼的命令，各種控制數(shù)據(jù)存儲(chǔ)于寄存器，根據(jù)該控制數(shù)據(jù)陀螺儀傳感器IC14內(nèi)的各處被 控制。SPI115除了具有連接于處理部100的芯片選擇端子CS、數(shù)據(jù)輸入端子DI、數(shù)據(jù)輸出 端子D0、以及計(jì)時(shí)端子CLK，還具有插入端子INT。MPU114從處理部100的姿勢(shì)更新部124 經(jīng)由SPI115輸入請(qǐng)求命令。所謂請(qǐng)求命令是以第二頻率f2為輸出頻率，請(qǐng)求從姿勢(shì)變化 計(jì)算部113輸出姿勢(shì)變化的命令。由命令解碼器114A解碼的請(qǐng)求，經(jīng)由寄存器114B被指 令給姿勢(shì)變化計(jì)算部113,輸出姿勢(shì)變化。也可以以第二頻率f2的周期從插入端子INT向 處理部100輸出插入輸出，在插入發(fā)生時(shí)，發(fā)出請(qǐng)求姿勢(shì)變化的輸出的命令（請(qǐng)求信號(hào)）。
[0071] 作為利用在命令解碼器114A中被解碼的寫入（W)命令而在寄存器114B中設(shè)定的 控制值，可以列舉近似函數(shù)次數(shù)、偏差更新定時(shí)、角速度轉(zhuǎn)換（量子化單位LSB - rad/s)系 數(shù)、DRDY輸出定時(shí)、姿勢(shì)運(yùn)算開始/停止、偏差推定值寫入等。此外，在通過命令解碼器114A 解碼的寫入（W)命令之后，能夠?qū)嵤┳藙?shì)變化運(yùn)算結(jié)果的讀出。姿勢(shì)變化運(yùn)算開始/停止 定時(shí)，能夠與第二頻率f2的第二周期τ 2 ( τ 2 = l/f2)相一致。
[0072] 在此，下面的式（1)示出姿勢(shì)變化計(jì)算部113、姿勢(shì)更新部124以及坐標(biāo)轉(zhuǎn)換部 125進(jìn)行的運(yùn)算的一例。
[0073] 式⑴
【主權(quán)項(xiàng)】
1. 一種姿勢(shì)計(jì)算裝置，其特征在于， 包括： 陀螺儀傳感器，檢測(cè)生物體或者移動(dòng)體的、相對(duì)于慣性坐標(biāo)系的第一坐標(biāo)系的角速度， 所述移動(dòng)體以所述生物體的能量為動(dòng)力源而移動(dòng)； 姿勢(shì)變化計(jì)算部，根據(jù)以第一頻率Π 輸入的所述角速度，計(jì)算所述生物體或者所述移 動(dòng)體的姿勢(shì)變化；以及 姿勢(shì)更新部，根據(jù)以第二頻率f2輸入的所述姿勢(shì)變化，更新所述生物體或者所述移動(dòng) 體的姿勢(shì)， 其中，所述第一頻率及所述第二頻率滿足Π >0 20Hz。
2. 根據(jù)權(quán)利要求1所述的姿勢(shì)計(jì)算裝置，其特征在于， 所述姿勢(shì)變化計(jì)算部運(yùn)算 exp ([ a ibb (t+ τ j) ] X) exp ([ a ibb (t+2 τ j) ] X)......exp ([ a ibb (t+k τ J ] X) 其中，T1 = 1/fl，k 為滿足（fl/f2)-l〈k彡 fl/f2 的整數(shù)，t 為時(shí)刻，[α^α+ητι)] 為從時(shí)刻t+n τ i的所述角速度求得的旋轉(zhuǎn)矩陣，η為滿足1彡η彡k的整數(shù)。
3. 根據(jù)權(quán)利要求2所述的姿勢(shì)計(jì)算裝置，其特征在于，進(jìn)一步包括： 加速度傳感器，檢測(cè)相對(duì)于所述慣性坐標(biāo)系的所述第一坐標(biāo)系的加速度；以及 坐標(biāo)轉(zhuǎn)換部，根據(jù)所述姿勢(shì)和以所述第二頻率f2被輸入的所述加速度，從所述第一坐 標(biāo)系向第二坐標(biāo)系進(jìn)行所述加速度的坐標(biāo)轉(zhuǎn)換。
4. 根據(jù)權(quán)利要求1至3任一項(xiàng)所述的姿勢(shì)計(jì)算裝置，其特征在于， 進(jìn)一步包括收容有所述陀螺儀傳感器的傳感器單元， 所述姿勢(shì)變化計(jì)算部由收容于所述傳感器單元的硬件構(gòu)成。
5. 根據(jù)權(quán)利要求1至4任一項(xiàng)所述的姿勢(shì)計(jì)算裝置，其特征在于，進(jìn)一步含有加算平 均部，所述加算平均部將m個(gè)以第三頻率f3被輸入的所述加速度進(jìn)行加算平均并以所述第 二頻率f2輸入到所述坐標(biāo)轉(zhuǎn)換部，其中，f2〈f3, m為2以上的整數(shù)，且（f3/f2)-l〈m彡f3/ f2〇
6. 根據(jù)權(quán)利要求1至5任一項(xiàng)所述的姿勢(shì)計(jì)算裝置，其特征在于， 所述姿勢(shì)更新部將請(qǐng)求輸出所述姿勢(shì)變化的請(qǐng)求信號(hào)輸出至所述姿勢(shì)變化計(jì)算部。
7. -種姿勢(shì)計(jì)算方法，其特征在于， 包括： 檢測(cè)生物體或者移動(dòng)體的、相對(duì)于慣性坐標(biāo)系的第一坐標(biāo)系的角速度，所述移動(dòng)體以 所述生物體的能量為動(dòng)力源而移動(dòng)； 根據(jù)以第一頻率Π 輸入的所述角速度，計(jì)算所述生物體或者所述移動(dòng)體的姿勢(shì)變化； 以及 根據(jù)以第二頻率f2輸入的姿勢(shì)變化，更新所述生物體或者所述移動(dòng)體的姿勢(shì)， 其中，所述第一頻率及所述第二頻率滿足Π >0 20Hz。
【專利摘要】本發(fā)明涉及姿勢(shì)計(jì)算裝置及姿勢(shì)計(jì)算方法。姿勢(shì)計(jì)算裝置包括：陀螺儀傳感器，檢測(cè)生物體或者以所述生物體的能量為動(dòng)力源而移動(dòng)的移動(dòng)體的、相對(duì)于慣性坐標(biāo)系的第一坐標(biāo)系的角速度；姿勢(shì)變化計(jì)算部，根據(jù)以第一頻率f1輸入的角速度，計(jì)算移動(dòng)體的姿勢(shì)變化；以及姿勢(shì)更新部，根據(jù)以第二頻率f2輸入的姿勢(shì)變化更新移動(dòng)體的姿勢(shì)，其中，所述第一頻率及所述第二頻率滿足f1>f2≥20Hz。
【IPC分類】G01C21-16
【公開號(hào)】CN104634344
【申請(qǐng)?zhí)枴緾N201410613659
【發(fā)明人】水落俊一, 牧克彥, 中島克仁
【申請(qǐng)人】精工愛普生株式會(huì)社
【公開日】2015年5月20日
【申請(qǐng)日】2014年11月4日
【公告號(hào)】US20150134294