CN103308925A - 一种一体化三维彩色激光雷达数据点云产生方法及其装置 - Google Patents

Abstract

本发明通过解决特征提取、异步同步、特征关联、数据差值等问题，将视觉数据和激光雷达数据进行融合，产生三维彩色深度图像数据，即三维彩色激光雷达数据点云，从而提供精确的距离信息、丰富的环境信息，有效克服传统激光雷达和视觉传感器在鲁棒性、信息量、精度和成本方面的缺陷。本发明将二维激光雷达、单目视觉传感器、惯性导航数据测量单元、驱动电机等集成为基于信息融合的、具有普遍适用性的、高精度的三维彩色激光雷达一体化单元，利用惯性导航数据和视觉数据对激光雷达数据进行二次对准，结构简单，无需对设备进行精确标定，性能可靠稳定、实用性强，可以广泛应用在运动条件下的室内外等各种环境中，具有一定普遍适用性。

Description

一种一体化三维彩色激光雷达数据点云产生方法及其装置

技术领域

本发明涉及一种数据点云产生方法及其装置，尤其涉及一种一体化三维彩色激光雷达数据点云产生方法及其装置。

背景技术

激光雷达和视觉传感器是移动机器人环境感知领域最常用传感器。传统的移动机器人导航系统利用视觉传感器获取丰富的环境信息，同时利用激光雷达获取精确的距离信息。视觉传感器信息量丰富，但这类传感器受环境光照条件等影响大。比如，在室外环境中应用良好的数据分析方法在应用在室内环境获取的数据时，会由于光照条件的剧烈变化而失效。同时，基于视觉数据的深度信息获取方法计算复杂度高，获得深度数据精度差。激光雷达数据具有良好的环境鲁棒性，获取的深度数据精度高。但是，激光雷达往往对环境过分的离散化，获取的数据粒度大，获取信息量有限，难以用于环境理解等应用。二维激光雷达获取信息量过小，难以应用于三维复杂环境的感知；普通三维激光雷达的价格过高、可观测距离相对较短，对空间的离散化程度过高，难以普遍应用于移动机器人导航任务中；传统的将摆动的二维激光雷达数据对准获取三维激光雷达数据点云的方法难以克服激光雷达单元运动所带来的数据变形，应用受到极大的限制。因此，环境感知的应用需求迫切需要一种能获取丰富环境信息的、具有精确距离信息的、具有一定普遍适用性的环境感知设备及方法。

发明内容

为解决现有技术中存在的信息量、传感器成本以及信息准确度等方面问题，本发明的目的在于提供一种一体化三维彩色激光雷达数据点云产生方法及其装置。

本发明通过解决特征提取、异步同步、特征关联、数据差值等问题，将视觉数据和激光雷达数据进行融合，产生三维彩色深度图像数据，即三维彩色激光雷达数据点云，从而提供精确的距离信息、丰富的环境信息，具有一定普遍适用性；将二维激光雷达、单目视觉传感器、惯性导航数据测量单元、驱动电机等集成为三维彩色激光雷达一体化单元，利用惯性导航数据和视觉数据对激光雷达数据进行二次对准，结构简单，无需对设备进行精确标定，性能可靠稳定、实用性强，可以广泛应用在运动条件下的室内外等各种环境中，具有一定普遍适用性。

本发明所采用的技术方案：

一种一体化三维彩色激光雷达数据点云产生方法，包括以下步骤：

（1）通过二维激光雷达对三维空间进行环境序列扫描，获取雷达数据；

（2）通过单目视觉传感器获得原始彩色图像；

（3）通过惯性导航单元获取一体化三维彩色激光雷达的运动信息，包括X、Y、Z三个方向的线速度信息以及三个轴向角速度信息；

（4）根据惯性导航单元与二维激光雷达的几何位置关系，计算出采样周期内一体化三维彩色激光雷达移动的距离和方向，利用逆投影方法对激光雷达数据进行初步修正， $\left[\begin{array}{c}x\′\\ y\′\\ z\′\\ 1\end{array}\right]=R\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right],$ 其中(xyz)为原激光雷达点坐标，(x′y′z′)为投影后激光雷达点坐标，R为投影矩阵，如下式所示：：

$R=\left[\begin{array}{cccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \mathrm{\Δx}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \mathrm{\Δy}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}& \mathrm{\Δz}\\ 0& 0& 0& 1\end{array}\right]$

式中φ、θ和ψ为沿三个轴向的旋转角度，△x、△y和△z为沿三个轴向的平移距离，其值为步骤（3）中获取的X、Y、Z三个方向的线速度信息以及三个轴向角速度信息对采样周期的积分；

（5）利用雷达数据的高度序列化的特点对数据进行聚类，具体方法为序列检测获取的二维观测中相邻数据点之间的距离，将距离小于门限值的点划分为同一个集合，激光雷达的二维数据可被划分为一个或多个集合；

（6）对步骤（5）中所获得的集合，对同一个集合内的数据点进行平滑滤波，如下式所示：

${\mathrm{\χ}}^2={\mathrm{\δ}}_i^T{\mathrm{\Σ}}_i^{-1}{\mathrm{\δ}}_i+\mathrm{\α}{({\mathrm{\θ}}_i-{\mathrm{\θ}}_{i-1})}^2---\left(i\right)$

式中δ_i为观测数据点i沿观测方向平移距离，Σ_i为激光雷达测距模型中的噪声方差，

