CN110703268B - 一种自主定位导航的航线规划方法和装置 - Google Patents

Abstract

本申请公开了一种自主定位导航的航线规划方法和装置，通过激光雷达传感器实时获取飞行过程中无人机与电力线、杆塔的位置距离数据，获取不同时刻的点云数据之后，进行特征点提取和匹配处理，通过SLAM算法推算无人机航迹，根据无人机航迹与预置导线的位移偏差和姿态偏差对无人机的坐标系进行平移和旋转，实现对无人机的航迹纠偏，解决了现有的无人机航空摄影测量技术需要按照预设飞行航线进行拍摄测量，在基准站和流动站通讯中断时，无人机航迹将发生偏移，影响航迹数据的拍摄质量的技术问题。

Description

一种自主定位导航的航线规划方法和装置

技术领域

本申请涉及自主导航技术领域，尤其涉及一种自主定位导航的航线规划方法和装置。

背景技术

航空摄影测量技术主要是通过遥感技术对无人机进行控制操作，无人机飞到需要测量的地域，通过已有的航线对航迹轨迹两边进行摄影测绘，绘制出地形图的作业。

目前的航空摄影测量过程中，无人机需要通过提前预设飞行航线执行任务航线，在给定的航线轨迹GPS定位检测地理位置进行航线的拍摄。使用RTK技术进行定位时，需要通过一个安装在地面的基准站和一个安装在无人机上的流动站，根据实时差分GPS可以实现厘米级定位，但是RTK技术实现需要一直保持基准站和流动站的数据交换，无人机在复杂地形中巡航时，难以保持基准站和流动站的通讯，往往会出现基准站和流动站的通讯中断，定位精度会下降，无人机航迹将发生偏移，影响航迹数据的拍摄质量。

发明内容

本申请实施例提供了一种自主定位导航的航线规划方法和装置，解决了现有的无人机航空摄影测量技术需要按照预设飞行航线进行拍摄测量，在基准站和流动站通讯中断时，无人机航迹将发生偏移，影响航迹数据的拍摄质量的技术问题。

有鉴于此，本申请第一方面提供了一种自主定位导航的航线规划方法，包括：

通过激光雷达传感器实时获取飞行过程中无人机与电力线、杆塔的位置距离数据，通过拍摄装置实时获取所述无人机不同时刻的点云数据；

将不同时刻的所述点云数据结合所述位置距离数据进行特征点提取和匹配处理，得到所述激光雷达传感器相对运动距离和相对运动姿态；

根据所述激光雷达传感器相对运动距离和相对运动姿态通过SLAM算法推算无人机航迹；

根据所述无人机航迹，计算所述无人机航迹与预置导线的位移偏差和姿态偏差，根据所述位移偏差和姿态偏差对所述无人机的坐标系进行平移和旋转，实现对所述无人机的航迹纠偏。

优选地，所述通过激光雷达传感器实时获取无人机与电力线、杆塔的位置距离数据，通过拍摄装置实时获取所述无人机不同时刻的点云数据，之前还包括：

根据预置导线控制无人机飞行至预置坐标位置，调整所述无人机的飞行位姿至预置位姿，所述位姿包括飞行高度和飞行角度。

优选地，所述特征点提取和匹配处理具体包括：

分别对所述点云数据和所述位置距离数据进行LiDAR插值处理，得到两个格网影像的强度波段信息；

分别对所述强度波段信息进行SIFT特征点提取，得到对应的特征向量；

根据所述特征向量通过K-d树进行特征匹配得到匹配点对，根据所述匹配点对得到所述激光雷达传感器相对运动距离和相对运动姿态。

优选地，所述根据所述激光雷达传感器相对运动距离和相对运动姿态通过SLAM算法推算无人机航迹，具体包括：

根据所述相对运动距离和相对运动姿态建立导线三维空间直角坐标系和无人机三维空间直角坐标系；

根据所述导线三维空间直角坐标系和所述无人机三维空间直角坐标系的相对角位置关系和空间距离平移关系，获取旋转矩阵和平移矩阵，根据所述旋转矩阵和所述平移矩阵，通过预置公式计算无人机航迹。

