CN105334518A - 一种基于室内四旋翼飞行器的激光雷达三维成像方法 - Google Patents

Abstract

本发明提供了一种基于室内四旋翼飞行器的激光雷达三维成像方法，步骤包括：读取加速度数据和距离数据，计算获得四旋翼飞行器的位移数据；读取惯性测量数据，计算获得四旋翼飞行器的姿态角度数据；读取深度断层图像数据，计算获得深度断层图像；根据位移数据和姿态角度数据确定深度断层图像数据的采集位置信息，再在相邻位置的深度断层图像之间使用滑动窗口牛顿插值法，从而实现三维成像。该三维成像方法具有高度的灵活性，相对于地面移动机器人能够降低复杂地面对飞行的影响，具有较高的适应能力。

Description

一种基于室内四旋翼飞行器的激光雷达三维成像方法

技术领域

本发明涉及一种三维成像方法，尤其是一种基于室内四旋翼飞行器的三维成像方法。

背景技术

随着现代航空航天事业的飞速发展，自主飞行器也在现代高新技术条件下快速发展。飞行器能在枯燥、危险、恶劣环境中发挥重要作用。在军事领域中能够实施低空侦察，情报采集等；在民用领域中可以完成资源勘探、边境巡逻、灾情监视等。而对于坍塌楼房，矿井等地面移动机器人无法胜任的环境下，灵活的自主飞行器对于探测未知环境是很好的选择。

对于三维成像方法，基于图像处理获取场景信息，通过图像拼接实现三维成像，该方法算法复杂，受光照影响，可靠性低；基于激光的三维成像方法主要应用于静态对象的三维建模，激光器的摆放位置严格，设备便捷度低；基于地面移动机器人的三维成像方法受环境障碍物影响大，无法全方位实现三维成像。

发明内容

本发明要解决的技术问题是现有的三维成像方法使用复杂，且常常由于环境障碍物影响无法全方位实现三维成像。

为了解决上述技术问题，本发明提供了一种基于室内四旋翼飞行器的激光雷达三维成像方法，包括如下步骤：

步骤1，读取四旋翼飞行器惯性导航系统测量的加速度数据以及测距系统测量的距离数据，再利用互补滤波法对加速度数据和距离数据进行数据融合，从而获得四旋翼飞行器的位移数据；

步骤2，读取四旋翼飞行器惯性导航系统测量的惯性测量数据，并利用卡尔曼滤波对惯性测量数据进行数据融合，获得四旋翼飞行器的姿态角度数据；

步骤3，读取四旋翼飞行器前进方向上激光雷达采集的深度断层图像数据，再对深度断层图像数据进行自适应中值滤波，并进行基于位移和角度的补偿，获得深度断层图像；

步骤4，根据步骤1的位移数据和步骤2的姿态角度数据确定四旋翼飞行器运动轨迹，并对运动轨迹上的相邻数据采集点的深度断层图像使用滑动窗口牛顿插值法，从而实现三维成像。

采用四旋翼飞行器作为搭载平台具有高度的灵活性，相对于地面移动机器人能够降低复杂地面对飞行的影响，具有较高的适应能力；采用测量的位移数据和姿态角度数据确定深度断层图像数据的位置，从而有效提高了激光雷达的扫描范围，扩大了三维成像的面积范围。

作为本发明的进一步限定方案，步骤1中，获得四旋翼飞行器位移数据的具体步骤为：

步骤1.1，读取的加速度数据为(a_x(t),a_y(t),a_z(t))，进行二重积分得到t_c-1到t_c时刻的近似位移(x_t,y_t,z_t)为：

$\left[\begin{array}{c}{x}_t\\ y_t\\ z_t\end{array}\right]=\left[\begin{array}{c}{\∫}_{t_{c-1}}^{t_c}\left(v_x\right(t_{c-1})+{\∫}_{t_{c-1}}^{t_c}{a}_x\left(t\right))dt\\ {\∫}_{t_{c-1}}^{t_c}\left(v_y\right(t_{c-1})+{\∫}_{t_{c-1}}^{t_c}{a}_y\left(t\right))dt\\ {\∫}_{t_{c-1}}^{t_c}\left(v_z\right(t_{c-1})+{\∫}_{t_{c-1}}^{t_c}{a}_z\left(t\right))dt\end{array}\right]---\left(1\right)$

