1.一种基于超宽带的自动跟随快速定位方法,在室内固定有两个微基站,其特征在于,包括以下步骤:分别测量标签与两个固定微基站之间发送数据包和接收数据包的时间戳,分别计算出标签与两个微基站之间的相对距离,得出标签的初始方位估计值;
对得到的初始方位估计值采用改进的多重滤波估计算法估计出待定位目标标签距离的最优值;改进主要在于通过设计算法仅用两个基站就实现了自动跟随中定位的需求,可精准快速定位到目标物的位置;
根据测出的距离,采用余弦定理计算出装有基站的平台到待定位目标标签的距离与相对角度,定位待定位目标标签与基站的相对位置,然后获取障碍物位置信息,将这些信息送给跟随物的处理器,通过D*路径规划算法为跟随物进行动态路径规划,运动控制模块根据实时规划的路径,并结合PID算法来控制跟随物的方向与速度,从而达到自动跟随的目的。
2.根据权利要求1所述的基于超宽带的自动跟随快速定位方法,其特征在于,所述两个基站之间的距离是固定的。
3.根据权利要求1所述的基于超宽带的自动跟随快速定位方法,其特征在于,所述改进的多重滤波估计算法包括的步骤为:①读取当前测量到的距离数据;
②由当前距离数据更新当前预测的距离,得到先验估计;
③计算多重滤波增益即一个权衡先验估计与测量值的权重;
④更新系统的距离作为滤波的输出,同时更新先验误差到后验误差;
⑤重复上述步骤。
4.根据权利要求1-3之一所述的基于超宽带的自动跟随快速定位方法,其特征在于,所述测量标签与两个固定微基站之间发送数据包和接收数据包的时间戳,分别计算出标签与两个微基站之间的相对距离,具体包括:通过基站和标签之间4次数据帧的收发,记录时间戳,计算得到每次收发数据的间隔时间,通过式1和式2可以计算得到电磁波在标签与基站A之间传播时间TpropA和在标签与基站B之间传播时间TpropB;
Tround1A表示标签发送数据帧1与标签收到基站A回复数据帧2的时间间隔,Tround2A表示基站A发送数据帧2与基站A收到标签回复数据帧4的时间间隔,Treply1A表示基站A收到数据帧1与基站A发送数据帧2的时间间隔,Treply2A表示标签收到数据帧2与标签发送数据帧4的时间间隔;
Tround1B表示标签发送数据帧1与标签收到基站B回复数据帧3的时间间隔,Tround2B表示基站B发送数据帧3与基站B收到标签回复数据帧4的时间间隔,Treply1B表示基站B收到数据帧1与基站B发送数据帧3的时间间隔,Treply2B表示标签收到数据帧3与标签发送数据帧4的时间间隔,将电磁波传播在基站和标签中传播的时间乘上电磁波在自由空间中的传播速度c,可得出标签分别与两个基站距离基站距离为:Dis1=c×TpropA (3)
Dis2=c×TpropB (4)
5.根据权利要求4所述的基于超宽带的自动跟随快速定位方法,其特征在于,所述根据测出的距离,采用余弦定理计算出装有基站的平台到待定位目标标签的距离与相对角度,定位待定位目标标签的位置,具体包括:已知基站BS1与BS2的距离是固定的,并取其二者的中点作为基准点,测出Dis1与Dis2的大小后,根据余弦定理可以得出α角根据余弦定理同理可得到Dis为:
由此得出标签距离在平台上基站的基准点的距离。
根据余弦定理可以得到θ角为:
6.根据权利要求5所述的基于超宽带的自动跟随快速定位方法,其特征在于,还包括采用采用障碍物检测算法,预判下一个状态的可移动性的步骤,具体包括:采用超声波避障模块,跟随物底部装有多个超声波测距模块,实现各个方向都可以探测到障碍物,实现视野无死角,通过实时感知障碍物来实现自动避障,跟随物在执避障动作的过程中,主要有直线行走和转弯两种状态,相对坐标系的建立是以跟随物自身的姿态划分的,以跟随物上微基站中心为坐标原点,以跟随物的速度方向为坐标轴正方向,用于标记当前障碍物相对于跟随物的位置。
7.根据权利要求5所述的基于超宽带的自动跟随快速定位方法,其特征在于,还包括根据PID控制、D*算法、标签距离与相对角度实现自动跟随的步骤:通过上述算法得到标签距离与相对角度,然后获取障碍物位置信息,将这些信息送给跟随物的处理器,通过D*路径规划算法为跟随物进行动态路径规划,运动控制模块根据实时规划的路径,并结合PID算法来实时控制跟随物的方向与速度,从而达到自动跟随的目的。