Positioning method, electronic equipment and storage medium

Abstract

The invention discloses a positioning method, electronic equipment and a storage medium, and the positioning method comprises the steps: determining the initial pose of a target object at the current moment based on sensor data collected by a sensor in the target object between the previous moment and the current moment and the pose of the target object at the previous moment; target data between the current moment and at least two historical moments are respectively utilized to construct at least one group of constraint factors, the at least two historical moments comprise a previous moment of the current moment and a historical moment adjacent to the previous moment, and the target data comprise a relative position relationship; and optimizing the initial pose by using at least one group of constraint factors to obtain a target pose of the target object at the current moment. According to the scheme, more accurate positioning can be realized.

Description

Technical Field

[0001] The present application relates to the field of computer technology, and in particular to a positioning method, an electronic device and a storage medium. Background Art

[0002] In recent years, robotics technology has developed rapidly and achieved amazing results. More and more robots can realize a variety of functions and play an irreplaceable role in various fields. Robotics has become one of the important technologies that promote the progress of mankind. Among them, SLAM (Simultaneous Localization and Mapping) technology plays a vital role in the field of mobile robots, enabling mobile robots to achieve autonomous positioning and environmental perception. However, at present, because certain errors are inevitable in the process of navigation and positioning, the positioning results of the robot may be more incorrect as time accumulates. There is an urgent need for a method to improve positioning accuracy. Summary of the invention

[0003] The present application at least provides a positioning method, an electronic device and a storage medium.

[0004] The present application provides a positioning method, comprising: determining the initial posture of a target object at the current moment based on sensor data collected by a sensor in a target object between the previous moment and the current moment and the posture of the target object at the previous moment; constructing at least one set of constraint factors using target data between the current moment and at least two historical moments respectively, wherein the at least two historical moments include the previous moment of the current moment and adjacent historical moments of the previous moment, and the target data include a relative position relationship; optimizing the initial posture using at least one set of constraint factors to obtain the target posture of the target object at the current moment.

[0005] In some embodiments, at least one group of constraint factors includes a first pre-integration constraint factor and a second pre-integration constraint factor, the target data also includes a zero bias predicted for the sensor at each moment, the relative position relationship includes a first type of relative position relationship determined using the sensor data, and the target data between the current moment and at least two historical moments are used to construct at least one group of constraint factors, including: determining the first pre-integration constraint factor based on the first type of position relationship between the current moment and the previous moment, the initial posture of the target object at the current moment, the historical posture of the target object at the previous moment, and the zero bias predicted for the sensor in the target object at the current moment and the previous moment; and determining the second pre-integration constraint factor based on the first type of relative position relationship between the current moment and the adjacent historical moments of the previous moment, the initial posture of the target object at the current moment, the historical posture of the target object at the adjacent historical moments of the previous moment, and the zero bias predicted for the sensor in the target object at the current moment and the adjacent historical moments of the previous moment.

[0006] In some embodiments, the target object includes a laser radar, which is used to collect radar point cloud data. At least one set of constraint factors includes a first laser odometer constraint factor and a second laser odometer constraint factor. The relative position relationship includes a second type of relative position relationship determined by using the radar point cloud data collected by the laser radar at each moment. The method also includes: matching the radar point cloud data collected at each moment with a preset map to obtain a second type of relative position relationship between the target object at a current moment and a previous moment, and a second type of relative position relationship between the target object at the current moment and adjacent historical moments of the previous moment. The preset map is constructed using the radar point cloud data collected by the target object at historical moments; constructing at least one set of constraint factors using the target data between the current moment and at least two historical moments, respectively, including: determining the first laser odometer constraint factor based on the initial posture, the posture of the target object at the previous moment, and the second type of relative position relationship between the current moment and the previous moment; and determining the second laser odometer constraint factor based on the initial posture, the posture of the target object at the adjacent historical moments of the previous moment, and the second type of relative position relationship between the current moment and the adjacent historical moments of the previous moment.

[0007] In some embodiments, the target object includes a laser radar, which is used to collect radar point cloud data. The method also includes: matching the radar point cloud data collected by the laser radar in the target object at the current moment with the prior map to obtain a candidate pose of the target object at the current moment; constructing a global map matching constraint factor based on the initial pose and the candidate pose; optimizing the initial pose using at least one set of constraint factors to obtain the target pose of the target object at the current moment, including: optimizing the initial pose using a combined global map matching constraint factor and at least one set of constraint factors to obtain the target pose of the target object at the current moment.

[0008] In some embodiments, the initial pose is optimized by combining a global map matching constraint factor and at least one group of constraint factors to obtain a target pose of the target object at the current moment, including: constructing incremental equations corresponding to each constraint factor respectively; adding the Hessian matrices corresponding to each incremental equation to obtain a target Hessian matrix; and optimizing the initial pose using the target Hessian matrix to obtain a target pose.

[0009] In some embodiments, the method also includes: obtaining multiple groups of constraint factors constructed from multiple historical moments; in response to the total number of moments of multiple historical moments and the current moment being greater than a preset number of moments, marginalizing the multiple groups of constraint factors constructed from multiple moments to obtain several groups of constraint factors after marginalization; optimizing the initial pose using at least one group of constraint factors to obtain the target pose of the target object at the current moment, including: optimizing the initial pose by combining at least one group of constraint factors constructed at the current moment and several groups of constraint factors.

[0010] In some embodiments, multiple groups of constraint factors constructed from multiple moments are marginalized to obtain several groups of constraint factors after marginalization, including: obtaining at least one first constraint factor from the multiple groups of constraint factors that is independent of the target historical moment and at least one second constraint factor from the multiple groups of constraint factors that is related to the target historical moment, the target historical moment being the oldest historical moment among the multiple historical moments; screening at least one second constraint factor to obtain the remaining several second constraint factors; combining at least one first constraint factor and several second constraint factors to obtain several groups of constraint factors after marginalization.

[0011] In some embodiments, the target object includes a laser radar, which is used to collect radar point cloud data. The method also includes: in response to the presence of a point cloud corresponding to a preset positioning object in the environment in the radar point cloud data collected by the laser radar at the current moment, obtaining multiple groups of constraint factors constructed at several historical moments, the number of the several moments being greater than the number of the multiple historical moments; constructing a constraint factor group based on the multiple groups of constraint factors constructed at the several historical moments and at least one group of constraint factors constructed at the current moment; optimizing the initial pose using at least one group of constraint factors to obtain the target pose of the target object at the current moment, including: combining each constraint factor in the constraint factor group to optimize the initial pose to obtain the target pose of the target object at the current moment.

[0012] In some embodiments, each constraint factor in the constraint factor group is combined to optimize the initial pose to obtain the target pose of the target object at the current moment, including: sparsifying each constraint factor in the constraint factor group to obtain a target constraint factor group, wherein the target constraint factor group includes multiple constraint factors; and combining each constraint factor in the target constraint factor group to optimize the initial pose to obtain the target pose of the target object at the current moment.

