A real-time pose estimation system for small ground vehicles

Abstract

The present invention relates to a real-time posture estimation system for a small ground carrier, comprising: a current carrier posture result processing module, a current moment environment grid map publishing module, a carrier posture history matrix updating module, a carrier motion parameter estimation module, a next moment carrier posture prediction module and a laser SLAM scanning callback function. The present invention greatly improves the working performance of the prior art under the high dynamic maneuvering state of the carrier without introducing a large amount of calculation.

Description

Technical Field

[0001] The invention relates to a real-time position and posture estimation system for a small ground carrier, and belongs to the field of navigation and positioning. Background Art

[0002] For laser SLAM (Simultaneous Location and Mapping) navigation and positioning, the current mainstream algorithms have three main theoretical support directions: filter-based, optimization-based, and graph-based. In 2011, Stefan and Johannes proposed the Hector SLAM algorithm based on optimization theory. Different from the filter-based Gmapping algorithm and the graph-based Cartographer SLAM algorithm, the laser SLAM algorithm based on optimization theory can work independently with the laser radar and is more conducive to mapping large unknown scenes.

[0003] The flowchart of the existing laser SLAM algorithm based on optimization is an iterative working principle. When the entire system starts working, the information and posture information of the historical map library are determined by the initial laser radar matching data and the fixed or randomly given carrier posture information. Note that the historical transformation matrix and the historical map library only refer to the map information and carrier posture information released in the previous cycle. After the laser radar scanning data is obtained at the current moment, it is preprocessed first, including filtering of invalid point information (filtering of data less than the minimum distance detected by the laser radar and greater than the maximum distance), conversion of information coordinate system (from the laser radar coordinate system to the body coordinate system, the specific coordinate system information will be defined in the algorithm improvement part), and updating and publishing of the laser point cloud (processed) database.

[0004] After obtaining the point cloud information at the current moment, the occupancy probability of a point on the map and its partial derivative with respect to the data coordinates are expressed by quadratic interpolation or cubic interpolation (because the pose information optimization solution derived by the Gauss-Newton method in the scan matching part requires the gradient of the point occupancy probability, and non-continuous data is replaced by interpolation). Then, the following objective optimization function is used to iterate the carrier pose update, where: and Represent the pose variables of the carrier at the previous moment and the current moment respectively, Representative use The coordinates of the i-th pixel point in the grid map represented by Represents the occupancy probability of the i-th pixel, and argmin represents the minimum value. That is, the occupancy probability of a pixel in the grid map is represented by the pose variable of the carrier at the previous moment, and the optimal pose change is obtained by iteratively maximizing the probability occupancy.

[0005]

[0006] After that, the pose information of the carrier at the previous moment is used for updating, and the grid map is published based on the new pose matrix. The existing Hector SLAM algorithm can work stably when the carrier is moving at a low speed, but when the carrier is performing high-dynamic maneuvers, serious overlapping of images will occur, resulting in a large deviation in pose estimation, and the practical performance will drop rapidly or even become unusable. Summary of the invention

[0007] The technology of the present invention solves the problem that when the carrier performs high-dynamic maneuvers (experimental conditions: 1.5rad / s rotation angular velocity for turning, 0.3m / s for straight driving), the existing grid map built based on the optimized laser SLAM algorithm will have serious overlapping phenomenon, resulting in large deviations in the carrier posture estimation, making the SLAM algorithm unusable. The present invention overcomes the above-mentioned problem of failure of carrier high-dynamic motion in the prior art, and provides a real-time posture estimation system for small ground carriers, which greatly improves the working performance of the prior art in the high-dynamic maneuvering state of the carrier without introducing a large amount of calculation.

[0008] One of the technical solutions of the present invention:

[0009] A real-time posture estimation system for a small ground carrier comprises: a current carrier posture result processing module (1), a current moment environment grid map publishing module (2), a carrier posture history matrix updating module (3), a carrier motion parameter estimation module (4), a next moment carrier posture prediction module (5) and a laser SLAM scanning callback function (6);

[0010] The current carrier position result processing module (1) outputs and displays the obtained carrier position information at the current moment, and provides it to the current moment grid map publishing module (2) and the carrier position history matrix updating module (3); when initialized, the carrier position information at the current moment is given by an external GNSS (Global Navigation Satellite System) system or the position is initialized by other methods; when not initialized, the carrier position information at the current moment is given by the laser SLAM scanning callback function (6);

