CN110554407B - 一种模拟船用激光雷达三维点云成像方法 - Google Patents

Abstract

本发明公开了一种模拟船用激光雷达三维点云成像方法，首先设置场景属性，模拟场景的各类初始状态信息；其次通过在线和离线两种方式实现场景的构建工作，用以生成基本的初始状态信息，该信息将构成后续实际计算的数据来源；再次，解算扫描物标三角面的结果数据，并通过物标点云结果的判定与合成，生成最后的扫描结果点云信息；最后，利用OpenGL技术，实现激光雷达扫描效果的可视化，提供三维可视信息。本发明可设置激光雷达的不同参数以及目标不同的距离方位等，可灵活构建激光雷达工作环境；可根据仿真工况，保存为规定格式的三维点云文件，便于其他软件使用，并增加了激光雷达扫描效果的可视化信息，便于实验人员观测。

Description

一种模拟船用激光雷达三维点云成像方法

技术领域

本发明涉及一种成像方法，特别是一种模拟船用激光雷达三维点云成像方法，属于通信信息模拟技术领域。

背景技术

激光雷达通过发射激光束，经过障碍物反射后，接收返回的激光束，用以判断目标的距离、方位、速度等信息，最终利用三维点云的方式加以反映。船用激光雷达主要功能是建立以本船为中心一定范围内的陆地、目标船、浮标等物标的三维点云，为本船提供一种在三维空间精准定位障碍物的信息。船用激光雷达三维点云信息在船舶领域，是一种十分重要的信息，一方面，三维点云信息可为船舶海上目标识别算法(船舶自主航行的核心算法之一)提供重要的训练样本集，另一方面，三维点云是开展船舶航行信息融合的基础信息源之一。因此获得船用激光雷达输出的三维点云信息，具有重要的理论研究和工程实践意义。

但是，在一般情况下，只有在船舶安装激光雷达等硬件设施、且船舶行驶在特定的海域情况下，才可获得预期的三维点云数据，即使在该前提下，也需要利用较长时间来获得不同海况下的点云成像数据。利用虚拟仿真方式模拟激光雷达的三维点云成像，可快速获得不同海况、不同扫描目标数量三维点云成像信息，大大降低连接激光雷达硬件带来的人力和物力上的投入。在激光雷达仿真方面，一些研究人员做过类似的研究工作，如张爱武等人在文献《移动激光雷达的瞬时三维构像方法》中，研究了一种处于移动状态的激光雷达的成像方法，该方法侧重于对目标的特征提取和目标识别，缺少对三维点云成像原理和三维重现的论述；许志勇等人在文献《基于激光雷达点云的滤波算法研究》中，着重研究了对点云信息滤波过程，并未从描述如何获取和模拟点云信息。其他学者多采用类似的方法开展激光雷达三维点云模拟成像的研究。因此，前人的研究存在如下问题：一方面，没有从机理角度出发，研究激光束与目标中各面的反射与接收处理原理，故并不能提供一种原理可靠的三维点云模拟过程；另一方面，前人的激光雷达点云研究成果，很难快速灵活地提供不同工况、不同扫描目标数量、不同可识别率条件下的点云信息，故并不能提供可直接用于船舶航行信息融合或目标识别的训练样本集。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种可快速获得不同海况、不同扫描目标数量三维点云成像信息的模拟船用激光雷达三维点云成像方法。

为解决上述问题，本发明的一种模拟船用激光雷达三维点云成像方法，包括以下步骤：

步骤1:设定场景中物标属性及被模拟的激光雷达的参数，建立坐标系：场景中的物标模型由三维建模软件建立，物标模型的表面由三角面拼接而成；物标属性包括物标名称、由物标表面的三角面的顶点三维位置信息构成的动态数组、物标所在的三维空间位置、物标的姿态角信息、物标表面的三角面颜色值；被模拟的激光雷达的参数包括：水平和垂直扫描角度范围、横向和纵向像素分辨率、伺服基座水平角度和俯仰角度、雷达相对于船舶质心安装的位置和姿态角和扫描结果点云的深度值；

坐标系包括：

(1)世界坐标系o_gx_gy_gz_g：原点o_g固定在海平面上一点，x_g指向北方，y_g指向东方，本专利中选择经度0°、纬度0°作为世界坐标系原点；

(2)船舶本体系o_sx_sy_sz_s：原点o_s位于船舶重心，x_s指向船首，y_s指向船舶右舷；

(3)雷达基座系o_rbx_rby_rbz_rb：原点o_rb位于基座矩形中心，x_rb和y_rb与船舶本体系中x_s和y_s方向相同；

(4)雷达伺服系o_rsx_rsy_rsz_rs：原点o_rs位于激光雷达伺服结构中心点，当伺服机构横摇和纵摇角度均为0°时，x_rs和y_rs方向与船舶本体系中x_s和y_s方向相同；

(5)扫描线坐标系o_rlx_rly_rlz_rl：是由雷达伺服系o_rsx_rsy_rsz_rs围绕z_rs轴旋转，被选择激光光束在x_rsy_rs平面的象限角而得到；

(6)三角面坐标系o_tx_ty_tz_t如下：设三角面的顶点为P_al、P_bl、P_cl，以P_al为原点，P_alP_bl边为x_t轴，P_alP_bl和P_alP_cl两条边的法向量为z_t轴，与x_t和y_t轴正交轴为y_t轴；

步骤2：在线设置航行场景状态或加载离线模型文件，初始化船舶航行场景，所述在线设置航行场景包括：在航行场景中生成全部种类的物标，包括船舶和港口，并分配唯一标识值，设定各物标的位置、姿态；

所述加载离线模型文件包括：加载离线文件中定义物标类型、物标在世界坐标系下的位置、航向、航速；每个离线文件表示仿真过程推进一帧的状态值，众多连续的离线文件构成一个航次的仿真试验；