优选地，所述预置公式为：

Vs＝Rvs×Ls+Tvs，

其中，Rvs为相对角位置关系，Tvs为空间距离平移关系，Ls为导线三维空间直角坐标系。

优选地，所述根据所述特征向量通过K-d树进行特征匹配得到匹配点对，根据所述匹配点对得到所述激光雷达传感器相对运动距离和相对运动姿态，之后还包括：

采用RANSAC稳健估计对所述匹配点对进行精化。

优选地，所述特征点提取和匹配处理，还包括：

对所述匹配点对进行连接点跟踪，将具有相同匹配点号的所有匹配点串成完整的连接点链。

优选地，所述对所述匹配点对进行连接点跟踪，将具有相同匹配点号的所有匹配点串成完整的连接点链，之后还包括：对所述连接点进行分类；

所述对所述连接点进行分类具体包括：

获取所述连接点的平面坐标，通过粗糙度分析方法计算所述连接点所处表面的稳定性；

将所述稳定性小于预置阈值的所述连接点的表面区域作为不稳定区域，将所述稳定性大于预置阈值的所述连接点的表面区域作为稳定区域，

将所述稳定区域的所述连接点标记为平高连接点，将所述不稳定区域的所述连接点标记为平面连接点。

优选地，所述根据所述无人机航迹，计算所述无人机航迹与预置导线的位移偏差和姿态偏差，根据所述位移偏差和姿态偏差对所述无人机的坐标系进行平移和旋转，实现对所述无人机的航迹纠偏，之后还包括：将所述无人机拍摄到的局部点云数据进行整合与配准；

所述将所述无人机拍摄到的局部点云数据进行整合与配准，具体包括：

通过迭代最近点法ICP(Iterative Closesl Point)将不同位置的扫描得到的预置容量三维空间数据点集转换到一个统一的坐标系中，完成三维点云数据的配准。

本申请第二方面提供了一种自主定位导航的航线规划装置，包括：

获取单元，用于通过激光雷达传感器实时获取飞行过程中无人机与电力线、杆塔的位置距离数据，通过拍摄装置实时获取所述无人机不同时刻的点云数据；

匹配单元，用于将不同时刻的所述点云数据结合所述位置距离数据进行特征点提取和匹配处理，得到所述激光雷达传感器相对运动距离和相对运动姿态；

航迹单元，用于根据所述激光雷达传感器相对运动距离和相对运动姿态通过SLAM算法推算无人机航迹；

纠偏单元，用于根据所述无人机航迹，计算所述无人机航迹与预置导线的位移偏差和姿态偏差，根据所述位移偏差和姿态偏差对所述无人机的坐标系进行平移和旋转，实现对所述无人机的航迹纠偏。

从以上技术方案可以看出，本申请实施例具有以下优点：

本申请中，提供了一种自主定位导航的航线规划方法，包括：通过激光雷达传感器实时获取飞行过程中无人机与电力线、杆塔的位置距离数据，通过拍摄装置实时获取无人机不同时刻的点云数据；将不同时刻的点云数据进行特征点提取和匹配处理，得到激光雷达传感器相对运动距离和相对运动姿态；根据激光雷达传感器相对运动距离和相对运动姿态通过SLAM算法推算无人机航迹；根据无人机航迹，计算无人机航迹与预置导线的位移偏差和姿态偏差，根据位移偏差和姿态偏差对无人机的坐标系进行平移和旋转，实现对无人机的航迹纠偏。本申请提供的方法，通过激光雷达传感器实时获取飞行过程中无人机与电力线、杆塔的位置距离数据，获取不同时刻的点云数据之后，进行特征点提取和匹配处理，通过SLAM算法推算无人机航迹，根据无人机航迹与预置导线的位移偏差和姿态偏差对无人机的坐标系进行平移和旋转，实现对无人机的航迹纠偏，解决了现有的无人机航空摄影测量技术需要按照预设飞行航线进行拍摄测量，在基准站和流动站通讯中断时，无人机航迹将发生偏移，影响航迹数据的拍摄质量的技术问题。

附图说明

图1为本申请实施例中的一种自主定位导航的航线规划方法的流程示意图；