[0011] The current moment environment grid map publishing module (2) uses the first frame point cloud data of the laser radar and the current moment carrier posture information to publish the initialization environment grid to the map laser SLAM scanning callback function (6) when initialized; when not initialized, uses the point cloud data of the laser radar and the current moment carrier posture information to publish the current moment environment grid map to the laser SLAM scanning callback function (6);

[0012] The carrier posture history matrix updating module (3) updates the carrier posture history matrix data according to the carrier posture information at the current moment by using a fixed weight or a degenerate weight method, and estimates the carrier posture result processing iteration cycle time for processing by the carrier posture prediction module (5) at the next moment; when the number of carrier posture data in the carrier posture history matrix reaches the set matrix capacity, the carrier posture history matrix is ​​output to the carrier motion parameter estimation module (4); the matrix capacity is set to ten or twenty iteration cycles;

[0013] The carrier motion parameter estimation module (4) estimates the motion parameters of the carrier based on the carrier posture history matrix and the accurate modeling of the carrier motion model offline, using the least square method or Kalman filtering, including the determination of the running state, i.e., stationary, left turn, right turn, and straight travel, and the estimation of the motion speed variables, i.e., the rotation radius, the rotation angular velocity, and the straight travel velocity, and outputs the motion parameter estimation result to the carrier posture prediction module (5) at the next moment;

[0014] The next moment carrier posture prediction module (5) performs integral prediction of the data iteration cycle time length using the carrier motion speed variable according to the data iteration cycle and the motion parameter estimation result, combined with the carrier motion model, and then obtains the next moment carrier posture prediction result, and outputs the next moment carrier posture prediction result to the laser SLAM scanning callback function (6);

[0015] The laser SLAM scanning callback function (6) waits for the laser radar to output the point cloud result at the next moment for the predicted result of the carrier's posture at the next moment; when the point cloud result at the next moment is output, the grid map released at the current moment is used for inter-frame matching. The result of the inter-frame matching is the estimated value of the carrier's posture at the next moment. After the result is obtained, it is output to the carrier posture processing module (1) at the current moment to complete the closed loop.

[0016] Through the iterative cycle of (1) to (6), the entire carrier pose estimation system is based on the carrier pose prediction result at the next moment and the environmental grid map at the current moment, and uses the lidar point cloud output result at the next moment for inter-frame matching. Through iterative updating, the real-time pose estimation of the carrier is output.

[0017] The second technical solution of the present invention:

[0018] A real-time posture estimation system for a small ground carrier comprises: a current carrier posture result processing module (1), a current moment environment grid map publishing module (2), a carrier posture history matrix updating module (3), a carrier motion parameter estimation module (4), a next moment carrier posture prediction module (5) and a laser SLAM scanning callback function (6);

[0019] The current carrier position and posture result processing module (1) outputs and displays the obtained carrier position and posture information at the current moment, and provides it to the current moment grid map publishing module (2) and the carrier position and posture history matrix updating module (3); when initialized, the carrier position and posture information at the current moment is given by the external GNSS system or other methods to initialize the posture; when not initialized, the carrier position and posture information at the current moment is given by the laser SLAM scanning callback function (6);

[0020] The current moment environment grid map publishing module (2) uses the first frame point cloud data of the laser radar and the current moment carrier posture information to publish the initialization environment grid to the map laser SLAM scanning callback function (6) when initialized; when not initialized, uses the point cloud data of the laser radar and the current moment carrier posture information to publish the current moment environment grid map to the laser SLAM scanning callback function (6);

[0021] The carrier posture history matrix updating module (3) smoothes the current posture according to the carrier posture information at the current moment by using the odometer and IMU (Inertial Measurement Unit) inertial system information fusion result, stores the smoothed result in the carrier posture history matrix, and estimates the carrier posture result processing iteration cycle time for the next moment carrier posture prediction module (5) to process; when the number of carrier posture data in the carrier posture history matrix reaches the set matrix capacity, the carrier posture history matrix is ​​output to the carrier motion parameter estimation module (4); the set matrix capacity is ten or twenty iteration cycles;

[0022] The carrier motion parameter estimation module (4) estimates the carrier motion parameters by the least square method or Kalman filtering based on the carrier position and posture history matrix and the accurate modeling of the carrier motion model offline, including the determination of the running state, i.e., stationary, left turn, right turn, and straight travel, and the estimation of the motion speed variables, i.e., the rotation radius, the rotation angular velocity, and the straight travel velocity; then, the motion parameter estimation result is weighted with the estimation result of the inertial information fusion to reduce the mapping and positioning deviation caused by the vibration of the ground carrier and the non-uniform acceleration start or braking; finally, the motion parameter estimation result is output to the carrier position and posture prediction module (5) at the next moment;