[0013] In some embodiments, the target object includes a laser radar, which is used to collect radar point cloud data. The method also includes: projecting the radar point cloud data collected by the laser radar into a preset map based on the target posture, and the preset map is constructed by radar point cloud data collected by the target object at a historical moment; using radar point cloud data for which no corresponding point cloud exists in the preset map, the preset map is updated.

[0014] The present application provides a positioning device, including: an initial posture determination module, a constraint factor construction module and a posture optimization module; the initial posture determination module is used to determine the initial posture of a target object at the current moment based on sensor data collected by a sensor in the target object between the previous moment and the current moment and the posture of the target object at the previous moment; the constraint factor construction module is used to construct at least one group of constraint factors using target data between the current moment and at least two historical moments respectively, the at least two historical moments include the previous moment of the current moment and adjacent historical moments of the previous moment, and the target data includes a relative position relationship; the posture optimization module is used to optimize the initial posture using at least one group of constraint factors to obtain the target posture of the target object at the current moment.

[0015] The present application provides an electronic device, including a memory and a processor, wherein the processor is used to execute program instructions stored in the memory to implement the above positioning method.

[0016] The present application provides a computer-readable storage medium on which program instructions are stored. When the program instructions are executed by a processor, the above-mentioned positioning method is implemented.

[0017] The above scheme, after obtaining the initial posture of the target object at the current moment, can construct constraint factors according to the relative position relationship between the current moment and the previous moment, as well as the position relationship between the current moment and the adjacent historical moments of the previous moment, to obtain at least one set of constraint factors, thereby optimizing the initial posture according to at least one set of constraint factors, which can reduce the impact of the accumulated error in the positioning process on the positioning accuracy, thereby improving the positioning accuracy.

[0018] It is to be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] The drawings herein are incorporated into the specification and constitute a part of the specification. These drawings illustrate embodiments consistent with the present application and are used together with the specification to illustrate the technical solution of the present application.

[0020] Figure 1 It is a flowchart of an embodiment of the positioning method of the present application;

[0021] Figure 2 is a schematic diagram of a pre-integration constraint factor shown in an embodiment of a positioning method of the present application;

[0022] Figure 3 is a schematic diagram of a laser odometer constraint factor shown in an embodiment of a positioning method of the present application;

[0023] Figure 4 is another flow chart of an embodiment of the positioning method of the present application;

[0024] Figure 5a An embodiment of the positioning method of the present application shows a schematic diagram of the Jacobian matrix of the pre-integrated constraint factor with respect to the optimization variable;

[0025] Figure 5b An embodiment of the positioning method of the present application shows a schematic diagram of the Hessian matrix corresponding to the first group of pre-integration constraint factors;

[0026] Figure 5c The first embodiment of the positioning method of the present application shows a schematic diagram of the Hessian matrix corresponding to the second group of pre-integration constraint factors;

[0027] Figure 5d An embodiment of the positioning method of the present application shows a schematic diagram of a Hessian matrix related to a pre-integration constraint factor;

[0028] Figure 6aAn embodiment of the positioning method of the present application shows a schematic diagram of the Jacobian matrix of the laser odometer constraint factor with respect to the optimization variable;

[0029] Figure 6b An embodiment of the positioning method of the present application shows a schematic diagram of the Hessian matrix corresponding to the first and second groups of laser odometer constraint factors;

[0030] Figure 6c The first embodiment of the positioning method of the present application shows a schematic diagram of the Hessian matrix corresponding to the third and fourth groups of laser odometer constraint factors;

[0031] Figure 6d An embodiment of the positioning method of the present application shows a schematic diagram of the Hessian matrix related to the laser odometer constraint factor;

[0032] Figure 7a An embodiment of the positioning method of the present application shows a schematic diagram of the Jacobian matrix of the global map matching constraint factor with respect to the optimization variable;

[0033] Figure 7b An embodiment of the positioning method of the present application shows a schematic diagram of the Hessian matrix corresponding to the global map matching constraint factor;

[0034] Figure 8 This is another flowchart of an embodiment of the positioning method of the present application;

[0035] Figure 9a An embodiment of the positioning method of the present application shows a schematic diagram of a target Hessian matrix that needs to be marginalized;

[0036] Figure 9b An embodiment of the positioning method of the present application shows a schematic diagram of a Hessian matrix associated with a pre-integrated constraint factor that is independent of the target historical moment;

[0037] Fig.9c An embodiment of the positioning method of the present application shows a schematic diagram of a Hessian matrix associated with a laser odometer constraint factor that is independent of the target historical moment;

[0038] Figure 9d An embodiment of the positioning method of the present application shows a schematic diagram of a Hessian matrix related to a global map matching constraint factor that is independent of the target historical moment;

[0039] Fig.9e An embodiment of the positioning method of the present application shows a schematic diagram of a Hessian matrix jointly formed by at least one first constraint factor that is independent of the target historical moment;

[0040] Fig.10a An embodiment of the positioning method of the present application shows a schematic diagram of a Hessian matrix associated with a pre-integrated constraint factor associated with a target historical moment;

[0041] Fig.10b An embodiment of the positioning method of the present application shows a schematic diagram of a Hessian matrix associated with a laser odometer constraint factor associated with a target historical moment;

[0042] Fig.10c An embodiment of the positioning method of the present application shows a schematic diagram of a Hessian matrix associated with a global map matching constraint factor associated with a target historical moment;

[0043] Fig.10d An embodiment of the positioning method of the present application shows a schematic diagram of a Hessian matrix jointly formed by at least one second constraint factor related to the target historical moment;

[0044] Fig.11a An embodiment of the positioning method of the present application shows a schematic diagram of the determined Hrr;

[0045] Fig.11b An embodiment of the positioning method of the present application shows a schematic diagram of the determined Hrm;

[0046] Fig.11c An embodiment of the positioning method of the present application shows a schematic diagram of the determined Hmm;

[0047] Fig.11d The first embodiment of the positioning method of the present application shows the determination of the obtained H p 2 Schematic diagram;

[0048] Fig.12 This is another flow chart of an embodiment of the positioning method of the present application;

[0049] Fig.13a An embodiment of the positioning method of the present application shows a schematic diagram of constraint factors before sparseness;

[0050] Fig.13b An embodiment of the positioning method of the present application shows a schematic diagram of the constraint factors after sparseness;

[0051] Fig.13c An embodiment of the positioning method of the present application shows a schematic diagram of the Hessian matrix corresponding to the sparse constraint factor;

[0052] Fig.14 It is a structural schematic diagram of an embodiment of a positioning device of the present application;

[0053] Fig.15 It is a structural schematic diagram of an embodiment of the electronic device of the present application;

[0054] Fig.16 It is a structural diagram of an embodiment of a computer-readable storage medium of the present application. DETAILED DESCRIPTION

[0055] The scheme of the embodiment of the present application is described in detail below in conjunction with the drawings of the specification.

[0056] In the following description, for the purpose of explanation rather than limitation, specific details such as specific system structures, interfaces, and technologies are provided to facilitate a thorough understanding of the present application.

