适用于烟囱内部作业的飞行平台定位装置和定位方法与流程

本发明涉及无人机定位领域，特别是涉及到一种适用于烟囱内部作业的飞行平台定位装置和定位方法。

背景技术

多旋翼无人飞行器又称多轴飞行器，是一种结构简单、能够垂直起降的、多旋翼式无人飞行器，凭借其可靠的稳定性，多旋翼无人机经常被用于航拍，桥梁检测，定点巡航等场景。以多旋翼无人飞行器为平台搭载用于烟囱内壁腐蚀图像采集设备可以有效的代替传统的人工检测方法，有利于提高检测效率，降低人员风险，减少检测成本。

定位系统在机器人，无人飞行器等移动平台中起着重要的作用。路径规划，环境检测，实时控制等操作和动作都依赖于定位系统可靠高效的定位结果。准确和稳定的定位系统和定位方法是机器人和无人飞行器等实现其功能的基础和根本。

目前无人飞行器的定位方式主要采用gps卫星定位。火力发电厂烟囱内部本封闭空间的特性影响gps定位系统和气压计的稳定性和准确性。无人机飞行器的控制系统的运算能力难以支持视觉为主的定位方式。

技术实现要素：

为了解决背景技术中存在的问题，本发明的目的在于提供一种适用于烟囱内部作业的飞行平台定位装置和定位方法，可以基于先验的应用环境，通过算法准确有效的解算出无人飞行平台定位装置在烟囱内部作业时的位置。

本发明的技术方案包括：

一、一种适用于烟囱内部作业的飞行平台定位装置：

包括半封闭空间内部作业无人机、辅助定位装置、基于旋翼无人机的中继器和地面监控站；半封闭空间内部作业无人机上安装有辅助定位装置，半封闭空间内部作业无人机经中继器与地面监控站连接。

所述半封闭空间内部作业无人机包括多轴飞行器支架和安装多轴飞行器支架上的飞行控制器、飞行执行机构、电源系统、下承载板、一体式云台相机、加速度计、陀螺仪、电子罗盘、气压计、数传模块和图传模块；一体式云台相机上装载摄像头，飞行执行机构布置在多轴飞行器支架周围各角处，多轴飞行器支架上安装有加速度计、陀螺仪、电子罗盘、气压计和飞行控制器，飞行执行机构、加速度计、陀螺仪、电子罗盘和气压计均连接到飞行控制器；多轴飞行器支架中设有下承载板，下承载板底面安装有一体式云台相机，下承载板上安装有数传模块、图传模块和电源系统，数传模块、图传模块、一体式云台相机和电源系统均与飞行控制器连接。

所述的中继器包括数传中继器和图传中继器，数传模块、图传模块分别经数传中继器和图传中继器与地面监控站无线通信连接。

所述辅助定位装置包括辅助定位装置支架以及安装在辅助定位装置支架上的辅助定位处理器、存储模块、电源管理模块、二维环形激光雷达和四路超声波传感器；辅助定位装置支架固定安装在多轴飞行器支架上，四路超声波传感器布置在辅助定位装置支架周围侧面；二维环形激光雷达布置在辅助定位装置支架顶面，辅助定位装置支架中安装有辅助定位处理器、存储模块和电源管理模块，辅助定位处理器和飞行控制器通讯连接，辅助定位处理器分别与存储模块、电源管理模块、二维环形激光雷达和四路超声波传感器连接。

二维环形激光雷达和四路超声波传感器均用于采集获得角度信息及其对应的距离信息。

所述的辅助定位装置中的辅助定位处理器和飞行控制器之间通过can总线以pnp方式通讯连接。

所述的四路超声波传感器分别布置在辅助定位装置支架周围侧面的四边边缘处并且传感器探头水平向外布置。

所述的飞行执行机构主要由电机和螺旋桨构成。

二、一种适用于烟囱内部作业的飞行定位方法：

1)所述飞行平台定位装置由空中从烟囱顶端竖直进入烟囱内部，通过二维环形激光雷达和四路超声波传感器获取飞行平台定位装置在水平面所处位置的二维空间信息；

2)通过机载电子罗盘获取飞行平台定位装置机头正前方向的绝对角度信息，通过辅助定位处理器对所获取的二维空间信息和绝对角度信息进行融合解算得到飞行平台定位装置在烟囱为坐标系下的二维平面坐标点(x',y')；

3)通过一体式云台相机的摄像头结合烟囱已知总高度获取飞行平台定位装置在烟囱内部距离烟囱底部的第一高度信息，通过气压计获取飞行平台定位装置相对地面的第二高度信息，通过辅助定位处理器对两个高度信息进行融合，获得飞行平台定位装置在烟囱内部作业时距离地面的绝对高度h；