为产生的滤波成本，α为平滑性倾向权值，θ_i为第i个观测点与第i-1个观测点及第i+1个观测点之间构成的夹角，χ²为由于平滑观测点所产生的卡方分布（χ²）值；

（7）利用结构张力模型提取出步骤（6）中平滑后的数据点集合中的形状发生了剧烈改变的数据点，即拐点，计算该中心点的结构张力，如下式所示：

$A=\underset{i}{\mathrm{\Σ}}\left[\begin{array}{cc}{v}_{i,x}^2& v_{i,x}{v}_{i,y}\\ v_{i,x}{v}_{i,y}& v_{i,y}^2\end{array}\right]---\left(\mathrm{ii}\right)$

其中v_i,x与v_i,y分别表示中心数据点i对应的模向量在x方向和y方向的分量，λ₁与λ₂为数据点的二维结构张力矩阵的特征值，二者中较小的特征值为模张力的显著性指标C=min(λ₁,λ₂)，将具有最大的显著性指标的数据点提取为特征点，将结构张力模型的逆矩阵A^-1作为特征点的不确定度；

（8）将原始图像与标准高斯函数进行卷积平滑图像，如下式所示：

L=G*L_o

其中L_o为原始观测图像，G为高斯滤波器，L_o为原始图像，L为平滑后图像；

（9）将平滑后的图像L与不同的模量进行卷积，获取图像中的边缘信息，如下式所示：

L_xx=[1 -2 1]*L

$L_{\mathrm{xy}}=\left[\begin{array}{ccc}-1/4& 0& 1/4\\ 0& 0& 0\\ 1/4& 0& -1/4\end{array}\right]*L$

$L_{\mathrm{yy}}=\left[\begin{array}{c}1\\ -2\\ 1\end{array}\right]*L$

式中L_xx，L_xy和L_yy为在X轴平行方向，X-Y中分线方向和Y方向的边缘图像；

（10）将原始图像中单像素所对应的物理距离视为三倍标准差所对应的误差距离，即3δ距离，获取图像信息的不确定度；

（11）将步骤（7）中提取的拐点数据与步骤（8）中所提取的图像边缘数据进行精确对准，如下式所示：

g(x,o)=0    （iii）

式中g(·)表示投影关系函数，x表示步骤（8）所提取的视觉图像中的边缘，o表示步骤（7）中提取的拐点，构建数据关联成本函数，如下式所示：

$C_{H_t}=I_{H_t,s}{(J_1{P}_t{J}_1^T+J_2{P}_{u_t}{J}_2^T)}^{-1}{I}_{H_t,s}^T+I_{H_t,o}{V}^{-1}{I}_{H_t,o}^T---\left(\mathrm{iv}\right)$

$J_1=\frac{\∂f\′}{\∂x_t}$          $J_2=\frac{\∂f\′}{\∂u_t}$

式中

是视觉特征估计位置与关联位置之间的距离（新息），是激光雷达特征估计位置与关联位置之间的距离（新息），P_t是视觉特征的协方差矩阵；

是投影矩阵估计值的不确定度矩阵，V是激光雷达特征的协方差矩阵；J₁及J₂是投影关系函数f'(·)分别对视觉系统位置和投影距离的偏导数；

（12）定义S为投影关系函数f(·)所构建的协方差矩阵，H为投影关系函数f(·)的Hessian矩阵，K=PH^TS^-1则为卡尔曼滤波增益矩阵，G为将激光雷达特征投影到视觉传感器坐标系下的投影矩阵对激光雷达坐标系的Hessian矩阵，I为单位矩阵，如下式所示：

S^-1=[I+K^TH^T-(HK)^T]S^-1

=[K^TH^T+(I-HK)^T(GVG^T)^-1(GVG^T)]S^-1

=[K^TH^T+(I-HK)^T(GVG^T)^-1(S-HPH^T)]S^-1

=[K^T(P^-1PH^TS^-1S)+(I-HK)^T(GVG^T)^-1(I-HK)S]S^-1

=[K^TP^-1KS+(I-HK)^T(GVG^T)^-1(I-HK)S]S^-1

=K^TP^-1K+(I-HK)^T(GVG^T)^-1(I-HK)

由此可知，卡尔曼滤波极小化了均方误差，式（iv）所定义的成本函数可以极小化关联误差；

（13）计算激光雷达与单目视觉传感器之间的投影关系，如下式所示：

$M=\underset{i}{\mathrm{\Σ}}(l_{r,x}^i-\stackrel{\‾}{l_{r,x}})(O_y^i-\stackrel{\‾}{O_y})-(l_{r,y}^i-\stackrel{\‾}{l_{r,y}})(O_x^i-\stackrel{\‾}{O_x})$

$N=\underset{i}{\mathrm{\Σ}}(l_{r,x}^i-\stackrel{\‾}{l_{r,x}})(O_x^i-\stackrel{\‾}{O_x})-(l_{r,y}^i-\stackrel{\‾}{l_{r,y}})(O_y^i-\stackrel{\‾}{O_y})$

u_θ=arctan(M/N)

$u_x=\stackrel{\‾}{O_x}-\stackrel{\‾}{I_{r,x}}\cos \left(u_{\mathrm{\θ}}\right)+\stackrel{\‾}{I_{r,y}}\sin \left(u_{\mathrm{\θ}}\right)$

$u_y=\stackrel{\‾}{O_y}-\stackrel{\‾}{I_{r,x}}\sin \left(u_{\mathrm{\θ}}\right)+\stackrel{\‾}{I_{r,y}}\cos \left(u_{\mathrm{\θ}}\right)$

