本發(fā)明屬于移動機(jī)器人領(lǐng)域，特別涉及一種慣性導(dǎo)航室內(nèi)移動機(jī)器人定位技術(shù)。

背景技術(shù)：

慣性導(dǎo)航是通過對速度積分得到位置并對總加速度積分得到速度的過程?？偧铀俣仁侵赣芍亓铀俣群捅皇┘拥姆侵亓Ξa(chǎn)生的加速度(亦即比力加速度)之和。慣性導(dǎo)航系統(tǒng)(inertialnavigationsystem，ins)包括：用于積分的導(dǎo)航計算機(jī)；用于給積分運(yùn)算定時的精密時鐘，測量比力加速度用的加速度計組臺；用于作為所算位置的一個函數(shù)而進(jìn)行的重力加速度計算而留于導(dǎo)航計算機(jī)中的重力模型軟件，以及為了定義作為速度計算一部分的加速度計三元組的角度方向所用的姿態(tài)基準(zhǔn)。在現(xiàn)代ins中，姿態(tài)基準(zhǔn)是由駐留于ins計算機(jī)中的軟件積分函數(shù)提供的，其輸入來自一個有三軸的慣性角速度傳感器。角速度傳感器和加速度計三元組安裝在一個公用的牢固構(gòu)架上，該構(gòu)架裝在ins的底盤上，以保證每個慣性傳感器之間的精確對準(zhǔn)，這樣的一種布置稱之為捷聯(lián)ins。因為慣性傳感器牢固地固定在底盤內(nèi)，所以也就牢固地固定于安裝ins的移動機(jī)器人上。

根據(jù)慣性導(dǎo)航理論，ins計算機(jī)中的基本函數(shù)有將角速率變換為姿態(tài)的積分函數(shù)(稱之為姿態(tài)積分).使用姿態(tài)數(shù)據(jù)將測得的加速度值轉(zhuǎn)換到適當(dāng)?shù)膶?dǎo)航坐標(biāo)系中，再將它積分成矢量速度的函數(shù)(稱之為矢量速度積分)，還有將導(dǎo)航系矢量速度積分成位置的函數(shù)(稱之為位置積分)。這樣就有了三個積分函數(shù)，姿態(tài)函數(shù)、矢量速度函數(shù)及位置函數(shù)，每個函數(shù)的精度要求很高，以確保函數(shù)誤差極小，符臺慣性傳感器精度的要求。在實際應(yīng)用中，給定初始機(jī)器人的位姿，即可經(jīng)由慣性導(dǎo)航理論得到移動機(jī)器人當(dāng)前的位姿。

為了達(dá)到在時域積分的功能，需要進(jìn)行數(shù)值積分。數(shù)值積分的目的是，通過在有限個采樣點上計算f(x)的值來逼近f(x)在區(qū)間[a,b]上的定積分。設(shè)a＝x0<x1<…<xm＝b.稱形如

且具有性質(zhì)的公式為數(shù)值積分或面積公式。項e[f]稱為積分的截斷誤差，值稱為面積節(jié)點，稱為權(quán)。通過m+1個等距點存在唯一的次數(shù)小于等于m的多項式pm(x)。當(dāng)用該多項式來近似[a,b]上的f(x)時，pm(x)的積分就近似等于f(x)的積分，這類公式稱為牛頓－科特斯公式。當(dāng)使用采樣點x0＝a和xm＝b時，稱為閉型牛頓－科特斯公式。

設(shè)xk＝x0+kh為等距節(jié)點，且fk＝f(xk)，辛普森積分公式為：辛普森公式的精度為n＝3，如果f∈c⁴[a,b]，則

對于慣性導(dǎo)航而言，其缺點在于以下幾點：在從慣性傳感器采集加速度信息的同時，不可避免的會收到噪聲的干擾，噪聲不光來自于傳感器本身的量化操作和電路噪聲也來自于在傳輸過程中受到外界條件的干擾；若對噪音不加控制抑或是慣性模塊本身就具有的零點漂移特性都必定會影響到接下來積分操作的精確度，此時噪聲就變成了趨勢項，在一次積分到速度信息時表現(xiàn)為一個隨時間增大的速度誤差，在二次積分后此誤差會再次放大，這就是所謂的累積誤差。對于慣性導(dǎo)航而言，累積誤差是影響其精度的最主要因素。