图2为本申请实施例中的一种自主定位导航的航线规划方法另一流程示意图；

图3为本申请实施例中的一种自主定位导航的航线规划装置的结构示意图；

图4为本申请实施例中的航迹推算原理示意图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

为了便于理解，请参阅图1，本申请实施例中提供的一种自主定位导航的航线规划方法，包括：

步骤101、通过激光雷达传感器实时获取飞行过程中无人机与电力线、杆塔的位置距离数据，通过拍摄装置实时获取无人机不同时刻的点云数据。

需要说明的是，本申请实施例中，在无人机上搭载激光雷达传感器，通过激光雷达传感器进行扫描，能够反馈载体在环境中的精确位置和物体距无人机拍摄点的距离。激光雷达传感器共有16路不同方向的激光器，能获得16路不同方向的点，计算某一方向的点坐标需要该点回波距离、垂直距离、水平角度和转台角度四个参数。位置距离数据可以由IMU和GPS通过惯性导航算法算出，激光雷达传感器也能够实现记录周围激光雷达采集到的物体信息呈现出一系列分散的、具有准确角度和距离信息的点，被称为点云。同时，无人机上搭载拍摄装置和微型电脑装置，通过拍摄装置，如摄像机，来实时获取无人机在不同时刻的点云数据。

步骤102、将不同时刻的点云数据结合位置距离数据进行特征点提取和匹配处理，得到激光雷达传感器相对运动距离和相对运动姿态。

需要说明的是，本申请实施例中，在获取到不同时刻的点云数据之后，可以结合激光雷达传感器扫描得到的电力线路和杆塔的位置距离数据，对点云数据进行特征点提取和特征点匹配处理，从而可以得到激光雷达传感器相对运动距离和相对运动姿态。

步骤103、根据激光雷达传感器相对运动距离和相对运动姿态通过SLAM算法推算无人机航迹。

需要说明的是，本申请实施例中，激光雷达传感器以预置的频率发射预置幅度的调制光，并测量发射和反向信号之间的相移，调制信号的波长为lamda＝c/f，其中c是光速，f是调制频率，测量到发射和反射光束之间的相移差theta之后，距离可由lamda*theta/4pi计算得到。先控制机器到达一个位姿，然后再进行观测。本申请实施例中，由控制数据和观测数据(激光雷达传感器获取到的数据)来求机器位姿，这就出现了一个逆向的过程即：先有位姿再得到观测数据，变成了先得到观测数据再来求位姿的过程。也就是需要由结果(观测数据)来反推原因(位姿)；因此这里就引入了贝叶斯公式，贝叶斯公式的存在就是为了求解这种由结果反推原因的问题；有了位姿就可以有观测数据计算在这个位置看到的概率。因此就要用贝叶斯公式将逆向的过程转换成正向的过程：

SLAM中的定位问题就变成了下面的公式：

本申请实施例中的航迹推算定位原理如图4所示，图4中，移动机器人从S点出发，且起始点S点的位位置是已知的，则S点时刻的机器人位姿信息是可以测量出来的，假设在t1时段内移动飞机的速度和方向不变，则可以求出t1时刻结束时移动飞机平台运动位子是S1，同理，可以求出S1,…,Sn的位移，最终通过位姿信息矢量合成到达G点的位置。

在推算出无人机航迹之后，可以直接得到无人机与电力线、杆塔的相对位移和相对旋转关系，为无人机航迹规划提供必要的条件。在室内环境中，机器人不能利用全局定位系统进行定位，而事先获取机器人工作环境的地图很困难，甚至是不可能的。SLAM技术使得机器人在自身位置不确定的条件下，经过一系列的位置并且在每一个位置获得传感器对环境的感知信息，在完全未知环境中创建地图，同时利用地图进行自主定位和导航。

步骤104、根据无人机航迹，计算无人机航迹与预置导线的位移偏差和姿态偏差，根据位移偏差和姿态偏差对无人机的坐标系进行平移和旋转，实现对无人机的航迹纠偏。

