[發(fā)明專利]清潔機(jī)器人的定位方法和設(shè)備

有效

• 申請(qǐng)?zhí)枺?/td>: 201010106551.8
• 申請(qǐng)日: 2010-01-28
• 公開（公告）號(hào): CN102138768A
• 公開（公告）日: 2011-08-03
• 發(fā)明（設(shè)計(jì)）人: 宋章軍;張建偉;胡穎;張建中;劉會(huì)芬
• 申請(qǐng)（專利權(quán)）人: 深圳先進(jìn)技術(shù)研究院
• 主分類號(hào): A47L9/28
• 分類號(hào): A47L9/28;A47L11/40;G01C21/06;G01C21/08
• 代理公司: 廣州華進(jìn)聯(lián)合專利商標(biāo)代理有限公司 44224
• 代理人: 吳平

1.一種清潔機(jī)器人的定位方法，包括：

根據(jù)從編碼器獲取的數(shù)據(jù)計(jì)算出當(dāng)前時(shí)刻較之前一時(shí)刻的相對(duì)角度值和相對(duì)行走距離；

其特征在于，

從電子羅盤獲取當(dāng)前時(shí)刻的絕對(duì)方向和磁場(chǎng)強(qiáng)度；

若所述磁場(chǎng)強(qiáng)度在預(yù)設(shè)的強(qiáng)度范圍內(nèi)，將所述絕對(duì)方向和所述相對(duì)行走距離作為定位數(shù)據(jù)；否則，將所述相對(duì)角度值和所述相對(duì)行走距離作為定位數(shù)據(jù)。

2.根據(jù)權(quán)利要求1所述的清潔機(jī)器人的定位方法，其特征在于，還包括通過概率方法利用感知模型和所述定位數(shù)據(jù)估計(jì)當(dāng)前位姿的預(yù)測(cè)結(jié)果以及計(jì)算當(dāng)前位姿的最優(yōu)化結(jié)果。

3.根據(jù)權(quán)利要求2所述的清潔機(jī)器人的定位方法，其特征在于，所述當(dāng)前位姿的最優(yōu)化結(jié)果采用卡爾曼濾波按照以下步驟獲得：

利用前一時(shí)刻位姿的最優(yōu)化結(jié)果和系統(tǒng)模型估計(jì)當(dāng)前位姿的預(yù)測(cè)結(jié)果；

利用該當(dāng)前位姿的預(yù)測(cè)結(jié)果的協(xié)方差和所述感知模型的協(xié)方差，計(jì)算當(dāng)前卡爾曼增益；

利用該當(dāng)前卡爾曼增益、所述當(dāng)前位姿的預(yù)測(cè)結(jié)果、當(dāng)前時(shí)刻的所述定位數(shù)據(jù)，計(jì)算得到所述當(dāng)前位姿的最優(yōu)化結(jié)果。

4.根據(jù)權(quán)利要求3所述的清潔機(jī)器人的定位方法，其特征在于，所述當(dāng)前位姿的最優(yōu)化結(jié)果按照下面公式得到：

X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-H?X(k|k-1))

式中，

X(k|k)是所述當(dāng)前位姿的最優(yōu)化結(jié)果；

X(k|k-1)是所述當(dāng)前位姿的預(yù)測(cè)結(jié)果；

Kg(k)是所述當(dāng)前卡爾曼增益；

Z(k)是當(dāng)前時(shí)刻的所述定位數(shù)據(jù)；

H是測(cè)量系統(tǒng)的參數(shù)，對(duì)于多測(cè)量系統(tǒng)，H為矩陣。

5.根據(jù)權(quán)利要求4所述的清潔機(jī)器人的定位方法，其特征在于，所述當(dāng)前位姿的預(yù)測(cè)結(jié)果X(k|k-1)是按照下面公式得到：

X(k|k-1)＝A?X(k-1|k-1)+B?U(k)

式中，

A和B是系統(tǒng)參數(shù)，對(duì)于多模型系統(tǒng)，它們?yōu)榫仃嚕?/p>

X(k-1/k-1)是前一時(shí)刻最優(yōu)化結(jié)果；

U(k)是當(dāng)前時(shí)刻對(duì)系統(tǒng)的控制量。

6.根據(jù)權(quán)利要求5所述的清潔機(jī)器人的定位方法，其特征在于，所述當(dāng)前卡爾曼增益Kg(k)按照下面公式得到：

Kg(k)＝P(k|k-1)H′/(H?P(k|k-1)H′+R)

式中，

P(k/k-1)是所述當(dāng)前位姿的預(yù)測(cè)結(jié)果X(k|k-1)的協(xié)方差；

H′是H的轉(zhuǎn)置矩陣，

R是所述感知模型的協(xié)方差。

7.根據(jù)權(quán)利要求1所述的清潔機(jī)器人的定位方法，其特征在于，當(dāng)所述磁場(chǎng)強(qiáng)度與評(píng)估基數(shù)之間的差值超過預(yù)設(shè)的補(bǔ)償閾值，所述電子羅盤進(jìn)行補(bǔ)償，并將補(bǔ)償后首次采樣的磁場(chǎng)強(qiáng)度設(shè)定為下次補(bǔ)償?shù)脑u(píng)估基數(shù)。

8.一種清潔機(jī)器人的定位設(shè)備，包括：

編碼器，

與該編碼器連接的相對(duì)數(shù)據(jù)計(jì)算單元，根據(jù)從編碼器獲取的數(shù)據(jù)計(jì)算出當(dāng)前時(shí)刻較之前一時(shí)刻的相對(duì)角度值和相對(duì)行走距離；

其特征在于，

電子羅盤，輸出當(dāng)前時(shí)刻的絕對(duì)方向和磁場(chǎng)強(qiáng)度；

與該電子羅盤和所述相對(duì)數(shù)據(jù)計(jì)算單元連接的比較輸出單元，當(dāng)所述磁場(chǎng)強(qiáng)度在預(yù)設(shè)的強(qiáng)度范圍內(nèi)，將所述絕對(duì)方向和所述相對(duì)行走距離作為定位數(shù)據(jù)，否則，將所述相對(duì)角度值和所述相對(duì)行走距離作為定位數(shù)據(jù)。

9.根據(jù)權(quán)利要求8所述的清潔機(jī)器人的定位設(shè)備，其特征在于，還包括與所述比較輸出單元連接的位姿確定模塊，通過概率方法利用感知模型和所述定位數(shù)據(jù)估計(jì)當(dāng)前位姿的預(yù)測(cè)結(jié)果以及計(jì)算當(dāng)前位姿的最優(yōu)化結(jié)果。

10.根據(jù)權(quán)利要求9所述的清潔機(jī)器人的定位設(shè)備，其特征在于，所述位姿確定模塊包括：

預(yù)測(cè)單元，利用前一時(shí)刻位姿的最優(yōu)化結(jié)果和系統(tǒng)模型估計(jì)當(dāng)前位姿的預(yù)測(cè)結(jié)果；

權(quán)值單元，利用該當(dāng)前位姿的預(yù)測(cè)結(jié)果的協(xié)方差和感知模型的協(xié)方差，計(jì)算當(dāng)前卡爾曼增益；

優(yōu)化單元，利用該當(dāng)前卡爾曼增益、所述當(dāng)前位姿的預(yù)測(cè)結(jié)果、當(dāng)前時(shí)刻的所述定位數(shù)據(jù)，計(jì)算所述當(dāng)前位姿的最優(yōu)化結(jié)果。