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