主权项 |
一种基于惯性的低成本机器人定位方法,其特征在于,包括下述步骤:步骤一:数据输入需要输入的数据包括:机器人的初始全局位姿,包括初始的横纵坐标,和初始航向距离;该三个参数由里程计输出;里程计输出还输出航向角增量ψ<sub>k</sub>;陀螺的比例系数N,k时刻陀螺输出的秒脉冲数M<sub>k</sub>,0时刻陀螺输出的秒脉冲数M<sub>0</sub>,k时刻与0时刻的时间差t,陀螺漂移ζ;步骤二:计算用下述公式计算<maths num="0001" id="cmaths0001"><math><![CDATA[<mrow><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>x</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><msub><mi>y</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><msub><mi>Ψ</mi><mi>k</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><mrow><msub><mi>x</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>+</mo><msub><mi>δ</mi><mi>k</mi></msub><msub><mi>cosΨ</mi><mi>k</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>y</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>+</mo><msub><mi>δ</mi><mi>k</mi></msub><msub><mi>sinΨ</mi><mi>k</mi></msub></mrow></mtd></mtr><mtr><mtd><mrow><msub><mi>Ψ</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>+</mo><msub><mi>ψ</mi><mi>k</mi></msub></mrow></mtd></mtr></mtable></mfenced></mrow>]]></math><img file="FDA0000834986130000011.GIF" wi="516" he="231" /></maths>其中(x<sub>k</sub>,y<sub>k</sub>,Ψ<sub>k</sub>)<sup>T</sup>为机器人的全局位姿,其中x<sub>k</sub>,y<sub>k</sub>为横纵坐标,Ψ<sub>k</sub>为航向距离,ψ<sub>k</sub>为航向角增量,角标k为时刻,δ<sub>k</sub>为机器人在k‑1至k时刻移动的距离,其中Ψ<sub>k</sub>=Ψ<sub>k‑1</sub>+ψ<sub>k</sub>=Ψ<sub>k‑1</sub>+N·(M<sub>k</sub>‑M<sub>0</sub>)·t+ζ·t其中,N为陀螺的比例系数,M<sub>k</sub>为k时刻陀螺输出的秒脉冲数,M<sub>0</sub>为0时刻陀螺输出的秒脉冲数,t为k时刻与0时刻的时间差,ζ为陀螺漂移,用公式计算出k时刻机器人的全局位姿,步骤三:循环在每一时刻重复执行步骤二,得到相应时刻的机器人全局位姿。。 |