Laser radar positioning method, device and equipment and storage medium

Abstract

The invention provides a laser radar positioning method and device, equipment and a storage medium, and relates to the technical field of automatic driving, in particular to the technical field of laser radar, positioning and the like. According to the specific implementation scheme, point clouds at multiple moments are obtained; for the point clouds at all moments, dividing the point clouds to obtain static point clouds and dynamic point clouds at the moments; obstacle point clouds corresponding to the same dynamic obstacle in the dynamic point clouds at different moments are determined; constructing a factor graph by using the static point cloud, the obstacle point cloud, the obstacle pose and the self-parking pose at each moment, and optimizing the factor graph; and determining a laser radar positioning result of the self-vehicle based on the self-vehicle position posture in the optimized factor graph. The stability and accuracy of laser radar positioning can be improved.

Description

Technical Field

[0001] The present disclosure relates to the field of autonomous driving technology, and in particular to technical fields such as laser radar and positioning. Background Art

[0002] Positioning is an important function of autonomous vehicles, especially the positioning system based on LiDAR can help vehicles achieve autonomous positioning. Existing LiDAR positioning methods are prone to positioning errors, especially in scenes with many dynamic objects, where the possibility of positioning errors is higher. How to improve the accuracy of LiDAR positioning of vehicles is a technical problem that needs to be solved. Summary of the invention

[0003] The present disclosure provides a laser radar positioning method, device, equipment and storage medium.

[0004] According to one aspect of the present disclosure, a laser radar positioning method is provided, comprising:

[0005] Get point clouds at multiple times;

[0006] For the point cloud at each moment, the point cloud is divided to obtain the static point cloud and the dynamic point cloud at that moment;

[0007] Determine obstacle point clouds corresponding to the same dynamic obstacle in dynamic point clouds at different times;

[0008] Using the static point cloud, obstacle point cloud, obstacle pose and ego-vehicle pose at each moment, a factor graph is constructed and optimized.

[0009] Based on the ego-vehicle pose in the optimized factor graph, the lidar positioning result of the ego-vehicle is determined.

[0010] According to another aspect of the present disclosure, a laser radar positioning device is provided, comprising:

[0011] Point cloud acquisition module, used to obtain point clouds at multiple moments;

[0012] A point cloud segmentation module is used to segment the point cloud at each moment to obtain a static point cloud and a dynamic point cloud at that moment;

[0013] A first determination module is used to determine obstacle point clouds corresponding to the same dynamic obstacle in dynamic point clouds at different times;

[0014] The factor graph processing module is used to construct a factor graph using the static point cloud, obstacle point cloud, obstacle pose and ego vehicle pose at each moment, and optimize the factor graph;

[0015] The second determination module is used to determine the laser radar positioning result of the ego vehicle based on the ego vehicle posture in the optimized factor graph.

[0016] According to another aspect of the present disclosure, there is provided an electronic device, comprising:

[0017] at least one processor; and

[0018] a memory communicatively connected to the at least one processor; wherein,

[0019] The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute any method in the embodiments of the present disclosure.

[0020] According to another aspect of the present disclosure, a non-transitory computer-readable storage medium storing computer instructions is provided, wherein the computer instructions are used to enable the computer to execute any method according to the embodiments of the present disclosure.

[0021] According to another aspect of the present disclosure, a computer program product is provided, including a computer program, which implements any method according to the embodiments of the present disclosure when executed by a processor.

[0022] When performing laser radar positioning, the present invention uses static point clouds and point clouds of dynamic obstacles as constraint factors, so that dynamic obstacles can be used to assist in laser radar positioning, thereby improving the accuracy of laser radar positioning, especially improving the accuracy of positioning when the static environment is degraded.

[0023] It should be understood that the content described in this section is not intended to identify the key or important features of the embodiments of the present disclosure, nor is it intended to limit the scope of the present disclosure. Other features of the present disclosure will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] The accompanying drawings are used to better understand the present solution and do not constitute a limitation of the present disclosure.

[0025] Figure 1 is a schematic flow chart of a laser radar positioning method proposed in an embodiment of the present disclosure;

[0026] Figure 2 is a schematic block diagram of a system principle of an embodiment of the present disclosure;

[0027] Figure 3 It is a schematic diagram of the factor graph optimization principle;

[0028] Figure 4 is an example 1 of a factor graph constructed according to an embodiment of the present disclosure;

[0029] Figure 5 is Example 2 of a factor graph constructed according to an embodiment of the present disclosure;

[0030] Figure 6 is a schematic structural diagram of a laser radar positioning device 600 according to an embodiment of the present disclosure;

[0031] Figure 7 A schematic block diagram of an example electronic device 700 that may be used to implement embodiments of the present disclosure is shown. DETAILED DESCRIPTION

[0032] The following is a description of exemplary embodiments of the present disclosure in conjunction with the accompanying drawings, including various details of the embodiments of the present disclosure to facilitate understanding, which should be considered as merely exemplary. Therefore, it should be recognized by those of ordinary skill in the art that various changes and modifications may be made to the embodiments described herein without departing from the scope of the present disclosure. Similarly, for the sake of clarity and conciseness, the description of well-known functions and structures is omitted in the following description.

[0033] The “and / or” in the embodiments of the present disclosure indicates that three relationships may exist. For example, A and / or B may indicate the following three situations: A exists alone, A and B exist at the same time, and B exists alone. The term “at least one” herein indicates any combination of at least two of any one or more of a plurality of types. For example, at least one of A, B, and C may indicate any one or more elements selected from the set consisting of A, B, and C. The terms “first” and “second” herein refer to and distinguish between multiple similar technical terms, and do not mean to limit the order or to limit the meaning to only two. For example, the first feature and the second feature refer to two types / two features. The first feature may be one or more, and the second feature may also be one or more.

[0034] Positioning is an important function of autonomous vehicles, especially the positioning system based on LiDAR and camera can help autonomous vehicles achieve autonomous positioning in scenarios where the Global Positioning System (GPS) fails. Existing LiDAR positioning methods generally assume that the environment in which the autonomous vehicle is located is completely static, that is, there are no moving objects in the field of view of the LiDAR sensor of the autonomous vehicle. However, in reality, autonomous vehicles often work in scenes with many dynamic objects (such as other vehicles and pedestrians). In order to avoid the influence of dynamic objects on positioning, existing LiDAR positioning methods usually adopt the strategy of eliminating dynamic objects and only using static environments for constraints. This approach has at least the following defects:

[0035] (1) It is only applicable to scenarios with few dynamic obstacles. When the autonomous vehicle is in a scenario with many dynamic obstacles (such as congested scenarios), the static environment is severely blocked. Therefore, the static environment is not sufficient to constrain the positioning of the autonomous vehicle, resulting in low positioning accuracy.

[0036] (2) In environments with static geometry degradation (such as tunnels), positioning errors may also occur due to insufficient constraints.

