1.一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法,其特征在于,按如下步骤进行:步骤S1,利用时间同步器读取同一时刻的组合导航系统数据与激光雷达点云数据,然后进入步骤S2;
步骤S2,计算激光雷达点云数据帧头激光点水平角度、激光雷达点云数据帧尾激光点水平角度,然后进入步骤S3;
步骤S3,计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间,然后进入步骤S4;
步骤S4,判断上一时刻是否有组合导航系统数据与激光雷达点云数据,若是,进入S5;
若否,进入步骤S6;
步骤S5,利用已计算得到的激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间,并用上一时刻和当前时刻组合导航系统数据,将运动信息赋予激光雷达点云数据进行矫正;
步骤S6,利用已计算得到的激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间,并用当前时刻组合导航系统数据,将运动信息赋予激光雷达点云数据进行矫正;
其中,激光雷达点云数据包括点云坐标和该帧时间戳。
2.根据权利要求1所述的一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法,其特征在于步骤S1的组合导航系统数据包括经纬度、高程、姿态角、速度、加速度、角速度、角加速度和时间戳中的一种或多种。
3.根据权利要求1所述的一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法,其特征在于步骤S1的时间同步器,利用激光雷达点云数据中的时间戳和组合导航系统数据中的时间戳进行对比配对,得到同一时刻的两组数据。
4.根据权利要求1所述的一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法,其特征在于步骤S2的激光雷达点云数据帧头激光点水平角度为激光雷达点云数据中第一个激光点的x值y值计算的角度;
所述计算采用的公式:
其中:θ为激光雷达点云数据帧头激光点水平角度,x、y为激光雷达点云数据帧头激光点x轴坐标值、y轴坐标值;
激光雷达点云数据帧尾激光点水平角度为帧头水平角角度水平加上角分辨率,并且要求激光雷达内部旋转电机为顺时针旋转。
5.根据权利要求1所述的一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法,其特征在于步骤S3是利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点之间的水平夹角及激光雷达内部旋转电机转动频率计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间,所述计算按如下步骤进行:步骤S3.1,计算激光雷达内部旋转电机转动1°所需时间,计算公式:其中:Δt1为激光雷达内部旋转电机转动1°所需时间,然后进入S3.2;
步骤S3.2计算激光雷达点云数据当前激光点水平角度,
计算公式:
其中:θ为激光雷达点云数据当前激光点水平角度,x、y为激光雷达点云数据当前激光点x轴坐标值、y轴坐标值,然后进入S3.3;
步骤S3.3计算激光雷达点云数据当前激光点水平角度与激光雷达点云数据帧尾激光点水平角度之间的夹角,计算公式:
Δθ=360°+θ1-θ0
其中:Δθ为激光雷达点云数据的当前激光点水平角度与激光雷达点云数据帧尾激光点水平角度之间的夹角,θ1为激光雷达点云数据当前激光点水平角度,θ0为激光雷达点云数据帧尾激光点水平角度,然后进入S3.4;
步骤S3.4,计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间,计算公式:
Δt=Δt1*Δθ
其中:Δt为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间,Δt1为激光雷达内部旋转电机转动1°所需时间,Δθ为激光雷达点云数据当前激光点水平角度与激光雷达点云数据帧尾激光点水平角度之间的夹角。
6.根据权利要求1所述的一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法,其特征在于步骤S5用上一时刻和当前时刻组合导航系统数据,将运动信息赋予激光雷达点云数据进行矫正,具体步骤如下:步骤S5.1,根据前一时刻和当前时刻组合导航系统的加速度和角加速度,计算该时间段内的加加速度(急动速度)和角加加速度,计算公式:
其中:j为无人船加加速度或角加加速度,a1为当前时刻无人船加速度或角加速度,a0为前一时刻无人船加速度或角加速度,t1为当前时刻时间,t0为前一时刻时间;然后进入S5.2;
步骤S5.2,将激光雷达点云数据帧尾激光点的坐标原点作为全局坐标系坐标原点,然后转到S5.3;
步骤S5.3,根据激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间计算平移量和旋转量,利用已计算得到的无人船加加速度和角加加速度,以及无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据当前激光点转换到全局坐标系下。
7.根据权利要求6所述的一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法,其特征在于步骤S5.3利用已计算得到的无人船加加速度和角加加速度,以及无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据转换到以激光雷达点云数据帧尾激光点的原点作为全局坐标系下的原点,具体步骤如下:步骤S5.3.1,利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船速度、加速度和加加速度,计算x轴、y轴、z轴的平移量,计算公式:
其中: 为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需要的平移量,vx、vy、vz为当前时刻无人船x轴方向、y轴方向、z轴方向速度,ax、ay、az为当前时刻无人船x轴方向、y轴方向、z轴方向加速度,jx、jy、jz为当前时刻无人船x轴方向、y轴方向、z轴方向加加速度,Δt为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间,然后转到S5.3.2;
步骤S5.3.2,利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船角速度、角加速度和角加加速度,计算激光雷达点云数据当前激光点无人船姿态角,计算公式:
其中:θ为激光雷达点云数据当前激光点无人船姿态角,θ1为激光雷达点云数据帧尾激光点无人船姿态角,ω为无人船角速度,a为无人船角加速度,j为无人船角加加速度;分别利用无人船x轴、y轴、z轴上的角速度、角加速度、角加加速度,计算无人船在激光雷达点云数据当前激光点下无人船姿态角,然后转到S5.3.3;
步骤S5.3.3,利用无人船姿态角,计算无人船x轴、y轴、z轴的旋转量,计算公式:
其中:Ryaw为无人船z轴方向旋转量,Rpitch为无人船y轴方向旋转量,Rroll为无人船z轴方向旋转量,θx、θy、θz为无人船x轴、y轴、z轴旋转角度,然后转到S5.3.4;
步骤S5.3.4,利用无人船x轴、y轴、z轴的旋转量,将激光雷达点云数据当前激光点旋转至水平姿态下,计算公式:
其中:Ryaw为无人船z轴方向旋转量,Rpitch为无人船y轴方向旋转量,Rpitch为无人船z轴方向旋转量, 为激光雷达点云数据当前激光点, 为水平姿态下的激光点,转到S5.3.5;
步骤S5.3.5,利用x轴、y轴、z轴的平移量,将位置由水平姿态下的激光点平移到全局坐标系下,计算公式:
其中: 为激光雷达点云数据去除运动畸变后的激光点, 为水平姿态下的激光点, 为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点的x轴、y轴、z轴的平移量。
8.根据权利要求1所述的一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法,其特征在于步骤S6用当前时刻组合导航系统数据,将运动信息赋予激光雷达点云数据进行矫正,具体步骤如下:步骤S6.1,将激光雷达点云数据帧尾激光点的坐标原点作为全局坐标系坐标原点,然后转到S6.2;
步骤S6.2,根据激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间计算平移量和旋转量,利用已知无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据当前激光点转换到全局坐标系下。
9.根据权利要求8所述的一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法,其特征在于步骤S6.2利用无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据转换到以激光雷达点云数据帧尾激光点的原点作为全局坐标系下的原点,具体步骤如下:步骤S6.2.1,利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船速度、加速度,计算x轴、y轴、z轴的平移量,计算公式:其中: 为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需要的平移量,vx、vy、vz为当前时刻无人船x轴方向、y轴方向、z轴方向速度,ax、ay、az为当前时刻无人船x轴方向、y轴方向、z轴方向加速度,Δt为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间;然后转到S6.2.2;
步骤S6.2.2,利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船角速度、角加速度,计算激光雷达点云数据当前激光点无人船姿态角,计算公式:其中:θ为激光雷达点云数据当前激光点无人船姿态角,θ1为激光雷达点云数据帧尾激光点无人船姿态角,ω为无人船角速度,a为无人船角加速度,j为无人船角加加速度,分别利用无人船x轴、y轴、z轴上的角速度、角加速度、角加加速度,计算无人船在激光雷达点云数据当前激光点下无人船姿态角,然后转到S6.2.3。
步骤S6.2.3,利用无人船姿态角,计算无人船x轴、y轴、z轴的旋转量,计算公式:其中:Ryaw为无人船z轴方向旋转量,Rpitch为无人船y轴方向旋转量,Rroll为无人船z轴方向旋转量,θx、θy、θz为无人船x轴、y轴、z轴旋转角度,转到S6.2.4;
步骤S6.2.4,利用无人船x轴、y轴、z轴的旋转量,将激光雷达点云数据当前激光点旋转至水平姿态下,计算公式:
其中:Ryaw为无人船z轴方向旋转量,Rpitch为无人船y轴方向旋转量,Rpitch为无人船z轴方向旋转量, 为激光雷达点云数据当前激光点, 为水平姿态下的激光点,转到S6.2.5;
步骤S6.2.5,利用x轴、y轴、z轴的平移量,将位置由水平姿态下的激光点平移到全局坐标系下,计算公式:
其中: 为激光雷达点云数据去除运动畸变后的激光点, 为水平姿态下的激光点, 为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点的x轴、y轴、z轴的平移量。
10.一种基于组合导航系统矫正激光雷达点云数据运动畸变矫正的系统,其特征在于,其包括实现如权利要求1至10任一项所述方法的流程模块。