式中，(v_x(t_c-1),v_y(t_c-1),v_z(t_c-1))为t_c-1时刻的速度；

步骤1.2，读取到t_c-1和t_c时刻的距离数据为和计算t_c-1到t_c时刻的近似位移(mx_t,my_t,mz_t)为：

$\left[\begin{array}{c}m{x}_t\\ {\mathrm{my}}_t\\ {\mathrm{mz}}_t\end{array}\right]=\left[\begin{array}{c}{x}_{m_t}-x_{m_{t-1}}\\ y_{m_t}-y_{m_{t-1}}\\ z_{m_t}-z_{m_{t-1}}\end{array}\right]---\left(2\right)$

步骤1.3，利用互补滤波法进行数据融合获得四旋翼飞行器的位移数据为：

$\left[\begin{array}{c}{\stackrel{\‾}{x}}_t\\ {\stackrel{\‾}{y}}_t\\ {\stackrel{\‾}{z}}_t\end{array}\right]=\left[\begin{array}{c}{a}_x({\stackrel{\‾}{x}}_{t-1}+x_t*T_u)+(1-a_x)*{\mathrm{mx}}_t\\ a_y({\stackrel{\‾}{y}}_{t-1}+y_t*T_u)+(1-a_y)*{\mathrm{my}}_t\\ a_z({\stackrel{\‾}{z}}_{t-1}+z_t*T_u)+(1-a_z)*{\mathrm{mz}}_t\end{array}\right]---\left(3\right)$

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\τ}}_x/({\mathrm{\τ}}_x+T_u)\\ {\mathrm{\τ}}_y/({\mathrm{\τ}}_y+T_u)\\ {\mathrm{\τ}}_z/({\mathrm{\τ}}_z+T_u)\end{array}\right]---\left(4\right)$

式中，T_u为超声波传感器采样周期，(τ_x,τ_y,τ_z)为置信程度，(a_x,a_y,a_z)为动态加权系数。

由于加速度积分得到的位移累积误差大，而测距系统得到的距离数据准确度高，但是存在大量噪声，所以采用互补滤波法进行数据融合，能够得到置信度更高的四旋翼飞行器位移。

作为本发明的进一步限定方案，步骤2中，获得四旋翼飞行器的姿态角度数据的具体步骤为：

步骤2.1，四旋翼飞行器惯性导航系统测量的X轴上惯性测量数据包括倾角陀螺仪测量角速度ω_gyro、陀螺仪测量噪声ω_g、螺仪常值误差b、加速度计测量角度以及角速度计测量噪声ω_a，四旋翼飞行器惯性导航系统的状态方程和输出方程为：

步骤2.2，设四旋翼飞行器惯性导航系统的采样周期为Ts，则上述方程离散化后为：

X(k|k-1)＝AX(k-1|k-1)+BU(k)(6)

式中， $A=\left[\begin{array}{cc}1& -Ts\\ 0& 1\end{array}\right],B=\left[\begin{array}{c}Ts\\ 0\end{array}\right],$ X(k-1|k-1)为k-1时刻的后验估计，X(k|k-1)为k时刻的先验估计，X(k|k-1)的协方差为：

P(k|k-1)＝AP(k-1|k-1)A^T+Q(7)

式中， $Q=\left[\begin{array}{cc}{q}_{acc}& 0\\ 0& q_{gyro}\end{array}\right]$ 为陀螺仪和加速度计的过程噪声矩阵，P(k-1|k-1)为协方差在k-1时刻的后验估计，P(k|k-1)为协方差矩阵在k时刻的先验估计；

步骤2.3，利用卡尔曼滤波对惯性测量数据进行数据融合，卡尔曼增益系数为：

K(k)＝P(k|k-1)+H^T/(HP(k|k-1))H^T+R)(8)

式中，H＝[10]，R为测量协方差矩阵，得到角度和角度协方差的最优估计值X(k|k)和P(k|k)为：