式中为图像数据中特征坐标，

为激光雷达特征坐标，

为所有图像数据特征的位置平均值，

为所有激光雷达特征的位置平均值，M和N为根据特征对应关系计算得到的方向矢量，u_θ为根据关联关系计算得到的激光雷达与图像传感器之间沿Z轴的旋转角度，u_x和u_y分别为根据关联关系计算得到的激光雷达与图像传感器在X轴、Y轴方向上的距离，在Z方向的距离在一体化单元单元设计时已确定，为已知量，X轴向旋转角始终保持为0，Y轴向旋转角度为单目视觉传感器的朝向角由控制系统决定，为已知量；

（14）由步骤（11）中获得激光雷达与单目视觉传感器之间的实时投影关系，利用一维线性插值降低激光雷达数据的数据粒度，从而降低对环境的离散化程度，如下式所示：

$y=y_0+(y_1-y_0)\frac{x-x_0}{x_1-x_0}$

式中(x₀,y₀)与(x₁,y₁)为已有激光雷达数据观测点位置，(x,y)为插入(x₀,y₀)与(x₁,y₁)之间的激光雷达点位置，迭代进行该过程，从而完成任意精细化程度的插值，获得对环境描述程度精细的三维激光雷达数据点云；

（15）由步骤（14）对激光雷达数据精细化产生数据点云，利用二维插值方法，从图像数据中获得数据点云中点的彩色信息，进而产生彩色三维激光雷达数据点云，如下式所示：

$f(x,y)\≈\frac{f\left(Q_{11}\right)}{(x_2-x_1)(y_2-y_1)}(x_2-x)(y_2-y)\frac{f\left(Q_{21}\right)}{(x_2-x_1)(y_2-y_1)}(x-x_1)(y_2-y)+$

$\frac{f\left(Q_{12}\right)}{(x_2-x_1)(y_2-y_1)}(x_2-x)(y-y_1)+\frac{f\left(Q_{22}\right)}{(x_2-x_1)(y_2-y_1)}(x-x_1)(y-y_1)$

$=\frac{1}{(x_2-x_1)(y_2-y_1)}\left(f\left(Q_{11}\right)(x_2-x)(y_2-y)\right)+f\left(Q_{21}\right)(x-x_1)(y_2-y)+$

$f\left(Q_{12}\right)(x_2-x)(y-y_1)+f\left(Q_{22}\right)(x-x_1)(y-y_1))$

$f\left(R_1\right)\≈\frac{x_2-x}{x_2-x_1}f\left(Q_{11}\right)+\frac{x-x_1}{x_2-x_1}f\left(Q_{21}\right)$

R₁=(x,y₁)

$f\left(R_2\right)\≈\frac{x_2-x}{x_2-x_1}f\left(Q_{12}\right)+\frac{x-x_1}{x_2-x_1}f\left(Q_{22}\right)$

R₂=(x,y₂)

$f\left(P\right)\≈\frac{y_2-y}{y_2-y_1}f\left(R_1\right)+\frac{y-y_1}{y_2-y_1}f\left(R_2\right)$

式中f(·)为数据点色彩信息函数，Q₁₁=(x₁,y₁)、Q₁₂=(x₁,y₂)、Q₂₁=(x₂,y₁)、Q₂₂=(x₂,y₂)分别为插值点上下左右四个方向的相邻数据点，(x₁,y₁)、(x₁,y₂)、(x₂,y₁)和(x₂,y₂)分别为插值点上下左右四个方向的相邻数据点的坐标，f(R₁)和f(R₂)分别为在X方向的y₁和y₂处线性插值结果，f(P)为Y方向线性插值结果，R₁为插值点上方数据点，R₂为插值点下方数据点。

步骤（1）中，所得二维激光雷达单次扫描数据为X-Y平面上的一组离散的点，在三维空间中的扫描为具有不同Z坐标值的多组离散的点，其空间分布表现为离散点构成的三维网格。

步骤（6）中，α的取值范围为0.5～2，对于观测误差较大的激光雷达，α取值应适当偏大，对观测误差较小的激光雷达，α取值适当偏小。

一种一体化三维彩色激光雷达数据点云产生装置，包括固定平台、惯性导航数据测量传感器、单目视觉传感单元、二维激光雷达单元。惯性导航数据测量单元设置在固定平台前端，二维激光雷达单元设置在惯性导航数据测量单元上方，单目视觉传感单元设置在固定平台后端。

二维激光雷达单元包括二维激光雷达、转轴和驱动舵机。

二维激光雷达可由驱动舵机驱动在平衡位置上下±25°范围内非匀速摆动。

单目视觉传感单元包括单目视觉传感器、转轴和驱动舵机。

单目视觉传感器由驱动舵机驱动在平衡位置左右±60°范围内根据应用要求转动。

单目视觉传感器和二维激光雷达均在平衡位置时，单目视觉传感器、二维激光雷达和惯性导航数据测量单元三者所在的X-Z平面重合。

本发明的有益效果：

