CN107219542B - 基于gnss/odo的机器人双轮差速定位方法 - Google Patents

基于gnss/odo的机器人双轮差速定位方法 Download PDF

Info

Publication number
CN107219542B
CN107219542B CN201710245625.8A CN201710245625A CN107219542B CN 107219542 B CN107219542 B CN 107219542B CN 201710245625 A CN201710245625 A CN 201710245625A CN 107219542 B CN107219542 B CN 107219542B
Authority
CN
China
Prior art keywords
robot
gnss
wheel
pose information
double
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710245625.8A
Other languages
English (en)
Other versions
CN107219542A (zh
Inventor
李耀宗
韩锐
王坤
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Qingdao Luteng Intelligent Equipment Technology Co ltd
Original Assignee
Beijing Krund Artificial Intelligent Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Krund Artificial Intelligent Technology Co ltd filed Critical Beijing Krund Artificial Intelligent Technology Co ltd
Priority to CN201710245625.8A priority Critical patent/CN107219542B/zh
Publication of CN107219542A publication Critical patent/CN107219542A/zh
Application granted granted Critical
Publication of CN107219542B publication Critical patent/CN107219542B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude
    • G01S19/41—Differential correction, e.g. DGPS [differential GPS]
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

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

Description

基于GNSS/ODO的机器人双轮差速定位方法
技术领域
本发明属于机器人技术领域，特别涉及一种机器人定位方法。
背景技术
机器人定位主要是指机器人通过外部传感器获取相关信息经过处理来确定自身及目标位姿的过程。现有的技术方案有里程计法、惯性导航法、卫星导航法、磁导航法等。其中，里程计法(ODO)是使用最为广泛的定位方法。在移动机器人车轮上装有光电编码器，通过对车轮转动的记录来实现对机器人的位姿跟踪。但此方法定位为航位推算法，是一个累加过程，其测量误差与车辆行驶路程长度有关，随着移动距离的增加，定位精度逐渐下降。惯性导航法利用惯性器件(加速度计和陀螺仪)来测量机器人本身的加速度以及角速度，并结合给定的初始条件，经过积分、解算得到每一时刻机器人的速度、位置、姿态等参数。但导航定位误差会随着时间积累，精度逐渐下降，所以惯性导航法不适合长时间的精确定位。卫星导航通过在机器人上安装GNSS移动站接收机，在另一已知点上安装GNSS基站，移动站GNSS接收基站GNSS的差分改正数据，为机器人提供高精度的位置、速度等信息来完成导航。但其具有局限性，即在卫星信号受到遮挡或干扰时无法提供有效的定位信息，不能满足导航具备完全自主性的要求。磁导航通过磁传感器检测磁场变化，从而进行定位导航。但此方法可变性和可维护性差，易受环境的影响。
发明内容
本发明的目的是：提供一种结合GNSS和ODO两传感器各自优点，扬长避短，基于GNSS/ODO的组合定位方法，避免GNSS定位信息由于遮挡或信号失锁时无法实现定位的问题，同时避免了ODO定位导致的累积误差问题，提高定位的可靠性。
本发明的技术方案是：一种基于GNSS/ODO的机器人双轮差速定位方法，包括以下步骤：
A.初始化机器人，利用GNSS获取初始时刻机器人的全局位姿信息；
设当前时刻为n，该时刻机器人的全局位姿信息由[xg(n),yg(n),θg(n)]描述，其中，[xg(n),yg(n)]表示机器人在全局坐标系中的平面坐标位置，θg(n)表示机器人的航向角；
B.依据n时刻机器人的全局位姿信息，根据公式(1.1)中双轮里程计该历元里的里程变化对机器人n+1时刻的全局位姿信息进行预测；
Figure BDA0001270560600000021
其中：L1、L2分别表示双轮里程计测量到的机器人左、右轮每历元的里程变化量，K1、K2分别为机器人左、右轮编码器矫正系数；L为机器人两轮中心轮距；
dθg为机器人在相邻两时刻转过的角度，(dxl,dyl)为机器人在本体坐标系下平面位置：
Figure BDA0001270560600000022
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)；系统状态转移矩阵Φ为：
Figure BDA0001270560600000023
其中，
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/L
F35＝L2/L
以双轮里程计差速解算的位置信息[xg(n+1),yg(n+1)]与GNSS差分定位输出的位置信息[xgnss(n+1),ygnss(n+1)]作为卡尔曼滤波器的量测信息，此时的量测方程为：
Figure BDA0001270560600000031
式中，观测方程
Figure BDA0001270560600000032
ε(t)为系统观测噪声矢量，为N(0,R)的高斯白噪声过程；并转到F步骤；
E.仅进行D步骤中的卡尔曼滤波预测，而不做修正，并转到F步骤；
F.输出滤波后的机器人的全局位姿信息；
I.令n＝n+1，转到步骤B，进行下一循环。
有益效果：利用本发明，机器人的定位、定向精度有显著提高，弥补了单一导航系统的缺陷，为机器人的自主导航提供了保障，相比于高精度传感器，大大减少了机器人成本，加快了建图效率，使得机器人能够更广泛使用。
附图说明
图1为本发明中所用坐标与参数示意图；
图2为本发明GNSS/ODO组合定位的原理图；
图3为本发明步骤流程图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
参见附图，一种基于GNSS/ODO的机器人双轮差速定位方法，包括以下步骤：
A.初始化机器人，利用GNSS获取初始时刻机器人的位姿信息；
设当前时刻为n，该时刻机器人的全局位姿信息由[xg(n),yg(n),θg(n)]描述，其中，[xg(n),yg(n)]表示机器人在全局坐标系中的平面坐标位置，θg(n)表示机器人的航向角；令L1、L2分别表示双轮里程计测量到的机器人左、右轮每历元的里程变化量，则机器人在相邻两时刻转过的角度dθg和机器人本体坐标系下平面位置(dxl,dyl)变化分别为
Figure BDA0001270560600000041
其中，K1、K2分别为机器人左、右轮编码器矫正系数(编码器安装在里程计上，用于记录每轮的脉冲数，然后计算每轮行走的距离)；L为机器人两轮中心轮距；
B.利用步骤A中GNSS获得的n时刻机器人全局位姿信息，根据公式(1.2)中双轮里程计该历元里的里程变化对机器人n+1时刻的全局位姿信息进行预测；
Figure BDA0001270560600000042
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)；系统状态转移矩阵Φ为：
Figure BDA0001270560600000051
其中，
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/L
F35＝L2/L
以双轮里程计差速解算的位置信息[xg(n+1),yg(n+1)]与GNSS差分定位输出的位置信息[xgnss(n+1),ygnss(n+1)]作为卡尔曼滤波器的量测信息，此时的量测方程为：
Figure BDA0001270560600000052
式中，观测方程
Figure BDA0001270560600000053
ε(t)为系统观测噪声矢量，为N(0,R)的高斯白噪声过程；并转到F步骤；
E.仅进行D步骤中的卡尔曼滤波预测，而不做修正，并转到F步骤；
F.输出n+1时刻机器人的位姿信息；
I.转到步骤B，进行下一循环。