針對這些干擾，適當(dāng)?shù)男拚吞幚砭妥兊煤苡斜匾?。要消除噪聲信號，需先觀察分析信號的特性，在時域無法觀察到噪聲信號的特點，這就涉及到對加速度信號在頻域上進(jìn)行分析。快速傅里葉變換(fastfouriertransform)是信號處理與數(shù)據(jù)分析領(lǐng)域里最重要的算法之一。fft(快速傅里葉變換)本身就是離散傅里葉變換(discretefouriertransform)的快速算法，使算法復(fù)雜度由原本的o(n^2)變?yōu)閛(nlogn)，離散傅里葉變換dft，如同更為人熟悉的連續(xù)傅里葉變換，有如下的正、逆定義形式：

forwarddiscretefouriertransform(dft):

inversediscretefouriertransform(idft):

xn到xk的轉(zhuǎn)化就是時域到頻域的轉(zhuǎn)換，這個轉(zhuǎn)換有助于研究信號的功率譜，和使某些問題的計算更有效率。對信號進(jìn)行快速傅里葉變換后，就可以得到噪聲與干擾大概分布的頻率。同時結(jié)合該慣性傳感器實際使用的場合，確定有效信號所在頻率，根據(jù)奈奎斯特采樣定理，即采樣頻率需大于兩倍頻帶以上，確定所用低通濾波器的通頻帶寬度，即可完成對原始信號進(jìn)行有效的消噪處理。

技術(shù)實現(xiàn)要素：

本發(fā)明為解決上述技術(shù)問題，提出了一種基于慣性導(dǎo)航的室內(nèi)移動機(jī)器人定位方法，選用辛普森積分法，實現(xiàn)在不增加計算量的前提下，顯著提到精度；并且在頻域上處理及濾波，解決了傳統(tǒng)慣性導(dǎo)航積分算法的累積誤差過大以至于影響到實際使用的問題。

本發(fā)明采用的技術(shù)方案是：一種基于慣性導(dǎo)航的室內(nèi)移動機(jī)器人定位方法，包括：

s1、接收來自慣性傳感器采集的各幀加速度數(shù)據(jù)以及歐拉角；

s2、對步驟s1得到的各幀加速度數(shù)據(jù)進(jìn)行校正；

s3、根據(jù)各歐拉角將步驟s2得到的對應(yīng)加速度數(shù)據(jù)變換為相對地面的加速度變量；

s4、對慣性傳感器處于運(yùn)動狀態(tài)對應(yīng)的加速度變量進(jìn)行離散積分，得到速度信息；

s5、對步驟s4得到的速度信息通過快速傅里葉變換對得到的速度信息在頻域上進(jìn)行濾波處理；得到速度頻域信號；

s6、對步驟s5得到的速度頻域信號進(jìn)行快速傅里葉反變換，得到速度時域信號；

s7、對步驟s6得到的速度時域信號進(jìn)行離散積分，得到初始位移信號；

s8、對步驟s7得到的初始位移信號通過快速傅里葉變換對得到的速度信息在頻域上進(jìn)行濾波處理，得到初始位移的頻域信號；

s9、對初始位移的頻域信號進(jìn)行快速傅里葉反變換，得到初始位移的時域信號。

進(jìn)一步地，所述步驟s1具體包括以下分步驟：

s11、設(shè)置串口協(xié)議為：每幀為定長，設(shè)定將原始加速度數(shù)據(jù)與歐拉角的排列順序；并初始化幀頭；

s12、若接收數(shù)據(jù)的幀頭不符合預(yù)設(shè)值，則將接收到的整幀數(shù)據(jù)舍棄；

s13、對步驟s12接收到數(shù)據(jù)根據(jù)排列順序進(jìn)行分割操作，得到一組加速度數(shù)據(jù)和歐拉角。

進(jìn)一步地，所述步驟s2具體為：

通過對步驟s1接收的各幀加速度數(shù)據(jù)進(jìn)行分析，判斷慣性傳感器的狀態(tài)；所述狀態(tài)包括：靜止?fàn)顟B(tài)和運(yùn)動狀態(tài)；

