CN111854756A - 一种基于单线激光的引水涵洞内无人机定位方法 - Google Patents

Abstract

本发明涉及一种基于单线激光的引水涵洞内无人机定位方法，包括步骤1：构建引水涵洞的点云模型；步骤2：获取无人机采集的高度信息和原始姿态数据；步骤3：通过位置获取子方法获得无人机位置数据，同时通过姿态获取子方法获得优化后的无人机姿态数据；步骤4：根据无人机的位置数据和优化后的姿态数据获得无人机在引水涵洞中的精确定位。与现有技术相比，本发明具有定位精度高、处理速度快等优点。

Description

一种基于单线激光的引水涵洞内无人机定位方法

技术领域

本发明涉及引水涵洞内无人机精确定位技术领域，尤其是涉及一种基于单线激光的引水涵洞内无人机定位方法。

背景技术

抽水蓄能电站利用电力负荷低谷时的电能抽水至上水库，在电力负荷高峰期再放水至下水库发电的水电站，又称蓄能式水电站。它可将电网负荷低时的多余电能，转变为电网高峰时期的高价值电能，还适于调频、调相，稳定电力***的周波和电压，而用于引水的引水涵洞的维护检修问题也逐渐得到了业界的普遍关注。以往，通常采用人工进入引水涵洞中观察壁面情况，一旦发生安全事故，涵洞外部的救援人员很难对涵洞内被困人员的精确定位和救援。

为解决上述问题，蓄能式水电站采用无人机技术来对引水涵洞进行维护检修，例如中国专利CN108681337A中公开了一种涵洞或桥梁无人机和无人机巡查方法，该专利中的无人机动力装置设有超声波避障***和视觉定位***，通过这两个***实现无人机的涵洞或桥梁中的自主循迹。但该专利通过视觉定位***采集的视觉图像含有较高的噪声，对无人机的定位会有较大的偏差，有可能会引起无人机偏离航线并伴随着坠机风险，所以在蓄能式水电站内亟需一种能够实现涵洞内无人机精确定位的无人机定位方法。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种定位精度高、处理速度快的基于单线激光的引水涵洞内无人机定位方法。

本发明的目的可以通过以下技术方案来实现：

一种基于单线激光的引水涵洞内无人机定位方法，包括：

步骤1：构建引水涵洞的点云模型；

步骤2：获取无人机采集的高度信息和原始姿态数据；

步骤3：通过位置获取子方法获得无人机位置数据，同时通过姿态获取子方法获得优化后的无人机姿态数据；

步骤4：根据无人机的位置数据和优化后的姿态数据获得无人机在引水涵洞中的精确定位。

优选地，所述的步骤1具体为：

根据引水涵洞的CAD图像获取引水涵洞的参数信息，根据涵洞的参数信息构建3D模型，然后通过3D模型生成引水涵洞的点云模型。

优选地，所述的步骤2具体为：

通过安装在无人机上的气压计获得无人机的高度数据，同时通过安装在无人机上的惯导设备获得无人机的原始姿态数据。

优选地，所述步骤3中位置获取子方法具体为：

步骤3-1-1：在引水涵洞的点云模型中获得涵洞中轴线的解析方程；

步骤3-1-2：将无人机高度信息代入中轴线解析方程，获得无人机的位置数据。

优选地，所述步骤3中姿态获取子方法具体为：

步骤3-2-1：获取单线激光数据；

步骤3-2-2：对单线激光数据进行预处理；

步骤3-2-3：将单线激光数据转移到引水涵洞点云模型坐标系中对应的位置，然后将单线激光数据与引水涵洞点云模型进行匹配，获得计算姿态数据；

步骤3-2-4：将原始姿态数据和计算姿态数据融合，获得优化后的无人机姿态数据。

更加优选地，所述的步骤3-2-2具体为：

对单线激光数据进行滤波去噪处理。

更加优选地，所述的步骤3-2-3中单线激光数据通过迭代最近点算法与引水涵洞点云模型进行匹配。

