Lidar-based positioning method and system for follow-up ultra-large load carrier vehicles

Abstract

The present invention discloses a positioning method and system for a follow-up type oversize cargo carrier vehicle based on lidar, which relates to the technical field of multi-vehicle intelligent cooperative positioning. The present invention includes: establishing coordinate systems for the leading vehicle, the trailing vehicle, the cargo, and the positioning module, calibrating the coordinate systems of the leading vehicle, the trailing vehicle, the cargo, and the positioning module, and storing the calibration results; obtaining point cloud information of the surrounding environment of the leading vehicle and the trailing vehicle based on lidar, constructing point cloud maps for the leading vehicle and the trailing vehicle respectively, and completing the self-positioning of the leading vehicle or the trailing vehicle based on the lidar inertial odometer in the positioning module. The present invention comprehensively combines single-vehicle autonomous positioning, multi-vehicle cooperative positioning, and cargo attitude estimation methods, and uses a factor graph optimization method to fuse the results of inter-vehicle perception and intra-vehicle prediction, realizing precise and reliable cooperative positioning of follow-up type carrier vehicles covering multi-source observation information and vehicle-cargo kinematic interaction relationships, improving the real-time performance of state estimation and positioning accuracy of follow-up type oversize cargo carrier vehicles.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-vehicle intelligent collaborative positioning, and specifically to a positioning method for following-type oversize load-carrying vehicles based on lidar. Background Art

[0002] Currently, a general load-carrying vehicle cannot carry oversize items such as large bridge structures. The common solution is to specially manufacture load-carrying vehicles for oversize items, which is costly and has low versatility. In contrast, multi-vehicle collaborative transportation is a more general solution. Due to the large mass and high center of mass of oversize goods, the positioning deviation between multiple vehicles can easily cause damage to the goods, and in severe cases, it may cause major losses of life and property. Moreover, the current inefficient manual collaborative transportation cannot meet the rapidly growing demand for oversize transportation.

[0003] Therefore, the present invention proposes a positioning method and system for following-type oversize load-carrying vehicles based on lidar. Summary of the Invention

[0004] The purpose of the present invention is to provide a positioning method and system for following-type oversize load-carrying vehicles based on lidar, which can integrate multi-source information and vehicle-cargo kinematic characteristics, reduce the uncertainty factors caused during the self-positioning process of the leading vehicle or the following vehicle, improve the positioning accuracy, and realize multi-vehicle collaborative positioning and cargo attitude estimation when following-type heavy-duty vehicles collaborate to carry oversize items, with strong environmental adaptability.

[0005] According to the first aspect of the present invention, to achieve the above object, the present invention provides the following technical solution: A positioning method for following-type oversize load-carrying vehicles based on lidar, which is used to estimate the relative pose between the leading vehicle and the following vehicle that collaborate to carry oversize items, and is realized by using a leading vehicle positioning module arranged at the front end of the leading vehicle, a following vehicle positioning module arranged at the rear end of the following vehicle, and a feature identifier arranged on the front end face of the support device, and includes the following steps:

[0006] Establish the coordinate systems of the leading vehicle, the following vehicle, the cargo, and the positioning module, calibrate the coordinate systems of the leading vehicle, the following vehicle, the cargo, and the positioning module, and store the calibration results;

[0007] Based on the lidar, obtain the point cloud information of the surrounding environment of the leading vehicle and the following vehicle, construct the point cloud maps of the leading vehicle and the following vehicle respectively, and complete the self-positioning of the leading vehicle or the following vehicle based on the lidar inertial odometer in the positioning module, and then make the following vehicle send the IMU measurement values and poses to the leading vehicle;

[0008] Based on the leading vehicle positioning module and the feature identifier on the support device, observe the relative attitude between the leading vehicle and the cargo, and based on the kinematic equation between the vehicle and the cargo, use the Kalman filter to estimate the cargo attitude in real time;

[0009] Multi-vehicle collaborative positioning is achieved based on real-time estimation of the cargo attitude. When the pose of the rear vehicle changes to the Nth key frame, the point cloud and the pose of the rear vehicle at this moment are transmitted to the front vehicle. The positioning of the rear vehicle and the cargo is corrected through the factor graph optimization method, and the optimized state of the rear vehicle is transmitted to the point cloud map of the rear vehicle to correct the map information.

[0010] Furthermore, establish the coordinate systems of the front vehicle, rear vehicle, cargo, and positioning module, and calibrate the coordinate systems of the front vehicle, rear vehicle, cargo, and positioning module as follows:

[0011] (21)Calibration of the positioning module

[0012] Take the IMU coordinate system as the coordinate system of the positioning module, move the positioning module, and the lidar uses the ICP point cloud registration method to estimate the pose change of the lidar coordinate system . The IMU uses the pre-integration method to estimate the pose change, and the hand-eye calibration method is used to determine the calibration extrinsic parameters from each lidar coordinate to the coordinate , where the calibration extrinsic parameter consists of the rotation matrix and the displacement vector :

[0013] ;

[0014] (22)Calibration between the positioning module and the vehicle coordinate system

[0015] Taking the support position of the front vehicle or rear vehicle as the origin, establish the vehicle coordinate system of the front vehicle or rear vehicle . Taking the forward direction of the front vehicle or rear vehicle as the positive direction of the X-axis and the vertically upward direction of the front vehicle or rear vehicle as the positive direction of the Z-axis, temporarily deploy RTK at the support position of the front vehicle or rear vehicle, move the front vehicle or rear vehicle, and respectively obtain the pose changes of the positioning module and the front vehicle or rear vehicle. The calibration extrinsic parameters from the positioning module coordinate to the front vehicle or rear vehicle coordinate are obtained through the hand-eye calibration method ;

[0016] (23)Calibration between the front vehicle coordinate system and the rear vehicle coordinate system

[0017] After the front vehicle arrives at the loading location, a point cloud environment map around the loading location is constructed based on the lidar inertial odometer, and the accuracy of the point cloud map is improved through loop closure correction. During the period when the front vehicle and the rear vehicle are waiting statically to load the oversize cargo, the rear vehicle transmits the lidar point cloud in the rear vehicle coordinate system to the front vehicle, and performs point cloud registration with the point cloud map established in the front vehicle coordinate system to obtain the initial calibration extrinsic parameters from the rear vehicle coordinate to the front vehicle coordinate , and, take the position where the front vehicle coordinate system is located at this time as the world coordinate system Origin;

[0018] (24) Calibration of Vehicle Coordinate System and Cargo Coordinate System

[0019] Take the direction from the rear vehicle support point to the front vehicle support point of the cargo as the attitude of the cargo in the world coordinate system. During transportation, the vehicle and the cargo are hinged. According to the externally calibrated parameters in step (23), the distance between the two support points of the corresponding cargo can be directly obtained and the initial heading angle in the world coordinate system ;