[0057] The term "and / or" in this article is only a description of the association relationship of associated objects, indicating that there may be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this article generally indicates that the objects associated before and after are in an "or" relationship. In addition, "many" in this article means two or more than two. In addition, the term "at least one" in this article means any combination of at least two of any one or more of a plurality of, for example, including at least one of A, B, and C, can mean including any one or more elements selected from the set consisting of A, B, and C.

[0058] In some application scenarios, the positioning method provided by the present application can be applied to the computer field, etc. The execution subject for implementing the positioning method described in the present application can be a positioning device, an electronic device, etc. For example, the positioning device can be set in a terminal device or a server or other processing device, wherein the terminal device can be a robot, an electronic device, a handheld device, a computing device, a vehicle-mounted device, a wearable device, etc. In some possible implementations, the positioning method can be implemented by a processor calling a computer-readable instruction stored in a memory.

[0059] See also Figure 1 , Figure 1 1 is a flow chart of an embodiment of the positioning method of the present application. Figure 1 As shown, the positioning method provided by the embodiment of the present disclosure may include the following steps:

[0060] Step S11: Determine the initial position and posture of the target object at the current moment based on the sensor data collected by the sensor in the target object between the previous moment and the current moment and the position and posture of the target object at the previous moment.

[0061] The pose at the last moment may be the pose of the target determined according to the positioning method. The pose at the first moment may be considered as the origin of the coordinate system of the target object or the pose at the first moment may be the pose determined by matching the radar point cloud data collected by the laser radar in the target object with the preset map. The sensor data may include the linear velocity collected by the odometer and the angular velocity collected by the gyroscope. The odometer provides the linear velocity, which can preliminarily estimate the change in the distance of the target object. Common ones include wheel encoders, so that the displacement of the target object at the current moment can be estimated based on the position change at the previous moment and the position at the previous moment. Exemplarily, integrating the linear velocity can obtain the moving distance of the target object from the previous moment to the current moment. The gyroscope provides the angular velocity, which can preliminarily estimate the change in the angle of the target object, so that the pose of the target object at the current moment can be estimated based on the pose and angle change at the previous moment. Exemplarily, integrating the angular velocity collected by the gyroscope can obtain the rotation angle of the target object from the previous moment to the current moment. Therefore, the initial position of the target object at the current moment can be determined based on the position at the previous moment, the moving distance, and the rotation angle.

[0062] Step S12: construct at least one set of constraint factors by using the target data between the current moment and at least two historical moments respectively.

[0063] Among them, at least two historical moments include the previous moment of the current moment and the adjacent historical moment of the previous moment. For example, if the current moment is moment t, the previous moment can be considered as moment t-1, and the adjacent historical moment of the previous moment can be considered as moment t-2. Optionally, the target data includes a relative position relationship. The relative position relationship can include relative position and / or relative posture. At least one group can be one group or multiple groups.

[0064] Step S13: Optimize the initial pose using at least one set of constraint factors to obtain the target pose of the target object at the current moment.

[0065] Exemplarily, the constraint factor may be a constraint equation, and the quantity to be optimized in the constraint equation may be a residual of the posture, and the initial posture can be optimized by optimizing the residual.

[0066] The above scheme, after obtaining the initial posture of the target object at the current moment, can construct constraint factors according to the relative position relationship between the current moment and the previous moment, as well as the position relationship between the current moment and the adjacent historical moments of the previous moment, to obtain at least one set of constraint factors, thereby optimizing the initial posture according to at least one set of constraint factors, which can reduce the impact of the accumulated error in the positioning process on the positioning accuracy, thereby improving the positioning accuracy.

[0067] In some embodiments, at least one set of constraint factors includes a first pre-integration constraint factor and a second pre-integration constraint factor, and the target data also includes a zero bias predicted for the sensor at each moment. Among them, the relative position relationship includes a first type of relative position relationship determined using sensor data. Exemplarily, the first type of relative position relationship can be a relative relationship in translation and rotation determined based on an odometer and a gyroscope in the target object. Specifically, it can be obtained by integrating the sensor data between two moments. The zero bias can be considered as an error in the sensor data. The initial posture includes translation and rotation matrices.

[0068] On this basis, the above step S12 may include the following steps:

[0069] Based on the first type of relative position relationship between the current moment and the previous moment, the initial position and posture of the target object at the current moment, the historical position and posture of the target object at the previous moment, and the zero bias predicted by the sensor in the target object at the current moment and at the previous moment, a first pre-integration constraint factor is determined. And, based on the first type of relative position relationship between the current moment and the adjacent historical moment of the previous moment, the initial position and posture of the target object at the current moment, the historical position and posture of the target object at the adjacent historical moment of the previous moment, and the zero bias predicted by the sensor in the target object at the current moment and at the adjacent historical moment of the previous moment, a second pre-integration constraint factor is determined.

[0070] Exemplarily, a first pre-integration constraint factor can be established for the current moment and the previous moment, and a second pre-integration constraint factor can be established for the current moment and the moment before the previous moment. The pre-integration constraint factor can be considered as a pre-integration constraint equation. The specific method of constructing a single pre-integration constraint factor can refer to formula (1):

[0071]

[0072] Among them, r B (P i ,R i ,B wi ,P j ,R j ,B wj ) can be considered as the quantity to be optimized (P i ,R i ,B wi ,P j ,R j ,B wj ), i represents the i moment, j represents the j moment, P i represents the predicted position of the target object at time i. Similarly, P j represents the predicted position of the target object at time j, T represents transposition, and R iRepresents the rotation matrix predicted for the target object at time i. Similarly, R j Represents the rotation matrix predicted for the target object at time j. The first type of relative position relationship between time i and time j includes and It represents the translation amount of the target object between time i and time j determined according to the sensor data. represents the rotation amount of the target object between time i and time j determined according to the sensor data, P i With R i The combination of is the position of the target object at time i. Similarly, P j With R j The combination of is the position and posture of the target object at time j. wj represents the zero bias of the target object prediction at time j, B wi Represents the zero bias predicted by the target object at time i. For example, if j is the current time, time i can be the previous time or the previous time before the previous time. That is, in addition to optimizing the position and posture of the target object at the current time, the position and posture of the previous time and the previous time before the previous time can also be optimized, and the zero bias of each time can also be optimized.

[0073] That is, in this embodiment, the odometer and gyroscope data between two moments can be pre-integrated to construct constraints. The pre-integration constraints between two poses constrain the pose and zero bias of the optimization variable. To better understand the pre-integration constraint factors in this solution, please refer to Figure 2 , Figure 2 In the figure, the circle represents the pose T(P+R), the rectangle represents the pre-integration constraint, and the pentagon represents the zero bias.