$\{\begin{array}{c}X\left(k\right|k)=X\left(k\right|k-1)+K\left(k\right)\left(Z\right(k)-HX(k|k-1\left)\right)\\ P\left(k\right|k)=(I-K(k\left)H\right)P\left(k\right|k-1)\end{array}---\left(9\right)$

式中，Z(k)为当前陀螺仪的测量值；

步骤2.4，四旋翼飞行器惯性导航系统测量的Y轴和Z轴上惯性测量数据处理过程与步骤2.1～步骤2.3相同，综合四旋翼飞行器在X轴、Y轴和Z轴上的惯性测量数据可得到四旋翼飞行器的姿态角度数据。

利用卡尔曼滤波对惯性测量数据进行数据融合，使测量的惯性数据具有较高的精确性，为三维成像提供较准确的位置基础。

作为本发明的进一步限定方案，步骤3中，获得深度断层图像的具体步骤为：

步骤3.1，以角度θ为X轴，深度h为Y轴，将深度断层图像展开为二维平面上的曲线h＝f(θ)，对该曲线进行自适应中值滤波，消除曲线上的椒盐噪声；

步骤3.2，设四旋翼飞行器的位移为倾角为(β,α)，对深度断层图像进行补偿的具体步骤如下：

步骤3.2.1，四旋翼飞行器前进方向上的倾角为对深度断层图像进行补偿后展开对应的曲线方程为根据t_c-1和t_c时刻的倾角动态加权得到理想的深度断层图像：

$s_t=\sqrt{{{\stackrel{\‾}{x}}_t}^2+{{\stackrel{\‾}{y}}_t}^2+{{\stackrel{\‾}{z}}_t}^2}---\left(10\right)$

H₁＝h'_t-1+s_t/cos(β_t-β_t-1)cos(α_t-α_t-1)(11)

H₂＝h'_t+s_t/tan(β_t-β_t-1)tan(α_t-α_t-1)(12)

h”＝(1-cos(β_t-β_t-1)cos(α_t-α_t-1))H₁+cos(β_t-β_t-1)cos(α_t-α_t-1)H₂(13)

式中，h'_t-1和h'_t为经过补偿后t_c-1和t_c时刻的深度，h”为经过位移和倾角补偿后的深度值，(α_t-1,β_t-1)为t_c-1时刻四旋翼飞行器的其中两轴姿态角，(α_t,β_t)为t_c时刻四旋翼飞行器的其中两轴姿态角。

采用自适应中值滤波，能够消除曲线上的椒盐噪声，使深度断层图像展开在二维平面上曲线更加平滑，减小偶然误差；采用对深度断层图像进行补偿，能够得到较理想的深度断层图像。

作为本发明的进一步限定方案，步骤4中，在相邻深度断层图像之间使用滑动窗口牛顿插值法实现三维成像的具体步骤为：

步骤4.1，对t_i(i＝0,1,2,...,n)时刻获得的深度断层图像补偿后展开得到曲线函数为：

f_i(θ)(θ＝0°～360°)(14)

式中，对于角度θ＝θ_j，深度断层图像都有一个相对应的取值f_i(θ_j)；

步骤4.2，根据步骤1得到各个深度断层图像相对于初始位置的距离，对有序数据对(x_i,f_i(θ))进行滑动窗口牛顿插值得到光滑曲线，插值公式如下：

P_n(x)＝f₀(x₀)+g[x₀,x₁](x-x₁)+g[x₀,x₁,x₂](x-x₀)(x-x₁)+

...+g[x₀,x₁,..,x_n](x-x₀)...(x-x_n-1)(15)

$g\[x_0,x_1,\mathrm{..},x_k\]=\frac{g\[x_0,x_1,\mathrm{..},x_{k-2},x_k\]-g\[x_0,x_1,\mathrm{..},x_{k-1}\]}{x_k-x_0}---\left(16\right)$

式中，x_i表示各个深度断层图像相对于初始位置的位移数据，n为滑动窗口大小，当深度断层图像的数目超过n后，移出第一个深度断层图像，同时移入当前更新的深度断层图像，再进行牛顿插值，从而实现基于滑动窗口的牛顿插值；

步骤4.3，对所以θ值的深度断层图像进行步骤4.2同样操作，拼接差值后的所有深度断层图像实现三维成像。

采用滑动窗口的牛顿插值能够使三维成像更加平滑，增强了三维成像效果。

本发明的有益效果在于：(1)采用四旋翼飞行器作为搭载平台具有高度的灵活性，相对于地面移动机器人能够降低复杂地面对飞行的影响，具有较高的适应能力；(2)采用测量的位移数据和姿态角度数据确定深度断层图像数据的位置，从而有效提高了激光雷达的扫描范围，扩大了三维成像的面积范围。

附图说明

图1为本发明的方法流程图。

具体实施方式

如图1所示，本发明的基于室内四旋翼飞行器的激光雷达三维成像方法，包括如下步骤：

步骤1，读取四旋翼飞行器惯性导航系统测量的加速度数据以及测距系统测量的距离数据，再利用互补滤波法对加速度数据和距离数据进行数据融合，从而获得四旋翼飞行器的位移数据，具体步骤为：

步骤1.1，读取的加速度数据为(a_x(t),a_y(t),a_z(t))，进行二重积分得到t_c-1到t_c时刻的近似位移(x_t,y_t,z_t)为：

$\left[\begin{array}{c}{x}_t\\ y_t\\ z_t\end{array}\right]=\left[\begin{array}{c}{\∫}_{t_{c-1}}^{t_c}\left(v_x\right(t_{c-1})+{\∫}_{t_{c-1}}^{t_c}{a}_x\left(t\right))dt\\ {\∫}_{t_{c-1}}^{t_c}\left(v_y\right(t_{c-1})+{\∫}_{t_{c-1}}^{t_c}{a}_y\left(t\right))dt\\ {\∫}_{t_{c-1}}^{t_c}\left(v_z\right(t_{c-1})+{\∫}_{t_{c-1}}^{t_c}{a}_z\left(t\right))dt\end{array}\right]---\left(1\right)$

式中，(v_x(t_c-1),v_y(t_c-1),v_z(t_c-1))为t_c-1时刻的速度；

步骤1.2，读取到t_c-1和t_c时刻的距离数据为和计算t_c-1到t_c时刻的近似位移(mx_t,my_t,mz_t)为：

$\left[\begin{array}{c}m{x}_t\\ {\mathrm{my}}_t\\ {\mathrm{mz}}_t\end{array}\right]=\left[\begin{array}{c}{x}_{m_t}-x_{m_{t-1}}\\ y_{m_t}-y_{m_{t-1}}\\ z_{m_t}-z_{m_{t-1}}\end{array}\right]---\left(2\right)$

步骤1.3，利用互补滤波法进行数据融合获得四旋翼飞行器的位移数据为：

$\left[\begin{array}{c}{\stackrel{\‾}{x}}_t\\ {\stackrel{\‾}{y}}_t\\ {\stackrel{\‾}{z}}_t\end{array}\right]=\left[\begin{array}{c}{a}_x({\stackrel{\‾}{x}}_{t-1}+x_t*T_u)+(1-a_x)*{\mathrm{mx}}_t\\ a_y({\stackrel{\‾}{y}}_{t-1}+y_t*T_u)+(1-a_y)*{\mathrm{my}}_t\\ a_z({\stackrel{\‾}{z}}_{t-1}+z_t*T_u)+(1-a_z)*{\mathrm{mz}}_t\end{array}\right]---\left(3\right)$

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\τ}}_x/({\mathrm{\τ}}_x+T_u)\\ {\mathrm{\τ}}_y/({\mathrm{\τ}}_y+T_u)\\ {\mathrm{\τ}}_z/({\mathrm{\τ}}_z+T_u)\end{array}\right]---\left(4\right)$

