CN103440358B - 一种基于dem数据的坡度拟合方法 - Google Patents

Abstract

一种基于DEM数据的坡度拟合方法，步骤为：（1）从三维成像敏感器获取三维地形数据；（2）对三维地形数据中包含的点进行间隔采样形成采集矩阵；（3）选取K₁X+K₂Y+K₃=Z作为坡度平面拟合方程；（4）对采集矩阵中的各采样点进行中心坐标变换；（5）将K的计算代数化，由此得到K1的各分量k_{1_1},k_{1_2}…k_{1_m}，以及K2的各分量k_{2_1},k_{2_2}…k_{2_n}；（6）对k_{1_1},k_{1_2}…k_{1_m}进行大小排序，得到中间值由此得到对k_{2_1},k_{2_2}…k_{2_n}进行大小排序，得到中间值由此得到（7）根据步骤（6）中得到的K₁、K₂，利用公式计算得到坡度角本发明方法具有地形坡度估计快速性、高鲁棒性的特点。

Description

一种基于DEM数据的坡度拟合方法

技术领域

本发明属于视觉导航技术领域，涉及一种快速、鲁棒坡度拟合方法。

背景技术

地外星体的着陆，例如月面探测，由于着陆地形未知且表面分布着大量的撞击坑和石块，因此确定安全着陆区域规避障碍至关重要。

当前，实施安全避障的敏感器包括光学成像敏感器和激光三维成像敏感器，前者利用二维灰度图进行障碍识别，后者利用高程数据，也就是DEM数据进行障碍识别。由于光学图像无法给出地形高度而激光三维成像敏感器可以提供高程信息，因此激光三维成像敏感器在美国、欧洲、日本以及我国航天科技中受到越来越多的关注，相应的三维数据的处理技术也发展迅速。

坡度拟合是高程数据处理中的重要环节。传统的坡度拟合方法分为两种，一种为直接最小二乘平面拟合、一种为随机选点拟合迭代。前者的优点是速度快一次输出结果，但是也存在明显不足易受到粗差的影响；后者通过多次拟合统计方式可以消除粗差影响但是所需要的运算量大。而且以上两种方法在视场边缘位置均出现求逆矩阵条件数偏大的情况，从而出现拟合坡度有误，在工程实现中将导致星体着陆或者巡视障碍判断失败，引起重大航天事故。

发明内容

本发明的技术解决问题是：克服现有的坡度拟合方法存在抗粗差能力差、计算量过大并会引起航天器着陆过程中的障碍判断失败等问题，提供了一种基于DEM数据的坡度拟合方法，通过方程变换、位置中心变换、参数中值引入等新处理方式的引入，解决了月面着陆过程中的坡度识别问题，提高了拟合时的速度，并可推广到更为广泛的火星、小行星或者地面用船用安全着陆等领域。

本发明的技术解决方案是：一种基于DEM数据的坡度拟合方法，步骤为：

（1）从三维成像敏感器获取三维地形数据，所述的三维地形数据为点的集合，每一个点均采用三个坐标值(X,Y,Z)表示，其中X,Y分别为地形平面的横纵坐标，Z为高程坐标；

（2）对步骤（1）的三维地形数据中包含的点进行间隔采样形成采样点集，其中X方向间隔dx，Y方向间隔dy，形成m×n采集矩阵；

（3）选取K₁X+K₂Y+K₃=Z作为坡度平面拟合方程，K1,K2,K3代表平面参数，记K=[K₁K₂K₃]^T=(G^TG)^-1G^Th，其中：

$G=\left[\begin{array}{ccc}{x}_{\mathrm{1,1}}& y_{\mathrm{1,1}}& 1\\ x_{\mathrm{1,2}}& y_{\mathrm{1,2}}& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ x_{1,n}& y_{1,n}& 1\\ x_{\mathrm{2,1}}& y_{\mathrm{2,1}}& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ x_{2,n}& y_{2,n}& 1\\ x_{\mathrm{3,1}}& y_{\mathrm{3,1}}& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ x_{m.1}& y_{m,1}& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ x_{m,n}& y_{m,n}& 1\end{array}\right],h={[z_{\mathrm{1,1}}{z}_{\mathrm{1,2}}\·\·\·z_{m,n}]}^T$

G的行数是m×n列数为3，h的行数是m×n列数为1；