需要说明的是，在得到无人机航迹之后，可以将无人机航迹与预置导线进行偏差计算，包括位移偏差和姿态偏差，即得到两者的平移关系和旋转关系，根据平移关系和旋转关系对无人机航迹进行纠偏。本申请实施例中的预置导线，可以是根据目标杆塔坐标构建的悬链线，预置导线的获取过程可以是：计算水平张力，确定抛物线，根据抛物线焦点轨迹确定悬链线，拟合成预置导线；水平张力可以根据以下公式获得：

F₀＝ay

其中，x和y是杆塔坐标的x坐标和y坐标，L是曲线中某点到坐标原点0的链索长度，α是该点的正切角，F0是0点处的水平张力，γ是链索的单位重量。利用以上公式，可以计算出任意点的水平张力，通过平面内任意3点不共线的特性，可以确定抛物线，沿着一条直线滚动的抛物线，平面内不共线的3点确定一条悬链线，可以拟合成一条预置导线。

本申请实施例中，提供了一种自主定位导航的航线规划方法，包括：通过激光雷达传感器实时获取飞行过程中无人机与电力线、杆塔的位置距离数据，通过拍摄装置实时获取无人机不同时刻的点云数据；将不同时刻的两片点云数据进行特征点提取和匹配处理，得到激光雷达传感器相对运动距离和相对运动姿态；根据激光雷达传感器相对运动距离和相对运动姿态通过SLAM算法推算无人机航迹，得到无人机与电力线、杆塔的相对位移和相对旋转关系；根据无人机航迹，计算无人机航迹与预置导线的位移偏差和姿态偏差，根据位移偏差和姿态偏差对无人机的坐标系进行平移和旋转，实现对无人机的航迹纠偏。本申请提供的方法，通过激光雷达传感器实时获取飞行过程中无人机与电力线、杆塔的位置距离数据，获取不同时刻的点云数据之后，进行特征点提取和匹配处理，通过SLAM算法推算无人机航迹，根据无人机航迹与预置导线的位移偏差和姿态偏差对无人机的坐标系进行平移和旋转，实现对无人机的航迹纠偏，解决了现有的无人机航空摄影测量技术需要按照预设飞行航线进行拍摄测量，在基准站和流动站通讯中断时，无人机航迹将发生偏移，影响航迹数据的拍摄质量的技术问题。

为了便于理解，请参阅图2，本申请实施例中另一种自主定位导航的航线规划方法，包括：

步骤201、根据预置导线控制无人机飞行至预置坐标位置，调整无人机的飞行位姿至预置位姿，位姿包括飞行高度和飞行角度。

需要说明的是，本申请实施例中，首先需要根据预置导线将无人机飞行至预置坐标位置，调整无人机的飞行高度和飞行角度。

步骤202、获取不同时刻的点云数据和与其对应的位置距离数据，分别对点云数据和位置距离数据进行LiDAR插值处理，得到两个格网影像的强度波段信息。

需要说明的是，原始的LiDAR数据是离散采样的三维点云，具有数据不连续、分布不均匀的缺点，给特征点的提取与定位带来了较大困难。当前发展较为成熟的特征点提取与匹配方法大都是针对光学图像，为了便于借鉴现有研究成果，本申请实施例采用内插的方式，将离散的三维点云转换为规则的格网影像，并在此基础上进行特征点的检测。为了将离散激光点内插成规则的格网影像，需要选择合适的插值方法。现有的插值方法很多，本申请实施例选择能够有效保持地物的几何形态的三角网插值方法。每个激光脚点数据中不仅记录有目标的高程信息，还记录有目标的激光反射强度信息，与高程信息相比强度值反映了地物对近红外激光的反射特性，与传统的光学图像更为接近，更适宜与特征点提取。因此，本申请实施例同时对强度和高程进行插值，得到的格网影像包括两个波段，第一个波段记录的是格网的强度信息，第二个波段记录的是格网的高程信息。

步骤203、分别对强度波段信息进行SIFT特征点提取，得到对应的特征向量。

需要说明的是，本申请实施例中，SIFT特征是一种典型的斑点特征，在图像处理领域已得到了广泛的应用，具有可靠性高、适用性强、对旋转和尺度变化不敏感等显著优势。本申请实施例中利用LiDAR插值影像的强度波段进行SIFT特征点检测，采用子像素插值方式计算特征点的定位坐标，利用强度信息建立特征点的描述向量。