[0020] (25) Calibration of Feature Identification and Cargo Coordinate System

[0021] The lidar in the front vehicle positioning module identifies the feature identification on the support device, filters the point cloud based on the lidar echo intensity, obtains the feature identification and calculates the normal vector of the plane where it is located , based on the externally calibrated parameters from the positioning module coordinates to the world coordinate system , the externally calibrated parameters from the lidar coordinates for observing the cargo to coordinates obtain the normal vector representation in the world coordinate system , calculate the included angle between the projection of in the XOY plane of the world coordinate system and the cargo heading angle .

[0022] Furthermore, (31) Use lidar to obtain the point cloud information of the surrounding environment, use Gaussian filtering to smooth the point cloud data and remove noise, use the incremental KD tree as the data structure of the point cloud space, and establish a point cloud map;

[0023] Gaussian filtering: Multiply the k-nearest neighbor points of each point by the Gaussian weight G, and then perform weighted averaging to obtain the filtered point cloud coordinate value :

[0024]

[0025]

[0026] In the formula, is the distance from the point cloud to the k-nearest neighbor point, is the standard deviation of the Gaussian function, is the current point coordinate, is the th point coordinate in the neighborhood;

[0027] (32) Combining the calibrated extrinsic parameters, the IMU tracks the movement of the lidar coordinate system by linearly interpolating the displacement and spherically interpolating the attitude, aligning all point clouds to the IMU coordinate system at the same moment to eliminate the motion distortion of the point clouds;

[0028] (33) Based on the point cloud map established in real time, with the point-plane distance as the observation error, the positioning information of the leading vehicle and the following vehicle is updated in real time through the error-state Kalman filter and ;

[0029] The discrete update equation for the self-positioning of the leading vehicle or the following vehicle is:

[0030]

[0031] where, represents the state quantity at time , represents the state quantity at time , represents the observed quantity at time , represents the noise at time ;

[0032] The state quantity , the observed quantity , the noise and the function f are respectively:

[0033]

[0034]

[0035]

[0036]

[0037] represents the rotation matrix, translation vector, and velocity vector of the leading vehicle or the following vehicle in the world coordinate system, respectively represent the zero biases of the accelerometer and the gyroscope, represents the sampling time interval of the IMU, represents the acceleration and angular velocity, "~" represents the observed quantity of the IMU, represents the matrix transpose, assuming the zero bias , satisfies the Wiener process, and the components of the noise are all independent zero-mean Gaussian white noises, and the symbol For parameterizing the state error on an n-dimensional manifold The specific operation rules are as follows:

[0038]

[0039]

[0040] is the exponential map in the three-dimensional rotation group SO(3);

[0041] Define the observation error of the point-plane distance as:

[0042]

[0043] In the formula, represents the rigid transformation matrix from the vehicle coordinate system coordinates of the leading vehicle or the following vehicle to the world coordinate system, represents the point cloud coordinates obtained in the vehicle coordinate system of the leading vehicle or the following vehicle, including the leading vehicle or the following vehicle , is the point cloud the nearest neighbor point in the point cloud map, is the normal vector of the corresponding plane;

[0044] (34) Set the key frame change threshold to a displacement of 0.4 m or a heading angle of 10° or 0.5 s. When the following vehicle is at the point cloud key frame, it sends the positioning result and the IMU measurement value to the leading vehicle.

[0045] Furthermore, based on the leading vehicle positioning module and the feature identification on the support device, the relative attitude between the leading vehicle and the goods is observed. Based on the kinematic equation between the vehicle and the goods, the attitude of the goods is estimated in real time using the Kalman filter as follows:

[0046] (41) According to the kinematic relationship between the vehicle and the goods, obtain the goods attitude prediction equation (1) and the corresponding discretized equation (2):

[0047] (1)

[0048] (2)

[0049] In the formula, , , , , 、 、 、 respectively represent the lateral velocity, longitudinal velocity, yaw angular velocity, and heading angle of the vehicle coordinate system, , , , The subscripts 1 and 3 in the middle represent the leading vehicle and the following vehicle respectively. and represent the distances from the leading vehicle and the following vehicle coordinate systems to their respective support points respectively. is the discrete time interval. is the process noise, and it is assumed to be zero-mean Gaussian white noise with variance . a and b are the representations of the results of formula derivation.

[0050] (42) Control the lidar of the leading vehicle positioning module to obtain the normal vector of the feature identifier of the support device in the leading vehicle coordinate system , and combine the positioning result of the positioning module to obtain the heading angle measurement value of the goods in the world coordinate system , so the observation equation (3) can be obtained:

[0051] (3)

[0052] In the formula, is the measurement noise, and it is assumed to be zero-mean Gaussian white noise with variance .

[0053] (43) Predict the discretized equation (2) and the observation equation (3), and use the Kalman filter to update the heading angle of the goods in real time .

[0054] Furthermore, multi-vehicle collaborative positioning is realized according to the real-time estimated attitude of the goods. When the pose of the following vehicle changes to the Nth key frame, the point cloud at this moment and the pose of the following vehicle are transmitted to the leading vehicle, and the positioning of the following vehicle and the goods is corrected through the factor graph optimization method, as follows:

[0055] (51) When the estimated attitude of the following vehicle reaches the change threshold of the th key frame, the point cloud information at this moment is sent to the leading vehicle;

[0056] (52) Define the state variables to be optimized of the multi-vehicle carrying system following at time as:

[0057] (4)

[0058] In the formula , represent the poses of the leading vehicle and the following vehicle in the world coordinate system at time, represents The state of the leading vehicle's IMU in the leading vehicle coordinate system at a certain moment, including the obtained acceleration, angular velocity, and the zero biases of the accelerometer and gyroscope represents The state of the following vehicle's IMU in the following vehicle coordinate system at a certain moment, including the obtained acceleration , angular velocity , the zero bias of the accelerometer and the zero bias of the gyroscope ; M is the number of key frames of the following vehicle's pose involved in the first-order factor graph optimization;

[0059] (53) Estimate the change in the vehicle's pose from a certain moment to another moment through the pre-integration method as follows: from a certain moment to another moment, specifically as follows:

[0060] (5)

[0061] where and are respectively the discretized zero-mean Gaussian white noises, and are respectively the zero biases of the accelerometer and gyroscope at time k, represents the change in a certain state quantity from i a certain moment to j another moment;

[0062] Construct the state residual based on the pre-integration estimation result of the IMU measurement value:

[0063] (6)

[0064] where is the gravity, is the inverse mapping of;

[0065] (54) Construct the residual with the point-to-plane distance between the point cloud of the following vehicle and the point cloud map of the leading vehicle as the error:

[0066] (7)

[0067] where is the nearest neighbor point of the point cloud of the following vehicle in the point cloud map, is the normal vector of the corresponding plane;