[0074] In some embodiments, the target object includes a laser radar, and the laser radar is used to collect radar point cloud data. At least one set of constraint factors includes a first laser odometry constraint factor and a second laser odometry constraint factor. Exemplarily, a first laser odometry constraint factor can be established for the current moment and the previous moment, and a second laser odometry constraint factor can be established for the current moment and the moment before the previous moment. Exemplarily, the target data also includes radar point cloud data collected by the laser radar in the target object at each moment. The laser radar can be a 2D laser radar or a 3D laser radar. This scheme takes the laser radar as a 2D laser radar as an example. 2D laser radar is a sensor for obtaining two-dimensional plane information, and is used to detect two-dimensional plane contour information of the surrounding environment. The relative position relationship includes a second type of relative position relationship determined by using the radar point cloud data collected by the laser radar at each moment. The positioning method may also include the following steps:

[0075] The radar point cloud data collected at each moment is matched with the preset map to obtain the second type of relative position relationship between the target object at the current moment and the previous moment, and the second type of relative position relationship between the target object at the current moment and the adjacent historical moments of the previous moment. The preset map is constructed using the radar point cloud data collected by the target object at the historical moment. The second type of relative position relationship can include the relationship between displacement and rotation.

[0076] Among them, the radar point cloud data collected at each moment is matched with the preset map respectively, and the second type of relative position relationship between the target object at the current moment and the previous moment and the second type of relative position relationship between the target object at the current moment and the adjacent historical moments of the previous moment can be obtained by matching the radar point cloud data collected at each moment with the preset map respectively, obtaining the first candidate posture of the target object at the current moment, the second candidate posture of the target object at the previous moment, and the third candidate posture of the target object at the adjacent historical moment of the previous moment, and taking the relative position relationship between the first candidate posture and the second candidate posture as the second type of relative position relationship between the target object at the current moment and the previous moment, and taking the relative position relationship between the first candidate posture and the third candidate posture as the second type of relative position relationship between the target object at the current moment and the adjacent historical moments of the previous moment. Exemplarily, a laser odometer can be constructed using a preset map, and then the posture of the target object at each moment is determined according to the laser odometer, thereby determining the relative position relationship of time at each moment.

[0077] On this basis, the above step S12 may also include the following steps: determining a first laser odometer constraint factor based on the initial posture, the posture of the target object at the previous moment, and the second type of relative position relationship between the current moment and the previous moment; and determining a second laser odometer constraint factor based on the initial posture, the posture of the target object at the adjacent historical moment of the previous moment, and the second type of relative position relationship between the current moment and the adjacent historical moment of the previous moment.

[0078] The first laser odometer constraint factor and the second laser odometer constraint factor may be laser odometer constraint equations. That is, after determining the position and posture of the target object at each historical moment, the radar point cloud data collected at each historical moment is projected onto the world coordinate system according to the position and posture at each historical moment to obtain a preset map. The specific method of constructing the first laser odometer constraint factor and the second laser odometer constraint factor may refer to formula (2):

[0079]

[0080] Among them, r L Represents the amount to be optimized (T bi ,T bj ).bi represents the position of the target object at time i, T vj represents the position of the target object at time j, T bi Can include and T bj Can include and W represents the coordinate system, and T represents the transposition. The pose includes the displacement and rotation matrix. represents the displacement of the target object at time j, Can be used with P j same, represents the displacement of the target object at time i, With P i same, Represents the relative displacement of the target object in the relative position relationship between time i and time j, Represents the relative rotation of the target object in the relative position relationship between time i and time j, represents the rotation matrix of the target object at time i, Can be used with R i , represents the rotation matrix of the target object at time j, Can be used with R j same.

[0081] This embodiment takes into account that in the process of positioning and mapping, due to environmental changes, the position and posture of the laser will fluctuate greatly when matching with the prior map. Adding laser odometer constraints can improve positioning stability and robustness. This solution adds laser odometer constraints between the current frame and the previous frame, and between the current frame and the previous frame. The laser odometer constrains the position and posture. For details, please refer to Figure 3 ,like Figure 3 The rectangular box shown represents the laser odometry constraint factor, and the circle represents the candidate pose of the target object at each moment.

[0082] In some embodiments, the target object includes a laser radar, which is used to collect radar point cloud data. The positioning method may also include the following steps: matching the radar point cloud data collected by the laser radar in the target object at the current moment with the prior map to obtain the candidate pose of the target object at the current moment. Then, based on the initial pose and the candidate pose, a global map matching constraint factor is constructed. On this basis, the above step S12 may include the following steps: optimizing the initial pose in combination with the global map matching constraint factor and at least one set of constraint factors to obtain the target pose of the target object at the current moment.

[0083] The radar point cloud data collected by the laser radar in the target object at the current moment is matched with the prior map to obtain a candidate pose of the target object at the current moment, which can be the fourth candidate pose. Among them, the global map matching constraint factor can be considered as a global map matching constraint equation.

[0084] The method of constructing the global map matching constraint factor can refer to formula (3):

[0085]

[0086] Among them, r G (T bj ) represents the quantity to be optimized (T bj ) residual. represents the displacement at time j, represents the rotation matrix at time j, represents the displacement in the fourth candidate pose mentioned above, represents the rotation matrix in the fourth candidate pose mentioned above.

[0087] Optionally, see Figure 4 The above-mentioned method of optimizing the initial pose by combining the global map matching constraint factor and at least one group of constraint factors to obtain the target pose of the target object at the current moment may include the following steps:

[0088] Step S21: construct the incremental equations corresponding to each constraint factor respectively.

[0089] Optionally, the constraint factor includes at least one of a pre-integration constraint factor, a laser odometer constraint factor, and a global map matching constraint factor. This embodiment takes the constraint factor including the pre-integration constraint factor, the laser odometer constraint factor, and the global map matching constraint factor as an example.

[0090] For the pre-integration constraint factor, when solving the optimization variable, the Gauss-Newton method can be used to solve it, and then the incremental equation is constructed. The incremental equation needs to solve the Jacobian of the residual with respect to the optimization variable and construct the corresponding Hessian matrix from the Jacobian. The Jacobian and its corresponding Hessian matrix model are as follows Figure 5a to Figure 5d shown. Figure 5a The Jacobian matrix of the pre-integrated constraint factor with respect to the optimization variable is shown as Figure 5a The first set of pre-integration constraint factors rB0 shown is constructed by the pose and zero bias corresponding to time 0 and time 1, and the second set of pre-integration constraint factors rB1 is constructed by the pose and zero bias corresponding to time 0 and time 2. The Hessian matrix corresponding to the first set of pre-integration constraint factors is as follows Figure 5b As shown, the Hessian matrix corresponding to the second group of pre-integration is Figure 5cAs shown. Add the Hessian matrices corresponding to the first and second groups of pre-integration constraints to obtain the Hessian matrix corresponding to the pre-integration constraint factors. Figure 5d As shown, Figure 5d The four elements formed by T0M0 in the upper left corner are the superposition of different pre-integration constraint factors on the matrix block.