[0023] The next moment carrier posture prediction module (5) performs integral prediction of the data iteration cycle time length using the carrier motion speed variable according to the data iteration cycle and the motion parameter estimation result, combined with the carrier motion model, and then obtains the next moment carrier posture prediction result, and outputs the next moment carrier posture prediction result to the laser SLAM scanning callback function (6);

[0024] The laser SLAM scanning callback function (6) waits for the laser radar to output the point cloud result at the next moment for the predicted result of the carrier's posture at the next moment; when the point cloud result at the next moment is output, the grid map released at the current moment is used for inter-frame matching. The result of the inter-frame matching is the estimated value of the carrier's posture at the next moment. After the result is obtained, it is output to the carrier posture processing module (1) at the current moment to complete the closed loop.

[0025] According to the above steps, through the iteration cycle of (1) to (6), the entire carrier pose estimation method is based on the carrier pose prediction result at the next moment and the environment grid map at the current moment, and uses the laser radar point cloud output result at the next moment for inter-frame matching. Through iterative updating, the real-time pose estimation of the carrier is output.

[0026] The advantages of the present invention compared with the prior art are:

[0027] (1) Introduce the pose prediction link in the existing laser SLAM real-time pose estimation system, realize the carrier motion state judgment and pose prediction by accurately modeling the carrier motion model, and use the carrier's historical pose estimation information to predict the current pose. The current pose prediction value replaces the previous pose estimation value to enter the laser SLAM frame matching link, obtain the current pose estimation value, and after publishing the grid map, continue to predict the carrier pose at the next moment, and realize iterative update based on the prediction value. Figure 1 The existing laser SLAM algorithm only includes three parts: the current carrier posture result processing module (1), the current moment environment grid map publishing module (2) and the laser SLAM scanning callback function (6). The real-time posture estimation system designed by the present invention introduces a carrier posture history matrix update module (3), a carrier motion parameter estimation module (4) and a next moment carrier posture prediction module (5). In practical applications, the working performance of the algorithm under the high dynamic maneuvering state of the carrier can be greatly improved without introducing a large amount of calculation.

[0028] (2) Technical Solution 2 adds an inertial auxiliary part compared to Technical Solution 1, namely, a wheel speed odometer and a MEMS (Micro-Electro-Mechanical System) inertial sensor. When a small ground vehicle is starting, braking, or the ground slips, the vehicle will vibrate at a low frequency. At the same time, the rapid acceleration change during starting and braking will make the vehicle posture estimation result inaccurate. In order to solve this problem, the present invention proposes Technical Solution 2. Figure 2 As shown, compared to Figure 1 Solution 1 shown in the figure adds an inertial auxiliary part, which can use the high-data-rate inertial data to smooth the low-data-rate laser SLAM frame matching function output, thereby improving the estimation accuracy of the carrier's position at the current moment and reducing the impact of carrier acceleration, deceleration and low-frequency vibration. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is a block diagram of the technical solution 1 of the present invention;

[0030] Figure 2 This is a block diagram of the second technical solution of the present invention;

[0031] Figure 3 Comparison of environmental grid map construction between the existing laser SLAM real-time pose estimation system under high-speed rotation of the carrier and the real-time pose estimation system of the present invention;

[0032] Figure 4 Comparison of the environment grid map construction between the existing laser SLAM real-time pose estimation system under combined maneuvering of carriers and the real-time pose estimation system of the present invention;

[0033] Figure 5 It is a partial flow chart of a carrier motion parameter estimation module (4) and a next moment carrier posture prediction module (5) for two-wheel and four-wheel carrier models;

[0034] Figure 6 It is a partial flow chart of a carrier motion parameter estimation module (4) and a next moment carrier posture prediction module (5) of an omnidirectional wheel carrier model. DETAILED DESCRIPTION

[0035] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0036] like Figure 1 As shown, a real-time posture estimation system embodiment 1 for a small ground carrier of the present invention comprises: a current carrier posture result processing module 1, a current moment environment grid map publishing module 2, a carrier posture history matrix updating module 3, a carrier motion parameter estimation module 4, a next moment carrier posture prediction module 5 and a laser SLAM scanning callback function 6;

