[发明专利]基于三维点云的定位方法、装置、设备及存储介质

权利要求书

1.一种基于三维点云的定位方法，其特征在于，包括：

获取车辆的组合导航数据和激光雷达测量的点云数据；

对所述组合导航数据进行解析，得到所述车辆的当前组合导航位姿信息，基于所述当前组合导航位姿信息确定所述车辆在点云定位中点云变换矩阵和平移矩阵；

基于所述变换矩阵和所述平移矩阵将所述车辆的当前帧点云与预先建立的地图进行匹配，得到所述车辆的激光雷达位姿信息；

对所述组合导航数据进行解析得到所述车辆的当前姿态信息，将所述当前姿态信息与所述激光雷达位姿信息结合，确定所述车辆的位姿状态计算方程；

采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位。

2.根据权利要求1所述的基于三维点云的定位方法，其特征在于，所述车辆的激光雷达位姿信息为：

X＝[L^T，W^T，V^T]^T

其中，L＝(l_x，l_y，l_z)^T用于表示所述激光雷达的位置信息，其中l_x为激光雷达x轴坐标信息，l_y为激光雷达y轴坐标信息，l_z为激光雷达z轴坐标信息；用于表示所述激光雷达的欧拉角，其中为俯仰角，θ为横滚角，ψ为航向角，V＝(v_x，v_y，v_z)^T用于表示所述激光雷达的速度，其中v_x为x轴速度，v_y为y轴速度，v_z为z轴速度。

3.根据权利要求1所述的基于三维点云的定位方法，其特征在于，所述车辆的当前姿态信息为：

U＝[w^T，a^T]^T

其中，w＝(w_x，w_y，w_z)^T用于表示所述车辆位姿的角速度，其中w_x为x轴的角速度，w_y为y轴的角速度，w_z为z轴的角速度；a＝(a_x，a_y，a_z)^T用于表示所述车辆位姿的加速度，其中a_x为x轴的加速度，a_y为y轴的加速度，a_z为z轴的加速度。

4.根据权利要求1所述的基于三维点云的定位方法，其特征在于，所述车辆的位姿状态计算方程为：

其中，表示车辆位姿速度状态方程，V为车辆速度，表示车辆位姿角速度状态方程，其中w表示车辆位姿角速度；

为方向余弦矩阵；为状态转移矩阵；为重力矩阵，其中g为重力加速度。

5.根据权利要求1所述的基于三维点云的定位方法，其特征在于，所述基于所述变换矩阵和所述平移矩阵将所述车辆的当前帧点云与预先建立的地图进行匹配之前，还包括：

对所述激光雷达测量的点云数据进行降采样处理，并对降采样处理后的所述点云数据进行特征提取。