[0037] In order to deal with the above problems, there are solutions that integrate GPS, inertial measurement unit (IMU) and other sensors such as wheel speed to solve the problem of lidar degradation. However, the lidar positioning solution itself is to solve the problem of GPS degradation, that is, it may face the situation where both lidar and GPS are degraded. In this case, fusing the detection results of GPS and lidar still cannot solve the problem of inaccurate positioning. In addition, sensors such as IMU and wheel speed have cumulative errors and can only guarantee positioning accuracy for a very short time. Therefore, fusing the detection results of IMU / wheel speed and lidar cannot solve the problem of inaccurate positioning well.

[0038] From the above analysis, it can be seen that the existing lidar positioning method is prone to positioning errors. When the autonomous driving vehicle that needs to be positioned (hereinafter referred to as the self-vehicle) is in a scene with many dynamic obstacles, the problem of positioning error is particularly prominent.

[0039] Based on this, an embodiment of the present disclosure proposes a laser radar positioning method. Figure 1 : is a schematic flow chart of a laser radar positioning method proposed in an embodiment of the present disclosure, including:

[0040] S110, obtaining point clouds at multiple times;

[0041] S120, dividing the point cloud at each moment to obtain a static point cloud and a dynamic point cloud at that moment;

[0042] S130, determining obstacle point clouds corresponding to the same obstacle in the dynamic point clouds at different times;

[0043] S140, constructing a factor graph using the static point cloud, obstacle point cloud, obstacle pose and ego-vehicle pose at each moment, and optimizing the factor graph;

[0044] S150: Determine the laser radar positioning result of the ego vehicle based on the ego vehicle posture in the optimized factor graph.

[0045] In some examples, in step S110, the laser radar system of the ego vehicle is used to obtain point clouds at multiple adjacent moments. The ego vehicle may also be referred to as the host vehicle, and the ego vehicle / host vehicle may be an autonomous driving vehicle. The laser radar positioning method proposed in the embodiment of the present disclosure can realize the positioning of the ego vehicle, for example, determining the position and posture of the ego vehicle at each moment.

[0046] In some examples, the laser radar system of the vehicle collects point clouds of the surrounding environment at multiple consecutive moments. For example, point clouds are collected at multiple moments such as time k (or the moment corresponding to the kth frame), time k+1 (or the moment corresponding to the k+1th frame), and time k+2 (or the moment corresponding to the k+2th frame); there is a time sequence between time k, time k+1, and time k+2, for example, time k is the earliest moment.

[0047] Among them, the point cloud collected at time k is called the k-th frame point cloud, the point cloud collected at time k+1 is called the k+1-th frame point cloud, the point cloud collected at time k+2 is called the k+2-th frame point cloud, and so on.

[0048] In step S120, the point clouds at each moment are divided. For example, the point cloud of the k+1th frame is divided into a static point cloud and a dynamic point cloud at the kth moment, the point cloud of the k+1th frame is divided into a static point cloud and a dynamic point cloud at the k+1th moment, the point cloud of the k+2th frame is divided into a static point cloud and a dynamic point cloud at the k+2th moment, and so on.

[0049] Among them, the static point cloud can also be called the static background point cloud, which includes the point clouds of all static obstacles within the detection range of the vehicle's laser radar; the dynamic point cloud includes the point clouds of all dynamic obstacles.

[0050] In some examples, a detection model (such as a 3D detection model) is used to divide the point cloud. The 3D detection model is a neural network model used for target detection in three-dimensional space. It can detect and locate objects from 3D point clouds or other three-dimensional data. In one embodiment of the present disclosure, a 3D detection model is used to identify dynamic point clouds in a point cloud based on a 3D bounding box, and the remaining points are treated as static point clouds.

[0051] For example, from the k-th frame point cloud, identify the static point cloud at time k, denoted as P k b ; The remaining points are dynamic point clouds, which may include multiple obstacle point clouds, each of which corresponds to a different dynamic obstacle; from the k+1 frame point cloud, identify the static point cloud at the k+1 moment, recorded as The remaining points are dynamic point clouds, which may include multiple obstacle point clouds, each obstacle point cloud corresponding to a different dynamic obstacle; and so on.

[0052] In some embodiments, in step S130, the method of determining the obstacle point cloud corresponding to the same dynamic obstacle includes:

[0053] In the dynamic point clouds at multiple consecutive moments, obstacle point clouds corresponding to the same dynamic obstacle are identified, and the same identifier is assigned to the obstacle point clouds corresponding to the same obstacle.

[0054] In some examples, a multi-object tracking (MOT) algorithm is used to identify obstacle point clouds. The MOT algorithm is an algorithm for tracking multiple targets in video or point cloud data. It can continuously detect and identify multiple targets in a scene in time series data and assign a unique ID to each target to maintain target consistency across frames.

[0055] The disclosed embodiment uses the MOT algorithm to associate dynamic obstacles in adjacent frames and obtain a continuous correspondence between dynamic obstacles between frames, thereby determining obstacle point clouds for the same obstacle in the factor graph, creating conditions for constructing the factor graph.

[0056] For example, the dynamic point cloud at time k includes obstacle point clouds corresponding to obstacle 1, obstacle 2, and obstacle 3; the dynamic point cloud at time k+1 includes obstacle point clouds corresponding to obstacle 1, obstacle 2, and obstacle 4. Based on the MOT algorithm, the obstacle point cloud corresponding to obstacle 1 in the point cloud at time k and the obstacle point cloud corresponding to obstacle 1 in the point cloud at time k+1 can be determined, and a unique ID is assigned to these two obstacle point clouds, such as ID=1; the obstacle point cloud corresponding to obstacle 2 in the point cloud at time k and the obstacle point cloud corresponding to obstacle 2 in the point cloud at time k+1 can also be determined, and a unique ID is assigned to these two obstacle point clouds, such as ID=2.

[0057] In some implementations, for each dynamic obstacle, the above step S140 may be used to construct and optimize a factor graph. Figure 2 is a schematic block diagram of a system principle of an embodiment of the present disclosure, such as Figure 2As shown, in some examples, a 3D detection model is used to divide the point cloud at each moment into a static point cloud and a dynamic point cloud; based on the MOT algorithm, in the dynamic point cloud at multiple consecutive moments, the obstacle point cloud corresponding to the same dynamic obstacle is identified, and the same identifier is assigned to the obstacle point cloud corresponding to the same obstacle. Based on the aforementioned obstacle point cloud and static point cloud, a factor graph can be constructed; in the factor graph, the obstacle point cloud of the same obstacle at multiple moments is used to form a dynamic constraint on the ego vehicle posture; and the static point cloud is used to form a static constraint on the ego vehicle posture. Based on the aforementioned dynamic constraints and static constraints, the factor graph is optimized to determine the posture of the ego vehicle at multiple moments. Since dynamic obstacles are used to assist in the positioning of the laser radar, that is, static obstacles and dynamic obstacles are used for laser positioning at the same time, the accuracy of the laser radar positioning can be improved; in particular, the robustness and accuracy of the positioning of the vehicle when the static environment is degraded can be improved.

[0058] The disclosed embodiment may determine the laser radar positioning result of the vehicle based on a factor graph for one dynamic obstacle; or may determine the laser radar positioning result of the vehicle based on a factor graph for multiple dynamic obstacles.

