CN106352879A - 一种基于图优化的uwb定位与编码器融合的位姿估计方法 - Google Patents

一种基于图优化的uwb定位与编码器融合的位姿估计方法 Download PDF

Info

Publication number
CN106352879A
CN106352879A CN201610856905.8A CN201610856905A CN106352879A CN 106352879 A CN106352879 A CN 106352879A CN 201610856905 A CN201610856905 A CN 201610856905A CN 106352879 A CN106352879 A CN 106352879A
Authority
CN
China
Prior art keywords
robot
pose
error
coordinate
estimation
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.)
Granted
Application number
CN201610856905.8A
Other languages
English (en)
Other versions
CN106352879B (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.)
Shenzhen City Purdue Technology Co Ltd
Shenzhen Pudu Technology Co Ltd
Original Assignee
Shenzhen City Purdue 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 Shenzhen City Purdue Technology Co Ltd filed Critical Shenzhen City Purdue Technology Co Ltd
Priority to CN201610856905.8A priority Critical patent/CN106352879B/zh
Publication of CN106352879A publication Critical patent/CN106352879A/zh
Application granted granted Critical
Publication of CN106352879B publication Critical patent/CN106352879B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Landscapes

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

Abstract

本申请设计一种基于图优化的UWB定位与编码器融合的位姿估计方法，该方法采用UWB定位与编码器融合来估算机器人位姿，对机器人位姿的估计精度高、计算量少，在一般手机使用的芯片上可实时计算。

Description

