CN113538699A - Positioning method, device and equipment based on three-dimensional point cloud and storage medium - Google Patents

Abstract

The present application provides a three-dimensional point cloud-based positioning method, device, equipment and storage medium, and relates to the technical field of automatic driving. The cloud data and integrated navigation data are processed. First, the current pose information of the vehicle is determined based on the integrated navigation data, and the transformation matrix and translation matrix in the point cloud positioning are determined. Then, the lidar pose of the vehicle is determined based on the transformation matrix and translation matrix. information, and then determine the vehicle's attitude calculation method equation based on the lidar position and attitude information and the vehicle's attitude information, and then use the extended Kalman filter method to locate the vehicle based on the position and attitude state calculation equation, which is more in line with the form of the vehicle rules to ensure the accuracy of vehicle positioning.

Description

Positioning method, device, device and storage medium based on 3D point cloud

technical field

The present application relates to the technical field of automatic driving, and in particular, the present application relates to a positioning method, apparatus, device and storage medium based on a three-dimensional point cloud.

Background technique

In the positioning process based on high-precision maps, due to the non-real-time nature of the map and sensor noise, there is a problem of inconsistency between the real-time collected data and the map. This problem usually affects the accuracy of the localization algorithm based on map matching, and even causes local optimality or the situation that the localization cannot converge. In view of the existing problems, there are many positioning methods that fuse different sensors. Most of the fusion methods are based on KF (Kalman filter). However, in the real vehicle motion process, it is not a linear system, and due to the noise in the point cloud, Using classic registration algorithms, such as ICP or NDT, there are cumulative errors, resulting in non-convergence of localization during the point cloud localization process, and ultimately resulting in inaccurate localization.

SUMMARY OF THE INVENTION

The purpose of the present application is to solve at least one of the above-mentioned technical defects, especially the existing technology has accumulated errors, resulting in non-convergence of positioning in the process of point cloud positioning, and finally resulting in the technical defects of inaccurate positioning.

In a first aspect, a three-dimensional point cloud-based positioning method is provided, including:

Obtain the integrated navigation data of the vehicle and the point cloud data measured by the lidar;

Analyzing the combined navigation data to obtain the current combined navigation pose information of the vehicle, and determining a point cloud transformation matrix and a translation matrix of the vehicle in point cloud positioning based on the current combined navigation pose information;

Matching the current frame point cloud of the vehicle with a pre-established map based on the transformation matrix and the translation matrix, to obtain the lidar pose information of the vehicle;

Analyzing the combined navigation data to obtain the current attitude information of the vehicle, and combining the current attitude information with the lidar attitude information to determine the vehicle's attitude and attitude calculation equation;

The vehicle is positioned based on the pose state calculation equation using an extended Kalman filter method.

As a possible implementation manner of the present application, in this implementation manner, the lidar pose information of the vehicle is:

X=[L ^T ,W ^T ,V ^T ] ^T

用于表示所述激光雷达的欧拉角，其中为

俯仰角，θ为横滚角，ψ为航向角，V＝(v_x,v_y,v_z)^T用于表示所述激光雷达的速度，其中v_x为x轴速度，v_y为y轴速度，v_z为z轴速度。Wherein, L=(l _x , l _y , l _z ) ^T is used to represent the position information of the lidar, where l _x is the x-axis coordinate information of the lidar, ly is the _y -axis coordinate information of the lidar, and l _z is Lidar z-axis coordinate information;

is used to represent the Euler angles of the lidar, where

Pitch angle, θ is the roll angle, ψ is the heading angle, V=(v _x , v _y , v _z ) ^T is used to represent the speed of the lidar, where v _x is the x-axis speed, and v _y is the y-axis Velocity, v _z is the z-axis velocity.

As a possible implementation manner of the present application, in this implementation manner, the current attitude information of the vehicle is:

U=[w ^T ,a ^T ] ^T

Wherein, w=(w _x , w _y , w _z ) ^T is used to represent the angular velocity of the vehicle pose, where w _x is the angular velocity of the x-axis, w _y is the angular velocity of the y-axis, and w _z is the angular velocity of the z-axis ; a=(a _x , a _y , a _z ) ^T is used to represent the acceleration of the vehicle posture, where a _x is the acceleration of the x-axis, a _y is the acceleration of the y-axis, and a _z is the acceleration of the z-axis.

As a possible implementation manner of the present application, in this implementation manner, the calculation equation of the pose state of the vehicle is:

表示车辆位姿速度状态方程，为车辆速度，

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