步骤3：模拟激光光束扫描实体物标，计算生成被扫描点的空间数据；

步骤4：激光雷达单帧推进生成点云文件；

步骤5：模拟扫描结果的可视化：解读步骤3生产的点云二进制文件，读取雷达扫描区域水平和垂直扫描范围、扫描点的三维位置、颜色和质量，采用三维场景显示技术绘制激光雷达三维点云数据。

本发明还包括：

1.步骤3模拟激光光束扫描实体物标，计算生成被扫描点的空间数据包括以下步骤：

S1：构成目标的三角面的形心坐标变换处理：

设任意一个三角面T_target顶点在目标船本体坐标系下坐标为P_ab＝(P_abx,P_aby,P_abz)，P_bb＝(P_bbx,P_bby,P_bbz)，P_cb＝(P_cbx,P_cby,P_cbz)，三角面形心P_cenb＝(P_cenbx,P_cenby,P_cenbz)满足：

将上式所示形心变换至雷达伺服系o_rsx_rsy_rsz_rs下得到三维坐标P_cens＝(P_censx,P_censy,P_censz)，三角面形心的水平象限角θ_Hcens及到原点o_rs的水平距离R_Hcens满足：

S2：解算扫描线扫描区域的矩形边界：

在雷达伺服坐标系和扫描线坐标系中，激光雷达扫描线穿过三角面T_target形心，T_target的顶点P_ab、P_bb、P_cb变换至扫描线坐标系o_rlx_rly_rlz_rl下的坐标为P_al＝(P_alx,P_aly,P_alz)、P_bl＝(P_blx,P_bly,P_blz)、P_cl＝(P_clx,P_cly,P_clz)，以及投影到扫描线坐标系x_rly_rl平面的顶点坐标

T_target的各顶点在扫描线坐标系x_rly_rl平面的水平象限角θ_HA、θ_HB、θ_HC、θ_ZA、θ_ZB、θ_ZC满足：

在世界坐标系下，T_target各顶点在水平面内的横向最小角θ_Hmin和最大角θ_Hmax，以及在垂直面内的纵向最小角θ_Zmin和最大角θ_Zmax满足：

设激光雷达水平扫描角度范围为θ_HScale，垂直扫描角度范围为θ_ZScale，横向像素角分辨间隔为θ_HPix，纵向像素角分辨间隔为θ_ZPix，得到三角面四个边界的像素索引号N_LIndex、N_RIndex、N_UIndex、N_BIndex满足：

以上像素边界取整数后，N_LIndex与0比较取大值，N_UIndex与0比较取大值，N_RIndex与θ_HScale比较取小值，N_BIndex和θ_ZScale比较取小值，以上取值作为最终的像素边界值；

S3：将三角面顶点变换到自身平面坐标系：

T_target的顶点P_ab、P_bb、P_cb经扫描线坐标系变换到三角面坐标系下的坐标为P_at＝(P_atx,P_aty,P_atz)、P_bt＝(P_btx,P_bty,P_btz)、P_ct＝(P_ctx,P_cty,P_ctz)；

S4：遍历所有激光雷达扫描线，解算该扫描线的最终扫描点：

根据激光雷达参数设置，得到扫描线角度矩形范围：

将激光雷达扫描角度矩形范围按照水平和垂直方向分别划分为θ_h和θ_z份，则激光雷达扫描区域的横向和纵向遍历角θ_HAng和θ_ZAng满足：

设激光雷达扫描距离为R_LiDAR，则该扫描线在雷达伺服坐标系下的起点坐标为P_{LineSt_s}＝(0,0,0)，终点坐标P_{LineEnd_s}满足：

根据S2中得到的所有三角面四个边界的像素索引号，与扫描线扫描矩阵的位置建立联系，找到每根扫描线穿过的所有三角面，然后将扫描线起点和终点坐标变换至找到的各个三角面坐标系下，即起点和终点的坐标为：P_{LineSt_t}＝(P_{LineSt_tx},P_{LineSt_ty},P_{LineSt_tz})，P_{LineEnd_t}＝(P_{LineEnd_tx},P_{LineEnd_ty},P_{LineEnd_tz})，该扫描线与三角面坐标系x_ty_t平面的交点在该坐标系下的坐标P_iner＝(P_inerx,P_inery,P_inerz)满足：

采用向量叉乘求取法向量的方式判断P_iner点是否在三角形T_target内，具体步骤如下：

(1)在三角面坐标系内，以三角形的一条边P_atP_bt为例，建立向量

和

求解两个向量的法向量坐标为(p_a1,p_a2,p_a3)：

(2)按照相同方法，分别求取

和

以及

和

的法向量(p_b1,p_b2,p_b3)和(p_c1,p_c2,p_c3)；

(3)如果p_a3、p_b3、p_c3均为正值或均为负值，则表示P_iner点在三角形T_target内，其他情况则表示P_iner点不在三角形T_target内；

如果点P_iner在三角形T_target内部，则表示扫描线检测到目标物标三角面，此时求解该扫描线检测到三角面的距离D_TScan：

当扫描线与两个及其以上三角面相交时，取最小的扫描距离D_TScan，当更新所有扫描距离后，得到该帧扫描矩形范围的目标物标所有三角面检测结果；

2.步骤4激光雷达单帧推进生成点云文件具体包括：

S1：定义三维点云数据结构：

三维点云数据包括激光雷达扫描区域水平和垂直扫描范围、点云三维位置、点云颜色和点的质量，点云颜色由步骤1的物标属性决定，点的质量Q_scan包括0或者1两种情况，激光雷达扫描精度半径D_prec取值按照下式计算：

S2：求解扫描点在世界坐标系下三维坐标值：

根据计算的扫描点距离D_TScan以及激光雷达水平扫描角度θ_HAng和垂直扫描角度θ_ZAng，扫描点在世界坐标系下的坐标P_poscan＝(P_poscanx,P_poscany,P_poscanz)满足：