[0037] The current carrier posture result processing module 1 outputs and displays the obtained carrier posture information at the current moment, and provides it to the current moment grid map publishing module 2 and the carrier posture history matrix updating module 3; when initialized, the current moment carrier posture information is given by the external GNSS system or other methods to initialize the posture; when not initialized, the current moment carrier posture information is given by the laser SLAM scanning callback function 6;

[0038] The current moment environment grid map publishing module 2, when initialized, uses the first frame point cloud data of the laser radar and the current moment carrier posture information to publish the initialized environment grid to the map laser SLAM scanning callback function 6; when not initialized, uses the point cloud data of the laser radar at the current moment and the current moment carrier posture information to publish the current moment environment grid map to the laser SLAM scanning callback function 6; the publishing process of the environment grid map: first, the radar system point cloud data obtained by the laser radar is filtered, and the point cloud data exceeding the scanning radius of the laser radar instrument is discarded and no longer used; the second step is to convert the radar system point cloud information after data filtering to this system, and the conversion relationship is determined by the installation angle of the laser radar on the carrier; the third step is to convert the point cloud information of this system to the environment map coordinate system, and the conversion relationship is determined by the current moment carrier posture information; finally, the point cloud data in the environment map coordinate system is used to determine the occupancy probability of each point in the environment map coordinate system and publish it. A high occupancy probability indicates that the wall or object is blocked and cannot be passed, and a low occupancy probability indicates that the point can be passed.

[0039] The carrier posture history matrix update module 3 updates the carrier posture history matrix according to the carrier posture information at the current moment by using a fixed weight or degenerate weight method, and estimates the iterative cycle time of the carrier posture result processing for the next moment carrier posture prediction module 5 to process; when the number of carrier posture data in the carrier posture history matrix reaches the set matrix capacity, the carrier posture history matrix is ​​output to the carrier motion parameter estimation module 4; the matrix capacity is set to ten or twenty iteration cycles;

[0040] The carrier motion parameter estimation module 4 estimates the motion parameters of the carrier by the least square method or Kalman filtering based on the carrier posture history matrix and the accurate modeling of the carrier motion model offline, including the determination of the running state, i.e., stationary, left turn, right turn, and straight travel, and the estimation of the motion speed variables, i.e., the rotation radius, the rotation angular velocity, and the straight travel velocity, and outputs the motion parameter estimation result to the carrier posture prediction module 5 at the next moment;

[0041] The next moment carrier posture prediction module 5, based on the data iteration cycle and the motion parameter estimation result, combined with the carrier motion model, uses the carrier motion speed variable to perform an integral prediction of the data iteration cycle time length, and then obtains the next moment carrier posture prediction result, and outputs the next moment carrier posture prediction result to the laser SLAM scanning callback function 6;

[0042] The carrier motion model in the carrier motion parameter estimation module 4 and the next moment carrier posture prediction module 5 is specifically implemented as follows: Figure 5 and Figure 6 shown.

[0043] The carrier motion models include two-wheel, four-wheel, and omnidirectional wheel models. The turning and displacement motions of the two-wheel and four-wheel carrier motion models are coupled, while the turning and displacement motions of the omnidirectional wheel model are decoupled. Therefore, Figure 5 is the implementation process of the two-wheel and four-wheel carrier motion models, while Figure 6 is the implementation process of the omnidirectional wheel carrier model.

[0044] The specific implementation process is introduced as follows:

[0045] (A) Two-wheel and four-wheel models

[0046] When the carrier model is a two-wheel or four-wheel model, as Figure 5 shown. Obtain the historical matrix input by the carrier pose historical matrix update module and the iteration period dt. The carrier pose historical matrix is divided into a matrix X representing the carrier position and a matrix YAW representing the carrier attitude angle. Matrix X contains two variables x and y, representing the x and y coordinates of the carrier position respectively. The matrix capacity is N, which can be taken as 10 or larger in actual use.

[0047] After that, perform a turning judgment. If abs(YAW[N] - YAW[0]) < thre_yaw, the carrier has not turned and enters the displacement judgment; otherwise, the carrier is in a turning state and enters the right-turn judgment. The turning judgment threshold thre_yaw is generally taken as 0.01 rad, approximately 1.145°.

