为：
[0077] P(k|k-1) =AP(k-l|k-l)AT+Q(7)
[0078] 式中
~陀螺仪和加速度计的过程噪声矩阵，P(k-1 |k-l)为协方 差在k_l时刻的后验估计，P(k|k_l)为协方差矩阵在k时刻的先验估计；
[0079] 步骤2. 3,利用卡尔曼滤波对惯性测量数据进行数据融合，卡尔曼增益系数为：
[0080] K(k) =P(kIk-1) +HT/(HP(k|k-1))HT+R) (8)
[0081] 式中，H= [1 0]，R为测量协方差矩阵，得到角度和角度协方差的最优估计值 x(k|k)和p(k|k)为： iX(k \ k)-X(k +Kik)(Z(k) - NX HiI /c -1))
[0082] i (9)
[ P{k\k) = (J-K(k)ii)P(k\k-\)
[0083] 式中，Z(k)为当前陀螺仪的测量值；
[0084] 步骤2. 4,四旋翼飞行器惯性导航系统测量的Y轴和Z轴上惯性测量数据处理过程 与步骤2. 1~步骤2. 3相同，综合四旋翼飞行器在X轴、Y轴和Z轴上的惯性测量数据可得 到四旋翼飞行器的姿态角度数据；
[0085] 步骤3,读取四旋翼飞行器前进方向上激光雷达采集的深度断层图像数据，再对深 度断层图像数据进行自适应中值滤波，并进行基于位移和角度的补偿，获得深度断层图像， 具体步骤为：
[0086] 步骤3. 1，以角度Θ为X轴，深度h为Y轴，将深度断层图像展开为二维平面上的 曲线h=f(Θ)，对该曲线进行自适应中值滤波，消除曲线上的椒盐噪声，自适应中值滤波 算法为：
[0087] Stepl:Ai=hned-hnin,A2=hned-hnax
[0088] 如果A!> 0&&A2< 0,转St印2 ;
[0089] 否则增大窗口尺寸，如果窗口尺寸<S_，转Stepl，否则输出h;
[0090] St印2:B! =h-hnin，B2 =h-h_
[0091] 如果队> 0&&B2< 0,输出h;
[0092] 否则输出hMd;
[0093] 其中，S_为滑动窗口中值滤波的最大窗口尺寸，h为当前窗口Sh*间位置处的深 度值，h_，h^，h_分别为当前窗口Sh中深度取值的最小值、中间值和最大值；
[0094]步骤3. 2,设四旋翼飞行器的位移为(HI)倾角为（弘β，α)，对深度断层图 像进行补偿的具体步骤如下：
[0095]步骤3. 2. 1，四旋翼飞行器前进方向上的倾角为f，对深度断层图像进行补偿后展 开对应的曲线方程为& 的，根据t。JPt。时刻的倾角动态加权得到理想的深度断层 图像：
[0096] (10)
[0097] Hi = h' t !+st/cos ( β t- β t i) cos ( a t- a t j) (11)
[0098] H2= h' t+st/tan ( β t- β t j) tan ( a t- a t j) (12)
[0099]h" = (1-cos(βt_βtJcos(at-at 丄））Hi+cos(βt_βtJcos(at-at 丄）H2 (13)
[0100] 式中，h'tJPh't为经过"=/'(r/i)补偿后t。JPt。时刻的深度，h"为经过位移和 倾角补偿后的深度值，（atl，0tl)为时刻四旋翼飞行器的其中两轴姿态角，（at，0t) 为t。时刻四旋翼飞行器的其中两轴姿态角，由于前后两次测量点的角度偏移比较小，使用 的近似计算关系为：
[0101]
[0102] 采用上式的近似计算关系能够有效减少计算量，降低方法的执行复杂度，使三维 成像更加迅速。
[0103] 步骤4,根据步骤1的位移数据和步骤2的姿态角度数据确定四旋翼飞行器运动轨 迹，并对运动轨迹上的相邻数据采集点的深度断层图像使用滑动窗口牛顿插值法，从而实 现三维成像，具体步骤为：
[0104] 步骤4. 1，对&(i= 0, 1，2, ...，η)时刻获得的深度断层图像补偿后展开得到曲线 函数为：
[0105] θ ) ( Θ =〇。~360。） (14)
[0106] 式中，对于角度θ = Θ j，深度断层图像都有一个相对应的取值匕（0 J ;
[0107] 步骤4. 2,根据步骤1得到各个深度断层图像相对于初始位置的距离，对有序数据 对（Xl，仁（Θ))进行滑动窗口牛顿插值得到光滑曲线，插值公式如下：
[0108] Pn(x) = f0(x〇)+g[x0, xj (x-x^+gtxo, x1; x2] (x-x〇) (x-Xi) +
[0109] ... +g[x0, Xi,.., xn] (x-x〇)... (x-xn i) (15)
[0110]
[0111] 式中，Xl表示各个深度断层图像相对于初始位置的位移数据，n为滑动窗口大小， 当深度断层图像的数目超过η后，移出第一个深度断层图像，同时移入当前更新的深度断 层图像，再进行牛顿插值，从而实现基于滑动窗口的牛顿插值；
[0112] 步骤4. 3,对所以Θ值的深度断层图像进行步骤4. 2同样操作，拼接差值后的所有 深度断层图像实现三维成像。
【主权项】
1. 一种基于室内四旋翼飞行器的激光雷达三维成像方法，其特征在于，包括如下步 骤： 步骤1，读取四旋翼飞行器惯性导航系统测量的加速度数据以及测距系统测量的距离 数据，再利用互补滤波法对加速度数据和距离数据进行数据融合，从而获得四旋翼飞行器 的位移数据； 步骤2,读取四旋翼飞行器惯性导航系统测量的惯性测量数据，并利用卡尔曼滤波对惯 性测量数据进行数据融合，获得四旋翼飞行器的姿态角度数据； 步骤3,读取四旋翼飞行器前进方向上激光雷达采集的深度断层图像数据，再对深度断 层图像数据进行自适应中值滤波，并进行基于位移和角度的补偿，获得深度断层图像； 步骤4,根据步骤1的位移数据和步骤2的姿态角度数据确定四旋翼飞行器运动轨迹， 并对运动轨迹上的相邻数据采集点的深度断层图像使用滑动窗口牛顿插值法，从而实现三 维成像。2. 根据权利要求1所述的基于室内四旋翼飞行器的激光雷达三维成像方法，其特征在 于，步骤1中，获得四旋翼飞行器位移数据的具体步骤为： 步骤1. 1，读取的加速度数据为（ax (t)，ay (t)，az⑴），进行二重积分得到t。濟t。时刻 的近似位移（xt，yt，zt)为：式中，（νχ(?<； 1)，VyUc 1)，νζ(?<； 1))为 tc 1 时刻的速度； 步骤1. 2,读取到t。挪t。时刻的距离数据为(心,_1，\1，\1)_和(\，)％，\)，计算 1:。1到 tc时刻的近似位移（mxt，myt，mzt)为：步骤1. 3,利用互补滤波法进行数据融合获得四旋翼飞行器t。时刻位移数据 为：式中，（?μ,^μ)为t。跗刻进行数据融合后的位移，Tu为超声波传感器采样周期， (τχ, Ty, τζ)为置信程度，（ax，ay，az)为动态加权系数。3. 根据权利要求1或2所述的基于室内四旋翼飞行器的激光雷达三维成像方法，其特 征在于，步骤2中，获得四旋翼飞行器的姿态角度数据的具体步骤为： 步骤2. 1，四旋翼飞行器惯性导航系统测量的X轴上惯性测量数据包括倾角陀螺仪 测量角速度《gyro、陀螺仪测量噪声cog、螺仪常值误差b、加速度计测量角度以及角速度 计测量噪声，四旋翼飞行器惯性导航系统的状态方程和输出方程为：步骤2. 2,设四旋翼飞行器惯性导航系统的采样周期为Ts，则上述方程离散化后为：式中，为k-Ι时刻的后验估计，X (k | k-Ι)为k时 刻的先验估计，X(k|k_l)的协方差为：式中，为陀螺仪和加速度计的过程噪声矩阵，P(k-1 |k-l)为协方差在 k_l时刻的后验估计，P(k|k_l)为协方差矩阵在k时刻的先验估计； 步骤2. 3,利用卡尔曼滤波对惯性测量数据进行数据融合，卡尔曼增益系数为： K (k) = P (k I k-1) +HT/ (HP (k | k-1)) HT+R) (8) 式中，H= [1 0]，R为测量协方差矩阵，得到角度和角度协方差的最优估计值X(k|k) 和 P (k | k)为：式中，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，设四旋翼飞行器的位移为(^,.，1\倾角为化，/?,?\对深度断层图像进行 补偿的具体步骤如下： 步骤3. 2. 1，四旋翼飞行器前进方向上的倾角为对深度断层图像进行补偿后展开对 应的曲线方程为& = ./<?-的，根据t。JP t。时刻的倾角动态加权得到理想的深度断层图 像：式中，h't JP h' t为经过友一的补偿后t。JP t。时刻的深度，h"为经过位移和倾 角补偿后的深度值，（a t u β t D为t。i时刻四旋翼飞行器的其中两轴姿态角，（a t，β t)为 t。时刻四旋翼飞行器的其中两轴姿态角。5.根据权利要求1所述的基于室内四旋翼飞行器的激光雷达三维成像方法，其特征在 于，步骤4中，在相邻深度断层图像之间使用滑动窗口牛顿插值法实现三维成像的具体步 骤为： 步骤4. 1，对tji = 0, 1，2,. . .，η)时刻获得的深度断层图像补偿后展开得到曲线函数 为： 仁（θ ) ( Θ = 〇。~360。） (14) 式中，对于角度θ = Θ,，深度断层图像都有一个相对应的取值fjep ; 步骤4. 2,根据步骤1得到各个深度断层图像相对于初始位置的位移数据，对有序数据 对（Xl，仁（Θ ))进行滑动窗口牛顿插值得到光滑曲线，插值公式如下：式中，Xl表示各个深度断层图像相对于初始位置的位移数据，η为滑动窗口大小，当深 度断层图像的数目超过η后，移出第一个深度断层图像，同时移入当前更新的深度断层图 像，再进行牛顿插值，从而实现基于滑动窗口的牛顿插值； 步骤4. 3,对所以Θ值的深度断层图像进行步骤4. 2同样操作，拼接差值后的所有深度 断层图像实现三维成像。
【专利摘要】本发明提供了一种基于室内四旋翼飞行器的激光雷达三维成像方法，步骤包括：读取加速度数据和距离数据，计算获得四旋翼飞行器的位移数据；读取惯性测量数据，计算获得四旋翼飞行器的姿态角度数据；读取深度断层图像数据，计算获得深度断层图像；根据位移数据和姿态角度数据确定深度断层图像数据的采集位置信息，再在相邻位置的深度断层图像之间使用滑动窗口牛顿插值法，从而实现三维成像。该三维成像方法具有高度的灵活性，相对于地面移动机器人能够降低复杂地面对飞行的影响，具有较高的适应能力。
【IPC分类】G01S17/89, G01C21/16, G01S17/02
【公开号】CN105334518
【申请号】CN201510855161
【发明人】陈春林, 王文庆, 窦宇豪, 苏三宝, 李珽光, 李博文, 朱张青, 辛博
【申请人】南京大学
【公开日】2016年2月17日
【申请日】2015年11月30日