[0068] (55) Based on the prediction and observation equations of the cargo heading angle , construct the residual sum:

[0069] (8)

[0070] (9)

[0071] (56) Based on the constructed residuals, the sliding window algorithm and factor graph optimization are used to solve for the optimal , to achieve multi-vehicle cooperative positioning, specifically as follows:

[0072] (10)

[0073] In the formula, represents the Huber norm, represents the Mahalanobis distance, represents the IMU measurement residual, represents the rear vehicle point cloud residual, represents the predicted attitude residual of the cargo, represents the observed attitude residual of the cargo;

[0074] (57) Send the corrected rear vehicle pose to the front vehicle to update the positioning state and point cloud map of the rear vehicle.

[0075] According to the second aspect of the present invention, the present invention provides a lidar-based following large-piece carrier vehicle positioning system for implementing the above-mentioned lidar-based following large-piece carrier vehicle positioning method, including:

[0076] A calibration unit for establishing the coordinate systems of the front vehicle, rear vehicle, cargo, and positioning module, calibrating the coordinate systems of the front vehicle, rear vehicle, cargo, and positioning module, and storing the calibration results;

[0077] A construction unit for obtaining the point cloud information of the surrounding environment of the front vehicle and rear vehicle based on lidar, constructing the point cloud maps of the front vehicle and rear vehicle respectively, and completing the self-positioning of the front vehicle or rear vehicle based on the lidar inertial odometer in the positioning module, and then enabling the rear vehicle to send the IMU measurement value and pose to the front vehicle;

[0078] A cargo attitude estimation unit for observing the relative attitude between the front vehicle and the cargo based on the front vehicle positioning module and the feature markers on the support device, and using Kalman filtering to estimate the cargo attitude in real time based on the kinematic equation between the vehicle and the cargo;

[0079] A multi-vehicle cooperative positioning unit for achieving multi-vehicle cooperative positioning according to the real-time estimated cargo attitude, transmitting the point cloud and rear vehicle pose at this moment to the front vehicle when the rear vehicle pose change reaches the Nth key frame, correcting the positioning of the rear vehicle and the cargo through the factor graph optimization method, and transmitting the optimized rear vehicle state to the point cloud map of the rear vehicle to correct the map information.

[0080] Further, the calibration unit includes a front vehicle positioning module disposed at the front end of the front vehicle and a rear vehicle positioning module disposed at the rear end of the rear vehicle;

[0081] The front vehicle positioning module includes three lidars for sensing surrounding space information and one lidar for observing the attitude of the goods, a front vehicle positioning calculation module, and an IMU and a communication module. The laser wavelength of the lidar for observing the attitude of the goods is 905 nm;

[0082] The rear vehicle positioning module includes two lidars for sensing surrounding space information, a rear vehicle positioning calculation module, and an IMU and a communication module.

[0083] Further, the goods attitude estimation unit includes a support device and a feature identifier disposed on the front end face of the support device. The feature identifier is set as a reflective tape.

[0084] According to a third aspect of the present invention, the present invention provides a terminal device, including a memory, a processor, and a computer program stored in the memory and capable of running on the processor. The memory stores a computer program capable of running on the processor. When the processor loads and executes the computer program, the above-mentioned lidar-based follow-up large-piece carrier vehicle positioning method is adopted.

[0085] According to a fourth aspect of the present invention, the present invention provides a storage medium containing computer-executable instructions. The computer-executable instructions are used to execute the above-mentioned lidar-based follow-up large-piece carrier vehicle positioning method when executed by a computer processor.

[0086] The present invention has at least the following beneficial effects:

[0087] 1. The positioning method proposed by the present invention combines single-vehicle autonomous positioning, multi-vehicle collaborative positioning, and goods attitude estimation methods. The factor graph optimization method is used to fuse the results of vehicle-to-vehicle perception and in-vehicle prediction, realizing accurate and reliable follow-up carrier vehicle collaborative positioning covering multi-source observation information and the kinematic interaction relationship between the vehicle and the goods, and improving the real-time performance and positioning accuracy of the state estimation of the follow-up large-piece carrier vehicle.

[0088] 2. The sensor modular layout scheme of the positioning module proposed by the present invention not only facilitates the calibration and debugging of the sensors with the body coordinate system of the front vehicle or the rear vehicle, but also improves the flexibility of sensor layout in the face of spatial constraints of special-shaped large-piece goods, and also leaves a calibrated sensor layout platform for other environmental perception requirements. For example, it can provide a good hardware platform for vehicle holographic environmental perception.

[0089] Of course, it is not necessary for any product implementing the present invention to achieve all the above-mentioned advantages at the same time. Description of the Drawings

[0090] Figure 1 It is a schematic flowchart of the positioning method described in the first embodiment of the present invention;

[0091] Figure 2 It is a schematic structural diagram of the positioning system described in the second embodiment of the present invention;

[0092] Figure 3 It is a schematic diagram of the calibration of the external parameters among the leading vehicle, the trailing vehicle, the goods, and the positioning module in the first embodiment of the present invention;

[0093] Figure 4 It is a schematic diagram of the multi-vehicle cooperative positioning principle based on factor graph optimization and sliding window algorithm in the first embodiment of the present invention. Detailed implementation manners

[0094] Next, the technical solutions in the embodiments of the present disclosure will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present disclosure. Obviously, the described embodiments are only a part of the embodiments of the present disclosure, rather than all the embodiments. Based on the embodiments in the present disclosure, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present disclosure.

[0095] Explanation of related terms:

[0096] Vehicle-goods refers to the vehicle and the goods carried on the vehicle. The vehicle includes the leading vehicle and the trailing vehicle, which can be generally referred to or specifically referred to as the leading vehicle or the trailing vehicle. When it comes to describing the relationship between a specific vehicle and the goods, it refers to that specific vehicle. For example, the kinematic equation between the vehicle and the goods refers to the kinematic equation between the leading vehicle or the trailing vehicle and the goods;

[0097] Vehicle: includes the leading vehicle and the trailing vehicle. For example, the vehicle coordinate system includes the leading vehicle coordinate system and the trailing vehicle coordinate system.

[0098] Embodiment 1:

[0099] Please refer to Figure 1 , the present invention provides a technical solution: a following-type oversized load carrier vehicle positioning method based on lidar, which is used to estimate the relative pose of the leading vehicle and the trailing vehicle carrying oversized goods in cooperation, and is realized by using a leading vehicle positioning module arranged at the front end of the leading vehicle, a trailing vehicle positioning module arranged at the rear end of the trailing vehicle, and a feature identifier arranged on the front end face of the support device, and includes the following steps:

[0100] S1. Establish the coordinate systems where the leading vehicle, the trailing vehicle, the goods, and the positioning module are located, calibrate the coordinate systems where the leading vehicle, the trailing vehicle, the goods, and the positioning module are located, and store the calibration results. As Figure 3 shown, specifically as follows:

[0101] (S11) Calibration of the positioning module

[0102] Take the IMU coordinate system as the positioning module coordinate system. Move the positioning module, and the lidar uses the ICP point cloud registration method to estimate the pose change of the lidar coordinate system . The IMU uses the pre-integration method to estimate the pose change, and the hand-eye calibration method is used among the sensors to determine the calibration extrinsic parameters from each lidar coordinate to the coordinate . Among them, the calibration extrinsic parameters consist of the rotation matrix and the displacement vector :

[0103] ;

[0104] (S12)Calibration of the positioning module and the vehicle coordinate system

[0105] Take the support position of the vehicle (front vehicle and / or rear vehicle) as the origin to establish the vehicle coordinate system . Take the forward direction of the vehicle as the positive direction of the X-axis, and the vertically upward direction of the vehicle as the positive direction of the Z-axis. Temporarily deploy RTK at the vehicle support position, move the vehicle, and respectively obtain the pose changes of the positioning module and the vehicle. Obtain the calibration extrinsic parameters from the positioning module coordinate to the vehicle coordinate through the hand-eye calibration method ;

[0106] (S13)Calibration of the front vehicle coordinate system and the rear vehicle coordinate system

[0107] After the front vehicle arrives at the loading location, build a point cloud environment map around the loading location based on the lidar inertial odometer, and improve the accuracy of the point cloud map through loop closure correction. During the period when the front vehicle and the rear vehicle are waiting statically to load the oversize cargo, the rear vehicle transfers the lidar point cloud in the rear vehicle coordinate system to the front vehicle, and performs point cloud registration with the point cloud map established in the front vehicle coordinate system to obtain the initial calibration extrinsic parameters from the rear vehicle coordinate to the front vehicle coordinate . And take the position where the front vehicle coordinate system is located at this time as the origin of the world coordinate system ;

[0108] (S14)Calibration of the vehicle coordinate system and the cargo coordinate system

[0109] Take the direction of the cargo from the rear vehicle support point to the front vehicle support point as the attitude of the cargo in the world coordinate system. During transportation, the vehicle and the cargo are articulated. According to the calibrated extrinsic parameters in step (S13), the distance between the two support points of the corresponding cargo and the initial heading angle in the world coordinate system can be directly obtained

[0110] (S15) Feature identification and calibration of the cargo coordinate system

[0111] The lidar in the front vehicle positioning module identifies the feature identification on the support device, filters the point cloud based on the lidar echo intensity, obtains the feature identification and calculates the normal vector of the plane where it is located , and obtains the normal vector based on the aforementioned calibrated extrinsic parameters Representation in the world coordinate system , calculate The projection of the world coordinate system XOY plane and the cargo heading angle of the included angle ;

[0112] It should be noted that the aforementioned calibrated extrinsic parameters include: the calibrated extrinsic parameters from the positioning module coordinates to the world coordinate system (which can also be expressed as , that is, V is the calibrated extrinsic parameter from the positioning module coordinates obtained by the front vehicle to the vehicle coordinates , since the vehicle has not moved, it is considered that the world coordinate system is the same as the front vehicle coordinate system, is the identity matrix), the calibrated extrinsic parameters from the lidar coordinates for observing the cargo to coordinates (that is, i = 1, corresponding to the lidar serial number for observing the cargo by the front vehicle, the calibrated extrinsic parameters from the lidar coordinates to coordinates ).

[0113] ;

[0114] S2. Obtain the point cloud information of the surrounding environment of the front vehicle and the rear vehicle based on the lidar, construct the point cloud maps of the front vehicle and the rear vehicle respectively, and complete the self-positioning of the front vehicle or the rear vehicle based on the lidar inertial odometer in the positioning module. Then, make the rear vehicle send the IMU measurement values and poses to the front vehicle, as follows:

[0115] (S21) Use the lidar to obtain the point cloud information of the surrounding environment, smooth the point cloud data with Gaussian filtering and remove the noise, use the incremental KD tree as the data structure of the point cloud space to reduce the depth imbalance caused by the addition and deletion of a large amount of data, and establish the point cloud map;

[0116] Gaussian filtering: Multiply the k-nearest neighbor points of each point by the Gaussian weight G, and then perform weighted averaging to obtain the filtered point cloud coordinate value :

[0117]

[0118]

[0119] In the formula, is the distance from the point cloud to the k-nearest neighbor point, is the standard deviation of the Gaussian function, is the current point coordinate, is the coordinate of the

[0120] (S22) Combine the calibrated extrinsic parameters. The IMU uses linear interpolation for displacement and spherical interpolation for attitude to track the movement of the lidar coordinate system, aligning all point clouds to the IMU coordinate system at the same moment to eliminate the motion distortion of the point clouds;

[0121] (S23) According to the point cloud map established in real time, using the point-plane distance as the observation error, the positioning information of the vehicle in front and the vehicle behind is updated in real time through error-state Kalman filtering and ;

[0122] The discrete update equation for the positioning of the vehicle in front or the vehicle behind and the ego-vehicle is

[0123]

[0124] In the formula, represents the state quantity at time , represents the state quantity at time , represents the observed quantity at time , represents the noise at time ;

[0125] The state quantity , the observed quantity , the noise and the function f are respectively:

[0126]

[0127]

[0128]

[0129]

[0130] represents the rotation matrix, translation vector, and velocity vector of the vehicle in front or the vehicle behind and the ego-vehicle in the world coordinate system, respectively represent the biases of the accelerometer and gyroscope, represents the sampling time interval of the IMU, represents the acceleration and angular velocity, and "~" represents the observed quantity of the IMU, Represents matrix transpose, assuming zero bias , Satisfies the Wiener process, and the components of the noise are all independent zero-mean Gaussian white noise. The symbol is used to parameterize the state error on an n-dimensional manifold as follows:

[0131]

[0132]

[0133] Is the exponential map in the three-dimensional rotation group SO(3);

[0134] Define the observation error of the point-plane distance as

[0135]

[0136] In the formula, Represents the rigid transformation matrix from the coordinate of the leading vehicle or following vehicle in the ego-vehicle coordinate system to the world coordinate system, Represents the point cloud coordinate obtained in the ego-vehicle coordinate system of the leading vehicle or following vehicle, including the leading vehicle or the following vehicle , is the point cloud The nearest neighbor point in the point cloud map, is The normal vector of the corresponding plane;

[0137] (S24) Set the key frame change threshold to a displacement of 0.4 m or a heading angle of 10° or 0.5 s. When the following vehicle is at the point cloud key frame, it sends the positioning result and the IMU measurement value to the leading vehicle;

[0138] S3. Based on the relative attitude between the leading vehicle and the goods observed by the leading vehicle positioning module and the feature markers on the support device, and based on the kinematic equation between the vehicle and the goods, use the Kalman filter to estimate the goods attitude in real time as follows:

[0139] (S31) According to the kinematic relationship between the vehicle and the goods, obtain the goods attitude prediction equation (1) and the corresponding discretized equation (2):

[0140] (1)

[0141] (2)

[0142] In the formula, , , ,

[0143] ; 、 、 、 represent the lateral velocity, longitudinal velocity, yaw rate, and heading angle of the vehicle coordinate system respectively, 、 、 、 where the subscripts 1 and 3 in [[ ]] represent the leading vehicle and the following vehicle respectively, and represent the distances from the leading vehicle and the following vehicle coordinate systems to their respective support points respectively, is the discrete time interval, is the process noise, and it is assumed to be zero-mean Gaussian white noise with variance , and a and b are the representations of the results of formula derivation;

[0144] (S32) Control the lidar of the leading vehicle positioning module to obtain the normal vector of the feature identifier of the support device in the leading vehicle coordinate system , and obtain the heading angle of the cargo in the world coordinate system by combining the positioning results of the positioning module. Therefore, the observation equation (3) can be obtained:

[0145] (3)

[0146] In the formula, is the measurement noise, and it is assumed to be zero-mean Gaussian white noise with variance ;

[0147] (S33) Predict the discretized equation (2) and the observation equation (3), and use the Kalman filter to update the heading angle of the cargo in real time;

[0148] S4. Achieve multi-vehicle collaborative positioning according to the real-time estimated cargo attitude. When the pose of the following vehicle changes to the Nth key frame, transfer the point cloud and the pose of the following vehicle at this moment to the leading vehicle, correct the positioning (pose of the following vehicle) of the following vehicle and the cargo through the factor graph optimization method, and transfer the optimized state of the following vehicle to the point cloud map of the following vehicle to correct the map information, as Figure 4 shown, specifically as follows:

[0149] (S41) When the estimated attitude (pose of the following vehicle) of the following vehicle reaches the change threshold of the th key frame, send the point cloud information and the estimated pose of the following vehicle at this moment to the leading vehicle;

[0150] (S42) Define the state variable to be optimized of the multi-vehicle transportation system at time as:

[0151] (4)

[0152] In the formula 、 represent the poses of the leading vehicle and the following vehicle in the world coordinate system at a certain moment, represents the state of the leading vehicle's IMU factor in the leading vehicle coordinate system at a certain moment, including the obtained acceleration, angular velocity, and the zero biases of the accelerometer and gyroscope, represents the state of the following vehicle's IMU in the following vehicle coordinate system at a certain moment, including the obtained acceleration 、angular velocity 、the zero bias of the accelerometer and the zero bias of the gyroscope ; M is the number of key frames of the following vehicle pose involved in the first-order factor graph optimization;

[0153] (S43) Estimate the change in vehicle pose from the moment of to the moment of through the pre-integration method, specifically as follows:

[0154] Estimate the change in vehicle pose from the moment of to the moment of through the pre-integration method, specifically as follows:

[0155] (5)

[0156] In the formula and are respectively the discretized zero-mean Gaussian white noise,, and are respectively the zero biases of the accelerometer and gyroscope at time k, is a composite function of the mapping from a vector to an anti-symmetric matrix and the exponential mapping from a Lie algebra to a Lie group, which transforms a rotation vector into a rotation matrix , represents the change in a certain state quantity from the moment of i to the moment of j ;

[0157] Construct a state residual based on the pre-integration estimation result of the IMU measurement value:

[0158] (6)

[0159] In the formula, is the gravity, is 's inverse mapping;

[0160] Construct the residual by taking the point - plane distance as the error between the rear vehicle point cloud and the front vehicle point cloud map :

[0161] (7)

[0162] In the formula is the nearest neighbor point of the rear vehicle point cloud in the point cloud map, is the normal vector of the corresponding plane;

[0163] (S45)Based on the prediction and observation equations of the cargo heading angle construct the residual sum:

[0164] (8)

[0165] (9)

[0166] (S46)Based on the above - constructed residuals, use the sliding window algorithm and factor graph optimization to solve the optimal to achieve multi - vehicle collaborative positioning, specifically as follows:

[0167] (10)

[0168] In the formula, represents the Huber norm, represents the Mahalanobis distance, represents the IMU measurement residual, represents the rear vehicle point cloud residual, represents the cargo predicted attitude residual, represents the cargo observed attitude residual;

[0169] (S47)Send the corrected rear vehicle pose to the front vehicle to update the positioning status of the rear vehicle and the point cloud map.

[0170] It should be noted that in the factor graph optimization and the sliding window algorithm, , , N is the key - frame threshold for the rear vehicle to send perception results to the front vehicle. Whenever the estimated attitude of the rear vehicle reaches the Nth key frame, the rear vehicle sends the perception results at that moment (the point cloud at that moment, the positioning results corresponding to the unsent rear vehicle key frames, and the pre - integrated results of the IMU observations during that period) to the front vehicle; M is the number of rear vehicle pose key frames involved in one factor graph optimization.

[0171] Embodiment 2:

[0172] This embodiment provides a positioning system for a follow-up ultra-large load carrier vehicle based on lidar, which is used to implement the positioning method for the follow-up ultra-large load carrier vehicle based on lidar described in Embodiment 1, and includes:

[0173] A calibration unit, which is used to establish the coordinate systems of the leading vehicle, the trailing vehicle, the cargo, and the positioning module, calibrate the coordinate systems of the leading vehicle, the trailing vehicle, the cargo, and the positioning module, and store the calibration results;

[0174] A construction unit, which is used to obtain the point cloud information of the surrounding environment of the leading vehicle and the trailing vehicle based on lidar, construct the point cloud maps of the leading vehicle and the trailing vehicle respectively, and complete the self-positioning of the leading vehicle or the trailing vehicle based on the lidar inertial odometer in the positioning module. Then, the trailing vehicle sends the IMU measurement value and pose to the leading vehicle;

[0175] A cargo attitude estimation unit, which is used to observe the relative attitude between the leading vehicle and the cargo based on the leading vehicle positioning module and the feature identification on the support device, and use the kinematic equation between the vehicle and the cargo to estimate the cargo attitude in real time by using Kalman filtering;

[0176] A multi-vehicle collaborative positioning unit, which is used to realize multi-vehicle collaborative positioning according to the real-time estimated cargo attitude. When the pose of the trailing vehicle changes to the Nth key frame, the point cloud at this moment and the pose of the trailing vehicle are transmitted to the leading vehicle, and the positioning of the trailing vehicle and the cargo is corrected by the factor graph optimization method, and the optimized state of the trailing vehicle is transmitted to the point cloud map of the trailing vehicle to correct the map information.

