CN111857167A - 一种基于单线激光数据椭圆拟合的引水涵洞内无人机定位方法 - Google Patents

Abstract

本发明涉及一种基于单线激光数据椭圆拟合的引水涵洞内无人机定位方法，包括：构建引水涵洞模型；获取无人机采集的高度信息、原始姿态数据以及单线激光数据；通过无人机采集的高度信息获得无人机在引水涵洞模型中的位置坐标数据，同时对单线激光数据进行预处理；对单线激光数据进行椭圆拟合，求解拟合后的椭圆参数；求解拟合椭圆的外接圆柱体；通过外接圆柱体求解无人机在引水涵洞中的计算姿态数据；将原始姿态数据与计算姿态数据进行融合，获得优化后的无人机姿态数据；根据无人机的位置坐标数据以及优化后的无人机姿态数据获得无人机在引水涵洞中的精确定位。与现有技术相比，本发明具有能够实现引水涵洞内无人机的高精度定位等优点。

Description

一种基于单线激光数据椭圆拟合的引水涵洞内无人机定位 方法

技术领域

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

背景技术

随着我国新兴能源的大规模开发利用，抽水蓄能电站的配置由过去单一的侧重于用电负荷中心逐步向用电负荷中心、能源基地、送出端和落地端等多方面发展，新能源的迅速发展需要加速抽水蓄能电站建设，而用于引水的引水涵洞的维护检修问题也逐渐得到了业界的普遍关注。以往，通常采用人工进入引水涵洞中观察壁面情况，一旦发生安全事故，涵洞外部的救援人员很难对涵洞内被困人员的精确定位和救援。

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

发明内容

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

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

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

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

步骤2：获取无人机采集的高度信息、原始姿态数据以及单线激光数据；

步骤3：通过无人机采集的高度信息获得无人机在引水涵洞模型中的位置坐标数据，同时对单线激光数据进行预处理；

步骤4：对单线激光数据进行椭圆拟合，求解拟合后的椭圆参数；

步骤5：求解拟合椭圆的外接圆柱体；

步骤6：通过外接圆柱体求解无人机在引水涵洞中的计算姿态数据；

步骤7：将原始姿态数据与计算姿态数据进行融合，获得优化后的无人机姿态数据；

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

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

根据引水涵洞的CAD模型获取引水涵洞的参数信息，根据涵洞的参数信息构建引水涵洞3D模型。

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

通过安装在无人机上的气压计获得无人机的高度数据，通过安装在无人机上的惯导设备获得无人机的原始姿态数据，通过安装在无人机上的激光雷达设备获得单线激光数据。

优选地，所述的步骤3中对单线激光数据进行的预处理具体为：

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

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

平面上椭圆曲线的一般方程表达式为：

x²+Axy+By²+Cx+Dy+E＝0

设a为椭圆半长轴，b为椭圆半短轴，x₀为椭圆中心点横坐标，y₀为椭圆中心点纵坐标，θ为椭圆长轴与横轴坐标系的夹角；

设置过渡变量F、G和H，具体为：

则

定义误差函数f，具体为：

其中，m为椭圆拟合采样点的数目；

将各个采样点的坐标代入误差函数后，利用最小二乘法求解A、B、C、D和E这个五个参数，并且求解出椭圆半长轴a、椭圆半短轴b、椭圆中心点横坐标x₀、椭圆中心点纵坐标y₀和椭圆长轴与横轴坐标系的夹角θ。

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

在拟合椭圆的焦点处，位于一个垂直于拟合椭圆平面的平面上，在高度为半短轴b的位置设置一点P，通过椭圆中心点与点P获得的直线即为外接圆柱的中轴线，通过中轴线即可获得多个外接圆柱，然后对多个外接圆柱进行筛选，获得最终的外接圆柱。

更加优选地，所述的对外接圆柱进行筛选的具体步骤为：

判断所有外接圆柱的中轴线斜率，去除斜率为负的外接圆柱，最终获得斜率为正的外接圆柱。

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