更加优选地，所述的单线激光数据与引水涵洞点云模型进行匹配的具体步骤为：

步骤3-2-3-1：搜索最近点：

取单线激光数据点云P中一点p_i，然后在模型点云M中找出距离p_i欧氏距离最近的点m_i，(p_i,m_i)就构成了一组对应点对集；

步骤3-2-3-2：通过奇异值分解的方法求解p_i与m_i之间的变换关系(R,t)，其中R为p_i与m_i的旋转关系，t为p_i与m_i的位移关系；

步骤3-2-3-3：对激光点云P中的每一个点p_i运用变换关系(R,t)，即P'＝RP+t得到点集P'，定义目标函数：

其中n为点云P中点云的个数；

步骤3-2-3-4：判断目标函数f(R,t)是否小于预设阈值ε或是否达到预设的最大迭代次数，若是，则结束本轮循环，输出(R,t)，否则，返回步骤3-2-3-1。

更加优选地，所述步骤3-2-3-1中采用KDTree方法搜索最近点。

更加优选地，所述的步骤3-2-4中原始姿态数据通过卡尔曼滤波融合算法与计算姿态数据进行融合，具体为：

对于惯导设备和激光雷达设备输出的姿态数据分别为R₁和R₂，其与真实位姿的关系满足：

R_i＝R+n_i,i＝1,2

其中，n₁和n₂分别表示惯导设备和激光雷达设备分别叠加在真实位姿R上的噪声；

惯导设备的状态误差导数为：

其中，

和

分别表示在t时刻时惯导设备的位置、速度、姿态角、加速度计bias和陀螺仪bias误差的导数；

表示惯导系与机体系的旋转关系；^∧表示从李代数到李群的变换；n_a、n_ω、

和

分别表示加速度计白噪声、陀螺仪白噪声、加速度计bias白噪声和陀螺仪bias白噪声；

其增量误差的协方差预测公式为：

P_t+δt＝(1+F_tδt)P_t(1+F_tδt)^T+(G_tδt)Q(G_tδt)^T

P的初始值设为0，Q表示噪声项对角协方差矩阵，具体为：

激光雷达设备进行激光扫描匹配时的误差采用一阶马尔科夫过程描述，具体为：

其中，

和

表示激光雷达设备的姿态角误差；T_1γ、T_1θ和T_1ψ分别为三个姿态角误差对应的相关时间；所述的ξ_1γ、ξ_1θ和ξ_1ψ为三个姿态角误差对应的白噪声；

协方差预测公式具体为：

P_t+δt＝(1+F_tδt)P_t(1+F_tδt)^T+(G_tδt)Q(G_tδt)^T

P的初始值设为0，Q表示噪声项对角协方差矩阵，具体为：

最终，融合后的真实姿态R具体为：

其中，W_i为信息矩阵，表示传感器输出在最终结果中所占比重，W₁和W₂分别为惯导设备和激光雷达设备协方差矩阵的逆。

与现有技术相比，本发明具有以下优点：

一、实现引水涵洞内无人机的高精度定位：本发明中的无人机定位方法通过无人机的高度数据来获得无人机在引水涵洞中的位置数据；通过将惯导设备采集的原始姿态数据与使用迭代最近点算法获得的计算姿态数据来获得经过优化的无人机姿态数据，最终再将计算得到的计算姿态数据与原始姿态数据进行融合，获得精确地无人机定位数据，定位的精度高。

二、处理速度快：本发明中的无人机定位方法使用迭代最近点算法求解计算位姿数据，并且使用了KDTree结构搜索空间最近点，加快了算法的处理速度，提高了处理效率。

附图说明

图1为本发明中引水涵洞内无人机定位方法的流程示意图；

图2为本发明中单线激光数据与引水涵洞点云模型进行匹配时的流程示意图；

图3为本发明实施例中引水涵洞3Dmax模型的结构示意图；

图4为本发明实施例中引水涵洞点云模型的结构示意图；