步骤204、根据特征向量通过K-d树进行特征匹配得到匹配点对，根据匹配点对得到激光雷达传感器相对运动距离和相对运动姿态，得到激光雷达传感器相对运动距离和相对运动姿态。

需要说明的是，k-d树(k-维树的缩写)是在k维欧几里德空间组织点的数据结构。本申请实施例中，通过K-d树对特征向量进行特征匹配得到匹配点对，首先通过二叉树搜索(比较待查询节点和分裂节点的分裂维的值，小于等于就进入左子树分支，大于就进入右子树分支直到叶子结点)，顺着“搜索路径”很快能找到最近邻的近似点，也就是与待查询点处于同一个子空间的叶子结点；然后再回溯搜索路径，并判断搜索路径上的结点的其他子结点空间中是否可能有距离查询点更近的数据点，如果有可能，则需要跳到其他子结点空间中去搜索(将其他子结点加入到搜索路径)。重复这个过程直到搜索路径为空。

步骤205、采用RANSAC稳健估计对匹配点对进行精化。

需要说明的是，由于初始匹配点中难以避免的存在误匹配，采用RANSAC稳健估计的方法，对初始匹配点进行精化。RANSAC估计时需要为inliers选择合适的数学模型，由于LiDAR条带之间的变形可以用仿射变换来粗略近似，采用仿射变换模型作为RANSAC挑点时的约束条件。

步骤206、对匹配点对进行连接点跟踪，将具有相同匹配点号的所有匹配点串成完整的连接点链。

需要说明的是，通过上述步骤可获得LiDAR条带两两之间的匹配点对。为了将各条带上的同名点链接起来，需要进行连接点跟踪。将所有特征点按像片号和特征点号进行编码，如第一张像片上的第一个特征点编码为1000001，第二张像片上的第2个特征点编码为2000002。对所有匹配点按像片号进行排序，并按逆序对匹配点进行追踪，就可将具有相同匹配点号的所有匹配点串成一个完整的连接点链。对匹配点对进行连接点跟踪，将具有相同匹配点号的所有匹配点串成完整的连接点链，可以为后期建模精细化，建模图形连贯与完整，防止建模中出现断裂、偏移或者扭曲等现象。

步骤207、对连接点进行分类。

对连接点进行分类的过程可以是：

a)获取连接点的平面坐标，通过粗糙度分析方法计算连接点所处表面的稳定性；

b)将稳定性小于预置阈值的连接点的表面区域作为不稳定区域，将稳定性大于预置阈值的连接点的表面区域作为稳定区域；

c)将稳定区域的连接点标记为平高连接点，将不稳定区域的连接点标记为平面连接点。

需要说明的是，从强度影像上提取的连接点仅考虑了辐射特征，没有顾及几何特性，造成许多连接点可能位于植被，建筑物边缘等表面不稳定区域。通过连接点的平面坐标可以在高程波段中取出其高程值，但高程值是通过三角形内插得到的，在不稳定区域上的连接点，其高程是不可靠。本申请实施例利用粗糙度分析的方法，考察连接点所处表面的稳定性，计算以连接点为圆心，指定半径内的激光点高程的均方根误差，并以此为标准判断表面的稳定性(误差小于阈值)。将处于稳定区域的连接点标记为平高连接点，意味着平差时连接点的三个坐标都可以参与计算；处理不稳定区域的连接点标记为平面连接点，即平差时只有平面坐标可以参与计算。

步骤208、根据相对运动距离和相对运动姿态建立导线三维空间直角坐标系和无人机三维空间直角坐标系。

需要说明的是，根据点的曲率c和点云密度来将部分点集划分为不同的类别导线，一般曲率较大的特征为导线特征，识别导线的这一过程为扫描匹配。通过读取点云中特征导线的三维空间坐标建立导线三维空间直角坐标系，建立无人机三维空间直角坐标系，从导线三维空间直角坐标系到无人机三维空间直角坐标系有一个相对角位置关系RPY角和空间距离平移关系。