S3：根据雷达扫描区域水平和垂直扫描范围、扫描到的目标点的三维位置和颜色以及扫描点的质量，将以上数据写入二进制文件，写入顺序为：首先将激光雷达的水平和垂直扫描范围写入文件，其次，将整个扫描区域所有扫描点的位置、颜色、质量依次全部写入文件。

3.步骤5中增加白色辅助圆圈轮廓。

4.步骤5中当一个目标物标点云质量数小于设定阈值时不显示。

5.步骤5中不显示辅助圆圈轮廓，显示结果以黑色为背景。

本发明有益效果：本专利提出一种模拟船用激光雷达三维点云成像的方法，该方法以三角面形式构建船舶航行环境下的各类目标，通过模拟扫描线检测三角面的复杂计算方式，用以模拟真实激光束投射到物体表面的检测效果，本专利可替代利用真实船用激光雷达来获取实际三维点云，在不使用真实雷达的基础上，可模拟不同的仿真工况，利用计算机就可以快速获得所需的模拟样本数据。本专利提出快速匹配扫描线与三角形的方法，提高了计算速度；本专利提出的方法可设置激光雷达的不同参数以及目标不同的距离方位等，可灵活构建激光雷达工作环境；本专利提出的方法可根据仿真工况，保存为规定格式的三维点云文件，便于其他软件使用，并增加了激光雷达扫描效果的可视化信息，便于实验人员观测。附图说明

图1是回波成像计算流程图；

图2是涉及的坐标系示意图；

图3船舶航行场景初始化方式示意图；

图4是激光雷达扫描目标三角面示意图；

图5是激光雷达扫描目标船模拟计算流程图；

图6是三角面形心坐标变换原理图；

图7是扫描线坐标矩形边界计算原理；

图8是遍历变换后扫描线示意图；

图9是遍历到的扫描线与三角形交点示意图；

图10是含有辅助圆圈轮廓的点云显示效果

图11是滤波后的目标船点云显示效果

图12是不同颜色三角面显示效果

具体实施方式

下面结合附图对本发明具体实施方式做进一步说明。

本专利提供一种模拟船用激光雷达三维点云成像方法，设定仿真场景中所有物标由数量众多的三角面拼接而成，并且设定不同初始仿真场景，通过模拟实际环境下的激光雷达扫描线形式，检测扫描线与构成模型的三角面的关系形成目标三维成像效果，并可输出标准的三维点云文件。本专利船用激光雷达三维点云成像计算流程如图1所示。

如图1所示，本专利首先设置场景属性，包络激光雷达的基本扫描属性(水平和垂直扫描角度等)、构成实体物标的三角面顶点信息(该三角面可由相关建模软件提供)、基本的坐标系及其相互变换等，由此模拟场景的各类初始状态信息；然后开展在线和离线场景构建工作，用以生成基本的初始状态信息，该信息将构成后续实际计算的数据来源；然后模拟激光光束扫描物标，解算被扫描物标空间信息，通过复杂的算法计算，解算扫描物标三角面的结果数据；然后通过物标点云判定与合成，生成最后的扫描结果点云信息；最后，利用OpenGL技术，实现模拟扫描效果的可视化，提供三维可视信息。

本发明包括以下步骤：

步骤1：设定场景中物标属性及激光雷达基本参数

本专利中除了本船外的其他所有物标(船舶、浮标、港口等)的表面均由众多三角面拼接而成，物标模型可利用三维建模软件建立，如3DMax、Creator等，经过建立后的三维模型可直接提供构成模型的所有三角面的顶点坐标信息，本专利定义物标属性包括：物标名称、物标表面的三角面动态数组、物标的位置和姿态、物标的颜色。本专利定义激光雷达参数设置包括：水平、垂直扫描角度范围、横纵向像素分辨率、伺服基座水平角度和俯仰角度、雷达相对于船舶质心安装的位置和姿态角、扫描结果点云的深度值。

本专利中激光扫描雷达安装在船舶的桅杆上，桅杆顶部有一个矩形基座，基座上部安装有用于控制激光雷达横摇和纵摇的伺服机构。为此，本专利定义模拟激光雷达扫描生成三维点云过程，涉及到的坐标系如下所示，所有坐标系的坐标轴方向均满足右手定则：

(1)世界坐标系o_gx_gy_gz_g：原点o_g固定在海平面上一点，x_g指向北方，y_g指向东方，本专利中选择经度0°、纬度0°作为世界坐标系原点。

(2)船舶本体系o_sx_sy_sz_s：原点o_s位于船舶重心，x_s指向船首，y_s指向船舶右舷。

(3)雷达基座系o_rbx_rby_rbz_rb：原点o_rb位于基座矩形中心，x_rb和y_rb与船舶本体系中x_s和y_s方向相同。

(4)雷达伺服系o_rsx_rsy_rsz_rs：原点o_rs位于激光雷达伺服结构中心点，当伺服机构横摇和纵摇角度均为0°时，x_rs和y_rs方向与船舶本体系中x_s和y_s方向相同。

(5)扫描线坐标系o_rlx_rly_rlz_rl：是由雷达伺服系o_rsx_rsy_rsz_rs围绕z_rs轴旋转，被选择激光光束在x_rsy_rs平面的象限角而得到。

(6)三角面坐标系o_tx_ty_tz_t如下：设三角面的顶点为P_al、P_bl、P_cl，以P_al为原点，P_alP_bl边为x_t轴，P_alP_bl和P_alP_cl两条边的法向量为z_t轴，与x_t和y_t轴正交轴为y_t轴。

其中，激光雷达扫描线坐标系和三角面坐标系，是本专利特有的。

步骤2：在线设置航行场景状态与加载离线模型文件

本专利中航行场景采用在线设置和加载离线文件两种方式，两种方式共同作用，实现初始化船舶航行场景的工作。