Represents the state equation of the vehicle pose and velocity, which is the vehicle speed,

Represents the state equation of the vehicle pose angular velocity, where w represents the vehicle pose angular velocity;

为方向余弦矩阵；

为状态转移矩阵；

为重力矩阵，其中g为重力加速度。

is the direction cosine matrix;

is the state transition matrix;

is the gravity matrix, where g is the acceleration of gravity.

As a possible implementation manner of the present application, in this implementation manner, before matching the point cloud of the current frame of the vehicle with the pre-established map based on the transformation matrix and the translation matrix, the method further includes:

Perform down-sampling processing on the point cloud data measured by the lidar, and perform feature extraction on the point cloud data after down-sampling processing.

As a possible implementation manner of the present application, in this implementation manner, using the extended Kalman filter method to locate the vehicle based on the pose and state calculation equation includes:

Calculate the a priori estimated covariance according to the formula (P ⁺ ) ^-1 =k·P ^-1 +(1-k)·C ^T R ^-1 C;

Determine the attitude prediction equation as

Determine the Kalman gain as K=PC ^T (k·R+ ^CT PC) ⁻¹ ;

对激光雷达位姿新进行更新；using the formula

Update the lidar pose;

Determine the updated covariance as P ⁺ =P-(1-k) ^-1 ·KCP;

为先验估计位姿，R为系统误差矩阵，C为点云初始配准时的姿态估计信息投影矩阵，F为激光雷达的姿态信息矩阵，k为帧间点云的匹配度，

其中，P_j表示当前帧点云中的一个点，Pi表示地图与中Pj最近的对应点。where P ⁺ is the prior estimated covariance matrix, P is the covariance matrix,

is the a priori estimated pose, R is the system error matrix, C is the projection matrix of the pose estimation information during the initial registration of the point cloud, F is the attitude information matrix of the lidar, k is the matching degree of the point cloud between frames,

Among them, P _j represents a point in the point cloud of the current frame, and Pi represents the closest corresponding point to Pj in the map.

In a second aspect, a positioning device based on a three-dimensional point cloud is provided, the device comprising:

The data acquisition module is used to acquire the integrated navigation data of the vehicle and the point cloud data measured by the lidar;

The matrix determination module is used to analyze the combined navigation data, obtain the current combined navigation pose information of the vehicle, and determine the point cloud transformation matrix and the point cloud transformation matrix of the vehicle in the point cloud positioning based on the current combined navigation pose information. translation matrix;

a pose determination module, configured to match the current frame point cloud of the vehicle with a pre-established map based on the transformation matrix and the translation matrix to obtain the lidar pose information of the vehicle;

an equation determination module, configured to analyze the combined navigation data to obtain the current attitude information of the vehicle, and combine the current attitude information with the lidar attitude information to determine the vehicle's attitude and attitude calculation equation;

The positioning module is configured to use the extended Kalman filter method to locate the vehicle based on the pose and state calculation equation.

In a third aspect, an electronic device is provided, the electronic device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above when executing the program 3D point cloud-based localization method.

In a fourth aspect, a computer-readable storage medium is provided, the storage medium stores at least one instruction, at least one piece of program, code set or instruction set, the at least one instruction, the at least one piece of program, the code set Or the instruction set is loaded and executed by the processor to implement the above-mentioned three-dimensional point cloud-based positioning method.

In this embodiment of the present application, the combined navigation data and the point cloud data measured by the lidar are obtained respectively, and the point cloud data and the combined navigation data are processed respectively, the current position and attitude information of the vehicle is determined based on the combined navigation data, and the point cloud positioning is determined. transformation matrix and translation matrix in The Mann filter method locates the vehicle based on the pose state calculation equation, which is more in line with the formal rules of the vehicle and ensures the accuracy of the vehicle positioning.

Description of drawings

In order to illustrate the technical solutions in the embodiments of the present application more clearly, the following briefly introduces the accompanying drawings that need to be used in the description of the embodiments of the present application.

1 is a schematic flowchart of a positioning method based on a three-dimensional point cloud provided by an embodiment of the present application;

2 is a flowchart of a positioning method according to an embodiment of the present application;

3 is a schematic structural diagram of a positioning device based on a three-dimensional point cloud provided by an embodiment of the present application;

FIG. 4 is a schematic structural diagram of an electronic device according to an embodiment of the present application.