Claims (1)

1.一种基于GNSS/ODO的机器人双轮差速定位方法，其特征在于，包括以下步骤：
A.初始化机器人，利用GNSS获取初始时刻机器人的全局位姿信息；
设当前时刻为n，该时刻机器人的全局位姿信息由[xg(n),yg(n),θg(n)]描述，其中，[xg(n),yg(n)]表示机器人在全局坐标系中的平面坐标位置，θg(n)表示机器人的航向角；
B.依据n时刻机器人的全局位姿信息，根据公式(1.1)中双轮里程计每历元里的里程变化对机器人n+1时刻的全局位姿信息进行预测；
Figure FDA0002356790090000011
其中：L1、L2分别表示双轮里程计测量到的机器人左、右轮每历元的里程变化量，K1、K2分别为机器人左、右轮编码器矫正系数；L为机器人两轮中心轮距；
dθg为机器人在相邻两时刻转过的角度，(dxl,dyl)为机器人在本体坐标系下平面位置：
Figure FDA0002356790090000012
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为左、右两轮标定因子误差；Δψ为安装误差；系统噪声为wn～N(0,Q)；系统状态转移矩阵Φ为：
Figure FDA0002356790090000021
其中，
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/L
F35＝L2/L
以双轮里程计差速解算的位置信息[xg(n+1),yg(n+1)]与GNSS差分定位输出的位置信息[xgnss(n+1),ygnss(n+1)]作为卡尔曼滤波器的量测信息，此时的量测方程为：
Figure FDA0002356790090000022
式中，观测方程
Figure FDA0002356790090000023
ε(t)为系统观测噪声矢量，为N(0,R)的高斯白噪声过程；并转到F步骤；
E.仅进行D步骤中的卡尔曼滤波预测，而不做修正，并转到F步骤；
F.输出滤波后的机器人的全局位姿信息；
I.令n＝n+1，转到步骤B，进行下一循环。
CN201710245625.8A 2017-04-14 2017-04-14 基于gnss/odo的机器人双轮差速定位方法 Active CN107219542B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710245625.8A CN107219542B (zh) 2017-04-14 2017-04-14 基于gnss/odo的机器人双轮差速定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710245625.8A CN107219542B (zh) 2017-04-14 2017-04-14 基于gnss/odo的机器人双轮差速定位方法

Publications (2)

Publication Number Publication Date
CN107219542A CN107219542A (zh) 2017-09-29
CN107219542B true CN107219542B (zh) 2020-07-31