(1)在线设置方式由系统后台自定义，在航行场景中生成船舶和港口等物标，并分配唯一标识值，设定各物标的位置、姿态等。考虑到模拟激光雷达点云成像的场景需要初始存在各类物标信息，以上信息的三角面表示数量巨大，在仿真初始阶段加载完成。

(2)加载离线文件中定义物标类型、物标在世界坐标系下的位置、航向、航速等。每个离线文件表示仿真过程推进一帧的状态值，众多连续的离线文件构成一个航次的仿真试验。本专利增加加载离线文件的初始化航行场景的方式，该离线文件需要采用固定的数据格式。

本专利中在线设置和加载离线文件，模拟总线插拔方式确定是否启动：以上两种方式中任意一种，加入到仿真数据链路中时，自动启动加载程序来初始化场景；当启动在线设置后，如再加载离线文件，离线文件中物标与在线设置的物标类型重复时，本专利定义判断机制，取在线设置参数和离线文件参数的交集，作为航行场景初始化阶段的初始参数。

步骤3：模拟激光光束扫描实体物标，计算生成被扫描点的空间数据

设一个三角面T_target，计算三角面形心变换至雷达伺服系o_rsx_rsy_rsz_rs下得到三维坐标P_cens＝(P_censx,P_censy,P_censz)，三角面形心的水平象限角θ_Hcens及到原点o_rs的水平距离R_Hcens为：

在雷达伺服坐标系和扫描线坐标系中，激光雷达扫描线穿过三角面T_target形心，T_target的顶点P_ab、P_bb、P_cb变换至扫描线坐标系o_rlx_rly_rlz_rl下的坐标为P_al＝(P_alx,P_aly,P_alz)、P_bl＝(P_blx,P_bly,P_blz)、P_cl＝(P_clx,P_cly,P_clz)，以及投影到扫描线坐标系x_rly_rl平面的顶点坐标

计算T_target的各顶点在扫描线坐标系x_rly_rl平面的水平象限角θ_HA、θ_HB、θ_HC、θ_ZA、θ_ZB、θ_ZC：

在世界坐标系下，T_target各顶点在水平面内的横向最小角θ_Hmin和最大角θ_Hmax，以及在垂直面内的纵向最小角θ_Zmin和最大角θ_Zmax，由下式计算：

设激光雷达水平扫描角度范围为θ_HScale，垂直扫描角度范围为θ_ZScale，横向像素角分辨间隔为θ_HPix，纵向像素角分辨间隔为θ_ZPix，按照下式得到该三角面四个边界的像素索引号N_LIndex、N_RIndex、N_UIndex、N_BIndex：

以上像素边界取整数后，N_LIndex和N_UIndex分别与0比较取大值，N_RIndex和N_BIndex分别与θ_HScale和θ_ZScale比较取小值，作为最终的像素边界值。将激光雷达扫描角度矩形范围按照水平和垂直方向分别划分为θ_h和θ_z份，为了遍历所有扫描线，激光雷达扫描区域的横向和纵向遍历角θ_HAng和θ_ZAng如下式计算：

根据得到的所有三角面四个边界的像素索引号，与扫描线扫描矩阵的位置建立联系，可快速找到每根扫描线穿过的所有三角面列表，然后将扫描线起点和终点坐标变换至找到的各个三角面坐标系下，即起点和终点的坐标为：P_{LineSt_t}＝(P_{LineSt_tx},P_{LineSt_ty},P_{LineSt_tz})，P_{LineEnd_t}＝(P_{LineEnd_tx},P_{LineEnd_ty},P_{LineEnd_tz})，设激光雷达扫描距离为R_LiDAR，根据下式求解该扫描线与三角面坐标系x_ty_t平面的交点在该坐标系下的坐标P_iner＝(P_inerx,P_inery,P_inerz)：

T_target的顶点P_ab、P_bb、P_cb变换至o_tx_ty_tz_t坐标系下的坐标为P_at＝(P_atx,P_aty,P_atz)、P_bt＝(P_btx,P_bty,P_btz)、P_ct＝(P_ctx,P_cty,P_ctz)。在三角形坐标系内，分别计算

和

和

以及

和

的法向量(p_a1,p_a2,p_a3)(p_b1,p_b2,p_b3)和(p_c1,p_c2,p_c3)。如果p_a3、p_b3、p_c3均为正值或均为负值，则表示P_iner点在三角形T_target内，其他情况则表示P_iner点不在三角形T_target内。如果点P_iner三角形T_target内部，则表示扫描线检测到目标船的三角面，此时求解该扫描线检测到三角面的距离D_TScan，当扫描线与两个及其以上三角面相交时，取最小的扫描距离D_TScan，当更新所有扫描距离后，可得到该帧扫描矩形范围的所有物标中三角面检测结果。

步骤4：激光雷达单帧推进生成点云文件

本专利针对港口模型，只计算一次模型三角面数据并存储其扫描数据，该数据在激光雷达扫描其他物标的过程中不参与计算，仅在最后通过坐标变换更新港口扫描数据。

为了能够将激光雷达扫描结果保存并为其他软件使用，本专利提供一种将激光雷达扫描结果保存为三维点云文件的方式。三维点云数据包括激光雷达扫描区域水平和垂直扫描范围、点云三维位置、点云颜色和点的质量，点云颜色由前面描述的物标属性决定，点的质量Q_scan包括0或者1两种情况，本专利定义激光雷达扫描精度半径D_prec，则Q_scan的取值按照下式计算：

根据计算的扫描点距离D_TScan以及激光雷达水平扫描角度θ_HAng和垂直扫描角度θ_ZAng，扫描点在世界坐标系下的坐标P_poscan＝(P_poscanx,P_poscany,P_poscanz)，由下式计算：