[0177] Furthermore, the calibration unit includes a leading vehicle positioning module arranged at the front end of the leading vehicle and a trailing vehicle positioning module arranged at the rear end of the trailing vehicle;

[0178] The leading vehicle positioning module includes three lidars for sensing the surrounding space information, one lidar for observing the cargo attitude, a leading vehicle positioning calculation module, and an IMU and a communication module. The laser wavelength of the lidar for observing the cargo attitude is 905 nm;

[0179] The trailing vehicle positioning module includes two lidars for sensing the surrounding space information, a trailing vehicle positioning calculation module, and an IMU and a communication module.

[0180] It should be noted that the leading vehicle positioning calculation module is used to control the lidar in the positioning module, estimate the cargo pose, establish the point cloud map obtained by observing the leading vehicle, fuse the point cloud information of the trailing vehicle and the point cloud map of the leading vehicle, and realize collaborative positioning; the IMU is used to observe the self-motion state of the leading vehicle in real time and estimate the self-pose of the leading vehicle; the communication module is used to receive the sensing results from the trailing vehicle and send the corrected trailing vehicle positioning results;

[0181] The rear vehicle positioning calculation module is used to control the lidar in the positioning module, establish a point cloud map obtained from the rear vehicle observation, and estimate the absolute pose of the rear vehicle; the IMU is used to observe the motion state of the rear vehicle in real time and estimate the pose of the rear vehicle itself; the communication module is used to send the perception results of the rear vehicle to the front vehicle and receive the corrected positioning of the rear vehicle from the front vehicle.

[0182] Furthermore, the cargo attitude estimation unit includes a support device and a feature identifier provided on the front end face of the support device, and the feature identifier is set as a reflective tape.

[0183] Specifically, the above-mentioned calibration unit, construction unit, cargo attitude estimation unit, and multi-vehicle cooperative positioning unit can be embedded in a computer processing system. The computer calls the above-mentioned units according to the above-mentioned lidar-based follow-up large-piece carrier vehicle positioning method to complete the task of real-time positioning of the front vehicle and the rear vehicle; the above-mentioned calibration unit, construction unit, cargo attitude estimation unit, and multi-vehicle cooperative positioning unit can perform operations according to the specific steps given by the above-mentioned lidar-based follow-up large-piece carrier vehicle positioning method.

[0184] It should be noted that it should be understood that the division of each unit of the above system is only a logical function division. In actual implementation, it can be fully or partially integrated into a physical entity, or physically separated. And these units can all be implemented in the form of software called by a processing element; they can also all be implemented in the form of hardware; it is also possible that some units are implemented in the form of software called by a processing element, and some units are implemented in the form of hardware. For example, the calibration unit can be a separately established processing element, or can be integrated in a certain chip of the above device. In addition, it can also be stored in the memory of the above device in the form of program code, and called and executed by a certain processing element of the above device to perform the functions of the above signal processing unit. The implementation of other units is similar. In addition, all or part of these units can be integrated together or independently implemented. The processing element mentioned here can be an integrated circuit with signal processing capabilities. In the implementation process, each step of the above method or each of the above units can be completed by the hardware integrated logic circuit or software-form instructions in the processor element.

[0185] For example, the above units may be one or more integrated circuits configured to implement the above methods. For example: one or more Application Specific Integrated Circuits (ASICs), or one or more Digital Signal Processors (DSPs), or one or more Field Programmable Gate Arrays (FPGAs), etc. Again, when a certain unit above is implemented in the form of a processing element scheduler code, the processing element may be a general-purpose processor, such as a Central Processing Unit (CPU) or other processors that can call program code. Again, these units may be integrated together and implemented in the form of a system-on-a-chip (SOC).

[0186] In summary, the present invention combines the methods of autonomous positioning of a single vehicle, multi-vehicle cooperative positioning, and cargo attitude estimation. It uses the factor graph optimization method to fuse the results of in-vehicle perception and in-vehicle prediction, and realizes accurate and reliable cooperative positioning of a following large-piece carrier vehicle that covers multi-source observation information and the kinematic interaction relationship between the vehicle and the cargo. In addition, the modular layout scheme of the sensors in the proposed positioning module not only facilitates the calibration and debugging of the sensors with the vehicle coordinate system, but also improves the flexibility of sensor layout in the face of spatial constraints of irregular oversize cargo, and also leaves a calibrated sensor layout platform for other environmental perception requirements. For example, it can provide a good hardware platform for vehicle holographic environmental perception.

[0187] Embodiment 3:

[0188] The present invention provides a terminal device, including a memory, a processor, and a computer program stored in the memory and capable of running on the processor. The memory stores a computer program capable of running on the processor. When the processor loads and executes the computer program, it adopts the above-mentioned method for positioning a following oversize carrier vehicle based on lidar.

[0189] It should be noted that the terminal device may be a computer device such as a desktop computer, a laptop computer, or a cloud server. And the terminal device includes but is not limited to a processor and a memory. For example, the terminal device may also include input / output devices, network access devices, and a bus, etc.

[0190] Further, the processor may be a central processing unit (CPU). Of course, according to the actual usage, other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), off-the-shelf programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. may also be used. The general-purpose processor may be a microprocessor or any conventional processor, etc. This application does not make any restrictions in this regard.

[0191] Embodiment 4:

[0192] The present invention provides a storage medium containing computer-executable instructions, and the computer-executable instructions are used to execute the above-mentioned lidar-based positioning method for oversized load carriers during following when executed by a computer processor.

[0193] Among them, the computer program can be stored in a computer-readable medium. The computer program includes computer program code, and the computer program code can be in the form of source code, object code, executable file, or some middleware form, etc. The computer-readable medium includes any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disc, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal, and software distribution medium, etc. It should be noted that the computer-readable medium includes but is not limited to the above-mentioned components.

[0194] It should be noted that in this article, relational terms such as first and second are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements not only includes those elements, but also includes other elements not expressly listed, or further includes elements inherent to such process, method, article or device.

[0195] For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances. When an element is referred to as being "assembled on", "installed on", "fixed to" or "disposed on" another element, it can be directly on the other element or there may also be an intermediate element. When an element is considered to be "connected" to another element, it can be directly connected to the other element or there may be an intermediate element at the same time. The terms "vertical", "horizontal", "upper", "lower", "left", "right" and similar expressions used herein are only for the purpose of illustration and do not represent the only implementation manner.

[0196] Although embodiments of the present invention have been shown and described, those of ordinary skill in the art will appreciate that various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the invention. The scope of the invention is defined by the appended claims and their equivalents.

[0197] In the description of this specification, the descriptions referring to the terms "one embodiment", "example", "specific example", etc. mean that the specific features, structures, materials or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present disclosure. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in a suitable manner in any one or more embodiments or examples.

Claims

