CN105258701A - 一种基于惯性的低成本机器人定位方法 - Google Patents

Abstract

本发明属于定位方法，具体涉及一种基于惯性的低成本机器人定位方法。它包括：步骤一：数据输入；步骤二：计算，计算出k时刻机器人的全局位姿，步骤三：循环，在每一时刻重复执行步骤二，得到相应时刻的机器人全局位姿。本发明的显著效果是：本发明仅依靠里程计和光纤陀螺仪的数据就可以完成定位，不需要依赖成本很高的GPS模块，且定位精度较好，有效降低了机器人定位部分的成本。

Description

一种基于惯性的低成本机器人定位方法

技术领域

本发明属于定位方法，具体涉及一种基于惯性的低成本机器人定位方法。

背景技术

智能移动机器人是一类能够通过传感器感知环境和自身状态，实现在有障碍物的环境中面向目标的自主导航运动，从而完成预定任务的机器人系统。要实现机器人自主导航运动，必须要解决环境建模、实时定位、路径规划、运动控制等一系列问题。

其中，定位移动机器人必须具有定位的能力，其目的就是确定机器人在运行环境中相对于世界坐标系的位置及航向。

现有的定位方法基本都来自于卫星、飞机、汽车等大型装置，成本较高。机器人系统，尤其是小型智能机器人，本身体积就较小，综合考虑成本等因素后，不适合直接移植现有的定位方法。

发明内容

本发明针对现有技术缺陷，提供一种基于惯性的低成本机器人定位方法。

本发明是这样实现的：一种基于惯性的低成本机器人定位方法，其特征在于，包括下述步骤：

步骤一：数据输入

需要输入的数据包括：

机器人的初始全局位姿，包括初始的横纵坐标，和初始航向距离；该三个参数由里程计输出；里程计输出还输出航向角增量ψ_k；

陀螺的比例系数N，k时刻陀螺输出的秒脉冲数M_k，0时刻陀螺输出的秒脉冲数M₀，k时刻与0时刻的时间差t，陀螺漂移ζ；

步骤二：计算

用下述公式计算

$\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\Ψ}}_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}+{\mathrm{\δ}}_{k}cos{\mathrm{\Ψ}}_k\\ y_{k-1}+{\mathrm{\δ}}_k{\mathrm{sin\Ψ}}_k\\ {\mathrm{\Ψ}}_{k-1}+{\mathrm{\ψ}}_k\end{array}\right]$

其中(x_k,y_k,Ψ_k)^T为机器人的全局位姿，其中x_k,y_k为横纵坐标，Ψ_k为航向距离，ψ_k为航向角增量，角标k为时刻，δ_k为机器人在k-1至k时刻移动的距离，

其中Ψ_k＝Ψ_k-1+ψ_k＝Ψ_k-1+N·(M_k-M₀)·t+ζ·t

其中，N为陀螺的比例系数，M_k为k时刻陀螺输出的秒脉冲数，M₀为0时刻陀螺输出的秒脉冲数，t为k时刻与0时刻的时间差，ζ为陀螺漂移，

用公式计算出k时刻机器人的全局位姿，

步骤三：循环

在每一时刻重复执行步骤二，得到相应时刻的机器人全局位姿。

本发明的显著效果是：本发明仅依靠里程计和光纤陀螺仪的数据就可以完成定位，不需要依赖成本很高的GPS模块，且定位精度较好，有效降低了机器人定位部分的成本。

具体实施方式

一种基于惯性的低成本机器人定位方法，包括下述步骤：

步骤一：数据输入

需要输入的数据包括：

机器人的初始全局位姿，包括初始的横纵坐标，和初始航向距离；该三个参数由里程计输出；里程计输出还输出航向角增量ψ_k；

陀螺的比例系数N，k时刻陀螺输出的秒脉冲数M_k，0时刻陀螺输出的秒脉冲数M₀，k时刻与0时刻的时间差t，陀螺漂移ζ；

步骤二：计算

用下述公式计算

$\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\Ψ}}_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}+{\mathrm{\δ}}_{k}cos{\mathrm{\Ψ}}_k\\ y_{k-1}+{\mathrm{\δ}}_k{\mathrm{sin\Ψ}}_k\\ {\mathrm{\Ψ}}_{k-1}+{\mathrm{\ψ}}_k\end{array}\right]$

其中(x_k,y_k,Ψ_k)^T为机器人的全局位姿，其中x_k,y_k为横纵坐标，Ψ_k为航向距离，ψ_k为航向角增量(该参数由里程计输出)，角标k为时刻，δ_k为机器人在k-1至k时刻移动的距离。

其中Ψ_k＝Ψ_k-1+ψ_k＝Ψ_k-1+N·(M_k-M₀)·t+ζ·t

其中，N为陀螺的比例系数，M_k为k时刻陀螺输出的秒脉冲数，M₀为0时刻陀螺输出的秒脉冲数，t为k时刻与0时刻的时间差，ζ为陀螺漂移。

用公式计算出k时刻机器人的全局位姿。

步骤三：循环

在每一时刻重复执行步骤二，得到相应时刻的机器人全局位姿。

Claims (1)

1.一种基于惯性的低成本机器人定位方法，其特征在于，包括下述步骤：

步骤一：数据输入

需要输入的数据包括：

机器人的初始全局位姿，包括初始的横纵坐标，和初始航向距离；该三个参数由里程计输出；里程计输出还输出航向角增量ψ_k；

陀螺的比例系数N，k时刻陀螺输出的秒脉冲数M_k，0时刻陀螺输出的秒脉冲数M₀，k时刻与0时刻的时间差t，陀螺漂移ζ；

步骤二：计算

用下述公式计算

$\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\Ψ}}_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}+{\mathrm{\δ}}_k{\mathrm{cos\Ψ}}_k\\ y_{k-1}+{\mathrm{\δ}}_k{\mathrm{sin\Ψ}}_k\\ {\mathrm{\Ψ}}_{k-1}+{\mathrm{\ψ}}_k\end{array}\right]$

其中(x_k,y_k,Ψ_k)^T为机器人的全局位姿，其中x_k,y_k为横纵坐标，Ψ_k为航向距离，ψ_k为航向角增量，角标k为时刻，δ_k为机器人在k-1至k时刻移动的距离，

其中Ψ_k＝Ψ_k-1+ψ_k＝Ψ_k-1+N·(M_k-M₀)·t+ζ·t

其中，N为陀螺的比例系数，M_k为k时刻陀螺输出的秒脉冲数，M₀为0时刻陀螺输出的秒脉冲数，t为k时刻与0时刻的时间差，ζ为陀螺漂移，

用公式计算出k时刻机器人的全局位姿，

步骤三：循环

在每一时刻重复执行步骤二，得到相应时刻的机器人全局位姿。。