一种基于图优化的UWB定位与编码器融合的位姿估计方法
技术领域
发明涉及一种机器人状态估计方法，具体涉及一种依据图优化理论，融合UWB定位坐标与编码器里程信息的机器人位姿估计方法。
背景技术
GPS全球卫星定位系统已经在室外有了广泛应用，但是在室内由于信号衰减无法准确定位。于是，基于蓝牙、WIFI、UWB信号的室内基站补充了GPS的不足。目前，无论是室外的GPS还是室内基站，都只能给出定位标签的坐标信息，而无法给出被定位对象的对方向信息。在汽车上，方向信息是不断定位求差分得到的，这是基于汽车移动速度较快，且无法横向移动这两点推测出的。在室内较慢移动的机器人则无法使用此法。也可增加一个定位标签，每台设备上使用两个标签的坐标差向量来给出方向信息，但这会增加一个标签的成本，这对绝大多数低成本机器人是不可接受的。
发明内容
本申请解决了在不额外增加定位标签的前提下，结合UWB定位坐标与轮子编码器信息融合估计双主动轮机器人的位置与方向测量的问题。
为解决上述技术问题，本申请采用以下技术方案：一种基于图优化的UWB定位与编码器融合位姿估计方法,包括以下步骤:
1、传感器的安装:
传感器包括基站和定位标签，基站的安装高度至少大于机器人；定位标签需安装在离机器人自转中心点有一段距离的地方，此距离需要大于定位算法的误差的标准差3倍以上，该误差指根据传感器测量的机器人坐标与机器人真实坐标之间的距离，且定位标签不在机器人自转转轴上；
2、初始位姿的获取:
.机器人开机后先测出定位标签的坐标；
.然后以小于10秒一圈的速度自转一圈，同时在自转过程中不断定位，得到一组坐标，将这组坐标取平均即可得到机器人自转中心点坐标；
c.由定位标签坐标以及自转中心点坐标，求得机器人的初始位姿。初始位姿的位置由中心点坐标给出，初始位姿的方向由中心点坐标和定位标签坐标的连线给出；
3、迭代:
a.机器人初始位姿共x、y、θ这3个自由度，x、y、θ分别表示机器人位置的横向坐标、纵向坐标以及机器人的朝向，考虑到测量精度，此处存在3个误差。
b.在机器人在运动过程中，读出一系列由编码器输出的测量值。两个轮子不妨设为2n个测量值，这2n个测量值对应2n个误差。
c.根据上述2n+3个变量，使用经典的测程法可以计算出机器人的当前位姿，计算的位姿即为机器人位姿的最优估计。
d.机器人在运动过程中根据UWB定位数据以及经典的GPS定位算法得到一个二自由度的定位坐标，此处只考虑机器人做平面平行运动，该坐标与上述基于测程法计算出的位姿中的位置必然不相等，存在一个距离偏差dis。加上上述2n+3个误差总共2n+4个误差,此2n+4个误差的自由度是2n+3。
e.定义2n+4个误差的二范数为位姿估计总误差，或者说位姿估计的评价函数，考虑到前2n+3个误差将决定最后一个误差，则位姿估计评价函数可以看作前2n+3个误差的函数，优化目标为位姿估计评价函数取值最小。
f.对2n+3个自变量的位姿估计评价函数求偏微分，得到梯度，向负梯度方向迭代，直到函数值最小，此时使用前2n+3个误差，加上前2n+3个测量值得到对2n+3个真值的估计，用这2n+3个估计使用测程法计算出最后对机器人位姿的估计，则此估计是在二范数意义下对机器人位姿的最优估计。
有益效果，该方法采用UWB定位与编码器融合来估算机器人位姿，对机器人位姿的估计精度高、计算量少，在一般手机使用的芯片上可实时计算。
附图说明
图1 .编码器里程估计的位姿，与UWB定位点构成的矢量图；
图2.算法计算过程流程图。
具体实施方式
一种基于图优化的UWB定位与编码器融合位姿估计方法,包括以下步骤:
（1）传感器的安装
传感器包括基站和定位标签，基站的安装高度至少大于机器人；定位标签需安装在离机器人自转中心点有一段距离的地方，此距离需要大于定位算法的误差的标准差3倍以上，该误差指根据传感器测量的机器人坐标与机器人真实坐标之间的距离，且定位标签不在机器人自转转轴上。
（2）初始位姿的获取
a.机器人开机后先测出定位标签的坐标。
b.然后以小于10秒一圈的速度自转一圈，同时在自转过程中不断定位，得到一组坐标。将这组坐标取平均即可得到机器人自转中心点坐标。
c.由定位标签坐标以及自转中心点坐标，可以求得机器人的初始位姿。初始位姿的位置由中心点坐标给出，初始位姿的方向由中心点坐标和定位标签坐标的连线给出。
（3）迭代
a.机器人初始位姿共x、y、θ这3个自由度，x、y、θ分别表示机器人位置的横向坐标、纵向坐标以及机器人的朝向，考虑到测量精度，此处存在3个误差。
b. 机器人在运动过程中，读出一系列编码器的测量值。两个轮子不妨设为2n个测量值，这2n个测量值对应2n个误差。
c.根据上述2n+3个变量，使用经典的测程法可以计算出机器人的当前位姿。考虑到这2n+3个变量的误差是不相关的（或相关性非常微弱），故据此使用测程法计算的位姿即为机器人位姿的最优估计。
d. 机器人在运动过程中根据UWB定位数据以及经典的GPS定位算法得到一个二自由度的定位坐标，此处只考虑机器人做平面平行运动，该坐标与上述基于测程法计算出的位姿中的位置必然不相等，存在一个距离偏差dis。加上上述2n+3个误差总共2n+4个误差。由于最后一个dis是根据前2n+3个变量及其误差计算出来的，则2n+4个测量误差中的前2n+3个与最后一个具有相关性（此处由于只关注定位坐标与其他方法计算的位置的距离，故定位坐标的x、y两个分量只得到dis一个误差），或者说这2n+4个误差的自由度是2n+3。
e. 定义2n+4个误差的二范数为位姿估计总误差，或者说位姿估计的评价函数，考虑到前2n+3个误差将决定最后一个误差，则位姿估计评价函数可以看作前2n+3个误差的函数。优化目标为位姿估计评价函数取值最小。
f. 对2n+3个自变量的位姿估计评价函数求偏微分，得到梯度，向负梯度方向迭代，直到函数值最小，此时使用前2n+3个误差，加上前2n+3个测量值得到对2n+3个真值的估计，用这2n+3个估计使用测程法计算出最后对机器人位姿的估计。则此估计是在二范数意义下对机器人位姿的最优估计。

Claims (1)