图5为本发明实施例中使用KDTree搜索空间最近点时的结构示意图；

图6为本发明实施例中实际点云的配准示意图。

图中标号所示：

A、单线激光数据在点云模型中的投影，B、无人机循迹轨迹。

具体实施方式

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

一种基于单线激光的引水涵洞内无人机定位方法，其流程如图1所示，包括：

步骤1：构建引水涵洞的点云模型；

具体为：根据引水涵洞的CAD图像获取引水涵洞的参数信息，根据涵洞的参数信息构建3D模型，然后通过3D模型生成引水涵洞的点云模型；

步骤2：获取无人机采集的高度信息和原始姿态数据；

具体为：通过安装在无人机上的气压计获得无人机的高度数据，同时通过安装在无人机上的惯导设备获得无人机的原始姿态数据；

步骤3：通过位置获取子方法获得无人机位置数据，同时通过姿态获取子方法获得优化后的无人机姿态数据；

位置获取子方法具体为：

步骤3-1-1：在引水涵洞的点云模型中获得涵洞中轴线的解析方程；

步骤3-1-2：将无人机高度信息代入中轴线解析方程，获得无人机的位置数据；

姿态获取子方法具体为：

步骤3-2-1：获取单线激光数据；

步骤3-2-2：对单线激光数据进行预处理，具体为对单线激光数据进行滤波去噪处理；

步骤3-2-3：将单线激光数据转移到引水涵洞点云模型坐标系中对应的位置，然后将单线激光数据与引水涵洞点云模型进行匹配，获得计算姿态数据；

步骤3-2-4：将原始姿态数据和计算姿态数据融合，获得优化后的无人机姿态数据；

步骤3-2-3中单线激光数据通过迭代最近点算法与引水涵洞点云模型进行匹配，其流程如图2所示，具体为：

步骤3-2-3-1：采用KDTree方法搜索最近点：

取单线激光数据点云P中一点p_i，然后在模型点云M中找出距离p_i欧氏距离最近的点m_i，(p_i,m_i)就构成了一组对应点对集；

步骤3-2-3-2：通过奇异值分解的方法求解p_i与m_i之间的变换关系(R,t)，其中R为p_i与m_i的旋转关系，t为p_i与m_i的位移关系；

步骤3-2-3-3：对激光点云P中的每一个点p_i运用变换关系(R,t)，即P'＝RP+t得到点集P'，定义目标函数：

其中n为点云P中点云的个数；

步骤3-2-3-4：判断目标函数f(R,t)是否小于预设阈值ε或是否达到预设的最大迭代次数，若是，则结束本轮循环，输出(R,t)，否则，返回步骤3-2-3-1；

步骤3-2-4中原始姿态数据通过卡尔曼滤波融合算法与计算姿态数据进行融合，具体为：

对于惯导设备和激光雷达设备输出的姿态数据分别为R₁和R₂，其与真实位姿的关系满足：

R_i＝R+n_i,i＝1,2

其中，n₁和n₂分别表示惯导设备和激光雷达设备分别叠加在真实位姿R上的噪声；

惯导设备的状态误差导数为：

其中，

和

分别表示在t时刻时惯导设备的位置、速度、姿态角、加速度计bias和陀螺仪bias误差的导数；

表示惯导系与机体系的旋转关系；^∧表示从李代数到李群的变换；n_a、n_ω、

和

分别表示加速度计白噪声、陀螺仪白噪声、加速度计bias白噪声和陀螺仪bias白噪声；

其增量误差的协方差预测公式为：

P_t+δt＝(1+F_tδt)P_t(1+F_tδt)^T+(G_tδt)Q(G_tδt)^T

P的初始值设为0，Q表示噪声项对角协方差矩阵，具体为：

激光雷达设备进行激光扫描匹配时的误差采用一阶马尔科夫过程描述，具体为：

其中，

和

表示激光雷达设备的姿态角误差；T_1γ、T_1θ和T_1ψ分别为三个姿态角误差对应的相关时间；所述的ξ_1γ、ξ_1θ和ξ_1ψ为三个姿态角误差对应的白噪声；