[0059] For example, a factor graph is constructed and optimized for obstacle 1. If the factor graph optimization is successful (the criteria for judging "successful optimization" will be introduced in the subsequent content), the position and posture of the ego vehicle at multiple times are determined based on the optimized factor graph. For another example, a factor graph is constructed and optimized for obstacle 1. If the factor graph optimization is unsuccessful, the factor graphs are constructed and optimized for other obstacles. If the factor graph optimization is successful, the position and posture of the ego vehicle at multiple times are determined based on the optimized factor graphs. For another example, factor graphs are constructed and optimized for multiple obstacles respectively. If multiple factor graphs are optimized successfully, the position and posture of the ego vehicle at multiple times are determined based on the successfully optimized factor graphs respectively; if the results determined based on different factor graphs are different, the optimization results of each factor graph are combined to determine the position and posture of the ego vehicle at multiple times.

[0060] The following introduces the factor graph:

[0061] The principle of Factor Graph Optimization (FGO) is based on graph theory and probabilistic graph models, and is mainly used to deal with large-scale nonlinear optimization problems. Its core idea is to describe the relationship and constraints between multiple variables through factor graphs, and then use nonlinear optimization algorithms to solve each variable. Factor graphs are composed of two basic concepts: nodes and edges. Figure 3 It is a schematic diagram of the factor graph optimization principle, such as Figure 3As shown in the figure, both circles and ovals represent nodes. The nodes represented by circles are variable nodes, i.e. nodes that need to be solved, such as the position of the vehicle; the nodes represented by ovals are constraint nodes, representing observation constraints, such as common road signs. The lines connecting the nodes represent edges, which represent the constraints between the nodes they connect.

[0062] When optimizing the factor graph, the embodiment of the present disclosure may adopt a nonlinear optimization method to iterate the values ​​of the nodes in the factor graph multiple times until a convergence condition is met; the convergence condition includes at least one of the following:

[0063] (1) The number of iterations is greater than or equal to the threshold;

[0064] (2) The values ​​of the adjusted nodes satisfy the constraints.

[0065] The nonlinear optimization algorithm includes at least one of the following: gradient descent algorithm, Newton algorithm, quasi-Newton algorithm, etc. The optimization direction may also be referred to as a search direction. When the optimization algorithm is a gradient descent algorithm, the optimization direction or the search direction may be a negative gradient direction or a gradient direction, and the step size may be an adjustment amplitude.

[0066] By using the above nonlinear optimization method to iteratively optimize the factor graph, the speed of factor graph optimization can be improved, thereby improving the speed and efficiency of lidar positioning.

[0067] The optimization process includes multiple iterations. During each iteration, the values ​​in the nodes are adjusted, and calculations are performed based on the adjusted nodes to determine whether the values ​​of the adjusted nodes meet the constraints. If not, the next iteration is performed, that is, the values ​​in the nodes are adjusted again, and it is determined again whether the values ​​of the adjusted nodes meet the constraints. This iteration is repeated until the convergence conditions are met, and the optimization process ends.

[0068] When the optimization process ends, if the values ​​of the nodes meet the constraints, the optimization of the factor graph is considered successful; if the number of iterations reaches the threshold but the constraints are still not met, the optimization of the factor graph is considered to have failed.

[0069] In some examples, constructing the factor graph includes:

[0070] Constructing multiple nodes in the factor graph, the multiple nodes include: static point clouds, obstacle point clouds, obstacle poses, and ego-vehicle poses at various moments;

[0071] Constructing constraints between nodes, the constraints including: a first constraint between static point clouds and ego-vehicle poses at adjacent moments, and a second constraint between obstacle point clouds, obstacle poses, and ego-vehicle poses at adjacent moments;

[0072] A factor graph is determined based on the plurality of nodes, the first constraint, and the second constraint.

[0073] The disclosed embodiment utilizes static point clouds, obstacle point clouds, obstacle poses and ego-vehicle poses at various moments to construct a factor graph, and locates the ego-vehicle based on the factor graph; it is capable of comprehensively utilizing static background (corresponding to the first constraint condition) and dynamic obstacles (corresponding to the second constraint condition) for positioning without directly excluding the information of dynamic obstacles, thereby being able to improve the positioning accuracy of the ego-vehicle.

[0074] Figure 4 is an example 1 of a factor graph constructed according to an embodiment of the present disclosure. Figure 4 As shown, the nodes in the factor graph include the following:

[0075] Static point cloud at time k Obstacle point cloud Obstacle pose and the ego vehicle posture (T k );

[0076] Static point cloud at time k+1 Obstacle point cloud Obstacle pose and the ego vehicle posture (T k+1 );

[0077] Static point cloud at time k+2 Obstacle point cloud Obstacle pose and the ego vehicle posture (T k+2 );

[0078] The factor graph shows nodes at three consecutive moments. In some embodiments, the factor graph includes a plurality of consecutive nodes; Figure 4 This is only an example and does not represent a limitation on the number of corresponding moments of nodes in the factor graph. In addition, the symbols representing each node in this embodiment are only examples and are not intended to limit the present disclosure.

[0079] like Figure 4 As shown, the constraints in the factor graph include a first constraint and a second constraint. The first constraint may represent the constraint factor of the static background inter-frame point cloud, and the second constraint may represent the constraint factor of the dynamic obstacle inter-frame point cloud. Figure 4 In the example, the first constraint condition represents the constraint relationship between the static point cloud and the ego-vehicle pose at adjacent moments. In some examples, constructing the first constraint condition includes:

[0080] Based on the ego-vehicle postures at adjacent moments, determining the ego-vehicle posture transformation matrix at adjacent moments;

[0081] The static point cloud at the previous moment is transformed using the ego-vehicle posture transformation matrix and the ego-vehicle posture at the previous moment among the adjacent moments to obtain a first intermediate value; and the static point cloud at the later moment is transformed using the ego-vehicle posture at the later moment among the adjacent moments to obtain a second intermediate value;

[0082] A first constraint condition is constructed, where the first constraint condition includes: a difference value between the first intermediate value and the second intermediate value is less than or equal to a first threshold value.

[0083] Taking the adjacent moments as moment k and moment k+1 as an example, moment k is the earlier moment and moment k+1 is the later moment. In some examples, the first constraint condition is expressed as:

[0084]

[0085] Among them, e1 approaches 0;

[0086] represents the static point cloud at time k, Represents the static point cloud at time k+1; the static point cloud can be a point cloud in the vehicle coordinate system;

[0087] T k represents the vehicle posture at time k, T k+1 represents the ego vehicle posture at time k+1; the ego vehicle posture can be the posture in the world coordinate system;

[0088] T k+1,k T represents the ego vehicle posture transformation matrix, that is, the posture transformation matrix of the ego vehicle from time k to time k+1; k+1,k According to T k and T k+1 Sure;

[0089] ||.|| 2 represents the square of the second-order norm;

[0090] i represents the identity of a point in the point cloud.

