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

权利要求书

1.一种基于GNSS/ODO的机器人双轮差速定位方法，其特征在于，包括以下步骤：

A.初始化机器人，利用GNSS获取初始时刻机器人的全局位姿信息；

设当前时刻为n，该时刻机器人的全局位姿信息由[x_g(n),y_g(n),θ_g(n)]描述，其中，[x_g(n),y_g(n)]表示机器人在全局坐标系中的平面坐标位置，θ_g(n)表示机器人的航向角；

B.依据n时刻机器人的全局位姿信息，根据公式(1.1)中双轮里程计每历元里的里程变化对机器人n+1时刻的全局位姿信息进行预测；

其中：L₁、L₂分别表示双轮里程计测量到的机器人左、右轮每历元的里程变化量，K₁、K₂分别为机器人左、右轮编码器矫正系数；L为机器人两轮中心轮距；

dθ_g为机器人在相邻两时刻转过的角度，(dx_l,dy_l)为机器人在本体坐标系下平面位置：

C.判断当前时刻是否为设定的GNSS数据修正时刻，如果是，则执行D步骤；如果不是，则执行E步骤；

D.根据步骤B中预测得到的n+1时刻的全局位姿信息，进行卡尔曼滤波“预测-修正”，卡尔曼组合滤波器设计如下：

系统状态变量和系统方程分别如公式(1.3)和(1.4)所示，

X_n＝[ΔX ΔY Δθ ΔK₁ ΔK₂ Δψ]^T (1.3)

X_n+1＝ΦX_n+w_n (1.4)

式中：[ΔX,ΔY,Δθ]为步骤B中双轮里程计的位置和航向误差；ΔK₁、ΔK₂为左、右两轮标定因子误差；Δψ为安装误差；系统噪声为w_n～N(0,Q)；系统状态转移矩阵Φ为：

其中，

F₁₃＝-[dx_l(n)·sinθ_g(n)+dy_l(n)·cosθ_g(n)]

F₁₄＝L₁/2·[L₁·K₁·cosθ_g(n)/L-sinθ_g(n)]

F₁₅＝-L₂/2·[L₂·K₂·cosθ_g(n)/L+sinθ_g(n)]

F₂₃＝dx_l·cosθ_g(n)-dy_l·sinθ_g(n)

F₂₄＝L₁/2·[L₁·K₁·sinθ_g(n)/L+cosθ_g(n)]

F₂₅＝-L₂/2·[L₂·K₂·sinθ_g(n)/L-cosθ_g(n)]

F₃₄＝-L₁/L

F₃₅＝L₂/L

以双轮里程计差速解算的位置信息[x_g(n+1),y_g(n+1)]与GNSS差分定位输出的位置信息[x_gnss(n+1),y_gnss(n+1)]作为卡尔曼滤波器的量测信息，此时的量测方程为：

式中，观测方程ε(t)为系统观测噪声矢量，为N(0,R)的高斯白噪声过程；并转到F步骤；

E.仅进行D步骤中的卡尔曼滤波预测，而不做修正，并转到F步骤；

F.输出滤波后的机器人的全局位姿信息；

I.令n＝n+1，转到步骤B，进行下一循环。