1. A laser radar-based tracking oversized cargo transport vehicle positioning method is used to estimate the relative posture of the leading vehicle and the trailing vehicle that are collaboratively carrying oversized cargo. The method is implemented by using a leading vehicle positioning module disposed at the front end of the leading vehicle, a trailing vehicle positioning module disposed at the rear end of the trailing vehicle, and a feature marker disposed at the front end of the supporting device. The method is characterized in that: The following steps are involved: Establish the coordinate system of the front vehicle, the rear vehicle, the cargo, and the positioning module, calibrate the coordinate system of the front vehicle, the rear vehicle, the cargo, and the positioning module, and store the calibration results; Based on the laser radar, the point cloud information of the surrounding environment of the front and rear vehicles is obtained, and the point cloud maps of the front and rear vehicles are constructed. The self-positioning of the front or rear vehicle is completed based on the laser radar inertial odometer in the positioning module, and then the rear vehicle sends the IMU measurement value and posture to the front vehicle; The relative posture between the front vehicle and the cargo is obtained based on the front vehicle positioning module and the feature markers on the support device. The Kalman filter is used to estimate the cargo posture in real time based on the kinematic equation between the vehicle and the cargo. Multi-vehicle collaborative positioning is achieved based on real-time estimation of cargo posture. When the posture change of the rear vehicle reaches the Nth key frame, the point cloud and the posture of the rear vehicle at this moment are transmitted to the front vehicle. The positioning of the rear vehicle and the cargo is corrected through the factor graph optimization method, and the optimized rear vehicle state is transmitted to the point cloud map of the rear vehicle to correct the map information.

2. The laser radar-based tracking oversized transport vehicle positioning method according to claim 1 is characterized in that: Establish the coordinate system of the front vehicle, rear vehicle, cargo, and positioning module, and calibrate the coordinate system of the front vehicle, rear vehicle, cargo, and positioning module, as follows: (21) Positioning module calibration Get IMU coordinate system I For the positioning module coordinate system, the mobile positioning module, the laser radar uses the ICP point cloud registration method to estimate the laser radar coordinate system L The IMU uses the pre-integration method to estimate the posture change, and the hand-eye calibration method is used to determine the coordinates of each lidar. Coordinate calibration extrinsic parameters , where the calibration external parameter By the rotation matrix and displacement vector composition: ; (22) Calibration of positioning module and vehicle coordinate system Take the support position of the front or rear vehicle as the origin and establish the vehicle coordinate system of the front or rear vehicle V , take the forward direction of the front or rear vehicle as the positive direction of the X axis, the vertical upward direction of the front or rear vehicle as the positive direction of the Z axis, temporarily arrange RTK at the support position of the front or rear vehicle, move the front or rear vehicle, obtain the position change of the positioning module and the front or rear vehicle respectively, and obtain the calibration external parameters from the positioning module coordinates to the front or rear vehicle coordinates through the hand-eye calibration method ; (23) Calibration of the front vehicle coordinate system and the rear vehicle coordinate system After the front vehicle arrives at the loading location, a point cloud environment map around the loading location is constructed based on the LiDAR inertial odometer, and the accuracy of the point cloud map is improved through loop correction. While the front vehicle and the rear vehicle are stationary waiting to load the oversized cargo, the rear vehicle will R The laser radar point cloud under the F The established point cloud map is used for point cloud registration to obtain the initial calibration external parameters from the rear vehicle coordinates to the front vehicle coordinates , and take the position of the front vehicle coordinate system at this time as the world coordinate system W origin; (24) Calibration of vehicle coordinate system and cargo coordinate system The direction of the cargo from the rear vehicle support point to the front vehicle support point is taken as the cargo posture in the world coordinate system. During transportation, the vehicle and the cargo are articulated. According to the calibrated external parameters in step (23), The distance between the two supporting points of the corresponding goods can be directly obtained and the initial heading angle in the world coordinate system ; (25) Feature identification and cargo coordinate system calibration The laser radar in the front vehicle positioning module recognizes the feature mark on the support device, filters the point cloud based on the laser echo intensity, obtains the feature mark and calculates the plane normal vector , based on the calibration external parameters of the positioning module coordinates to the world coordinate system , observe the laser radar coordinates of the goods to Coordinate calibration extrinsic parameters Get the normal vector Representation in world coordinate system ,calculate Projection on the XOY plane of the world coordinate system and cargo heading angle Angle .

3. The laser radar-based tracking oversized transport vehicle positioning method according to claim 1 is characterized in that: Based on the laser radar, the point cloud information of the surrounding environment of the front and rear vehicles is obtained, and the point cloud maps of the front and rear vehicles are constructed. The self-positioning of the front or rear vehicle is completed based on the laser radar inertial odometer in the positioning module, as follows: (31) Use laser radar to obtain point cloud information of the surrounding environment, use Gaussian filtering to smooth the point cloud data, use incremental KD tree as the data structure of the point cloud space, and establish a point cloud map; Gaussian filtering: multiply the k nearest neighbors of each point by the Gaussian weight G, and then perform weighted average to obtain the filtered point cloud coordinate value : In the formula, is the distance from the point cloud to the k nearest neighbor points, is the standard deviation of the Gaussian function, is the current point coordinate, For the neighborhood The coordinates of the points; (32) Combined with the calibrated external parameters, the IMU uses linear interpolation displacement and spherical interpolation attitude to track the movement of the lidar coordinate system, align all point clouds to the IMU coordinate system at the same time, and eliminate point cloud motion distortion; (33) Based on the point cloud map established in real time, the point-to-surface distance is used as the observation error, and the positioning information of the leading and trailing vehicles is updated in real time through the error state Kalman filter. and ; The discrete update equation for the positioning of the leading or following vehicle is: In the formula, express State quantity at time , express State quantity at time , express Observable quantity at time , express Noise of the moment ; State quantity , Observation ,noise and function f They are: Represents the rotation matrix, translation vector, and velocity vector of the leading or trailing vehicle in the world coordinate system. Represent the zero bias of the accelerometer and gyroscope respectively, Represents the sampling time interval of IMU, Represents acceleration and angular velocity, "~" represents the IMU observation, represents the matrix transpose, assuming zero bias , Satisfies the Wiener process, noise The components of are all independent zero-mean Gaussian white noise, symbol Used to parameterize the state error on an n-dimensional manifold The specific operation rules are as follows: is the exponential map in the three-dimensional rotation group SO(3); Defining the observation error of point-to-surface distance for: In the formula, Represents the rigid transformation matrix from the front or rear vehicle's ego coordinate system to the world coordinate system, Represents the point cloud coordinates obtained in the vehicle coordinate system of the front or rear vehicle. Including the front car Or the rear car , Point Cloud The nearest neighbor point in the point cloud map, for The normal vector of the corresponding plane; (34) The key frame change threshold is set to 0.4 m displacement or 10° heading angle or 0.5 s. The rear vehicle sends the positioning result and IMU measurement value to the front vehicle at the point cloud key frame.

