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

权利要求书

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

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

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

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

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

R=cosθcosψsinφsinθcosψ-cosφsinψsinφsinψ+cosφsinθcosψΔxcosθsinψcosφcosψ+sinφsinθsinψcosφsinθsinψ-sinφcosψΔy-sinθsinφcosθcosφcosθΔz0001]]>

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

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

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

χ2=δiTΣi-1δi+α(θi-θi-1)2---(i)]]>

式中δ_i为观测数据点i沿观测方向平移距离，Σ_i为激光雷达测距模型中的噪声方差，为产生的滤波成本，α为平滑性倾向权值，θ_i为第i个观测点与第i-1个观测点及第i+1个观测点之间构成的夹角，χ²为由于平滑观测点所产生的卡方分布（χ²）值；

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

A=Σivi,x2vi,xvi,yvi,xvi,yvi,y2---(ii)]]>

其中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

Lxy=-1/401/40001/40-1/4*L]]>

Lyy=1-21*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）中提取的拐点，构建数据关联成本函数，如下式所示：

CHt=IHt,s(J1PtJ1T+J2PutJ2T)-1IHt,sT+IHt,oV-1IHt,ot---(iv)]]>

J1=∂f′∂xt]]>    J2=∂f′∂xt]]>

式中是视觉特征估计位置与关联位置之间的距离（新息），是激光雷达特征估计位置与关联位置之间的距离（新息），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=Σi(lr,xi-lr,x‾)(Oyi-Oy‾)-(lr,yi-lr,y‾)(Oxi-Ox‾)]]>

N=Σi(lr,xi-lr,x‾)(Oxi-Ox‾)-(lr,yi-lr,y‾)(Oyi-Oy‾)]]>

u_θ=arctan(M/N)

ux=Ox‾-lr,x‾cos(uθ)+lr,y‾sin(uθ)]]>

uy=Oy‾-lr,x‾sin(uθ)+lr,y‾cos(uθ)]]>

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

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

y=y0+(y1-y0)x-x0x1-x0]]>

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

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

f(x,y)≈f(Q11)(x2-x1)(y2-y1)(x2-x)(y2-y)f(Q21)(x2-x1)(y2-y1)(x-x1)(y2-y)+]]>

f(Q12)(x2-x1)(y2-y1)(x2-x)(y-y1)+f(Q22)(x2-x1)(y2-y1)(x-x1)(y-y1)]]>

=1(x2-x1)(y2-y1)(f(Q11)(x2-x)(y2-y))+f(Q21)(x-x1)(y2-y)+]]>

f(Q12)(x2-x)(y-y1)+f(Q22)(x-x1)(y-y1))]]>

f(R1)≈x2-xx2-x1f(Q11)+x-x1x2-x1f(Q21)]]>

R₁=(x,y₁)

f(R2)≈x2-xx2-x1f(Q12)+x-x1x2-x1f(Q22)]]>

R₂=(x,y₂)

f(P)≈y2-yy2-y1f(R1)+y-y1y2-y1f(R2)]]>

式中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₂为插值点下方数据点。