根据雷达扫描区域水平和垂直扫描范围、扫描到目标点的三维位置和颜色以及点的质量，将以上数据写入二进制文件，写入顺序为：首先将激光雷达的水平和垂直扫描范围写入文件，其次，将整个扫描区域所有扫描点的位置、颜色、质量依次全部写入文件。

步骤5：模拟扫描结果的可视化

本专利在保存激光雷达点云文件的基础上，设计解读点云二进制文件的功能，读取雷达扫描区域水平和垂直扫描范围、扫描点的三维位置、颜色和质量，在此基础上，采用OpenGL三维场景显示技术绘制激光雷达三维点云数据。

本专利在显示激光点云时，增加白色辅助圆圈轮廓，提高三维显示立体感，为了突出目标船的对比度，本专利在点云显示环节增加滤波功能，滤波原理是当一个目标船点云质量数小于一定值即不显示，本专利定义该阈值为1000，并且在该模式下不显示辅助圆圈轮廓，显示结果以黑色为背景。本专利中扫描结果的点云数据含有颜色信息，显示点云可以切换到扫描颜色模式，更加直观查看三角面的密集程度。

本发明具体实施方式还包括：

1、设定场景中物标属性及激光雷达基本参数

本专利中的三维场景，除了本船外的其他所有物标(船舶、浮标、港口等)的表面均由众多三角面拼接而成，物标模型可利用三维建模软件建立，如3DMax、Creator等，经过建立后的三维模型可直接提供构成模型的所有三角面的顶点坐标信息，本专利定义物标属性包括：

(1)物标名称：用于区别不同物标的唯一标识，本专利中用汉字及字母表示该标识，如“大型游轮A”、“小型快艇”等；

(2)物标表面的三角面动态数组：本专利中所有的物标均由三角面拼接而成，专利中采用动态数组存储所有三角面的顶点三维位置信息；

(3)物标的位置和姿态：用于描述物标所在的三维空间位置和物标的姿态角信息；

(4)物标的颜色：本专利定义物标表面的三角面颜色值，包括RGB三个值表示，该属性可为后续可视化增加直观逼真效果。

本专利定义激光雷达参数设置：

(1)水平、垂直扫描角度范围；

(2)横向、纵向像素分辨率；

(3)伺服基座水平角度、俯仰角度；

(4)雷达相对于船舶质心安装的位置和姿态角；

(5)扫描结果点云的深度值：存储每根扫描线检测到三角面的距离值。

本专利中激光扫描雷达安装在船舶的桅杆上，桅杆顶部有一个矩形基座，基座上部安装有用于控制激光雷达横摇和纵摇的伺服机构。为此，本专利定义模拟激光雷达扫描生成三维点云过程，涉及到的坐标系示意图如图2所示，所有坐标系的坐标轴方向均满足右手定则：

(1)世界坐标系o_gx_gy_gz_g：原点o_g固定在海平面上一点，x_g指向北方，y_g指向东方，本专利中选择经度0°、纬度0°作为世界坐标系原点。

(2)船舶本体系o_sx_sy_sz_s：原点o_s位于船舶重心，x_s指向船首，y_s指向船舶右舷。

(3)雷达基座系o_rbx_rby_rbz_rb：原点o_rb位于基座矩形中心，x_rb和y_rb与船舶本体系中x_s和y_s方向相同。

(4)雷达伺服系o_rsx_rsy_rsz_rs：原点o_rs位于激光雷达伺服结构中心点，当伺服机构横摇和纵摇角度均为0°时，x_rs和y_rs方向与船舶本体系中x_s和y_s方向相同。

(5)扫描线坐标系o_rlx_rly_rlz_rl：是由雷达伺服系o_rsx_rsy_rsz_rs围绕z_rs轴旋转，被选择激光光束在x_rsy_rs平面的象限角而得到。

(6)三角面坐标系o_tx_ty_tz_t如下：设三角面的顶点为P_al、P_bl、P_cl，以P_al为原点，P_alP_bl边为x_t轴，P_alP_bl和P_alP_cl两条边的法向量为z_t轴，与x_t和y_t轴正交轴为y_t轴。

2、在线设置航行场景状态与加载离线模型文件

本专利中航行场景采用在线设置和加载离线文件两种方式，两种方式共同作用，实现初始化船舶航行场景的工作。

(1)在线设置方式由系统后台自定义，在航行场景中生成船舶和港口等物标，并分配唯一标识值，设定各物标的位置、姿态等。

考虑到模拟激光雷达点云成像的场景需要初始存在各类物标信息，以上信息的三角面表示数量巨大，在仿真初始阶段加载完成，本专利提出在线设置船舶航行场景方式，该方式可动态设置或修改物标位置、航向、速度等状态信息。

(2)加载离线文件中定义物标类型、物标在世界坐标系下的位置、航向、航速等。每个离线文件表示仿真过程推进一帧的状态值，众多连续的离线文件构成一个航次的仿真试验。

本专利考虑到不同的仿真任务，在在线设置的基础上，增加加载离线文件的初始化航行场景的方式，该离线文件可采用其他方式生成，但是需要采用本专利要求的数据格式，具体数据格式如下：

表1初始化航行场景离线文件格式

物标类型

经度

纬度

高度

位置X

位置Y

位置Z

航向角

航速

集装箱船A

港口

岛屿

浮标

......

本专利中在线设置和加载离线文件，模拟总线插拔方式确定是否启动，初始化方式如图3所示：以上两种方式中任意一种，加入到仿真数据链路中时，自动启动加载程序来初始化场景；当启动在线设置后，如再加载离线文件，离线文件中物标与在线设置的物标类型重复时，本专利定义判断机制，取在线设置参数和离线文件参数的交集，作为航行场景初始化阶段的初始参数。

3、模拟激光光束扫描实体物标，计算生成被扫描点的空间数据

本专利在模拟激光雷达扫描目标过程中，采用模拟激光束扫描检测目标船、港口及其他物标模型的三角面的方式，模拟过程示意图如4所示：

