本发明涉及基于rfid移动机器人定位技术领域，具体地，涉及一种基于rfid的移动机器人位姿估计方法。
背景技术：
随着机器人在人们日常生活应用的发展，室内移动服务机器人的研究成为热点。在室内智能机器人定位感知系统中，机器人的自身定位与位姿估计能力对于路径规划与运动控制极其重要，是机器人实现自主导航的关键，对于提高机器人的自动化水平具有重要的意义。目前，gps被广泛应用在移动机器人定位系统中，但由于受到价格成本、功率消耗、可扩展性等因素的影响，面对复杂的室内环境，容易产生定位较大误差，不能够对目标进行实时跟踪定位。
基于视觉的位姿测量系统，利用车载摄像机拍摄到的周围环境中的特定物体作为参考点，实现移动机器人的自主定位与位姿估计，然而此方法易受光照等外部环境的影响。激光测距传感器和超声波测距传感器系统借助机器人与周围物体之间的距离，提供精确的位置信息位，然而易受到多径效应和非视距效应的影响，存在定位误差是一个不可逾越的障碍。
按照接收无线信号强度来定位是当今研究的热点定位技术。基于信号强度的定位方法无须额外硬件，检测设备和机制简单，硬件成本低，容易实现，利用对接收无线信号的强度，推算收发节点间的距离，因而基于信号强度的定位技术在室内无线传感器网络定位技术中较为常用。
视频识别技术(rfid)是一种非接触式传感器技术，因其具有传统的传感器技术所不具备的优点而越来越引起人们的关注。rfid技术不受光线强度的影响，不受浓烟、浓雾、灰尘的影响，因此广泛应用于工厂、煤矿以及复杂的室内环境中。
因此，本发明提出了一种基于rfid信号强度的移动机器人位置定位与位姿估计方法，将基于rfid信号强度的机器人定位系统与基于极大似然估计的机器人定位算法相结合，并将加权最小二乘算法应用到机器人坐标的极大似然方程组求解中，实现了两种算法的融合，有效地提高了定位精度，且系统受环境的影响小，定位误差比较稳定。
技术实现要素：
本发明的目的在于，针对上述问题，提出一种基于rfid的移动机器人位姿估计方法，以实现有效地提高定位精度，系统受环境的影响小以及定位误差稳定的优点。
为实现上述目的，本发明采用的技术方案是：一种基于rfid的移动机器人位姿估计方法，主要包括：
步骤100：基于rfid接收无线信号强度的位置坐标估计算法；
步骤200：移动机器人方位角θ的估计。
进一步地，所述步骤100具体包括：
步骤110：由自适应极大似然估计方法建立机器人坐标的极大似然估计法模型；
步骤120：用加权最小二乘法对极大似然方程组进行求解。
进一步地，所述步骤110具体包括：
步骤111：应用对数常态分布传播模型，得出标签到读写器的距离d与读写器接收到的标签的信号强度值p(d)之间的函数关系；
步骤112：根据信号强度值的正态分布函数，获取参考标签的信号强度；
步骤113：采用自适应极大似然估计法计算位置坐标。
进一步地，所述步骤111具体包括：
利用无线信号传播原理，在理想状态下，应用对数常态分布传播模型，可以得出标签到读写器的距离d与读写器接收到的标签的信号强度值p(d)之间的函数关系为：
式中，n为信号传播常量，p(d0)为到d＝1m时的信号强度值；在一定环境中，p(d0)与n的值都是已知的，所以通过测量p(d)值就能够得出d的值。
进一步地，所述步骤112具体包括：
根据统计学理论，在某个距离下的信号强度值满足正态分布，概率值越大，说明测量的信号强度值与真实值越接近。因此对读取到的信号强度值进行优先处理。正态分布密度函数为
极大似然函数为
其中，μ、δ2是多次测量的均值和标准差。如果测量个数为m，则μ与δ2的极大似然估计值为
其中，xi为第i个rfid标签的信号强度值。将实测的信号强度值带入式(4)，求出μ和δ2，然后带入式(3)，得到l(μ,δ)的值，保留l(μ,δ)值为0.5～1的实测值，对其求平均，作为机器人读取到的参考标签的信号强度。
进一步地，所述步骤113具体包括：
a：车载rfid阅读器在机器人运动过程中，根据接收的无线信号搜索地面上的rfid标签；
b：机器人每次读取的rfid标签的数目是自适应变化的，根据rfid标签健康状态与rfid信号强度门限值选取不同的rfid标签的数目；
c：根据接收到的无线信号强度值剔除工作不正常的rfid标签，并排除无线信号强度值偏低的rfid标签；
d：不进行几何配置的选择，根据极大似然估计法模型，直接利用留下的n个rfid标签信息进行最大似然坐标估计。
进一步地，所述步骤113中所述步骤d具体包括：
已知1,2,3,l,n个节点的坐标为(x1,y1),(x2,y2),l,(xn,yn)，以及它们到未知节点t的距离分别为d1,d2,l,dn。假设未知节点t的坐标为(x,y)，则
式(5)中个方程分别减去最后一个方程可得：
式(6)可表示为如下线性方程的形式：axt＝b，其中：
x＝[xy]。
进一步地，所述步骤120具体包括：
下面利用加权最小二乘法(weightedleastsquare)求线性方程axt＝b的解为
x＝(atwa)-1atwb(7)
一般来说，权值的选择通常根据各次测量的精度来选择，应保证与未知rfid标签节点越远的点，因其精度下降，它所占的权重越小，权值选取如下：
式中，k为路径衰减因子，di,i＝1,2l,n-1为第i个rfid标签到阅读器的距离。
进一步地，所述步骤200具体包括：
根据机器人的运动几何模型，确定机器人的坐标并计算机器人的方向角；
在基于rfid信号强度的室内移动机器人位置坐标估计系统中，依据机器人在移动过程中的几何模型，可知，如果机器人在初始位置a的位姿是已知的，表示为a(x0,y0,θ0)，经过一个采样周期t时间后，机器人到达b点位置，由式(7)可计算出机器人在b点的坐标(xk,yk)，在b点的位姿表示为(xk,yk,θk)。由余弦定理可得机器人在这个过程中旋转角度为
式中，r为某时刻k机器人的转弯半径，如果机器人左右轮的速度为v1与v2，由机器人车轮上的光电编码盘测得，机器人的宽度为b，r与l的计算公式为
关于机器人位姿中的方向角θ的估计，有如下关系：
θk＝θ0+φk
(12)。
本发明的有益技术效果：
本发明的一种基于rfid的移动机器人位姿估计方法，主要包括：步骤100：基于rfid接收无线信号强度的位置坐标算法；步骤200：移动机器人方位角θ的估计。基于rfid信号强度的机器人定位系统与基于极大似然估计的机器人定位算法相结合，并将加权最小二乘算法应用到机器人坐标的极大似然方程组求解中，实现了两种算法的融合，有效地提高了定位精度，且系统受环境的影响小，定位误差比较稳定。
本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。
下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。
附图说明
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：
图1为本发明所述一种基于rfid的移动机器人位姿估计方法的机器人控制系统结构示意图；
图2为本发明所述一种基于rfid的移动机器人位姿估计方法的系统实验装置示意图；
图3为本发明所述一种基于rfid的移动机器人位姿估计方法的自适应极大似然估计法位置坐标计算方法流程图；
图4为本发明所述一种基于rfid的移动机器人位姿估计方法的极大似然估计法模型结构示意图；
图5为本发明所述一种基于rfid的移动机器人位姿估计方法的运动中的机器人几何模型示意图。
具体实施方式
以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。
如图2所示，本发明实验前，固定rfid标签，移动rfid阅读器，每隔5cm测量信号强度值，对每个位置测量20次取平均值，通过曲线拟合得出式(1)中的p(d0)＝-45.63，n＝1.832。实验过程中，机器人在规划点处连续20次读取每个参考标签的信号强度值。
一种基于rfid的移动机器人位姿估计方法，主要包括：
步骤100：基于rfid接收无线信号强度的位置坐标估计算法；
步骤200：移动机器人方位角θ的估计。
所述步骤100具体包括：
步骤110：由自适应极大似然估计方法建立机器人坐标的极大似然估计法模型；
步骤120：用加权最小二乘法对极大似然方程组进行求解。
所述步骤110具体包括：
步骤111：应用对数常态分布传播模型，得出标签到读写器的距离d与读写器接收到的标签的信号强度值p(d)之间的函数关系；
步骤112：根据信号强度值的正态分布函数，获取参考标签的信号强度；
步骤113：采用自适应极大似然估计法计算位置坐标。
所述步骤111具体包括：
利用无线信号传播原理，在理想状态下，应用对数常态分布传播模型，可以得出标签到读写器的距离d与读写器接收到的标签的信号强度值p(d)之间的函数关系为：
式中，n为信号传播常量，p(d0)为到d＝1m时的信号强度值；在一定环境中，p(d0)与n的值都是已知的，所以通过测量p(d)值就能够得出d的值。
所述步骤112具体包括：
根据统计学理论，在某个距离下的信号强度值满足正态分布，概率值越大，说明测量的信号强度值与真实值越接近。因此对读取到的信号强度值进行优先处理。正态分布密度函数为
极大似然函数为
其中，μ、δ2是多次测量的均值和标准差。如果测量个数为m，则μ与δ2的极大似然估计值为
其中，xi为第i个rfid标签的信号强度值。将实测的信号强度值带入式(4)，求出μ与δ2，然后带入式(3)，得到l(μ,δ)的值，保留l(μ,δ)值为0.5～1的实测值，对其求平均，作为机器人读取到的参考标签的信号强度。
如图3所示，所述步骤113具体包括：
a：车载rfid阅读器在机器人运动过程中，根据接收的无线信号搜索地面上的rfid标签；
b：机器人每次读取的rfid标签的数目是自适应变化的，根据rfid标签健康状态与rfid信号强度门限值选取不同的rfid标签的数目；
c：根据接收到的无线信号强度值剔除工作不正常的rfid标签，并排除无线信号强度值偏低的rfid标签；
d：不进行几何配置的选择，根据极大似然估计法模型，直接利用留下的n个rfid标签信息进行最大似然坐标估计。
所述步骤113中所述步骤d具体包括：
根据图4所示的极大似然估计法模型，已知1,2,3,l,n个节点的坐标为
(x1,y1),(x2,y2),l,(xn,yn)，以及它们到未知节点t的距离分别为d1,d2,l,dn。假设未知节点t的坐标为(x,y)，则
式(5)中个方程分别减去最后一个方程可得：
式(6)可表示为如下线性方程的形式：axt＝b，其中：
x＝[xy]。
所述步骤120具体包括：
下面利用加权最小二乘法(weightedleastsquare)求线性方程axt＝b的解为
x＝(atwa)-1atwb(7)
一般来说，权值的选择通常根据各次测量的精度来选择，应保证与未知rfid标签节点越远的点，因其精度下降，它所占的权重越小，权值选取如下：
式中，k为路径衰减因子，di,i＝1,2l,n-1为第i个rfid标签到阅读器的距离。
所述步骤200具体包括：
根据机器人的运动几何模型，确定机器人的坐标并计算机器人的方向角；
在基于rfid信号强度的室内移动机器人位置坐标估计系统中，依据机器人在移动过程中的几何模型，如图5可知，如果机器人在初始位置a的位姿是已知的，表示为a(x0,y0,θ0)，经过一个采样周期t时间后，机器人到达b点位置，由式(7)可计算出机器人在b点的坐标(xk,yk)，在b点的位姿表示为(xk,yk,θk)。由余弦定理可得机器人在这个过程中旋转角度为
式中，r为某时刻k机器人的转弯半径，如果机器人左右轮的速度为v1与v2，由机器人车轮上的光电编码盘测得，机器人的宽度为b，r与l的计算公式为
关于机器人位姿中的方向角θ的估计，有如下关系：
θk＝θ0+φk(12)。
至少可以达到以下有益效果：
本发明的一种基于rfid的移动机器人位姿估计方法，主要包括：步骤100：基于rfid接收无线信号强度的位置坐标算法；步骤200：移动机器人方位角θ的估计。基于rfid信号强度的机器人定位系统与基于极大似然估计的机器人定位算法相结合，并将加权最小二乘算法应用到机器人坐标的极大似然方程组求解中，实现了两种算法的融合，有效地提高了定位精度，且系统受环境的影响小，定位误差比较稳定。
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。