CN103644917A - 移动测量平台激光雷达旋转与平移参数计算方法 - Google Patents

Abstract

本发明提出了一种移动测量平台激光雷达旋转与平移参数计算方法，包括步骤：选定M个控制点；建立N个状态变量且第i个记为Q_i，令迭代次数n＝0；建立N个速度变量且第i个记为V_i；建立N个局部最优解变量且第i个记为建立N个局部最优变量对应成本且第i个记为F_i；计算成本函数如果令计算最小成本F_m，如果第m个变量的为全局最优解，则F^best＝F_m，如果F^best＜ε则退出，输出Q^best；构造速度变化值，更新速度变量和变量状态，令n＝n+1，返回计算成本函数。该方法鲁莽性强，不需要精确的对值进行初始化也可以收敛；其公式全部是正向计算，不需要进行矩阵求导等复杂运算，提高了计算速度。

Description

移动测量平台激光雷达旋转与平移参数计算方法

技术领域

本发明涉及一种移动测量平台系统标定的方法,尤其是对GNSS（全球定位系统），IMU（惯性导航单元）与Lidar(激光雷达)组成的移动测量平台，求解激光雷达相对于POS坐标系的旋转与平移参数的方法。

背景技术

移动测量系统集成激光雷达、GNSS全球定位系统、IMU惯性导航单元等传感器，可以快速采集道路周边高精度的点云数据。在移动测量系统中，激光雷达、GNSS、IMU传感器安装在一个刚性平台上，整个平台安装在移动车辆上。在车辆前进过程中，同步采集激光点云数据，GNSS原始数据，IMU的三轴加速度与三轴角速度，各种数据通过时间进行严格同步。在数据处理过程中，把GNSS原始数据与IMU的原始数据首先进行POS解算，获得一定时间间隔周期的POS数据（位置与姿态数据，包括WGS84坐标与该时刻平台的姿态）。因为激光雷达采集的数据是用极坐标表示的相对于激光雷达本身设备坐标系的点云坐标，因此，需要把激光雷达坐标系下的点云坐标转换为WGS84坐标系下的坐标或者东北高坐标系下的坐标。在这一转换过程中，需要已知激光雷达相对于POS坐标系的旋转矩阵与平移向量。

在求解以上问题时，目前用的比较多的是最小二乘法，比如文献《车载系统激光与POS总体检较方法》中使用最小二乘法来求解。但是这种方法实现困难，需要对矩阵进行偏微分计算，工作量大，易出错；另外，在收敛时需要指定初始值，存在不收敛的可能，需要通过多次改变初值进行计算以确保结果的准确性。

发明内容

为了克服上述现有技术中存在的缺陷，本发明的目的是提供一种移动测量平台激光雷达旋转与平移参数计算方法，该方法鲁莽性强，不需要精确的对值进行初始化，也可以收敛；该方法内的公式全部是正向计算，不需要进行矩阵求导等复杂运算，提高了计算速度。

为了实现本发明的上述目的，本发明提供了一种移动测量平台激光雷达旋转与平移参数计算方法，包括如下步骤：

S1，选定M个控制点，M为正整数，P_j＝(x_j,y_j,z_j)表示激光扫描仪坐标系中第j个控制点的点坐标，P_j点在东北高坐标系中的坐标为P_j点采集时刻平台的POS为POS_j＝(x,y,z,pitch,roll,yaw)，1≤j≤M；所述变量POS_j前三个分量表示GNSS天线相位中心在东北高坐标系中的位置，后三个分量表示平台姿态，既俯仰、横滚、航向角度；

S2，建立N个状态变量，所述N为正整数，第i个状态变量记为Q_i＝(x_i,y_i,z_i,θ_i,ω_i,ψ_i)，其中，x_i,y_i,z_i表示第i个变量的位置分量，θ_i,ω_i,ψ_i为旋转角度分量，其中，1≤i≤N，令迭代的次数n＝0;

S3，建立N个速度变量，所述N为正整数，第i个速度变量记为V_i＝(V_i ^x,V_i ^y,V_i ^z,V_i ^θ,V_i ^ω,V_i ^ψ)，1≤i≤N；其中，前三个分量表示位置变化的速度，后三个分量表示旋转角度变化的速度；

S4，建立N个局部最优解变量，所述N为正整数，第i个局部最优解变量记为