[0091] For the laser odometer constraint factor, the Jacobian and Hessian matrices of the residual with respect to the optimization variables are visualized as follows: Figures 6a to 6d As shown: Figure 6a The Jacobian matrix of the laser odometry constraint factor with respect to the optimization variable is shown as Figure 6a The first group of laser odometer constraint factors rL0 is constructed by the poses corresponding to time 0 and time 1, and the second group of laser odometer constraint factors rL1 is constructed by the poses corresponding to time 0 and time 2. The same is true for the third and fourth groups. The Hessian matrices corresponding to the first and second groups of laser odometer constraint factors are as follows: Figure 6b As shown, the Hessian matrices corresponding to the third and fourth groups of laser odometers are as follows: Figure 6c As shown. Add the Hessian matrices corresponding to the first and second groups of laser odometer constraints to obtain the Hessian matrix corresponding to the laser odometer constraint factor. Figure 6d As shown, Figure 6d The two elements at T1T1 and T2T2 are the superposition of different laser odometer constraint factors on the matrix block.

[0092] For the global map matching constraint factor, the Jacobian and Hessian matrices of the residual with respect to the optimization variables are visualized as follows: Figure 7a and Figure 7b As shown, Figure 7a The Jacobian matrix of the global map matching constraint factor with respect to the optimization variables is shown as Figure 7a The first group of global map matching constraint factors rG0 shown are constructed by the posture corresponding to time 0, the second group of global map matching constraint factors rG1 are constructed by the posture corresponding to time 1, and the third group of global map matching constraint factors rG2 are constructed by the posture corresponding to time 2. Figure 7b The Hessian matrix obtained by adding the Hessian matrices corresponding to the three sets of global map matching constraint factors is shown.

[0093] Step S22: Add the Hessian matrices corresponding to the incremental equations to obtain the target Hessian matrix.

[0094] The method of adding the Hessian matrices corresponding to each constraint factor to obtain the target Hessian matrix can refer to the method of adding the Hessian matrices in the above-mentioned pre-integration constraint factor or the method of adding the Hessian matrices in other constraint factors. In some application scenarios, the constraint factors include the pre-integration constraint factor, the laser odometer constraint factor, and the global map matching constraint factor. For example, the Hessian matrices corresponding to the pre-integration constraint factor, the laser odometer constraint factor, and the global map matching constraint factor can be directly added to obtain the target Hessian matrix.

[0095] Step S23: Optimize the initial pose using the target Hessian matrix to obtain the target pose.

[0096] The Gauss-Newton method is a solution to the optimization problem, which is developed from the idea of ​​linear regression. Its main idea is to approximate the optimal solution of the nonlinear least squares problem through continuous iteration. Specifically, the Gauss-Newton method constructs a linear model and uses the Jacobian matrix for approximation to finally solve the incremental equation. The Gauss-Newton method updates the parameters by solving the incremental equation, so that the residual r is continuously reduced to achieve the purpose of optimization.

[0097] In some embodiments, the positioning method may further include the following steps: obtaining multiple sets of constraint factors constructed from multiple historical moments. In response to the total number of moments of the multiple historical moments and the current moment being greater than a preset number of moments, marginalizing the multiple sets of constraint factors constructed from the multiple moments to obtain several sets of constraint factors after marginalization. On this basis, the above step S13 may include the following steps: optimizing the initial posture by combining at least one set of constraint factors constructed from the current moment and several sets of constraint factors.

[0098] When the area of ​​the target object movement region is larger, the number of variable nodes that can be jointly optimized is greater, and the corresponding jointly optimized Hessian matrix is ​​also larger, and the time consumed by the optimization and the computing resources occupied will be greatly increased. This embodiment proposes a sliding window marginalization method, that is, a fixed number of variables in the sliding window are optimized each time, and then the oldest frame is marginalized, and the constraints of the oldest frame are transferred to the constraints between the associated variables in the sliding window, so that the constraint factors are not lost, and the accuracy of the final optimization result is guaranteed.

[0099] Among them, see Figure 8 The above method of marginalizing the multiple sets of constraint factors constructed at multiple moments to obtain the marginalized sets of constraint factors may include the following steps:

[0100] Step S31: obtaining at least one first constraint factor that is unrelated to the target historical moment from among the multiple sets of constraint factors and at least one second constraint factor that is related to the target historical moment from among the multiple sets of constraint factors.

[0101] Among them, the target historical moment is the oldest historical moment in multiple historical moments. Exemplarily, the target historical moment can be the oldest moment in the sliding window. In this embodiment, the moments included in the sliding window can be 4 to 6, and in this embodiment, the moments included in the sliding window are 4 as an example. When the fifth moment arrives, the oldest frame needs to be marginalized. At least one first constraint factor that is independent of the target historical moment can be considered as a constraint factor that is not constructed using the pose and zero bias of the target historical moment, and the second constraint factor related to the target historical moment can be considered as a constraint factor constructed using the pose and / or zero bias data of the target historical moment.

[0102] For example, the Hessian matrix composed of multiple sets of constraint factors can be considered as the target Hessian matrix used at the last moment. Marginalizing multiple sets of constraint factors can be considered as marginalizing the target Hessian matrix used at the last moment. Figure 9a For example, the matrix obtained that is independent of the target historical moment includes the Hessian matrix related to the pre-integration constraint factor. Figure 9b , the Hessian matrix related to the laser odometry constraint factor can be found in Fig.9c As shown, the Hessian matrix related to the global map matching constraint factor can be found in Figure 9d As shown, the Hessian matrix composed of at least one first constraint factor that is independent of the target historical moment can be referred to Fig.9e shown.

[0103] At least one second constraint factor related to the target historical moment in the plurality of constraint factors may include a Hessian matrix related to the pre-integrated constraint factor. Fig.10a , the Hessian matrix related to the laser odometry constraint factor can be found in Fig.10b As shown, the Hessian matrix related to the global map matching constraint factor can be found in Fig.10c As shown, the Hessian matrix formed by at least one second constraint factor related to the target historical moment can be referred to Fig.10d shown.

[0104] Step S32: screening at least one second constraint factor to obtain the remaining second constraint factors.

[0105] The marginalization method can refer to formula (4):

[0106]

[0107] Among them, b p and H pare the residuals and Hessian matrices after marginalization. bmr is the residual corresponding to the remaining variables in the sliding window after marginalization. Hmm is the Hessian matrix corresponding to the marginalized variables, Hrr is the Hessian matrix corresponding to the remaining variables, and Hmr is the Hessian matrix between the marginalized variables and the remaining variables, which is obtained by multiplying the corresponding Jacobians. Hrm and Hmr are transposed of each other.

[0108] For example, Hrr, Hrm, Hmm and Hmr can be found based on the Hessian matrix formed by at least one second constraint factor related to the target historical moment, so that the marginalized matrix can be obtained. Figures 11a to 11c .

[0109] Step S33: combining at least one first constraint factor and a plurality of second constraint factors to obtain a plurality of marginalized groups of constraint factors.