本发明涉及一种一体化三维彩色激光雷达数据点云产生方法，通过解决特征提取、异步同步、特征关联、数据差值等问题，将视觉数据和激光雷达数据进行融合，产生三维彩色深度图像数据，即三维彩色激光雷达数据点云，从而提供精确的距离信息、丰富的环境信息，有效克服传统激光雷达和视觉传感器在鲁棒性、信息量、精度和成本方面的缺陷。本发明还涉及一种一体化三维彩色激光雷达数据点云产生装置，将二维激光雷达、单目视觉传感器、惯性导航数据测量单元、驱动电机等集成为基于信息融合的、具有普遍适用性的、高精度的三维彩色激光雷达一体化单元，利用惯性导航数据和视觉数据对激光雷达数据进行二次对准，结构简单，无需对设备进行精确标定，性能可靠稳定、实用性强，可以广泛应用在运动条件下的室内外等各种环境中，具有一定普遍适用性。

附图说明

图1为本发明的一体化三维彩色激光雷达数据点云产生装置的结构示意图。

图中：1、单目视觉传感器，2、转轴，3、驱动舵机，4、二维激光雷达，5、转轴，6、驱动舵机，7、惯性导航数据测量单元。

具体实施方式

以下结合附图和具体实施方式对本发明作进一步详细说明。

如图1所示，一种一体化三维彩色激光雷达数据点云产生装置，包括固定平台、惯性导航数据测量传感器、单目视觉传感单元、二维激光雷达单元。惯性导航数据测量单元设置在固定平台前端。二维激光雷达单元设置在惯性导航数据测量单元上方，包括二维激光雷达、转轴和驱动舵机，二维激光雷达可由驱动舵机驱动在平衡位置上下±25°范围内非匀速摆动。单目视觉传感单元设置在固定平台后端，包括单目视觉传感器、转轴和驱动舵机，单目视觉传感器由驱动舵机驱动在平衡位置左右±60°范围内根据应用要求转动。单目视觉传感器和二维激光雷达均在平衡位置时，单目视觉传感器、二维激光雷达和惯性导航数据测量单元三者所在的X-Z平面重合。

一种一体化三维彩色激光雷达数据点云产生方法，具体步骤为：

（1）通过二维激光雷达对三维空间进行环境序列扫描，获取雷达数据，所得二维激光雷达单次扫描数据为X-Y平面上的一组离散的点，在三维空间中的扫描为具有不同Z坐标值的多组离散的点，其空间分布表现为离散点构成的三维网格；

（2）通过单目视觉传感器获得原始彩色图像；

（3）惯性导航单元获取一体化三维彩色激光雷达的运动信息，包括X、Y、Z三个方向的线速度信息以及三个轴向角速度信息；

