本發(fā)明屬于移動(dòng)機(jī)器人定位領(lǐng)域，具體涉及一種聲信號(hào)到達(dá)角度及相對位移的移動(dòng)機(jī)器人位姿估計(jì)方法。

背景技術(shù)：

1、隨著移動(dòng)機(jī)器人的廣泛應(yīng)用和普及，對機(jī)器人位置-姿態(tài)估計(jì)的精度要求也日益提高。準(zhǔn)確的位姿估計(jì)是機(jī)器人自主導(dǎo)航、路徑規(guī)劃和任務(wù)執(zhí)行的基礎(chǔ)?，F(xiàn)有的多種定位方法各有優(yōu)缺點(diǎn)：

2、激光雷達(dá)定位：高精度的激光雷達(dá)是目前較為成熟的定位方案。然而，室內(nèi)環(huán)境中常見的透明障礙物和反光物體會(huì)嚴(yán)重影響激光雷達(dá)的測量精度，導(dǎo)致定位誤差增大；

3、視覺定位：視覺傳感器由于成本適中、體積小巧，成為一種流行的估計(jì)方法。然而，光線、遮擋等環(huán)境因素會(huì)較大干擾視覺定位效果，并且使用攝像頭時(shí)可能造成隱私問題；

4、超寬帶定位：超寬帶在理想條件下能夠?qū)崿F(xiàn)厘米級(jí)的定位精度。然而，超寬帶基站的成本較高，在信號(hào)被遮擋時(shí)精度將大幅下降，且也存在無法獲取姿態(tài)的問題。

技術(shù)實(shí)現(xiàn)思路

1、為了解決背景技術(shù)中存在的問題和不足，本發(fā)明的主要目的是提供一種聲信號(hào)到達(dá)角度及相對位移的移動(dòng)機(jī)器人位姿估計(jì)方法，通過聲信號(hào)與麥克風(fēng)陣列相結(jié)合，通過聲信號(hào)的高精度到達(dá)角度估計(jì)與多源傳感器的數(shù)據(jù)融合，實(shí)現(xiàn)對機(jī)器人位姿的高精度測量，是一種低成本且可靠的移動(dòng)機(jī)器人位姿估計(jì)方法。

2、本發(fā)明采用的技術(shù)方案是：

3、本發(fā)明的聲信號(hào)到達(dá)角度及相對位移的移動(dòng)機(jī)器人位姿估計(jì)方法，包括：

4、s1、在移動(dòng)機(jī)器人的工作環(huán)境下布置若干個(gè)聲信號(hào)發(fā)射基站，每個(gè)聲信號(hào)發(fā)射基站發(fā)射定制信號(hào)，并采用麥克風(fēng)陣列實(shí)時(shí)接收定制信號(hào)。

5、s2、采用多重信號(hào)分類算法針對實(shí)時(shí)接收到的定制信號(hào)提取到達(dá)角度，并根據(jù)得到的到達(dá)角度和聲信號(hào)發(fā)射基站的坐標(biāo)進(jìn)行優(yōu)化得到移動(dòng)機(jī)器人的初步位姿。

6、s3、對步驟s1實(shí)時(shí)接收到的定制信號(hào)進(jìn)行信號(hào)分離得到直達(dá)信號(hào)和混響信號(hào)，根據(jù)直達(dá)信號(hào)和混響信號(hào)選取符合條件的直達(dá)信號(hào)作為關(guān)鍵幀信號(hào)，計(jì)算每相鄰兩個(gè)關(guān)鍵幀信號(hào)之間的距離，結(jié)合每相鄰兩個(gè)關(guān)鍵幀信號(hào)之間的距離和初步位姿采用圖優(yōu)化算法處理得到優(yōu)化位姿，作為移動(dòng)機(jī)器人最終的位姿。

7、所述步驟s1中，聲信號(hào)發(fā)射基站發(fā)射的定制信號(hào)按照以下公式設(shè)置：

8、x(t)=exp(j×2×π( f0t+kt2/2))

9、式中，x(t)為聲信號(hào)發(fā)射基站發(fā)射的定制信號(hào)隨時(shí)間變化的函數(shù)，j為虛數(shù)單位， f0為起始頻率，k為調(diào)頻率，t為時(shí)間，exp()為自然指數(shù)函數(shù)。

10、所述步驟s2具體為：

11、s21、采用分?jǐn)?shù)階傅里葉變換方法對接收到的定制信號(hào)進(jìn)行預(yù)處理。

12、s22、將預(yù)處理后的定制信號(hào)采用多重信號(hào)分類算法計(jì)算到達(dá)角度，得到若干個(gè)定制信號(hào)的到達(dá)角度。

13、s23、將若干個(gè)聲信號(hào)發(fā)射基站的坐標(biāo)和定制信號(hào)的到達(dá)角度代入到優(yōu)化目標(biāo)函數(shù)中，計(jì)算得到移動(dòng)機(jī)器人的初步位姿。

14、所述步驟s22具體為：

15、1)根據(jù)步驟s21預(yù)處理后的定制信號(hào)處理得到定制信號(hào)的協(xié)方差矩陣。

16、所述定制信號(hào)的協(xié)方差矩陣按照以下公式計(jì)算：

17、r=(1/n)*∑t=1n(x(t)xh(t))

18、式中，r為定制信號(hào)的協(xié)方差矩陣，n是采樣點(diǎn)的數(shù)量，t為時(shí)間，x(t)為定制信號(hào)，xh(t)是定制信號(hào)x(t)的共軛轉(zhuǎn)置。

19、2)對得到的定制信號(hào)的協(xié)方差矩陣進(jìn)行特征值分解，得到定制信號(hào)的信號(hào)子空間量和噪聲子空間量。

