[發(fā)明專利]一種機(jī)器人傾倒檢測(cè)的方法、系統(tǒng)、存儲(chǔ)介質(zhì)和設(shè)備

在審

• 申請(qǐng)?zhí)枺?/td>: 201810067863.9
• 申請(qǐng)日: 2018-01-24
• 公開（公告）號(hào): CN108225271A
• 公開（公告）日: 2018-06-29
• 發(fā)明（設(shè)計(jì)）人: 李庭亮
• 申請(qǐng)（專利權(quán)）人: 南京阿凡達(dá)機(jī)器人科技有限公司
• 主分類號(hào): G01C9/00
• 分類號(hào): G01C9/00
• 代理公司: 上海碩力知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 31251
• 代理人: 郭桂峰

1.一種機(jī)器人傾倒檢測(cè)的方法，其特征在于，包括：

步驟S100連續(xù)獲取陀螺儀測(cè)量數(shù)據(jù)和加速度儀測(cè)量數(shù)據(jù)，將陀螺儀測(cè)量數(shù)據(jù)作為k時(shí)刻控制量，將加速度儀測(cè)量數(shù)據(jù)作為k時(shí)刻觀測(cè)角度，k是大于等于1的整數(shù)；

步驟S200根據(jù)卡爾曼濾波原理，將所述k時(shí)刻控制量和所述k時(shí)刻觀測(cè)角度以及k-1時(shí)刻的機(jī)器人與地面的夾角進(jìn)行融合，獲取k時(shí)刻機(jī)器人與地面的夾角；

步驟S300若所述k時(shí)刻機(jī)器人與地面的夾角超出預(yù)設(shè)范圍，判斷所述機(jī)器人傾倒。

2.根據(jù)權(quán)利要求1所述的機(jī)器人傾倒檢測(cè)方法，其特征在于，在所述步驟S200根據(jù)卡爾曼濾波原理，將所述k時(shí)刻控制量和所述k時(shí)刻觀測(cè)角度以及k-1時(shí)刻的機(jī)器人與地面的夾角進(jìn)行融合，獲取k時(shí)刻機(jī)器人與地面的夾角，包括：

步驟S210獲取卡爾曼算法中的參數(shù)，所述參數(shù)包括：狀態(tài)矩陣A、控制矩陣B和測(cè)量矩陣H，單位矩陣I；

步驟S220根據(jù)所述k-1時(shí)刻機(jī)器人與地面的夾角X(k-1|k-1)、所述k時(shí)刻控制量以及所述狀態(tài)矩陣A、所述控制矩陣B，計(jì)算k時(shí)刻估測(cè)角度X(k|k-1)；

步驟S230根據(jù)所述k-1時(shí)刻機(jī)器人與地面的夾角X(k-1|k-1)的協(xié)方差P(k-1|k-1)、所述狀態(tài)矩陣A，計(jì)算所述k時(shí)刻估測(cè)角度X(k|k-1)的協(xié)方差P(k|k-1)；

步驟S240通過所述k時(shí)刻估測(cè)角度X(k|k-1)的協(xié)方差P(k|k-1)和所述測(cè)量矩陣H，獲取k時(shí)刻卡爾曼增益Kg(k)；

步驟S250通過所述k-1時(shí)刻估測(cè)角度X(k|k-1)、k時(shí)刻觀測(cè)角度Z(k)、所述k時(shí)刻卡爾曼增益Kg(k)和所述測(cè)量矩陣H，獲取k時(shí)刻最優(yōu)估測(cè)角度X(k|k)，所述X(k|k)為k時(shí)刻機(jī)器人與地面的夾角；

步驟S260通過所述k-1時(shí)刻估測(cè)角度的協(xié)方差P(k|k-1)、所述k時(shí)刻卡爾曼增益Kg(k)和所述測(cè)量矩陣H、所述單位矩陣I，更新k時(shí)刻最優(yōu)估測(cè)角度的協(xié)方差P(k|k)。

3.根據(jù)權(quán)利要求4所述的機(jī)器人傾倒檢測(cè)方法，其特征在于，所述步驟S210獲取卡爾曼算法中的參數(shù)，所述參數(shù)包括：系統(tǒng)的狀態(tài)矩陣A、控制矩陣B和測(cè)量矩陣H、單位矩陣I，具體包括：

獲取所述狀態(tài)矩陣所述控制矩陣所述測(cè)量矩陣H＝|1 0|；

I為單位矩陣

4.根據(jù)權(quán)利要求1所述的機(jī)器人傾倒檢測(cè)方法，其特征在于，步驟S300之后進(jìn)一步包括：當(dāng)所述機(jī)器人傾倒時(shí)，控制所述機(jī)器人智能響應(yīng)，所述響應(yīng)包括語音提示用戶停止運(yùn)動(dòng)或關(guān)閉系統(tǒng)電源。

5.一種基于機(jī)器人傾倒的檢測(cè)系統(tǒng)，其特征在于，應(yīng)用于機(jī)器人進(jìn)行傾倒檢測(cè)，所述基于機(jī)器人傾倒的檢測(cè)系統(tǒng)包括：

角度采集模塊，用于采集陀螺儀測(cè)量數(shù)據(jù)和加速度儀測(cè)量數(shù)據(jù)，并將所述采集數(shù)據(jù)發(fā)送至角度計(jì)算模塊；

角度計(jì)算模塊，與所述角度采集模塊連接，用于將所述陀螺儀測(cè)量數(shù)據(jù)作為k時(shí)刻控制量，將所述加速度儀測(cè)量數(shù)據(jù)作為k時(shí)刻觀測(cè)角度，k是大于等于1的整數(shù)，以及根據(jù)卡爾曼濾波原理，將所述k時(shí)刻控制量和所述k時(shí)刻觀測(cè)角度以及k-1時(shí)刻的機(jī)器人與地面的夾角進(jìn)行融合，計(jì)算k時(shí)刻機(jī)器人與地面的夾角；

傾倒判斷模塊，與所述角度計(jì)算模塊連接，用于當(dāng)所述機(jī)器人與地面的夾角超出預(yù)設(shè)范圍，判斷所述機(jī)器人傾倒。