The above and other features, advantages and aspects of the various embodiments of the present application will become more apparent with reference to the following detailed description in conjunction with the accompanying drawings. Throughout the drawings, the same or similar reference numbers refer to the same or similar elements. It should be understood that the drawings are schematic and that the originals and elements are not necessarily drawn to scale.

Detailed ways

The following describes in detail the embodiments of the present application, examples of which are illustrated in the accompanying drawings, wherein the same or similar reference numerals refer to the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present application, but not to be construed as a limitation on the present application.

It will be understood by those skilled in the art that the singular forms "a", "an", "the" and "the" as used herein can include the plural forms as well, unless expressly stated otherwise. It should be further understood that the word "comprising" used in the specification of this application refers to the presence of stated features, integers, steps, operations, elements and/or components, but does not preclude the presence or addition of one or more other features, Integers, steps, operations, elements, components and/or groups thereof. It will be understood that when we refer to an element as being "connected" or "coupled" to another element, it can be directly connected or coupled to the other element or intervening elements may also be present. Furthermore, "connected" or "coupled" as used herein may include wirelessly connected or wirelessly coupled. As used herein, the term "and/or" includes all or any element and all combination of one or more of the associated listed items.

In order to make the objectives, technical solutions and advantages of the present application clearer, the embodiments of the present application will be further described in detail below with reference to the accompanying drawings.

The three-dimensional point cloud-based positioning method, device, electronic device and computer-readable storage medium provided by the present application aim to solve the above technical problems in the prior art.

The technical solutions of the present application and how the technical solutions of the present application solve the above-mentioned technical problems will be described in detail below with specific examples. The following specific embodiments may be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments. The embodiments of the present application will be described below with reference to the accompanying drawings.

An embodiment of the present application provides a positioning method based on a three-dimensional point cloud. As shown in FIG. 1 , the method includes:

Step S101, acquiring the combined navigation data of the vehicle and the point cloud data measured by the lidar;

Step S102, analyze the combined navigation data to obtain the current combined navigation pose information of the vehicle, and determine the point cloud transformation matrix and translation matrix of the vehicle in the point cloud positioning based on the current combined navigation pose information;

Step S103, matching the current frame point cloud of the vehicle with a pre-established map based on the transformation matrix and the translation matrix to obtain the lidar pose information of the vehicle;

Step S104, analysing the combined navigation data to obtain the current attitude information of the vehicle, combining the current attitude information with the lidar attitude information to determine the vehicle attitude and attitude calculation equation;

Step S105, using an extended Kalman filter method to locate the vehicle based on the pose and state calculation equation.

In the embodiment of this application, the provided positioning method based on 3D point cloud is suitable for the field of automatic driving or navigation technology. During the positioning process based on high-precision map, due to the non-real-time nature of the map and sensor noise, the difference between the real-time collected data and the map inconsistencies exist. This problem usually affects the accuracy of the localization algorithm based on map matching, and even causes local optimality or the situation that the localization cannot converge. In view of the existing problems, there are many positioning methods that fuse different sensors. Most of the fusion methods are based on KF (Kalman filter). However, in the real vehicle motion process, it is not a linear system, and due to the noise in the point cloud, Using classic registration algorithms, such as ICP or NDT, there are accumulated errors, resulting in non-convergence of positioning in the process of point cloud positioning, and ultimately inaccurate positioning. For this reason, a point cloud based on EKF (Extended Kalman Filter) is proposed. positioning method.

For the convenience of description, taking a specific embodiment as an example, the combined navigation of lidar and GPS is installed on an autonomous vehicle, or a vehicle with automatic navigation function, and the sensors on the vehicle receive the combined navigation data and the point cloud data measured by the lidar. , and process the combined navigation data and the point cloud data measured by the lidar respectively, wherein, for the combined navigation data, the analysis is performed first to obtain the pose information of the vehicle at the current moment, where the pose information Z ₀ =(x ₀ , y ₀ , z ₀ , yaw ₀ , pitch ₀ , roll ₀ ) as the initial positioning pose information of the vehicle, and then calculate the midpoint of the point cloud positioning of the vehicle in the point cloud data measured by the lidar according to the pose information at the current moment Cloud transformation matrix R ₀ and translation matrix t ₀ , and then based on the transformation matrix and translation matrix, the current frame point cloud is matched with the pre-established map, and the lidar pose information of the vehicle is determined

X=[L ^T ,W ^T ,V ^T ] ^T

用于表示所述激光雷达的俯仰角、横滚角、以及航向角的欧拉角，V＝(v_x,v_y,vz)^T用于表示所述激光雷达的速度。Among them, L=(l _x , _ly , lz) ^T is used to represent the position information of the lidar,

