1.一种基于超宽带技术的多移动机器人高精度协同跟踪方法,其特征在于,包括以下步骤:步骤1,在机器人操作系统ROS下搭建多移动机器人协同跟踪实验平台;
所述的步骤1,具体做法是:
在机器人操作系统ROS下搭建多移动机器人协同跟踪实验平台,即利用主控机远程控制多个移动机器人随机运动,机器人在运动过程中能够有效避开环境中的所有静态以及随机出现的动态障碍物;当机器人检测到动态障碍物时,对局部路径进行修改,从而有效避开动态障碍物,继续向目标位置运动;最终多个机器人之间相互配合,分别到达各自的目标位置;
步骤2,利用超宽带传感器建立多节点间测距网络,同时获得机器人与机器人,机器人与锚点之间的距离信息,距离信息包括到达时间、测距值和接收信号强度;
步骤3,采用基于贝叶斯滤波的超宽带测距误差削弱算法,对步骤2中获得的测距值的LOS和NLOS误差进行有效削弱,还原距离真值;
所述的步骤3,具体做法是:
步骤3.1,在各种传播和障碍物材质下采集大量数据,包括测量距离 和接收信号强度,建立概率传感器模型RSS和TOA;
所述的步骤3.1中,概率传感器建模过程如下:为了建立UWB传感器模型,在LOS和NLOS情况下,采集大量数据,包括:到达时间 ,测量距离 和接收信号强度 ;NLOS情况包括木板障碍物NLOSW和金属障碍物NLOSM;采集建模数据的原则为:通信节点间真实欧式距离为 时, 为所采集建模数据的组数,在每一真实距离下采集大量数据,最终得到 组测量数据,建立概率传感器模型RSS和TOA;
建立概率传感器模型RSS:
概率传感器模型RSS表示为 ,其中 ,表示传播和障碍物材质属性,分别代表无障碍物、木板障碍物和金属障碍物,对应于室内环境中的三种情况,即无NLOS效应、微弱NOLS效应、较强NLOS效应, 表示接收信号强度,表示利用TOA方法得到的测量距离;
首先,以横轴为测量距离,纵轴为接收信号的强度,将坐标系按一定大小范围划成大小相同的格子,其中,横轴被换分成了 个等范围区间,纵轴被划分成了Nzrss个等范围区间;
其次,将采集到的每对测量值投入到相应格子中,每对测量值包括测量距离值与接收信号强度,格子里的数值加1;最终,每个格子里的数除以每列测量数据总数就可以得到概率传感器模型RSS,此模型可以用 大小的概率矩阵和概率分布图来表示;
建立概率传感器模型TOA:
TOA模型表示为 ,其中 ,表示两个通信节点之间的真实欧式距离,表示利用TOA方法得到的测量距离;
与概率传感器模型RSS的建模过程类似,首先,以横轴为真实距离,纵轴为测量距离,将坐标系按一定大小范围划成大小相同的格子,其中横轴被划分成了 个等范围区间,纵轴被划分成了 个等范围区间;其次,将采集到的测量距离值投到相应的格子里,格子里数值加1;最终,每个格子里的数除以每列数据总数就得到概率传感器模型TOA,此模型可以用大小的概率矩阵和概率分布图来表示;
步骤3.2,在贝叶斯滤波框架下利用概率传感器模型RSS对障碍物材质进行识别,估计出测量距离 在任意传播下所测量得到的概率;
所述的步骤3.2,具体做法是:在贝叶斯滤波框架下利用所建立的概率传感器模型RSS,对通信节点之间的障碍物属性进行识别,在此基础上结合概率传感器模型TOA模型对UWB测距误差进行削弱,得到准确的测距估计值;
1)预测:
(1)
其中, 是 时刻到时刻的状态转移函数, 是从初始时刻到 时刻的测量数据集, 是 时刻的后验状态, 是时刻的预测状态;状态的转移遵循马尔科夫过程;
后验状态 可被分解为:
(2)
定义状态转移函数 下一时刻维持上一时刻状态的概率为 ,而转移到其他状态的概率是等几率的;上一时刻状态转移到其他两种状态下的概率都为 ;
因此,时刻的预测状态 表示为:
(3)
假设每一时刻维持当前状态以及转换到其他两种状态的概率值保持不变,这是因为状态转移函数与测距节点的位置以及其他状态信息是相互独立的,因此不需要环境或障碍物布局作为先验信息;
2)更新:
给定时刻的测量数据,包括接收信号强度和测量距离, ,利用所建立的概率传感器模型可以获得距离似然函数,如式(4)所示:(4)
其中, 和 是利用RSS概率传感器模型和TOA概率传感器模型来获得;
利用距离似然函数对贝叶斯滤波进行更新,如式(5)所示:(5)
通过计算从式(5)中可以进一步提取出t时刻的后验状态 , 表示给定时刻的测量数据后利用所建立的概率传感器模型获得的距离似然函数,如式(6)所示:(6)
式中, 是测量距离在第i个真实距离 下与测量数据 中两通信节点之间的障碍物材质属性 的联合概率分布,由式(6)可知边缘概率 , 和
,描述了两个UWB通信节点之间的障碍物材质情况,即测量数据在各种传播和障碍物材质属性下的概率;该概率也将作为下一时刻贝叶斯滤波的先验状态,对下一时刻的障碍物材质进行识别;
步骤3.3,结合概率传感器模型TOA获得距离概率密度函数,通过求取期望即可得到测距误差削弱后的距离估计值;
步骤4,采用协同跟踪算法,即基于Gibbs采样的协同粒子滤波算法对多个移动机器人的位置信息进行估计;
步骤5,采用OptiTrack运动捕获系统获得多个机器人相应运动的真实轨迹,对协同跟踪算法进行评价。
2.根据权利要求1所述的一种基于超宽带技术的多移动机器人高精度协同跟踪方法,其特征在于,所述的步骤3.3,具体做法是:在贝叶斯滤波框架下对障碍物材质进行识别之后,结合概率传感器模型TOA可对UWB测距误差进行削弱,对状态 进行边缘化之后可以求得在各种传播和障碍物材质下的测量距离在第个真实距离下的概率:(7)
式中, 是测量距离在第i个真实距离 下与测量数据 中两通信节点之间的障碍物材质属性 的联合概率分布,
最终,通过归一化处理求取期望即可得到精确的距离估计值:(8)
式中,为第i个真实距离, 为测量距离在第i个真实距离下的概率值,n为在各种传播和障碍物材质下距离测量总次数;
对UWB测距误差有效削弱之后,将得到的精确距离估计值融合到协同跟踪算法中,提高多机器人定位精度。
3.根据权利要求1所述的一种基于超宽带技术的多移动机器人高精度协同跟踪方法,其特征在于,所述的步骤5,具体做法是:在多机器人协同跟踪系统中,将获得的机器人与机器人、机器人与锚点之间的精确距离信息融合到协同跟踪算法中,定出每个机器人在任意时刻的位置信息,为了对协同跟踪结果进行评价,在OptiTrack运动捕获系统下获得每个机器人相应运动的真实轨迹。