（4）对采集矩阵中的各采样点进行中心坐标变换，变换公式为

x_i,j=x_i,j-x_(m+1)/2,_(n+1)/2y_i,j=y_i,j-y_{(m+1)/2,(n+1)/2}

其中x_i,j和y_i,j为采样矩阵中处于第i行第j列的采样点的横坐标和纵坐标；

（5）利用步骤（4）的结果，将K的计算代数化，由此得到K1的各分量k_{1_1},k_{1_2}Lk_{1_m}，以及K2的各分量k_{2_1},k_{2_2}Lk_{2_n}，其中m个K₁分量值代表了采集矩阵中各行的K₁分量值，n个K₂分量值代表了采集矩阵中各列的K₂分量值；

（6）对k_{1_1},k_{1_2}...k_{1_m}进行大小排序，得到中间值由此得到对k_{2_1},k_{2_2}...k_{2_n}进行大小排序，得到中间值由此得到 $K_2=n\·{\stackrel{\‾}{k}}_2;$

（7）根据步骤（6）中得到的K₁、K₂，利用公式计算得到坡度角。

本发明与现有技术相比的优点在于：本发明方法通过方程变换、位置中心变换、参数中值引入等新处理方式的引入，解决了传统拟合方法存在粗差大的问题，具有方法简便、计算量小、速度快、计算精度高、鲁棒性好的特点，可以为航天器在地外星体着陆过程中的快速避障、安全避障提供技术保证，并可推广到更为广泛的火星、小行星或者地面用船用安全着陆等领域。

附图说明

图1为本发明方法的流程框图；

图2为本发明中心坐标变换结果示意图；

图3为本发明矩阵G的形式图；

图4为地形拟合采样点为3×3时本发明矩阵G的形式图。

具体实施方式

本发明的坡度拟合方法流程如图1所示，具体步骤如下：

1）三维地形数据输入。三维地形数据由三维成像敏感器直接输出得到，敏感器可以是激光成像雷达，输出的格式为“点集”，记为U，每个点格式为(X,Y,Z)代表三个坐标值，X,Y为平面的横纵坐标，Z为高程坐标。

2）拟合数据采集，由于敏感器输出的点数量较多，考虑快速计算要求，需要对点集按照一定间隔采样（X方向间隔dx，Y方向间隔dy），一般选择奇数矩阵采集，行数记为m，列数记为n，例如3×3、5×5、7×7，记为(x_i,j,y_i,jz_i,j)，i、j分别为采集矩阵的元素的行号和列号，i∈[1m],j∈[1n]。

3）拟合方程确定。

坡度平面拟合方程选用K₁X+K₂Y+K₃=Z，其中X,Y,Z为待拟合地形的三维坐标，属于已知量，直接从三维成像敏感器上读出，X,Y为平面的横纵坐标，Z为高程坐标。K1,K2,K3代表平面参数，属于未知量。此拟合方程把拟合参数由3个变为了2个，对于计算量的提升有帮助。

假设K为平面拟合参数：K=[K₁K₂K₃]^T=(G^TG)^-1G^Th，其中G的形式如图3所示，h=[z_1,1,1z_1,2...z_m,n]^T。

G的行数是m×n列数为3，h的行数是m×n列数为1。

G的下标是2维变化的，逐行按列引用排列，x₁₁～x_1n、y₁₁～y_1n是第一行数据，x₂₁～x_2n、y₂₁～y_2n是第二行数据，一直排列到最后第m行数据x_m1～x_mn、y_m1～y_mn，m,n代表采样的行列数，在前面2）中已有说明。

下标为点位置标记，（m,n）代表采样点的行列，例如采样矩阵为3×3的，则m=3,n=3。

4）中心坐标变换

为求得参数K₁,K₂,K₃，需要求逆矩阵为G^T.G，一般使用矩阵条件数来描述矩阵的病态情况，经过分析仿真可知，当拟合三维数据的X,Y分布呈现0对称分布时条件数最小，因此X,Y横纵坐标进行如下的变换：

x_i,j=x_i,j-x_{(m+1)/2,(n+1)/2}y_i,j=y_i,j-y_{(m+1)/2,(n+1)/2}

其中下标(m+1)/2,(n+1)/2代表采样点集的中心位置点。