在引水涵洞模型中，无人机采集的高度信息所确定的无人机高度附近将拟合椭圆外接圆柱体与引水涵洞模型进行匹配，在匹配度最高处拟合椭圆所在平面的倾斜角度即为无人机的计算姿态数据。

优选地，所述的步骤7中原始姿态数据通过卡尔曼滤波融合算法与计算姿态数据进行融合。

更加优选地，所述的融合步骤具体为：

对于惯导设备和激光雷达设备输出的姿态数据分别为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₂分别为惯导设备和激光雷达设备协方差矩阵的逆。

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

本发明中的引水涵洞内无人机定位方法实现了引水涵洞内无人机的高精度定位：首先通过无人机的高度数据来获得无人机在引水涵洞内的位置数据；使用椭圆拟合确定外接圆柱体，然后通过外接圆柱体与引水涵洞模型进行匹配来获得无人机计算姿态数据，最后将计算姿态数据与无人机惯导设备采集的原始姿态数据进行融合，获得优化后的无人机姿态数据，提高了无人机定位的准确度，实现了无人机的高精度定位。

附图说明

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

图2为本发明中椭圆拟合时的结构示意图；

图3为本发明中恢复外接圆柱体时的结构示意图；

图4为本发明实施例中由椭圆恢复的外接圆柱体结构示意图；

图5为本发明实施例中将外接圆柱体与引水涵洞模型进行匹配时的结构示意图；

图6为本发明实施例中最终得到的实际配准图。

图中标号所示：

A、单线激光数据投影，B、无人机循迹轨迹。

具体实施方式

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

一种单线激光数据椭圆拟合的引水涵洞内无人机定位方法，其流程如图1所示，包括：

步骤1：根据引水涵洞的CAD模型获取引水涵洞的参数信息，根据涵洞的参数信息构建引水涵洞3D模型；

步骤2：通过安装在无人机上的气压计获得无人机的高度数据，通过安装在无人机上的惯导设备获得无人机的原始姿态数据，通过安装在无人机上的激光雷达设备获得单线激光数据；

步骤3：通过无人机采集的高度信息获得无人机在引水涵洞模型中的位置坐标数据，同时对单线激光数据进行预处理，预处理具体为滤波去噪处理；

步骤4：对单线激光数据进行椭圆拟合，求解拟合后的椭圆参数；

具体为：平面上椭圆曲线的一般方程表达式为：

x²+Axy+By²+Cx+Dy+E＝0

设a为椭圆半长轴，b为椭圆半短轴，x₀为椭圆中心点横坐标，y₀为椭圆中心点纵坐标，θ为椭圆长轴与横轴坐标系的夹角；

设置过渡变量F、G和H，具体为：

则

定义误差函数f，具体为：

其中，m为椭圆拟合采样点的数目；

将各个采样点的坐标代入误差函数后，利用最小二乘法求解A、B、C、D和E这个五个参数，并且求解出椭圆半长轴a、椭圆半短轴b、椭圆中心点横坐标x₀、椭圆中心点纵坐标y₀和椭圆长轴与横轴坐标系的夹角θ，本实施例在进行椭圆拟合时的结构示意图如图2所示。

步骤5：求解拟合椭圆的外接圆柱体，具体为：

在拟合椭圆的焦点处，位于一个垂直于拟合椭圆平面的平面上，在高度为半短轴b的位置设置一点P，通过椭圆中心点与点P获得的直线即为外接圆柱的中轴线，通过中轴线即可获得多个外接圆柱，然后对多个外接圆柱进行筛选，获得最终的外接圆柱。

筛选方法为：判断所有外接圆柱的中轴线斜率，去除斜率为负的外接圆柱，最终获得斜率为正的外接圆柱，本实施例中恢复外接圆柱体时的结构示意图如图3所示，最终恢复的外接圆柱体如图4所示。

步骤6：通过外接圆柱体求解无人机在引水涵洞中的计算姿态数据，具体为：

在引水涵洞模型中，无人机采集的高度信息所确定的无人机高度附近将拟合椭圆外接圆柱体与引水涵洞模型进行匹配，在匹配度最高处拟合椭圆所在平面的倾斜角度即为无人机的计算姿态数据，本实施例在将拟合椭圆外接圆柱体与引水涵洞进行匹配时的结构示意图如图5所示。

