CN111221000B - 基于激光雷达的船舶边界探测方法 - Google Patents

Abstract

本发明公开的基于激光雷达的船舶边界探测方法，利用三个激光雷达探测船舶周边障碍物，通过对三个激光雷达发送的网络数据包进行处理后，船载终端根据每个激光雷达的边界距离最小的扫描点距离船舶中心位置点的距离、方位角、高度、边界距离和接近速度的信息，在船舶的驾驶台的屏幕上显示这些信息，同时判断边界距离是否小于报警门限值，如果边界距离小于报警门限值，则船载终端将屏幕上显示的信息设成红色，同时通过喇叭发出语音报警。本发明船舶边界探测方法能够精确探测船舶周围近距离障碍物，使船员能够及时知道船舶周边可能出现的障碍物或碰撞点，有利于船舶安全航行、停泊、作业等，避免发生不必要的碰撞，为船舶提供多一重的安全保障。

Description

基于激光雷达的船舶边界探测方法

技术领域

本发明涉及船舶领域，具体涉及一种基于激光雷达的船舶边界探测方法。

背景技术

随着全球经济一体化的不断深入，航运产业迅猛发展，船舶数据急剧增加。在海上交通环境日益复杂的情况下，船舶自身周围情况的探测难度越来越大。

雷达作为船舶的主要助航仪器，从出现至今一直发挥着重要作用。雷达是自主式导航设备，可以扫描到海面上的具有一定大小的物标并将其回波显示在雷达显示器上，从而将海面上物表和本船的相对位置关系清晰显示，让操作者获得较为全面的交通形式图像。目前航海雷达的探测距离可达10～20海里，精度达到30米左右。同时雷达也有很多缺点：相同频率的雷达在距离较近时会产生同频干扰杂波；雷达存在30～50米的固定盲区；雷达的方位角精度差；一般方位角误差在1°左右，且随量程变化而变化。

船用AIS自动识别系统是继雷达之后最重要的避碰助航设备，该系统无需人工维护和参与，能够自动发射和接收船舶识别和航行相关信息，通信可靠性高，不受气象海况影响，不会因杂波干扰而丢失小目标。AIS作用距离在15～30海里，精度稳定在10～30米。但AIS也存在一些局限性：如一些大的障碍会使AIS行程信号盲区，在航行实践中也会产生目标丢失等现象；并非所有船舶都安装AIS设备，这就使得一些未安装AIS设备的小船无法被探测到。

在上述的传统技术下，船舶很难探测到自身周围近距离的未安装AIS设备的小船或浮标之类的障碍物；或在夜晚视线不好情况下，船舶的停泊或拖轮的助泊作业时，无法精确的测量出船舶周围障碍物的距离和方位角。

随着科技的飞速发展，激光雷达在测扫领域异军突起，现在主流的激光雷达测量精度可达毫米级，测量频率可达数十赫兹，而且测量结果稳定可靠，几乎没有误差。基于这些的优点，本发明提出一种基于激光雷达的船舶边界探测方法，使用激光雷达进行船舶边界探测可以有效弥补上述的问题。

发明内容

本发明所要解决的技术问题是，针对现有技术的不足，提供一种基于激光雷达的船舶边界探测方法，该探测方法能够精确探测船舶周围近距离障碍物，有利于船舶安全航行、停泊、作业等，避免发生不必要的碰撞。

本发明解决上述技术问题所采用的技术方案为：基于激光雷达的船舶边界探测方法，包括以下步骤：

步骤1：测量船舶的尺寸信息，即船舶的长度、宽度和高度；

步骤2：在船舶的驾驶台上安装船载终端，将船载终端接入船载以太网，并设置船载终端的网络地址；

步骤3：在船舶的罗经甲板上水平安装罗经甲板激光雷达，在船舶的左舷上垂直安装左舷激光雷达，在船舶的右舷上垂直安装右舷激光雷达，分别测量这三个激光雷达的安装位置点相对船舶中心位置点的偏移量和旋转角度，船舶中心位置点即为船舶的横轴和纵轴的交点；

步骤4：分别将罗经甲板激光雷达、左舷激光雷达和右舷激光雷达用网线接入船载以太网，再分别将这三个激光雷达上电，然后分别设置这三个激光雷达的网络地址和数据端口；