The Euler angle used to represent the pitch angle, roll angle, and heading angle of the lidar, V=(v _x , _vy , vz) ^T is used to represent the speed of the lidar.

Analyze the combined navigation data to obtain the current attitude information of the vehicle, combine the current attitude information with the lidar attitude information, and determine the vehicle's attitude and attitude calculation equation:

为方向余弦矩阵；

为状态转移矩阵；

为重力矩阵。

is the direction cosine matrix;

is the state transition matrix;

is the gravity matrix.

确定卡尔曼增益为K＝PC^T(k·R+C^TPC)^-1；采用公式

对激光雷达位姿新进行更新；确定更新后的协方差为P⁺＝P-(1-k)^-1·KCP；其中，P为先验估计协方差矩阵，R为系统误差矩阵，C为点云初始配准时的姿态估计信息投影矩阵，F为激光雷达的姿态信息矩阵，k为帧间点云的匹配度，

其中，P_j表示当前帧点云中的一个点，P_i表示地图与中P_j最近的对应点。Using the extended Kalman filter method to locate the vehicle based on the pose state calculation equation, including: according to the formula (P ⁺ ) ^-1 =k·P ^-1 +(1-k)·C ^T R ^-1 C Calculate the prior estimated covariance; determine the attitude prediction equation as

Determine the Kalman gain as K=PC ^T (k·R+ ^CT PC) ^-1 ; adopt the formula

Update the lidar pose; determine the updated covariance as P ⁺ =P-(1-k) ^-1 ·KCP; where P is the prior estimated covariance matrix, R is the system error matrix, and C is the The projection matrix of the attitude estimation information during the initial registration of the point cloud, F is the attitude information matrix of the lidar, k is the matching degree of the point cloud between frames,

Among them, P _j represents a point in the point cloud of the current frame, and P _i represents the closest corresponding point to P _j in the map.

In this embodiment of the present application, the combined navigation data and the point cloud data measured by the lidar are obtained respectively, and the point cloud data and the combined navigation data are processed respectively, the current position and attitude information of the vehicle is determined based on the combined navigation data, and the point cloud positioning is determined. transformation matrix and translation matrix in The Mann filter method locates the vehicle based on the pose state calculation equation, which is more in line with the formal rules of the vehicle and ensures the accuracy of the vehicle positioning.

As a possible implementation manner of the present application, in this implementation manner, the current attitude information of the vehicle is obtained by analyzing the integrated navigation data, and the current attitude information of the vehicle is obtained as:

U=[w ^T ,a ^T ] ^T

Wherein, w=(w _x , w _y , w _z ) ^T is used to represent the angular velocity of the vehicle pose, and a=(a _x , a _y , _az ) ^T is used to represent the acceleration of the vehicle pose.

As a possible implementation manner of the present application, in this implementation manner, before matching the point cloud of the current frame of the vehicle with the pre-established map based on the transformation matrix and the translation matrix, the method further includes:

Perform down-sampling processing on the point cloud data measured by the lidar, and perform feature extraction on the point cloud data after down-sampling processing.

In the embodiment of the present application, the vehicle acquires point cloud data and integrated navigation data measured by lidar, and performs different processing on these two sets of data. The obtained point cloud data is subjected to down-sampling processing. The purpose of down-sampling is to filter out points that are too far from the lidar and points that are too far from the center of the lidar, so as to improve the accuracy and efficiency of registration. Then, extract feature points from the point cloud data: the points of a scan are classified by the curvature value, and the feature points whose curvature is greater than the threshold are edge points, and those whose curvature is less than the threshold are plane points. In order to distribute the feature points evenly in the environment, a scan is divided into 4 independent sub-regions, each sub-region provides at most 2 edge points and 4 planes. In addition, unstable feature points (defect points) are excluded. At the same time, perform data analysis on the acquired integrated navigation data, obtain GPS data, and perform UTM transformation on the GPS data, that is, convert the latitude and longitude to local coordinates according to the GPS data, and the result obtained from the transformation is used to calculate the rotation matrix of the initial point cloud registration. R and translation matrix t. When the point cloud of the current frame has been matched to the pre-built map, the LiDAR pose measurement value of the current vehicle can be calculated, and GPS data is no longer required.