本专利模拟扫描物标流程图如图5所示：本专利的总体思路是遍历构成各个物标的每个三角面，选择一个目标，然后选择该目标的一个三角面，解算该三角面在世界坐标系下外接矩形的像素索引值，在根据激光雷达扫描线角度范围，确定扫描线穿过选定三角面外接矩形的情况(即穿过三角面或未穿过三角面)，然后进一步判断扫描线与被选定三角面交点情况(即交点的空间坐标)。判断是否更新扫描信息，如果需要更新扫描信息条件时，开始缓存激光雷达扫描结果，如果不需要更新扫描信息条件时，则再进一步判断是否遍历完所有扫描线，如果未遍历完所有扫描线，则重新选择下一个三角面，如果已遍历完所有扫描线，则再进一步判断是否遍历所有目标，如未遍历完所有目标，则重新选择下一个目标，如遍历完所有目标，则结束算法。

如流程图所示，具体的实现步骤如下所示：

Step 1：构成目标的三角面的形心坐标变换处理

如图6所示，设其中一个三角面T_target顶点在目标船本体坐标系下坐标为P_ab＝(P_abx,P_aby,P_abz)，P_bb＝(P_bbx,P_bby,P_bbz)，P_cb＝(P_cbx,P_cby,P_cbz)，由下式计算三角面形心P_cenb＝(P_cenbx,P_cenby,P_cenbz)：

将上式所示形心变换至雷达伺服系o_rsx_rsy_rsz_rs下得到三维坐标P_cens＝(P_censx,P_censy,P_censz)，三角面形心的水平象限角θ_Hcens及到原点o_rs的水平距离R_Hcens由下式计算：

Step 2：解算扫描线扫描区域的矩形边界

在如图7所示的雷达伺服坐标系和扫描线坐标系中，激光雷达扫描线穿过三角面T_target形心，T_target的顶点P_ab、P_bb、P_cb变换至扫描线坐标系o_rlx_rly_rlz_rl下的坐标为P_al＝(P_alx,P_aly,P_alz)、P_bl＝(P_blx,P_bly,P_blz)、P_cl＝(P_clx,P_cly,P_clz)，以及投影到扫描线坐标系x_rly_rl平面的顶点坐标

因此，T_target的各顶点在扫描线坐标系x_rly_rl平面的水平象限角θ_HA、θ_HB、θ_HC、θ_ZA、θ_ZB、θ_ZC可由下两式计算：

在世界坐标系下，T_target各顶点在水平面内的横向最小角θ_Hmin和最大角θ_Hmax，以及在垂直面内的纵向最小角θ_Zmin和最大角θ_Zmax，由下式计算：

设激光雷达水平扫描角度范围为θ_HScale，垂直扫描角度范围为θ_ZScale，横向像素角分辨间隔为θ_HPix，纵向像素角分辨间隔为θ_ZPix，得到该三角面四个边界的像素索引号N_LIndex、N_RIndex、N_UIndex、N_BIndex如下公式计算：

注意：以上像素边界取整数后，N_LIndex和N_UIndex分别与0比较取大值，N_RIndex和N_BIndex分别与θ_HScale和θ_ZScale比较取小值，作为最终的像素边界值。

Step 3：将三角面顶点变换到自身平面坐标系

T_target的顶点P_ab、P_bb、P_cb经扫描线坐标系变换到三角面坐标系下的坐标为P_at＝(P_atx,P_aty,P_atz)、P_bt＝(P_btx,P_bty,P_btz)、P_ct＝(P_ctx,P_cty,P_ctz)。

Step 4：遍历所有激光雷达扫描线，解算该扫描线的最终扫描点

根据激光雷达参数设置，可得到如图8所示的扫描线角度矩形范围：

将激光雷达扫描角度矩形范围按照水平和垂直方向分别划分为θ_h和θ_z份，为了遍历所有扫描线，则下图所示激光雷达扫描区域的横向和纵向遍历角θ_HAng和θ_ZAng如下式计算：

设激光雷达扫描距离为R_LiDAR，则该扫描线在雷达伺服坐标系下的起点坐标为P_{LineSt_s}＝(0,0,0)，终点坐标P_{LineEnd_s}由下式计算：

本专利根据Step 2中得到的所有三角面四个边界的像素索引号，与扫描线扫描矩阵的位置建立联系，可快速找到每根扫描线穿过的所有三角面，然后将扫描线起点和终点坐标变换至找到的各个三角面坐标系下，即起点和终点的坐标为：P_{LineSt_t}＝(P_{LineSt_tx},P_{LineSt_ty},P_{LineSt_tz})，P_{LineEnd_t}＝(P_{LineEnd_tx},P_{LineEnd_ty},P_{LineEnd_tz})，根据下式求解该扫描线与三角面坐标系x_ty_t平面的交点在该坐标系下的坐标P_iner＝(P_inerx,P_inery,P_inerz)：

为了判断P_iner点是否在三角形T_target内，需要在三角面坐标系中，需要判断P_iner点与Step 3中计算得到的P_at、P_bt、P_ct三个点的关系，本专利采用向量叉乘求取法向量的方式，具体步骤如下：

(1)在图9所示的三角形坐标系内，以三角形的一条边P_atP_bt为例，建立向量

和

求解两个向量的法向量坐标为(p_a1,p_a2,p_a3)。

(2)按照相同方法，分别求取

和

以及

和

的法向量(p_b1,p_b2,p_b3)和(p_c1,p_c2,p_c3)。

(3)如果p_a3、p_b3、p_c3均为正值或均为负值，则表示P_iner点在三角形T_target内，其他情况则表示P_iner点不在三角形T_target内。

在本专利中，如果点P_iner三角形T_target内部，则表示扫描线检测到目标船的三角面，此时求解该扫描线检测到三角面的距离D_TScan：

当扫描线与两个及其以上三角面相交时，取最小的扫描距离D_TScan，当更新所有扫描距离后，可得到该帧扫描矩形范围的目标船所有三角面检测结果。