步骤5：罗经甲板激光雷达、左舷激光雷达和右舷激光雷达工作，探测船舶周边障碍物；开启船载工控机的3个UDP端口，通过船载工控机的3个UDP端口分别接收三个激光雷达发送的网络数据包，每个所述的网络数据包包括12个数据块，每个所述的数据块包括32个通道数据，每个所述的通道数据包括一个2字节的距离值和一个1字节的反射强度值；

步骤6：船载工控机将接收的三个激光雷达发送的网络数据包分别存入三个双缓存区，每个所述的双缓存区包括两个相同的缓存区，当其中一个缓存区存满数据后，数据自动存入另一个缓存区中；每个所述的缓存区包括字节数组存储区、存储索引值和状态位，缓存初始状态下，存储索引值和状态位为0，当接收的网络数据包存入字节数组存储区时，存储索引值自动加1，当存储索引值达到字节数组存储区的最大值时，状态位置1；

步骤7：分别对每个所述的双缓存区进行检查，当发现某一个缓存区已存满网络数据包时，对该缓存区内存储的网络数据包进行逐个提取解析，得到360°全景扫描数据，然后将该缓存区内的字节数组存储区清空，并将其存储索引值和状态位分别置0；其中，对网络数据包的提取解析方法包括步骤a～步骤d：

步骤a：提取网络数据包中每个数据块的方位角，对构成每个数据块的两个字节的方位角进行高低位排序，即两个字节的方位角为高位高字节、低位低字节，将高位字节向左移8位后与低位字节合并，得到16位无符号整型数值，再将16位无符号整型数值除以100，得到一个代表方位角0-359.99°的浮点数；

步骤b：提取每个通道数据的距离值，对构成每个通道数据的两个字节的距离值进行高低位排序，即两个字节的距离值为高位高字节、低位低字节，将高位字节向左移8位后与低位字节合并，得到16位无符号整型数值，再将16位无符号整型数值除以500，得到一个代表距离值的浮点数，单位为米；

步骤c：将相邻的两个数据块的方位角相减，再将方位角相减得到的差值除以32，得到每个通道数据的方位角；

步骤d：以每个激光雷达的横轴和纵轴的交点为雷达中心位置点，分别以三个激光雷达的雷达中心位置点为中心，建立三个三维笛卡尔坐标系，其中，X轴为水平横向，左为负，右为正；Y轴为水平纵向，前为正，后为负；Z轴为垂直方向，上为正，下为负；每个激光雷达对应的三维笛卡尔坐标系的建立过程为：

假设每个通道数据的方位角为α，垂直角为ω，距离值为R，计算该通道数据的扫描点数据X_i,Y_i,Z_i：

X_i＝R·cosω·sinα

Y_i＝R·cosω·cosα

Z_i＝R·sinα

其中，i＝1表示左舷激光雷达，i＝2表示右舷激光雷达，i＝3表示罗经甲板激光雷达；

每个激光雷达的所有扫描点数据组成的360°全景扫描数据即为该激光雷达的扫描数据；

步骤8：将三个激光雷达的扫描数据进行旋转变换，其中左舷激光雷达的扫描数据进行顺时针90°的旋转变换，得到旋转变换数据X_r1,Y_r1,Z_r1，计算式为：

X_r1＝-Y₁

Y_r1＝Z₁

Z_r1＝-X₁

右舷激光雷达的扫描数据进行逆时针90°的旋转变换，得到旋转变换数据X_r2,Y_r2,Z_r2，计算式为：

X_r2＝Y₂

Y_r2＝Z₂

Z_r2＝X₂

罗经甲板激光雷达的扫描数据进行水平向下45°的旋转变换，得到旋转变换数据X_r3,Y_r3,Z_r3，计算式为：

X_r3＝X₃

Y_r3＝Y₃·cos(45°)

Z_r3＝R·sin(ω-45°)

步骤9：分别根据三个激光雷达的安装位置点对得到的旋转变换数据进行平移变换，得到三个激光雷达的安装位置点相对船舶中心位置点的平移数据；以船舶中心位置点为原点，建立船舶的三维笛卡尔坐标系：

假设每个激光雷达的安装位置点为X_fi,Y_fi,Z_fi，经过平移变换后得到平移数据X_mi,Y_mi,Z_mi，计算式为：

X_mi＝X_ri-X_fi

Y_mi＝Y_ri-Y_fi

Z_mi＝Z_ri-Z_fi