用于表示所述激光雷达的俯仰角、横滚角、以及航向角的欧拉角，V＝(v_x,v_y,v_z)^T用于表示所述车辆的速度。经过组合导航原始数据解析后可以得到姿态信息U＝[w^T,a^T]^T，其中，w＝(w_x,w_y,w_z)^T用于表示所述激光雷达的角速度，a＝(a_x,a_y,a_z)^T用于表示所述激光雷达的加速度。接着可以建立车辆位姿估计状态方程：In the embodiment of the present application, as shown in FIG. 2 , after processing the point cloud data and integrated navigation data measured by the lidar, EKF fusion needs to be performed to establish the Extended Kalman (EKF) state and observation state model. Lidar attitude information X=[L ^T , W ^T , V ^T ] ^T , where L=(l _x , _ly , l _z ) ^T is used to represent the position information of the lidar,

The Euler angle used to represent the pitch angle, roll angle, and heading angle of the lidar, V=(v _x , _vy , v _z ) ^T is used to represent the speed of the vehicle. The attitude information U=[w ^T , a ^T ] ^T can be obtained after analyzing the original data of the combined navigation, where w=(w _x , w _y , w _z ) ^T is used to represent the angular velocity of the lidar, a=( a _x , a _y , _az ) ^T is used to represent the acceleration of the lidar. Then the state equation for vehicle pose estimation can be established:

Among them, E _w is the state transition matrix, R _w is the direction cosine matrix, and G represents the gravity. In the process of vehicle motion, the direction of the x-axis of the combined navigation and lidar navigation is consistent, so the corresponding matrix is:

卡尔曼增益：K＝PCTk·R+CTPC-1；LiDAR姿态更新：

更新后验估计协方差：P⁺＝P-(1-k)^-1·KCP；其中P为先验估计协方差矩阵，R为系统误差矩阵，C为点云初始配准时的姿态估计信息投影矩阵，F为激光雷达输出的姿态信息矩阵，k为帧间点云的匹配度，

P_j表示当前帧点云中的一个点，P_i表示地图中与P_j最近的对应点；根据EKF最后得到激光雷达测量的姿态的最优估计值

经过坐标变换得到当前车辆的定位信息。Finally, predict and update the EKF: a priori estimated covariance: (P ⁺ ) ^-1 = k · P ^-1 + (1-k) · C ^T R ^-1 C; lidar attitude prediction equation:

Kalman gain: K=PCTk R+CTPC-1; LiDAR attitude update:

Update the posterior estimated covariance: P ⁺ =P-(1-k) ^-1 ·KCP; where P is the prior estimated covariance matrix, R is the system error matrix, and C is the attitude estimation information projection during the initial registration of the point cloud matrix, F is the attitude information matrix output by lidar, k is the matching degree of point cloud between frames,

P _j represents a point in the point cloud of the current frame, and P _i represents the corresponding point closest to P _j in the map; according to the EKF, the optimal estimated value of the attitude measured by the lidar is finally obtained

The positioning information of the current vehicle is obtained through coordinate transformation.

In this embodiment of the present application, the combined navigation data and the point cloud data measured by the lidar are obtained respectively, and the point cloud data and the combined navigation data are processed respectively, the current position and attitude information of the vehicle is determined based on the combined navigation data, and the point cloud positioning is determined. transformation matrix and translation matrix in The Mann filter method locates the vehicle based on the pose state calculation equation, which is more in line with the formal rules of the vehicle and ensures the accuracy of the vehicle positioning.

The embodiments of the present application provide a positioning method, device, device, and storage medium device based on a three-dimensional point cloud. As shown in FIG. 3 , the three-dimensional point cloud-based positioning device 30 may include: a data acquisition module 301 , a matrix determination module 302, a pose determination module 303, an equation determination module 304, and a positioning module 305, wherein,

The data acquisition module 301 is used to acquire the integrated navigation data of the vehicle and the point cloud data measured by the lidar;

A matrix determination module 302, configured to analyze the combined navigation data, obtain the current combined navigation pose information of the vehicle, and determine the point cloud transformation matrix of the vehicle in the point cloud positioning based on the current combined navigation pose information and translation matrix;

A pose determination module 303, configured to match the current frame point cloud of the vehicle with a pre-established map based on the transformation matrix and the translation matrix to obtain the lidar pose information of the vehicle;

Equation determination module 304, configured to parse the combined navigation data to obtain the current attitude information of the vehicle, combine the current attitude information with the lidar attitude information, and determine the vehicle's attitude and attitude state calculation equation ;

The positioning module 305 is configured to use the extended Kalman filter method to locate the vehicle based on the pose and state calculation equation.