[0110] Among them, H p Can be H p 1 and H p 2 add together to get, H p 1 can be the above Fig.9e The Hessian matrix, H p 2 can be determined based on Hrr, Hrm, Hmm, and the specific H p 2 For reference Fig.11d For example, Fig.9e The Hessian matrix shown is similar to Fig.11d The Hessian matrix is ​​added to obtain H in formula (4). p , that is, the marginalized Hessian matrix is ​​obtained.

[0111] Optionally, each constraint factor can be fused through weights, and the weights can be preset or dynamically set. In some application scenarios, the weights are dynamically set in the following ways: the covariance matrix of the pre-integration constraint factor is derived from the pre-integration model. The covariance matrix of the laser odometer can be derived from the matching scores of the two frames of laser data constructed with the prior map and the preset map constructed by it. If the laser matching scores of the two key frames are greater than the threshold s1, the corresponding covariance is ε1 (0.2, 0.2, 0.2, 0.015, 0.015, 0.015); if the laser matching score of one frame is less than the threshold s1, the defense difference matrix is ​​ε1*10; if the laser matching scores of the two frames are less than the threshold s1, the defense difference matrix is: ε1*100. The covariance matrix corresponding to the residual constructed by prior map matching is derived from the matching score between the current frame and the prior map. If the matching score is greater than s2, the covariance is ε2(0.05, 0.05, 0.05, 0.01, 0.01, 0.01), otherwise the covariance is ε2*100. The larger the covariance, the lower the matching weight during fusion.

[0112] In some embodiments, the target object includes a laser radar, which is used to collect radar point cloud data. Fig.12 The positioning method may further include the following steps:

[0113] Step S41: In response to the presence of a point cloud corresponding to a preset positioning object in the environment in the radar point cloud data collected by the laser radar at the current moment, multiple groups of constraint factors constructed at several historical moments are obtained.

[0114] Optionally, the number of the plurality of moments is greater than the number of the plurality of historical moments. Exemplarily, if the point cloud corresponding to the preset positioning object is not collected, the moments included in the sliding window may be at most 4 moments, and if the point cloud corresponding to the preset positioning object is collected, the moments included in the sliding window may be 6 or more moments. The preset positioning object may be an artificial feature.

[0115] Step S42: construct a constraint factor group based on multiple groups of constraint factors constructed at several historical moments and at least one group of constraint factors constructed at the current moment.

[0116] As can be seen from the above figures, after marginalization, as long as a constraint relationship is generated between the marginalized variables, the corresponding Hessian matrix block needs to be filled, and the Hessian matrix will become dense. During the map construction process, when detecting artificial features (landmarks) such as reflective columns and reflective strips and updating them to the map, multiple adjacent key frames will observe the same landmark point at the same time. In order to ensure the reliability of the constructed landmark, the number of key frames observing the landmark cannot be too small. Therefore, when the landmark is detected, the sliding window size becomes 6, and the optimization nodes in the sliding window can all observe the same reflective column features. After marginalizing the oldest frame, the constraint factors between the remaining 5 variable nodes in the sliding window are as many as 10, and the calculation efficiency is too low. For example, if the marginalized sliding window also contains constraints of 6 moments, you can refer to Fig.13a .

[0117] On this basis, the above step S13 may include the following steps: combining the constraint factors in the constraint factor group, optimizing the initial posture, and obtaining the target posture of the target object at the current moment.

[0118] In some embodiments, the above-mentioned method of optimizing the initial pose by combining the constraint factors in the constraint factor group to obtain the target pose of the target object at the current moment may be: sparsely distribute the constraint factors in the constraint factor group to obtain a target constraint factor group, wherein the target constraint factor group includes multiple constraint factors. Then, the initial pose is optimized by combining the constraint factors in the target constraint factor group to obtain the target pose of the target object at the current moment.

[0119] The larger the sliding window is, the greater the degree of common view is, and the denser the matrix is ​​after marginalization. Therefore, the embodiment can refer to the principle of chou-liu-tree to further sparse the dense matrix, that is, the constraints with the closest relationship between the optimized variable nodes are retained, and the constraint relationship between the nodes with weak constraints is deleted, so as to reduce the relationship between the matrix density and the sliding window size, thereby improving the calculation efficiency.

[0120] The specific sparse method can be:

[0121] Assume that the relative positions of the optimization variable nodes T1 and T2 constructed by pre-integration obey Gaussian distribution The relative position constructed by the laser odometer follows a Gaussian distribution The distribution of the two nodes obtained by prior map matching is The relative posture probability distribution between postures T1 and T2 after marginalization is Then the distribution of T0 and T1 is: e(u 12 ,ε 12 ), please refer to formula (5) for details:

[0122]

[0123] Then, the relative posture distribution eij between two connected nodes is calculated in turn, where i is 1-5, j is 1-5, and i is not equal to j.

[0124] Calculate the mutual information I and retain the edges with the largest mutual information. The method of calculating the mutual information may include but is not limited to formula (6):

[0125]

[0126] The constraint factor graph corresponding to the retained constraint edges can be referenced Fig.13b , the corresponding Hessian matrix can be found in Fig.13c .

[0127] Optionally, the target object includes a laser radar, and the laser radar is used to collect radar point cloud data. After executing the above step S13, the following steps may also be performed:

[0128] Based on the target pose, the radar point cloud data collected by the LiDAR is projected onto the preset map, which is constructed from the radar point cloud data collected by the target object at a historical moment. Then, the preset map is updated using the radar point cloud data that does not have a corresponding point cloud in the preset map.

[0129] From above Figures 13a to 13c It can be seen that the density of the Hessian matrix is ​​greatly reduced, thereby improving the computational efficiency. After solving the pose, the lidar point cloud data corresponding to the pose is projected onto the world coordinate system according to the pose, thereby constructing a map that reflects the current positioning environment.

[0130] In some embodiments, the odometer, gyroscope and 2D laser radar are used to build a map while cooperating with the target object to locate the prior map. When the prior map is used for positioning, the odometer, gyroscope and laser matching are tightly coupled, and a real-time map reflecting the navigation environment at that time is constructed based on the optimized positioning posture and the corresponding key frame radar point cloud data. The odometer and gyroscope construct a pre-integration model to calculate the corresponding posture change of three adjacent laser key frames. During the positioning process, a laser odometer is constructed to calculate the corresponding posture change of three adjacent laser key frames. The laser and map matching provide absolute posture constraints, and the three residual terms are jointly optimized to estimate the current posture of the device. When the number of optimized variables is greater than a certain threshold, the sliding window marginalization method is used to limit the number of optimizations, and the constraints of the removed variables are transferred to the variables in the sliding window through the marginalization method. The chou-liu-tree method is used to sparse the density between the variables in the sliding window to speed up the optimization of the posture of the variables in the sliding window.

[0131] For example, if the target object is a robot, the robot generates a two-dimensional grid map based on the warehouse environment, which includes shelves, workbenches, house support frames, and goods to be moved. The mobile robot uses odometers, laser data, and prior maps for positioning, and builds and updates sub-maps in real time. It can also use sub-maps to build laser odometers and determine redundant mapping areas, and use the remaining sub-maps to generate the latest map of the current environment.