[0048] Displacement judgment. If ||X[N] - X[0]||2 < thre_X, the carrier is stationary. The displacement judgment threshold thre_X is generally taken as 0.05. The predicted result of the carrier pose at the next moment is still the carrier pose information at the current moment, that is Otherwise, the carrier moves. First, estimate the carrier displacement speed by least squares, and use equal weights with the formula Degenerate weights use the formula where is the carrier speed information to be calculated, z is the carrier pose information at each moment in the carrier pose historical matrix, H is the integral relationship matrix between the carrier speed and the carrier pose information at each moment, and W is the weight value of the degenerate weight method of the carrier pose historical matrix update module 3. Generally, the carrier pose information at a moment closer to the current moment has a greater weight. Then, use the following formula for carrier pose prediction:

[0049] Position prediction: k = 1, 2…N - 1, N

[0050]

[0051] Attitude angle prediction:

[0052] Right turn judgment, for two-wheel or four-wheel models, when the carrier's rotational motion is coupled with the displacement motion, the carrier's attitude angle can be used to construct the carrier's position information relationship at different times as follows, where is the carrier position information at the kth moment in the carrier posture history matrix, X r0 is the coordinate of the right rotation point of the carrier in the carrier system, ψ k , Δψ k They are respectively the carrier attitude angle at the kth moment and the change in the carrier attitude angle from the kth moment to the Nth moment. is the predicted value of the position of the carrier at the Nth moment. If N = 10, 9 groups can be constructed based on X[k] prediction combinations of, where k = 1, 2…, N-1.

[0053]

[0054]

[0055] ψ k =YAW[k],Δψ k =YAW[N]-YAW[k],k=1,2…N-2,N-1

[0056]

[0057] Finally, the predicted value of the carrier position at the Nth moment is If the carrier pose history matrix update module (3) uses the degradation weight to update the carrier pose history matrix, then Where W i , W k are the weight values ​​of the carrier posture information at the i-th and k-th moments respectively. When the carrier is in the right turn state, otherwise it is in the left turn state. The right turn judgment threshold thre_X is generally taken as 0.05.

[0058] After obtaining the left or right turn state, the carrier rotation angular velocity is estimated by the least squares method Then the following prediction is performed, that is, the historical posture of the carrier from the 1st to the Nth moment is used to predict the posture information of the carrier at the next moment. In order to use the k-th moment pose information to find the predicted position value of the carrier at the next moment, the predicted attitude angle value is X z0 is the coordinate of the carrier rotation point in the carrier system, right turn state X z0 =X r0 That is, the coordinates of the right rotation point of the carrier in the carrier system, the left rotation state X z0 =X l0 That is, the coordinates of the carrier's left rotation point in the carrier's own system.

[0059]

[0060]

[0061]

[0062]

[0063] Similar to the carrier right-turn judgment, the pose prediction result of the carrier at the next moment finally takes the value: the non-degenerate weight position prediction is The pose prediction is The degenerate weight position prediction is The pose prediction is where W i 、W k are the weight values of the pose information of the carrier at the i-th and k-th moments respectively. The obtained pose prediction result of the carrier at the next moment is output to the laser SLAM scan callback function (6).

[0064] (B) Omnidirectional wheel model

[0065] When the carrier model is an omnidirectional wheel model, as Figure 6 shown. Obtain the historical matrix input by the carrier pose historical matrix update module and the iteration period dt. The carrier pose historical matrix is divided into a matrix X representing the carrier position and a matrix YAW representing the carrier attitude angle. Matrix X contains two variables x and y, representing the x and y coordinates of the carrier position respectively, The matrix capacity size is N, which can be taken as 10 or larger in actual use.

[0066] Since the turning and displacement motions of the carrier with an omnidirectional wheel model are decoupled, the motion parameter estimation and the pose prediction algorithm of the carrier at the next moment are relatively simple and can be decomposed into three independent branches: displacement judgment in the x direction, displacement judgment in the y direction, and turning judgment.

[0067] The judgment conditions for the three branches are abs(x[k] - x[0]) < thre_x, abs(y[k] - y[0]) < thre_y. The displacement judgment thresholds thre_x and thre_y are 0.05 m, and abs(YAW[N] - YAW[0]) < thre_yaw. The rotation judgment threshold thre_yaw is 0.01 rad. After the three branches are judged to be valid, the displacement speed or rotation angular velocity is estimated using the least squares method. After that, the pose prediction process of the carrier at the next moment can be expressed by the following formula, that is, in the flow chart where x[k] and y[k] come from the carrier position coordinates at the k-th moment in matrix X, that is k ranges from 1 to N, that is, the carrier position coordinates at the 1st to Nth moments are used to predict the carrier position coordinates at the next moment, and the prediction results are averaged or weighted averaged to obtain the final result. The prediction of the attitude angle is similar to the position coordinates and will not be repeated.