As a possible implementation manner of the present application, in this implementation manner, the current attitude information of the vehicle is:

U=[w ^T ,a ^T ] ^T

Wherein, w=(w _x , w _y , w _z ) ^T is used to represent the angular velocity of the vehicle pose, and a=(a _x , a _y , _az ) ^T is used to represent the acceleration of the vehicle pose.

As a possible implementation manner of the present application, in this implementation manner, the calculation equation of the pose state of the vehicle is:

in,

为方向余弦矩阵；

为状态转移矩阵；

为重力矩阵。

is the direction cosine matrix;

is the state transition matrix;

is the gravity matrix.

As a possible implementation manner of the present application, in this implementation manner, before matching the point cloud of the current frame of the vehicle with the pre-established map based on the transformation matrix and the translation matrix, the method further includes:

Perform down-sampling processing on the point cloud data measured by the lidar, and perform feature extraction on the point cloud data after down-sampling processing.

As a possible implementation manner of the present application, in this implementation manner, using the extended Kalman filter method to locate the vehicle based on the pose and state calculation equation includes:

Calculate the a priori estimated covariance according to the formula (P ⁺ ) ^-1 =k·P ^-1 +(1-k)·C ^T R ^-1 C;

Determine the attitude prediction equation as

Determine the Kalman gain as K=PC ^T (k·R+ ^CT PC) ⁻¹ ;

对激光雷达位姿新进行更新；using the formula

Update the lidar pose;

Determine the updated covariance as P ⁺ =P-(1-k) ^-1 ·CP;

其中，P_j表示当前帧点云中的一个点，P_i表示地图与中P_j最近的对应点。Among them, P is the prior estimation covariance matrix, R is the system error matrix, C is the attitude estimation information projection matrix during the initial registration of the point cloud, F is the attitude information matrix of the lidar, and k is the matching degree of point clouds between frames,

Among them, P _j represents a point in the point cloud of the current frame, and P _i represents the closest corresponding point to P _j in the map.

In this embodiment of the present application, the combined navigation data and the point cloud data measured by the lidar are obtained respectively, and the point cloud data and the combined navigation data are processed respectively, the current position and attitude information of the vehicle is determined based on the combined navigation data, and the point cloud positioning is determined. transformation matrix and translation matrix in The Mann filter method locates the vehicle based on the pose state calculation equation, which is more in line with the formal rules of the vehicle and ensures the accuracy of the vehicle positioning.

An embodiment of the present application provides an electronic device, the electronic device includes: a memory and a processor; at least one program, stored in the memory, is used to acquire the integrated navigation data of the vehicle and the information measured by the lidar when executed by the processor. point cloud data; analyze the combined navigation data to obtain the current combined navigation pose information of the vehicle, and determine the point cloud transformation matrix and translation matrix of the vehicle in the point cloud positioning based on the current combined navigation pose information ; Based on the transformation matrix and the translation matrix, the current frame point cloud of the vehicle is matched with a pre-established map to obtain the lidar pose information of the vehicle; The integrated navigation data is analyzed to obtain the The current attitude information of the vehicle, and the current attitude information is combined with the lidar position and attitude information to determine the vehicle's attitude and state calculation equation; the extended Kalman filtering method is used to calculate the position and attitude state calculation equation based on the Position the vehicle.

Compared with the prior art, it can be realized: the embodiment of the present application obtains the integrated navigation data and the point cloud data measured by the lidar respectively, and processes the point cloud data and the integrated navigation data respectively, and first determines the current state of the vehicle based on the integrated navigation data. pose information, and determine the transformation matrix and translation matrix in the point cloud positioning, and then determine the vehicle's lidar pose information based on the transformation matrix and translation matrix, and then determine the vehicle's position and attitude information based on the lidar pose information and the vehicle's attitude information. Then, the extended Kalman filter method is used to locate the vehicle based on the posture and state calculation equation, which is more in line with the formal rules of the vehicle and ensures the accuracy of the vehicle positioning.

In an optional embodiment, an electronic device is provided. As shown in FIG. 4 , the electronic device 4000 shown in FIG. 4 includes: a processor 4001 and a memory 4003 . The processor 4001 is connected to the memory 4003, for example, through the bus 4002. Optionally, the electronic device 4000 may also include a transceiver 4004 . It should be noted that, in practical applications, the transceiver 4004 is not limited to one, and the structure of the electronic device 4000 does not constitute a limitation to the embodiments of the present application.

