一种基于室内四旋翼飞行器的激光雷达三维成像方法
【技术领域】
[0001] 本发明涉及一种三维成像方法，尤其是一种基于室内四旋翼飞行器的三维成像方 法。
【背景技术】
[0002] 随着现代航空航天事业的飞速发展，自主飞行器也在现代高新技术条件下快速发 展。飞行器能在枯燥、危险、恶劣环境中发挥重要作用。在军事领域中能够实施低空侦察， 情报采集等；在民用领域中可以完成资源勘探、边境巡逻、灾情监视等。而对于坍塌楼房，矿 井等地面移动机器人无法胜任的环境下，灵活的自主飞行器对于探测未知环境是很好的选 择。
[0003] 对于三维成像方法，基于图像处理获取场景信息，通过图像拼接实现三维成像，该 方法算法复杂，受光照影响，可靠性低；基于激光的三维成像方法主要应用于静态对象的三 维建模，激光器的摆放位置严格，设备便捷度低；基于地面移动机器人的三维成像方法受环 境障碍物影响大，无法全方位实现三维成像。
【发明内容】
[0004] 本发明要解决的技术问题是现有的三维成像方法使用复杂，且常常由于环境障碍 物影响无法全方位实现三维成像。
[0005] 为了解决上述技术问题，本发明提供了一种基于室内四旋翼飞行器的激光雷达三 维成像方法，包括如下步骤：
[0006] 步骤1，读取四旋翼飞行器惯性导航系统测量的加速度数据以及测距系统测量的 距离数据，再利用互补滤波法对加速度数据和距离数据进行数据融合，从而获得四旋翼飞 行器的位移数据；
[0007] 步骤2,读取四旋翼飞行器惯性导航系统测量的惯性测量数据，并利用卡尔曼滤波 对惯性测量数据进行数据融合，获得四旋翼飞行器的姿态角度数据；
[0008] 步骤3,读取四旋翼飞行器前进方向上激光雷达采集的深度断层图像数据，再对 深度断层图像数据进行自适应中值滤波，并进行基于位移和角度的补偿，获得深度断层图 像；
[0009] 步骤4,根据步骤1的位移数据和步骤2的姿态角度数据确定四旋翼飞行器运动轨 迹，并对运动轨迹上的相邻数据采集点的深度断层图像使用滑动窗口牛顿插值法，从而实 现三维成像。
[0010] 采用四旋翼飞行器作为搭载平台具有高度的灵活性，相对于地面移动机器人能够 降低复杂地面对飞行的影响，具有较高的适应能力；采用测量的位移数据和姿态角度数据 确定深度断层图像数据的位置，从而有效提高了激光雷达的扫描范围，扩大了三维成像的 面积范围。
[0011] 作为本发明的进一步限定方案，步骤1中，获得四旋翼飞行器位移数据的具体步 骤为：
[0012] 步骤1. 1，读取的加速度数据为（ax(t)，ay(t)，az(t))，进行二重积分得到t。濟tc 时刻的近似位移（xt，yt，zt)为：
[0013]
(1)
[0014] 瓦甲，CvxUcJ，vyUcAvzUcW艿tc !时刻的速度；
[0015] 步骤1· 2,读取到t。^t。时刻的距离数据为，為 tc丨至丨」t。时刻的近似位移（mxt,myt,mzt)为：
[0016]
^ .....-.」 (2)
[0017] 步骤1. 3,利用互补滤波法进行数据融合获得四旋翼飞行器的位移数据 为：
[0018]
[0019]
[0020] 式中，Tu为超声波传感器采样周期，（τχ，τy，τz)为置信程度，（ax，ay，az)为动态 加权系数。
[0021] 由于加速度积分得到的位移累积误差大，而测距系统得到的距离数据准确度高， 但是存在大量噪声，所以采用互补滤波法进行数据融合，能够得到置信度更高的四旋翼飞 行器位移。
[0022] 作为本发明的进一步限定方案，步骤2中，获得四旋翼飞行器的姿态角度数据的 具体步骤为：
[0023] 步骤2. 1，四旋翼飞行器惯性导航系统测量的X轴上惯性测量数据包括倾角资、陀 螺仪测量角速度《gyra、陀螺仪测量噪声cog、螺仪常值误差b、加速度计测量角度以及角 速度计测量噪声，四旋翼飞行器惯性导航系统的状态方程和输出方程为：
[0024] (5)
[0025] 步骤2. 2,设四旋翼飞行器惯性导航系统的采样周期为Ts，则上述方程离散化后 为：
[0026] X(k|k-1) =AX(k-l|k-l)+BU(k) (6)
[0027] 式中：
X(k-l|k-l)为k-1时刻的后验估计，X(k|k-1) 为k时刻的先验估计，X(k|k_l)的协方差为：
[0028] P(k|k-1) =AP(k-l|k-l)AT+Q(7)
[0029] 式中
b陀螺仪和加速度计的过程噪声矩阵，P(k-11k-1)为协方 差在k_l时刻的后验估计，P(k|k_l)为协方差矩阵在k时刻的先验估计；
[0030] 步骤2. 3,利用卡尔曼滤波对惯性测量数据进行数据融合，卡尔曼增益系数为：
[0031] K(k) =P(kIk-1) +HT/(HP(k|k-1))HT+R) (8)
[0032] 式中，H= [1 0]，R为测量协方差矩阵，得到角度和角度协方差的最优估计值 X(k|k)和P(k|k)为：
[0033]
[0034] 式中，Z(k)为当前陀螺仪的测量值；
[0035] 步骤2. 4,四旋翼飞行器惯性导航系统测量的Y轴和Z轴上惯性测量数据处理过程 与步骤2. 1~步骤2. 3相同，综合四旋翼飞行器在X轴、Y轴和Z轴上的惯性测量数据可得 到四旋翼飞行器的姿态角度数据。
[0036] 利用卡尔曼滤波对惯性测量数据进行数据融合，使测量的惯性数据具有较高的精 确性，为三维成像提供较准确的位置基础。
[0037] 作为本发明的进一步限定方案，步骤3中，获得深度断层图像的具体步骤为：
[0038] 步骤3. 1，以角度Θ为X轴，深度h为Y轴，将深度断层图像展开为二维平面上的 曲线h=f(Θ)，对该曲线进行自适应中值滤波，消除曲线上的椒盐噪声；
[0039] 步骤3.2,设四旋翼飞行器的位移为倾角为（終.β，α)，对深度断层图 ? 像进行补偿的具体步骤如下：
[0040] 步骤3. 2. 1，四旋翼飞行器前进方向上的倾角为对深度断层图像进行补偿后展 开对应的曲线方程为& =Λ?-的，根据t。JPt。时刻的倾角动态加权得到理想的深度断层 图像：
[0041]
[0042] Hj=h't !+st/cos(βt-βtj)cos(at-atj) (11)
[0043] H2=h't+st/tan(βt-βtj)tan(at-atj) (12)
[0044] h" =(1-cos(β t_ β t J cos(at-at丄））Hi+cos(β t_ βtJ cos (at-at丄）H2 (13)
[0045] 式中，h'tJPh't为经过"=/(??)补偿后t。JPt。时刻的深度，h"为经过位移和 倾角补偿后的深度值，（atl，0tl)为时刻四旋翼飞行器的其中两轴姿态角，（at，0t) 为t。时刻四旋翼飞行器的其中两轴姿态角。
[0046] 采用自适应中值滤波，能够消除曲线上的椒盐噪声，使深度断层图像展开在二维 平面上曲线更加平滑，减小偶然误差；采用对深度断层图像进行补偿，能够得到较理想的深 度断层图像。
[0047] 作为本发明的进一步限定方案，步骤4中，在相邻深度断层图像之间使用滑动窗 口牛顿插值法实现三维成像的具体步骤为：
[0048] 步骤4. 1，对&(i= 0, 1，2, ...，n)时刻获得的深度断层图像补偿后展开得到曲线 函数为：
[0049] θ) (Θ=〇。~360。） (14)
[0050] 式中，对于角度θ=Θj，深度断层图像都有一个相对应的取值仁（0J;
[0051] 步骤4. 2,根据步骤1得到各个深度断层图像相对于初始位置的距离，对有序数据 对（Xl，仁（Θ))进行滑动窗口牛顿插值得到光滑曲线，插值公式如下：
[0052] Pn(x) =f0(x〇)+g[x0,xj(x-x^+gtxo,X1;X2] (x-X〇) (x-Xi) +
[0053] ...+g[x0,Xi,..,xn] (x-x〇)...(x-xni) (15)
[0054] i1I
[0055] 式中，Xl表示各个深度断层图像相对于初始位置的位移数据，n为滑动窗口大小， 当深度断层图像的数目超过η后，移出第一个深度断层图像，同时移入当前更新的深度断 层图像，再进行牛顿插值，从而实现基于滑动窗口的牛顿插值；
[0056] 步骤4. 3,对所以Θ值的深度断层图像进行步骤4. 2同样操作，拼接差值后的所有 深度断层图像实现三维成像。
[0057] 采用滑动窗口的牛顿插值能够使三维成像更加平滑，增强了三维成像效果。
[0058] 本发明的有益效果在于：（1)采用四旋翼飞行器作为搭载平台具有高度的灵活 性，相对于地面移动机器人能够降低复杂地面对飞行的影响，具有较高的适应能力；（2)采 用测量的位移数据和姿态角度数据确定深度断层图像数据的位置，从而有效提高了激光雷 达的扫描范围，扩大了三维成像的面积范围。
【附图说明】
[0059] 图1为本发明的方法流程图。
【具体实施方式】
[0060] 如图1所示，本发明的基于室内四旋翼飞行器的激光雷达三维成像方法，包括如 下步骤：
[0061] 步骤1，读取四旋翼飞行器惯性导航系统测量的加速度数据以及测距系统测量的 距离数据，再利用互补滤波法对加速度数据和距离数据进行数据融合，从而获得四旋翼飞 行器的位移数据，具体步骤为：
[0062] 步骤1. 1，读取的加速度数据为（ax (t)，ay (t)，az⑴），进行二重积分得到t。濟t。 时刻的近似位移（Xt，yt，Zt)为：
[0063](? L *
-n j
[0064] 式中，（vjt。vjt。J)为t。i时刻的速度；
[0065] 步骤1· 2,读取到t。廊t。时刻的距离数据为和CVJVZ,?,)，计算 tci至ljt。时刻的近似位移（mxt,myt,mzt)为：
[0066] (2)
[0067] 步骤1. 3,利用互补滤波法进行数据融合获得四旋翼飞行器的位移数据 为：
[0068；
[0069；
[0070] 式中，（?,.ρΜ)为t。跗刻进行数据融合后的位移，Tu为超声波传感器采样周 期，（τχ,Ty,τζ)为置信程度，（ax，ay，az)为动态加权系数。
[0071] 步骤2,读取四旋翼飞行器惯性导航系统测量的惯性测量数据，并利用卡尔曼滤波 对惯性测量数据进行数据融合，获得四旋翼飞行器的姿态角度数据，具体步骤为：
[0072] 步骤2. 1，四旋翼飞行器惯性导航系统测量的X轴上惯性测量数据包括倾角％J它 螺仪测量角速度《gyra、陀螺仪测量噪声cog、螺仪常值误差b、加速度计测量角度以及角 速度计测量噪声，四旋翼飞行器惯性导航系统的状态方程和输出方程为：
[0073]
[0074] 步骤2. 2,设四旋翼飞行器惯性导航系统的采样周期为Ts，则上述方程离散化后 为：
[0075] X(k|k-1) =AX(k-l|k-l)+BU(k) (6)
[0076] 式中，
X(k-l|k-l)为k-1时刻的后验估计，X(k|k-1)为 k时刻的先验估计，X(k|k_l)的协方差