4)组合步骤2)获得的二维平面坐标点和步骤3)获得的绝对高度获得飞行平台定位装置以烟囱为坐标系下的空间位置(x',y'，h)。

所述步骤1)中，通过二维环形激光雷达所获取飞行平台定位装置在水平面所处位置的二维空间信息，具体为：

1.1)通过二维环形激光雷达得到各个采样点的采样信息，采样信息包括角度信息θi和距离信息di，i为二维环形激光雷达采样一周时的采样点序数；

1.2)二维环形激光雷达采样一周得到的采样信息构成一个采样点集，对采样点集去除误差：

1.2.1)通过二维环形激光雷达采样一周得到的采样点的角度信息θi和距离信息di，采用以下公式获取相邻采样点之间的采样差信息(δθn，δdn)：

δθn＝θi-θi-1，δdn＝di-di-1，n＝(1,2,3...m)

其中，δθn为相邻采样点角度变化信息，δdn为相邻角度变化信息δθn所对应的距离变化信息，m表示采样差总数，n表示采样差的序数；

1.2.2)将采样点集以平均分布的每五个点为一组的采样差信息建立拟合点集，从而将采样点集的采样差信息分为多个拟合点集，第k个拟合点集φk表示为：

φk＝{(δθk,δdk)}(k＝n,n+s,n+2s,n+3s,n+4s),(n＝1，2，3，...s),(s＝1/5m)

将每个拟合点集φk二次函数多项式f(x)＝ax²+bx+c拟合并通过最小二乘法解算每个拟合点集φk的的参数解(ak,bk,ck)，(ak,bk,ck)表示第k个拟合点集φk通过二次函数多项式拟合后的第一、第二、第三参数解，将各个拟合点集的参数解构成多项式参数解集：

u＝{(ak,bk,ck)|k＝1,2,3...s}

1.2.3)针对每个参数解(ak,bk,ck)采用以下公式的筛选条件进行判断：

其中，δ1、δ2、δ3分别表示第一、第二、第三参数解的偏差阈值；

若参数解(ak,bk,ck)中的任一参数解不满足上述参数解(ak,bk,ck)筛选条件，则将该参数解(ak,bk,ck)的拟合点集φk对应的采样点从采样点集中剔除，得到去除误差的采样点集；

1.3)通过扩展卡尔曼滤波算法对去除误差的采样点集处理去除噪声干扰，得到的二维环形激光雷达采样一周得到的雷达初始采样信息集ψ1＝{(θ'i,d'i)}。

所述步骤1)中，通过四路超声波传感器所获取飞行平台定位装置在水平面所处位置的二维空间信息，具体为：

以飞行平台定位装置的无人机机头正前方向为0°，作为参考方向，通过测量得到四路超声波传感器的四个超声波传感器分别在飞行平台定位装置上相对于参考方向的安装角度α，超声波传感器通过卡尔曼滤波后获取距离初始距离信息l，四路超声波传感器分别得到超声波初始采样信息集ψ2＝{(αu,lu)|u＝1,2,3,4}。

所述步骤3)具体为：

3.1)固定机载摄像头朝向为水平，飞行平台定位装置根据摄像头在从烟囱顶端下降飞行过程中采集到的图像序列解算出飞行平台定位装置在烟囱内部由烟囱顶端向烟囱底端飞行的距离c，结合烟囱已知总高度h获取飞行平台定位装置在烟囱内部距离烟囱底部的第一高度信息h1＝h-c；

3.2)通过气压计经过卡尔曼滤波滤波后获得第二高度信息h2；

3.3)通过以下公式的加权数据融合算法计算获取飞行平台定位装置在烟囱内部作业时距离地面的绝对高度h：

h＝ph1+(1-p)h2

其中，p表示高度权值。

所述步骤2)具体为：

2.1)将雷达初始采样信息集ψ1和超声波初始采样信息集ψ2合并，组成以飞行平台定位装置中心为原点、以飞行平台定位装置机头正前方为0°的水平面坐标系下的二维测量信息集ψ＝ψ1∪ψ2＝{(ωt，zt)}，其中ωt为第t个二维角度信息，zt为第t个二维距离信息；

2.2)根据二维测量信息集电子罗盘获取的机头正前方向与正东方向之间的逆时针绝对夹角采用以下公式通过极坐标直角坐标系转换修正方向偏差得到二维空间直角坐标信息(xt,yt)：

其中，表示通过电子罗盘获得的逆时针绝对夹角；

