一种基于惯性和二维码导航的机器人初始对准方法技术

本发明专利技术涉及一种基于惯性和二维码导航的机器人初始对准方法，其通过粗对准、精对准、平移位置对准的方法，解决了机器人惯导系统在室内平面的初值对准存在的对准时间长和控制算法复杂的问题，且通过结合惯性传感器和车体与二位码的角度进行Kalman融合，使得对准时间能够减少，计算复杂度低。

An Initial Alignment Method for Robots Based on Inertia and Two-Dimensional Code Navigation

The present invention relates to an initial alignment method for robots based on inertial navigation and two-dimensional code navigation. By means of rough alignment, precise alignment and translation alignment, it solves the problem of long alignment time and complex control algorithm in initial alignment of robot inertial navigation system on indoor plane, and makes Kalman fusion by combining the angle of inertial sensor and vehicle body and binary code. The alignment time can be reduced and the computational complexity is low.

【技术实现步骤摘要】
一种基于惯性和二维码导航的机器人初始对准方法
本专利技术涉及导航对准领域，具体涉及一种基于惯性和二维码导航的机器人初始对准方法。
技术介绍
在制造生产线中，机器人能高效、准确、灵活地完成物料的搬运任务，大大提高了生产的柔性和企业的竞争力。惯性导航搬运机器人具有准确性高、灵活性强、便于组合和兼容、不依赖外部信息、工作不受气象条件和人为干扰影响适用领域广，但是随着长时间的运行，惯性传感器存在严重的漂移。因此，本专利采用惯性-二维码导航的组合导航方式。在惯性-二维码组合导航系统中，初始对准是指提供捷联惯导的导航解算所需要的初值，包括初始位置、初始速度与初始姿态。由于惯导系统的初值误差在以后的导航解算中有积累作用，因此必须保证初始对准误差在一定范围内，特别是初始姿态误差，必须严格限制。现有的惯导系统的初始对准方法有：罗经回路法、卡尔曼滤波法以及粒子滤波法等。当基座存在明显晃动或者在运动环境下，陀螺输出的信噪比低，难以从陀螺输出中提取地球自转角速度信息，解析对准误差很大，甚至不可用。为抑制基座晃动干扰及惯性器件误差，较早出现的是基于经典控制理论的罗经对准，随着对初始对准研究的深入，对准方案也由原来的基于经典控制理论的频域法，转移到基于现代控制理论的最优估计方法，卡尔曼滤波算法是最优估计方法在初始对准中应用的核心算法，由于目前卡尔曼滤波等算法在运行前一般要求给定粗略的初始姿态角，对准过程因此被分为粗对准和精对准两个阶段。粗对准阶段获得粗略的初始姿态角，精对准阶段运用卡尔曼滤波等算法获得更高精度的姿态。当初始姿态误差角为小角度时，构成卡尔曼算法状态方程的SINS误差方程近似为线性方程，目前基于线性方程的初始对准技术已经比较成熟。但是，这些方法在实际应用中受到室内环境、计算复杂度等影响。
技术实现思路
(一)要解决的技术问题基于现有技术的缺陷，为解决机器人惯导系统在室内平面的初值对准存在的对准时间长和控制算法复杂的问题，本专利技术提出了一种基于惯性和二维码导航的机器人初始对准方法，其适用于新进入路径或故障恢复的AGV小车快速的初始对准工作。(二)技术方案为了达到上述目的，本专利技术提供了一种基于惯性和二维码导航的机器人初始对准方法，所述初始对准方法包括如下步骤：S1：角度粗对准：对捷联惯导系统和二维码扫码枪进行初始化，根据扫码枪读取搬运机器人与二维码的角θ，对搬运机器人进行快速的粗对准，使达到目标角度θ0的粗略误差范围；S2：角度精对准：通过读取搬运机器人与二维码的角度θ和陀螺仪的角速度Gyro，其中陀螺仪的最初角速度为粗对准结束时刻小车的角速度，运用Kalman滤波进行精对准，使达到目标精度范围；S3：角度对准后通过平移进行位置对准，使位置达到精度范围，初始对准结束；其中，步骤S2还包括了以下内容：使用Kalman滤波算法进行解算和迭代，记机器人的系统模型为：其中，X(k)是k时刻系统的状态向量，Z(k)是k时刻的测量向量，U(k)是k时刻输入向量，A、B、H分别是系统状态转移矩阵、输入控制矩阵和测量矩阵，W(k)和V(k)分别是过程噪声和测量噪声，W(k)和V(k)是两个互不相关的高斯白噪声序列；且满足：其中，q(k)和r(k)为高斯白噪声均值参数，Q(k)和R(k)为方差阵参数，E[W(k)]为W(k)的期望函数，其均为k时刻的函数；在单个信息融合周期内的解算步骤如下：(1)状态预测X(k|k-1)＝AX(k-1)+BU(k-1)(4)(2)新息序列更新Zk＝Zk-HX(k|k-1)(5)这里的新息Zk即测量值和预测值的差值，即由最近时刻的测量值Zk作为携带关于状态的新信息来修正状态预测值X(k|k-1)；(3)状态预测均方差矩阵更新P(k|k-1)＝AP(k-1)AT+Q(k)(6)(4)滤波增益更新Kalman滤波增益Kk是在状态估计均方差最小的准则下求解的，其值由最初均方误差P(0)、状态预测均方差P(k|k-1)及测量噪声R(k)确定；(5)线性加权平均状态估计X(k|k)＝X(k|k-1)+KkZk(8)(6)状态估计均方差误差更新P(k|k)＝(I-KkH)P(k|k-1)(9)均方差P(k|k)是滤波器的重要组成部分，代表了估计的精度和可靠性；考虑到机器人基本在水平面运动，因此，只需要对航向角进行调控，于是，将扫码枪测量得到的机器人和二维码的角度θ作为测量值进行Kalman滤波精对准，其系统状态方程和测量方程如下：其中dt为实际运行周期，Gyro为陀螺仪测的角速度的值，Angle为角度，Qbias为陀螺仪漂移，wangle为角度噪声，wgyro为陀螺仪噪声；其中z(k)为扫码枪测量角度值，Vangle为角度测量噪声；根据多次工程实践，设置初始值为：Qbias(0)＝0，wangle、wgyro和Vangle取为常数。进一步的，所述步骤S2中还包括如下具体的Kalman滤波迭代步骤：1)一步预测当前车身角度Angle(k|k-1)＝Angle(k-1|k-1)+(Gyro(k-1)-Q_bias(k-1))*dt(12)2)协方差预测P(k|k-1)＝AP(k-1|k-1)A′+Q(13)其中初始协方差矩阵P(0)＝[[1，0]，[0，1]]，具体设置3)滤波增益更新其中k0为角度增益，k1为角度漂移增益，具体设置H＝[10]，Vangle为常值；由式(13)和(14)可以计算得到k0和k1；4)新息序列更新Angel_err＝z(k)-Angle(k|k-1)(15)其中Angel_err为测量角度z(k)与估计角度Angle(k|k-1)的差值；Angle(k|k)＝Angle(k|k-1)-k0*Angel_err(16)Q_bias(k)＝Q_bias(k-1)-k0*Angel_err(17)由式(16)和式(17)可以求得Kalman滤波角度和角度漂移值；5)新协方差矩阵更新P(k|k)＝(I-KkH)P(k|k-1)(18)迭代直到角度误差达到期望精度范围，迭代停止。进一步的，所述步骤S1还包括了如下角度粗对准的实施过程：根据扫码枪得到机器人初始位置(x，y)和二维码的角度θ，首先给定目标角度θ0的值，可以得到角度误差Δθ：Δθ＝θ0-θ(1)当Δθ≠0时，控制机器人原地旋转，然后将Δθ通过位置式PD控制算法得到舵机A轮和B轮速度v，对舵机速度进行控制，直到机器人和二维码的角度θ在粗略误差范围内，则粗对准进行完毕。进一步的，所述步骤S3还包括了如下位置对准的过程实施：根据扫码枪扫二维码得到小车目标位置(x0，y0)和当前位置(x，y)，当|y0-y|＞0.01时，如果y0-y＞0，给定舵轮速度v＝0.02m/s；如果y0-y＜0，给定舵轮速度v＝-0.02m/s；直到|y0-y|＜0.01，使机器人的前后轮打角0°并使定舵轮速度v＝0；当|x0-x|＞0.01时，如果x0-x＞0，给定舵轮速度v＝0.02m/s，如果x0-x＜0，给定舵轮速度v＝-0.02m/s，直到|X0-x|＜0.01，则给定定舵轮速度v＝0，位置对准结束。(三)有益效果由上述技术方案可知，本专利技术提出的一种基于惯性和二维码导航的机器人初始对准方法，其有益效果在于提出了一种运用二维码和惯性传感器进行初始对准的方法，创新本文档来自技高网...

