[发明专利]基于UWB定位及航位推算的室内AGV导航控制方法

摘要

本发明公开了一种基于UWB定位及航位推算的室内AGV导航控制方法，单个待定位节点与单个参考节点之间的距离测量；单个待定位节点与多个参考节点之间的距离测量；待定位节点的定位；航向角的测量；AGV行驶距离的测量；航位推算的定位导航以及组合定位导航几个控制步骤，其中待定位节点的定位利用内四边形质心法定位，通过事先设定好的通讯机制，测量单个待定位节点与多个参考节点之间的信号飞行时间，从而计算距离；最后，通过内四边形质心算法来实现定位计算，分别以各个参考节点为圆心，相应的测量距离为半径画圆，利用四个相近的圆交点，组成四边形，再通过定位修正因子来计算得到内四边形的四个顶点，计算得到待定位节点的估计坐标。

主权项

1.一种基于UWB定位及航位推算的室内AGV导航控制方法，其特征在于：包括：单个待定位节点与单个参考节点之间的距离测量：通过设置在待定位节点与参考节点上的用于UWB无线信号传输的模组来获取UWB信号在待定位节点与参考节点上两节间发送与接收信号的时间，从而获取信号在两节点间的传播时间，再与传播速度相乘就得到所测的单个待定位节点与单个参考节点之间的距离；单个待定位节点与多个参考节点之间的距离测量：待定位节点使用广播方式，向各个参考节点传输信号，分配各参考节点的响应时间；参考节点在接收待定位节点的广播帧后，重新进入接收状态，等待待定位节点的请求信号；待定位节点发送请求帧后，进入接收状态，等待参考节点响应信号；各参考节点依据通信前广播帧分配的响应时间，依序向带定位节点作出回应，发送成功后进入接收状态，等待待定位节点终止帧信号；待定位节点接收完各参考节点响应帧后，将各时间节点打包形成终止帧，以广播方式发送给相应参考节点；各参考节点在接收待定位节点的终止帧后，解析数据帧中的有效数据，计算与待定位节点之间距离，隔一段时间，将距离值返送至待定位节点；待定位节点在获取与各参考节点之间的距离值后，使用内四边形质心定位算法软件计算出自身坐标信息；待定位节点的定位：利用内四边形质心法定位，通过事先设定好的通讯机制，测量单个待定位节点与多个参考节点之间的信号飞行时间，从而计算距离；最后，通过内四边形质心算法来实现定位计算，分别以各个参考节点为圆心，相应的测量距离为半径画圆，利用四个相近的圆交点，组成四边形，再通过定位修正因子来计算得到内四边形的四个顶点，计算得到待定位节点的估计坐标；航向角的测量：利用MEMS惯性传感器，测量空间方向上的原始数据，对原始数据通过动态卡尔曼滤波与数据融合算法分离出每一个传感器在受干扰状况下所测量的精确数据，通过精确数据计算得到航向角数据；AGV行驶距离的测量：利用AGV自身的编码器，在已知车轮直径以及行驶一周输出脉冲数，计算得到输出一个脉冲，AGV相应的行驶距离，从而根据一段时间内捕获的脉冲数计算得到相应的行驶距离；航位推算的定位导航：在得到高精度的航向角与行驶距离后，通过相应的方法计算得到平面内x、y轴的变化量，再根据前一时刻位置坐标就可以求出当前位置坐标；组合定位导航：UWB定位数据每隔一定时间获取一次，而航位推算数据都是每隔一定时间获取一次，将UWB获取的定位坐标与同时获得的航位推算坐标之间的距离为路径误差d与ε值相比较，ε为设定路径误差；当d＞ε时，直接将UWB定位坐标赋值给航位推算所得AGV当前的坐标，作为新的起点，对下一次航位进行推算；否则，以航位推算出来的AGV当前坐标为导航结果继续推算下去。