一种用于室内煤场的无人机定位导航方法
阅读说明：本技术 一种用于室内煤场的无人机定位导航方法 (Unmanned aerial vehicle positioning and navigation method for indoor coal yard ) 是由 沈润杰 刘晨旭 张建卜 王超 于 2019-10-22 设计创作，主要内容包括：本发明涉及一种用于室内煤场的无人机定位导航方法,包括：步骤S1：建立UWB局部坐标系；步骤S2：建立状态空间方程,将UWB数据与惯性测量单元数据加入优化方程,通过卡尔曼滤波算法进行数据融合,计算出基本信息；步骤S3：检测卫星信号,若存在则将卫星信号转换为局部坐标进行滤波处理,与UWB的位置信息融合获得融合位置坐标；步骤S4：采集图像数据建立先验地图,结合融合位置坐标,使用ICP算法进行重定位,采用ORB-SLAM算法实现导航地图的维护与更新；步骤S5：根据输出的置信度矩阵,与置信度阈值进行比较,若满足则输出导航地图中的具体坐标,否则输出融合位置坐标。与现有技术相比,本发明具有结合多种定位数据,保障导航方法精确性与稳定性等优点。(The invention relates to an unmanned aerial vehicle positioning and navigation method for an indoor coal yard, which comprises the following steps: step S1: establishing a UWB local coordinate system; step S2: establishing a state space equation, adding UWB data and inertial measurement unit data into an optimization equation, performing data fusion through a Kalman filtering algorithm, and calculating basic information; step S3: detecting satellite signals, converting the satellite signals into local coordinates to perform filtering processing if the satellite signals exist, and fusing the local coordinates with UWB position information to obtain fused position coordinates; step S4: acquiring image data to establish a prior map, combining with the fusion position coordinates, using an ICP algorithm to perform relocation, and using an ORB-SLAM algorithm to realize maintenance and update of a navigation map; step S5: and comparing the output confidence matrix with a confidence threshold, if so, outputting specific coordinates in the navigation map, and otherwise, outputting the coordinates of the fusion position. Compared with the prior art, the method has the advantages of combining various positioning data, guaranteeing the accuracy and stability of the navigation method and the like.)
技术领域
本发明涉及导航控制技术领域，尤其是涉及一种用于室内煤场的无人机定位导航方法。
背景技术
随着无人机控制技术发展与成熟，其在警用、城市管理、农业、地质、气象、电力、抢险救灾等行业领域的应用快速增长，市场上成熟的无人机控制技术以GNSS导航为基础，主要应用在露天环境下，而对于GNSS信号较差的室内环境，尚无成熟稳定的导航方法。火电站煤场的盘煤工作是无人机的典型应用场景，随着环保要求的提升，露天煤场逐渐被室内煤场所取代。在GNSS信号微弱的室内煤场环境下，需要一种新的导航技术能够为无人机提供精确地位置信息，保障无人机完成盘煤工作。
发明内容
本发明的目的就是为了克服上述现有技术存在的室内环境下无法为无人机提供精确地位置信息的缺陷而提供一种用于室内煤场的无人机定位导航方法。
本发明的目的可以通过以下技术方案来实现：
一种用于室内煤场的无人机定位导航方法，包括以下步骤：
步骤S1：布置UWB基站，建立UWB定位局部坐标系；
步骤S2：建立UWB基站与惯性测量单元的联合状态空间方程，将UWB数据与惯性测量单元的加速度数据加入统一优化方程中，通过卡尔曼滤波算法进行数据融合，计算出无人机的速度、位置信息；
步骤S3：检测GPS或北斗卫星信号，若存在则将卫星信号转换为局部坐标后进行滤波处理，与UWB定位局部坐标系的位置信息融合后，获得融合位置坐标；
步骤S4：采集煤场顶棚、墙壁等固定结构的图像数据建立先验地图，根据所述先验地图与步骤S3中的融合位置坐标，使用ICP算法进行重定位，并采用ORB-SLAM算法实现先验地图所对应导航地图的维护与更新；
步骤S5：根据ICP算法输出的置信度矩阵，与置信度阈值进行比较，若满足置信度阈值，输出无人机在步骤S4导航地图模型中的具体坐标，否则输出步骤S3的融合位置坐标。
所述卡尔曼滤波的输入值还包括UWB定位局部坐标系输出的无人机的初步UWB位置坐标。
所述GPS或北斗卫星信号与UWB数据融合完成初步定位具体是指通过高斯投影法将大地坐标转化为所述UWB定位局部坐标系的平面坐标。
所述高斯投影法的转换公式具体如下：
l＝L-L0，η＝e′·cosB，t＝tanB
其中，x为平面坐标的横坐标，y为平面坐标的纵坐标，L0为3度带中央子午线经度，e′为椭圆的第二偏心率，L为所求点经度，B为所求点纬度，l、η、t为转换过程的辅助变量，N为卯酉圈子午线半径。
所述UWB位置坐标的信号和GPS或北斗卫星信号经过加权平均处理，得到平均信号强度α和β，根据现场信号强度分析，计算得到良好信号强度阈值α1和β1，归一化处理后得到所述两种信号的权重α′和β′，根据所述权重计算出融合位置坐标。
所述融合位置坐标的计算公式具体为：
X＝α′X1+β′X2
其中，X为无人机的融合位置坐标，X1为UWB位置坐标的信号，X2为GPS或北斗卫星信号。
所述ICP算法的具体步骤为：
步骤S401：在目标点云P中取点集pi∈P；
步骤S402：找出源点云Q中的对应点集qi∈Q，使得||pi-qi||＝min；
步骤S403：计算旋转矩阵R和平移矩阵t，使得误差函数最小；
步骤S404：对pi使用步骤S403求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，得到新的对应点集{p′i＝Rpi+t,pi∈P}；
步骤S405：计算p′i与对应点集qi的平均距离d；
步骤S406：若d小于既定阈值或者迭代超过最大迭代次数，则停止迭代计算,否则返回步骤S402，直到满足上述收敛条件为止。
所述无人机在步骤S4导航地图模型中的具体坐标或步骤S3的融合位置坐标需要使用低通滤波器对不良数据进行滤除。
与现有技术相比，本发明具有以下有益效果：
1.本发明融合了GPS或北斗导航、UWB定位、惯性导航和视觉SLAM等多种定位数据，互为冗余备份，保障了导航方法的精确性与稳定性。
2.本发明针对煤场的实际结构为SLAM过程设计了特殊的先验地图，保障SLAM在实际环境下的可行性。
3.本发明通过多重卡尔曼滤波将UWB信息与惯性测量单元提供的加速度信息进行滤波融合，提高了UWB数据与惯导数据的融合精度。
附图说明
图1为本发明的流程示意图；
图2为本发明GPS或北斗卫星信号与UWB位置坐标融合的流程示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
一种用于室内煤场的无人机定位导航方法，包括以下步骤：
步骤S1：布置UWB基站，建立UWB定位局部坐标系；
步骤S2：建立UWB基站与惯性测量单元的联合状态空间方程，将UWB数据与惯性测量单元的加速度数据加入统一优化方程中，通过卡尔曼滤波算法进行数据融合，计算出无人机的速度、位置信息；
步骤S3：检测GPS或北斗卫星信号，若存在则将卫星信号转换为局部坐标后进行滤波处理，与UWB定位局部坐标系的位置信息融合后，获得融合位置坐标；
步骤S4：采集煤场顶棚、墙壁等固定结构的图像数据建立先验地图，根据先验地图与步骤S3中的融合位置坐标，使用ICP算法进行重定位，并采用ORB-SLAM算法实现先验地图所对应导航地图的维护与更新；
步骤S5：根据ICP算法输出的置信度矩阵，与置信度阈值进行比较，若满足置信度阈值，输出无人机在步骤S4导航地图模型中的具体坐标，否则输出步骤S3的融合位置坐标。
卡尔曼滤波的输入值还包括UWB定位局部坐标系输出的无人机的初步UWB位置坐标。
GPS或北斗卫星信号与UWB数据融合完成初步定位具体是指在工作场地附近选择两点，精确测量其通过GPS或北斗卫星得到的经纬度坐标及其在UWB坐标系下精确地位置坐标，通过高斯投影法将大地坐标转化为UWB定位局部坐标系的平面坐标。
高斯投影法的转换公式具体如下：
l＝L-L0，η＝e′·cosB，t＝tanB
其中，x为平面坐标的横坐标，y为平面坐标的纵坐标，L0为3度带中央子午线经度，e′为椭圆的第二偏心率，L为所求点经度，B为所求点纬度，l、η、t为转换过程的辅助变量，N为卯酉圈子午线半径。
如图2所示，UWB位置坐标的信号和GPS或北斗卫星信号经过加权平均处理，得到平均信号强度α和β，根据现场信号强度分析，计算得到良好信号强度阈值α1和β1，归一化处理后得到两种信号的权重α′和β′，根据权重计算出融合位置坐标。
融合位置坐标的计算公式具体为：
X＝α′Xl+β′X2
其中，X为无人机的融合位置坐标，X1为UWB位置坐标的信号，X2为GPS或北斗卫星信号。
根据上述公式，当在室内GPS或北斗信号过于微弱或不存在的地方，α′近似于0，则X＝β′X2，即无人机的最终位置完全取决于UWB信号；当在室外离UWB基站较远处，β′近似于0，则X＝α′X1，即无人机的最终位置完全取决于GPS或北斗信号。
由于煤场顶棚结构的高度相似性，因此ICP算法必须经过大量的迭代之后才能达到收敛条件，同时算法匹配结果可能会陷入局部最优解，无法得到正确的位置信息。因此使用第三步得到的融合定位信息为ICP算法提供一个相对准确的初始位置，在此基础上完成精确匹配。
ICP算法的具体步骤为：
步骤S401：在目标点云P中取点集pi∈P；
步骤S402：找出源点云Q中的对应点集qi∈Q，使得||pi-qi||＝min；
步骤S403：计算旋转矩阵R和平移矩阵t，使得误差函数最小；
步骤S404：对pi使用步骤S403求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，得到新的对应点集{p′i＝Rpi+t,pi∈P}；
步骤S405：计算p′i与对应点集qi的平均距离d；
步骤S406：若d小于既定阈值或者迭代超过最大迭代次数，则停止迭代计算,否则返回步骤S402，直到满足上述收敛条件为止。
误差函数为E(R,t)具体为：
其中，n为最邻近点对的个数，pi为目标点云P中的一点，qi为源点云Q中与pi对应的最近点，R为旋转矩阵，t为平移向量。
无人机在步骤S4导航地图模型中的具体坐标或步骤S3的融合位置坐标需要使用低通滤波器对不良数据进行滤除，实现无人机的稳定导航，低通滤波器参数根据实际情况调节，兼顾滤波效果与实时性。