1.一种基于图优化的UWB定位与编码器融合的位姿估计方法，包括以下步骤：
（1）传感器的安装:
传感器包括基站和定位标签，基站的安装高度至少大于机器人；定位标签需安装在离机器人自转中心点有一段距离的地方，此距离需要大于定位算法的误差的标准差3倍以上，该误差指根据传感器测量的机器人坐标与机器人真实坐标之间的距离，且定位标签不在机器人自转转轴上；
（2）初始位姿的获取:
.机器人开机后先测出定位标签的坐标；
.然后以小于10秒一圈的速度自转一圈，同时在自转过程中不断定位，得到一组坐标，将这组坐标取平均即可得到机器人自转中心点坐标；
c.由定位标签坐标以及自转中心点坐标，求得机器人的初始位姿。初始位姿的位置由中心点坐标给出，初始位姿的方向由中心点坐标和定位标签坐标的连线给出；
（3）迭代:
a.机器人初始位姿共x、y、θ这3个自由度，x、y、θ分别表示机器人位置的横向坐标、纵向坐标以及机器人的朝向，考虑到测量精度，此处存在3个误差；
b.在机器人在运动过程中，读出一系列由编码器输出的测量值。两个轮子不妨设为2n个测量值，这2n个测量值对应2n个误差；
c.根据上述2n+3个变量，使用经典的测程法可以计算出机器人的当前位姿，计算的位姿即为机器人位姿的最优估计；
d.机器人在运动过程中根据UWB定位数据以及经典的GPS定位算法得到一个二自由度的定位坐标，此处只考虑机器人做平面平行运动，该坐标与上述基于测程法计算出的位姿中的位置必然不相等，存在一个距离偏差dis。加上上述2n+3个误差总共2n+4个误差,此2n+4个误差的自由度是2n+3；
e.定义2n+4个误差的二范数为位姿估计总误差，或者说位姿估计的评价函数，考虑到前2n+3个误差将决定最后一个误差，则位姿估计评价函数可以看作前2n+3个误差的函数，优化目标为位姿估计评价函数取值最小；
f.对2n+3个自变量的位姿估计评价函数求偏微分，得到梯度，向负梯度方向迭代，直到函数值最小，此时使用前2n+3个误差，加上前2n+3个测量值得到对2n+3个真值的估计，用这2n+3个估计使用测程法计算出最后对机器人位姿的估计，则此估计是在二范数意义下对机器人位姿的最优估计。
CN201610856905.8A 2016-09-28 2016-09-28 一种基于图优化的uwb定位与编码器融合的位姿估计方法 Active CN106352879B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610856905.8A CN106352879B (zh) 2016-09-28 2016-09-28 一种基于图优化的uwb定位与编码器融合的位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610856905.8A CN106352879B (zh) 2016-09-28 2016-09-28 一种基于图优化的uwb定位与编码器融合的位姿估计方法

Publications (2)

Publication Number Publication Date
CN106352879A true CN106352879A (zh) 2017-01-25
CN106352879B CN106352879B (zh) 2019-08-13

Family

ID=57865422

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610856905.8A Active CN106352879B (zh) 2016-09-28 2016-09-28 一种基于图优化的uwb定位与编码器融合的位姿估计方法

Country Status (1)