[0091] As in the above formula (1), the first intermediate value is equal to The second intermediate value is equal to The square of the second-order norm of the difference between the first intermediate value and the second intermediate value approaches 0, which means that the difference between the first intermediate value and the second intermediate value is less than or equal to the first threshold.

[0092] It can be seen that the first constraint condition is based on the principle of the ICP algorithm. It uses the static point clouds at adjacent moments under the ego-vehicle coordinates and the posture of the ego-vehicle at each adjacent moment to establish geometric constraints between the static point clouds at adjacent moments, and uses it as a constraint condition for the lidar positioning of the ego-vehicle.

[0093] like Figure 4 In the example, the second constraint condition represents the constraint relationship between the obstacle point cloud, obstacle pose and ego vehicle pose at adjacent moments. In some examples, constructing the second constraint condition includes:

[0094] Based on the obstacle postures at adjacent moments, determine the obstacle posture transformation matrix of the dynamic obstacle at adjacent moments;

[0095] Based on the obstacle posture transformation matrix and the posture of the ego vehicle at an earlier time in the adjacent time, the obstacle point cloud at the earlier time is transformed to obtain a third intermediate value; and based on the posture of the ego vehicle at a later time in the adjacent time, the obstacle point cloud at the later time is transformed to obtain a fourth intermediate value;

[0096] A second constraint condition is constructed, where the second constraint condition includes: a difference value between the third intermediate value and the fourth intermediate value is less than or equal to a second threshold value.

[0097] Taking the adjacent moments as moment k and moment k+1 as an example, moment k is the earlier moment and moment k+1 is the later moment. In some examples, the second constraint condition is expressed as:

[0098]

[0099] Among them, e2 approaches 0; among them,

[0100] represents the obstacle point cloud at time k, Represents the obstacle point cloud at time k+1; the obstacle point cloud can be a point cloud in the vehicle coordinate system;

[0101] represents the obstacle position at time k, represents the obstacle posture at time k+1; the obstacle posture can be the posture in the world coordinate system;

[0102] represents the obstacle posture transformation matrix, that is, the obstacle posture transformation matrix from time k to time k+1; Can be based on and Sure;

[0103] T k represents the vehicle posture at time k, T k+1 represents the ego vehicle posture at time k+1; the ego vehicle posture can be the posture in the world coordinate system;

[0104] ||.|| 2 represents the square of the second-order norm;

[0105] i represents the identity of a point in the point cloud.

[0106] As in the above formula (2), the third intermediate value is equal to The fourth intermediate value is equal to The square of the second-order norm of the difference between the third intermediate value and the fourth intermediate value approaches 0, which means that the difference between the third intermediate value and the fourth intermediate value is less than or equal to the second threshold.

[0107] It can be seen that the second constraint condition is based on a similar principle to the ICP algorithm, using the obstacle point clouds at adjacent moments under the ego vehicle coordinates and the position and posture of the obstacle at each moment in the adjacent moments to establish geometric constraints between the dynamic point clouds at adjacent moments, and use it as another constraint condition for laser radar positioning of the ego vehicle. The disclosed embodiment combines the first constraint condition with the second constraint condition, and can use both dynamic obstacles and static obstacles as positioning factors, avoiding the direct elimination of the dynamic obstacle factor, thereby solving the problem that the static point cloud cannot provide sufficient constraints, resulting in the point cloud matching algorithm failing to converge.

[0108] In addition to the above nodes and constraints, the factor graph proposed in the embodiment of the present disclosure may also include other nodes and constraints. Figure 5 This is Example 2 of a factor graph constructed according to an embodiment of the present disclosure. Figure 5 As shown, the nodes in the factor graph may also include: the pose of the detection frame of the dynamic obstacle at each moment; the constraint condition also includes: a third constraint condition between the obstacle pose, the vehicle pose and the pose of the detection frame of the dynamic obstacle at the same moment.

[0109] by Figure 5 For example, the nodes in the factor graph also include:

[0110] The position and posture of the detection frame of the dynamic obstacle at time k The position at time k+1 And the position at time k+2

[0111] The factor graph shows nodes at three consecutive moments. In some embodiments, the factor graph includes a plurality of consecutive nodes; Figure 5 This is only an example and does not represent a limitation on the number of corresponding moments of nodes in the factor graph. In addition, the symbols representing each node in this embodiment are only examples and are not intended to limit the present disclosure.

[0112] like Figure 5 As shown, the constraint conditions in the factor graph may also include a third constraint condition, which may represent a constraint factor of the detection box. Figure 5In the example, the third constraint condition represents the constraint relationship between the obstacle pose, the vehicle pose, and the pose of the detection frame of the dynamic obstacle at the same time. In some examples, the third constraint condition is constructed, including:

[0113] Based on the ego-vehicle posture, transform the posture of the detection frame of the dynamic obstacle to obtain a fifth intermediate value;

[0114] A third constraint condition is constructed, where the third constraint condition includes: a difference value between the fifth intermediate value and the obstacle posture is less than or equal to a third threshold value.

[0115] Taking time k as an example, in some examples, the third constraint is expressed as:

[0116]

[0117] Among them, e3 approaches the identity matrix;

[0118] T k represents the ego-vehicle posture at time k, which can be the posture in the world coordinate system;

[0119] The pose of the detection frame representing the dynamic obstacle at time k, which may be the pose in the ego-vehicle coordinate system;

[0120] represents the obstacle posture at time k, which can be the posture in the world coordinate system;

[0121] (.) -1 Indicates the inverse of a matrix.

[0122] As in the above formula (3), the fifth intermediate value is equal to It represents the position of the detection frame in the world coordinate system; ideally, the detection frame position is equal to the obstacle position; therefore, in formula (3), the product of the inverse matrix of the fifth intermediate value and the obstacle position approaches the unit matrix, which means that the difference between the fifth intermediate value and the obstacle position is less than or equal to the third threshold.

[0123] It can be seen that the embodiment of the present disclosure establishes a geometric constraint between a dynamic obstacle and the detection frame of the dynamic obstacle, and uses it as another constraint condition (i.e., the third constraint condition) for laser radar positioning of the ego-vehicle, thereby further increasing the number and direction of constraints and improving the accuracy of laser radar positioning.

[0124] In some embodiments, the nodes in the factor graph may also include: the speed of the dynamic obstacles at each moment; the constraints in the factor graph may also include at least one of the fourth constraint and the fifth constraint; wherein the fourth constraint includes the constraint between the speeds of the dynamic obstacles at adjacent moments, and the fifth constraint includes the constraint between the obstacle posture and the speed of the dynamic obstacle at adjacent moments.

[0125] by Figure 5 For example, the nodes in the factor graph may also include:

[0126] The velocity of the dynamic obstacle at time k (v k ), the speed at time k+1 (v k+1 ), and the velocity at time k+2 (v k+2 ). The aforementioned symbols are only examples and are not intended to limit the present disclosure.

[0127] like Figure 5 As shown, the constraints in the factor graph may also include a fourth constraint and / or a fifth constraint. In some examples, the fourth constraint includes: the difference value of the speed of the dynamic obstacle at adjacent moments is less than or equal to a fourth threshold.