[0132] In some application scenarios, the method of locating the target object may include the following steps: first import the prior map and perform relocation. Obtain the odometer and gyroscope information, and predict the current moment's posture based on the odometer and gyroscope data and the previous moment's posture. Determine whether the variable of the laser odometer information is greater than the threshold or whether the main laser matching time interval is greater than the threshold. If not, continue to execute the steps of obtaining the odometer and gyroscope information, and predicting the current moment's posture based on the odometer and gyroscope data and the previous moment's posture. Otherwise, match the laser data with the prior map to obtain the global map matching constraint factor, and determine the pre-integral constraint factor and the laser odometer constraint factor. Then add variables to the sliding window, and jointly optimize the constraint factors corresponding to the variables. Determine whether the number of jointly optimized variables is greater than the preset sliding window size. If not, directly perform joint optimization or perform joint optimization after sparseness. If so, remove the oldest frame from the sliding window, limit the number of optimized variables, and marginalize the constraints of the oldest frame to the constraints of the variables in the sliding window. If artificial features such as reflective columns are detected, the sliding window size is increased and the number of common view frames is increased to ensure the correctness of artificial feature solution. When artificial features are detected, chou-liu-tree and the covariance matrix between constrained time variables are used after marginalization to find the maximum spanning tree between nodes to reduce the density of the matrix. After pose optimization, the radar point cloud data of all key frames are projected into the map coordinate system according to the pose of each node to construct a point cloud map.

[0133] In the above scheme, the odometer and gyroscope are used to construct the constraint relationship between the current moment and the previous two moments, and the laser odometer also constructs the constraint relationship between the current moment and the previous two moments, thereby reducing the influence of the changes in the environment corresponding to the prior map on the positioning during mapping. When optimizing the posture, the sliding window method is used to control the number of variables optimized at the same time to improve the calculation efficiency, and the marginalization idea is used to completely transfer the constraints of the variables that slide out to the constraints between the variables in the sliding window, instead of directly discarding the oldest frame, so as to improve the calculation efficiency while ensuring the accuracy of the variable solution.

[0134] In addition, in order to improve computational efficiency, it is necessary to ensure the sparsity of the Hessian matrix constructed by the variables in the sliding window. When artificial features are detected, the size of the sliding window is increased to ensure the reliability of establishing artificial features. When it is detected that the co-viewing degree of the variables in the sliding window is very high, that is, when the artificial features are identified and updated to the map during the mapping process, after marginalization, the chou-liu-tree and the covariance matrix between the variables are used to find the maximum spanning tree, thereby reducing the redundancy of the relationship between the variables and sparse the Hessian matrix to improve computational efficiency.

[0135] See also Fig.14 , Fig.14 It is a structural diagram of an embodiment of the positioning device of the present application. The positioning device 40 can execute the above positioning method. The positioning device 40 includes an initial posture determination module 41, a constraint factor construction module 42 and a posture optimization module 43; the initial posture determination module 41 is used to determine the initial posture of the target object at the current moment based on the sensor data collected by the sensor in the target object between the previous moment and the current moment and the posture of the target object at the previous moment; the constraint factor construction module 42 is used to construct at least one set of constraint factors using the target data between the current moment and at least two historical moments respectively, and the at least two historical moments include the previous moment of the current moment and the adjacent historical moments of the previous moment, and the target data includes a relative position relationship; the posture optimization module 43 is used to optimize the initial posture using at least one set of constraint factors to obtain the target posture of the target object at the current moment.

[0136] The above scheme, after obtaining the initial posture of the target object at the current moment, can construct constraint factors according to the relative position relationship between the current moment and the previous moment, as well as the position relationship between the current moment and the adjacent historical moments of the previous moment, to obtain at least one set of constraint factors, thereby optimizing the initial posture according to at least one set of constraint factors, which can reduce the impact of the accumulated error in the positioning process on the positioning accuracy, thereby improving the positioning accuracy.

[0137] The functions of each module can be found in the positioning method embodiment and will not be described in detail here.

[0138] See also Fig.15 , Fig.15 : is a schematic diagram of the structure of an embodiment of an electronic device of the present application. The electronic device 50 includes a memory 51 and a processor 52, and the processor 52 is used to execute program instructions stored in the memory 51 to implement the steps in any of the above positioning method embodiments. In a specific implementation scenario, the electronic device 50 may include but is not limited to: a mobile robot, a vehicle, a microcomputer, a server, and in addition, the electronic device 50 may also include a carrier device such as a laptop computer and a tablet computer, which is not limited here.

[0139] The electronic device may be the target object mentioned above or a device that establishes a communication connection with the target object.

