一種基于慣性的低成本機(jī)器人定位方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于定位方法，具體涉及一種基于慣性的低成本機(jī)器人定位方法。
【背景技術(shù)】
[0002] 智能移動(dòng)機(jī)器人是一類能夠通過傳感器感知環(huán)境和自身狀態(tài)，實(shí)現(xiàn)在有障礙物的 環(huán)境中面向目標(biāo)的自主導(dǎo)航運(yùn)動(dòng)，從而完成預(yù)定任務(wù)的機(jī)器人系統(tǒng)。要實(shí)現(xiàn)機(jī)器人自主導(dǎo) 航運(yùn)動(dòng)，必須要解決環(huán)境建模、實(shí)時(shí)定位、路徑規(guī)劃、運(yùn)動(dòng)控制等一系列問題。
[0003] 其中，定位移動(dòng)機(jī)器人必須具有定位的能力，其目的就是確定機(jī)器人在運(yùn)行環(huán)境 中相對(duì)于世界坐標(biāo)系的位置及航向。
[0004] 現(xiàn)有的定位方法基本都來自于衛(wèi)星、飛機(jī)、汽車等大型裝置，成本較高。機(jī)器人系 統(tǒng)，尤其是小型智能機(jī)器人，本身體積就較小，綜合考慮成本等因素后，不適合直接移植現(xiàn) 有的定位方法。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明針對(duì)現(xiàn)有技術(shù)缺陷，提供一種基于慣性的低成本機(jī)器人定位方法。
[0006] 本發(fā)明是這樣實(shí)現(xiàn)的：一種基于慣性的低成本機(jī)器人定位方法，其特征在于，包括 下述步驟：
[0007] 步驟一：數(shù)據(jù)輸入
[0008] 需要輸入的數(shù)據(jù)包括：
[0009] 機(jī)器人的初始全局位姿，包括初始的橫縱坐標(biāo)，和初始航向距離；該三個(gè)參數(shù)由里 程計(jì)輸出；里程計(jì)輸出還輸出航向角增量ιΚ;
[0010] 陀螺的比例系數(shù)N，k時(shí)刻陀螺輸出的秒脈沖數(shù)Mk，0時(shí)刻陀螺輸出的秒脈沖數(shù)M。， k時(shí)刻與0時(shí)刻的時(shí)間差t，陀螺漂移ζ ;
[0011] 步驟二：計(jì)算
[0012] 用下述公式計(jì)算
[0014] 其中（xk，yk，Wk) T為機(jī)器人的全局位姿，其中xk，yk為橫縱坐標(biāo)，Ψ k為航向距離， t為航向角增量，角標(biāo)k為時(shí)刻，δ k為機(jī)器人在k-Ι至k時(shí)刻移動(dòng)的距離，
[0015] 其中 Wk= Ψ k !+iK= Ψ k !+N · (Mk-M0) · t+ζ · t
[0016] 其中，N為陀螺的比例系數(shù)，k時(shí)刻陀螺輸出的秒脈沖數(shù)，M。為0時(shí)刻陀螺輸 出的秒脈沖數(shù)，t為k時(shí)刻與0時(shí)刻的時(shí)間差，ζ為陀螺漂移，
[0017] 用公式計(jì)算出k時(shí)刻機(jī)器人的全局位姿，
[0018] 步驟三：循環(huán)
[0019] 在每一時(shí)刻重復(fù)執(zhí)行步驟二，得到相應(yīng)時(shí)刻的機(jī)器人全局位姿。
[0020] 本發(fā)明的顯著效果是：本發(fā)明僅依靠里程計(jì)和光纖陀螺儀的數(shù)據(jù)就可以完成定 位，不需要依賴成本很高的GPS模塊，且定位精度較好，有效降低了機(jī)器人定位部分的成 本。
【具體實(shí)施方式】
[0021] 一種基于慣性的低成本機(jī)器人定位方法，包括下述步驟：
[0022] 步驟一：數(shù)據(jù)輸入
[0023] 需要輸入的數(shù)據(jù)包括：
[0024] 機(jī)器人的初始全局位姿，包括初始的橫縱坐標(biāo)，和初始航向距離；該三個(gè)參數(shù)由里 程計(jì)輸出；里程計(jì)輸出還輸出航向角增量ιΚ;
[0025] 陀螺的比例系數(shù)N，k時(shí)刻陀螺輸出的秒脈沖數(shù)Mk，0時(shí)刻陀螺輸出的秒脈沖數(shù)M。， k時(shí)刻與0時(shí)刻的時(shí)間差t，陀螺漂移ζ ;
[0026] 步驟二：計(jì)算
[0027] 用下述公式計(jì)算
[0029] 其中（xk，yk，Wk) T為機(jī)器人的全局位姿，其中xk，yk為橫縱坐標(biāo)，Ψ k為航向距離， t為航向角增量（該參數(shù)由里程計(jì)輸出），角標(biāo)k為時(shí)刻，δ k為機(jī)器人在k-Ι至k時(shí)刻移 動(dòng)的距離。
[0030] 其中 Wk= Ψ k !+ Φ k= Ψ k !+N · (Mk-M。）· t+ ζ · t
[0031] 其中，N為陀螺的比例系數(shù)，MkS k時(shí)刻陀螺輸出的秒脈沖數(shù)，M。為0時(shí)刻陀螺輸 出的秒脈沖數(shù)，t為k時(shí)刻與0時(shí)刻的時(shí)間差，ζ為陀螺漂移。
[0032] 用公式計(jì)算出k時(shí)刻機(jī)器人的全局位姿。
[0033] 步驟三：循環(huán)
[0034] 在每一時(shí)刻重復(fù)執(zhí)行步驟二，得到相應(yīng)時(shí)刻的機(jī)器人全局位姿。
【主權(quán)項(xiàng)】
1. 一種基于慣性的低成本機(jī)器人定位方法，其特征在于，包括下述步驟： 步驟一：數(shù)據(jù)輸入 需要輸入的數(shù)據(jù)包括： 機(jī)器人的初始全局位姿，包括初始的橫縱坐標(biāo)，和初始航向距離；該三個(gè)參數(shù)由里程計(jì) 輸出；里程計(jì)輸出還輸出航向角增量ιΚ; 陀螺的比例系數(shù)N，k時(shí)刻陀螺輸出的秒脈沖數(shù)Mk，0時(shí)刻陀螺輸出的秒脈沖數(shù)M。，k時(shí) 刻與〇時(shí)刻的時(shí)間差t，陀螺漂移ζ; 步驟二：計(jì)算 用下述公式計(jì)算其中（xk，yk，ψι<)τ為機(jī)器人的全局位姿，其中Xk，yk為橫縱坐標(biāo)，ψk為航向距離，Φk為 航向角增量，角標(biāo)k為時(shí)刻，δk為機(jī)器人在k-i至k時(shí)刻移動(dòng)的距離， 其中 Ψ??=ψkι+φk=ψkι+Ν · (Mk-M。）· t+ζ·t 其中，N為陀螺的比例系數(shù)，Mksk時(shí)刻陀螺輸出的秒脈沖數(shù)，M。為0時(shí)刻陀螺輸出的 秒脈沖數(shù)，t為k時(shí)刻與0時(shí)刻的時(shí)間差，ζ為陀螺漂移， 用公式計(jì)算出k時(shí)刻機(jī)器人的全局位姿， 步驟三：循環(huán) 在每一時(shí)刻重復(fù)執(zhí)行步驟二，得到相應(yīng)時(shí)刻的機(jī)器人全局位姿。。
【專利摘要】本發(fā)明屬于定位方法，具體涉及一種基于慣性的低成本機(jī)器人定位方法。它包括：步驟一：數(shù)據(jù)輸入；步驟二：計(jì)算，計(jì)算出k時(shí)刻機(jī)器人的全局位姿，步驟三：循環(huán)，在每一時(shí)刻重復(fù)執(zhí)行步驟二，得到相應(yīng)時(shí)刻的機(jī)器人全局位姿。本發(fā)明的顯著效果是：本發(fā)明僅依靠里程計(jì)和光纖陀螺儀的數(shù)據(jù)就可以完成定位，不需要依賴成本很高的GPS模塊，且定位精度較好，有效降低了機(jī)器人定位部分的成本。
【IPC分類】G01C21/16
【公開號(hào)】CN105258701
【申請(qǐng)?zhí)枴緾N201510726457
【發(fā)明人】王芳, 呂博, 呂翀, 季冉鳴
【申請(qǐng)人】北京自動(dòng)化控制設(shè)備研究所
【公開日】2016年1月20日
【申請(qǐng)日】2015年10月30日