式中，为t_c-1时刻进行数据融合后的位移，T_u为超声波传感器采样周期，(τ_x,τ_y,τ_z)为置信程度，(a_x,a_y,a_z)为动态加权系数。

步骤2，读取四旋翼飞行器惯性导航系统测量的惯性测量数据，并利用卡尔曼滤波对惯性测量数据进行数据融合，获得四旋翼飞行器的姿态角度数据，具体步骤为：

步骤2.1，四旋翼飞行器惯性导航系统测量的X轴上惯性测量数据包括倾角陀螺仪测量角速度ω_gyro、陀螺仪测量噪声ω_g、螺仪常值误差b、加速度计测量角度以及角速度计测量噪声ω_a，四旋翼飞行器惯性导航系统的状态方程和输出方程为：

步骤2.2，设四旋翼飞行器惯性导航系统的采样周期为Ts，则上述方程离散化后为：

X(k|k-1)＝AX(k-1|k-1)+BU(k)(6)

式中， $A=\left[\begin{array}{cc}1& -Ts\\ 0& 1\end{array}\right],B=\left[\begin{array}{c}Ts\\ 0\end{array}\right],$ X(k-1|k-1)为k-1时刻的后验估计，X(k|k-1)为k时刻的先验估计，X(k|k-1)的协方差为：