[0140] Specifically, the processor 52 is used to control itself and the memory 51 to implement the steps in any of the above positioning method embodiments. The processor 52 can also be called a CPU (Central Processing Unit). The processor 52 may be an integrated circuit chip with signal processing capabilities. The processor 52 can also be a general-purpose processor, a digital signal processor (Digital Signal Processor, DSP), an application-specific integrated circuit (Application Specific Integrated Circuit, ASIC), a field-programmable gate array (Field-Programmable Gate Array, FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc. In addition, the processor 52 can be implemented by an integrated circuit chip.

[0141] The above scheme, after obtaining the initial posture of the target object at the current moment, can construct constraint factors according to the relative position relationship between the current moment and the previous moment, as well as the position relationship between the current moment and the adjacent historical moments of the previous moment, to obtain at least one set of constraint factors, thereby optimizing the initial posture according to at least one set of constraint factors, which can reduce the impact of the accumulated error in the positioning process on the positioning accuracy, thereby improving the positioning accuracy.

[0142] See also Fig.16 , Fig.16 The computer-readable storage medium 60 stores program instructions 61, which implement the steps in any of the above positioning method embodiments when executed by a processor.

[0143] The above scheme, after obtaining the initial posture of the target object at the current moment, can construct constraint factors according to the relative position relationship between the current moment and the previous moment, as well as the position relationship between the current moment and the adjacent historical moments of the previous moment, to obtain at least one set of constraint factors, thereby optimizing the initial posture according to at least one set of constraint factors, which can reduce the impact of the accumulated error in the positioning process on the positioning accuracy, thereby improving the positioning accuracy.

[0144] In some embodiments, the functions or modules included in the device provided by the embodiments of the present disclosure can be used to execute the method described in the above method embodiments. The specific implementation can refer to the description of the above method embodiments, and for the sake of brevity, it will not be repeated here.

[0145] The above description of various embodiments tends to emphasize the differences between the various embodiments. The same or similar aspects can be referenced to each other, and for the sake of brevity, they will not be repeated herein.

[0146] In the several embodiments provided in the present application, it should be understood that the disclosed methods and devices can be implemented in other ways. For example, the device implementation described above is only schematic. For example, the division of modules or units is only a logical function division. There may be other division methods in actual implementation. For example, units or components can be combined or integrated into another system, or some features can be ignored or not executed. In another image position, the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, and the indirect coupling or communication connection of devices or units can be electrical, mechanical or other forms.

[0147] In addition, each functional unit in each embodiment of the present application can be integrated into a processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of a software functional unit. If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor (processor) to perform all or part of the steps of each implementation method of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program code.

Claims

1. A positioning method, characterized in that: include: Determining an initial position and posture of the target object at a current moment based on sensor data collected by a sensor in the target object between a previous moment and a current moment and a position and posture of the target object at a previous moment; constructing at least one set of constraint factors by respectively using target data between the current moment and at least two historical moments, wherein the at least two historical moments include a moment before the current moment and an adjacent historical moment before the moment, and the target data include a relative position relationship; The initial pose is optimized using the at least one set of constraint factors to obtain a target pose of the target object at a current moment.

2. The method according to claim 1, characterized in that The at least one group of constraint factors includes a first pre-integration constraint factor and a second pre-integration constraint factor, the target data also includes a zero bias predicted for the sensor at each moment, the relative position relationship includes a first type of relative position relationship determined using the sensor data, and the at least one group of constraint factors is constructed by using the target data between the current moment and at least two historical moments respectively, including: Determining a first pre-integration constraint factor based on a first type of positional relationship between the current moment and the previous moment, an initial position and posture of the target object at the current moment, a historical position and posture of the target object at the previous moment, and a zero bias predicted by a sensor in the target object at the current moment and at the previous moment; and, Based on the first type of relative position relationship between the current moment and the adjacent historical moment of the previous moment, the initial posture of the target object at the current moment, the historical posture of the target object at the adjacent historical moment of the previous moment, and the zero bias predicted by the sensor in the target object at the current moment and at the adjacent historical moment of the previous moment, a second pre-integration constraint factor is determined.

3. The method according to claim 1, characterized in that The target object includes a laser radar, and the laser radar is used to collect radar point cloud data. The at least one set of constraint factors includes a first laser odometer constraint factor and a second laser odometer constraint factor. The relative position relationship includes a second type of relative position relationship determined by using radar point cloud data collected by the laser radar at each time. The method also includes: Matching the radar point cloud data collected at each moment with a preset map respectively, to obtain a second type of relative position relationship between the target object at a current moment and a previous moment, and a second type of relative position relationship between the target object at adjacent historical moments between the current moment and the previous moment, wherein the preset map is constructed using the radar point cloud data collected by the target object at historical moments; The step of constructing at least one set of constraint factors by respectively utilizing the target data between the current moment and at least two historical moments includes: The first laser odometer constraint factor is determined based on the initial posture, the posture of the target object at the previous moment, and the second type of relative position relationship between the current moment and the previous moment; and the second laser odometer constraint factor is determined based on the initial posture, the posture of the target object at the adjacent historical moment of the previous moment, and the second type of relative position relationship between the current moment and the adjacent historical moment of the previous moment.

4. The method according to any one of claims 1 to 3, characterized in that The target object includes a laser radar, and the laser radar is used to collect radar point cloud data. The method also includes: Matching the radar point cloud data collected by the laser radar in the target object at the current moment with the prior map to obtain a candidate pose of the target object at the current moment; Based on the initial pose and the candidate pose, construct a global map matching constraint factor; The step of optimizing the initial posture by using the at least one set of constraint factors to obtain the target posture of the target object at the current moment includes: The initial pose is optimized in combination with the global map matching constraint factor and the at least one group of constraint factors to obtain a target pose of the target object at the current moment.

5. The method according to claim 4, characterized in that The step of optimizing the initial pose by combining the global map matching constraint factor and the at least one group of constraint factors to obtain the target pose of the target object at the current moment includes: Constructing the incremental equations corresponding to the constraint factors respectively; Adding the Hessian matrices corresponding to the incremental equations to obtain a target Hessian matrix; The initial pose is optimized using the target Hessian matrix to obtain the target pose.

6. The method according to any one of claims 1 to 3, characterized in that: The method further comprises: Obtain multiple sets of constraint factors constructed at multiple historical moments; In response to the total number of moments of the multiple historical moments and the current moment being greater than a preset number of moments, marginalizing the multiple groups of constraint factors constructed by the multiple moments to obtain a plurality of groups of constraint factors after marginalization; The step of optimizing the initial posture by using the at least one set of constraint factors to obtain the target posture of the target object at the current moment includes: The initial posture is optimized by combining the at least one set of constraint factors constructed at the current moment and the plurality of sets of constraint factors.

7. The method according to claim 6, characterized in that The marginalizing the multiple groups of constraint factors constructed at the multiple moments to obtain the marginalized groups of constraint factors includes: Obtaining at least one first constraint factor that is unrelated to a target historical moment among the multiple sets of constraint factors and at least one second constraint factor that is related to the target historical moment among the multiple sets of constraint factors, wherein the target historical moment is the oldest historical moment among the multiple historical moments; Screening the at least one second constraint factor to obtain a plurality of remaining second constraint factors; The at least one first constraint factor and the plurality of second constraint factors are combined to obtain a plurality of marginalized groups of constraint factors.

8. The method according to claim 6, characterized in that The target object includes a laser radar, and the laser radar is used to collect radar point cloud data. The method also includes: In response to the presence of a point cloud corresponding to a preset positioning object in the environment in the radar point cloud data collected by the laser radar at the current moment, obtaining multiple groups of constraint factors constructed at a number of historical moments, the number of the several moments being greater than the number of the multiple historical moments; Constructing a constraint factor group based on the multiple groups of constraint factors constructed at the several historical moments and the at least one group of constraint factors constructed at the current moment; The step of optimizing the initial posture by using the at least one set of constraint factors to obtain the target posture of the target object at the current moment includes: The initial pose is optimized by combining the constraint factors in the constraint factor group to obtain the target pose of the target object at the current moment.

9. The method according to claim 8, characterized in that The step of combining the constraint factors in the constraint factor group to optimize the initial pose to obtain the target pose of the target object at the current moment includes: Sparsely transform each constraint factor in the constraint factor group to obtain a target constraint factor group, wherein the target constraint factor group includes a plurality of constraint factors; The initial pose is optimized by combining the constraint factors in the target constraint factor group to obtain the target pose of the target object at the current moment.

10. The method according to any one of claims 1 to 3, characterized in that: The target object includes a laser radar, and the laser radar is used to collect radar point cloud data. The method also includes: Projecting the radar point cloud data collected by the laser radar onto a preset map based on the target posture, wherein the preset map is constructed from the radar point cloud data collected by the target object at a historical moment; The preset map is updated by using radar point cloud data for which no corresponding point cloud exists in the preset map.

11. An electronic device, characterized in that: The invention comprises a memory and a processor, wherein the processor is used to execute program instructions stored in the memory to implement the method according to any one of claims 1 to 10.

12. A computer-readable storage medium having program instructions stored thereon, characterized in that: When the program instructions are executed by a processor, the method according to any one of claims 1 to 10 is implemented.

Images