基于GNSS/ODO的机器人双轮差速定位方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN107219542A(43)申请公布日2017.09.29(21)申请号201710245625.8(22)申请日2017.04.14(71)申请人北京克路德人工智能科技有限公司地址100089北京市海淀区长春桥路11号4号楼17层1703室(72)发明人李耀宗韩锐王坤(74)专利代理机构北京理工大学专利中心11120代理人仇蕾安杨志兵(51)Int.Cl.G01S19/41(2010.01)G01S19/47(2010.01)G01S19/53(2010.01)权利要求书2页说明书4页附图2页(54)发明名称基于GNSS/ODO的机器人双轮差速定位方法(57)摘要本发明属于机器人技术领域，特别涉及一种机器人定位方法。基于GNSS/ODO的机器人双轮差速定位方法，步骤为：A.初始化机器人，利用GNSS获取初始时刻机器人的位姿信息；B.利用双轮里程计定位算法对机器人n+1时刻的全局位姿信息进行推算；C.判断当前时刻是否为设定的GNSS数据修正时刻，如果是，则执行D步骤；如果不是，则执行E步骤；D.根据步骤B中推算得到的n+1时刻的无人车位姿状态，进行卡尔曼滤波“预测-修正”；E.仅进行D步骤中的卡尔曼滤波预测，而不做修正；F.输出滤波后的机器人的全局位姿信息。利用本发明，机器人的定位、定向精度有显著提高。CN107219542ACN107219542A权利要求书1/2页1.一种基于GNSS/ODO的机器人双轮差速定位方法，其特征在于，包括以下步骤：A.初始化机器人，利用GNSS获取初始时刻机器人的全局位姿信息；设当前时刻为n，该时刻机器人的全局位姿信息由[xg(n),yg(n),θg(n)]描述，其中，[xg(n),yg(n)]表示机器人在全局坐标系中的平面坐标位置，θg(n)表示机器人的航向角；B.依据n时刻机器人的全局位姿信息，根据公式(1.1)中双轮里程计该历元里的里程变化对机器人n+1时刻的全局位姿信息进行预测；其中：L1、L2分别表示双轮里程计测量到的机器人左、右轮每历元的里程变化量，K1、K2分别为机器人左、右轮编码器矫正系数；L为机器人两轮中心轮距；dθg为机器人在相邻两时刻转过的角度，(dxl,dyl)为机器人在本体坐标系下平面位置：C.判断当前时刻是否为设定的GNSS数据修正时刻，如果是，则执行D步骤；如果不是，则执行E步骤；D.根据步骤B中预测得到的n+1时刻的全局位姿信息，进行卡尔曼滤波“预测-修正”，卡尔曼组合滤波器设计如下：系统状态变量和系统方程分别如公式(1.3)和(1.4)所示，TXn＝[ΔXΔYΔθΔK1ΔK2Δψ](1.3)Xn+1＝ΦXn+wn(1.4)式中：[ΔX,ΔY,Δθ]为步骤B中双轮里程计的位置和航向误差；ΔK1、ΔK2为左、右两轮标定因子误差；Δψ为安装误差；系统噪声为wT～N(0,Q)；系统状态转移矩阵Φ为：其中，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)]2CN107219542A权利要求书2/2页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)]作为卡尔曼滤波器的量测信息，此时的量测方程为：式中，观测方程ε(t)为系统观测噪声矢量，为N(0,R)的高斯白噪声过程；并转到F步骤；E.仅进行D步骤中的卡尔曼滤波预测，而不做修正，并转到F步骤；F.输出滤波后的机器人的全局位姿信息；I.令n＝n+1，转到步骤B，进行下一循环。3CN107219542A说明书1/4页基于GNSS/ODO的机器人双轮差速定位方法技术领域[0001]本发明属于机器人技术领域，特别涉及一种机器人定位方法。背景技术[0002]机器人定位主要是指机器人通过外部传感器获取相关信息经过处理来确定自身及目标位姿的过程。现有的技术方案有里程计法、惯性导航法、卫星导航法、磁导航法等。其中，里程计法(ODO)是使用最为广泛的定位方法。在移动机器人车轮上装有光电编码器，通过对车轮转动的记录来实现对机器人的位姿跟踪。但此方法定位为航位推算法，是一个累加过程，其测量误差与车辆行驶路程长度有关，随着移动距离的增加，定位精度逐渐下降。惯性导航法利用惯性器件(