并將慣性傳感器處于靜止?fàn)顟B(tài)下，在設(shè)定時間采集到的若干幀加速度數(shù)據(jù)的平均值作為慣性傳感器處于運(yùn)動狀態(tài)下對應(yīng)加速度數(shù)據(jù)的零點漂移量，用加速度數(shù)據(jù)原始值減去零點漂移量值完成零點校正。

更進(jìn)一步地，所述靜止?fàn)顟B(tài)或運(yùn)動狀態(tài)的判斷過程為：若該設(shè)定時間內(nèi)接收到各幀之間的加速度數(shù)據(jù)的差分值滿足固定的概率密度，則判斷慣性傳感器處于靜止?fàn)顟B(tài)；否則慣性傳感器處于運(yùn)動狀態(tài)。

進(jìn)一步地，步驟s3具體為：根據(jù)旋轉(zhuǎn)矩陣通過下式將步驟s2得到加速度數(shù)據(jù)變換到地面參考坐標(biāo)系中：

其中，ax和ay為加速度數(shù)據(jù)相對于車體坐標(biāo)系的坐標(biāo)；ae和an為加速度數(shù)據(jù)相對于地面坐標(biāo)系的坐標(biāo)；euler為歐拉角；e、n為地面坐標(biāo)系的兩個軸向。

進(jìn)一步地，所述離散積分采用辛普森積分。

進(jìn)一步地，所述步驟s4還包括：若是慣性傳感器處于靜止?fàn)顟B(tài)，則對應(yīng)的速度置為零。

本發(fā)明的有益效果：本發(fā)明的一種基于慣性導(dǎo)航的室內(nèi)移動機(jī)器人定位方法，首先，通過對接收到的慣性導(dǎo)航傳感器輸出的原始加速度數(shù)據(jù)進(jìn)行狀態(tài)分類；并根據(jù)靜止?fàn)顟B(tài)下的設(shè)定時間內(nèi)各幀的加速度數(shù)據(jù)平均值作為零點漂移量，來對運(yùn)動狀態(tài)下的加速度數(shù)據(jù)進(jìn)行校正，有效減小由加速度計本身特性帶來的靜態(tài)漂移以及零點漂移對積分算法精度的影響，顯著減小了累積誤差；其次，選擇了辛普森積分方法這樣一種兼顧精度與計算量的方法，相對于被廣泛使用的梯形積分方法，在沒有增大多少計算量的前提下，顯著提高了精度；再者，在時域分析的基礎(chǔ)上添加了頻域分析作為第二種減小誤差的方法，通過在頻域上處理和濾波，對于結(jié)果精度有很大提高，根本上解決了傳統(tǒng)慣性導(dǎo)航積分算法的累積誤差過大以至于影響到實際使用的問題。

附圖說明

圖1是本發(fā)明的方法的主流程圖。

圖2是本發(fā)明涉及到的坐標(biāo)轉(zhuǎn)換示意圖。

具體實施方式

為便于本領(lǐng)域技術(shù)人員理解本發(fā)明的技術(shù)內(nèi)容，下面結(jié)合附圖對本發(fā)明內(nèi)容進(jìn)一步闡釋。

如圖1所示為本發(fā)明的方案流程圖，本發(fā)明的技術(shù)方案為：一種基于慣性導(dǎo)航的室內(nèi)移動機(jī)器人定位方法，包括：

s1、接收來自慣性傳感器采集的加速度數(shù)據(jù)以及歐拉角；

慣性傳感器采集的加速度數(shù)據(jù)，具體過程如下：

1)、在傳感器端設(shè)置數(shù)據(jù)輸出格式，通過如下設(shè)置可使每幀加速度數(shù)據(jù)等長，方便接下來的數(shù)據(jù)分割與處理。

第一位：初始位，幀頭，設(shè)置為符號“！”

第二至第七位：x方向原始加速度，若沒有6位則補(bǔ)足6位；

第八位：空格，用來間隔；

第九至十四位：y方向原始加速度，若沒有6位則補(bǔ)足6位；

第十五位：空格，用來間隔；

