CN106352879B - 一种基于图优化的uwb定位与编码器融合的位姿估计方法 - Google Patents
一种基于图优化的uwb定位与编码器融合的位姿估计方法 Download PDFInfo
- Publication number
- CN106352879B CN106352879B CN201610856905.8A CN201610856905A CN106352879B CN 106352879 B CN106352879 B CN 106352879B CN 201610856905 A CN201610856905 A CN 201610856905A CN 106352879 B CN106352879 B CN 106352879B
- 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.)
- Active
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定位坐标与编码器里程信息的机器人位姿估计方法。
背景技术
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)初始位姿的获取:
a.机器人开机后先测出定位标签的坐标；
b.然后以小于10秒一圈的速度自转一圈，同时在自转过程中不断定位，得到一组坐标，将这组坐标取平均即可得到机器人自转中心点坐标；
c.由定位标签坐标以及自转中心点坐标，求得机器人的初始位姿，初始位姿的位置由中心点坐标给出，初始位姿的方向由中心点坐标和定位标签坐标的连线给出；
(3)迭代:
a.机器人初始位姿共x、y、θ这3个自由度，x、y、θ分别表示机器人位置的横向坐标、纵向坐标以及机器人的朝向，考虑到测量精度，此处存在3个误差；
b.机器人在运动过程中，读出一系列由编码器输出的测量值，机器人两个轮子共输出2n个测量值，这2n个测量值对应2n个误差；
c.根据上述2n个误差和所述步骤(3)的a中的3个误差，共2n+3个误差，使用经典的测程法可以计算出机器人的当前位姿，计算的位姿即为机器人位姿的最优估计；
d.机器人在运动过程中根据UWB定位数据以及经典的GPS定位算法得到一个二自由度的定位坐标，此处只考虑机器人做平面平行运动，该坐标与基于测程法计算出的位姿中的位置坐标必然不相等，存在一个距离偏差dis，上述2n+3个误差加上距离偏差dis总共2n+4个误差,此2n+4个误差的自由度是2n+3；
e.定义2n+4个误差的二范数为位姿估计总误差，或者说位姿估计的评价函数，考虑到前2n+3个误差将决定最后一个误差，则位姿估计评价函数可以看作前2n+3个误差的函数，优化目标为位姿估计评价函数取值最小；
f.对2n+3个自变量的位姿估计评价函数求偏微分，得到梯度，向负梯度方向迭代，直到函数值最小，此时使用前2n+3个误差，加上前2n+3个测量值得到对2n+3个真值的估计，采用2n+3个估计并使用测程法计算出最后对机器人位姿的估计，最后对机器人位姿的估计是在二范数意义下对机器人位姿的最优估计。
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 CN106352879A (zh) | 2017-01-25 |
| CN106352879B true 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) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107065858A (zh) * | 2017-02-13 | 2017-08-18 | 四川超影科技有限公司 | 基于超宽带的巡检机器人导航方法 |
| CN110650435B (zh) * | 2018-12-13 | 2021-04-30 | 杭州电子科技大学 | 基于UWB定位和ZigBee定位的机器学习融合方法 |
| CN111708010B (zh) * | 2019-03-01 | 2024-04-12 | 北京图森智途科技有限公司 | 一种可移动设备的定位方法、装置、***及可移动设备 |
| CN110186459B (zh) * | 2019-05-27 | 2021-06-29 | 深圳市海柔创新科技有限公司 | 导航方法、移动载体及导航*** |
| CN112833876B (zh) * | 2020-12-30 | 2022-02-11 | 西南科技大学 | 一种融合里程计与uwb的多机器人协作定位方法 |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8098150B2 (en) * | 2007-05-25 | 2012-01-17 | Palo Alto Research Center Incorporated | Method and system for locating devices with embedded location tags |
| US20140304107A1 (en) * | 2012-12-03 | 2014-10-09 | CLARKE William McALLISTER | Webrooming with rfid-scanning robots |
| CN103389087B (zh) * | 2013-08-07 | 2016-06-15 | 上海海事大学 | 一种轮式机器人位姿解算方法 |
| CN104020444B (zh) * | 2014-06-23 | 2016-03-23 | 西北工业大学 | 一种双轮差动式机器人室内组合定位方法 |
| CN104590414B (zh) * | 2014-12-29 | 2017-11-14 | 合肥工业大学 | 一种具有相对位姿检测功能的全方位轮式移动机器人 |
| CN105547305B (zh) * | 2015-12-04 | 2018-03-16 | 北京布科思科技有限公司 | 一种基于无线定位和激光地图匹配的位姿解算方法 |
| CN105783913A (zh) * | 2016-03-08 | 2016-07-20 | 中山大学 | 一种融合车载多传感器的slam装置及其控制方法 |
| CN105828431A (zh) * | 2016-04-25 | 2016-08-03 | 上海理工大学 | 基于uwb的自主跟随机器人定位方法及*** |
-
2016
- 2016-09-28 CN CN201610856905.8A patent/CN106352879B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN106352879A (zh) | 2017-01-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106352879B (zh) | 一种基于图优化的uwb定位与编码器融合的位姿估计方法 | |
| CN104567931B (zh) | 一种室内惯性导航定位的航向漂移误差消除方法 | |
| CN106979780B (zh) | 一种无人车实时姿态测量方法 | |
| CN103412565B (zh) | 一种具有全局位置快速估计能力的机器人定位方法 | |
| US10288431B2 (en) | Device for estimating moving object travel direction and method for estimating travel direction | |
| CN103162689B (zh) | 辅助车载定位***及车辆的辅助定位方法 | |
| CN108844533A (zh) | 一种基于多传感器融合和姿态解算的自由姿态pdr定位方法 | |
| CN108362282A (zh) | 一种基于自适应零速区间调整的惯性行人定位方法 | |
| CN104197935B (zh) | 一种基于移动智能终端的室内定位方法 | |
| CN105509765A (zh) | 一种惯性/dvl/usbl安装误差标定方法 | |
| CN109870173A (zh) | 一种基于校验点的海底管道惯性导航***的轨迹修正方法 | |
| CN104424655A (zh) | 点云曲面重构***及方法 | |
| CN109931955A (zh) | 基于状态相关李群滤波的捷联惯性导航***初始对准方法 | |
| CN103776446A (zh) | 一种基于双mems-imu的行人自主导航解算算法 | |
| CN104697523A (zh) | 基于迭代计算的惯性/地磁匹配定位方法 | |
| KR101576424B1 (ko) | 실내 측위를 위한 지자기 센서 자동 보정 방법 | |
| JP2014149211A (ja) | オフセット推定装置及びプログラム | |
| CN111982099A (zh) | 机器人混合定位方法、装置、设备及计算机可读介质 | |
| CN104331078A (zh) | 一种基于位置映射算法的多机器人协作定位方法 | |
| CN106979779A (zh) | 一种无人车实时姿态测量方法 | |
| CN103076023A (zh) | 一种步长计算方法和装置 | |
| CN109000639A (zh) | 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置 | |
| CN204329976U (zh) | 一种机器人航向测定装置及包含该装置的机器人 | |
| CN107463871A (zh) | 一种基于角特征加权的点云匹配方法 | |
| KR101301462B1 (ko) | 저가형 관성 센서를 이용한 보행자 관성 항법 장치 및 그 항법 |
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 |