协方差预测公式具体为：

P_t+δt＝(1+F_tδt)P_t(1+F_tδt)^T+(G_tδt)Q(G_tδt)^T

P的初始值设为0，Q表示噪声项对角协方差矩阵，具体为：

最终，融合后的真实姿态R具体为：

其中，W_i为信息矩阵，表示传感器输出在最终结果中所占比重，W₁和W₂分别为惯导设备和激光雷达设备协方差矩阵的逆。

步骤4：根据无人机的位置数据和优化后的姿态数据获得无人机在引水涵洞中的精确定位。

下面提供一个具体的实施例：

根据某引水涵洞施工时的项目施工CAD图获取引水涵洞的内径、姿态角和长度等参数信息，并根据上述参数信息构建简化的3Dmax模型，建立的3Dmax模型如图3所示，然后通过3Dmax模型生成引水涵洞点云模型，如图4所示。

在引水涵洞点云模型中确定引水涵洞的中轴线解析方程，根据无人机气压计回传的高度数据和惯导设备传回的含有较大噪声的姿态数据，将其代入引水涵洞中轴线解析方程即可获得无人机的位置数据，本实施例中的中轴线解析方程具体为：

中轴线方程可以分为两个部分，一部分为圆弧段，另一部分为斜线段，同时由于斜线段的重复性，当高度高于30米后，可以近似认为无人机一直处于30米的高度。

然后通过迭代最近点ICP算法与引水涵洞点云模型进行匹配，匹配时使用KDTree结构搜索空间最近点，KDTree搜索空间最近点时的结构如图5所示，通过树的节点将三维空间不断细化，加速空间点的索引速度；然后获得计算姿态数据，最后将计算姿态数据与原始姿态数据进行融合，获得优化后的无人机姿态数据，实现位姿校正，得到最终的定位结果，如图6所示，图6中黑色点云为引水涵洞模型，A为单线激光数据点云与模型点云中进行ICP配准后的姿态，并可从中获取无人机的姿态B为对无人机姿态进行融合校正后，无人机根据该校正后的定位数据进行的循迹轨迹，可以看出无人机的轨迹与引水涵洞的中轴线的偏差很小，所以本发明中的无人机定位方法达到了预期的效果，能够为无人机的自动循迹提供精确的定位数据。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种基于单线激光的引水涵洞内无人机定位方法，其特征在于，包括：

步骤1：构建引水涵洞的点云模型；

步骤2：获取无人机采集的高度信息和原始姿态数据；

步骤3：通过位置获取子方法获得无人机位置数据，同时通过姿态获取子方法获得优化后的无人机姿态数据；

步骤4：根据无人机的位置数据和优化后的姿态数据获得无人机在引水涵洞中的精确定位。

2.根据权利要求1所述的一种基于单线激光的引水涵洞内无人机定位方法，其特征在于，所述的步骤1具体为：

根据引水涵洞的CAD图像获取引水涵洞的参数信息，根据涵洞的参数信息构建3D模型，然后通过3D模型生成引水涵洞的点云模型。

3.根据权利要求1所述的一种基于单线激光的引水涵洞内无人机定位方法，其特征在于，所述的步骤2具体为：

通过安装在无人机上的气压计获得无人机的高度数据，同时通过安装在无人机上的惯导设备获得无人机的原始姿态数据。

4.根据权利要求1所述的一种基于单线激光的引水涵洞内无人机定位方法，其特征在于，所述步骤3中位置获取子方法具体为：

步骤3-1-1：在引水涵洞的点云模型中获得涵洞中轴线的解析方程；

步骤3-1-2：将无人机高度信息代入中轴线解析方程，获得无人机的位置数据。

5.根据权利要求1所述的一种基于单线激光的引水涵洞内无人机定位方法，其特征在于，所述步骤3中姿态获取子方法具体为：

步骤3-2-1：获取单线激光数据；