Country Link
CN (1) CN106352879B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107065858A (zh) * 2017-02-13 2017-08-18 四川超影科技有限公司 基于超宽带的巡检机器人导航方法
CN110186459A (zh) * 2019-05-27 2019-08-30 深圳市海柔创新科技有限公司 导航方法、移动载体及导航系统
CN110650435A (zh) * 2018-12-13 2020-01-03 杭州电子科技大学 基于UWB定位和ZigBee定位的机器学习融合方法
CN111708010A (zh) * 2019-03-01 2020-09-25 北京图森智途科技有限公司 一种可移动设备的定位方法、装置、系统及可移动设备
CN112833876A (zh) * 2020-12-30 2021-05-25 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2008292491A (ja) * 2007-05-25 2008-12-04 Palo Alto Research Center Inc 測位用タグ及びその随伴先物品の測位方法及びシステム
CN103389087A (zh) * 2013-08-07 2013-11-13 上海海事大学 一种轮式机器人位姿解算方法
CN104020444A (zh) * 2014-06-23 2014-09-03 西北工业大学 一种双轮差动式机器人室内组合定位方法
US20140304107A1 (en) * 2012-12-03 2014-10-09 CLARKE William McALLISTER Webrooming with rfid-scanning robots
CN104590414A (zh) * 2014-12-29 2015-05-06 合肥工业大学 一种具有相对位姿检测功能的全方位轮式移动机器人
CN105547305A (zh) * 2015-12-04 2016-05-04 北京布科思科技有限公司 一种基于无线定位和激光地图匹配的位姿解算方法
CN105783913A (zh) * 2016-03-08 2016-07-20 中山大学 一种融合车载多传感器的slam装置及其控制方法
CN105828431A (zh) * 2016-04-25 2016-08-03 上海理工大学 基于uwb的自主跟随机器人定位方法及系统

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2008292491A (ja) * 2007-05-25 2008-12-04 Palo Alto Research Center Inc 測位用タグ及びその随伴先物品の測位方法及びシステム
US20140304107A1 (en) * 2012-12-03 2014-10-09 CLARKE William McALLISTER Webrooming with rfid-scanning robots
CN103389087A (zh) * 2013-08-07 2013-11-13 上海海事大学 一种轮式机器人位姿解算方法
CN104020444A (zh) * 2014-06-23 2014-09-03 西北工业大学 一种双轮差动式机器人室内组合定位方法
CN104590414A (zh) * 2014-12-29 2015-05-06 合肥工业大学 一种具有相对位姿检测功能的全方位轮式移动机器人
CN105547305A (zh) * 2015-12-04 2016-05-04 北京布科思科技有限公司 一种基于无线定位和激光地图匹配的位姿解算方法
CN105783913A (zh) * 2016-03-08 2016-07-20 中山大学 一种融合车载多传感器的slam装置及其控制方法
CN105828431A (zh) * 2016-04-25 2016-08-03 上海理工大学 基于uwb的自主跟随机器人定位方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
P. ALLGEUER 等，: ""Robust sensor fusion for robot attitude estimation"", 《2014 IEEE-RAS INTERNATIONAL CONFERENCE ON HUMANOID ROBOTS, MADRID》 *
梁明杰 等，: ""基于图优化的同时定位与地图创建综述"", 《机器人》 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107065858A (zh) * 2017-02-13 2017-08-18 四川超影科技有限公司 基于超宽带的巡检机器人导航方法
CN110650435A (zh) * 2018-12-13 2020-01-03 杭州电子科技大学 基于UWB定位和ZigBee定位的机器学习融合方法
CN110650435B (zh) * 2018-12-13 2021-04-30 杭州电子科技大学 基于UWB定位和ZigBee定位的机器学习融合方法
CN111708010A (zh) * 2019-03-01 2020-09-25 北京图森智途科技有限公司 一种可移动设备的定位方法、装置、系统及可移动设备
CN111708010B (zh) * 2019-03-01 2024-04-12 北京图森智途科技有限公司 一种可移动设备的定位方法、装置、系统及可移动设备
CN110186459A (zh) * 2019-05-27 2019-08-30 深圳市海柔创新科技有限公司 导航方法、移动载体及导航系统
CN110186459B (zh) * 2019-05-27 2021-06-29 深圳市海柔创新科技有限公司 导航方法、移动载体及导航系统
CN113390420A (zh) * 2019-05-27 2021-09-14 深圳市海柔创新科技有限公司 导航方法、移动载体及导航系统
CN112833876A (zh) * 2020-12-30 2021-05-25 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法
CN112833876B (zh) * 2020-12-30 2022-02-11 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法

Also Published As

Publication number Publication date
CN106352879B (zh) 2019-08-13

Similar Documents

Publication Publication Date Title
CN106352879A (zh) 一种基于图优化的uwb定位与编码器融合的位姿估计方法
CN103412565B (zh) 一种具有全局位置快速估计能力的机器人定位方法
CN106979780B (zh) 一种无人车实时姿态测量方法
CN105190238B (zh) 用于改进骑行导航的方法和装置
CN104121905B (zh) 一种基于惯性传感器的航向角获取方法
CN103162689B (zh) 辅助车载定位系统及车辆的辅助定位方法
CN108844533A (zh) 一种基于多传感器融合和姿态解算的自由姿态pdr定位方法
CN108731670A (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN103791902B (zh) 适用于高机动载体的星敏感器自主导航方法
CN106052555A (zh) 一种工业机器人基座坐标测量方法
US10557711B2 (en) Apparatus for inferring pedestrian position based on pedestrian movement detection, and method therefor
CN104197935B (zh) 一种基于移动智能终端的室内定位方法
CN106840152A (zh) 一种面向室内移动机器人的高精度组合导航系统及方法
CN105091911A (zh) 一种pos系统动态定位精度检测系统及方法
CN105509765A (zh) 一种惯性/dvl/usbl安装误差标定方法
CN109870173A (zh) 一种基于校验点的海底管道惯性导航系统的轨迹修正方法
CN106197406A (zh) 一种基于惯性导航和rssi无线定位的融合方法
CN104251699B (zh) 一种室内空间定位方法
CN107782304A (zh) 移动机器人的定位方法及装置、移动机器人及存储介质
CN108507572A (zh) 一种基于mems惯性测量单元的姿态定位误差修正方法
CN105157691B (zh) 一种指南针方位的确定方法及装置
CN107463871A (zh) 一种基于角特征加权的点云匹配方法
CN106979779A (zh) 一种无人车实时姿态测量方法
CN108168552A (zh) 一种看护机器人路标定位系统
CN107976186B (zh) 无锚点的相对定位方法

Legal Events

Date Code Title Description
C06 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