建立N个局部最优变量对应成本，第i个局部最优变量对应成本F_i＝+∞，其中，1≤i≤N；

S5，计算每个状态变量Q_i的成本函数

如果 $F_i^{\mathrm{new}}<F_i,$ 则令 $F_i=F_i^{\mathrm{new}},Q_i^{\mathrm{best}}=Q_i,$ 1≤i≤N，1≤j≤M；

S6，计算最小成本如果第m个变量的局部最优解

为目前的全局最优解，则

F^best＝F_m，1≤m≤N，如果F^best＜ε则退出，输出Q^best，其中，ε为退出阈值;

S7，根据每一个变量的局部最优解

全局最优解Q^best和当前变量值Q_i构造一个速度变化值ΔV_i并更新速度变量V_i，1≤i≤N；

S8，更新变量状态：Q_i＝Q_i+V_i，令n＝n+1，返回步骤S5。

本发明的移动测量平台激光雷达旋转与平移参数计算方法，该方法鲁莽性强，不需要精确的对值进行初始化，也可以收敛；该方法内的公式全部是正向计算，不需要进行矩阵求导等复杂运算，提高了计算速度。

在本发明的一种优选实施方式中，步骤S7的具体方法为：

S71，计算速度变量变化值

其中，c₁、c₂为常数，r₁、r₂为随机数;

S72，计算搜索步长因子

其中，c₃、c₄为常数，n为迭代次数;

S73，分别将V_i ^x,V_i ^y,V_i ^z作为V_i ^P，对V_i ^P＝λ·norm(norm(V_i ^P)+rand(1,c₅)·norm(ΔV_i ^P))进行计算，得到V_i ^Px、V_i ^Py、V_i ^Pz，其中，ΔV_i ^P分别为ΔV_i中V_i ^x,V_i ^y,V_i ^z对应的分量，norm()为归一化函数，rand(1,c₅)表示1到c₅的随机数，c₅为常数；

S74，分别将V_i ^θ,V_i ^ω,V_i ^ψ作为V_i ^A，对V_i ^A＝λ·norm(norm(V_i ^A)+rand(1,c₅)·norm(ΔV_i ^A))进行计算，得到V_i ^Aθ、V_i ^Aω、V_i ^Aψ，其中，ΔV_i ^A为变量ΔV_i中V_i ^θ,V_i ^ω,V_i ^ψ对应的分量，norm()为归一化函数，rand(1,c₅)表示1到c₅的随机数，c₅为常数；

S75，用V_i ^Px、V_i ^Py、V_i ^Pz更新V_i的前三个分量，用V_i ^Aθ、V_i ^Aω、V_i ^Aψ更新V_i的后三个分量。

本发明在大量随机可行解迭代的基础上，通过局部最优解与全局最优解对当前所有可行解进行优化，从而进一步逼近真实的全局最优解，提高收敛速度。

在本发明的一种优选实施方式中，1≤c₁≤c₂≤2,r₁,r₂∈[0,1]的随机数。

在本发明的另一种优选实施方式中，c₁＝1.5,c₂＝2。

在本发明的一种优选实施方式中，1≤c₃≤10，50≤c₄≤200。

在本发明的另一种优选实施方式中，c₃=2，c₄=100。

在本发明的一种优选实施方式中，1≤c₅≤10。

在本发明的另一种优选实施方式中，c₅=1.5。

在本发明的一种优选实施方式中，步骤S5中转换函数 $f(P_j,{\mathrm{POS}}_j,Q_i)=R_g^P(R_P^L\left[\begin{array}{c}x\\ y\\ z\end{array}\right]+\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right])+\left[\begin{array}{c}{\stackrel{\&OverBar;}{X}}_P\\ {\stackrel{\&OverBar;}{Y}}_P\\ {\stackrel{\&OverBar;}{Z}}_P\end{array}\right],$ 其用于把激光雷达坐标系中的点(x,y,z)转换到东北高坐标系下，其中，

由POS_j中的三个角度分量按欧拉角构造的旋转矩阵；

由Q_i的三个角度分量按欧拉角构造的旋转矩阵；(x,y,z)^T为P_j，(X₀,Y₀,Z₀)^T为Q_i的前三个坐标分量；

为POS_j中的前三个位置分量。

在本发明的另一种优选实施方式中，ε=1e-6。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是现有技术中激光雷达坐标的示意图；

图2是本发明移动测量平台激光雷达旋转与平移参数计算方法的流程图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