步骤209、根据导线三维空间直角坐标系和无人机三维空间直角坐标系的相对角位置关系和空间距离平移关系，获取旋转矩阵和平移矩阵，根据旋转矩阵和平移矩阵，通过预置公式计算无人机航迹。

进一步地，预置公式为Vs＝Rvs×Ls+Tvs，Es＝Rev×Vs+Tev。

其中，Rvs为相对角位置关系，Tvs为空间距离平移关系，Ls为导线三维空间直角坐标系，Vs为无人机三维空间直角坐标系，Es为预期无人机位置坐标系，Rev为Es与Ls相对角位置，Tev为Es与Ls空间距离平移关系。

RPY角起源于描绘航船的姿态，第一个是绕船前进的方向x轴方向的roll转角Φ，第二个是绕水平y轴方向的Pitch角θ，第三个是绕铅锤z方向的Yaw角β。RPY法中的x，y，z三轴是原坐标系的三轴。对于旋转矩阵，假设Pa为原坐标系，坐标系经过绕RPY角一定顺序旋转到新坐标系Pb则旋转矩阵Rab关系式为：Pa＝Rab×Pb，对于平移矩阵，假设Pa为原坐标系，坐标系沿xyz方向平移平移到新坐标系Pb则平移矩阵Tab关系式为：Pa＝Pb+Tab。则，可以通过旋转矩阵和平移矩阵得到无人机三维空间直角坐标系，通过预置公式Vs＝Rvs×Ls+Tvs解算出导线与无人机的相对角位置关系Rvs和空间位置距离Tvs，同理得到Es＝Rev×Vs+Tev，通过上述两个公式即可以得到无人机当前位置与预期位置旋转角位置关系与空间平移关系，可以进行航线的校准。

步骤210、根据无人机航迹，计算无人机航迹与预置导线的位移偏差和姿态偏差，根据位移偏差和姿态偏差对无人机的坐标系进行平移和旋转，实现对无人机的航迹纠偏。

需要说明的是，在得到无人机航迹之后，可以计算无人机航迹与预置导线的位移偏差和姿态偏差，根据位移偏差和姿态偏差对无人机的坐标系进行平移和旋转，实现对无人机的航迹纠偏。可以根据无人机三维空间直角坐标系建立预期无人机位置三维空间直角坐标系(Es)，则有，Es＝Rev×Vs+Tev，解算出无人机三维空间直角坐标系与预期坐标系的旋转矩阵Rev和平移矩阵Tev来实现最佳航线的配准。

步骤211、将无人机拍摄到的局部点云数据进行整合与配准。

将无人机拍摄到的局部点云数据进行整合与配准，具体包括：

通过迭代最近点法ICP(Iterative Closesl Point)将不同位置的扫描得到的预置容量三维空间数据点集转换到一个统一的坐标系中，完成三维点云数据的配准。

需要说明的是，为了得到物体表面完整的点云数据，需要对这些局部点云数据进行整合和配准。因此，在得到点云数据之后，为了得到三维模型的原始曲面，必须要将不同角度，不同位置的扫描得到的预置容量(大于300万点云)三维空间数据点集转换到一个统一的坐标系中，该技术称之为数据缝合，即三维点云数据的配准。点云配准是点云数据获取后的第一步处理，也是所有后续的处理的基础。因此，配准的精度将直接关系到建模精度。

根据各种形式的点云数据提出一种点集与点集的匹配算法通过实践证明是一个解决复杂配准问题的关键方法。也就是迭代最近点法ICP(IterativeClosesl Point)，这是一种高层次的基于自由形态曲面的配准方法，能够将不同位置的扫描得到的预置容量三维空间数据点集转换到一个统一的坐标系中。

为了便于理解，请参阅图3，本申请实施例提供了一种自主定位导航的航线规划装置，包括：

获取单元301，用于通过激光雷达传感器实时获取飞行过程中无人机与电力线、杆塔的位置距离数据，通过拍摄装置实时获取无人机不同时刻的点云数据。

匹配单元302，用于将不同时刻的两片点云数据结合位置距离数据进行特征点提取和匹配处理，得到激光雷达传感器相对运动距离和相对运动姿态。