其中，i＝1表示左舷激光雷达，i＝2表示右舷激光雷达，i＝3表示罗经甲板激光雷达；

步骤10：根据船舶的尺寸信息，分别对三个激光雷达的平移数据进行边界计算，得到船舶边界数据，所述的船舶边界数据包括每个通道数据的扫描点距离船舶中心位置点的距离d_ci、方位角φ_ci、高度H_ci和边界距离D_i，将船舶的长度、宽度和高度分别记为L、W和H，假设船头近似为半椭圆，将船头的长半径记为R_l；d_ci,φ_ci,H_ci,D_i的计算式为：

φ_ci＝arctan(X_mi/Y_mi)

H_ci＝Z_i

k＝X_mi/Y_mi

其中，i＝1表示左舷激光雷达，i＝2表示右舷激光雷达，i＝3表示罗经甲板激光雷达；

步骤11：记录每个激光雷达的边界距离最小的扫描点，假设t₁时刻该扫描点距离船舶中心位置点的距离为d_t1，t₂时刻该扫描点距离船舶中心位置点的距离为d_t2，则该扫描点所在的船舶周边障碍物接近船舶的接近速度v_i为：

步骤12：所述的船载工控机将每个激光雷达的边界距离最小的扫描点距离船舶中心位置点的距离、方位角、高度、边界距离和接近速度打包成网络数据包并发送给所述的船载终端；

步骤13：船载终端接收到包含每个激光雷达的边界距离最小的扫描点距离船舶中心位置点的距离、方位角、高度、边界距离和接近速度的信息后，以俯视和后视的视角在船舶的驾驶台的屏幕上显示这些信息，同时判断边界距离是否小于报警门限值，如果边界距离小于报警门限值，则船载终端将屏幕上显示的信息设成红色，同时通过喇叭发出语音报警。

作为优选，所述的罗经甲板激光雷达、所述的左舷激光雷达和所述的右舷激光雷达的型号均为Velodyne VLP-16；每个所述的网络数据包为1248字节的UDP报文，每个所述的网络数据包包括一个42字节的帧头、1200字节的所述的12个数据块、4字节的一个时间戳和2字节的一个工厂序号；每个所述的数据块包括一个2字节的标志位、一个2字节的方位角和96字节的所述的32个通道数据。

进一步地，所述的报警门限值为25米。

与现有技术相比，本发明具有如下优点：本发明公开的基于激光雷达的船舶边界探测方法，利用三个激光雷达探测船舶周边障碍物，通过对三个激光雷达发送的网络数据包进行处理后，计算得到每个通道数据的扫描点距离船舶中心位置点的距离d_ci、方位角φ_ci、高度H_ci和边界距离D_i，以及每个激光雷达的边界距离最小的扫描点所在的船舶周边障碍物接近船舶的接近速度v_i，船载终端根据每个激光雷达的边界距离最小的扫描点距离船舶中心位置点的距离、方位角、高度、边界距离和接近速度的信息，在船舶的驾驶台的屏幕上显示这些信息，同时判断边界距离是否小于报警门限值，如果边界距离小于报警门限值，则船载终端将屏幕上显示的信息设成红色，同时通过喇叭发出语音报警。本发明探测方法能够精确探测船舶周围近距离障碍物，使船员在船舶航行和停靠过程中，在出现天气恶劣、视线模糊或存在视角盲区情况下，及时知道船舶周边可能出现的障碍物或碰撞点，有利于船舶安全航行、停泊、作业等，避免发生不必要的碰撞，为船舶提供多一重的安全保障。

附图说明

图1为本发明中激光雷达发送的网络数据包的数据格式；

图2为本发明中激光雷达发送的网络数据包中每个通道数据对应的方位角示意图；

图3为本发明中激光雷达发送的网络数据包中每个通道数据对应的垂直角示意图；

图4为本发明中激光雷达发送的网络数据包中每个通道数据对应的垂直角汇总；

图5为本发明中船载终端接收到网络数据包后的信息显示界面示意图；

图6为本发明中船载终端接收到网络数据包后的信息显示和语音报警流程。

具体实施方式

以下结合附图实施例对本发明作进一步详细描述。

实施例的基于激光雷达的船舶边界探测方法，包括以下步骤：

步骤1：测量船舶的尺寸信息，测得船舶的长度、宽度和高度分别为40米、12.7米和11米；

