基于卡尔曼滤波算法的室内环境基站坐标位置标定方法与流程
本发明涉及一种基于卡尔曼滤波算法的室内环境基站坐标位置标定方法。
背景技术：
移动机器人代表了机电一体化的较高水平，该机器人的广泛应用以自主导航作为前提，而导航精度离不开较高的定位精度。在室内环境中布置传感器网络可以较好完成机器人的定位与导航功能。
现有技术中，通常采用多个距离信息组成多个距离方程，使用最小二乘法求出目标点的坐标，从而使得方程的残差最小；但是由于室内环境的复杂性，传感器网络在测距过程中由于噪声干扰，所得到的距离信息会与实际距离信息产生较大差异，从而会使得定位信息存在较大偏差。因此，在存在测量误差的情况下，保证定位精度就成为本发明亟待解决的问题。
目前大部分的定位算法旨在已知基站位置，从而估计被测目标的位置信息。然而在较大的空间中，标定基站位置成为一个重要难题，最经典的方法是将移动目标放置在三个固定且已知的位置上，然后用三点测量法来对基站的位置进行估计。由于噪声的存在，该种方法并不能得到非常准确的位置信息，定位精度较差。
技术实现要素：
本发明所要解决的技术问题是提供一种基于卡尔曼滤波算法的室内环境基站坐标位置标定方法，本方法克服传统定位方法的缺陷，能够较为准确的完成室内复杂环境下的基站位置标定，并且抗干扰能力较强，有效提高定位精度。
为解决上述技术问题，本发明基于卡尔曼滤波算法的室内环境基站坐标位置标定方法包括如下步骤：
步骤一、设置基于无线传感器网络的室内定位系统，其中单目标定位由一个标签和多个基站组成，标签与基站均采用超宽带模块实现超宽频信息传输，所有基站安装在同一水平面上，被定位目标与该水平面定高，将标签安装至移动机器人上，采用直角三角形的几何关系求解标签与基站之间的水平距离：
其中，d为标签与基站之间的距离，δh为标签与基站的的高度差，d水平为标签与基站的水平距离；
步骤二、每个基站与标签之间测距采用tof原理，将所有的基站与标签设置同步时钟，基站向四周发送无线信号，该无线信号中包含发送时刻的时间戳，标签接收到该无线信号时，根据标签的时间戳与接收到无线信息中的时间戳进行比对，通过时间差计算标签与基站之间的距离：
d＝c·(treceive-tsend)(2)
其中，d为标签与基站之间的距离，c为光在真空中的传播速度，treceive为接收到无线信号的时间，tsend为发送无线信号的时间；
步骤三、标签与基站之间的距离信息存在噪声，其描述如下：
dis(t)＝dreal(t)+n(t)(3)
其中，dis(t)为在t时刻所测量的距离值，dreal(t)为t时刻的真实距离，n(t)为满足均值为0、方差为σ2的高斯随机变量；
步骤四、假设在空间中布置基站个数为n个，且第i个基站坐标为(xi,yi)，在空间内标定四个坐标点，四个坐标点分别表示为(xa,ya)，(xb,yb)，(xc,yc)，(xd,yd)，且满足ya＝yb，xb＝xc，yc＝yd，xa＝xd，即四个坐标点连成矩形；
步骤五、将四个移动目标分别放置在四个标定坐标点，并按逆时针方向直线运动至下一个标定点，即目标a从(xa,ya)点运动到(xb,yb)点，目标b从(xb,yb)点运动到(xc,yc)点，目标c从(xc,yc)点运动到(xd,yd)点，目标d从(xd,yd)点运动到(xa,ya)点，且四个目标的运动速度为匀速；在目标运动过程中，每一个采样周期都获取每一个移动目标与所有基站之间的测量距离信息dm,i,k，其中dm,i,k表示目标m在k时刻与第i个基站的测量距离；
步骤六、初步估计每个基站的位置，使用四个目标分别在初始点测量与每一个基站的距离da,i,0，db,i,0，dc,i,0，dd,i,0，使用最小二乘方法初步求取每一个基站的位置信息，第i个基站与四个目标的距离公式方程组如下：
将式(4)的前三个等式分别与第四个等式相减，整理得到下式：
a0yi＝b0(5)
其中，
yi＝[xi,yi]t，其中，[*]t表示矩阵[*]的转置矩阵，
使用最小二乘法求解yi，求解公式如下：
根据式(6)初步求得每一个基站的初始点(xi,0,yi,0)；
步骤七、定义目标状态向量为x＝[x,y,vx,vy]t，其中x和y表示目标的位置坐标，vx和vy表示目标在x方向和y方向的速度，
则四个目标的状态向量分别定义为xa,xb,xc,xd，
采用第一步扩展卡尔曼滤波算法，预测公式如下所示：
xm,k|k-1＝amxm,k-1|k-1(7)
其中，m＝a,b,c或d，分别表示四个目标，xm,k-1|k-1表示k-1时刻的目标m的状态，xm,k|k-1＝[xm,k|k-1,ym,k|k-1,vx,m,k|k-1,vy,m,k|k-1]t为预测阶段目标的状态向量，pm,k-1|k-1表示k-1时刻目标m的状态误差协方差，pm,k|k-1表示预测阶段的协方差矩阵，q表示过程协方差矩阵，设置为对角矩阵，当为首个时刻时，(xi,yi)的值采用基站的初始点(xi,0,yi,0)，hm(xm,k|k-1)表示预测阶段目标m与各个基站的预测距离向量；
当m＝a或c时，矩阵am和hm满足：
当m＝b或d时，矩阵am和hm满足：
其中hm为hm(xm,k|k-1)的雅克比矩阵，hm,i(xm,k|k-1)表示hm(xm,k|k-1)向量的第i个元素，δt为采样周期；
卡尔曼增益公式如下：
其中r为测量协方差矩阵，设置为对角矩阵，
修正公式如下所示：
xm,k|k＝xm,k|k-1+km,k(mm,k-hm)(11)
pm,k|k＝pm,k|k-1-km,khmpm,k|k-1(12)
其中，mm,k为目标m在k时刻与所有基站的距离测量值所组成的列向量，xm,k|k为修正后的目标m的状态向量，pm,k|k为修正后的误差协方差矩阵，hm表示预测阶段目标m与各个基站的预测距离向量；
步骤八、根据步骤七获取到目标a,b,c,d的位置信息，(xa,k,ya,k),(xb,k,yb,k),(xc,k,yc,k),(xd,k,yd,k)，
采用第二步扩展卡尔曼滤波算法，
xi,k|k-1＝aixi,k-1|k-1(13)
其中，
其中，hi为hi(xi,k|k-1)的雅克比矩阵，hi,p(xi,k|k-1)为hi(xi,k|k-1)列向量的第p个元素；
卡尔曼增益公式如下：
其中r′为测量协方差矩阵，设置为对角矩阵，
修正公式如下：
xi,k|k＝xi,k|k-1+ki,k(mi,k-hi)(18)
pi,k|k＝pi,k|k-1-ki,khipi,k|k-1(19)
其中，mi,k为基站i在k时刻与所有目标的距离测量值所组成的列向量，xi,k|k为修正后的基站i的状态向量，pi,k|k为修正后的误差协方差矩阵；
步骤九、在每一个采样时刻，使用步骤七和步骤八进行迭代，直至四个目标点到达其对应的终点，当四个目标点到达目标点之后，每一个基站的状态向量的位置信息即为优化后基站的坐标位置信息。
由于本发明基于卡尔曼滤波算法的室内环境基站坐标位置标定方法采用了上述技术方案，即本方法采用无线传感器网络构建室内定位系统，所有基站安装在同一水平面上，被定位目标与该水平面定高，将标签安装至移动机器人上，采用直角三角形的几何关系求解标签与基站之间的水平距离；所有基站与标签设置同步时钟，基站向四周发送包含发送时刻的时间戳的无线信号，标签接收到该无线信号时，通过时间差计算标签与基站之间的距离；描述标签与基站之间距离信息的噪声，设定在空间中布置基站个数及坐标，并在空间内标定四个连成矩形的坐标点，移动目标沿坐标点运动，并在每个采样周期获取移动目标与所有基站之间的测量距离信息；使用移动目标在初始点测量与每一个基站的距离，初步估计每个基站的位置；经二步扩展卡尔曼滤波算法得到优化后基站的坐标位置信息。本方法克服传统定位方法的缺陷，能够较为准确的完成室内复杂环境下的基站位置标定，并且抗干扰能力较强，有效提高定位精度。
附图说明
下面结合附图和实施方式对本发明作进一步的详细说明：
图1为本方法步骤四中四个坐标点连成矩形示意图。
具体实施方式
本发明基于卡尔曼滤波算法的室内环境基站坐标位置标定方法包括如下步骤：
步骤一、设置基于无线传感器网络的室内定位系统，其中单目标定位由一个标签和多个基站组成，标签与基站均采用超宽带模块实现超宽频信息传输，所有基站安装在同一水平面上，被定位目标与该水平面定高，将标签安装至移动机器人上，采用直角三角形的几何关系求解标签与基站之间的水平距离：
其中，d为标签与基站之间的距离，δh为标签与基站的的高度差，d水平为标签与基站的水平距离；
步骤二、每个基站与标签之间测距采用tof原理，将所有的基站与标签设置同步时钟，基站向四周发送无线信号，该无线信号中包含发送时刻的时间戳，标签接收到该无线信号时，根据标签的时间戳与接收到无线信息中的时间戳进行比对，通过时间差计算标签与基站之间的距离：
d＝c·(treceive-tsend)(2)
其中，d为标签与基站之间的距离，c为光在真空中的传播速度，treceive为接收到无线信号的时间，tsend为发送无线信号的时间；
步骤三、标签与基站之间的距离信息存在噪声，其描述如下：
dis(t)＝dreal(t)+n(t)(3)
其中，dis(t)为在t时刻所测量的距离值，dreal(t)为t时刻的真实距离，n(t)为满足均值为0、方差为σ2的高斯随机变量；
步骤四、假设在空间中布置基站个数为n个，且第i个基站坐标为(xi,yi)，在空间内标定四个坐标点，四个坐标点分别表示为(xa,ya)，(xb,yb)，(xc,yc)，(xd,yd)，且满足ya＝yb，xb＝xc，yc＝yd，xa＝xd，即四个坐标点连成矩形；
步骤五、将四个移动目标分别放置在四个标定坐标点，并按逆时针方向直线运动至下一个标定点，即目标a从(xa,ya)点运动到(xb,yb)点，目标b从(xb,yb)点运动到(xc,yc)点，目标c从(xc,yc)点运动到(xd,yd)点，目标d从(xd,yd)点运动到(xa,ya)点，且四个目标的运动速度为匀速；在目标运动过程中，每一个采样周期都获取每一个移动目标与所有基站之间的测量距离信息dm,i,k，其中dm,i,k表示目标m在k时刻与第i个基站的测量距离；
步骤六、初步估计每个基站的位置，使用四个目标分别在初始点测量与每一个基站的距离da,i,0，db,i,0，dc,i,0，dd,i,0，使用最小二乘方法初步求取每一个基站的位置信息，第i个基站与四个目标的距离公式方程组如下：
将式(4)的前三个等式分别与第四个等式相减，整理得到下式：
a0yi＝b0(5)
其中，
yi＝[xi,yi]t，其中，[*]t表示矩阵[*]的转置矩阵。，
使用最小二乘法求解yi，求解公式如下：
根据式(56)初步求得每一个基站的初始点(xi,0,yi,0)；
步骤七、定义目标状态向量为x＝[x,y,vx,vy]t，其中x和y表示目标的位置坐标，vx和vy表示目标在x方向和y方向的速度，
则四个目标的状态向量分别定义为xa,xb,xc,xd，
采用第一步扩展卡尔曼滤波算法，预测公式如下所示：
xm,k|k-1＝amxm,k-1|k-1(7)
其中，m＝a,b,c或d，分别表示四个目标，xm,k-1|k-1表示k-1时刻的目标m的状态，xm,k|k-1＝[xm,k|k-1,ym,k|k-1,vx,m,k|k-1,vy,m,k|k-1]t为预测阶段目标的状态向量，pm,k-1|k-1表示k-1时刻目标m的状态误差协方差，pm,k|k-1表示预测阶段的协方差矩阵，q表示过程协方差矩阵，设置为对角矩阵，(xi,yi)为第i个基站的位置信息，当为首个时刻时，(xi,yi)的值采用基站的初始点(xi,0,yi,0)，hm(xm,k|k-1)表示预测阶段目标m与各个基站的预测距离向量；
当m＝a或c时，矩阵am和hm满足：
当m＝b或d时，矩阵am和hm满足：
其中hm为hm(xm,k|k-1)的雅克比矩阵，hm,i(xm,k|k-1)表示hm(xm,k|k-1)向量的第i个元素，δt为采样周期；
卡尔曼增益公式如下：
其中r为测量协方差矩阵，设置为对角矩阵，
修正公式如下所示：
xm,k|k＝xm,k|k-1+km,k(mm,k-hm)(11)
pm,k|k＝pm,k|k-1-km,khmpm,k|k-1(12)
其中，mm,k为目标m在k时刻与所有基站的距离测量值所组成的列向量，xm,k|k为修正后的目标m的状态向量，pm,k|k为修正后的误差协方差矩阵，hm表示预测阶段目标m与各个基站的预测距离向量；
步骤八、根据步骤七获取到目标a,b,c,d的位置信息，(xa,k,ya,k),(xb,k,yb,k),(xc,k,yc,k),(xd,k,yd,k)，
采用第二步扩展卡尔曼滤波算法，
xi,k|k-1＝aixi,k-1|k-1(13)
其中，
其中，hi为hi(xi,k|k-1)的雅克比矩阵，hi,p(xi,k|k-1)为hi(xi,k|k-1)列向量的第p个元素；
卡尔曼增益公式如下：
其中r′为测量协方差矩阵，设置为对角矩阵，
修正公式如下：
xi,k|k＝xi,k|k-1+ki,k(mi,k-hi)(18)
pi,k|k＝pi,k|k-1-ki,khipi,k|k-1(19)
其中，mi,k为基站i在k时刻与所有目标的距离测量值所组成的列向量，xi,k|k为修正后的基站i的状态向量，pi,k|k为修正后的误差协方差矩阵；
步骤九、在每一个采样时刻，使用步骤七和步骤八进行迭代，直至四个目标点到达其对应的终点，当四个目标点到达目标点之后，每一个基站的状态向量的位置信息即为优化后基站的坐标位置信息。
本方法利用扩展卡尔曼滤波算法实现室内环境下基站坐标位置的标定，扩展卡尔曼滤波算法的原理描述如下：
构建预测模型：
预测阶段计算：
xk|k-1＝axk-1|k-1
pk|k-1＝apk-1|k-1at+q
其中，xk-1|k-1为k-1时刻坐标信息的最优估计，
校正阶段计算：
kk＝pk|k-1hkt(hkpk|k-1hkt+r)
xk|k＝xk|k-1+kk(mk-hk)
pk|k＝pk|k-1-kkhkpk|k-1
其中，r为测量协方差矩阵，mk为测量的距离向量，pk|k为校正误差协方差矩阵，xk|k即为在k时刻的坐标最优估计。
本方法考虑到传感器网络在测距过程中存在的噪声干扰，基站与标签之间通信采用超宽频传输技术，具有抗干扰性能强、传输速率高、系统容量大、发送功率小、精度高等特点，同时利用扩展卡尔曼滤波算法解决所得到的距离信息与实际距离信息之间产生较大差异的问题，克服定位信息存在较大偏差的缺陷，从而保证了定位精度，完善了机器人的定位与导航功能。
- 还没有人留言评论。精彩留言会获得点赞！