在本发明的描述中，除非另有规定和限定，需要说明的是，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是机械连接或电连接，也可以是两个元件内部的连通，可以是直接相连，也可以通过中间媒介间接相连，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

在本发明中，利用的数学坐标系有激光雷达坐标系、POS（GPS/IMU）坐标系和WGS-84大地坐标系。其中，激光雷达坐标系，如图1所示，具体的参数及其含义为：

原点：以激光雷达发射顶点为原点O；

X轴：激光扫描平面0°对应的轴；

Y轴：激光扫描平面90°对应的轴；

Z轴：垂直XOY平面，并且与X、Y轴构成右手坐标系；

激光扫描在X-O-Y平面，从上往下观察，激光扫描方向为逆时针旋转。

在图1中，X-O-Y平面上一个激光点的极坐标表示为(σ,θ)，则该点在激光雷达坐标系下的坐标为(x_L,y_L,z_L)=(σcos(θ),σsin(θ),0)。

POS（GPS/IMU）坐标系，具体的参数及其含义为：

原点：GPS天线相位中心作为原点；

坐标系三个轴与IMU内部的三个陀螺仪轴平行；

X轴控制俯仰；Y轴控制侧滚，Z轴控制航向；

WGS-84大地坐标系，具体的参数及其含义为：

原点：地球质心，

X轴：指向BIH1984.0定义的地区极（CTP）方向；

Z轴：指向BIH1984.0的零子午面和CTP赤道的交点；

Y轴：与Z，X轴构成的右手坐标系。

GPS坐标采用的是WGS-84经纬度坐标，为了计算各种坐标系的转换，需要把GPS的经纬度坐标转换为东北高坐标系。

东北高坐标系：首先选定一个点作为原点，以选定的点为原点，在该点按照三轴方向建立坐标系，用于表示局部小范围的笛卡尔坐标系，

X轴：指向东；

Y轴：指向北；

Z轴：过原点垂直水平面向上。

使用GNSS采集的WGS84坐标可以通过计算公式在WGS84坐标系与东北高坐标系之间进行坐标互转。

本发明提供了一种移动测量平台激光雷达旋转与平移参数计算方法，如图2所示，包括如下步骤：

S1，选定M个控制点，M为正整数，在本实施方式中，M优选为大于2的正整数，在本发明的一种优选实施方式中，M优选为3-10的正整数，在本发明的一种更加优选的实施方式中，M优选为3。P_j＝(x_j,y_j,z_j)表示激光扫描仪坐标系中第j个控制点的点坐标，P_j点在东北高坐标系中的坐标为

P_j点采集时刻平台的POS为POS_j＝(x,y,z,pitch,roll,yaw)，1≤j≤M，变量POS_j前三个分量x、y、z表示GNSS天线相位中心在东北高坐标系中的位置，后三个分量表示平台姿态，即俯仰角度pitch、横滚角度roll、航向角度yaw；

S2，建立N个状态变量，所述N为正整数，第i个状态变量记为Q_i＝(x_i,y_i,z_i,θ_i,ω_i,ψ_i)，其中，x_i,y_i,z_i表示第i个变量的位置分量，θ_i,ω_i,ψ_i为旋转角度分量，其中，1≤i≤N，令迭代的次数n＝0;

S3，建立N个速度变量，所述N为正整数，第i个速度变量记为V_i＝(V_i ^x,V_i ^y,V_i ^z,V_i ^θ,V_i ^ω,V_i ^ψ)，1≤i≤N，其中，前三个分量V_i ^x,V_i ^y,V_i ^z表示位置变化的速度，后三个分量V_i ^θ,V_i ^ω,V_i ^ψ表示旋转角度变化的速度；

S4，建立N个局部最优解变量，所述N为正整数，第i个局部最优解变量记为

，建立N个局部最优变量对应成本，第i个局部最优变量对应成本F_i＝+∞，其中，1≤i≤N；

S5，计算每个状态变量Q_i的成本函数

如果 $F_i^{\mathrm{new}}<F_i,$ 则令 $F_i=F_i^{\mathrm{new}},Q_i^{\mathrm{best}}=Q_i,$ 1≤i≤N，1≤j≤M；

S6，计算最小成本如果第m个变量的局部最优解为目前的全局最优解，则

F^best＝F_m，1≤m≤N，如果F^best＜ε则退出，输出Q^best，其中，ε为退出阈值;