【技术保护点】
1.一种基于惯性和二维码导航的机器人初始对准方法，其特征在于，所述初始对准方法包括如下步骤：S1：角度粗对准：对捷联惯导系统和二维码扫码枪进行初始化，根据扫码枪读取搬运机器人与二维码的角θ，对搬运机器人进行快速的粗对准，使达到目标角度θ0的粗略误差范围；S2：角度精对准：通过读取搬运机器人与二维码的角度θ和陀螺仪的角速度Gyro，其中陀螺仪的最初角速度为粗对准结束时刻小车的角速度，运用Kalman滤波进行精对准，使达到目标精度范围；步骤S2还包括了以下内容：使用Kalman滤波算法进行解算和迭代，记机器人的系统模型为：

【技术特征摘要】
1.一种基于惯性和二维码导航的机器人初始对准方法，其特征在于，所述初始对准方法包括如下步骤：S1：角度粗对准：对捷联惯导系统和二维码扫码枪进行初始化，根据扫码枪读取搬运机器人与二维码的角θ，对搬运机器人进行快速的粗对准，使达到目标角度θ0的粗略误差范围；S2：角度精对准：通过读取搬运机器人与二维码的角度θ和陀螺仪的角速度Gyro，其中陀螺仪的最初角速度为粗对准结束时刻小车的角速度，运用Kalman滤波进行精对准，使达到目标精度范围；步骤S2还包括了以下内容：使用Kalman滤波算法进行解算和迭代，记机器人的系统模型为：其中，X(k)是k时刻系统的状态向量，Z(k)是k时刻的测量向量，U(k)是k时刻输入向量，A、B、H分别是系统状态转移矩阵、输入控制矩阵和测量矩阵，W(k)和V(k)分别是过程噪声和测量噪声，W(k)和V(k)是两个互不相关的高斯白噪声序列；且满足：其中，q(k)和r(k)为高斯白噪声均值参数，Q(k)和R(k)为方差阵参数，E[W(k)]为W(k)的期望函数，其均为k时刻的函数；在单个信息融合周期内的解算步骤如下：(1)状态预测X(k|k-1)＝AX(k-1)+BU(k-1)(4)(2)新息序列更新Zk＝Zk-HX(k|k-1)(5)这里的新息Zk即测量值和预测值的差值，即由最近时刻的测量值Zk作为携带关于状态的新信息来修正状态预测值X(k|k-1)；(3)状态预测均方差矩阵更新P(k|k-1)＝AP(k-1)AT+Q(k)(6)(4)滤波增益更新Kalman滤波增益Kk是在状态估计均方差最小的准则下求解的，其值由最初均方误差P(0)、状态预测均方差P(k|k-1)及测量噪声R(k)确定；(5)线性加权平均状态估计X(k|k)＝X(k|k-1)+KkZk(8)(6)状态估计均方差误差更新P(k|k)＝(I-KkH)P(k|k-1)(9)均方差P(k|k)是滤波器的重要组成部分，代表了估计的精度和可靠性；考虑到机器人基本在水平面运动，因此，只需要对航向角进行调控，于是，将扫码枪测量得到的机器人和二维码的角度θ作为测量值进行Kalman滤波精对准，其系统状态方程和测量方程如下：其中dt为实际运行周期，Gyro为陀螺仪测的角速度的值，Angle为角度，Qbias为陀螺仪漂移，wangle为角度噪声，wgyro为陀螺仪噪声；其中z(k)为扫码枪测量角度值，vangle为角度测量噪声；根据多次工程实践，设置初始值为：Qbias(0)＝0，wang...

【专利技术属性】
技术研发人员：王强，陈宇，龙小军，黄科科，崔冠冠，周正，
申请(专利权)人：楚天智能机器人长沙有限公司，
类型：发明
国别省市：湖南,43