20、所述定制信號(hào)的協(xié)方差矩陣按照以下公式進(jìn)行特征值分解：

21、r=esλsesh+enλnenh

22、式中，r為定制信號(hào)的協(xié)方差矩陣，esh為定制信號(hào)的信號(hào)子空間量，enh為定制信號(hào)的噪聲子空間量，es和λs分別是信號(hào)子空間量的特征向量和特征值矩陣，en和λn分別是噪聲子空間量的特征向量和特征值矩陣。

23、3)根據(jù)得到的定制信號(hào)的噪聲子空間量構(gòu)建空間譜函數(shù)。

24、所述空間譜函數(shù)按照以下公式設(shè)置：

25、p(θ)=1/(ah(θ)enenha(θ))

26、式中，θ為定制信號(hào)的到達(dá)角度，p(θ)為空間譜函數(shù)，en為噪聲子空間量的特征向量，a(θ)為陣列流形向量，enh為定制信號(hào)的噪聲子空間量，ah(θ)為陣列流形向量的共軛轉(zhuǎn)置。

27、4)根據(jù)空間譜函數(shù)搜索所有的峰值位置，搜索到的所有峰值位置對應(yīng)的到達(dá)角度即為所有定制信號(hào)的到達(dá)角度。

28、所述步驟s23中，優(yōu)化目標(biāo)函數(shù)按照以下公式設(shè)置：

29、f=∑i=1n|arccos((tr(rptrz,itrd,i)-1)/2)|2

30、式中，f為優(yōu)化目標(biāo)函數(shù)，n為聲信號(hào)發(fā)射基站的個(gè)數(shù)，i為聲信號(hào)發(fā)射基站的索引，rpt為移動(dòng)機(jī)器人的位姿的旋轉(zhuǎn)矩陣，rz,it為移動(dòng)機(jī)器人的位姿與所有聲信號(hào)發(fā)射基站坐標(biāo)構(gòu)成的向量的角度對應(yīng)的旋轉(zhuǎn)矩陣，rd,i為定制信號(hào)的到達(dá)角度的旋轉(zhuǎn)矩陣，tr()表示矩陣求跡運(yùn)算。

31、所述步驟s3具體為：

32、s31、將步驟s1中麥克風(fēng)陣列接收到的定制信號(hào)進(jìn)行信號(hào)分離，分別得到直達(dá)信號(hào)和混響信號(hào)。

33、s32、分別計(jì)算直達(dá)信號(hào)和混響信號(hào)的能量，選取直達(dá)信號(hào)和混響信號(hào)的能量比值大于能量比閾值所對應(yīng)的直達(dá)信號(hào)作為關(guān)鍵幀信號(hào)。

34、s33、采用調(diào)頻連續(xù)波方法計(jì)算每相鄰兩個(gè)關(guān)鍵幀信號(hào)之間的距離，即得到移動(dòng)機(jī)器人在每相鄰兩個(gè)關(guān)鍵幀信號(hào)之間的移動(dòng)距離。

35、s34、將步驟s33得到的移動(dòng)距離和步驟s2得到的初步位姿代入到圖優(yōu)化算法中對移動(dòng)機(jī)器人的位姿進(jìn)行優(yōu)化，得到優(yōu)化位姿，即為移動(dòng)機(jī)器人最終的位姿。

36、所述步驟s33中的調(diào)頻連續(xù)波方法按照以下公式設(shè)置：

37、 fp=(br)/(ct)+(fminv+bv)/c

38、式中， fp為頻率分量，r為移動(dòng)機(jī)器人在相鄰兩個(gè)關(guān)鍵幀信號(hào)之間的移動(dòng)距離，b為定制信號(hào)的帶寬，c為光速，t為定制信號(hào)的調(diào)頻周期，v為移動(dòng)機(jī)器人的移動(dòng)速度，fmin為最小頻率。

39、所述步驟s34，圖優(yōu)化算法具體為：

40、將步驟s2得到的初步位姿作為圖結(jié)點(diǎn)，步驟s33得到的移動(dòng)距離作為邊，采用高斯-牛頓法進(jìn)行優(yōu)化。

41、本發(fā)明的有益效果是：

42、1、本發(fā)明的進(jìn)步在于利用16-20khz范圍內(nèi)的定制信號(hào)作為激勵(lì)信號(hào)，并通過麥克風(fēng)陣列捕捉這些信號(hào)的到達(dá)角度，從而獲取環(huán)境的空間結(jié)構(gòu)信息。

43、2、本發(fā)明設(shè)置的定制信號(hào)一方面能夠盡可能的減少對人的干擾，另一方面相對于超聲信號(hào)在空氣中衰減較弱，能夠有較好定位精度的同時(shí)保持較大的工作范圍。

44、3、本發(fā)明采用分?jǐn)?shù)階傅里葉變換和多重信號(hào)分類算法對信號(hào)進(jìn)行預(yù)處理和到達(dá)角度估計(jì)，能夠在復(fù)雜室內(nèi)環(huán)境中實(shí)現(xiàn)高精度的位姿估計(jì)。

45、4、本發(fā)明結(jié)合基于粒子濾波的優(yōu)化算法和圖優(yōu)化算法，進(jìn)一步提升了位姿解算的實(shí)時(shí)可靠性。

46、5、本發(fā)明提出的聲信號(hào)到達(dá)角度及相對位移的位姿估計(jì)方法，具有部署簡單、成本低廉的優(yōu)勢，特別適用擋光照條件復(fù)雜或存在煙霧遮的室內(nèi)環(huán)境。因此本發(fā)明在智能家居、安防巡檢、室內(nèi)導(dǎo)航等多個(gè)領(lǐng)域具有廣泛的應(yīng)用前景。