（4）根据惯性导航单元与二维激光雷达的几何位置关系，计算出采样周期内一体化三维彩色激光雷达移动的距离和方向，利用逆投影方法对激光雷达数据进行初步修正， $\left[\begin{array}{c}x\text{'}\\ y\text{'}\\ z\text{'}\\ 1\end{array}\right]=R\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right],$ 其中(xyz)为原激光雷达点坐标，(x′y′z′)为投影后激光雷达点坐标，R为投影矩阵，如下式所示：：

$R=\left[\begin{array}{cccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \mathrm{\Δx}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \mathrm{\Δy}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}& \mathrm{\Δz}\\ 0& 0& 0& 1\end{array}\right]$

式中φ、θ和ψ为沿三个轴向的旋转角度，△x、△y和△z为沿三个轴向的平移距离，其值为步骤（3）中获取的X、Y、Z三个方向的线速度信息以及三个轴向角速度信息对采样周期的积分；

（5）利用雷达数据的高度序列化的特点对数据进行聚类，具体方法为序列检测获取的二维观测中相邻数据点之间的距离，将距离小于门限值的点划分为同一个集合，激光雷达的二维数据可能被划分为一个或多个集合；

（6）对步骤（5）中所获得的集合，对同一个集合内的数据点进行平滑滤波，具体方法为：利用基于激光雷达测距噪声模型的概率优化方法进行滤波，提取轮廓信息，在不改变点正确分布的情况下极小化测距噪声对观测的影响，如下式所示：

${\mathrm{\χ}}^2={\mathrm{\δ}}_i^T{\mathrm{\Σ}}_i^{-1}{\mathrm{\δ}}_i+\mathrm{\α}{({\mathrm{\θ}}_i-{\mathrm{\θ}}_{i-1})}^2---\left(i\right)$

式中δ_i为观测数据点i沿观测方向平移距离，Σ_i为激光雷达测距模型中的噪声方差，

为产生的滤波成本，α为平滑性倾向权值，θ_i为第i个观测点与第i-1个观测点及第i+1个观测点之间构成的夹角，χ²为由于平滑观测点所产生的卡方分布（χ²）值；

（7）利用结构张力模型提取出步骤（6）中平滑后的数据点集合中的形状发生了剧烈改变的数据点，即拐点。具体方法为：将同一集合内激光雷达数据点近似看做均匀分布，定义一个圆形数据窗口来减小朝向对特征提取的影响，计算在数据窗口中心点的激光雷达点的模向量，该模向量为该激光雷达点与数据窗口边缘激光雷达点之间构成的夹角的中分线，计算该中心点的结构张力，如下式所示：

$\underset{i}{\mathrm{\Σ}}\left[\begin{array}{cc}{v}_{i,x}^2& v_{i,x}{v}_{i,y}\\ v_{i,x}{v}_{i,y}& v_{i,y}^2\end{array}\right]---\left(\mathrm{ii}\right)$

其中v_i,x与v_i,y分别表示中心数据点i对应的模向量在x方向和y方向的分量，λ₁与λ₂为数据点的二维结构张力矩阵的特征值，二者中较小的特征值为模张力的显著性指标C=min(λ₁,λ₂)，将具有最大的显著性指标的数据点提取为特征点，将结构张力模型的逆矩阵A^-1作为特征点的不确定度；

（8）将原始图像与标准高斯函数进行卷积平滑图像，如下式所示：

L=G*L_o

其中L_o为原始观测图像，G为高斯滤波器，L_o为原始图像，L为平滑后图像；

（9）将平滑后的图像L与不同的模量进行卷积，则可以获取图像中的边缘信息，如下式所示：

L_xx=[1 -2 1]*L

$L_{\mathrm{xy}}=\left[\begin{array}{ccc}-1/4& 0& 1/4\\ 0& 0& 0\\ 1/4& 0& -1/4\end{array}\right]*L$

$L_{\mathrm{xy}}=\left[\begin{array}{c}1\\ -2\\ 1\end{array}\right]*L$

式中L_xx，L_xy和L_yy为在X轴平行方向，X-Y中分线方向和Y方向的边缘图像；

（10）将原始图像中单像素所对应的物理距离视为三倍标准差所对应的误差距离，即3δ距离，获取图像信息的不确定度；

（11）将步骤（7）中提取的拐点数据与步骤（8）中所提取的图像边缘数据进行精确对准，具体方法为：激光雷达与视觉数据之间的几何关系构成了一个投影矩阵，正确的激光雷达特征在经过投影后应与视觉数据中的边际完美的重合，即：

g(x,o)=0    （iii）

式中g(·)表示投影关系函数，x表示步骤（8）所提取的视觉图像中的边缘，o表示步骤（7）中提取的拐点，

构建数据关联成本函数如下式所示：

$C_{H_t}=I_{H_t,s}{(J_1{P}_t{J}_t^T+J_2{P}_{u_t}{J}_2^T)}^{-1}{I}_{{H_t}^{,s}}^T+I_{H_{t,},o}{V}^{-1}{I}_{H_t,o}^T---\left(\mathrm{iv}\right)$

$J_1=\frac{\∂f\′}{\∂x_t}$         $J_2=\frac{\∂f\′}{\∂u_t}$

式中

是视觉特征估计位置与关联位置之间的距离（新息），是激光雷达特征估计位置与关联位置之间的距离（新息），P_t是视觉特征的协方差矩阵；

是投影矩阵估计值的不确定度矩阵，V是激光雷达特征的协方差矩阵；J₁及J₂是投影关系函数f'(·)分别对视觉系统位置和投影距离的偏导数；

（12）定义S为投影关系函数f(·)所构建的协方差矩阵，H为投影关系函数f(·)的Hessian矩阵，K=PH^TS^-1则为卡尔曼滤波增益矩阵，G为将激光雷达特征投影到视觉传感器坐标系下的投影矩阵对激光雷达坐标系的Hessian矩阵，I为单位矩阵，如下式所示：

S^-1=[I+K^TH^T-(HK)^T]S^-1

=[K^TH^T+(I-HK)^T(GVG^T)^-1(GVG^T)]S^-1

=[K^TH^T+(I-HK)^T(GVG^T)^-1(S-HPH^T)]S^-1

=[K^T(P^-1PH^TS^-1S)+(I-HK)^T(GVG^T)^-1(I-HK)S]S^-1

=[K^TP^-1KS+(I-HK)^T(GVG^T)^-1(I-HK)S]S^-1

=K^TP^-1K+(I-HK)^T(GVG^T)^-1(I-HK)

由此可知，卡尔曼滤波极小化了均方误差，式（iv）所定义的成本函数可以极小化关联误差；

（13）计算激光雷达与单目视觉传感器之间的投影关系，如下式所示：

$M=\underset{i}{\mathrm{\Σ}}(l_{r,x}^i-\stackrel{\‾}{l_{r,x}})(O_y^i-\stackrel{\‾}{O_y})-(l_{r,y}^i-\stackrel{\‾}{l_{r,y}})(O_x^i-\stackrel{\‾}{O_x})$

$N=\underset{i}{\mathrm{\Σ}}(l_{r,x}^i-\stackrel{\‾}{l_{r,x}})(O_x^i-\stackrel{\‾}{O_x})-(l_{r,y}^i-\stackrel{\‾}{l_{r,y}})(O_y^i-\stackrel{\‾}{O_y})$

u_θ=arctan(M/N)

$u_x=\stackrel{\‾}{O_x}-\stackrel{\‾}{I_{r,x}}\cos \left(u_{\mathrm{\θ}}\right)+\stackrel{\‾}{I_{r,y}}\sin \left(u_{\mathrm{\θ}}\right)$

$u_y=\stackrel{\‾}{O_y}-\stackrel{\‾}{I_{r,x}}\sin \left(u_{\mathrm{\θ}}\right)-\stackrel{\‾}{I_{r,y}}\cos \left(u_{\mathrm{\θ}}\right)$

式中

为图像数据中特征坐标，

为激光雷达特征坐标，

为所有图像数据特征的位置平均值，为所有激光雷达特征的位置平均值，M和N为根据特征对应关系计算得到的方向矢量，u_θ为根据关联关系计算得到的激光雷达与图像传感器之间沿Z轴的旋转角度，u_x和u_y分别为根据关联关系计算得到的激光雷达与图像传感器在X轴、Y轴方向上的距离，在Z方向的距离在一体化单元设计时已确定，为已知量，X轴向旋转角始终保持为0，Y轴向旋转角度为单目视觉传感器的朝向角由控制系统决定，为已知量；

（14）由步骤（11）中获得激光雷达与单目视觉传感器之间的实时投影关系，利用一维线性插值降低激光雷达数据的数据粒度，从而降低对环境的离散化程度，如下式所示：

$y=y_0+(y_1-y_0)\frac{x-x_0}{x_1-x_0}$

式中(x₀,y₀)与(x₁,y₁)为已有激光雷达数据观测点位置，(x,y)为插入(x₀,y₀)与(x₁,y₁)之间的激光雷达点位置，迭代进行该过程，从而完成任意精细化程度的插值，获得对环境描述程度精细的三维激光雷达数据点云；

（15）由步骤（14）对激光雷达数据精细化产生数据点云，利用二维插值方法，从图像数据中获得数据点云中点的彩色信息，进而产生彩色三维激光雷达数据点云，如下式所示：

$f(x,y)\≈\frac{f\left(Q_{11}\right)}{(x_2-x_1)(y_2-y_1)}(x_2-x)(y_2-y)+\frac{f\left(Q_{21}\right)}{(x_2-x_1)(y_2-y_1)}(x-x_1)(y_2-y)+$

$\frac{f\left(Q_{12}\right)}{(x_2-x_1)(y_2-y_1)}(x_2-x)(y-y_1)+\frac{f\left(Q_{22}\right)}{(x_2-x_1)(y_2-y_1)}(x-x_1)(y-y_1)$

$=\frac{1}{(x_2-x_1)(y_2-y_1)}(f\left(Q_{11}\right)(x_2-x)(y_2-y)+f\left(Q_{21}\right)(x-x_1)(y_2-y)+$

$f\left(Q_{12}\right)(x_2-x)(y-y_1)+f\left(Q_{22}\right)(x-x_1)(y-y_1))$

$f\left(R_1\right)\≈\frac{x_2-x}{x_2-x_1}f\left(Q_{11}\right)+\frac{x-x_1}{x_2-x_1}f\left(Q_{21}\right)$

R₁=(x,y₁)

$f\left(R_2\right)\≈\frac{x_2-x}{x_2-x_1}f\left(Q_{12}\right)+\frac{x-x_1}{x_2-x_1}f\left(Q_{22}\right)$

R₂=(x,y₂)

$f\left(P\right)\≈\frac{y_2-y}{y_2-y_1}f\left(R_1\right)+\frac{y-y_1}{y_2-y_1}f\left(R_2\right)$

式中f(·)为数据点色彩信息函数，Q₁₁=(x₁,y₁)、Q₁₂=(x₁,y₂)、Q₂₁=(x₂,y₁)、Q₂₂=(x₂,y₂)分别为插值点上下左右四个方向的相邻数据点，(x₁,y₁)、(x₁,y₂)、(x₂,y₁)和(x₂,y₂)分别为插值点上下左右四个方向的相邻数据点的坐标，f(R₁)和f(R₂)分别为在X方向的y₁和y₂处线性插值结果，f(P)为Y方向线性插值结果，R₁为插值点上方数据点，R₂为插值点下方数据点。

Claims (9)

1.一种一体化三维彩色激光雷达数据点云产生方法，其特征在于：包括以下步骤：

（1）通过二维激光雷达对三维空间进行环境序列扫描，获取雷达数据；

（2）通过单目视觉传感器获得原始彩色图像；

（3）通过惯性导航单元获取一体化三维彩色激光雷达的运动信息，包括X、Y、Z三个方向的线速度信息以及三个轴向角速度信息；

（4）根据惯性导航单元与二维激光雷达的几何位置关系，计算出采样周期内一体化三维彩色激光雷达移动的距离和方向，利用逆投影方法对激光雷达数据进行初步修正， $\left[\begin{array}{c}x\text{'}\\ y\text{'}\\ z\text{'}\\ 1\end{array}\right]=R\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right],$ 其中(xyz)为原激光雷达点坐标，(x′y′z′)为投影后激光雷达点坐标，R为投影矩阵，如下式所示：：

$R=\left[\begin{array}{cccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \mathrm{\Δx}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \mathrm{\Δy}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}& \mathrm{\Δz}\\ 0& 0& 0& 1\end{array}\right]$

式中φ、θ和ψ为沿三个轴向的旋转角度，△x、△y和△z为沿三个轴向的平移距离，其值为步骤（3）中获取的X、Y、Z三个方向的线速度信息以及三个轴向角速度信息对采样周期的积分；

（5）利用雷达数据的高度序列化的特点对数据进行聚类，具体方法为序列检测获取的二维观测中相邻数据点之间的距离，将距离小于门限值的点划分为同一个集合，激光雷达的二维数据可被划分为一个或多个集合；

（6）对步骤（5）中所获得的集合，对同一个集合内的数据点进行平滑滤波，如下式所示：

${\mathrm{\χ}}^2={\mathrm{\δ}}_i^T{\mathrm{\Σ}}_i^{-1}{\mathrm{\δ}}_i+\mathrm{\α}{({\mathrm{\θ}}_i-{\mathrm{\θ}}_{i-1})}^2---\left(i\right)$

式中δ_i为观测数据点i沿观测方向平移距离，Σ_i为激光雷达测距模型中的噪声方差，

为产生的滤波成本，α为平滑性倾向权值，θ_i为第i个观测点与第i-1个观测点及第i+1个观测点之间构成的夹角，χ²为由于平滑观测点所产生的卡方分布（χ²）值；

（7）利用结构张力模型提取出步骤（6）中平滑后的数据点集合中的形状发生了剧烈改变的数据点，即拐点，计算该中心点的结构张力，如下式所示：

$A=\underset{i}{\mathrm{\Σ}}\left[\begin{array}{cc}{v}_{i,x}^2& v_{i,x}{v}_{i,y}\\ v_{i,x}{v}_{i,y}& v_{i,y}^2\end{array}\right]---\left(\mathrm{ii}\right)$

其中v_i,x与v_i,y分别表示中心数据点i对应的模向量在x方向和y方向的分量，λ₁与λ₂为数据点的二维结构张力矩阵的特征值，二者中较小的特征值为模张力的显著性指标C=min(λ₁,λ₂)将具有最大的显著性指标的数据点提取为特征点，将结构张力模型的逆矩阵A^-1作为特征点的不确定度；

（8）将原始图像与标准高斯函数进行卷积平滑图像，如下式所示：

L=G*L_o

其中L_o为原始观测图像，G为高斯滤波器，L_o为原始图像，L为平滑后图像；

（9）将平滑后的图像L与不同的模量进行卷积，获取图像中的边缘信息，如下式所示：

L_xx=[1 -2 1]*L

$L_{\mathrm{xy}}=\left[\begin{array}{ccc}-1/4& 0& 1/4\\ 0& 0& 0\\ 1/4& 0& -1/4\end{array}\right]*L$

$L_{\mathrm{yy}}=\left[\begin{array}{c}1\\ -2\\ 1\end{array}\right]*L$

式中L_xx，L_xy和L_yy为在X轴平行方向，X-Y中分线方向和Y方向的边缘图像；

（10）将原始图像中单像素所对应的物理距离视为三倍标准差所对应的误差距离，即3δ距离，获取图像信息的不确定度；

（11）将步骤（7）中提取的拐点数据与步骤（8）中所提取的图像边缘数据进行精确对准，如下式所示：

g(x,o)=0     （iii）

式中g(·)表示投影关系函数，x表示步骤（8）所提取的视觉图像中的边缘，o表示

步骤（7）中提取的拐点，构建数据关联成本函数，如下式所示：

$C_{H_t}=I_{H_t,s}{(J_1{P}_t{J}_1^T+J_2{P}_{u_t}{J}_2^T)}^{-1}{I}_{H_t,s}^T+I_{H_t,o}{V}^{-1}{I}_{H_t,o}^t---\left(\mathrm{iv}\right)$

$J_1=\frac{\∂f\′}{\∂x_t}$      $J_2=\frac{\∂f\′}{\∂x_t}$

式中

是视觉特征估计位置与关联位置之间的距离（新息），

是激光雷达特征估计位置与关联位置之间的距离（新息），P_t是视觉特征的协方差矩阵；

是投影矩阵估计值的不确定度矩阵，V是激光雷达特征的协方差矩阵；J₁及J₂是投影关系函数f'(·)分别对视觉系统位置和投影距离的偏导数；

（12）定义S为投影关系函数f(·)所构建的协方差矩阵，H为投影关系函数f(·)的Hessian矩阵，K=PH^TS^-1则为卡尔曼滤波增益矩阵，G为将激光雷达特征投影到视觉传感器坐标系下的投影矩阵对激光雷达坐标系的Hessian矩阵，I为单位矩阵，如下式所示：

S^-1=[I+K^TH^T-(HK)^T]S^-1

=[K^TH^T+(I-HK)^T(GVG^T)^-1(GVG^T)]S^-1

=[K^TH^T+(I-HK)^T(GVG^T)^-1(S-HPH^T)]S^-1

=[K^T(P^-1PH^TS^-1S)+(I-HK)^T(GVG^T)^-1(I-HK)S]S^-1

=[K^TP^-1KS+(I-HK)^T(GVG^T)^-1(I-HK)S]S^-1

=K^TP^-1K+(I-HK)^T(GVG^T)^-1(I-HK)

由此可知，卡尔曼滤波极小化了均方误差，式（iv）所定义的成本函数可以极小化关联误差；

（13）计算激光雷达与单目视觉传感器之间的投影关系，如下式所示：

$M=\underset{i}{\mathrm{\Σ}}(l_{r,x}^i-\stackrel{\‾}{l_{r,x}})(O_y^i-\stackrel{\‾}{O_y})-(l_{r,y}^i-\stackrel{\‾}{l_{r,y}})(O_x^i-\stackrel{\‾}{O_x})$

$N=\underset{i}{\mathrm{\Σ}}(l_{r,x}^i-\stackrel{\‾}{l_{r,x}})(O_x^i-\stackrel{\‾}{O_x})-(l_{r,y}^i-\stackrel{\‾}{l_{r,y}})(O_y^i-\stackrel{\‾}{O_y})$

u_θ=arctan(M/N)

$u_x=\stackrel{\‾}{O_x}-\stackrel{\‾}{l_{r,x}}\cos \left(u_{\mathrm{\θ}}\right)+\stackrel{\‾}{l_{r,y}}\sin \left(u_{\mathrm{\θ}}\right)$

$u_y=\stackrel{\‾}{O_y}-\stackrel{\‾}{l_{r,x}}\sin \left(u_{\mathrm{\θ}}\right)+\stackrel{\‾}{l_{r,y}}\cos \left(u_{\mathrm{\θ}}\right)$

式中

为图像数据中特征坐标，

为激光雷达特征坐标，

为所有图像数据特征的位置平均值，

为所有激光雷达特征的位置平均值，M和N为根据特征对应关系计算得到的方向矢量，u_θ为根据关联关系计算得到的激光雷达与图像传感器之间沿Z轴的旋转角度，u_x和u_y分别为根据关联关系计算得到的激光雷达与图像传感器在X轴、Y轴方向上的距离，在Z方向的距离在一体化单元设计时已确定，为已知量，X轴向旋转角始终保持为0，Y轴向旋转角度为单目视觉传感器的朝向角由控制系统决定，为已知量；

（14）由步骤（11）中获得激光雷达与单目视觉传感器之间的实时投影关系，利用一维线性插值降低激光雷达数据的数据粒度，从而降低对环境的离散化程度，如下式所示：

$y=y_0+(y_1-y_0)\frac{x-x_0}{x_1-x_0}$

式中(x₀,y₀)与(x₁,y₁)为已有激光雷达数据观测点位置，(x,y)为插入(x₀,y₀)与(x₁,y₁)之间的激光雷达点位置，迭代进行该过程，从而完成任意精细化程度的插值，获得对环境描述程度精细的三维激光雷达数据点云；

（15）由步骤（14）对激光雷达数据精细化产生数据点云，利用二维插值方法，从图像数据中获得数据点云中点的彩色信息，进而产生彩色三维激光雷达数据点云，如下式所示：

$f(x,y)\≈\frac{f\left(Q_{11}\right)}{(x_2-x_1)(y_2-y_1)}(x_2-x)(y_2-y)\frac{f\left(Q_{21}\right)}{(x_2-x_1)(y_2-y_1)}(x-x_1)(y_2-y)+$

$\frac{f\left(Q_{12}\right)}{(x_2-x_1)(y_2-y_1)}(x_2-x)(y-y_1)+\frac{f\left(Q_{22}\right)}{(x_2-x_1)(y_2-y_1)}(x-x_1)(y-y_1)$

$=\frac{1}{(x_2-x_1)(y_2-y_1)}\left(f\left(Q_{11}\right)(x_2-x)(y_2-y)\right)+f\left(Q_{21}\right)(x-x_1)(y_2-y)+$

$f\left(Q_{12}\right)(x_2-x)(y-y_1)+f\left(Q_{22}\right)(x-x_1)(y-y_1))$

$f\left(R_1\right)\≈\frac{x_2-x}{x_2-x_1}f\left(Q_{11}\right)+\frac{x-x_1}{x_2-x_1}f\left(Q_{21}\right)$

R₁=(x,y₁)

$f\left(R_2\right)\≈\frac{x_2-x}{x_2-x_1}f\left(Q_{12}\right)+\frac{x-x_1}{x_2-x_1}f\left(Q_{22}\right)$

R₂=(x,y₂)

$f\left(P\right)\≈\frac{y_2-y}{y_2-y_1}f\left(R_1\right)+\frac{y-y_1}{y_2-y_1}f\left(R_2\right)$

式中f(·)为数据点色彩信息函数，Q₁₁=(x₁,y₁)、Q₁₂=(x₁,y₂)、Q₂₁=(x₂,y₁)、Q₂₂=(x₂,y₂)分别为插值点上下左右四个方向的相邻数据点，(x₁,y₁)、(x₁,y₂)、(x₂,y₁)和(x₂,y₂)分别为插值点上下左右四个方向的相邻数据点的坐标，f(R₁)和f(R₂)分别为在X方向的y₁和y₂处线性插值结果，f(P)为Y方向线性插值结果，R₁为插值点上方数据点，R₂为插值点下方数据点。

2.根据权利要求1所述的一体化三维彩色激光雷达数据点云产生方法，其特征在于：步骤（2）中，所得二维激光雷达单次扫描数据为X-Y平面上的一组离散的点，在三维空间中的扫描为具有不同Z坐标值的多组离散的点，其空间分布表现为离散点构成的三维网格。

3.根据权利要求1所述的一体化三维彩色激光雷达数据点云产生方法，其特征在于：步骤（7）中，α的取值范围为0.5～2，对于观测误差较大的激光雷达，α取值应适当偏大，对观测误差较小的激光雷达，α取值适当偏小。

4.一体化三维彩色激光雷达数据点云产生装置，包括固定平台、惯性导航数据测量传感器、单目视觉传感单元、二维激光雷达单元，其特征在于：所述的惯性导航数据测量单元设置在固定平台前端，所述的二维激光雷达单元设置在惯性导航数据测量单元上方，所述的单目视觉传感单元设置在固定平台后端。

5.根据权利要求4所述的一体化三维彩色激光雷达数据点云产生装置，其特征在于：所述的二维激光雷达单元包括二维激光雷达、转轴和驱动舵机。

6.根据权利要求5所述的二维激光雷达单元，其特征在于：所述的二维激光雷达可由驱动舵机驱动在平衡位置上下±25°范围内非匀速摆动。

7.根据权利要求4所述的一体化三维彩色激光雷达数据点云产生装置，其特征在于：所述的单目视觉传感单元包括单目视觉传感器、转轴和驱动舵机。

8.根据权利要求7所述的单目视觉传感单元，其特征在于：所述的单目视觉传感器由驱动舵机驱动在平衡位置左右±60°范围内根据应用要求转动。

9.根据权利要求4所述的一体化三维彩色激光雷达数据点云产生装置，其特征在于：所述的单目视觉传感器和二维激光雷达均在平衡位置时，单目视觉传感器、二维激光雷达和惯性导航数据测量单元三者所在的X-Z平面重合。

Images