[0128] Taking the adjacent moments as moment k and moment k+1 as an example, moment k is the earlier moment and moment k+1 is the later moment. In some examples, the fourth constraint condition is expressed as:

[0129] e4=v k+1 -v k (4)

[0130] Among them, e4 approaches the zero vector;

[0131] v k+1 represents the velocity of the dynamic obstacle at time k+1, v k Represents the speed of the dynamic obstacle at time k; the speed can be a three-dimensional vector, and the three elements in the three-dimensional vector respectively represent the speed of the dynamic obstacle in the directions of the three coordinate axes.

[0132] As shown in the above formula (4), the difference between the speed of the dynamic obstacle at time k+1 and the speed at time k approaches the zero vector, which means that the difference in the speed of the dynamic obstacle at adjacent times is less than or equal to the fourth threshold.

[0133] In some examples, the fifth constraint condition is constructed in the following manner:

[0134] Determine a first transformation matrix based on the speed of the dynamic obstacle at adjacent moments and the time interval between adjacent moments;

[0135] Based on the first transformation matrix, transform the obstacle posture at a previous moment in adjacent moments to obtain a sixth intermediate value;

[0136] A fifth constraint condition is constructed, and the fifth constraint condition includes: a difference value between the sixth intermediate value and the obstacle posture at a subsequent moment in the adjacent moments is less than or equal to a fifth threshold value.

[0137] Taking the adjacent moments as moment k and moment k+1 as an example, moment k is the earlier moment and moment k+1 is the later moment. In some examples, the fifth constraint condition is expressed as:

[0138]

[0139] Among them, e5 approaches the identity matrix;

[0140] represents the obstacle position at time k, represents the obstacle posture at time k+1; the obstacle posture can be the posture in the world coordinate system;

[0141] v k represents the obstacle speed at time k (approximately the obstacle speed at time k+1);

[0142] △t represents the time interval between two adjacent moments;

[0143] exp(), which means mapping the vector in the brackets into a matrix;

[0144] As shown in equation (5), the first transformation matrix is ​​equal to exp(v k △t); Based on the first transformation matrix, the obstacle posture at the previous moment is transformed to obtain the sixth intermediate value; the sixth intermediate value is equal to Ideally, the sixth intermediate value is equal to the obstacle posture at the later moment; therefore, in equation (5), the product of the inverse matrix of the obstacle posture at the later moment and the sixth intermediate value approaches the unit matrix, which means that the difference between the sixth intermediate value and the obstacle posture at the later moment is less than or equal to the fifth threshold.

[0145] The fourth and fifth constraints are constraints based on the principle of kinematics. The fourth and fifth constraints are combined with the aforementioned constraints to jointly constrain the optimization of the factor graph, which can improve the accuracy of the optimized factor graph, thereby improving the accuracy and robustness of the LiDAR positioning.

[0146] The above describes a method for laser radar positioning of a self-vehicle using a factor graph optimization method. The disclosed embodiment can also set corresponding weights for each constraint condition, and optimize the factor graph based on each weight.

[0147] For example, setting the overall constraints of the factor graph is:

[0148] E=w1e1+w2e5+w3e3+w4e4+w5e5 (6)

[0149] Among them, w1, w2, w3, w4, and w5 are the weights of the first constraint, the second constraint, the third constraint, the fourth constraint, and the fifth constraint, respectively. By setting the weights of each constraint, different importance levels can be set for different constraints, so that in the factor graph optimization process, optimization can be performed with emphasis according to different importance levels.

[0150] The following is an exemplary description of multiple iterations:

[0151] by Figure 5 Take the factor graph shown as an example, Figure 5 In the figure, the nodes represented by circles are constraint nodes, and the nodes represented by ovals are variable nodes. During the factor graph optimization process, the values ​​of constraint nodes remain unchanged, and the values ​​of variable nodes are adjusted in each iteration, so that the nodes after adjustment finally meet the constraints between nodes.

[0152] After constructing the factor graph, first determine the fixed values ​​of each constraint node and the initial value of each variable node. In one example, the initial value of the ego vehicle's posture is determined by IMU, wheel speed meter or uniform velocity model, the initial value of the obstacle's posture is determined by the orientation and center point position of the bounding box detected by the model, and the obstacle speed can be determined by observation or empirical value.