P(k|k-1)＝AP(k-1|k-1)A^T+Q(7)

式中， $Q=\left[\begin{array}{cc}{q}_{acc}& 0\\ 0& q_{gyro}\end{array}\right]$ 为陀螺仪和加速度计的过程噪声矩阵，P(k-1|k-1)为协方差在k-1时刻的后验估计，P(k|k-1)为协方差矩阵在k时刻的先验估计；

步骤2.3，利用卡尔曼滤波对惯性测量数据进行数据融合，卡尔曼增益系数为：

K(k)＝P(k|k-1)+H^T/(HP(k|k-1))H^T+R)(8)

式中，H＝[10]，R为测量协方差矩阵，得到角度和角度协方差的最优估计值X(k|k)和P(k|k)为：

$\{\begin{array}{c}X\left(k\right|k)=X\left(k\right|k-1)+K\left(k\right)\left(Z\right(k)-HX(k|k-1\left)\right)\\ P\left(k\right|k)=(I-K(k\left)H\right)P\left(k\right|k-1)\end{array}---\left(9\right)$

式中，Z(k)为当前陀螺仪的测量值；

步骤2.4，四旋翼飞行器惯性导航系统测量的Y轴和Z轴上惯性测量数据处理过程与步骤2.1～步骤2.3相同，综合四旋翼飞行器在X轴、Y轴和Z轴上的惯性测量数据可得到四旋翼飞行器的姿态角度数据；

步骤3，读取四旋翼飞行器前进方向上激光雷达采集的深度断层图像数据，再对深度断层图像数据进行自适应中值滤波，并进行基于位移和角度的补偿，获得深度断层图像，具体步骤为：

步骤3.1，以角度θ为X轴，深度h为Y轴，将深度断层图像展开为二维平面上的曲线h＝f(θ)，对该曲线进行自适应中值滤波，消除曲线上的椒盐噪声，自适应中值滤波算法为：

Step1:A₁＝h_med-h_min,A₂＝h_med-h_max

如果A₁＞0&&A₂＜0，转Step2；

否则增大窗口尺寸，如果窗口尺寸≤S_max，转Step1，否则输出h；

Step2:B₁＝h-h_min,B₂＝h-h_max

如果B₁＞0&&B₂＜0，输出h；

否则输出h_med；

其中，S_max为滑动窗口中值滤波的最大窗口尺寸，h为当前窗口S_h中间位置处的深度值，h_min,h_med,h_max分别为当前窗口S_h中深度取值的最小值、中间值和最大值；

步骤3.2，设四旋翼飞行器的位移为倾角为(β,α)，对深度断层图像进行补偿的具体步骤如下：

步骤3.2.1，四旋翼飞行器前进方向上的倾角为对深度断层图像进行补偿后展开对应的曲线方程为根据t_c-1和t_c时刻的倾角动态加权得到理想的深度断层图像：

$s_t=\sqrt{{{\stackrel{\‾}{x}}_t}^2+{{\stackrel{\‾}{y}}_t}^2+{{\stackrel{\‾}{z}}_t}^2}---\left(10\right)$

H₁＝h'_t-1+s_t/cos(β_t-β_t-1)cos(α_t-α_t-1)(11)

H₂＝h'_t+s_t/tan(β_t-β_t-1)tan(α_t-α_t-1)(12)

h”＝(1-cos(β_t-β_t-1)cos(α_t-α_t-1))H₁+cos(β_t-β_t-1)cos(α_t-α_t-1)H₂(13)

式中，h'_t-1和h'_t为经过补偿后t_c-1和t_c时刻的深度，h”为经过位移和倾角补偿后的深度值，(α_t-1,β_t-1)为t_c-1时刻四旋翼飞行器的其中两轴姿态角，(α_t,β_t)为t_c时刻四旋翼飞行器的其中两轴姿态角,由于前后两次测量点的角度偏移比较小，使用的近似计算关系为：

$\left\{\begin{array}{c}cos({\mathrm{\α}}_t-{\mathrm{\α}}_{t-1})\≈cos({\mathrm{\β}}_t-{\mathrm{\β}}_{t-1})\≈1\\ tan({\mathrm{\β}}_t-{\mathrm{\β}}_{t-1})\≈{\mathrm{\β}}_t-{\mathrm{\β}}_{t-1}\\ tan({\mathrm{\α}}_t-{\mathrm{\α}}_{t-1})\≈{\mathrm{\α}}_t-{\mathrm{\α}}_{t-1}\end{array}\right.$

采用上式的近似计算关系能够有效减少计算量，降低方法的执行复杂度，使三维成像更加迅速。

步骤4，根据步骤1的位移数据和步骤2的姿态角度数据确定四旋翼飞行器运动轨迹，并对运动轨迹上的相邻数据采集点的深度断层图像使用滑动窗口牛顿插值法，从而实现三维成像，具体步骤为：

步骤4.1，对t_i(i＝0,1,2,...,n)时刻获得的深度断层图像补偿后展开得到曲线函数为：

f_i(θ)(θ＝0°～360°)(14)

式中，对于角度θ＝θ_j，深度断层图像都有一个相对应的取值f_i(θ_j)；

步骤4.2，根据步骤1得到各个深度断层图像相对于初始位置的距离，对有序数据对(x_i,f_i(θ))进行滑动窗口牛顿插值得到光滑曲线，插值公式如下：

P_n(x)＝f₀(x₀)+g[x₀,x₁](x-x₁)+g[x₀,x₁,x₂](x-x₀)(x-x₁)+

...+g[x₀,x₁,..,x_n](x-x₀)...(x-x_n-1)(15)

$g\[x_0,x_1,\mathrm{..},x_k\]=\frac{g\[x_0,x_1,\mathrm{..},x_{k-2},x_k\]-g\[x_0,x_1,\mathrm{..},x_{k-1}\]}{x_k-x_0}---\left(16\right)$

式中，x_i表示各个深度断层图像相对于初始位置的位移数据，n为滑动窗口大小，当深度断层图像的数目超过n后，移出第一个深度断层图像，同时移入当前更新的深度断层图像，再进行牛顿插值，从而实现基于滑动窗口的牛顿插值；

步骤4.3，对所以θ值的深度断层图像进行步骤4.2同样操作，拼接差值后的所有深度断层图像实现三维成像。

Claims (5)

1.一种基于室内四旋翼飞行器的激光雷达三维成像方法，其特征在于，包括如下步骤：

步骤1，读取四旋翼飞行器惯性导航系统测量的加速度数据以及测距系统测量的距离数据，再利用互补滤波法对加速度数据和距离数据进行数据融合，从而获得四旋翼飞行器的位移数据；

步骤2，读取四旋翼飞行器惯性导航系统测量的惯性测量数据，并利用卡尔曼滤波对惯性测量数据进行数据融合，获得四旋翼飞行器的姿态角度数据；

步骤3，读取四旋翼飞行器前进方向上激光雷达采集的深度断层图像数据，再对深度断层图像数据进行自适应中值滤波，并进行基于位移和角度的补偿，获得深度断层图像；

步骤4，根据步骤1的位移数据和步骤2的姿态角度数据确定四旋翼飞行器运动轨迹，并对运动轨迹上的相邻数据采集点的深度断层图像使用滑动窗口牛顿插值法，从而实现三维成像。

2.根据权利要求1所述的基于室内四旋翼飞行器的激光雷达三维成像方法，其特征在于，步骤1中，获得四旋翼飞行器位移数据的具体步骤为：

步骤1.1，读取的加速度数据为(a_x(t),a_y(t),a_z(t))，进行二重积分得到t_c-1到t_c时刻的近似位移(x_t,y_t,z_t)为：

$\left[\begin{array}{c}{x}_t\\ y_t\\ z_t\end{array}\right]=\left[\begin{array}{c}{\∫}_{t_{c-1}}^{t_c}\left(v_x\left(t_{c-1}\right)+{\∫}_{t_{c-1}}^{t_c}{a}_x\left(t\right)\right)dt\\ {\∫}_{t_{c-1}}^{t_c}\left(v_y\left(t_{c-1}\right)+{\∫}_{t_{c-1}}^{t_c}{a}_y\left(t\right)\right)dt\\ {\∫}_{t_{c-1}}^{t_c}\left(v_z\left(t_{c-1}\right)+{\∫}_{t_{c-1}}^{t_c}{a}_z\left(t\right)\right)dt\end{array}\right]---\left(1\right)$

式中，(v_x(t_c-1),v_y(t_c-1),v_z(t_c-1))为t_c-1时刻的速度；

步骤1.2，读取到t_c-1和t_c时刻的距离数据为和，计算t_c-1到t_c时刻的近似位移(mx_t,my_t,mz_t)为：

$\left[\begin{array}{c}m{x}_t\\ {\mathrm{my}}_t\\ {\mathrm{mz}}_t\end{array}\right]=\left[\begin{array}{c}{x}_{m_t}-x_{m_{t-1}}\\ y_{m_t}-y_{m_{t-1}}\\ z_{m_t}-z_{m_{t-1}}\end{array}\right]---\left(2\right)$

步骤1.3，利用互补滤波法进行数据融合获得四旋翼飞行器t_c时刻位移数据为：

$\left[\begin{array}{c}{\stackrel{\‾}{x}}_t\\ {\stackrel{\‾}{y}}_t\\ {\stackrel{\‾}{z}}_t\end{array}\right]=\left[\begin{array}{c}{a}_x({\stackrel{\‾}{x}}_{t-1}+x_t*T_u)+(1-a_x)*m{x}_t\\ a_y({\stackrel{\‾}{y}}_{t-1}+y_t*T_u)+(1-a_y)*{\mathrm{my}}_t\\ a_z({\stackrel{\‾}{z}}_{t-1}+z_t*T_u)+(1-a_z)*{\mathrm{mz}}_t\end{array}\right]---\left(3\right)$

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\τ}}_x/({\mathrm{\τ}}_x+T_u)\\ {\mathrm{\τ}}_y/({\mathrm{\τ}}_y+T_u)\\ {\mathrm{\τ}}_z/({\mathrm{\τ}}_z+T_u)\end{array}\right]---\left(4\right)$