2.3)将二维空间直角坐标信息(xt,yt)拟合成二维圆形曲线方程(x-w)²+(y-e)²＝r²，获得二维圆形曲线方程的参数w、e和r，其中(w,e)为拟合圆心坐标并作为烟囱圆形结构中心点位置坐标，r为拟合圆半径；

2.4)通过坐标转换将烟囱圆形结构中心点位置坐标(m,n)转换到(0，0)，获取飞行平台定位装置的二维空间直角坐标信息(xt,yt)在烟囱为坐标系下的二维平面坐标点(x',y')。

本发明对于现有的技术具有以下有益效果：

本发明将将二维激光雷达结合四路超声波传感器搭载在无人飞行器平台上，获取飞行平台定位装置在二维平面中距离墙壁的极坐标信息，并通过先验的结构信息，提高定位算法的准确性和速度。通过相机标定法和飞行平台定位装置距离信息获得从顶部向下运行的距离，结合气压计信息获取飞行器在烟囱内部的高度信息，扩展了无人机飞行平台定位装置的应用领域，在无人飞行器定位领域和火力发电厂烟囱内壁腐蚀检测的行业应用领域均具有实际的应用价值。

附图说明

图1为本发明飞行平台定位装置俯视结构图。

图2为本发明飞行平台定位装置一侧视结构图。

图3为本发明飞行平台定位装置另一侧视局部结构图。

图4为本发明内各个部分模块电气连接结构图。

图5为本发明的定位系统流程图。

图6是发明实施例在烟囱个内部二维平面定位示意图。

图中：1、二维环形激光雷达，2、四路超声波传感器，3、辅助定位处理器，4、辅助定位装置支架，5、加速度计，6、陀螺仪，7、电子罗盘，8、数传模块，9、图传模块，10、电源系统，11、下承载板，12、一体式云台相机，13、多轴飞行器支架，14气压计，15、数传中继器，16、图传中继器、17、飞行执行机构，18、飞行控制器。s1、半封闭空间内部作业无人机，s2、辅助定位装置，s3、中继器，s4、地面监控站。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

如图1所示，本发明包括半封闭空间内部作业无人机s1、辅助定位装置s2、基于旋翼无人机的中继器s3和地面监控站s4；半封闭空间内部作业无人机s1上安装有辅助定位装置s2，半封闭空间内部作业无人机s1经中继器s3与地面监控站s4连接。地面监控站s4可以采用计算机。

如图1-图3所示，半封闭空间内部作业无人机s1包括多轴飞行器支架13和安装多轴飞行器支架13上的飞行控制器18、飞行执行机构17、电源系统10、下承载板11、一体式云台相机12、加速度计5、陀螺仪6、电子罗盘7、气压计14、数传模块8和图传模块9；一体式云台相机12上装载摄像头，飞行执行机构17布置在多轴飞行器支架13周围各角处，飞行执行机构17主要由电机和螺旋桨构成，多轴飞行器支架13上安装有加速度计5、陀螺仪6、电子罗盘7、气压计14和飞行控制器18，飞行执行机构17、加速度计5、陀螺仪6、电子罗盘7和气压计14均连接到飞行控制器18；多轴飞行器支架13中设有下承载板11，下承载板11底面安装有一体式云台相机12，下承载板11上安装有数传模块8、图传模块9和电源系统10，数传模块8、图传模块9、一体式云台相机12和电源系统10均与飞行控制器18连接；电源系统10用于对飞行控制器18及无人机中与飞行控制器18连接的各个器件进行供电。

如图4所示，中继器s3包括数传中继器15和图传中继器16，数传模块8、图传模块9分别经数传中继器15和图传中继器16与地面监控站s4无线通信连接；辅助定位装置s2包括辅助定位装置支架4以及安装在辅助定位装置支架4上的辅助定位处理器3、存储模块19、电源管理模块20、二维环形激光雷达1和四路超声波传感器2；辅助定位装置支架4固定安装在多轴飞行器支架13上，位于下承载板11上方，四路超声波传感器2布置在辅助定位装置支架4周围侧面，沿辅助定位装置支架4周向间隔均布，四路超声波传感器2的传感器探头朝向分别朝向相对于多轴飞行器支架13的东南西北四个方向，其中一个超声波传感器2的传感器探头朝向无人机s1机头正方向；二维环形激光雷达1布置在辅助定位装置支架4顶面，辅助定位装置支架4中安装有辅助定位处理器3、存储模块19和电源管理模块20，辅助定位处理器3和飞行控制器18通讯连接，辅助定位处理器3分别与存储模块19、电源管理模块20、二维环形激光雷达1和四路超声波传感器2连接，电源管理模块20用于对辅助定位处理器3、存储模块19、二维环形激光雷达1和四路超声波传感器2进行供电。