步骤3-2-2：对单线激光数据进行预处理；

步骤3-2-3：将单线激光数据转移到引水涵洞点云模型坐标系中对应的位置，然后将单线激光数据与引水涵洞点云模型进行匹配，获得计算姿态数据；

步骤3-2-4：将原始姿态数据和计算姿态数据融合，获得优化后的无人机姿态数据。

6.根据权利要求5所述的一种基于单线激光的引水涵洞内无人机定位方法，其特征在于，所述的步骤3-2-2具体为：

对单线激光数据进行滤波去噪处理。

7.根据权利要求5所述的一种基于单线激光的引水涵洞内无人机定位方法，其特征在于，所述的步骤3-2-3中单线激光数据通过迭代最近点算法与引水涵洞点云模型进行匹配。

8.根据权利要求7所述的一种基于单线激光的引水涵洞内无人机定位方法，其特征在于，所述的单线激光数据与引水涵洞点云模型进行匹配的具体步骤为：

步骤3-2-3-1：搜索最近点：

取单线激光数据点云P中一点p_i，然后在模型点云M中找出距离p_i欧氏距离最近的点m_i，(p_i,m_i)就构成了一组对应点对集；

步骤3-2-3-2：通过奇异值分解的方法求解p_i与m_i之间的变换关系(R,t)，其中R为p_i与m_i的旋转关系，t为p_i与m_i的位移关系；

步骤3-2-3-3：对激光点云P中的每一个点p_i运用变换关系(R,t)，即P'＝RP+t得到点集P'，定义目标函数：

其中n为点云P中点云的个数；

步骤3-2-3-4：判断目标函数f(R,t)是否小于预设阈值ε或是否达到预设的最大迭代次数，若是，则结束本轮循环，输出(R,t)，否则，返回步骤3-2-3-1。

9.根据权利要求8所述的一种基于单线激光的引水涵洞内无人机定位方法，其特征在于，所述步骤3-2-3-1中采用KDTree方法搜索最近点。

10.根据权利要求5所述的一种基于单线激光的引水涵洞内无人机定位方法，其特征在于，所述的步骤3-2-4中原始姿态数据通过卡尔曼滤波融合算法与计算姿态数据进行融合，具体为：

对于惯导设备和激光雷达设备输出的姿态数据分别为R₁和R₂，其与真实位姿的关系满足：

R_i＝R+n_i,i＝1,2

其中，n₁和n₂分别表示惯导设备和激光雷达设备分别叠加在真实位姿R上的噪声；

惯导设备的状态误差导数为：

其中，

和

分别表示在t时刻时惯导设备的位置、速度、姿态角、加速度计bias和陀螺仪bias误差的导数；

表示惯导系与机体系的旋转关系；^表示从李代数到李群的变换；n_a、n_ω、

和

分别表示加速度计白噪声、陀螺仪白噪声、加速度计bias白噪声和陀螺仪bias白噪声；

其增量误差的协方差预测公式为：

P_t+δt＝(1+F_tδt)P_t(1+F_tδt)^T+(G_tδt)Q(G_tδt)^T

P的初始值设为0，Q表示噪声项对角协方差矩阵，具体为：

激光雷达设备进行激光扫描匹配时的误差采用一阶马尔科夫过程描述，具体为：

其中，

和

表示激光雷达设备的姿态角误差；T_1γ、T_1θ和T_1ψ分别为三个姿态角误差对应的相关时间；所述的ξ_1γ、ξ_1θ和ξ_1ψ为三个姿态角误差对应的白噪声；

协方差预测公式具体为：

P_t+δt＝(1+F_tδt)P_t(1+F_tδt)^T+(G_tδt)Q(G_tδt)^T

P的初始值设为0，Q表示噪声项对角协方差矩阵，具体为：

最终，融合后的真实姿态R具体为：

其中，W_i为信息矩阵，表示传感器输出在最终结果中所占比重，W₁和W₂分别为惯导设备和激光雷达设备协方差矩阵的逆。

Images