[0068] Position prediction: k = 1,2…N-1,N;

[0069]

[0070]

[0071] Posture prediction: k = 1,2…N-1,N

[0072]

[0073] Finally, similar to part (A), we have That is, each X position includes two coordinates, x and y. The predicted result of the carrier position at the next moment is finally obtained and enters the laser SLAM scanning callback function 6 for the next step of processing.

[0074] The laser SLAM scanning callback function 6, for the prediction result of the carrier posture at the next moment, waits for the output of the point cloud result of the laser radar at the next moment; when the point cloud result is output at the next moment, the grid map released at the current moment is used for inter-frame matching. The result of the inter-frame matching is the estimated value of the carrier posture at the next moment. After obtaining the result, it is output to the carrier posture processing module 1 at the current moment to complete the closed loop. Different from the original laser SLAM system, the posture optimization formula at this time is changed to the following formula, where Represents the predicted result of the carrier position at the next moment, i.e. Representative use The coordinates of the i-th pixel point in the grid map represented by Represents the occupancy probability of the i-th pixel, and the optimal iterative change of the pose is obtained by maximizing the probability occupancy.

[0075]

[0076] The final pose change This makes the iteration speed of the optimization formula faster, or the iteration accuracy higher under limited resources, so the system of the present invention has better working performance than the original laser SLAM system when the carrier performs high-dynamic maneuvers.

[0077] Through the cycle of 1 to 6 iterations, the entire carrier pose estimation system is based on the carrier pose prediction result at the next moment and the environmental grid map at the current moment, and uses the lidar point cloud output result at the next moment for inter-frame matching. Through iterative updating, it outputs the real-time pose estimation of the carrier.

[0078] like Figure 2 As shown, a real-time posture estimation system embodiment 2 for a small ground carrier of the present invention comprises: a current carrier posture result processing module 1, a current moment environment grid map publishing module 2, a carrier posture history matrix updating module 3, a carrier motion parameter estimation module 4, a next moment carrier posture prediction module 5 and a laser SLAM scanning callback function 6;

[0079] The current carrier posture result processing module 1 outputs and displays the obtained carrier posture information at the current moment, and provides it to the current moment grid map publishing module 2 and the carrier posture history matrix updating module 3; when initialized, the current moment carrier posture information is given by the external GNSS system or other methods to initialize the posture; when not initialized, the current moment carrier posture information is given by the laser SLAM scanning callback function 6;

[0080] The current moment environment grid map publishing module 2, when initialized, uses the first frame point cloud data of the laser radar and the current moment carrier posture information to publish the initialization environment grid to the map laser SLAM scanning callback function 6; when not initialized, uses the current moment laser radar point cloud data and the current moment carrier posture information to publish the current moment environment grid map to the laser SLAM scanning callback function 6;

[0081] The carrier posture history matrix update module 3 smoothes the current posture according to the carrier posture information at the current moment by using the fusion result of the odometer and the IMU inertial system information, and stores the smoothed result in the carrier posture history matrix. At the same time, the carrier posture result processing iteration cycle time is estimated for the next moment carrier posture prediction module 5 to process; when the number of carrier posture data in the carrier posture history matrix reaches the set matrix capacity, the carrier posture history matrix is ​​output to the carrier motion parameter estimation module 4; the set matrix capacity is ten or twenty iteration cycles;

[0082] The carrier motion parameter estimation module 4 estimates the motion parameters of the carrier through the least square method or Kalman filtering based on the carrier posture history matrix and the accurate modeling of the carrier motion model offline, including the identification of the running state, i.e., stationary, left turn, right turn, and straight travel, and the estimation of the motion speed variables, i.e., the rotation radius, the rotation angular velocity, and the straight travel velocity; then, the motion parameter estimation result is weighted with the estimation result of the inertial information fusion to reduce the mapping and positioning deviation caused by the vibration of the ground carrier and the non-uniform acceleration start or braking, and the weight of the inertial assistance can; finally, the motion parameter estimation result is output to the carrier posture prediction module 5 at the next moment;

[0083] The next moment carrier posture prediction module 5, based on the data iteration cycle and the motion parameter estimation result, combined with the carrier motion model, uses the carrier motion speed variable to perform an integral prediction of the data iteration cycle time length, and then obtains the next moment carrier posture prediction result, and outputs the next moment carrier posture prediction result to the laser SLAM scanning callback function 6;