4、激光雷达单帧推进生成点云文件

由于构成港口模型的三角面数量庞大，并且港口位置是固定不变的，因此本专利针对港口模型，只计算一次模型三角面数据并存储其扫描数据，该数据在激光雷达扫描其他目标船的过程中不参与计算，仅在最后通过坐标变换更新港口扫描数据。

为了能够将激光雷达扫描结果保存并为其他软件使用，本专利提供一种将激光雷达扫描结果保存为三维点云文件的方式，保存步骤如下：

Step 1：定义三维点云数据结构

三维点云数据包括激光雷达扫描区域水平和垂直扫描范围、点云三维位置、点云颜色和点的质量，点云颜色由前面描述的物标属性决定，点的质量Q_scan包括0或者1两种情况，本专利定义激光雷达扫描精度半径D_prec，则Q_scan的取值按照下式计算：

Step 2：求解扫描点在世界坐标系下三维坐标值

根据计算的扫描点距离D_TScan以及激光雷达水平扫描角度θ_HAng和垂直扫描角度θ_ZAng，扫描点在世界坐标系下的坐标P_poscan＝(P_poscanx,P_poscany,P_poscanz)，由下式计算：

Step 3：根据雷达扫描区域水平和垂直扫描范围、扫描到的目标点的三维位置和颜色以及扫描点的质量，将以上数据写入二进制文件，写入顺序为：首先将激光雷达的水平和垂直扫描范围写入文件，其次，将整个扫描区域所有扫描点的位置、颜色、质量依次全部写入文件。

5、模拟扫描结果的可视化

本专利在保存激光雷达点云文件的基础上，设计解读点云二进制文件的功能，读取雷达扫描区域水平和垂直扫描范围、扫描点的三维位置、颜色和质量，在此基础上，采用OpenGL三维场景显示技术绘制激光雷达三维点云数据。

本专利在显示激光点云时，增加白色辅助圆圈轮廓，提高三维显示立体感，含有辅助圆圈轮廓的点云显示效果如图10所示；

为了突出目标船的对比度，本专利增加滤波功能，滤波原理是当一个目标船点云质量数小于一定值即不显示，本专利定义该阈值为1000，并且在该模式下不显示辅助圆圈轮廓，显示结果以黑色为背景，经过滤波后的目标船点云显示效果如图11所示，小目标自动过滤掉，仅留下大目标。

本专利中扫描结果的点云数据含有颜色信息，因此本专利显示点云可以切换到扫描颜色模式，更加直观查看三角面的密集程度，显示效果如图12所示。

Claims (4)

1.一种模拟船用激光雷达三维点云成像方法，其特征在于，包括以下步骤：

步骤1:设定场景中物标属性及被模拟的激光雷达的参数，建立坐标系：场景中的物标模型由三维建模软件建立，物标模型的表面由三角面拼接而成；物标属性包括物标名称、由物标表面的三角面的顶点三维位置信息构成的动态数组、物标所在的三维空间位置、物标的姿态角信息、物标表面的三角面颜色值；被模拟的激光雷达的参数包括：水平和垂直扫描角度范围、横向和纵向像素分辨率、伺服基座水平角度和俯仰角度、雷达相对于船舶质心安装的位置和姿态角和扫描结果点云的深度值；

坐标系包括：

(1)世界坐标系o_gx_gy_gz_g：原点o_g固定在海平面上一点，x_g指向北方，y_g指向东方，本专利中选择经度0°、纬度0°作为世界坐标系原点；

(2)船舶本体系o_sx_sy_sz_s：原点o_s位于船舶重心，x_s指向船首，y_s指向船舶右舷；

(3)雷达基座系o_rbx_rby_rbz_rb：原点o_rb位于基座矩形中心，x_rb和y_rb与船舶本体系中x_s和y_s方向相同；

(4)雷达伺服系o_rsx_rsy_rsz_rs：原点o_rs位于激光雷达伺服结构中心点，当伺服机构横摇和纵摇角度均为0°时，x_rs和y_rs方向与船舶本体系中x_s和y_s方向相同；

(5)扫描线坐标系o_rlx_rly_rlz_rl：是由雷达伺服系o_rsx_rsy_rsz_rs围绕z_rs轴旋转，被选择激光光束在x_rsy_rs平面的象限角而得到；

(6)三角面坐标系o_tx_ty_tz_t如下：设三角面的顶点为P_al、P_bl、P_cl，以P_al为原点，P_alP_bl边为x_t轴，P_alP_bl和P_alP_cl两条边的法向量为z_t轴，与x_t和y_t轴正交轴为y_t轴；

步骤2：在线设置航行场景状态或加载离线模型文件，初始化船舶航行场景，所述在线设置航行场景包括：在航行场景中生成全部种类的物标，包括船舶和港口，并分配唯一标识值，设定各物标的位置、姿态；

所述加载离线模型文件包括：加载离线文件中定义物标类型、物标在世界坐标系下的位置、航向、航速；每个离线文件表示仿真过程推进一帧的状态值，众多连续的离线文件构成一个航次的仿真试验；

步骤3：模拟激光光束扫描实体物标，计算生成被扫描点的空间数据，具体包括以下步骤：

S1：构成目标的三角面的形心坐标变换处理：

设任意一个三角面T_target顶点在目标船本体坐标系下坐标为P_ab＝(P_abx,P_aby,P_abz)，P_bb＝(P_bbx,P_bby,P_bbz)，P_cb＝(P_cbx,P_cby,P_cbz)，三角面形心P_cenb＝(P_cenbx,P_cenby,P_cenbz)满足：

将上式所示形心变换至雷达伺服系o_rsx_rsy_rsz_rs下得到三维坐标P_cens＝(P_censx,P_censy,P_censz)，三角面形心的水平象限角θ_Hcens及到原点o_rs的水平距离R_Hcens满足：