航迹单元303，用于根据激光雷达传感器相对运动距离和相对运动姿态通过SLAM算法推算无人机航迹，得到无人机与电力线、杆塔的相对位移和相对旋转关系。

纠偏单元304，用于根据无人机航迹，计算无人机航迹与预置导线的位移偏差和姿态偏差，根据位移偏差和姿态偏差对无人机的坐标系进行平移和旋转，实现对无人机的航迹纠偏。

飞行单元305，用于根据预置导线控制无人机飞行至预置坐标位置，调整无人机的飞行位姿至预置位姿，位姿包括飞行高度和飞行角度。

航迹单元303，具体用于根据相对运动距离和相对运动姿态建立导线三维空间直角坐标系和无人机三维空间直角坐标系；根据导线三维空间直角坐标系和无人机三维空间直角坐标系的相对角位置关系和空间距离平移关系，获取旋转矩阵和平移矩阵，根据旋转矩阵和平移矩阵，通过预置公式计算无人机航迹。

配准单元306，用于将无人机拍摄到的局部点云数据进行整合与配准。

配准单元306，具体用于通过迭代最近点法ICP(Iterative Closesl Point)将不同位置的扫描得到的预置容量三维空间数据点集转换到一个统一的坐标系中，完成三维点云数据的配准。

匹配单元302，具体用于获取不同时刻的片点云数据和与其对应的位置距离数据，分别对点云数据和位置距离数据进行LiDAR插值处理，得到两个格网影像的强度波段信息；分别对强度波段信息进行SIFT特征点提取，得到对应的特征向量；根据特征向量通过K-d树进行特征匹配得到匹配点对，根据匹配点对得到激光雷达传感器相对运动距离和相对运动姿态，得到激光雷达传感器相对运动距离和相对运动姿态；采用RANSAC稳健估计对匹配点对进行精化；对匹配点对进行连接点跟踪，将具有相同匹配点号的所有匹配点串成完整的连接点链；对连接点进行分类。