步骤2：在船舶的驾驶台上安装船载终端，将船载终端接入船载以太网，并设置船载终端的网络地址为“192.168.10.231”；

步骤3：在船舶的罗经甲板上水平安装罗经甲板激光雷达，在船舶的左舷上垂直安装左舷激光雷达，在船舶的右舷上垂直安装右舷激光雷达，罗经甲板激光雷达、左舷激光雷达和右舷激光雷达的型号均为Velodyne VLP-16；分别测量这三个激光雷达的安装位置点相对船舶中心位置点的偏移量和旋转角度，船舶中心位置点即为船舶的横轴和纵轴的交点；测得罗经甲板激光雷达的以米为单位的偏移量为(0，2.7，10.8)，旋转角度为-90°；左舷激光雷达的以米为单位的偏移量为(-3，1.85，5.8)，旋转角度为90°；右舷激光雷达的以米为单位的偏移量为(3，1.85，5.8)，旋转角度为-90°；

步骤4：分别将罗经甲板激光雷达、左舷激光雷达和右舷激光雷达用网线接入船载以太网，再分别将这三个激光雷达上电，然后分别设置这三个激光雷达的网络地址和数据端口，其中，罗经甲板激光雷达的网络地址设置为“192.168.10.232”，数据端口设置为2368；左舷激光雷达的网络地址设置为“192.168.10.233”，数据端口设置为2367；右舷激光雷达的网络地址设置为“192.168.10.234”，数据端口设置为2366；

步骤5：罗经甲板激光雷达、左舷激光雷达和右舷激光雷达工作，探测船舶周边障碍物；利用现有的Socket编程技术和多线程技术开启船载工控机的3个UDP端口，通过船载工控机的3个UDP端口分别接收三个激光雷达发送的网络数据包，如图1所示，每个网络数据包为1248字节的UDP报文，每个网络数据包包括一个42字节的帧头、1200字节的12个数据块、4字节的一个时间戳和2字节的一个工厂序号；每个数据块包括一个2字节的标志位、一个2字节的方位角和96字节的32个通道数据，每个通道数据包括一个2字节的距离值和一个1字节的反射强度值；

步骤6：船载工控机将接收的三个激光雷达发送的网络数据包分别存入三个双缓存区，每个双缓存区包括两个相同的缓存区，当其中一个缓存区存满数据后，数据自动存入另一个缓存区中；每个缓存区包括字节数组存储区、存储索引值和状态位，缓存初始状态下，存储索引值和状态位为0，当接收的网络数据包存入字节数组存储区时，存储索引值自动加1，当存储索引值达到字节数组存储区的最大值时，状态位置1；

步骤7：启动3个线程分别对每个双缓存区进行检查，当发现某一个缓存区已存满网络数据包时，对该缓存区内存储的网络数据包进行逐个提取解析，得到360°全景扫描数据，然后将该缓存区内的字节数组存储区清空，并将其存储索引值和状态位分别置0；其中，对网络数据包的提取解析方法包括步骤a～步骤d：

步骤a：提取网络数据包中每个数据块的方位角，计算得到两个字节的方位角分别为0x12和0x34，对这两个方位角进行高低位排序，即两个字节的方位角为高位高字节、低位低字节，根据高位高字节、低位低字节方式合并成0x3412，转换成十进制数为13330，再除以100得133.4，由此得方位角为133.4°；

步骤b：提取每个通道数据的距离值，计算得到两个字节的距离值分别为0x56和0x78，对这两个字节的距离值进行高低位排序，即两个字节的距离值为高位高字节、低位低字节，根据高位高字节、低位低字节方式合并成0x7856，转换成十进制数为30806，再除以500得61.6，由此得距离值为61.6米；

步骤c：相邻的两个数据块的方位角分别为30°和30.32°，将其相减并将差值除以32，得到相邻的两个数据块的方位角差值为0.01°，则0-32个通道数据的方位角分别为30°、30.01°…30.32°；

步骤d：以每个激光雷达的横轴和纵轴的交点为雷达中心位置点，分别以三个激光雷达的雷达中心位置点为中心，建立三个三维笛卡尔坐标系，其中，X轴为水平横向，左为负，右为正；Y轴为水平纵向，前为正，后为负；Z轴为垂直方向，上为正，下为负；每个激光雷达对应的三维笛卡尔坐标系的建立过程为：