The processor 4001 may be a CPU (Central Processing Unit, central processing unit), a general-purpose processor, a DSP (Digital Signal Processor, data signal processor), an ASIC (Application Specific Integrated Circuit, an application-specific integrated circuit), an FPGA (Field Programmable Gate Array, Field Programmable Gate Array) or other programmable logic devices, transistor logic devices, hardware components, or any combination thereof. It may implement or execute the various exemplary logical blocks, modules and circuits described in connection with this disclosure. The processor 4001 may also be a combination for realizing computing functions, such as a combination of one or more microprocessors, a combination of a DSP and a microprocessor, and the like.

The bus 4002 may include a path to transfer information between the components described above. The bus 4002 may be a PCI (Peripheral Component Interconnect, Peripheral Component Interconnect Standard) bus or an EISA (Extended Industry Standard Architecture, Extended Industry Standard Architecture) bus or the like. The bus 4002 can be divided into an address bus, a data bus, a control bus, and the like. For ease of presentation, only one thick line is used in FIG. 4, but it does not mean that there is only one bus or one type of bus.

The memory 4003 can be a ROM (Read Only Memory, read only memory) or other types of static storage devices that can store static information and instructions, a RAM (Random Access Memory, random access memory) or other types that can store information and instructions. A dynamic storage device can also be an EEPROM (Electrically Erasable Programmable Read Only Memory), a CD-ROM (Compact Disc Read Only Memory, a CD-ROM), or other CD-ROM storage, CD-ROM storage (including compressed CD-ROM, laser compact disc, compact disc, digital versatile disc, blu-ray disc, etc.), magnetic disk storage medium or other magnetic storage device, or any other medium capable of carrying or storing desired program code in the form of instructions or data structures and capable of being accessed by a computer , but not limited to this.

The memory 4003 is used for storing the application program code for executing the solution of the present application, and the execution is controlled by the processor 4001 . The processor 4001 is configured to execute the application program code stored in the memory 4003 to implement the content shown in the foregoing method embodiments.

Embodiments of the present application provide a computer-readable storage medium, where a computer program is stored on the computer-readable storage medium, and when the computer program runs on the computer, the computer can execute the corresponding content in the foregoing method embodiments. Compared with the prior art, the embodiment of the present application obtains the combined navigation data and the point cloud data measured by the lidar respectively, and processes the point cloud data and the combined navigation data respectively, and first determines the current position and attitude of the vehicle based on the combined navigation data. information, and determine the transformation matrix and translation matrix in the point cloud positioning, and then determine the vehicle's lidar pose information based on the transformation matrix and translation matrix, and then determine the vehicle's attitude calculation based on the lidar pose information and the vehicle's attitude information. Then, the extended Kalman filtering method is used to locate the vehicle based on the pose and state calculation equation, which is more in line with the formal rules of the vehicle and ensures the accuracy of the vehicle positioning.

It should be understood that although the various steps in the flowchart of the accompanying drawings are sequentially shown in the order indicated by the arrows, these steps are not necessarily executed in sequence in the order indicated by the arrows. Unless explicitly stated herein, the execution of these steps is not strictly limited to the order and may be performed in other orders. Moreover, at least a part of the steps in the flowchart of the accompanying drawings may include multiple sub-steps or multiple stages, and these sub-steps or stages are not necessarily executed at the same time, but may be executed at different times, and the execution sequence is also It does not have to be performed sequentially, but may be performed alternately or alternately with other steps or at least a portion of sub-steps or stages of other steps.

The above are only part of the embodiments of the present application. It should be pointed out that for those skilled in the art, without departing from the principles of the present application, several improvements and modifications can also be made. It should be regarded as the protection scope of this application.

Claims (9)

1. a positioning method based on three-dimensional point cloud, is characterized in that, comprises: Obtain the integrated navigation data of the vehicle and the point cloud data measured by the lidar; Analyzing the combined navigation data to obtain the current combined navigation pose information of the vehicle, and determining a point cloud transformation matrix and a translation matrix of the vehicle in point cloud positioning based on the current combined navigation pose information; Matching the current frame point cloud of the vehicle with a pre-established map based on the transformation matrix and the translation matrix, to obtain the lidar pose information of the vehicle; Analyzing the combined navigation data to obtain the current attitude information of the vehicle, and combining the current attitude information with the lidar attitude information to determine the vehicle's attitude and attitude calculation equation; The vehicle is positioned based on the pose state calculation equation using an extended Kalman filter method. 2. The three-dimensional point cloud-based positioning method according to claim 1, wherein the laser radar pose information of the vehicle is: X=[L ^T , W ^T , V ^T ] ^T Among them, L=(l _x , _ly , l _z ) ^T is used to represent the position information of the lidar, where l _x is the x-axis coordinate information of the lidar, _{ly is the y-axis coordinate information of the lidar, and l z is} Lidar z-axis coordinate information;