S7，根据每一个变量的局部最优解

全局最优解Q^best和当前变量值Q_i构造一个速度变化值ΔV_i并更新速度变量V_i，1≤i≤N；

S8，更新变量状态：Q_i＝Q_i+V_i，令n＝n+1，返回步骤S5。

本发明的移动测量平台激光雷达旋转与平移参数计算方法，该方法鲁莽性强，不需要精确的对值进行初始化，也可以收敛；该方法内的公式全部是正向计算，不需要进行矩阵求导等复杂运算，提高了计算速度。

在本发明的一种优选实施方式中，步骤S7的具体方法为：

S71，计算速度变量变化值

其中，c₁、c₂为常数，r₁、r₂为随机数;

S72，计算搜索步长因子其中，c₃、c₄为常数，n为迭代次数;

S73，计算V_i ^P＝λ·norm(norm(V_i ^P)+rand(1,c₅)·norm(ΔV_i ^P))，其中V_i ^P为速度变量V_i的前三个分量，即分别将V_i ^x,V_i ^y,V_i ^z作为V_i ^P代入V_i ^P＝λ·norm(norm(V_i ^P)+rand(1,c₅)·norm(ΔV_i ^P))进行迭代计算，得到V_i ^Px、V_i ^Py、V_i ^Pz，ΔV_i ^P为变量ΔV_i的前三个分量，在本步骤中，在同一个公式中出现的只为同一个速度分量及其相应的速度变化量，例如，V_i ^x和ΔV_i ^x，norm()为归一化函数，rand(1,c₅)表示1到c₅的随机数，c₅为常数；

S74，计算V_i ^A＝λ·norm(norm(V_i ^A)+rand(1,c₅)·norm(ΔV_i ^A))，其中V_i ^A为速度变量V_i的后三个分量，即分别将V_i ^θ,V_i ^ω,V_i ^ψ作为V_i ^A代入V_i ^A＝λ·norm(norm(V_i ^A)+rand(1,c₅)·norm(ΔV_i ^A))进行迭代计算，得到V_i ^Aθ、V_i ^Aω、V_i ^Aψ，ΔV_i ^A为变量ΔV_i的后三个分量，在本步骤中，在同一个公式中出现的为同一个旋转角度分量及其相应的旋转角度变化量，例如，V_i ^θ和ΔV_i ^θ，norm()为归一化函数，rand(1,c₅)表示1到c₅的随机数，c₅为常数；

S75，用V_i ^Px、V_i ^Py、V_i ^Pz更新V_i的前三个分量，用V_i ^Aθ、V_i ^Aω、V_i ^Aψ更新V_i的后三个分量。

在本发明的一种优选实施方式中，1≤c₁≤c₂≤2,r₁,r₂∈[0,1]的随机数。在本发明的一种更加优选的实施方式中，c₁＝1.5,c₂＝2。

在本发明的一种优选实施方式中，1≤c₃≤10，50≤c₄≤200。在本发明的一种更加优选的实施方式中，c₃=2，c₄=100。

在本发明的一种优选实施方式中，1≤c₅≤10。在本发明的一种更加优选的实施方式中，c₅=1.5。

本发明在大量随机可行解迭代的基础上，通过局部最优解与全局最优解对当前所有可行解进行优化，从而进一步逼近真实的全局最优解，提高了速度。

在本发明的一种优选实施方式中，需要把激光扫描仪坐标系下的坐标转换到东北高坐标系下需要进行两次旋转与偏移运算。

第一次转换：把激光扫描仪坐标系转换到POS坐标系，即：

$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=R_P^L\left[\begin{array}{c}x\\ y\\ z\end{array}\right]+\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right]$

其中(x,y,z)^T为激光扫描仪坐标系下的点；

为激光扫描仪到POS坐标系的旋转矩阵，(X₀,Y₀,Z₀)^T为激光扫描仪原点到POS坐标系原点的偏移。(X,Y,Z)^T为激光点在POS坐标系下的坐标。

第二次转换：把POS坐标系下的坐标转换到东北高坐标系下，及：

$\left[\begin{array}{c}\stackrel{\&OverBar;}{X}\\ \stackrel{\&OverBar;}{Y}\\ \stackrel{\&OverBar;}{Z}\end{array}\right]{=R}_g^P\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]+\left[\begin{array}{c}{\stackrel{\&OverBar;}{X}}_p\\ {\stackrel{\&OverBar;}{Y}}_p\\ {\stackrel{\&OverBar;}{Z}}_p\end{array}\right]$