[0084] The laser SLAM scanning callback function 6 waits for the laser radar to output the point cloud result at the next moment for the carrier posture prediction result at the next moment; when the point cloud result at the next moment is output, the grid map released at the current moment is used for inter-frame matching. The result of the inter-frame matching is the carrier posture estimation value at the next moment. After obtaining the result, it is output to the carrier posture processing module 1 at the current moment to complete the closed loop.

[0085] According to the above steps, through the cycle of 1 to 6 iterations, the entire carrier pose estimation method is based on the carrier pose prediction result at the next moment and the environment grid map at the current moment, and uses the laser radar point cloud output result at the next moment for frame matching. Through iterative updates, the real-time pose estimation of the carrier is output.

[0086] The other parts of the above-mentioned embodiment 2 are the same as those of embodiment 1.

[0087] The small ground carrier of the present invention is defined as:

[0088] Experimental vector parameters:

[0089] The actual experimental carrier is a McNay mother wheel trolley chassis, which can turn in place and make a U-turn.

[0090] Data support:

[0091] Since high-speed turning has a more obvious effect on testing the algorithm's pose estimation technology than straight driving, the mapping effects of the existing algorithm and the algorithm of the present invention at the same rotation speed are given here (since the grid map is published based on the carrier pose estimation result at the current moment, it can most intuitively reflect the carrier pose estimation deviation in the local environment).

[0092] like Figure 3 As shown, the carrier rotates at an angular rate of 1.5 rad / s, or about 86° / s, and rotates 360° in situ. The normal environment grid map construction effect should be that the carrier position is estimated to be the same before and after the rotation, and the grid map published at the same time has no rotation overlay. Figure 3 (b) in the figure is the mapping effect of the real-time pose estimation system of the present invention, and it can be seen that it basically meets the requirements of normal mapping effect; Figure 3(a) The mapping effect of the existing laser SLAM real-time pose estimation system does not meet the requirements. First, the grid map created has obvious rotation overlap. Secondly, the solid line in the center of the figure is the moving route formed by the pose estimation result of the carrier at the current moment during the rotation process. It is obviously no longer at one point, but has drifted a long distance. From the above comparison, it can be seen that the real-time pose estimation system proposed by the present invention has greatly improved working performance when the carrier rotates at high speed compared with the existing laser SLAM real-time pose estimation system.

[0093] like Figure 4 As shown, the carrier combination maneuvers (rotation angular rate 1.5 rad / s, i.e., about 86° / s, straight-line speed 0.3 m / s). The carrier maneuver path starts from the same starting point of the rotation test, makes an "S" shape maneuver to the balcony, turns around, makes an "S" shape maneuver to the door, and turns around to return to the starting point. Figure 4 From (a) and (b) in Figure 1, we can see that the existing laser SLAM system has a poor mapping effect, and Figure 3 The system fails and is unavailable. The impact is greatest at the two U-turns, where the pose estimation result deviates significantly from the normal value. Figure 4 (b) The pose estimation system of the present invention performs normally. The above shows that the performance of the real-time pose estimation system of the present invention is greatly improved when the carrier performs high-speed maneuvers compared with the existing laser SLAM real-time pose estimation system.

[0094] The above embodiments are provided only for the purpose of describing the present invention, and are not intended to limit the scope of the present invention. The scope of the present invention is defined by the appended claims. Various equivalent substitutions and modifications made without departing from the spirit and principle of the present invention should all be included in the scope of the present invention.

Claims