[0153] In the first iteration, adjust the initial values ​​of each variable node (such as Figure 5 The obstacle speed, obstacle posture, and vehicle posture in the algorithm are used to determine whether the first to fifth constraints are satisfied between the nodes using the adjusted variable nodes and constraint nodes; if all or part of the constraints are not satisfied, the second iteration process is entered;

[0154] In the second iteration, the values ​​of each variable node (the values ​​adjusted in the first iteration) are further adjusted, and the adjusted variable nodes and constraint nodes are used to determine whether the first to fifth constraint conditions are satisfied between the nodes; if all or part of the constraints are not satisfied, the third iteration is entered;

[0155] Until the variable nodes and constraint nodes after adjustment satisfy the first to fifth constraints, the factor graph optimization is considered successful, and the vehicle posture (including direction and position) after the last adjustment is used as the laser radar positioning result of the vehicle. If the number of iterations reaches the preset number threshold, and the nodes still do not satisfy all the constraints from the first to the fifth constraints, the factor graph optimization is considered to have failed, that is, the current obstacle cannot be used to locate the vehicle; in this case, other obstacles can be selected, and the obstacle can continue to execute the method proposed in the embodiment of the present disclosure to construct the factor graph and optimize it.

[0156] In the factor graph optimization process, the numerical values ​​corresponding to multiple variable nodes can be adjusted based on a nonlinear optimization algorithm to obtain multiple adjusted values ​​of variable nodes, which can include: determining the optimization direction and step size corresponding to multiple variable nodes based on the nonlinear optimization algorithm and multiple constraints in the factor graph; and obtaining the values ​​of the optimized variable nodes based on the optimization direction and step size. The nonlinear optimization algorithm includes at least one of the following: a gradient descent algorithm, a Newton algorithm, a quasi-Newton algorithm, etc. The optimization direction can also be referred to as a search direction. When the optimization algorithm is a gradient descent algorithm, the optimization direction or the search direction can be a negative gradient direction or a gradient direction, and the step size can be the adjustment amplitude.

[0157] Whether a constraint is satisfied can be determined based on the threshold corresponding to the constraint. Among them, one or more thresholds corresponding to each constraint can be set according to the actual situation, and one or more thresholds corresponding to one or more constraints can be the same or different, which is not limited here. In addition, setting different weights for constraints can be reflected in different settings of the thresholds corresponding to the constraints; if the threshold corresponding to a constraint is set more strictly, it means that the weight of the constraint is higher; if the threshold corresponding to a constraint is set more loosely, it means that the weight of the constraint is lower.

[0158] The disclosed embodiment also provides a laser radar positioning device, Figure 6 is a schematic diagram of the structure of a laser radar positioning device 600 according to an embodiment of the present disclosure, comprising:

[0159] A point cloud acquisition module 610 is used to acquire point clouds at multiple moments;

[0160] The point cloud division module 620 is used to divide the point cloud at each moment to obtain a static point cloud and a dynamic point cloud at that moment;

[0161] A first determination module 630 is used to determine obstacle point clouds corresponding to the same dynamic obstacle in the dynamic point clouds at different times;

[0162] A factor graph processing module 640 is used to construct a factor graph using the static point cloud, obstacle point cloud, obstacle pose and ego-vehicle pose at each moment, and optimize the factor graph;

[0163] The second determination module 650 is used to determine the laser radar positioning result of the ego vehicle based on the ego vehicle posture in the optimized factor graph.

[0164] In some implementations, the factor graph processing module 640 is configured to:

[0165] Construct multiple nodes in the factor graph, the multiple nodes include: static point cloud, obstacle point cloud, obstacle pose and ego vehicle pose at each moment;

[0166] Constructing constraints between nodes, the constraints including: a first constraint between static point clouds and ego-vehicle poses at adjacent moments, and a second constraint between obstacle point clouds, obstacle poses, and ego-vehicle poses at adjacent moments;

[0167] Based on a plurality of nodes and constraints, the factor graph is determined.

[0168] In some implementations, the factor graph processing module 640 is configured to:

[0169] Based on the ego-vehicle postures at adjacent moments, determining the ego-vehicle posture transformation matrix at adjacent moments;

[0170] The static point cloud at the previous moment is transformed using the ego-vehicle posture transformation matrix and the ego-vehicle posture at the previous moment among the adjacent moments to obtain a first intermediate value; and the static point cloud at the later moment is transformed using the ego-vehicle posture at the later moment among the adjacent moments to obtain a second intermediate value;

[0171] A first constraint condition is constructed, where the first constraint condition includes: a difference value between the first intermediate value and the second intermediate value is less than or equal to a first threshold value.

[0172] In some implementations, the factor graph processing module 640 is configured to:

[0173] Based on the obstacle postures at adjacent moments, determining the obstacle posture transformation matrix of the dynamic obstacle at adjacent moments;

[0174] Based on the obstacle posture transformation matrix and the posture of the ego vehicle at the previous moment in the adjacent moments, the obstacle point cloud at the previous moment is transformed to obtain a third intermediate value; and based on the posture of the ego vehicle at the subsequent moment in the adjacent moments, the obstacle point cloud at the subsequent moment is transformed to obtain a fourth intermediate value;

[0175] A second constraint condition is constructed, and the second constraint condition includes: a difference value between the third intermediate value and the fourth intermediate value is less than or equal to a second threshold value.

[0176] In some implementations, the node further includes: the pose of the detection box of the dynamic obstacle at each moment;

[0177] The constraint conditions also include: a third constraint condition between the obstacle pose, the ego-vehicle pose and the pose of the detection frame of the dynamic obstacle at the same time.

[0178] In some implementations, the factor graph processing module 640 is configured to:

[0179] Based on the ego-vehicle posture, transform the posture of the detection frame of the dynamic obstacle to obtain a fifth intermediate value;

[0180] A third constraint condition is constructed, where the third constraint condition includes: a difference value between the fifth intermediate value and the obstacle posture is less than or equal to a third threshold value.

[0181] In some embodiments, the node further includes: the speed of the dynamic obstacle at each moment;

[0182] The constraint condition also includes at least one of the fourth constraint condition and the fifth constraint condition; wherein,

[0183] The fourth constraint condition includes a constraint condition between the speeds of the dynamic obstacle at adjacent moments;

[0184] The fifth constraint condition includes a constraint condition between the obstacle posture at adjacent moments and the speed of the dynamic obstacle.

[0185] In some implementations, the fourth constraint condition includes: a difference in speed of the dynamic obstacle at adjacent moments is less than or equal to a fourth threshold.

[0186] In some implementations, the factor graph processing module 640 is configured to:

[0187] Determine a first transformation matrix based on the speed of the dynamic obstacle at adjacent moments and the time interval between adjacent moments;

[0188] Based on the first transformation matrix, transform the obstacle posture at a previous moment in adjacent moments to obtain a sixth intermediate value;

[0189] A fifth constraint condition is constructed, and the fifth constraint condition includes: a difference value between the sixth intermediate value and the obstacle posture at a subsequent moment in adjacent moments is less than or equal to a fifth threshold value.

[0190] In some implementations, the point cloud acquisition module 610 is used to:

[0191] The laser radar system of the vehicle is used to obtain point clouds at multiple adjacent moments.

[0192] In some implementations, the first determining module 630 is configured to:

[0193] In the dynamic point clouds at multiple consecutive moments, obstacle point clouds corresponding to the same dynamic obstacle are identified, and the same identifier is assigned to the obstacle point clouds corresponding to the same dynamic obstacle.

[0194] In some implementations, the factor graph processing module 640 is configured to:

[0195] A nonlinear optimization method is used to iterate the values ​​of the nodes in the factor graph for multiple times until a convergence condition is satisfied; the convergence condition includes at least one of the following:

[0196] The number of iterations is greater than or equal to the threshold;

[0197] The adjusted node values ​​satisfy the constraint.

[0198] In some implementations, the factor graph processing module 640 is configured to:

[0199] Set corresponding weights for the first constraint, the second constraint, the third constraint, the fourth constraint and the fifth constraint respectively;

[0200] Based on the individual weights, the factor graph is optimized.

[0201] For the description of specific functions and examples of each module and submodule of the device in the embodiment of the present disclosure, reference can be made to the relevant description of the corresponding steps in the above method embodiment, which will not be repeated here.

[0202] In the technical solution disclosed in the present invention, the acquisition, storage and application of personal information of users involved are in compliance with the provisions of relevant laws and regulations and do not violate public order and good morals.

[0203] According to an embodiment of the present disclosure, the present disclosure also provides an electronic device, a readable storage medium and a computer program product.

[0204] Figure 7A schematic block diagram of an example electronic device 700 that can be used to implement an embodiment of the present disclosure is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as personal digital assistants, cellular phones, smart phones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the present disclosure described and / or required herein.

[0205] like Figure 7 As shown, the device 700 includes a computing unit 701, which can perform various appropriate actions and processes according to a computer program stored in a read-only memory (ROM) 702 or a computer program loaded from a storage unit 708 into a random access memory (RAM) 703. In the RAM 703, various programs and data required for the operation of the device 700 can also be stored. The computing unit 701, the ROM 702, and the RAM 703 are connected to each other via a bus 704. An input / output (I / O) interface 705 is also connected to the bus 704.

[0206] A number of components in the device 700 are connected to the I / O interface 705, including: an input unit 706, such as a keyboard, a mouse, etc.; an output unit 707, such as various types of displays, speakers, etc.; a storage unit 708, such as a disk, an optical disk, etc.; and a communication unit 709, such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 709 allows the device 700 to exchange data with other devices through a computer network such as the Internet and / or various telecommunication networks.

[0207] The computing unit 701 may be a variety of general and / or special processing components with processing and computing capabilities. Some examples of the computing unit 701 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, digital signal processors (DSPs), and any appropriate processors, controllers, microcontrollers, etc. The computing unit 701 performs the various methods and processes described above, such as detection methods. For example, in some embodiments, the detection method may be implemented as a computer software program, which is tangibly contained in a machine-readable medium, such as a storage unit 708. In some embodiments, part or all of the computer program may be loaded and / or installed on the device 700 via ROM 702 and / or communication unit 709. When the computer program is loaded into RAM 703 and executed by the computing unit 701, one or more steps of the detection method described above may be performed. Alternatively, in other embodiments, the computing unit 701 may be configured to perform the detection method in any other appropriate manner (e.g., by means of firmware).

[0208] Various implementations of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chips (SOCs), load programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include: being implemented in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which can be a special purpose or general purpose programmable processor that can receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device.

[0209] The program code for implementing the method of the present disclosure may be written in any combination of one or more programming languages. These program codes may be provided to a processor or controller of a general-purpose computer, a special-purpose computer, or other programmable data processing device, so that the program code, when executed by the processor or controller, enables the functions / operations specified in the flow chart and / or block diagram to be implemented. The program code may be executed entirely on the machine, partially on the machine, partially on the machine and partially on a remote machine as a stand-alone software package, or entirely on a remote machine or server.

[0210] In the context of the present disclosure, a machine-readable medium may be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, device, or equipment. A machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or equipment, or any suitable combination of the foregoing. A more specific example of a machine-readable storage medium may include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0211] To provide interaction with a user, the systems and techniques described herein can be implemented on a computer having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user can provide input to the computer. Other types of devices can also be used to provide interaction with the user; for example, the feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including acoustic input, voice input, or tactile input).