S2：解算扫描线扫描区域的矩形边界：

在雷达伺服坐标系和扫描线坐标系中，激光雷达扫描线穿过三角面T_target形心，T_target的顶点P_ab、P_bb、P_cb变换至扫描线坐标系o_rlx_rly_rlz_rl下的坐标为P_al＝(P_alx,P_aly,P_alz)、P_bl＝(P_blx,P_bly,P_blz)、P_cl＝(P_clx,P_cly,P_clz)，以及投影到扫描线坐标系x_rly_rl平面的顶点坐标

T_target的各顶点在扫描线坐标系x_rly_rl平面的水平象限角θ_HA、θ_HB、θ_HC、θ_ZA、θ_ZB、θ_ZC满足：

在世界坐标系下，T_target各顶点在水平面内的横向最小角θ_Hmin和最大角θ_Hmax，以及在垂直面内的纵向最小角θ_Zmin和最大角θ_Zmax满足：

设激光雷达水平扫描角度范围为θ_HScale，垂直扫描角度范围为θ_ZScale，横向像素角分辨间隔为θ_HPix，纵向像素角分辨间隔为θ_ZPix，得到三角面四个边界的像素索引号N_LIndex、N_RIndex、N_UIndex、N_BIndex满足：

以上像素边界取整数后，N_LIndex与0比较取大值，N_UIndex与0比较取大值，N_RIndex与θ_HScale比较取小值，N_BIndex和θ_ZScale比较取小值，以上取值作为最终的像素边界值；

S3：将三角面顶点变换到自身平面坐标系：

T_target的顶点P_ab、P_bb、P_cb经扫描线坐标系变换到三角面坐标系下的坐标为P_at＝(P_atx,P_aty,P_atz)、P_bt＝(P_btx,P_bty,P_btz)、P_ct＝(P_ctx,P_cty,P_ctz)；

S4：遍历所有激光雷达扫描线，解算该扫描线的最终扫描点：

根据激光雷达参数设置，得到扫描线角度矩形范围：

将激光雷达扫描角度矩形范围按照水平和垂直方向分别划分为θ_h和θ_z份，则激光雷达扫描区域的横向和纵向遍历角θ_HAng和θ_ZAng满足：

设激光雷达扫描距离为R_LiDAR，则该扫描线在雷达伺服坐标系下的起点坐标为P_{LineSt_s}＝(0,0,0)，终点坐标P_{LineEnd_s}满足：

根据S2中得到的所有三角面四个边界的像素索引号，与扫描线扫描矩阵的位置建立联系，找到每根扫描线穿过的所有三角面，然后将扫描线起点和终点坐标变换至找到的各个三角面坐标系下，即起点和终点的坐标为：P_{LineSt_t}＝(P_{LineSt_tx},P_{LineSt_ty},P_{LineSt_tz})，P_{LineEnd_t}＝(P_{LineEnd_tx},P_{LineEnd_ty},P_{LineEnd_tz})，该扫描线与三角面坐标系x_ty_t平面的交点在该坐标系下的坐标P_iner＝(P_inerx,P_inery,P_inerz)满足：

采用向量叉乘求取法向量的方式判断P_iner点是否在三角形T_target内，具体步骤如下：

(1)在三角面坐标系内，以三角形的一条边P_atP_bt为例，建立向量

和

求解两个向量的法向量坐标为(p_a1,p_a2,p_a3)：

(2)按照相同方法，分别求取

和

以及

和

的法向量(p_b1,p_b2,p_b3)和(p_c1,p_c2,p_c3)；

(3)如果p_a3、p_b3、p_c3均为正值或均为负值，则表示P_iner点在三角形T_target内，其他情况则表示P_iner点不在三角形T_target内；

如果点P_iner在三角形T_target内部，则表示扫描线检测到目标物标三角面，此时求解该扫描线检测到三角面的距离D_TScan：

当扫描线与两个及其以上三角面相交时，取最小的扫描距离D_TScan，当更新所有扫描距离后，得到该帧扫描矩形范围的目标物标所有三角面检测结果；

步骤4：激光雷达单帧推进生成点云文件，具体包括：

S1：定义三维点云数据结构：

三维点云数据包括激光雷达扫描区域水平和垂直扫描范围、点云三维位置、点云颜色和点的质量，点云颜色由步骤1的物标属性决定，点的质量Q_scan包括0或者1两种情况，激光雷达扫描精度半径D_prec取值按照下式计算：

S2：求解扫描点在世界坐标系下三维坐标值：

根据计算的扫描点距离D_TScan以及激光雷达水平扫描角度θ_HAng和垂直扫描角度θ_ZAng，扫描点在世界坐标系下的坐标P_poscan＝(P_poscanx,P_poscany,P_poscanz)满足：

S3：根据雷达扫描区域水平和垂直扫描范围、扫描到的目标点的三维位置和颜色以及扫描点的质量，将以上数据写入二进制文件，写入顺序为：首先将激光雷达的水平和垂直扫描范围写入文件，其次，将整个扫描区域所有扫描点的位置、颜色、质量依次全部写入文件；

步骤5：模拟扫描结果的可视化：解读步骤3生产的点云二进制文件，读取雷达扫描区域水平和垂直扫描范围、扫描点的三维位置、颜色和质量，采用三维场景显示技术绘制激光雷达三维点云数据。

2.根据权利要求1所述的一种模拟船用激光雷达三维点云成像方法，其特征在于：步骤5中增加白色辅助圆圈轮廓。

3.根据权利要求1或2所述的一种模拟船用激光雷达三维点云成像方法，其特征在于：步骤5中当一个目标物标点云质量数小于设定阈值时不显示。

4.根据权利要求3所述的一种模拟船用激光雷达三维点云成像方法，其特征在于：步骤5中不显示辅助圆圈轮廓，显示结果以黑色为背景。

Images