1. A real-time pose estimation system for a small ground vehicle, characterized in that: include: A current carrier position result processing module (1), a current moment environment grid map publishing module (2), a carrier position history matrix updating module (3), a carrier motion parameter estimation module (4), a next moment carrier position prediction module (5) and a laser SLAM scanning callback function (6); The current carrier position result processing module (1) outputs and displays the obtained carrier position information at the current moment, and provides it to the current moment grid map publishing module (2) and the carrier position history matrix updating module (3); when initialized, the carrier position information at the current moment is given by the external GNSS system or the initialization position; when not initialized, the carrier position information at the current moment is given by the laser SLAM scanning callback function (6); The current moment environment grid map publishing module (2) uses the first frame point cloud data of the laser radar and the current moment carrier posture information to publish the initialization environment grid to the map laser SLAM scanning callback function (6) when initialized; when not initialized, uses the current moment laser radar point cloud data and the current moment carrier posture information to publish the current moment environment grid map to the laser SLAM scanning callback function (6); The carrier posture history matrix updating module (3) updates the carrier posture history matrix data according to the carrier posture information at the current moment by using a fixed weight or a degenerate weight method, and estimates the carrier posture result processing iteration cycle time for processing by the carrier posture prediction module (5) at the next moment; when the number of carrier posture data in the carrier posture history matrix reaches a set matrix capacity, the carrier posture history matrix is ​​output to the carrier motion parameter estimation module (4); the matrix capacity is set to ten or twenty iteration cycles; The carrier motion parameter estimation module (4) estimates the carrier motion parameters based on the carrier posture history matrix and the accurate modeling of the carrier motion model offline, using the least square method or Kalman filtering, including the determination of the running state, i.e., stationary, left turn, right turn, and straight travel, and the estimation of the motion speed variables, i.e., the rotation radius, the rotation angular velocity, and the straight travel velocity, and outputs the motion parameter estimation results to the carrier posture prediction module (5) at the next moment; The next moment carrier posture prediction module (5) performs integral prediction of the data iteration cycle time length using the carrier motion speed variable according to the data iteration cycle and the motion parameter estimation result, combined with the carrier motion model, and then obtains the next moment carrier posture prediction result, and outputs the next moment carrier posture prediction result to the laser SLAM scanning callback function (6); The laser SLAM scanning callback function (6) waits for the laser radar to output the point cloud result at the next moment for the predicted result of the carrier's posture at the next moment; when the point cloud result at the next moment is output, the grid map released at the current moment is used for inter-frame matching. The result of the inter-frame matching is the estimated value of the carrier's posture at the next moment. After the result is obtained, it is output to the current carrier posture result processing module (1) to complete the closed loop.

2. A real-time pose estimation system for a small ground vehicle, characterized in that include: A current carrier position result processing module (1), a current moment environment grid map publishing module (2), a carrier position history matrix updating module (3), a carrier motion parameter estimation module (4), a next moment carrier position prediction module (5) and a laser SLAM scanning callback function (6); The current carrier position result processing module (1) outputs and displays the obtained carrier position information at the current moment, and provides it to the current moment grid map publishing module (2) and the carrier position history matrix updating module (3); when initialized, the carrier position information at the current moment is given by the external GNSS system or the initialization position; when not initialized, the carrier position information at the current moment is given by the laser SLAM scanning callback function (6); The current moment environment grid map publishing module (2) uses the first frame point cloud data of the laser radar and the current moment carrier posture information to publish the initialization environment grid to the map laser SLAM scanning callback function (6) when initialized; when not initialized, uses the current moment laser radar point cloud data and the current moment carrier posture information to publish the current moment environment grid map to the laser SLAM scanning callback function (6); The carrier posture history matrix updating module (3) smoothes the current posture according to the carrier posture information at the current moment by using the fusion result of the odometer and the IMU inertial system information, and stores the smoothed result in the carrier posture history matrix, and at the same time estimates the iterative cycle time of the carrier posture result processing for processing by the carrier posture prediction module (5) at the next moment; when the number of carrier posture data in the carrier posture history matrix reaches the set matrix capacity, the carrier posture history matrix is ​​output to the carrier motion parameter estimation module (4); the set matrix capacity is ten or twenty iterative cycles; The carrier motion parameter estimation module (4) estimates the carrier motion parameters by the least square method or Kalman filtering based on the carrier posture history matrix and the accurate modeling of the carrier motion model offline, including the determination of the running state, i.e., stationary, left turn, right turn, and straight travel, and the estimation of the motion speed variables, i.e., the rotation radius, the rotation angular velocity, and the straight travel velocity; then, the motion parameter estimation result is weighted with the estimation result of the inertial information fusion to reduce the mapping and positioning deviation caused by the vibration of the ground carrier and the non-uniform acceleration start or braking; finally, the motion parameter estimation result is output to the carrier posture prediction module (5) at the next moment; The next moment carrier posture prediction module (5) performs integral prediction of the data iteration cycle time length using the carrier motion speed variable according to the data iteration cycle and the motion parameter estimation result, combined with the carrier motion model, and then obtains the next moment carrier posture prediction result, and outputs the next moment carrier posture prediction result to the laser SLAM scanning callback function (6); The laser SLAM scanning callback function (6) waits for the laser radar to output the point cloud result at the next moment for the predicted result of the carrier's posture at the next moment; when the point cloud result at the next moment is output, the grid map released at the current moment is used for inter-frame matching. The result of the inter-frame matching is the estimated value of the carrier's posture at the next moment. After the result is obtained, it is output to the current carrier posture result processing module (1) to complete the closed loop.

Images