完成中心坐标变换后，G矩阵的中间行的数据为[001]，两侧的矢量按照拟合数据X,Y采样间隔dx,dy来确定。例如地形拟合采样点为3×3（m=3,n=3），其结果如图2所示，此时x_2,2=0,y_2,2=0，根据采样间隔x_1,1=-dx,y_1,1=dy，同理得到所有采样9个点的数据，如图4所示。

5）代数式变换

中心变换后G的矩阵式是确定的，那么原有的拟合公式可以实现代数式变换描述：

T=(G·GT)^-1·G^T，其输出是确定的3×n（3行n列）矩阵，

$\left[\begin{array}{c}{K}_1\\ K_2\\ K_3\end{array}\right]=\begin{array}{c}\left[\begin{array}{ccc}{T}_{\mathrm{1,1}}& ...& T_{1,n}\\ T_{\mathrm{2,1}}& ...& T_{2,n}\\ 1/n& ...& 1/n\end{array}\right]\·\end{array}\left[\begin{array}{c}{Z}_{\mathrm{1,1}}\\ .\\ .\\ .\\ Z_{m,n}\end{array}\right]$

由于矩阵G只与间隔dx,dy以及采样点数m,n有关，那么对应T的单元T_i,j也就成为只与dx,dy以及m,n有关的数据，这就为代数式变换取代矩阵计算奠定了理论基础。

例如设定m=3,n=3，根据G阵得到K值计算的代数化描述：

$\begin{array}{c}{K}_1=k_{1\_1}+k_{1\_2}+k_{1\_3}=(-\frac{1}{6\mathrm{dx}}{Z}_{\mathrm{1,1}}+\frac{1}{6\mathrm{dx}}{Z}_{\mathrm{1,3}})+(-\frac{1}{6\mathrm{dx}}{Z}_{\mathrm{2,1}}+\frac{1}{6\mathrm{dx}}{Z}_{\mathrm{2,3}})+\\ (-\frac{1}{6\mathrm{dx}}{Z}_{\mathrm{3,1}}+\frac{1}{6\mathrm{dx}}{Z}_{\mathrm{3,3}})\end{array}$

$\begin{array}{c}{K}_2=k_{2\_1}+k_{2\_2}+k_{2\_3}=(-\frac{1}{6\mathrm{dy}}{Z}_{\mathrm{1,1}}+\frac{1}{6\mathrm{dy}}{Z}_{\mathrm{3,1}})+(-\frac{1}{6\mathrm{dy}}{Z}_{\mathrm{1,2}}+\frac{1}{6\mathrm{dy}}{Z}_{\mathrm{3,2}})+\\ (-\frac{1}{6\mathrm{dx}}{Z}_{\mathrm{1,3}}+\frac{1}{6\mathrm{dx}}{Z}_{\mathrm{3,3}})\end{array}$

k_{1_1},k_{1_2},k_{1_3}分别代表了地形采样数据（3×3，共3行3列）第1行、第2行、第3行的K₁分量值，若采样行列为5×5数据则共有k_{1_1},k_{1_2},k_{1_3},k_{1_4},k_{1_5}共5个K₁分量值，若采样行列为m×n则共有k_{1_1},k_{1_2}...k_{1_m}共m个K₁分量值，同理按照T的方式分解计算，可以得到代数化描述。

k_{2_1},k_{2_2},k_{2_3}分别代表了地形采样数据（3×3，共3行3列）第1列、第2列、第3列的K₂分值，若采样行列为5×5数据则共有k_{2_1},k_{2_2},k_{2_3},k_{2_4},k_{2_5}共5个K₂分量值，若采样行列为m×n则共有k_{2_1},k_{2_2}...k_{2_m}共n个K₂分量值，同理按照T的方式分解计算，可以得到代数化描述。

对于K₃的计算，直接采取如下的方式：

$K_3=\frac{1}{m.n}{Z}_{\mathrm{1,1}}+\frac{1}{m.n}{Z}_{\mathrm{1,2}}+...+\frac{1}{m.n}{Z}_{m,n}$

6）K中值计算

若按照5）中计算K值的代数化描述公式可以直接计算出来K₁,K₂,K₃，但是直接累加会带来最小二乘法的“粗差”问题，因此本发明提出K中值计算方法，针对K值分量进行大小排序，选择中间值乘上分量个数作为输出。

对于K₁的计算步骤：

a对k_{1_1},k_{1_2}...k_{1_m}进行大小排序

b得到中间值