每个通道数据对应的方位角示意图如图2所示，垂直角示意图如图3所示，垂直角汇总如图4所示，假设每个通道数据的方位角为α，垂直角为ω，距离值为R，由于每个通道数据的扫描点数据X_i,Y_i,Z_i相同，以图4中通道序号为“0”的通道数据的垂直角为例，以方位角α＝30°、垂直角ω＝-15°、距离值R＝61.6米，计算该通道数据的扫描点数据X_i,Y_i,Z_i：

X_i＝R·cosω·sinα

Y_i＝R·cosω·cosα

Z_i＝R·sinα

计算得到扫描点数据X_i,Y_i,Z_i分别为27.95,51.53,30.8；其中，i＝1表示左舷激光雷达，i＝2表示右舷激光雷达，i＝3表示罗经甲板激光雷达；

每个激光雷达的所有扫描点数据组成的360°全景扫描数据即为该激光雷达的扫描数据；

步骤8：将三个激光雷达的扫描数据进行旋转变换，其中左舷激光雷达的扫描数据进行顺时针90°的旋转变换，得到旋转变换数据X_r1,Y_r1,Z_r1，计算式为：

X_r1＝-Y₁

Y_r1＝Z₁

Z_r1＝-X₁

计算得到旋转变换数据X_r1,Y_r1,Z_r1分别为-51.53,30.8,-27.95；

右舷激光雷达的扫描数据进行逆时针90°的旋转变换，得到旋转变换数据X_r2,Y_r2,Z_r2，计算式为：

X_r2＝Y₂

Y_r2＝Z₂

Z_r2＝X₂

计算得到旋转变换数据X_r1,Y_r1,Z_r1分别为51.53,30.8,27.95；

罗经甲板激光雷达的扫描数据进行水平向下45°的旋转变换，得到旋转变换数据X_r3,Y_r3,Z_r3，计算式为：

X_r3＝X₃

Y_r3＝Y₃·cos(45°)

Z_r3＝R·sin(ω-45°)

计算得到旋转变换数据X_r1,Y_r1,Z_r1分别为27.95,36.55,-53.35；

步骤9：分别根据三个激光雷达的安装位置点对得到的旋转变换数据进行平移变换，得到三个激光雷达的安装位置点相对船舶中心位置点的平移数据；以船舶中心位置点为原点，建立船舶的三维笛卡尔坐标系：

假设每个激光雷达的安装位置点为X_fi,Y_fi,Z_fi，经过平移变换后得到平移数据X_mi,Y_mi,Z_mi，计算式为：

X_mi＝X_ri-X_fi

Y_mi＝Y_ri-Y_fi

Z_mi＝Z_ri-Z_fi

其中，i＝1表示左舷激光雷达，i＝2表示右舷激光雷达，i＝3表示罗经甲板激光雷达；

由步骤3已知罗经甲板激光雷达的以米为单位的偏移量为(0，2.7，10.8)、左舷激光雷达的以米为单位的偏移量为(-3，1.85，5.8)、右舷激光雷达的以米为单位的偏移量为(3，1.85，5.8)，分别经过平移变换后得到平移数据(15.53，17.8，33.95)、(-20.53，18.8，30.95)、(-51.53，30.8，-27.95)；

步骤10：根据船舶的尺寸信息，分别对三个激光雷达的平移数据进行边界计算，得到船舶边界数据，船舶边界数据包括每个通道数据的扫描点距离船舶中心位置点的距离d_ci、方位角φ_ci、高度H_ci和边界距离D_i，将船舶的长度、宽度和高度分别记为L、W和H，由步骤1已知L＝40米、W＝12.7米、H＝11米；

假设船头近似为半椭圆，将船头的长半径记为R_l；d_ci,φ_ci,H_ci,D_i的计算式为：

φ_ci＝arctan(X_mi/Y_mi)

H_ci＝Z_i

k＝X_mi/Y_mi

其中，i＝1表示左舷激光雷达，i＝2表示右舷激光雷达，i＝3表示罗经甲板激光雷达；计算可知每个通道数据的扫描点距离船舶中心位置点的距离d_ci为61.6米、方位角φ_ci为30°、高度H_ci为5米、边界距离D_i为13米；

步骤11：记录每个激光雷达的边界距离最小的扫描点，假设t₁时刻该扫描点距离船舶中心位置点的距离为d_t1，t₂时刻该扫描点距离船舶中心位置点的距离为d_t2，则该扫描点所在的船舶周边障碍物接近船舶的接近速度v_i为：