式中，为t_c-1时刻进行数据融合后的位移，T_u为超声波传感器采样周期，(τ_x,τ_y,τ_z)为置信程度，(a_x,a_y,a_z)为动态加权系数。

3.根据权利要求1或2所述的基于室内四旋翼飞行器的激光雷达三维成像方法，其特征在于，步骤2中，获得四旋翼飞行器的姿态角度数据的具体步骤为：

步骤2.1，四旋翼飞行器惯性导航系统测量的X轴上惯性测量数据包括倾角陀螺仪测量角速度ω_gyro、陀螺仪测量噪声ω_g、螺仪常值误差b、加速度计测量角度以及角速度计测量噪声ω_a，四旋翼飞行器惯性导航系统的状态方程和输出方程为：

步骤2.2，设四旋翼飞行器惯性导航系统的采样周期为Ts，则上述方程离散化后为：

X(k|k-1)＝AX(k-1|k-1)+BU(k)(6)

式中， $A=\left[\begin{array}{cc}1& -\mathrm{Ts}\\ 0& 1\end{array}\right],B=\left[\begin{array}{c}\mathrm{Ts}\\ 0\end{array}\right],$ X(k-1|k-1)为k-1时刻的后验估计，X(k|k-1)为k时刻的先验估计，X(k|k-1)的协方差为：