其中(X,Y,Z)^T为POS坐标系下的坐标；

为POS坐标系到东北高坐标系的旋转矩阵，

为POS坐标系原点在东北高坐标系下的偏移。

为东北高坐标系下的坐标。

合并两次转换，得到扫描仪坐标系到东北高坐标系的转换公式为：

$\left[\begin{array}{c}\stackrel{\&OverBar;}{X}\\ \stackrel{\&OverBar;}{Y}\\ \stackrel{\&OverBar;}{Z}\end{array}\right]=R_g^P(R_P^L\left[\begin{array}{c}x\\ y\\ z\end{array}\right]+\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right])+\left[\begin{array}{c}{\stackrel{\&OverBar;}{X}}_P\\ {\stackrel{\&OverBar;}{Y}}_P\\ {\stackrel{\&OverBar;}{Z}}_P\end{array}\right]$

在系统标定过程中通过3对以上的控制点求取

和(X₀,Y₀,Z₀)^T及激光扫描仪坐标系在POS坐标系中的旋转矩阵与偏移向量。在点云坐标解算过程中带入点云坐标，POS解算后的yaw、pitch、roll三个角度以及

坐标即可把点云坐标转换为东北高坐标系中，完成点云坐标解算。

本发明就是通过至少3组控制点求解

和(X₀,Y₀,Z₀)^T的方法，即激光扫描仪坐标系相对于POS坐标系的三个旋转角度参数和三个位置偏移参数。

步骤S5中转换函数 $f(P_j,{\mathrm{POS}}_j,Q_i)=R_g^P(R_P^L\left[\begin{array}{c}x\\ y\\ z\end{array}\right]+\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right])+\left[\begin{array}{c}{\stackrel{\&OverBar;}{X}}_P\\ {\stackrel{\&OverBar;}{Y}}_P\\ {\stackrel{\&OverBar;}{Z}}_P\end{array}\right],$ 其用于把激光雷达坐标系中的点(x,y,z)转换到东北高坐标系下，其中，

由POS_j中的三个角度分量按欧拉角构造的旋转矩阵；由Q_i的三个角度分量按欧拉角构造的旋转矩阵；(x,y,z)^T为P_j，(X₀,Y₀,Z₀)^T为Q_i的前三个坐标分量；

为POS_j中的前三个位置分量。

在本发明的一种优选实施方式中，退出阈值进可根据精度需要而定，在本实施方式中，具体取值为：ε=1e-6。

在本实施方式中，在步骤S8之后还具有以下步骤：将计算出的移动测量平台激光雷达旋转与平移参数传输给激光雷达，将激光雷达坐标系下的点云坐标转换为WGS84坐标系下的坐标或者东北高坐标系下的坐标。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (10)

1.一种移动测量平台激光雷达旋转与平移参数计算方法，其特征在于，包括如下步骤：

S1，选定M个控制点，M为正整数，P_j＝(x_j,y_j,z_j)表示激光扫描仪坐标系中第j个控制点的点坐标，P_j点在东北高坐标系中的坐标为

P_j点采集时刻平台的POS为POS_j＝(x,y,z,pitch,roll,yaw)，1≤j≤M，所述变量POS_j前三个分量表示GNSS天线相位中心在东北高坐标系中的位置，后三个分量表示平台姿态，包括俯仰、横滚、航向角度；

S2，建立N个状态变量，所述N为正整数，第i个状态变量记为Q_i＝(x_i,y_i,z_i,θ_i,ω_i,ψ_i)，其中，x_i,y_i,z_i表示第i个变量的位置分量，θ_i,ω_i,ψ_i为旋转角度分量，其中，1≤i≤N，令迭代的次数n＝0;

S3，建立N个速度变量，所述N为正整数，第i个速度变量记为V_i＝(V_i ^x,V_i ^y,V_i ^z,V_i ^θ,V_i ^ω,V_i ^ψ)，1≤i≤N，其中，前三个分量表示位置变化的速度，后三个分量表示旋转角度变化的速度；

S4，建立N个局部最优解变量，所述N为正整数，第i个局部最优解变量记为建立N个局部最优变量对应成本，第i个局部最优变量对应成本F_i＝+∞，其中，1≤i≤N；