c得到 $K_1=m.{\stackrel{\‾}{k}}_1$

对于K₂的计算步骤：

a对k_{2_1},k_{2_2}...k_{2_n}进行大小排序

b得到中间值

c得到 $K_2=n.{\stackrel{\‾}{k}}_2$

对于K₃的计算步骤：

a对z_1,1,z_1,2...z_m,n进行大小排序

b得到中间值

c得到 $K_3=m.n.{\stackrel{\‾}{k}}_3$

7）地形坡度角计算

地形平面描述：

K₁X+K₂Y+K₃=Z

那么平面方程有：

K₁X+K₂Y-Z=-K₃

则平面法线矢量描述为（K₁,K₂,-1），单位化之后为

$(\frac{K_1}{\sqrt{K_1^2+K_2^2+1}},\frac{K_2}{\sqrt{K_1^2+K_2^2+1}},\frac{-1}{\sqrt{K_1^2+K_2^2+1}})$

考虑到地形坡度为小角度（小于90），坡度定义为地形坡面与水平面的夹角为，那么与平面的夹角余弦

反正弦三角函数的展开公式为：

$\arcsin \left(x\right)=x+\frac{1}{6}{x}^3+...+\frac{\left(2n\right)!}{2^{2n}{(n!)}^2(2n+1)}{x}^{2n+1}$

选择2次展开，那么对应于坡度角有：

8）障碍判断

根据航天着陆工程要求，设定可以承受的地形坡度为?_T，那么有如下的障碍判断。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

Claims (1)

1.一种基于DEM数据的坡度拟合方法，其特征在于步骤如下：

(1)从三维成像敏感器获取三维地形数据，所述的三维地形数据为点的集合，每一个点均采用三个坐标值(X,Y,Z)表示，其中X,Y分别为地形平面的横纵坐标，Z为高程坐标；

(2)对步骤(1)的三维地形数据中包含的点进行间隔采样形成采样点集，其中X方向间隔dx，Y方向间隔dy，形成m×n采集矩阵；

(3)选取K₁X+K₂Y+K₃＝Z作为坡度平面拟合方程，K1,K2,K3代表平面参数，记K＝[K₁ K₂ K₃]^T＝(G^TG)^-1G^Th，其中：

$G=\left[\begin{array}{ccc}{x}_{\mathrm{1,1}}& y_{\mathrm{1,1}}& 1\\ x_{\mathrm{1,2}}& y_{\mathrm{1,2}}& 1\\ .& .& .\\ .& .& .\\ .& .& .\\ x_{1,n}& y_{1,n}& 1\\ x_{\mathrm{2,1}}& y_{\mathrm{2,1}}& 1\\ .& .& .\\ .& .& .\\ .& .& .\\ x_{2,n}& y_{2,n}& 1\\ x_{\mathrm{3,1}}& y_{\mathrm{3,1}}& 1\\ .& .& .\\ .& .& .\\ .& .& .\\ x_{m,1}& y_{m,1}& 1\\ .& .& .\\ .& .& .\\ .& .& .\\ x_{m,n}& y_{m,n}& 1\end{array}\right],$ h＝[z_1,1 z_1,2 … z_m,n]^T

G的行数是m×n列数为3，h的行数是m×n列数为1；

(4)对采集矩阵中的各采样点进行中心坐标变换，变换公式为

x_i,j＝x_i,j-x_{(m+1)/2,(n+1)/2} y_i,j＝y_i,j-y_{(m+1)/2,(n+1)/2}

其中x_i,j和y_i,j为采集矩阵中处于第i行第j列的采样点的横坐标和纵坐标；

(5)利用步骤(4)的结果，将K的计算代数化，由此得到K1的各分量k_{1_1},k_{1_2}…k_{1_m}，以及K2的各分量k_{2_1},k_{2_2}…k_{2_n}，其中m个K₁分量值代表了采集矩阵中各行的K₁分量值，n个K₂分量值代表了采集矩阵中各列的K₂分量值；

(6)对k_{1_1},k_{1_2}…k_{1_m}进行大小排序，得到中间值由此得到对k_{2_1},k_{2_2}…k_{2_n}进行大小排序，得到中间值由此得到 $K_2=n\·{\stackrel{\‾}{k}}_2;$

(7)根据步骤(6)中得到的K₁、K₂，利用公式计算得到坡度角