第十六位至第二十一位：euler角，不足6位補(bǔ)足6位；

第二十二、二十三位：換行回車鍵。

本申請采用的是如上所述的每幀23位定長的串口協(xié)議，采用此種協(xié)議更易保持?jǐn)?shù)據(jù)完整性，也方便處理。但是在實際的處理過程中，串口協(xié)議的固定要求是每幀數(shù)據(jù)定長，具體每幀的長度根據(jù)實際情況決定。

2)、串口接收數(shù)據(jù)：通過預(yù)先設(shè)置波特率等串口屬性，完成串口數(shù)據(jù)的接收。在這其中根據(jù)1)中設(shè)置的串口協(xié)議對每幀的幀頭以及幀長進(jìn)行判斷，防止由于串口單次傳輸數(shù)據(jù)不完整造成的數(shù)據(jù)丟失和錯誤。若幀頭不符合預(yù)設(shè)值，則將接收到的整幀數(shù)據(jù)舍棄。比如本申請中設(shè)定的每幀長度為23位，若接收到的數(shù)據(jù)未滿23位，則等待直到接收到的數(shù)據(jù)大于23位，從中將前23位提取出來作為完整的數(shù)據(jù)幀。

具體的，本申請中設(shè)定的每幀長度為23位，第一位是幀頭，設(shè)置為符號“！”；第二至第七位是x方向原始加速度，若沒有6位則補(bǔ)足6位；第八位是空格，用來間隔；第九至十四位是y方向原始加速度，若沒有6位則補(bǔ)足6位；第十五位是空格，用來間隔；第十六位至第二十一位是euler角，不足6位補(bǔ)足6位；第二十二、二十三位：換行回車鍵。

3)、數(shù)據(jù)分割：對步驟2接收到的整幀數(shù)據(jù)進(jìn)行分割操作，從中提取出本專利需要的x方向線加速度，y方向線加速度和euler角(歐拉角)信息。

具體為：串口接收到每幀原始數(shù)據(jù)后；將該幀數(shù)據(jù)中的所有空格替換為逗號(即“，”)，以逗號作為分割的幾段有效數(shù)據(jù)則分別為x方向線加速度，y方向線加速度和euler角。

s2、對步驟s1得到的各幀加速度數(shù)據(jù)進(jìn)行校正；具體為：對接收到的原始數(shù)據(jù)進(jìn)行初步處理。

首先，進(jìn)行的是狀態(tài)劃分；對步驟s1接收到的各幀加速度數(shù)據(jù)進(jìn)行狀態(tài)判斷；狀態(tài)包括：靜止?fàn)顟B(tài)與運(yùn)動狀態(tài)；

然后，對步驟s1接收到的各幀加速度數(shù)據(jù)進(jìn)行概率分析，通過實驗發(fā)現(xiàn)不論是初始靜止還是運(yùn)動后靜置，加速度幀與幀之間的差分值滿足固定的概率密度：即在一定時間采集到的數(shù)據(jù)中95％都小于0.035。根據(jù)以上發(fā)現(xiàn)，可以通過對1s內(nèi)收集到的100個數(shù)據(jù)進(jìn)行分析，若滿足條件則判定在此時傳感器模塊也即是機(jī)器人平臺處于靜止?fàn)顟B(tài)；將此時的運(yùn)動狀態(tài)記錄為靜止?fàn)顟B(tài)，以待之后使用；否則記錄為運(yùn)動狀態(tài)。

其次，考慮到慣性模塊存在零點漂移現(xiàn)象，在判斷傳感器當(dāng)前處于靜止?fàn)顟B(tài)時，記錄靜止?fàn)顟B(tài)下一定時間內(nèi)采集到的加速度數(shù)據(jù)的平均值作為接下來運(yùn)動狀態(tài)加速度的零點漂移量；

最后，在輸出運(yùn)動狀態(tài)加速度時，用加速度原始值減去得到的零點漂移量值完成零點校正。

s3、根據(jù)歐拉角將步驟s2得到的加速度數(shù)據(jù)變換為相對地面的加速度變量；如圖2所示。對于二維導(dǎo)航系統(tǒng)；n，e為地面參考系的兩軸，為車體相對于n軸的順時針偏轉(zhuǎn)角度，本申請中通過將車體初始位置對準(zhǔn)n軸，然后用步驟s1得到的euler角(歐拉角)來代替角來進(jìn)行接下來的運(yùn)算。