S5，计算每个状态变量Q_i的成本函数如果 $F_i^{\mathrm{new}}<F_i,$ 则令 $F_i=F_i^{\mathrm{new}},Q_i^{\mathrm{best}}=Q_i,$ 1≤i≤N，1≤j≤M；

S6，计算最小成本

如果第m个变量的局部最优解为目前的全局最优解,则

F^best＝F_m，1≤m≤N，如果F^best＜ε则退出，输出Q^best，其中，ε为退出阈值;

S7，根据每一个变量的局部最优解

全局最优解Q^best和当前变量值Q_i构造一个速度变化值ΔV_i并更新速度变量V_i，1≤i≤N；

S8，更新变量状态：Q_i＝Q_i+V_i，令n＝n+1，返回步骤S5。

2.如权利要求1所述的移动测量平台激光雷达旋转与平移参数计算方法，其特征在于，步骤S7的具体方法为：

S71，计算速度变量变化值

其中，c₁、c₂为常数，r₁、r₂为随机数;

S72，计算搜索步长因子

其中，c₃、c₄为常数，n为迭代次数;

S73，分别将V_i ^x,V_i ^y,V_i ^z作为V_i ^P，对V_i ^P＝λ·norm(norm(V_i ^P)+rand(1,c₅)·norm(ΔV_i ^P))进行计算，得到V_i ^Px、V_i ^Py、V_i ^Pz，其中，ΔV_i ^P分别为ΔV_i中V_i ^x,V_i ^y,V_i ^z对应的分量，norm()为归一化函数，rand(1,c₅)表示1到c₅的随机数，c₅为常数；

S74，分别将V_i ^θ,V_i ^ω,V_i ^ψ作为V_i ^A，对V_i ^A＝λ·norm(norm(V_i ^A)+rand(1,c₅)·norm(ΔV_i ^A))进行计算，得到V_i ^Aθ、V_i ^Aω、V_i ^Aψ，其中，ΔV_i ^A为变量ΔV_i中V_i ^θ,V_i ^ω,V_i ^ψ对应的分量，norm()为归一化函数，rand(1,c₅)表示1到c₅的随机数，c₅为常数；

S75，用V_i ^Px、V_i ^Py、V_i ^Pz更新V_i的前三个分量，用V_i ^Aθ、V_i ^Aω、V_i ^Aψ更新V_i的后三个分量。

3.如权利要求2所述的移动测量平台激光雷达旋转与平移参数计算方法，其特征在于，1≤c₁≤c₂≤2,r₁,r₂∈[0,1]的随机数.

4.如权利要求3所述的移动测量平台激光雷达旋转与平移参数计算方法，其特征在于，c₁＝1.5,c₂＝2。

5.如权利要求2所述的移动测量平台激光雷达旋转与平移参数计算方法，其特征在于，1≤c₃≤10，50≤c₄≤200。

6.如权利要求5所述的移动测量平台激光雷达旋转与平移参数计算方法，其特征在于，c₃=2，c₄=100。

7.如权利要求2所述的移动测量平台激光雷达旋转与平移参数计算方法，其特征在于，1≤c₅≤10。

8.如权利要求7所述的移动测量平台激光雷达旋转与平移参数计算方法，其特征在于，c₅=1.5。

9.如权利要求1所述的移动测量平台激光雷达旋转与平移参数计算方法，其特征在于，步骤S5中转换函数 $f(P_j,{\mathrm{POS}}_j,Q_i)=R_g^P(R_P^L\left[\begin{array}{c}x\\ y\\ z\end{array}\right]+\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right])+\left[\begin{array}{c}{\stackrel{\&OverBar;}{X}}_P\\ {\stackrel{\&OverBar;}{Y}}_P\\ {\stackrel{\&OverBar;}{Z}}_P\end{array}\right],$ 其用于把激光雷达坐标系中的点(x,y,z)转换到东北高坐标系下，其中，

由POS_j中的三个角度分量按欧拉角构造的旋转矩阵；由Q_i的三个角度分量按欧拉角构造的旋转矩阵；(x,y,z)^T为P_j，(X₀,Y₀,Z₀)^T为Q_i的前三个坐标分量；为POS_j中的前三个位置分量。

10.如权利要求1所述的移动测量平台激光雷达旋转与平移参数计算方法，其特征在于，ε=1e-6。

Images