计算可得每个激光雷达的边界距离最小的扫描点所在的船舶周边障碍物接近船舶的接近速度v_i为5.1米每秒；

步骤12：船载工控机将每个激光雷达的边界距离最小的扫描点距离船舶中心位置点的距离、方位角、高度、边界距离和接近速度打包成网络数据包并发送给船载终端；

步骤13：船载终端接收到包含每个激光雷达的边界距离最小的扫描点距离船舶中心位置点的距离、方位角、高度、边界距离和接近速度的信息后，以俯视和后视的视角在船舶的驾驶台的屏幕上显示这些信息(信息显示界面示意图见图5，)，同时判断边界距离是否小于报警门限值25米，如果边界距离小于报警门限值25米，则船载终端将屏幕上显示的信息设成红色，同时通过喇叭发出语音报警，其中，船载终端接收到网络数据包后的信息显示和语音报警流程见图6。

Claims (3)

1.基于激光雷达的船舶边界探测方法，其特征在于，包括以下步骤：

步骤1：测量船舶的尺寸信息，即船舶的长度、宽度和高度；

步骤2：在船舶的驾驶台上安装船载终端，将船载终端接入船载以太网，并设置船载终端的网络地址；

步骤3：在船舶的罗经甲板上水平安装罗经甲板激光雷达，在船舶的左舷上垂直安装左舷激光雷达，在船舶的右舷上垂直安装右舷激光雷达，分别测量这三个激光雷达的安装位置点相对船舶中心位置点的偏移量和旋转角度，船舶中心位置点即为船舶的横轴和纵轴的交点；

步骤4：分别将罗经甲板激光雷达、左舷激光雷达和右舷激光雷达用网线接入船载以太网，再分别将这三个激光雷达上电，然后分别设置这三个激光雷达的网络地址和数据端口；

步骤5：罗经甲板激光雷达、左舷激光雷达和右舷激光雷达工作，探测船舶周边障碍物；开启船载工控机的3个UDP端口，通过船载工控机的3个UDP端口分别接收三个激光雷达发送的网络数据包，每个所述的网络数据包包括12个数据块，每个所述的数据块包括32个通道数据，每个所述的通道数据包括一个2字节的距离值和一个1字节的反射强度值；

步骤6：船载工控机将接收的三个激光雷达发送的网络数据包分别存入三个双缓存区，每个所述的双缓存区包括两个相同的缓存区，当其中一个缓存区存满数据后，数据自动存入另一个缓存区中；每个所述的缓存区包括字节数组存储区、存储索引值和状态位，缓存初始状态下，存储索引值和状态位为0，当接收的网络数据包存入字节数组存储区时，存储索引值自动加1，当存储索引值达到字节数组存储区的最大值时，状态位置1；

步骤7：分别对每个所述的双缓存区进行检查，当发现某一个缓存区已存满网络数据包时，对该缓存区内存储的网络数据包进行逐个提取解析，得到360°全景扫描数据，然后将该缓存区内的字节数组存储区清空，并将其存储索引值和状态位分别置0；其中，对网络数据包的提取解析方法包括步骤a～步骤d：

步骤a：提取网络数据包中每个数据块的方位角，对构成每个数据块的两个字节的方位角进行高低位排序，即两个字节的方位角为高位高字节、低位低字节，将高位字节向左移8位后与低位字节合并，得到16位无符号整型数值，再将16位无符号整型数值除以100，得到一个代表方位角0-359.99°的浮点数；

步骤b：提取每个通道数据的距离值，对构成每个通道数据的两个字节的距离值进行高低位排序，即两个字节的距离值为高位高字节、低位低字节，将高位字节向左移8位后与低位字节合并，得到16位无符号整型数值，再将16位无符号整型数值除以500，得到一个代表距离值的浮点数，单位为米；

步骤c：将相邻的两个数据块的方位角相减，再将方位角相减得到的差值除以32，得到每个通道数据的方位角；

步骤d：以每个激光雷达的横轴和纵轴的交点为雷达中心位置点，分别以三个激光雷达的雷达中心位置点为中心，建立三个三维笛卡尔坐标系，其中，X轴为水平横向，左为负，右为正；Y轴为水平纵向，前为正，后为负；Z轴为垂直方向，上为正，下为负；每个激光雷达对应的三维笛卡尔坐标系的建立过程为：