P(k|k-1)＝AP(k-1|k-1)A^T+Q(7)

式中， $Q=\left[\begin{array}{cc}{q}_{\mathrm{acc}}& 0\\ 0& q_{\mathrm{gyro}}\end{array}\right]$ 为陀螺仪和加速度计的过程噪声矩阵，P(k-1|k-1)为协方差在k-1时刻的后验估计，P(k|k-1)为协方差矩阵在k时刻的先验估计；

步骤2.3，利用卡尔曼滤波对惯性测量数据进行数据融合，卡尔曼增益系数为：

K(k)＝P(k|k-1)+H^T/(HP(k|k-1))H^T+R)(8)

式中，H＝[10]，R为测量协方差矩阵，得到角度和角度协方差的最优估计值X(k|k)和P(k|k)为：

$\left\{\begin{array}{c}X\left(k|k\right)=X\left(k|k-1\right)+K\left(k\right)\left(Z\left(k\right)-HX\left(k|k-1\right)\right)\\ P\left(k|k\right)=\left(I-K\left(k\right)H\right)P\left(k|k-1\right)\end{array}\right.---\left(9\right)$

式中，Z(k)为当前陀螺仪的测量值；

步骤2.4，四旋翼飞行器惯性导航系统测量的Y轴和Z轴上惯性测量数据处理过程与步骤2.1～步骤2.3相同，综合四旋翼飞行器在X轴、Y轴和Z轴上的惯性测量数据可得到四旋翼飞行器的姿态角度数据。

4.根据权利要求1所述的基于室内四旋翼飞行器的激光雷达三维成像方法，其特征在于，步骤3中，获得深度断层图像的具体步骤为：

步骤3.1，以角度θ为X轴，深度h为Y轴，将深度断层图像展开为二维平面上的曲线h＝f(θ)，对该曲线进行自适应中值滤波，消除曲线上的椒盐噪声；

步骤3.2，设四旋翼飞行器的位移为倾角为对深度断层图像进行补偿的具体步骤如下：

步骤3.2.1，四旋翼飞行器前进方向上的倾角为对深度断层图像进行补偿后展开对应的曲线方程为根据t_c-1和t_c时刻的倾角动态加权得到理想的深度断层图像：

$s_t=\sqrt{{{\stackrel{\‾}{x}}_t}^2+{{\stackrel{\‾}{y}}_t}^2+{{\stackrel{\‾}{z}}_t}^2}---\left(10\right)$

H₁＝h'_t-1+s_t/cos(β_t-β_t-1)cos(α_t-α_t-1)(11)

H₂＝h'_t+s_t/tan(β_t-β_t-1)tan(α_t-α_t-1)(12)

h”＝(1-cos(β_t-β_t-1)cos(α_t-α_t-1))H₁+cos(β_t-β_t-1)cos(α_t-α_t-1)H₂(13)

式中，h'_t-1和h'_t为经过补偿后t_c-1和t_c时刻的深度，h”为经过位移和倾角补偿后的深度值，(α_t-1,β_t-1)为t_c-1时刻四旋翼飞行器的其中两轴姿态角，(α_t,β_t)为t_c时刻四旋翼飞行器的其中两轴姿态角。

5.根据权利要求1所述的基于室内四旋翼飞行器的激光雷达三维成像方法，其特征在于，步骤4中，在相邻深度断层图像之间使用滑动窗口牛顿插值法实现三维成像的具体步骤为：

步骤4.1，对t_i(i＝0,1,2,...,n)时刻获得的深度断层图像补偿后展开得到曲线函数为：

f_i(θ)(θ＝0°～360°)(14)

式中，对于角度θ＝θ_j，深度断层图像都有一个相对应的取值f_i(θ_j)；

步骤4.2，根据步骤1得到各个深度断层图像相对于初始位置的位移数据，对有序数据对(x_i,f_i(θ))进行滑动窗口牛顿插值得到光滑曲线，插值公式如下：

P_n(x)＝f₀(x₀)+g[x₀,x₁](x-x₁)+g[x₀,x₁,x₂](x-x₀)(x-x₁)+

...+g[x₀,x₁,..,x_n](x-x₀)...(x-x_n-1)(15)

$g\[x_0,x_1,\mathrm{..},x_k\]=\frac{g\[x_0,x_1,\mathrm{..},x_{k-2},x_k\]-g\[x_0,x_1,\mathrm{..},x_{k-1}\]}{x_k-x_0}---\left(16\right)$

式中，x_i表示各个深度断层图像相对于初始位置的位移数据，n为滑动窗口大小，当深度断层图像的数目超过n后，移出第一个深度断层图像，同时移入当前更新的深度断层图像，再进行牛顿插值，从而实现基于滑动窗口的牛顿插值；

步骤4.3，对所以θ值的深度断层图像进行步骤4.2同样操作，拼接差值后的所有深度断层图像实现三维成像。