辅助定位装置s2中的辅助定位处理器3和飞行控制器18通讯连接，将从二维环形激光雷达1和四路超声波传感器2采集到的数据与无人机s1的飞行控制器18本身采集到的飞行数据融合处理，获得无人机在半封闭空间内部的空间位置信息。

辅助定位装置s2中的辅助定位处理器3和飞行控制器18之间通过can总线以pnp方式通讯连接。

四路超声波传感器2分别布置在辅助定位装置支架4周围侧面的四边边缘处并且传感器探头水平向外布置。

如图5所示，本发明的具体实施例及其实施工作过程如下：

1)飞行平台定位装置由空中从烟囱顶端竖直进入烟囱内部，如图6所示，通过二维环形激光雷达1和四路超声波传感器3获取飞行平台定位装置在水平面所处位置的二维空间信息；

a)通过二维环形激光雷达1所获取飞行平台定位装置在水平面所处位置的二维空间信息，具体为：

1.1)通过二维环形激光雷达1得到各个采样点的采样信息，采样信息包括角度信息θi和距离信息di，i为二维环形激光雷达1采样一周时的采样点序数，角度信息θi是指二维环形激光雷达1探头采样时的圆周角度值，距离信息di是指二维环形激光雷达1探头采样时的所探测到距离烟囱内壁的间距值；

1.2)烟囱内部同一水平面的内壁形状视为圆形结构，二维环形激光雷达1采样一周得到的采样信息构成一个采样点集(θi,di)，对采样点集通过设定采样阈值δl(l＝1,2,3)去除误差：

1.2.1)通过二维环形激光雷达1采样一周得到的采样点的角度信息θi和距离信息di，采用以下公式获取相邻采样点之间的采样差信息(δθn，δdn)：

δθn＝θi-θi-1，δdn＝di-di-1，n＝(1,2,3...m)

其中，δθn为相邻采样点角度变化信息，δdn为相邻角度变化信息δθn所对应的距离变化信息，m表示采样差总数，n表示采样差的序数；

采样差信息(δθn，δdn)满足二次函数多项式：

δdn＝aδθn²+bδθn+c

其中，(ak,bk,ck)表示第k个拟合点集φk通过二次函数多项式拟合后的第一、第二、第三参数解。

1.2.2)将采样点集以平均分布的每五个点为一组的采样差信息建立拟合点集，从而将采样点集的采样差信息分为多个拟合点集，第k个拟合点集φk表示为：

φk＝{(δθk,δdk)}(k＝n,n+s,n+2s,n+3s,n+4s),(n＝1，2，3，...s),(s＝1/5m)

将每个拟合点集φk二次函数多项式f(x)＝ax²+bx+c拟合并通过最小二乘法解算每个拟合点集φk的的参数解(ak,bk,ck)，(ak,bk,ck)表示第k个拟合点集φk通过二次函数多项式拟合后的第一、第二、第三参数解，将各个拟合点集的参数解构成多项式参数解集：

u＝{(ak,bk,ck)|k＝1,2,3...s}

1.2.3)针对每个参数解(ak,bk,ck)采用以下公式的筛选条件进行判断：

其中，δ1、δ2、δ3分别表示第一、第二、第三参数解的偏差阈值，具体实施中取为δ1＝0.7，δ2＝0.5，δ3＝0.3；

若参数解(ak,bk,ck)中的任一参数解不满足上述参数解(ak,bk,ck)筛选条件，则将该参数解(ak,bk,ck)的拟合点集φk对应的采样点从采样点集(θi,di)中剔除，得到去除误差的采样点集；

1.3)通过扩展卡尔曼滤波算法对去除误差的采样点集处理去除噪声干扰，得到的二维环形激光雷达1采样一周得到的雷达初始采样信息集ψ1＝{(θ'i,d'i)}，作为二维空间信息。

b)通过四路超声波传感器3所获取飞行平台定位装置在水平面所处位置的二维空间信息，具体为：

以飞行平台定位装置的无人机机头正前方向为0°，作为参考方向，通过测量得到四路超声波传感器3的四个超声波传感器分别在飞行平台定位装置上相对于参考方向的安装角度α，超声波传感器通过卡尔曼滤波后获取距离初始距离信息l，四路超声波传感器3分别得到超声波初始采样信息集ψ2＝{(au,lu)|u＝1,2,3,4}，作为二维空间信息，u表示超声波传感器在飞行平台定位装置上安装位置的序数。

