[发明专利]基于GNSS/ODO的机器人双轮差速定位方法

摘要

本发明属于机器人技术领域，特别涉及一种机器人定位方法。基于GNSS/ODO的机器人双轮差速定位方法，步骤为：A.初始化机器人，利用GNSS获取初始时刻机器人的位姿信息；B.利用双轮里程计定位算法对机器人n+1时刻的全局位姿信息进行推算；C.判断当前时刻是否为设定的GNSS数据修正时刻，如果是，则执行D步骤；如果不是，则执行E步骤；D.根据步骤B中推算得到的n+1时刻的无人车位姿状态，进行卡尔曼滤波“预测‑修正”；E.仅进行D步骤中的卡尔曼滤波预测，而不做修正；F.输出滤波后的机器人的全局位姿信息。利用本发明，机器人的定位、定向精度有显著提高。

主权项

一种基于GNSS/ODO的机器人双轮差速定位方法，其特征在于，包括以下步骤：A.初始化机器人，利用GNSS获取初始时刻机器人的全局位姿信息；设当前时刻为n，该时刻机器人的全局位姿信息由[xg(n),yg(n),θg(n)]描述，其中，[xg(n),yg(n)]表示机器人在全局坐标系中的平面坐标位置，θg(n)表示机器人的航向角；B.依据n时刻机器人的全局位姿信息，根据公式(1.1)中双轮里程计该历元里的里程变化对机器人n+1时刻的全局位姿信息进行预测；θg(n+1)=θg(n)+dθgxg(n+1)=xg(n)+[dxl×cosθg(n)-dyl×sinθg(n)]yg(n+1)=yg(n)+[dxl×sinθg(n)+dyl×cosθg(n)]---(1.1)]]>其中：L1、L2分别表示双轮里程计测量到的机器人左、右轮每历元的里程变化量，K1、K2分别为机器人左、右轮编码器矫正系数；L为机器人两轮中心轮距；dθg为机器人在相邻两时刻转过的角度，(dxl,dyl)为机器人在本体坐标系下平面位置：dθg=L2×K2-L1×K1Ldxl=-L2×K2+L1×K12(L2×K2-L1×K1)×L×[1-cos(dθ)]dyl=L2×K2+L1×K12(L2×K2-L1×K1)×L×sin(dθ)---(1.2)]]>C.判断当前时刻是否为设定的GNSS数据修正时刻，如果是，则执行D步骤；如果不是，则执行E步骤；D.根据步骤B中预测得到的n+1时刻的全局位姿信息，进行卡尔曼滤波“预测‑修正”，卡尔曼组合滤波器设计如下：系统状态变量和系统方程分别如公式(1.3)和(1.4)所示，Xn＝[ΔX ΔY Δθ ΔK1 ΔK2 Δψ]T       (1.3)Xn+1＝ΦXn+wn     (1.4)式中：[ΔX,ΔY,Δθ]为步骤B中双轮里程计的位置和航向误差；ΔK1、ΔK2为左、右两轮标定因子误差；Δψ为安装误差；系统噪声为wT～N(0,Q)；系统状态转移矩阵Φ为：Φ=10F13F14F15Yn01F23F24F25-Xn001F34F350000100000010000001]]>其中，F13＝‑[dxl(n)·sinθg(n)+dyl(n)·cosθg(n)]F14＝L1/2·[L1·K1·cosθg(n)/L‑sinθg(n)]F15＝‑L2/2·[L2·K2·cosθg(n)/L+sinθg(n)]F23＝dxl·cosθg(n)‑dyl·sinθg(n)F24＝L1/2·[L1·K1·sinθg(n)/L+cosθg(n)]F25＝‑L2/2·[L2·K2·sinθg(n)/L‑cosθg(n)]F34＝‑L1/LF35＝L2/L以双轮里程计差速解算的位置信息[xg(n+1),yg(n+1)]与GNSS差分定位输出的位置信息[xgnss(n+1),ygnss(n+1)]作为卡尔曼滤波器的量测信息，此时的量测方程为：Z(t)=x-xgnssy-ygnss=Hxy·X(t)+ϵ(t)]]>式中，观测方程ε(t)为系统观测噪声矢量，为N(0,R)的高斯白噪声过程；并转到F步骤；E.仅进行D步骤中的卡尔曼滤波预测，而不做修正，并转到F步骤；F.输出滤波后的机器人的全局位姿信息；I.令n＝n+1，转到步骤B，进行下一循环。