[0212] The systems and techniques described herein may be implemented in a computing system that includes back-end components (e.g., as a data server), or a computing system that includes middleware components (e.g., an application server), or a computing system that includes front-end components (e.g., a user computer with a graphical user interface or a web browser through which a user can interact with implementations of the systems and techniques described herein), or a computing system that includes any combination of such back-end components, middleware components, or front-end components. The components of the system may be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include: a local area network (LAN), a wide area network (WAN), and the Internet.

[0213] A computer system may include a client and a server. The client and the server are generally remote from each other and usually interact through a communication network. The relationship of client and server is generated by computer programs running on respective computers and having a client-server relationship with each other. The server may be a cloud server, a server of a distributed system, or a server combined with a blockchain.

[0214] It should be understood that the various forms of processes shown above can be used to reorder, add or delete steps. For example, the steps recorded in this disclosure can be executed in parallel, sequentially or in different orders, as long as the desired results of the technical solutions disclosed in this disclosure can be achieved, and this document does not limit this.

[0215] The above specific implementations do not constitute a limitation on the protection scope of the present disclosure. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modification, equivalent substitution and improvement made within the principles of the present disclosure shall be included in the protection scope of the present disclosure.

Claims

1. A laser radar positioning method, comprising: Get point clouds at multiple times; For the point cloud at each moment, dividing the point cloud to obtain a static point cloud and a dynamic point cloud at that moment; Determining obstacle point clouds corresponding to the same dynamic obstacle in the dynamic point clouds at different times; Using the static point cloud, the obstacle point cloud, the obstacle pose and the ego-vehicle pose at each moment, a factor graph is constructed, and the factor graph is optimized; Based on the ego-vehicle pose in the optimized factor graph, the lidar positioning result of the ego-vehicle is determined.

2. The method according to claim 1, wherein: The step of constructing a factor graph by using the static point cloud, the obstacle point cloud, the obstacle pose and the ego-vehicle pose at each moment includes: Constructing a plurality of nodes in the factor graph, the plurality of nodes comprising: the static point cloud, the obstacle point cloud, the obstacle pose and the ego-vehicle pose at each moment; Constructing constraints between the nodes, the constraints comprising: a first constraint between the static point cloud and the ego-vehicle pose at adjacent moments, and a second constraint between the obstacle point cloud, the obstacle pose, and the ego-vehicle pose at adjacent moments; The factor graph is determined based on the plurality of nodes and the constraints.

3. The method according to claim 2, wherein: Constructing the first constraint condition includes: Based on the ego-vehicle posture at adjacent moments, determining the ego-vehicle posture transformation matrix of the ego-vehicle at the adjacent moments; The static point cloud at the previous moment is transformed using the ego-vehicle posture transformation matrix and the ego-vehicle posture at the previous moment among the adjacent moments to obtain a first intermediate value; and the static point cloud at the later moment is transformed using the ego-vehicle posture at the later moment among the adjacent moments to obtain a second intermediate value; The first constraint condition is constructed, and the first constraint condition includes: a difference value between the first intermediate value and the second intermediate value is less than or equal to a first threshold.

4. The method according to claim 2 or 3, wherein: Constructing the second constraint condition includes: Determine, based on the obstacle pose at adjacent moments, an obstacle pose transformation matrix of the dynamic obstacle at the adjacent moments; Based on the obstacle posture transformation matrix and the ego-vehicle posture at a previous moment in the adjacent moments, transform the obstacle point cloud at the previous moment to obtain a third intermediate value; and based on the ego-vehicle posture at a later moment in the adjacent moments, transform the obstacle point cloud at the later moment to obtain a fourth intermediate value; The second constraint condition is constructed, where the second constraint condition includes: a difference value between the third intermediate value and the fourth intermediate value is less than or equal to a second threshold value.

5. The method according to any one of claims 2 to 4, wherein: The node also includes: the position and posture of the detection frame of the dynamic obstacle at each moment; The constraint condition also includes: a third constraint condition between the obstacle posture, the vehicle posture and the posture of the detection frame of the dynamic obstacle at the same time.

6. The method according to claim 5, wherein: Constructing the third constraint condition includes: Based on the ego-vehicle posture, transforming the posture of the detection frame of the dynamic obstacle to obtain a fifth intermediate value; The third constraint condition is constructed, and the third constraint condition includes: a difference value between the fifth intermediate value and the obstacle posture is less than or equal to a third threshold.

7. The method according to any one of claims 2 to 6, wherein: The node also includes: the speed of the dynamic obstacle at each moment; The constraint condition also includes at least one of the fourth constraint condition and the fifth constraint condition; wherein, The fourth constraint condition includes a constraint condition between the speeds of the dynamic obstacle at adjacent moments; The fifth constraint condition includes a constraint condition between the obstacle posture and the speed of the dynamic obstacle at the adjacent moments.

8. The method according to claim 7, wherein: The fourth constraint condition includes: a difference value of the speed of the dynamic obstacle at adjacent moments is less than or equal to a fourth threshold.

9. The method according to claim 7 or 8, wherein: Constructing the fifth constraint condition includes: Determining a first transformation matrix based on the speed of the dynamic obstacle at the adjacent moments and the time interval between the adjacent moments; Based on the first transformation matrix, transform the obstacle posture at a previous moment in the adjacent moments to obtain a sixth intermediate value; The fifth constraint condition is constructed, and the fifth constraint condition includes: a difference value between the sixth intermediate value and the obstacle posture at a later moment in the adjacent moments is less than or equal to a fifth threshold.