2)通过机载电子罗盘7获取飞行平台定位装置机头正前方向的绝对角度信息，通过辅助定位处理器2对所获取的二维空间信息和绝对角度信息进行融合解算得到飞行平台定位装置在烟囱为坐标系下的二维平面坐标点(x',y')；

2.1)将雷达初始采样信息集ψ1和超声波初始采样信息集ψ2合并，组成以飞行平台定位装置中心为原点、以飞行平台定位装置机头正前方为0⁰的水平面坐标系下能够反映二维空间环境角度和距离的二维测量信息集ψ＝ψ1∪ψ2＝{(ωt，zt)}，t＝i+u，t＝1,...,m+4，其中ωt为第t个二维角度信息，zt为第t个二维距离信息；

2.2)根据二维测量信息集电子罗盘14获取的机头正前方向与正东方向之间的逆时针绝对夹角采用以下公式通过极坐标直角坐标系转换修正方向偏差得到二维空间直角坐标信息(xt,yt)：

其中，表示通过电子罗盘7获得的逆时针绝对夹角；

2.3)通过随机抽样一致性算法和最小二乘法将二维空间直角坐标信息(xt,yt)拟合成二维圆形曲线方程(x-w)²+(y-e)²＝r²，获得二维圆形曲线方程的参数w、e和r，具体是先用随机抽样一致性算法得到局内点，再用最小二乘法对局内点拟合求解，其中(w,e)为拟合圆心坐标并作为烟囱圆形结构中心点位置坐标，r为拟合圆半径；

2.4)通过坐标转换将烟囱圆形结构中心点位置坐标(m,n)转换到0，0，获取飞行平台定位装置的二维空间直角坐标信息(xt,yt)在烟囱为坐标系下的二维平面坐标点(x',y')。

3)通过一体式云台相机12的摄像头结合烟囱已知总高度获取飞行平台定位装置在烟囱内部距离烟囱底部的第一高度信息，通过气压计14获取飞行平台定位装置相对地面的第二高度信息，通过辅助定位处理器2对两个高度信息进行融合，获得飞行平台定位装置在烟囱内部作业时距离地面的绝对高度h；

3.1)固定机载摄像头朝向为水平，飞行平台定位装置根据摄像头在从烟囱顶端下降飞行过程中采集到的图像序列解算出飞行平台定位装置在烟囱内部由烟囱顶端向烟囱底端飞行的距离c，结合烟囱已知总高度h获取飞行平台定位装置在烟囱内部距离烟囱底部的第一高度信息h1＝h-c；

3.2)通过气压计14经过卡尔曼滤波滤波后获得第二高度信息h2；

3.3)通过以下公式的加权数据融合算法计算获取飞行平台定位装置在烟囱内部作业时距离地面的绝对高度h：

h＝ph1+(1-p)h2

其中，p表示高度权值。

具体实施通过辅助定位处理器根据装置运行情况和图像高度信息与气压计高度信息的实时稳定情况，自动调整权值。

通过一体式云台相机12的摄像头结合烟囱已知总高度获取飞行平台定位装置与烟囱顶端入口之间的距离信息c，具体为；

首先，以摄像头的位置作为飞行平台定位装置的位置，根据飞行平台定位装置在烟囱内部的定位算法获取摄像头与其采集当下时刻图像时所朝向的烟囱内壁之间的距离z，由距离z结合摄像头标定获取所能采集到的图像的宽度系数b计算得到采集图像的宽度y＝b*z，宽度为图像的纵方向，对应于沿竖直方向的距离；

接着，控制飞行平台定位装置从烟囱顶部进入烟囱内部采集图像信息，采集飞行平台定位装置带动摄像头旋转一周为一次作业的。一次作业完成后竖直下降一段距离进行第二次作业，保证每次竖直向下的一段距离使得上一次作业和下一次作业同一侧图像的特征点重复率在30％-50％之间，依次作业至底部。飞行平台定位装置上以摄像头正东方向的图像为高度解算图像。

通过特征点匹配的算法拼接正东方向的图像，根据正东方向每两张连续图特征点之间的图像距离，解算出依次下降的距离y1,y2,y3…，最后根据依次下降的距离累加计算出飞行平台定位装置在烟囱内部由烟囱顶部向烟囱底部飞行的距离c＝y1+y2+y3+…。

4)组合步骤2)获得的二维平面坐标点和步骤3)获得的绝对高度获得飞行平台定位装置以烟囱为坐标系下的空间位置(x',y'，h)。

由此，本发明将二维激光雷达结合四路超声波传感器搭载在无人飞行器平台上，能准确获取飞行平台在二位平面中距离墙壁的位置，获得从顶部向下运行的距离，结合气压计信息获取飞行器在烟囱内部的高度信息，具有其突出显著的技术效果。