Family

ID=59927845

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710245625.8A Active CN107219542B (zh) 2017-04-14 2017-04-14 基于gnss/odo的机器人双轮差速定位方法

Country Status (1)

Country Link
CN (1) CN107219542B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108036792A (zh) * 2017-12-11 2018-05-15 苏州中德睿博智能科技有限公司 一种用于移动机器人的里程计与测量位姿的数据融合方法
CN108896049A (zh) * 2018-06-01 2018-11-27 重庆锐纳达自动化技术有限公司 一种机器人室内运动定位方法
CN111114292A (zh) * 2018-10-31 2020-05-08 深圳市优必选科技有限公司 一种静音机器的底盘结构
CN110906941B (zh) * 2019-12-03 2023-06-23 苏州智加科技有限公司 用于长距离隧道的自动驾驶地图的构建方法及系统
CN111098335B (zh) * 2019-12-26 2021-06-08 浙江欣奕华智能科技有限公司 一种双轮差速驱动机器人里程计标定方法及装置
CN113029201B (zh) * 2021-04-07 2023-04-07 北京布科思科技有限公司 双轮差速模型的里程校准方法和装置、设备及存储介质

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5906655A (en) * 1997-04-02 1999-05-25 Caterpillar Inc. Method for monitoring integrity of an integrated GPS and INU system
JP5586994B2 (ja) * 2010-03-11 2014-09-10 三菱電機株式会社 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム
CN105824039A (zh) * 2016-03-17 2016-08-03 孙红星 基于里程计的克服卫星失锁时的gnss/ins车载组合定位定向算法

Also Published As

Publication number Publication date
CN107219542A (zh) 2017-09-29

Similar Documents

Publication Publication Date Title
CN107219542B (zh) 基于gnss/odo的机器人双轮差速定位方法
CN110411462B (zh) 一种gnss/惯导/车道线约束/里程计多源融合方法
CN109946732B (zh) 一种基于多传感器数据融合的无人车定位方法
US10788830B2 (en) Systems and methods for determining a vehicle position
CN106840179B (zh) 一种基于多传感器信息融合的智能车定位方法
CN110160542B (zh) 车道线的定位方法和装置、存储介质、电子装置
CN101846734B (zh) 农用机械导航定位方法、系统及农用机械工控机
WO2017107434A1 (zh) 一种基于单天线的gnss-ins车辆定姿方法
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN107132563B (zh) 一种里程计结合双天线差分gnss的组合导航方法
CN109343095B (zh) 一种车载导航车辆组合定位装置及其组合定位方法
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN111044073B (zh) 基于双目激光高精度agv位置感知方法
CN110361008B (zh) 地下车库自动泊车的定位方法及装置
CN108345021B (zh) 一种多普勒雷达辅助gps/ins车辆测速方法
CN104061899A (zh) 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法
KR101115012B1 (ko) 로봇의 각속도 오차 보정 장치 및 방법
CN113670334B (zh) 一种飞行汽车的初始对准方法和装置
CN114076610B (zh) Gnss/mems车载组合导航系统的误差标定、导航方法及其装置
CN109470276B (zh) 基于零速修正的里程计标定方法与装置
CN103994766A (zh) 一种抗gps失效固定翼无人机定向方法
US20220009551A1 (en) Method and system for providing transformation parameters
Hu et al. Kilometer sign positioning-aided INS/odometer integration for land vehicle autonomous navigation

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20210329

Address after: 266300 No. 6 Fenhe Road, Jiaozhou economic and Technological Development Zone, Qingdao, Shandong

Patentee after: QINGDAO KRUND ROBOT Co.,Ltd.

Address before: Room 1703, Floor 17, Building 4, 11 Changchunqiao Road, Haidian District, Beijing

Patentee before: BEIJING KRUND ARTIFICIAL INTELLIGENT TECHNOLOGY Co.,Ltd.

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20220525

Address after: No.6 Fenhe Road, Jiaozhou economic and Technological Development Zone, Qingdao, Shandong Province 266000

Patentee after: Qingdao Kelu Intelligent Technology Co.,Ltd.

Address before: 266300 No. 6 Fenhe Road, Jiaozhou economic and Technological Development Zone, Qingdao, Shandong

Patentee before: QINGDAO KRUND ROBOT Co.,Ltd.

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20230918

Address after: No.6 Fenhe Road, Jiaozhou economic and Technological Development Zone, Qingdao, Shandong Province 266000

Patentee after: Qingdao Luteng Intelligent Equipment Technology Co.,Ltd.

Address before: No.6 Fenhe Road, Jiaozhou economic and Technological Development Zone, Qingdao, Shandong Province 266000

Patentee before: Qingdao Kelu Intelligent Technology Co.,Ltd.