步骤7：通过卡尔曼滤波融合算法将原始姿态数据与计算姿态数据进行融合，获得优化后的无人机姿态数据，具体步骤为：

对于惯导设备和激光雷达设备输出的姿态数据分别为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₂分别为惯导设备和激光雷达设备协方差矩阵的逆。

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

A为单线激光数据的投影，单线激光数据点云与模型点云中进行ICP配准后的姿态，并可从中获取无人机的姿态，B为对无人机姿态进行融合校正后，无人机根据该校正后的定位数据进行的循迹轨迹，可以看出无人机的轨迹与引水涵洞的中轴线的偏差很小，所以本发明中的无人机定位方法达到了预期的效果，能够为无人机的自动循迹提供精确的定位数据。

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

Claims (10)

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

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

步骤2：获取无人机采集的高度信息、原始姿态数据以及单线激光数据；

步骤3：通过无人机采集的高度信息获得无人机在引水涵洞模型中的位置坐标数据，同时对单线激光数据进行预处理；

步骤4：对单线激光数据进行椭圆拟合，求解拟合后的椭圆参数；

步骤5：求解拟合椭圆的外接圆柱体；

步骤6：通过外接圆柱体求解无人机在引水涵洞中的计算姿态数据；

步骤7：将原始姿态数据与计算姿态数据进行融合，获得优化后的无人机姿态数据；

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

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

根据引水涵洞的CAD模型获取引水涵洞的参数信息，根据涵洞的参数信息构建引水涵洞3D模型。

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

通过安装在无人机上的气压计获得无人机的高度数据，通过安装在无人机上的惯导设备获得无人机的原始姿态数据，通过安装在无人机上的激光雷达设备获得单线激光数据。

4.根据权利要求1所述的一种基于单线激光数据椭圆拟合的引水涵洞内无人机定位方法，其特征在于，所述的步骤3中对单线激光数据进行的预处理具体为：

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

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

平面上椭圆曲线的一般方程表达式为：

x²+Axy+By²+Cx+Dy+E＝0

设a为椭圆半长轴，b为椭圆半短轴，x₀为椭圆中心点横坐标，y₀为椭圆中心点纵坐标，θ为椭圆长轴与横轴坐标系的夹角；

设置过渡变量F、G和H，具体为：

则

定义误差函数f，具体为：

其中，m为椭圆拟合采样点的数目；

将各个采样点的坐标代入误差函数后，利用最小二乘法求解A、B、C、D和E这个五个参数，并且求解出椭圆半长轴a、椭圆半短轴b、椭圆中心点横坐标x₀、椭圆中心点纵坐标y₀和椭圆长轴与横轴坐标系的夹角θ。

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

在拟合椭圆的焦点处，位于一个垂直于拟合椭圆平面的平面上，在高度为半短轴b的位置设置一点P，通过椭圆中心点与点P获得的直线即为外接圆柱的中轴线，通过中轴线即可获得多个外接圆柱，然后对多个外接圆柱进行筛选，获得最终的外接圆柱。

7.根据权利要求6所述的一种基于单线激光数据椭圆拟合的引水涵洞内无人机定位方法，其特征在于，所述的对外接圆柱进行筛选的具体步骤为：

判断所有外接圆柱的中轴线斜率，去除斜率为负的外接圆柱，最终获得斜率为正的外接圆柱。

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

在引水涵洞模型中，无人机采集的高度信息所确定的无人机高度附近将拟合椭圆外接圆柱体与引水涵洞模型进行匹配，在匹配度最高处拟合椭圆所在平面的倾斜角度即为无人机的计算姿态数据。

9.根据权利要求1所述的一种基于单线激光数据椭圆拟合的引水涵洞内无人机定位方法，其特征在于，所述的步骤7中原始姿态数据通过卡尔曼滤波融合算法与计算姿态数据进行融合。

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

对于惯导设备和激光雷达设备输出的姿态数据分别为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