4. The laser radar-based tracking oversized transport vehicle positioning method according to claim 1 is characterized in that: Based on the observation of the front vehicle positioning module and the feature identification on the support device, the relative posture between the front vehicle and the cargo is obtained. Based on the kinematic equation between the vehicle and the cargo, the Kalman filter is used to estimate the cargo posture in real time, as follows: (41) According to the kinematic relationship between vehicle and cargo, the cargo posture prediction equation (1) and the corresponding discretization equation (2) are obtained: （1） （2） In the formula, , , , , , , , Represent the lateral velocity, longitudinal velocity, yaw rate and heading angle of the vehicle coordinate system respectively. , , , The subscripts 1 and 3 represent the front and rear vehicles respectively. and Respectively represent the distances from the front and rear vehicle coordinate systems to their respective support points, is a discrete time interval, is the process noise, and its variance is assumed to be Zero-mean Gaussian white noise, a and b are the representations of the formula derivation results; (42) Control the laser radar of the front vehicle positioning module to obtain the normal vector of the feature identifier of the support device in the front vehicle coordinate system , combined with the positioning results of the positioning module to obtain the heading angle measurement value of the cargo in the world coordinate system , so we can get the observation equation (3): （3） In the formula, is the measurement noise, and is assumed to be the variance Zero-mean Gaussian white noise; (43) Predict the discretized equation (2) and the observation equation (3), and use Kalman filtering to update the cargo heading angle in real time .

5. The laser radar-based tracking oversized cargo transport vehicle positioning method according to claim 1, characterized in that: The multi-vehicle collaborative positioning is realized based on the real-time estimation of the cargo posture. When the posture change of the rear vehicle reaches the Nth key frame, the point cloud and the posture of the rear vehicle at this moment are transmitted to the front vehicle. The positioning of the rear vehicle and the cargo is corrected by the factor graph optimization method, as follows: (51) When the estimated posture of the rear vehicle reaches When a key frame changes the threshold, the point cloud information at this moment is sent to the preceding vehicle; (52) Definition State variables to be optimized for multi-vehicle transportation system with time-to-time tracking for: （4） In the formula , represent The position of the front and rear vehicles in the world coordinate system at the moment, represent The state of the front vehicle IMU in the front vehicle coordinate system at the moment, including the obtained acceleration , angular velocity , accelerometer and angular velocity meter Zero bias, represent The state of the rear vehicle IMU in the rear vehicle coordinate system at time t, including the obtained acceleration , angular velocity , Accelerometer bias and the zero bias of the angular velocity sensor ; M is the number of key frames of the rear vehicle posture involved in a factor graph optimization; (53) IMU is realized by pre-integration method Time to The estimation of vehicle posture change at the moment is as follows: （5） In the formula and are the discretized zero-mean Gaussian white noise, and are the zero bias of the accelerometer and gyroscope at time k, Represents a state quantity from i Time has come j The change at each moment is based on the pre-integrated estimation result of the IMU measurement value to construct the state residual: （6） In the formula, is gravity, for The inverse mapping of (54) The point cloud map of the rear vehicle and the point cloud map of the front vehicle is constructed with the point-to-surface distance as the error. : （7） In the formula Point cloud for the rear vehicle The nearest neighbor point in the point cloud map, for The normal vector of the corresponding plane; (55) Based on cargo heading angle The prediction and observation equations are constructed to construct the residuals and: （8） （9） (56) Based on the construction residual, the sliding window algorithm and factor graph optimization are used to solve the optimal , to achieve multi-vehicle collaborative positioning, as follows: （10） In the formula, represents the Huber norm, represents the Mahalanobis distance, Represents the IMU measurement residual, represents the residual of the rear vehicle point cloud, Represents the residual error of cargo prediction posture, Represents the residual of cargo observation posture; (57) The corrected position of the rear vehicle is sent to the front vehicle, and the positioning status and point cloud map of the rear vehicle are updated.

6. A laser radar-based follow-up oversized cargo transport vehicle positioning system, used to implement the laser radar-based follow-up oversized cargo transport vehicle positioning method described in any one of claims 1 to 5, characterized in that: include: A calibration unit is used to establish a coordinate system where the front vehicle, the rear vehicle, the cargo, and the positioning module are located, calibrate the coordinate system where the front vehicle, the rear vehicle, the cargo, and the positioning module are located, and store the calibration results; A construction unit is used to obtain point cloud information of the surrounding environment of the front vehicle and the rear vehicle based on the laser radar, construct point cloud maps of the front vehicle and the rear vehicle respectively, and complete the self-vehicle positioning of the front vehicle or the rear vehicle based on the laser radar inertial odometer in the positioning module, and then enable the rear vehicle to send the IMU measurement value and posture to the front vehicle; The cargo posture estimation unit is used to obtain the relative posture between the leading vehicle and the cargo based on the leading vehicle positioning module and the characteristic identification on the supporting device, and to estimate the cargo posture in real time using Kalman filtering based on the kinematic equation between the vehicle and the cargo; The multi-vehicle collaborative positioning unit is used to realize multi-vehicle collaborative positioning based on the real-time estimation of the cargo posture. When the posture change of the rear vehicle reaches the Nth key frame, the point cloud and the posture of the rear vehicle at this moment are transmitted to the front vehicle. The positioning of the rear vehicle and the cargo is corrected through the factor graph optimization method, and the optimized rear vehicle state is transmitted to the point cloud map of the rear vehicle to correct the map information.

7. The laser radar-based tracking oversized cargo transport vehicle positioning system according to claim 6 is characterized in that: The calibration unit includes a front vehicle positioning module arranged at the front end of the front vehicle and a rear vehicle positioning module arranged at the rear end of the rear vehicle; The front vehicle positioning module includes three laser radars that perceive surrounding space information, a laser radar that observes the cargo posture, a front vehicle positioning calculation module, an IMU and a communication module. The laser wavelength of the laser radar that observes the cargo posture is 905nm; The rear vehicle positioning module includes two laser radars for sensing surrounding space information, a rear vehicle positioning calculation module, and an IMU and communication module.

8. The laser radar-based tracking oversized cargo transport vehicle positioning system according to claim 7 is characterized in that: The cargo posture estimation unit comprises a supporting device and a characteristic mark arranged on the front end surface of the supporting device, wherein the characteristic mark is arranged as a reflective tape.

9. A terminal device, comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that: The memory stores a computer program that can be run on the processor. When the processor loads and executes the computer program, the laser radar-based following oversized transport vehicle positioning method described in any one of claims 1 to 5 is adopted.

10. A storage medium containing computer executable instructions, characterized in that: The computer executable instructions are used to execute the laser radar-based following oversized transport vehicle positioning method as described in any one of claims 1 to 5 when executed by a computer processor.

Images