本申请的说明书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(英文全称：Read-OnlyMemory，英文缩写：ROM)、随机存取存储器(英文全称：Random Access Memory，英文缩写：RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。

Claims (8)

1.一种自主定位导航的航线规划方法，其特征在于，包括：

通过激光雷达传感器实时获取飞行过程中无人机与电力线、杆塔的位置距离数据，通过拍摄装置实时获取所述无人机不同时刻的点云数据；

将不同时刻的所述点云数据结合所述位置距离数据进行特征点提取和匹配处理，得到所述激光雷达传感器相对运动距离和相对运动姿态；

根据所述激光雷达传感器相对运动距离和相对运动姿态通过SLAM算法推算无人机航迹；

根据所述无人机航迹，计算所述无人机航迹与预置导线的位移偏差和姿态偏差，根据所述位移偏差和姿态偏差对所述无人机的坐标系进行平移和旋转，实现对所述无人机的航迹纠偏；

所述根据所述激光雷达传感器相对运动距离和相对运动姿态通过SLAM算法推算无人机航迹，具体包括：

根据所述相对运动距离和相对运动姿态建立导线三维空间直角坐标系和无人机三维空间直角坐标系；

根据所述导线三维空间直角坐标系和所述无人机三维空间直角坐标系的相对角位置关系和空间距离平移关系，获取旋转矩阵和平移矩阵，根据所述旋转矩阵和所述平移矩阵，通过预置公式计算无人机航迹；

所述预置公式为：

Vs＝Rvs×Ls+Tvs

Es＝Rev×Vs+Tev

其中，Rvs为相对角位置关系，Tvs为空间距离平移关系，Ls为导线三维空间直角坐标系，Es为预期无人机位置坐标系，Rev为Es与Ls相对角位置，Tev为Es与Ls空间距离平移关系。

2.根据权利要求1所述的自主定位导航的航线规划方法，其特征在于，所述通过激光雷达传感器实时获取无人机与电力线、杆塔的位置距离数据，通过拍摄装置实时获取所述无人机不同时刻的点云数据，之前还包括：

根据预置导线控制无人机飞行至预置坐标位置，调整所述无人机的飞行位姿至预置位姿，所述位姿包括飞行高度和飞行角度。

3.根据权利要求1所述的自主定位导航的航线规划方法，其特征在于，所述特征点提取和匹配处理具体包括：

分别对所述点云数据和所述位置距离数据进行LiDAR插值处理，得到两个格网影像的强度波段信息；

分别对所述强度波段信息进行SIFT特征点提取，得到对应的特征向量；

根据所述特征向量通过K-d树进行特征匹配得到匹配点对，根据所述匹配点对得到所述激光雷达传感器相对运动距离和相对运动姿态。

4.根据权利要求3所述的自主定位导航的航线规划方法，其特征在于，所述根据所述特征向量通过K-d树进行特征匹配得到匹配点对，根据所述匹配点对得到所述激光雷达传感器相对运动距离和相对运动姿态，之后还包括：

采用RANSAC稳健估计对所述匹配点对进行精化。

5.根据权利要求3或4所述的自主定位导航的航线规划方法，其特征在于，所述特征点提取和匹配处理，还包括：

对所述匹配点对进行连接点跟踪，将具有相同匹配点号的所有匹配点串成完整的连接点链。

6.根据权利要求5所述的自主定位导航的航线规划方法，其特征在于，所述对所述匹配点对进行连接点跟踪，将具有相同匹配点号的所有匹配点串成完整的连接点链，之后还包括：对所述连接点进行分类；

所述对所述连接点进行分类具体包括：

获取所述连接点的平面坐标，通过粗糙度分析方法计算所述连接点所处表面的稳定性；

将所述稳定性小于预置阈值的所述连接点的表面区域作为不稳定区域，将所述稳定性大于预置阈值的所述连接点的表面区域作为稳定区域；

将所述稳定区域的所述连接点标记为平高连接点，将所述不稳定区域的所述连接点标记为平面连接点。

7.根据权利要求1所述的自主定位导航的航线规划方法，其特征在于，所述根据所述无人机航迹，计算所述无人机航迹与预置导线的位移偏差和姿态偏差，根据所述位移偏差和姿态偏差对所述无人机的坐标系进行平移和旋转，实现对所述无人机的航迹纠偏，之后还包括：将所述无人机拍摄到的局部点云数据进行整合与配准；

所述将所述无人机拍摄到的局部点云数据进行整合与配准，具体包括：

通过迭代最近点法ICP将不同位置的扫描得到的预置容量三维空间数据点集转换到一个统一的坐标系中，完成三维点云数据的配准。

8.一种自主定位导航的航线规划装置，其特征在于，包括：

获取单元，用于通过激光雷达传感器实时获取飞行过程中无人机与电力线、杆塔的位置距离数据，通过拍摄装置实时获取所述无人机不同时刻的点云数据；

匹配单元，用于将不同时刻的两片所述点云数据结合所述位置距离数据进行特征点提取和匹配处理，得到所述激光雷达传感器相对运动距离和相对运动姿态；

航迹单元，用于根据所述激光雷达传感器相对运动距离和相对运动姿态通过SLAM算法推算无人机航迹；

纠偏单元，用于根据所述无人机航迹，计算所述无人机航迹与预置导线的位移偏差和姿态偏差，根据所述位移偏差和姿态偏差对所述无人机的坐标系进行平移和旋转，实现对所述无人机的航迹纠偏；

所述航迹单元，具体用于：

根据所述相对运动距离和相对运动姿态建立导线三维空间直角坐标系和无人机三维空间直角坐标系；

根据所述导线三维空间直角坐标系和所述无人机三维空间直角坐标系的相对角位置关系和空间距离平移关系，获取旋转矩阵和平移矩阵，根据所述旋转矩阵和所述平移矩阵，通过预置公式计算无人机航迹；

其中，所述预置公式为：

Vs＝Rvs×Ls+Tvs

Es＝Rev×Vs+Tev

其中，Rvs为相对角位置关系，Tvs为空间距离平移关系，Ls为导线三维空间直角坐标系，Es为预期无人机位置坐标系，Rev为Es与Ls相对角位置，Tev为Es与Ls空间距离平移关系。

Images