10. The method according to any one of claims 1 to 9, wherein: The step of obtaining point clouds at multiple moments includes: The laser radar system of the vehicle is used to obtain point clouds at multiple adjacent moments.

11. The method according to any one of claims 1 to 9, wherein: The determining of obstacle point clouds corresponding to the same dynamic obstacle in the dynamic point clouds at different times includes: In the dynamic point clouds at a plurality of consecutive moments, obstacle point clouds corresponding to the same dynamic obstacle are identified, and the same identifier is assigned to the obstacle point clouds corresponding to the same dynamic obstacle.

12. The method according to any one of claims 2 to 9, wherein: The step of optimizing the factor graph comprises: A nonlinear optimization method is used to iterate the values ​​of the nodes in the factor graph for multiple times until a convergence condition is satisfied; the convergence condition includes at least one of the following: The number of iterations is greater than or equal to the threshold; The adjusted value of the node satisfies the constraint condition.

13. The method according to claim 12, wherein: The step of optimizing the factor graph comprises: Set corresponding weights for the first constraint condition, the second constraint condition, the third constraint condition, the fourth constraint condition, and the fifth constraint condition respectively; Based on the respective weights, the factor graph is optimized.

14. A laser radar positioning device, comprising: Point cloud acquisition module, used to obtain point clouds at multiple moments; A point cloud segmentation module, for segmenting the point cloud at each moment to obtain a static point cloud and a dynamic point cloud at that moment; A first determination module, configured to determine obstacle point clouds corresponding to the same dynamic obstacle in the dynamic point clouds at different times; A factor graph processing module, used to construct a factor graph using the static point cloud, the obstacle point cloud, the obstacle pose and the ego-vehicle pose at each moment, and optimize the factor graph; The second determination module is used to determine the laser radar positioning result of the ego vehicle based on the ego vehicle posture in the optimized factor graph.

15. The device according to claim 14, wherein: The factor graph processing module is used to: Constructing a plurality of nodes in the factor graph, the plurality of nodes comprising: the static point cloud, the obstacle point cloud, the obstacle pose and the ego-vehicle pose at each moment; Constructing constraints between the nodes, the constraints comprising: a first constraint between the static point cloud and the ego-vehicle pose at adjacent moments, and a second constraint between the obstacle point cloud, the obstacle pose, and the ego-vehicle pose at adjacent moments; The factor graph is determined based on the plurality of nodes and the constraints.

16. The device according to claim 15, wherein: The factor graph processing module is used to: Based on the ego-vehicle posture at adjacent moments, determining the ego-vehicle posture transformation matrix of the ego-vehicle at the adjacent moments; The static point cloud at the previous moment is transformed using the ego-vehicle posture transformation matrix and the ego-vehicle posture at the previous moment among the adjacent moments to obtain a first intermediate value; and the static point cloud at the later moment is transformed using the ego-vehicle posture at the later moment among the adjacent moments to obtain a second intermediate value; The first constraint condition is constructed, and the first constraint condition includes: a difference value between the first intermediate value and the second intermediate value is less than or equal to a first threshold.

17. The device according to claim 15 or 16, wherein: The factor graph processing module is used to: Determine, based on the obstacle pose at adjacent moments, an obstacle pose transformation matrix of the dynamic obstacle at the adjacent moments; Based on the obstacle posture transformation matrix and the ego-vehicle posture at a previous moment in the adjacent moments, transform the obstacle point cloud at the previous moment to obtain a third intermediate value; and based on the ego-vehicle posture at a later moment in the adjacent moments, transform the obstacle point cloud at the later moment to obtain a fourth intermediate value; The second constraint condition is constructed, where the second constraint condition includes: a difference value between the third intermediate value and the fourth intermediate value is less than or equal to a second threshold value.

18. The device according to any one of claims 15 to 17, wherein: The node also includes: the position and posture of the detection frame of the dynamic obstacle at each moment; The constraint condition also includes: a third constraint condition between the obstacle posture, the vehicle posture and the posture of the detection frame of the dynamic obstacle at the same time.

19. The device according to claim 18, wherein: The factor graph processing module is used to: Based on the ego-vehicle posture, transforming the posture of the detection frame of the dynamic obstacle to obtain a fifth intermediate value; The third constraint condition is constructed, and the third constraint condition includes: a difference value between the fifth intermediate value and the obstacle posture is less than or equal to a third threshold.

20. The device according to any one of claims 15 to 19, wherein: The node also includes: the speed of the dynamic obstacle at each moment; The constraint condition also includes at least one of the fourth constraint condition and the fifth constraint condition; wherein, The fourth constraint condition includes a constraint condition between the speeds of the dynamic obstacle at adjacent moments; The fifth constraint condition includes a constraint condition between the obstacle posture and the speed of the dynamic obstacle at the adjacent moments.

21. The device according to claim 20, wherein: The fourth constraint condition includes: a difference value of the speed of the dynamic obstacle at adjacent moments is less than or equal to a fourth threshold.

22. The device according to claim 20 or 21, wherein: The factor graph processing module is used to: Determining a first transformation matrix based on the speed of the dynamic obstacle at the adjacent moments and the time interval between the adjacent moments; Based on the first transformation matrix, transform the obstacle posture at a previous moment in the adjacent moments to obtain a sixth intermediate value; The fifth constraint condition is constructed, and the fifth constraint condition includes: a difference value between the sixth intermediate value and the obstacle posture at a later moment in the adjacent moments is less than or equal to a fifth threshold.

23. The device according to any one of claims 14 to 22, wherein: The point cloud acquisition module is used to: The laser radar system of the vehicle is used to obtain point clouds at multiple adjacent moments.

24. The device according to any one of claims 14 to 22, wherein: The first determining module is used to: In the dynamic point clouds at a plurality of consecutive moments, obstacle point clouds corresponding to the same dynamic obstacle are identified, and the same identifier is assigned to the obstacle point clouds corresponding to the same dynamic obstacle.

25. The device according to any one of claims 15 to 22, wherein: The factor graph processing module is used to: A nonlinear optimization method is used to iterate the values ​​of the nodes in the factor graph for multiple times until a convergence condition is satisfied; the convergence condition includes at least one of the following: The number of iterations is greater than or equal to the threshold; The adjusted value of the node satisfies the constraint condition.

26. The device according to claim 25, wherein The factor graph processing module is used to: Set corresponding weights for the first constraint condition, the second constraint condition, the third constraint condition, the fourth constraint condition, and the fifth constraint condition respectively; Based on the weights, the factor graph is optimized.

27. An electronic device comprising: at least one processor; as well as a memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the method according to any one of claims 1 to 13.

28. A non-transitory computer-readable storage medium storing computer instructions, wherein: The computer instructions are used to cause the computer to execute the method according to any one of claims 1-13.

29. A computer program product comprising a computer program, which, when executed by a processor, implements the method according to any one of claims 1 to 13.

Images