假设每个通道数据的方位角为α，垂直角为ω，距离值为R，计算该通道数据的扫描点数据X_i,Y_i,Z_i：

X_i＝R·cosω·sinα

Y_i＝R·cosω·cosα

Z_i＝R·sinα

其中，i＝1表示左舷激光雷达，i＝2表示右舷激光雷达，i＝3表示罗经甲板激光雷达；

每个激光雷达的所有扫描点数据组成的360°全景扫描数据即为该激光雷达的扫描数据；

步骤8：将三个激光雷达的扫描数据进行旋转变换，其中左舷激光雷达的扫描数据进行顺时针90°的旋转变换，得到旋转变换数据X_r1,Y_r1,Z_r1，计算式为：

X_r1＝-Y₁

Y_r1＝Z₁

Z_r1＝-X₁

右舷激光雷达的扫描数据进行逆时针90°的旋转变换，得到旋转变换数据X_r2,Y_r2,Z_r2，计算式为：

X_r2＝Y₂

Y_r2＝Z₂

Z_r2＝X₂

罗经甲板激光雷达的扫描数据进行水平向下45°的旋转变换，得到旋转变换数据X_r3,Y_r3,Z_r3，计算式为：

X_r3＝X₃

Y_r3＝Y₃·cos(45°)

Z_r3＝R·sin(ω-45°)

步骤9：分别根据三个激光雷达的安装位置点对得到的旋转变换数据进行平移变换，得到三个激光雷达的安装位置点相对船舶中心位置点的平移数据；以船舶中心位置点为原点，建立船舶的三维笛卡尔坐标系：

假设每个激光雷达的安装位置点为X_fi,Y_fi,Z_fi，经过平移变换后得到平移数据X_mi,Y_mi,Z_mi，计算式为：

X_mi＝X_ri-X_fi

Y_mi＝Y_ri-Y_fi

Z_mi＝Z_ri-Z_fi

其中，i＝1表示左舷激光雷达，i＝2表示右舷激光雷达，i＝3表示罗经甲板激光雷达；

步骤10：根据船舶的尺寸信息，分别对三个激光雷达的平移数据进行边界计算，得到船舶边界数据，所述的船舶边界数据包括每个通道数据的扫描点距离船舶中心位置点的距离d_ci、方位角φ_ci、高度H_ci和边界距离D_i，将船舶的长度、宽度和高度分别记为L、W和H，假设船头近似为半椭圆，将船头的长半径记为R_l；d_ci,φ_ci,H_ci,D_i的计算式为：

φ_ci＝arctan(X_mi/Y_mi)

H_ci＝Z_i

k＝X_mi/Y_mi

其中，i＝1表示左舷激光雷达，i＝2表示右舷激光雷达，i＝3表示罗经甲板激光雷达；

步骤11：记录每个激光雷达的边界距离最小的扫描点，假设t₁时刻该扫描点距离船舶中心位置点的距离为d_t1，t₂时刻该扫描点距离船舶中心位置点的距离为d_t2，则该扫描点所在的船舶周边障碍物接近船舶的接近速度v_i为：

步骤12：所述的船载工控机将每个激光雷达的边界距离最小的扫描点距离船舶中心位置点的距离、方位角、高度、边界距离和接近速度打包成网络数据包并发送给所述的船载终端；

步骤13：船载终端接收到包含每个激光雷达的边界距离最小的扫描点距离船舶中心位置点的距离、方位角、高度、边界距离和接近速度的信息后，以俯视和后视的视角在船舶的驾驶台的屏幕上显示这些信息，同时判断边界距离是否小于报警门限值，如果边界距离小于报警门限值，则船载终端将屏幕上显示的信息设成红色，同时通过喇叭发出语音报警。

2.根据权利要求1所述的基于激光雷达的船舶边界探测方法，其特征在于，所述的罗经甲板激光雷达、所述的左舷激光雷达和所述的右舷激光雷达的型号均为Velodyne VLP-16；每个所述的网络数据包为1248字节的UDP报文，每个所述的网络数据包包括一个42字节的帧头、1200字节的所述的12个数据块、4字节的一个时间戳和2字节的一个工厂序号；每个所述的数据块包括一个2字节的标志位、一个2字节的方位角和96字节的所述的32个通道数据。

3.根据权利要求2所述的基于激光雷达的船舶边界探测方法，其特征在于，所述的报警门限值为25米。

Images