根據(jù)旋轉(zhuǎn)矩陣通過以下轉(zhuǎn)換可將相對于車體坐標(biāo)系的ax和ay變換到地面參考坐標(biāo)系中，便于進(jìn)行進(jìn)一步的定位與導(dǎo)航操作：

按照上式求出相對于地面參考系的e，n方向加速度me和mn基礎(chǔ)上，在上位機(jī)端進(jìn)行迭加操作即可求出移動機(jī)器人相對于地面參考系初始位置的e，n方向速度。

s4、對處于運(yùn)動狀態(tài)的加速度變量進(jìn)行離散積分，得到速度信息；若是處于靜止?fàn)顟B(tài)，則對應(yīng)的速度置為零；在判定當(dāng)前處于運(yùn)動狀態(tài)時才進(jìn)行此模塊的計算，盡可能減少由傳感器誤差積分后造成的累積誤差。在比較了梯形積分，辛普森積分后選擇了辛普森積分作為此專利的積分方法。其優(yōu)勢體現(xiàn)在計算量相對于梯形積分并沒有多出太多的前提下，計算精度高了兩個級數(shù)。

對運(yùn)動狀態(tài)下的加速度信息進(jìn)行一次積分得到速度信號，將速度信息儲存為全局變量以備之后的軌跡跟蹤及運(yùn)動控制環(huán)節(jié)使用。與此同時還需對之前數(shù)據(jù)處理步驟記錄的運(yùn)動狀態(tài)進(jìn)行判斷，若記錄為靜止?fàn)顟B(tài)則速度直接輸出為零，跳過積分操作。

s5、對步驟s4得到的速度信息通過快速傅里葉變換(fft)對得到的速度信息在頻域上進(jìn)行濾波處理；得到速度頻域信號；

s6、對步驟s5得到的速度頻域信號進(jìn)行快速傅里葉反變換，得到速度時域信號；

對一次積分后的速度信息進(jìn)行快速傅里葉變換。采用的算法為一維頻率抽取的基2fft算法。選用快速傅里葉方法可以顯著地減少運(yùn)算次數(shù)，提高運(yùn)算速度。

首先，對得到的速度頻域信號進(jìn)行快速傅里葉變換，由實際機(jī)器人平臺運(yùn)行的狀態(tài)可知，有效的信號的頻率大概存在于10hz左右，因此根據(jù)奈奎斯特采樣定律，將頻域分析的通頻帶定在20hz，選取合適的濾波器類型，在頻域上對信號進(jìn)行濾波，消除高頻噪聲。

然后，對得到的頻域信號進(jìn)行傅里葉反變換，重新得到時域上的信號。

s7、對步驟s6得到的速度時域信號進(jìn)行離散積分，得到初始位移信號；

使用一次積分得到的并經(jīng)過濾波處理的速度信息，在此基礎(chǔ)上再進(jìn)行一次時域積分得到位移信息。同時已知初始坐標(biāo)的情況下，位移信息就可等效為機(jī)器人平臺當(dāng)前的坐標(biāo)值。

s8、對步驟s7得到的初始位移信號通過快速傅里葉變換(fft)對得到的速度信息在頻域上進(jìn)行濾波處理，得到初始位移的頻域信號；

對二次積分后的位移信息進(jìn)行快速傅里葉變換。采用的算法為一維頻率抽取的基2fft算法。與之前對速度信息的處理類似，進(jìn)行頻域濾波處理。

s9、對初始位移的頻域信號進(jìn)行快速傅里葉反變換，得到初始位移的時域信號。

本領(lǐng)域的普通技術(shù)人員將會意識到，這里所述的實施例是為了幫助讀者理解本發(fā)明的原理，應(yīng)被理解為本發(fā)明的保護(hù)范圍并不局限于這樣的特別陳述和實施例。對于本領(lǐng)域的技術(shù)人員來說，本發(fā)明可以有各種更改和變化。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的權(quán)利要求范圍之內(nèi)。