is used to represent the Euler angles of the lidar, where

Pitch angle, θ is the roll angle, ψ is the heading angle, V=(v _x , v _y , v _z ) ^T is used to represent the speed of the lidar, where v _x is the x-axis speed, and v _y is the y-axis Velocity, v _z is the z-axis velocity. 3. The positioning method based on three-dimensional point cloud according to claim 1, is characterized in that, the current attitude information of described vehicle is: U=[w ^T , a ^T ] ^T Wherein, w=(w _x , w _y , w _z ) ^T is used to represent the angular velocity of the vehicle pose, where w _x is the angular velocity of the x-axis, w _y is the angular velocity of the y-axis, and w _z is the angular velocity of the z-axis ; a=(a _x , a _y , _az ) ^T is used to represent the acceleration of the vehicle posture, where a _x is the acceleration of the x-axis, a _y is the acceleration of the y-axis, and a _z is the acceleration of the z-axis. 4. The positioning method based on three-dimensional point cloud according to claim 1, is characterized in that, the posture state calculation equation of described vehicle is:

in,

represents the state equation of vehicle pose and velocity, V is the vehicle speed,

Represents the state equation of the vehicle pose angular velocity, where w represents the vehicle pose angular velocity;

is the direction cosine matrix;

is the state transition matrix;

is the gravity matrix, where g is the acceleration of gravity. 5. The three-dimensional point cloud-based positioning method according to claim 1, wherein before the current frame point cloud of the vehicle is matched with a pre-established map based on the transformation matrix and the translation matrix ,Also includes: Perform down-sampling processing on the point cloud data measured by the lidar, and perform feature extraction on the point cloud data after down-sampling processing. 6. The three-dimensional point cloud-based positioning method according to claim 1, wherein the described vehicle is positioned based on the pose state calculation equation using an extended Kalman filter method, comprising: Calculate the a priori estimated covariance according to the formula (P ⁺ ) ^-1 =k·P ^-1 +(1-k)·C ^T R ^-1 C; Determine the attitude prediction equation as

Determine the Kalman gain as K=PC ^T (k·R+ ^CT PC) ⁻¹ ; using the formula

Update the lidar pose; Determine the updated covariance as P ⁺ =P-(1-k) ^-1 ·KCP; Among them, P+ is the prior estimated covariance matrix, P is the covariance matrix,

is the a priori estimated pose, R is the system error matrix, C is the projection matrix of the pose estimation information during the initial registration of the point cloud, F is the attitude information matrix of the lidar, k is the matching degree of the point cloud between frames,

Among them, Pj represents a point in the point cloud of the current frame, and Pi represents the closest corresponding point to Pj in the map. 7. A positioning device based on three-dimensional point cloud, is characterized in that, comprises: The data acquisition module is used to acquire the integrated navigation data of the vehicle and the point cloud data measured by the lidar; The matrix determination module is used to analyze the combined navigation data, obtain the current combined navigation pose information of the vehicle, and determine the point cloud transformation matrix and the point cloud transformation matrix of the vehicle in the point cloud positioning based on the current combined navigation pose information. translation matrix; a pose determination module, configured to match the current frame point cloud of the vehicle with a pre-established map based on the transformation matrix and the translation matrix to obtain the lidar pose information of the vehicle; an equation determination module, configured to analyze the combined navigation data to obtain the current attitude information of the vehicle, and combine the current attitude information with the lidar attitude information to determine the vehicle's attitude and attitude calculation equation; The positioning module is configured to use the extended Kalman filter method to locate the vehicle based on the pose and state calculation equation. 8. An electronic device, comprising a memory, a processor and a computer program stored on the memory and running on the processor, wherein the processor implements any one of claims 1 to 6 when executing the program The three-dimensional point cloud-based positioning method described in item. 9. A computer-readable storage medium, wherein the storage medium stores at least one instruction, at least one piece of program, code set or instruction set, the at least one instruction, the at least one piece of program, the code set Or the instruction set is loaded and executed by the processor to implement the three-dimensional point cloud-based positioning method according to any one of claims 1 to 6 .

Images