Electric heavy truck self-adaptive positioning method and system based on multi-sensor fusion

Abstract

The invention relates to the technical field of electric heavy trucks, and discloses an electric heavy truck self-adaptive positioning method and system based on multi-sensor fusion, and the method is characterized in that a sensor data preprocessing module and a space-time synchronization module are constructed to carry out the preprocessing of data obtained by sensors; performing basic positioning on the electric heavy truck based on an Apriltag label positioning algorithm and a laser slam positioning algorithm; carrying out real-time positioning and correction on the electric heavy truck in a moving state by an Apriltag-assisted laser slam correction algorithm based on ESKF; the stability of a GPS signal in a current environment is automatically detected and analyzed through an adaptive controller module, a fuzzy control selector is adopted to adaptively switch a positioning mode according to a real-time condition, and an Apriltag auxiliary laser slam positioning algorithm based on an ESKF is designed according to the difference of different sensors carried by the electric heavy truck. And then considering the operation requirements of the electric heavy truck in complex environments such as unstable GPS signals in a mining area, designing an adaptive positioning system of the electric heavy truck so as to improve the positioning precision and robustness.

Description

Technical Field

[0001] The present invention relates to the field of electric heavy trucks, and more specifically, to an electric heavy truck adaptive positioning method and system based on multi-sensor fusion. Background Art

[0002] As a clean, low-emission means of transportation, electric heavy trucks have received extensive attention and application in the field of mining transportation. However, in mining transportation, the terrain and building obstructions inside the mining area will cause the attenuation and interruption of GPS signals. High walls, valleys, and huge mechanical equipment will block the arrival of satellite signals, making it impossible for the receiver to obtain enough available satellite signals for positioning. As a result, the GPS positioning system faces problems such as signal obstruction, multipath effect, and positioning error accumulation in mining transportation, and thus cannot meet the needs of accurate and reliable positioning.

[0003] Therefore, the present invention provides an electric heavy truck adaptive positioning method and system based on multi-sensor fusion, which improves the above-mentioned technical problems. Summary of the invention

[0004] The disclosed embodiments are intended to address the deficiencies of the prior art and provide an adaptive positioning method and system for electric heavy trucks based on multi-sensor fusion. The present invention first designs an ESKF-based apriltag-assisted laser slam positioning algorithm in view of the differences in different sensors carried by electric heavy trucks. Then, considering the operating requirements of electric heavy trucks in more complex environments such as unstable GPS signals in mining areas, an adaptive positioning system for electric heavy trucks is designed to improve the accuracy and robustness of positioning. This solves the technical problem that the GPS positioning system faces problems such as signal shielding, multipath effects, and accumulated positioning errors during transportation in mining areas, and cannot meet the requirements for accurate and reliable positioning.

[0005] The above technical objectives of the present invention are achieved through the following technical solutions: an electric heavy truck adaptive positioning method and system based on multi-sensor fusion, comprising the following steps:

[0006] S1, constructing a sensor data preprocessing module and a spatiotemporal synchronization module to preprocess the data acquired by the sensor;

[0007] S2, basic positioning of electric heavy trucks based on apriltag positioning algorithm and laser slam positioning algorithm;

[0008] S3, based on ESKF's apriltag-assisted laser slam correction algorithm, real-time positioning and correction of electric heavy trucks in motion;

[0009] S4, automatically detect and analyze the stability of the GPS signal in the current environment through the adaptive controller module, and use the fuzzy control selector to adaptively switch the positioning mode according to the real-time situation.

[0010] As a preferred technical solution of the present invention, the process of real-time positioning and correction of the electric heavy truck in mobile state by the ESKF-based apriltag-assisted laser slam correction algorithm is as follows: when the apriltag image is captured, the absolute position observed by the apriltag tag is integrated into the odometer data of the laser SLAM through ESKF to correct the SLAM, thereby improving the positioning accuracy of the robot and correcting the accumulated error of the odometer;

[0011] The updating process of the integration is as follows: firstly, the nominal state variable x of the system and the error state variable δx are given, and the input is u m , the noise is i:

[0012]

[0013] Then the ESKF prediction equation can be written as:

[0014]

[0015] F x and F i is the Jacobian matrix of f() with respect to the error and disturbance vectors; Q i is the covariance matrix of the perturbation pulse;

[0016] Among them, the state transfer matrix F x for:

[0017]

[0018] System input Jacobian matrix F i for:

[0019]

[0020] The state transfer covariance matrix is:

[0021]

[0022] Error Kalman filter update steps:

[0023]

[0024] Among them, y is the measurement value, H is the measurement matrix of the error state quantity δx, and since the error state quantity is 0, the nominal state quantity is used as the true value state quantity;

[0025] The measurement matrix H is:

[0026]

[0027] Matrix H x is the Jacobian matrix of the measurement function to the true value state quantity, and its form varies depending on the sensor used; the true value state to error state matrix X δx for:

[0028]

[0029] Simplified, we get:

[0030]

[0031] in,

[0032]

[0033] Update state:

[0034]

[0035] In the error Kalman filter, the covariance matrix P also needs to be reset:

[0036] P←GPG T (3-35).

[0037] As a preferred technical solution of the present invention, the process of automatically detecting and analyzing the stability of the GPS signal in the current environment by the adaptive controller module is as follows:

[0038] Assume that the number of satellites for the main antenna, the horizontal precision factor, and the number of satellites for the secondary antenna are N s , D s and H p , the probability distribution functions are and Then the information entropy can be expressed as:

[0039]

[0040]

[0041]

[0042] Among them, n, h, and d represent the number of satellites of the main antenna, the number of satellites of the secondary antenna, and the value of the horizontal precision factor respectively; the three information entropies are weighted averaged to calculate the information entropy H of GPS positioning. GPS :

[0043]

[0044] Among them, ω1, ω2, and ω3 are the weight coefficients of the number of satellites for the main antenna, the number of satellites for the secondary antenna, and the horizontal precision factor, respectively;

[0045] Finally, the confidence C of GPS positioning is calculated based on the information entropy. GPS , the calculation formula is as follows:

[0046]

[0047] Where N is the number of possible states, which can be determined according to the number of satellites in the main antenna, the number of satellites in the secondary antenna, and the range of the horizontal precision factor;

[0048] When H GPS The smaller it is, the more certain the GPS positioning status is, and the corresponding confidence level C GPS The higher the H GPS The larger the value is, the more uncertain the GPS positioning status is, and the corresponding confidence level C GPS The lower.

[0049] As a preferred technical solution of the present invention, the process of using a fuzzy control selector to adaptively switch the positioning mode according to the real-time situation is:

[0050] The switching of the positioning algorithm is based on the value C calculated by the GPS confidence discrimination model in formula 4-6. GPS When the GPS confidence is greater than the threshold P, RTK-GPS positioning is used. When it is less than the threshold, the positioning algorithm based on lidar slam and visual information fusion is used.

[0051] An electric heavy truck adaptive positioning method based on multi-sensor fusion, comprising a hardware system and a software system;

[0052] The hardware system consists of a 3D laser radar sensor, a GPS sensor, an IMU inertial measurement sensor and a visual camera sensor; the hardware system is used to provide hardware support for the software system;

[0053] The 3D laser radar sensor is used to monitor the application scene environment information and obtain three-dimensional point cloud data; the GPS sensor is used to obtain the carrier position, posture, and speed information; the IMU inertial measurement sensor is used to obtain three-dimensional angular velocity, acceleration, magnetic field information and fused three-dimensional azimuth angle, position and speed information; the visual camera sensor is used to observe the application scene environment information;

[0054] The software system consists of a robot operating system module, an ESKF-based apriltag-assisted laser slam positioning module, and an adaptive controller module; the software system is used to parse the data of the hardware system and complete the adaptive positioning of the electric heavy truck through the operation of the fusion positioning algorithm, and transmit the real-time posture and point cloud map of the electric heavy truck to the terminal display;

[0055] The robot operating system module is used to provide a software framework; the ESKF-based AprilTag-assisted laser slam positioning module fuses the output posture of the AprilTag tag positioning with the posture output by the laser slam through the error state Kalman filter to complete the real-time estimation of the posture of the electric heavy truck robot; the adaptive controller module is composed of a GPS confidence discrimination model and a fuzzy controller model, and is used to switch the positioning mode.

[0056] In summary, the present invention has the following beneficial effects:

[0057] (1) Designed the hardware and software system for electric heavy truck positioning, rationally selected the hardware such as lidar, GPS, IMU, camera, etc., and designed the software system including the adaptive controller module and the ESKF-based apriltag-assisted laser slam positioning module in the ROS environment.

[0058] (2) A multi-sensor fusion positioning algorithm suitable for electric heavy trucks in mining transportation is designed based on ESKF apriltag assisted laser slam positioning. The absolute position of the apriltag observation information obtained by the camera is fused and updated using the error state Kalman filter and the laser slam odometry. Experiments have verified that the designed fusion positioning algorithm is applicable to the scenario and is superior to the separate tag positioning and laser slam positioning algorithms.

[0059] (3) Aiming at the operation requirements of electric heavy trucks in complex environments such as unstable GPS signals in mining areas, an adaptive positioning system for electric heavy trucks is designed. The positioning system includes a sensor preprocessing and time-space synchronization module, a GPS confidence discrimination model, and an adaptive positioning module based on the model using fuzzy control. By improving the positioning system with the adaptive positioning module, the adaptive positioning system for electric heavy trucks designed by the present invention has higher accuracy and robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 A flow chart of an electric heavy truck adaptive positioning method based on multi-sensor fusion provided in an embodiment of the present invention;

[0061] Figure 2 A block diagram of an adaptive positioning system provided by an embodiment of the present invention;

[0062] Figure 3 A schematic diagram of a topic communication mechanism provided by an embodiment of the present invention;

[0063] Figure 4 The ROS node diagram of the ESKF-based apriltag-assisted laser slam positioning module provided in the embodiment of the present invention;

[0064] Figure 5 The ROS node diagram of the adaptive controller module provided by the embodiment of the present invention;

[0065] Figure 6 A working principle diagram of a laser radar provided in an embodiment of the present invention;

[0066] Figure 7 A schematic diagram of a camera imaging model provided by an embodiment of the present invention;

[0067] Figure 8 A schematic diagram of an RTK-GPS measurement model provided in an embodiment of the present invention;

[0068] Fig. 9 The original image and the image processed by adaptive threshold provided by the embodiment of the present invention;

[0069] Fig.10 An image segmented by continuous boundaries provided by an embodiment of the present invention;

[0070] Fig.11 An image of quadrilateral fitting provided by an embodiment of the present invention;

[0071] Fig.12 A schematic diagram of label decoding provided by an embodiment of the present invention;

[0072] Fig.13 The same label provided by the embodiment of the present invention is represented in different viewing angles;

[0073] Fig.14 A schematic diagram of label detection provided by an embodiment of the present invention;

[0074] Fig.15 A schematic diagram of a PNP problem provided by an embodiment of the present invention;

[0075] Fig.16 Apriltag positioning effect diagram provided by an embodiment of the present invention;

[0076] Fig.17 A laser slam algorithm framework diagram provided by an embodiment of the present invention;

[0077] Fig.18 The laser point cloud motion distortion provided by the embodiment of the present invention;

[0078] Fig.19 A schematic diagram of IMU and LiDAR timestamps provided in an embodiment of the present invention;

[0079] Fig. 20 A framework diagram of a local point cloud map model provided by an embodiment of the present invention;

[0080] Fig.21 A geometric feature matching model framework diagram provided by an embodiment of the present invention;

[0081] Fig. 22 This is a laser slam positioning effect diagram provided by an embodiment of the present invention;

[0082] Fig.23 A flow chart of an ESKF-based apriltag-assisted laser slam correction algorithm provided in an embodiment of the present invention;

[0083] Fig.24 The electric heavy truck environment adaptive positioning system framework provided by the embodiment of the present invention;

[0084] Fig.25 A schematic diagram of adaptive switching parameters of a positioning algorithm provided by an embodiment of the present invention;

[0085] Fig.26 This is the adaptive switching process of the positioning algorithm provided in the embodiment of the present invention. DETAILED DESCRIPTION

[0086] The present application is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present application, but do not limit the present application in any form. It should be noted that, for those of ordinary skill in the art, several variations and improvements can also be made without departing from the concept of the present application. These all belong to the protection scope of the present application.

[0087] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0088] It should be noted that, if there is no conflict, the various features in the embodiments of the present application can be combined with each other, all within the scope of protection of the present application. In addition, although the functional module division is performed in the device schematic diagram and the logical order is shown in the flow chart, in some cases, the steps shown or described can be performed in a sequence different from the module division in the device or the flow chart. In addition, the words "first", "second", "third", etc. used herein do not limit the data and execution order, but only distinguish the same items or similar items with basically the same functions and effects.

[0089] Unless otherwise defined, all technical and scientific terms used in this specification have the same meaning as those commonly understood by those skilled in the art to which this application belongs. The terms used in this specification and in the specification of this application are only for the purpose of describing specific embodiments and are not intended to limit this application. The term "and / or" used in this specification includes any and all combinations of one or more of the related listed items.

[0090] In addition, the technical features involved in each embodiment of the present application described below can be combined with each other as long as they do not conflict with each other.

[0091] The disclosed embodiments are intended to solve the problem that the terrain and building obstructions inside the mining area will cause the attenuation and interruption of the GPS signal. High walls, valleys, and huge mechanical equipment will block the arrival of satellite signals, making it impossible for the receiver to obtain enough available satellite signals for positioning, resulting in the GPS positioning system facing problems such as signal obstruction, multipath effects, and positioning error accumulation during transportation in the mining area. In view of this, the disclosed embodiments propose an adaptive positioning method for electric heavy trucks based on multi-sensor fusion. First, according to the differences between different sensors carried by electric heavy trucks, an apriltag-assisted laser slam positioning algorithm based on ESKF is designed. Then, considering the operating requirements of electric heavy trucks in more complex environments such as unstable GPS signals in mining areas, an adaptive positioning system for electric heavy trucks is designed to improve the accuracy and robustness of positioning.

[0092] Please refer to Figure 2 , Figure 2 The framework diagram of the electric heavy truck adaptive positioning system based on multi-sensor fusion described in the embodiment of the present disclosure is shown. The positioning system mainly includes a hardware system and a software system:

[0093] The hardware system consists of 3D lidar sensor, GPS sensor, IMU inertial measurement sensor and visual camera sensor; the hardware system is used to provide hardware support for the software system;

[0094] 3D LiDAR sensors are used to monitor the environmental information of application scenarios and obtain three-dimensional point cloud data. Since the outdoor environment is complex and open, the use of three-dimensional LiDAR can obtain three-dimensional point cloud data to better serve the autonomous positioning and mapping of electric heavy trucks. Therefore, the pandarXT LiDAR is used, which has a detection probability of more than 90% for objects with a reflectivity of 10% at 80m under 100k Lux sunlight. The vertical resolution and point frequency are both twice that of traditional 16-line LiDARs. It can also achieve millimeter-level accuracy (1σ) in zero blind spots and core ranging range.

[0095] The GPS sensor is used to obtain the carrier's position, attitude, and speed information; the GPS sensor uses the CGI-410GPS sensor, which has a built-in high-precision MEMS gyroscope and accelerometer, and uses multi-sensor data fusion technology to combine satellite positioning with inertial measurement, and can provide high-precision carrier position, attitude, speed, and sensor information in real time. If this sensor is installed on the patrol robot, the robot's centimeter-level accuracy position can be obtained.

[0096] The IMU inertial measurement sensor is used to obtain three-dimensional angular velocity, acceleration, magnetic field information, and fused three-dimensional azimuth angle, position and speed information. Since the sensor used in the electric heavy truck system is a three-dimensional laser radar, the IMU is required to provide the direction of gravity acceleration to ensure that the three-dimensional laser radar can be used normally; therefore, the YIS500 inertial measurement sensor is used. This type of inertial measurement sensor is installed on the patrol robot, which can measure the acceleration information of the robot more accurately, and can use the magnetometer to measure the direction of gravity acceleration at the current position, providing prior information for the three-dimensional laser radar.

[0097] Visual camera sensors are used to observe environmental information in application scenarios. Since the outdoor environment is complex and open, cameras can be used to obtain richer visual information and better assist electric heavy trucks in completing autonomous positioning tasks. Therefore, the imx390 camera is used, which has 1080p resolution, 30 frames per second, low noise, and a dynamic sensitivity of up to 120dB. Even in an environment below 0.1 lux, it can capture high-quality color images.

[0098] The software system consists of a robot operating system module, an ESKF-based apriltag-assisted laser slam positioning module, and an adaptive controller module. The software system is used to analyze the data of the hardware system and complete the adaptive positioning of the electric heavy truck through the calculation of the fusion positioning algorithm, and transmit the real-time position and point cloud map of the electric heavy truck to the terminal display.

[0099] The Robot Operating System (ROS) module is used to provide a software framework; based on the distributed and loosely coupled characteristics of the ROS system, the software system can be distributed and run on the robot platform and remote terminals. The platform computing center is responsible for sensor data analysis, positioning and mapping algorithm operation, while the remote terminal controls the robot's movement while displaying the robot's position and point cloud map in real time.

[0100] The ROS topic communication mechanism is used to achieve effective transmission of software system related data. ROS defines each process that executes a task as a node. The topic mechanism can realize asynchronous communication between nodes. It is suitable for data communication with continuous updates and less logical processing, and is mainly composed of publishers and subscribers. Figure 3As shown in the figure, the publisher node defines a topic of a certain message type and broadcasts it. After the subscriber confirms the topic name and message type, it subscribes to the topic and receives the message. This mechanism allows multiple subscribers to subscribe to the same topic. LiDAR and IMU data need to be subscribed by multiple modules at the same time, so the topic mechanism is used for data communication.

[0101] ROS provides convenient and practical development tools, including visualization tool rviz, data recording tool rosbag, and node display module rqt tool. rviz is a visualization tool that displays the original laser point cloud, point cloud map and robot posture in the operation interface; rosbag can save point cloud map, trajectory, and original sensor data, which is convenient for debugging and running algorithms; rqt tool can view the connection relationship between nodes and the flow of message transmission.

[0102] The ESKF-based apriltag-assisted laser slam positioning module fuses the output pose of apriltag label positioning with the pose output by laser slam through the error state Kalman filter to achieve real-time estimation of the pose of the electric heavy truck robot. The node diagram of this module in ROS is as follows: Figure 4 As shown, it includes laser slam node, apriltag node and error state Kalman filter pose estimation node.

[0103] The ellipse represents the node that performs the task, and the nodes establish data transmission based on the topic. The node lidar slam_node is responsible for predicting the posture of the electric heavy truck using IMU data, then using the posture predicted by IMU to perform motion compensation on lidar, extract effective feature points, save a small amount of point cloud with obvious feature information, and finally fuse the imu prediction data with the laser feature points to estimate the posture, and establish a local map to output the estimated posture / lidar_pose. The node Apriltag_node is responsible for obtaining the global posture information of the unmanned vehicle. Since the apriltag tag includes the GPS information of the tag and the corresponding Euler angle data, the translation matrix and rotation matrix between the unmanned vehicle and the tag at the current moment can be obtained by detecting the tag, and then the posture information of the current vehicle can be obtained based on the rotation and translation relationship and the posture / apr_pose can be output. The node ESKF_node is responsible for fusing the absolute position in the observation information of the apriltag tag obtained by the camera with the laser slam odometer using the error state Kalman filter and updating it, and outputting the posture / filter_pose after fusion filtering.

[0104] The adaptive controller module includes the GPS confidence discrimination model and the fuzzy controller model. The node diagram of this module in ROS is as follows: Figure 5As shown, it includes confidence_node node and fuzzy control_node node.

[0105] The ellipse represents the node that performs the task, and the nodes establish data transmission based on the topic. The node confidence_node is responsible for using the GPS data to design the GPS confidence discrimination model using the entropy method to output the GPS confidence value / confidence. The node fuzzy control_node is responsible for inputting the current positioning mode, GPS confidence value and duration into the fuzzy controller to determine whether the positioning mode needs to be switched.

[0106] Please refer to Figure 1 , Figure 1 The flowchart of the electric heavy truck adaptive positioning method based on multi-sensor fusion according to the embodiment of the present disclosure is shown. The overall process mainly includes the following 4 steps:

[0107] S1. Construct a sensor data preprocessing module and a spatiotemporal synchronization module to preprocess the data acquired by the sensor.

[0108] The sensor data preprocessing module includes: three-dimensional laser observation model, camera imaging model, and GPS positioning model.

[0109] S11. Three-dimensional laser observation model.

[0110] In the field of autonomous driving, LiDAR sensors are widely used due to their high reliability, strong stability, and high resolution. LiDAR sensors emit a laser source and record the time from the infrared laser emitted by the laser to the light reflected by the scanned object. Based on the propagation speed of the laser, the distance between the light source and the object being measured can be calculated. The working principle diagram is as follows: Figure 6 shown.

[0111] The laser radar rotates 360° to collect data during operation, and one frame of point cloud data can be obtained for each rotation. According to the rotation mode, it can be divided into mechanical laser radar and solid-state laser radar. The 32 laser beams emitted by the pandarXT laser radar are evenly distributed from -16° to 15° in the vertical viewing angle of 31°. At an acquisition frequency of 10hz, the horizontal angular resolution is 0.18°, the minimum detection distance is 0.05m, the maximum detection distance can reach 120m, and the detection probability of sensing an object with a reflectivity of 10% at 80m under 100k Lux sunlight exceeds 90%. And it can achieve millimeter-level accuracy (1σ) in zero blind spots and core ranging range.

[0112] S12, camera imaging model.

[0113] The imaging of the camera is based on the principle of pinhole imaging. A point in space is projected onto a pixel on the image. The reference coordinate systems involved in this process are: world coordinate system, camera coordinate system, and image coordinate system. The movement of the camera can be regarded as the change in the position of the camera coordinate system relative to the world coordinate system. The imaging of the camera also has a transformation matrix from the camera coordinate system to the image coordinate system.

[0114] The pinhole imaging model of the camera, as well as the schematic diagrams of the world coordinate system, camera coordinate system, and image coordinate system during the imaging process are shown in the figure below. Figure 7 shown.

[0115] Usually use the translation vector and the rotation matrix Describes the transformation relationship between the camera coordinate system and the world coordinate system. Represents the value of the origin of the camera coordinate system relative to the world coordinate system. Represents the orthogonal rotation transformation of the camera coordinate system relative to the world coordinate system.

[0116] Assume that the coordinates of the space point A in the world coordinate system and the camera coordinate system are [x w ,y w ,z w ],[x c ,y c ,z c ], then there exists the following homogeneous transformation relationship:

[0117]

[0118] H is called the camera's external parameter matrix, which is determined by the camera's installation position and posture in the robot. Assume that the focal length of the camera, that is, the distance from the origin of the camera plane to the origin of the image plane, is f. According to the triangle similarity principle, point A[x c ,y c ,z c The projection position A′[x′, y′] of the image plane satisfies the following relationship:

[0119]

[0120] Assume that the pixel coordinates of point A′ are (u, v), the scaling ratios of the pixels on the x and y axes are a and b, and the image origin translation is (c x ,c y ), then the coordinates of the pixel point satisfy the following relationship:

[0121]

[0122] Substitute equation 2-3 into equation 2-2, and let f x =af,f y= bf, we get the following coordinate equation:

[0123]

[0124] The K matrix is ​​called the camera intrinsic parameter matrix, which needs to be obtained through calibration.

[0125] Combining equations 2-1 and 2-4, we can see that the point [x w ,y w ,z w ] is projected onto the pixel point (u,v) on the image. The process satisfies the following relationship:

[0126]

[0127] S13, GPS positioning model.

[0128] The GPS positioning system is usually composed of satellites in the air and receiving stations on the ground. The receiving stations can calculate the longitude, latitude, and altitude of the receiving stations by simultaneously receiving and calculating the distance between the signals transmitted by multiple satellites and the user's receiver.

[0129] RTK, or carrier phase differential technology, is a commonly used high-precision dynamic positioning technology. In order to improve the positioning accuracy of GPS, it is generally combined with RTK technology, that is, using a fixed base station on the ground to assist in positioning calculations. The specific solution process of RTK is as follows: the base station collects data transmitted by the satellite, and transmits its observation values ​​and base station coordinate information to the GPS mobile station through data channels such as radio stations and 4G modules. The GPS mobile station obtains the positioning result by performing real-time solution on the collected satellite data and the data received from the base station. RTK-GPS positioning system, such as Figure 8 shown.

[0130] S14. Data preprocessing.

[0131] Since the robot operating system module is used to provide the software framework, it is necessary to extract the required information from the raw output data of IMU, GPS, camera, and lidar, and convert it into a form that conforms to the ROS communication format.

[0132] Specifically: for each frame of sensor data, identify the frame header and frame tail identifiers, then perform byte cutting on the data stream in a frame according to the specific identifier, finally extract the required information and convert it into the format of topic in ROS for publication. The raw GPS data conforms to the standard NMEA-0183 format, refer to the ROS open source library nmea navsat driver for format parsing, and then extract key data. The camera uses its own driver code library to extract the camera's raw image. The IMU uses a serial port reading and parsing program to communicate with the lower computer. Finally, use the rqt graph tool in ROS to view the structure diagram of the sensor data preprocessing nodes and published messages (topics).

[0133] The space-time synchronization module includes: time synchronization and space synchronization.

[0134] S15. Time synchronization.

[0135] The data release frequencies of LIDAR, IMU, GPS, and cameras used in electric heavy trucks are different. The release frequency of LIDAR is 20HZ, the release frequency of IMU is 200Hz, the release frequency of GPS is 1Hz, and the release frequency of camera is 30Hz. Therefore, time synchronization is required before multi-sensor data fusion.

[0136] Based on data preprocessing, it can be seen that the LIDAR, IMU, GPS, and camera data released by ROS all contain timestamps (time_stamp).

[0137] Specifically: For sensor data that need to be fused, such as LIDAR and IMU, they are placed in the same ROS node, and the data topics of LIDAR and IMU are received in the node. The data with a faster frequency is first stored in the ROS stack, and then the received LIDAR and IMU data are passed to the same callback function, and then time synchronization processing is performed in the callback function, and finally the thread of the multi-sensor fusion algorithm is entered.

[0138] The time synchronization processing method designed in the callback function is: for the lidar slam fusion positioning algorithm, in the lidar slam callback function, the timestamp of the LIDAR is used as the benchmark. When formula 4-1 is satisfied, this group of GPS and IMU is considered to be time-synchronized data, and only then does it enter the thread of the GPS / IMU fusion algorithm.

[0139] |t LIDAR，i -t IMU，j |<0.01s (4-1)

[0140] Similarly, the apriltag tag identification and positioning algorithm, in the GPS / vision callback function, takes the GPS timestamp as the reference. When formula 4-2 is satisfied, this set of GPS and camera data is considered to be time-synchronized data. Only then does it enter the apriltag tag identification and positioning algorithm.

[0141] |t GPS，i -t CAMERA，j |<0.01s (4-2).

[0142] S16, spatial synchronization.

[0143] Due to the installation position, angle and other reasons of different sensors, the reference coordinate systems of the raw data released do not completely overlap, so spatial synchronization between sensors is required. The present invention needs to consider the sensor spatial synchronization of GPS / IMU fusion positioning and IMU / vision fusion positioning respectively.

[0144] In the GPS / IMU fusion positioning algorithm, GPS releases longitude and latitude data, which is based on the WGS-84 ellipsoidal coordinate system as a reference system, while the data released by IMU is based on the Cartesian coordinate system as a reference system. Therefore, it is necessary to convert the GPS data into an xyz coordinate system first. The method used in the present invention is: converting GPS from the WGS-84 coordinate system to the UTM (Universal Mercator Coordinate System), the UTM coordinate system is a coordinate system in which the xyz axes are mutually perpendicular to each other, just like the Cartesian coordinate system, wherein xyz corresponds to the east, north, and vertical ground upward directions, respectively, and the first frame of GPS data received is used as the UTM coordinate origin. Since GPS and IMU only have an offset of 10cm in the y direction, after the IMU performs offset compensation, it can also be used as the origin for mileage mapping with the UTM coordinate origin as the origin. So far, the coordinate systems of GPS and IMU have been unified, that is, the spatial synchronization of GPS and IMU has been achieved.

[0145] In the IMU / vision fusion positioning algorithm, there is a position and angle offset between the IMU and the binocular camera, so the spatial synchronization of the IMU and the camera is to find their position transfer matrix. This application uses the Kalibr tool to calibrate the IMU and binocular camera, that is, use the binocular camera of the spherical robot to shoot the checkerboard, and move the spherical robot in all directions to fully activate the IMU, and then construct an optimization problem containing the reprojection error term and the IMU error term to obtain the external parameters between each sensor.

[0146] S2. Perform basic positioning of electric heavy trucks based on the AprilTag positioning algorithm and the laser SLAM positioning algorithm.

[0147] S21, apriltag label positioning algorithm.

[0148] Tag detection can obtain the global position information of the unmanned vehicle. Since the prior tag includes the GPS information of the tag location and the corresponding Euler angle data, the translation matrix and rotation matrix of the unmanned vehicle relative to the tag at the current moment can be obtained by detecting the tag, and then the current vehicle's position information and GPS coordinates can be obtained based on the rotation and translation relationship.

[0149] The Apriltag algorithm based on the visual benchmark library is an electric heavy truck positioning algorithm. Label detection and recognition technology is an important component of label positioning. During the movement of the camera, the label may appear at any position in the camera's field of view, and is often accompanied by large rotations and offsets, and may also be interfered with by other visual features in the field of view. Therefore, finding the correct label boundary is of great significance to label positioning.

[0150] S211, adaptive threshold.

[0151] The Apriltag positioning algorithm uses an adaptive threshold method and uses different thresholds for different scenarios. The input image is grayed out, the image is divided into fixed pixel blocks of 4*4 size, and the minimum and maximum values ​​in each pixel block are calculated. In order to prevent artifacts from being formed by excessive differences in the extreme values ​​of the boundary pixel blocks, the extreme values ​​of each pixel block are determined by the nearby 3x3 pixel blocks. The average of the calculated maximum and minimum values ​​is used as the threshold, and the pixels in the image below this threshold are set to a black value of 255, and those above the threshold are set to a white value of 0, thereby achieving binarization, such as Fig. 9 shown.

[0152] S212: Continuous boundary segmentation.

[0153] Use the union-find algorithm to segment the connected domains of light and dark pixels, and assign a unique ID number to each connected domain. By using a hash table to efficiently complete clustering, the elements in each cluster can be indexed by the ID number of the connected domain, thereby achieving boundary segmentation. The effect after segmentation is as follows Fig.10 At the same time, the same white pixel is allowed to appear in the two generated clusters, thus solving the single-pixel width problem mentioned earlier that cannot separate two different connected domains.

[0154] S213, quadrilateral fitting.

[0155] Next, quadrilateral fitting is performed on the disordered boundary points in each cluster. In order to improve computational efficiency and speed, first, an approximate segmentation is calculated by finding a small number of corner points and then traversing all possible corner point combinations. The contour points are sorted by angle around their center in the same rotation direction. Through this sorting, straight line fitting is performed on the points within a relative range from the center point. The contour point clusters of all clusters are traversed, and the peak of the mean square error function is selected to identify the corner points. The principal component analysis method is used for straight line fitting. Finally, a combination of corner points that can form a quadrilateral is found in the corner point set, and straight line fitting is performed on all sides of the candidate quadrilateral, and the four corner points with the smallest straight line fitting error are selected as the final corner points.

[0156] S214: Decoding the label.

[0157] After detecting the quadrilaterals in the image, the next step is to decode the fitted quadrilaterals, match them with the label encoding library, and remove the quadrilaterals that do not meet the conditions. Fig.11 As shown, when decoding the tag, first check whether there is a black square frame on the outer layer, and then read the square of the inner dot matrix to see if there is a valid coding area. According to different encoding methods, AprilTag is divided into different types, and the number of dot matrices in its graphic structure is different. The image pixel value is compared with the specified threshold. If it is greater than the threshold, the corresponding area on the tag is 1, and if it is less than the threshold, the corresponding area on the tag is 0. In order to increase the robustness to lighting, the threshold model shown in Formula 3-1 is defined. The threshold model of black pixels and the threshold model of white pixels are established respectively. The threshold model is learned using known labels containing black and white pixels. The model parameters can be calculated using least squares regression. The threshold used when decoding the tag data bit is the average value of the predicted values ​​of the black and white threshold models.

[0158] I(x,y)=Ax+Bxy+Cy+D (3-1)

[0159] In the embodiment, if the side length of Tag16h5 is 6 black squares, the border composed of the outermost black squares is removed, and the effective information area is a 4x4 part, which contains both black squares and white squares and is used to store other beacon information. Fig.12 As shown, during the experiment, artificial beacons were identified and their dot matrix and effective coding area were divided.

[0160] There are four cases when the image is corrected to positive projection through perspective transformation during decoding, such as Fig.13As shown in the figure, they correspond to 0°, 90°, 180°, and 270° respectively. Therefore, for the binary code obtained after decoding, it is first rotated 90° three times, and then the code before rotation and the code after rotation are matched with the code library in turn. When it is observed that the Hamming distance between the code and a code in the code library is less than the user-specified threshold, it is considered that the detected quadrilateral belongs to the AprilTag identification, and the coded ID and the corresponding rotation direction are recorded. The final detection effect is as follows: Fig.14 shown.

[0161] S215, label pose calculation.

[0162] After obtaining the correct boundary of the label, we combine the camera’s intrinsic parameter model to construct Fig.15 As shown in Figure 1, the PNP problem is solved using direct linear transform (DLT).

[0163] In the figure, p1, p2, p3, and p4 are four corner points in the label coordinate system with the label center as the origin, the adjacent sides of the label rectangle as the x-axis and the y-axis, and the z-axis perpendicular to the label plane. Their coordinate values ​​can be calculated from the actual physical distance of the label. p1′, p2′, p3′, and p4′ are the corresponding positions projected onto the imaging plane. For a pair of matching points, according to the camera model described above, we can get:

[0164]

[0165] in,

[0166]

[0167] Because the four corners of the label are coplanar with the xy axis plane of the label coordinate system, z w is always equal to 0, so it can be sorted into:

[0168]

[0169] make Expand to get:

[0170]

[0171] Eliminating s gives:

[0172]

[0173] Arranged into matrix form, we can get:

[0174]

[0175] Because KH and H are equivalent, and h33 =[0,0,1][t x , t y , t z ] T =t s is not 0, so we can set h 33 =1, then there are 8 unknowns in formula 3-4. Each feature point can provide 2 constraint equations, and four pairs of matching points can be used to calculate the H matrix, thereby obtaining the first two columns of the scaled rotation matrix R and the translation vector t. Since each column of the rotation matrix is ​​a unit vector, and the tag must appear in front of the camera, the scale of the H matrix can be obtained, thereby obtaining the first two columns of the actual rotation matrix and the translation vector. At the same time, R is an orthogonal matrix, and its third column can be obtained by cross-producting the first two columns, that is, r3=r1×r2. At this point, the six-degree-of-freedom pose of the tag coordinate system relative to the camera coordinate system can be obtained. Since the apriltag tag includes the GPS information of the tag and the corresponding Euler angle data, the translation matrix and rotation matrix between the unmanned vehicle and the tag at the current moment can be obtained by detecting the tag, and then the current vehicle's pose information can be obtained based on the rotation and translation relationship. Its positioning effect is as follows Fig.16 shown.

[0176] S22, laser slam positioning algorithm.

[0177] S221, laser slam algorithm framework.

[0178] LiDAR can obtain accurate 3D point cloud data, and IMU can estimate the pose more accurately in a short time. By combining the advantages of both, a more accurate and robust laser slam positioning algorithm is designed. IMU measurement is used to remove point cloud distortion caused by rapid motion, and the predicted pose of IMU is used as the initial value of point cloud matching, thereby improving the accuracy of pose estimation. Fig.17 This is the framework diagram of the laser SLAM algorithm, which consists of two parts: laser point cloud preprocessing and pose estimation based on feature point cloud matching.

[0179] Laser point cloud preprocessing, using IMU measurement values ​​to remove point cloud distortion caused by robot motion, using LOAM-based geometric feature extraction method to extract edges and plane points, and determining the key frame selection strategy, and establishing a local map to store feature points to complete point cloud preprocessing. Pose estimation based on feature point cloud matching, using the integral value of IMU measurement as the initial pose of point cloud matching, realizing data association between feature points and local point cloud maps, constructing and solving optimization problems with change matrices as variables, obtaining the car pose, and using this pose to update local maps.

[0180] S222, laser point cloud preprocessing.

[0181] Laser point clouds are large, disordered, and noisy. They will be distorted when moving quickly. If they are used directly to establish data association, the speed and accuracy of point cloud matching will be affected. Motion compensation is performed on the initial point cloud to eliminate distortion and extract effective feature points to improve the accuracy of point cloud matching. The specific process is as follows:

[0182] S2221, laser point cloud motion distortion removal

[0183] The laser radar scanner needs to rotate one circle to obtain a complete frame of point cloud data. Fig.18 As shown in the figure, if the system is stationary, the coordinate origin of a frame of point cloud is the same. When the system is in motion, the real coordinate origin of each point is constantly changing, resulting in motion distortion of the point cloud. When the system moves faster or rotates, the point cloud distortion is particularly serious. In order to ensure the mapping effect and positioning accuracy of the laser SLAM algorithm, the laser point cloud needs to be distorted.

[0184] In order to remove point cloud distortion, it is necessary to obtain the position change of the laser radar during each frame scanning process and convert the laser point to the same coordinate system. Since the IMU measurement frequency can reach 200Hz, which is 20 times that of the laser radar, it can accurately reflect the radar position change in a short time. Therefore, this paper uses IMU data to dedistort the laser point cloud. The state of the laser radar is positioned as:

[0185] X i =[p i , v i ,q i , b a T , b ω T ] T

[0186] Where X i Represents the state of the robot, p i is the position of the robot in the world coordinate system, v i is the speed of the robot in the world coordinate system, q i is the posture of the robot in the world coordinate system, b a is the acceleration drift of IMU, b ω is the angular velocity drift of the IMU.

[0187] The state quantity of the mobile robot in the jth frame is obtained by discretizing the IMU data and the state quantity of the i-th frame. The calculation formula is:

[0188]

[0189]

[0190]

[0191] in, g w is the gravity vector in the current state, is the linear acceleration vector, is the angular velocity vector. In order to reduce the number of integration times, IMU pre-integration is used to calculate the current state change. The calculation formula is as follows:

[0192]

[0193]

[0194]

[0195] The frequency of LiDAR is much lower than that of IMU, so there will be multiple IMU data between two LiDAR frames, such as Fig.19 Assume that the timestamps corresponding to the two frames of lidar data are t k and t k+1 In this paper, the robot is calculated by pre-integration at t k to k+1 Relative pose change over time Assume that the robot is at t∈[t k , t k+1 ] time to make a uniform motion, based on which the linear interpolation can be used to obtain the position transformation of any laser point relative to the first laser point. The calculation formula is:

[0196]

[0197] Among them, t i is the time of the i-th laser point in the laser point cloud, T i is the position transformation matrix of the i-th laser point relative to the first laser point, and then the current laser point is converted to the coordinate system corresponding to the first laser point. The calculation formula is:

[0198] h′ i =T i h i (3-12)

[0199] where h i is the position of the laser point before distortion correction, h′ i To determine the position of the laser point after distortion correction, each laser point in the laser point cloud is transformed in position and posture, thus completing the dedistortion of the laser point cloud.

[0200] S2222. Construct a local map based on key frames.

[0201] In the limited moving distance of electric heavy trucks, most of the point clouds scanned at adjacent moments are repeated. If a global map is constructed to store all point clouds, it will definitely take up a huge amount of memory. Therefore, referring to the key frame strategy of the visual odometer, key frames are selected by judging the change in posture, and a small number of key frame point clouds are retained as the global map. Assume that the robot posture corresponding to the i-th key frame is X i ; When the robot is relative to X i When the posture change exceeds the set threshold, a new laser radar frame P is selected i+1 Save as a key frame, the corresponding pose is X i+1 The non-critical LiDAR frames between two key frames are discarded. By extracting key frames, the memory resource consumption is reduced while ensuring the map density. The position change Δp and rotation change thresholds Δθ are set to 0.5m and 0.2° respectively.

[0202] Most traditional SLAM algorithms use the method of searching for an area similar to the latest point cloud in the global map, and selecting the point cloud in this area to establish data association with the latest frame of point cloud. The amount of data association calculation and the misassociation rate of this method will increase significantly with the increase of map size, and the real-time and accuracy of pose estimation cannot be guaranteed in large-scale outdoor scenes. Therefore, a sliding window-based method is used to establish a local point cloud map containing a small number of key frames, and point clouds closer to the robot are used for data association, which reduces the amount of calculation and improves the matching accuracy. Fig. 20 For the local point cloud map model based on the sliding window, the key frames within 50m near the current robot position are searched and saved with the local map. As the robot moves, the point clouds far away from the current robot are eliminated and the latest key frames are added.

[0203] S223, pose estimation based on point cloud matching. The specific process is:

[0204] S2231. Initial pose prediction.

[0205] When a new frame of laser point cloud is received, it needs to be converted to the global coordinate system using the initial pose, and then the data association between the current frame and the local map laser point is established. LOAM uses the uniform motion theory to provide the initial pose for the odometer, that is, assuming that the mobile robot moves at a uniform speed, the pose at the next moment is calculated by interpolating the pose at the previous moment, and the current point cloud is converted to the global coordinate system at the previous moment. This method can achieve good results when the mobile robot is running at a uniform speed, but the outdoor environment is complex and changeable, and the mobile robot often cannot run smoothly, so it cannot provide a stable and good initial value for point cloud matching. The acquisition time of the two frames of point cloud differs by 0.1S, and the IMU has high measurement accuracy in a short time. Therefore, the integral value of the IMU measurement value is used to provide the initial pose for point cloud matching.

[0206] Assume that the posture at time i is The change in posture from time i to time k is obtained by integrating the IMU data, ΔT ik Including the position and posture changes {ΔX, ΔR}, the posture T′ of the mobile robot at time K is R，K for:

[0207] T′ R，K =T R，K ΔT ik

[0208] Multiply the coordinates of the current frame point cloud by T′ R，K The coordinates in the global coordinate system can be obtained, and the optimization formula can be constructed through point cloud matching to iteratively solve the robot posture T R，K .

[0209] S2232: Geometric feature point matching.

[0210] Find the edge and plane feature points corresponding to the current point cloud in the local map for matching to improve the accuracy and efficiency of point cloud matching. Fig.21 As shown in the figure, in the local map, adjacent edge feature points are fitted into straight lines, and the distance from the point to the straight line is calculated to achieve edge point matching. Adjacent plane feature points in the local map are fitted into planes, and the distance from the point to the plane is calculated to achieve plane point matching, and the corresponding residual formula is constructed for the subsequent pose solution.

[0211] Edge point matching is achieved by calculating the distance from the point to the line. i+1 The IMU predicts the pose to the global coordinate system. Let the laser point k be P i+1 An edge feature in the local edge point cloud map is searched using the kd-tree radius search algorithm. Get the points u and v closest to point k, calculate the distance from k to the straight line where points u and v are located, and construct the residual formula d e :

[0212]

[0213] Similarly, assuming k is P i+1 A plane feature point is found in the local plane point cloud map through the kd-tree radius search algorithm Search for the nearest point v to point k, and fit the nearest points u and w to v into a plane, calculate the distance from k to the fitting plane, and construct the distance residual d from the point to the plane p :

[0214]

[0215] By calculating the distance between point and line, point and plane, point-line and line-plane constraint information is constructed, and data association between geometric feature points is realized.

[0216] S2233, posture solution.

[0217] Point cloud matching pose estimation is important. The robot pose is solved using laser observation data and IMU measurement values. When a new point cloud is received, the pose obtained by IMU integration is linearly interpolated, and a frame of point cloud is converted to the scanning starting point. The motion distortion of the point cloud is removed, and the edge and plane feature points are extracted according to the curvature information. By judging whether the current key frame map is empty, it is determined whether the frame is the first frame of point cloud. If it is the first frame of point cloud, it is directly added to the key frame point cloud map and waits for subsequent point cloud frames to be added. If it is not the first frame of point cloud, a kd-tree nearest neighbor search tree of the saved key frame point cloud is constructed. The search tree is used to extract several key frames within 50m of the current pose range to construct a local map, and the map is downsampled. The most relevant feature points of the latest point cloud are searched in the local point cloud, the corresponding geometric constraints are constructed, and the Levenberg-Marquardt method is used to solve and obtain the optimal pose of the robot. It is determined whether the frame meets the key frame judgment conditions. If it meets the key frame judgment conditions, it is added to the key frame map. If not, the next frame is read and looped.

[0218] After completing the time and space alignment of the IMU and LiDAR data, the robot's position and posture are calculated by establishing a data association between the current point cloud and the feature points in the map. Fig. 22 shown.

[0219] S3. The ESKF-based apriltag-assisted laser slam correction algorithm performs real-time positioning and correction of electric heavy trucks in motion.

[0220] Due to sensor noise and imperfect motion models, the positioning error of laser SLAM will gradually accumulate. In order to improve the positioning accuracy, the error state Kalman filter (ESKF) can be used to integrate the absolute position observed by the AprilTag tag into the SLAM odometer data to correct the laser SLAM, thereby obtaining a more accurate robot positioning estimate. The specific process is:

[0221] S31, error state Kalman filter algorithm.

[0222] Since the motion model and measurement model of the mobile robot are nonlinear, the Error-State Kalman Filter (ESKF) algorithm is used to deal with nonlinear situations. First, the Taylor series expansion is used to approximate the formula of the smooth function with a polynomial function, and then the Kalman filter framework is used for filtering to complete the state estimation of the target.

[0223] ESKF assumes that the system state transition probability and measurement probability are controlled by nonlinear functions g and h, respectively, so the state prediction equation and measurement equation of the system are obtained as follows:

[0224]

[0225] Among them, x t is the state at time t, x t-1 is the state at time t-1, u t is the control of t, ε t The mean is 0 and the variance is R t The motion noise, z t is the observed value at time t, δ t The expected value is 0 and the variance is Q t of measurement noise.

[0226] ESKF uses the first-order Taylor expansion method to linearize the nonlinear function. The linearization is approximated by a linear function that is tangent to the nonlinear function at the mean of the Gaussian function. The state of the function g is expressed by its posterior mean μ t-1 The value at is approximate, adding μ t-1 and u t Taylor expansion is performed on the slope tangent to g at , and g is linearized to obtain:

[0227] g(u t , x t-1 )≈g(u t , μ t-1 )+g(u t , μ t-1 )′(x t-1 , μ t-1 ) (3-16)

[0228] The calculation is as follows (3-17):

[0229]

[0230] Among them, G t is an n×n matrix, where n is the dimension of the state, G t is the Jacobian matrix, G t The value of u depends on t and μ t-1 .

[0231] Linearize the measurement function h, Carry out Taylor expansion at, and we get:

[0232]

[0233] Among them, H tis the Jacobian matrix, which is the prediction of the state at time t Calculation, the calculation form is as follows (3-19):

[0234]

[0235] ESKF is mainly divided into two parts: prediction and update, which are mainly composed of the following five equations:

[0236] Status prediction:

[0237]

[0238]

[0239]

[0240] Measurement Update:

[0241]

[0242]

[0243] in, is the mean of the state predictions obtained from the equations of motion, is the predicted covariance, K refers to the Kalman gain, which indicates the degree of correction to the predicted value. The smaller the Kalman gain, the more trust the motion model has, and the smaller the correction to the predicted mean, and vice versa. t is the optimal a posteriori estimate of the state at time t, z t represents the true measurement, represents the predicted measurement, ∑ t refers to the covariance of the posterior state and I refers to the identity matrix.

[0244] The main process of ESKF is the continuous iteration of the above five equations, starting from the posterior state bel(x t-1 ) and the current moment control can get the predicted state at the current moment, and then the predicted state is corrected by the current moment measurement to get the current final posterior estimate bel(x t ), and repeat the iterations continuously.

[0245] S32, system modeling of laser slam and apriltag fusion.

[0246] ESKF is used to fuse the laser slam and apriltag. The fusion steps are as follows:

[0247] S321, fusion positioning prediction.

[0248] First, the nominal state variable x of the system is given, the error state variable δx, and the input is u m , the noise is i:

[0249]

[0250] Then the ESKF prediction equation can be written as:

[0251]

[0252] F x and F i is the Jacobian matrix of f() with respect to the error and disturbance vectors; Q i is the covariance matrix of the perturbation pulse. The expressions for the Jacobian and covariance matrices above are detailed below. All state-related values ​​appearing here are extracted directly from the nominal state.

[0253] Among them, the state transfer matrix F x for:

[0254]

[0255] System input Jacobian matrix F i for:

[0256]

[0257] The state transfer covariance matrix is:

[0258]

[0259] S321: Fusion positioning update.

[0260] Error Kalman filter update steps:

[0261]

[0262] Among them, y is the measurement value, H is the measurement matrix of the error state quantity δx, and since the error state quantity is 0, the nominal state quantity is used as the true value state quantity.

[0263] The measurement matrix H is:

[0264]

[0265] Matrix H x The Jacobian matrix of the measurement function to the true value state quantity is different depending on the sensor used. The true value state to error state matrix X δx for:

[0266]

[0267] Simplified, we get:

[0268]

[0269] in,

[0270]

[0271] Update state:

[0272]

[0273] In the error Kalman filter, the covariance matrix P also needs to be reset:

[0274] P←GPG T (3-35)

[0275] S33, implementation steps of the ESKF-based apriltag-assisted laser slam correction algorithm.

[0276] The absolute position of the observation information of the AprilTag obtained by the camera is fused and updated using the extended Kalman filter and the laser SLAM odometer to achieve real-time positioning and correction of the mobile electric heavy truck.

[0277] The specific process is:

[0278] First, in an environment that meets the laser radar observation conditions, when the apriltag image is captured, the algorithm fuses the absolute position of the apriltag label observation into the laser SLAM odometer data through ESKF to correct the SLAM, thereby improving the robot's positioning accuracy and correcting the odometer cumulative error. For scenes with repeated environments and similar textures, the artificial landmark information obtained by the camera can be flexibly used as observation information to make up for the lack of laser radar light divergence, and the robot odometer predicted posture is discretely corrected online. Through the process of estimation, observation, and correction, the robot's corrected posture information is closer to the real robot's global posture information.

[0279] Set the robot's initial position to x t-l , the camera reads data and sets it to x c , and use both as input information for the motion model sampling process. When the robot does not capture the AprilTag during motion, the motion model position x based on the laser radar observation information t ; When the camera acquires the apriltag image, the absolute value of the error covariance E of the current frame is greater than the artificially set critical value ε. Let ε be (3, 3) T , then the sampling algorithm outputs the value stored in the apriltag coordinate xc , and let the current position x t The pose provided by the apriltag information continues to be sampled until the next apriltag is scanned, thereby achieving continuous resampling of apriltag and fusion positioning of the robot.

[0280] The apriltag assisted laser slam correction algorithm process based on ESKF is as follows: Fig.23 As shown in the figure: the location information stored in apriltag is used to correct the odometer, and the corrected measurement value is then input into the system's motion state equation for motion sampling.

[0281] S34. Calculation of positioning indicators.

[0282] Using the EVO evaluation tool, the RMSE (root mean square error) calculation formula is:

[0283]

[0284] n represents the number of experiments, λ i represents the result obtained by the positioning algorithm, γ i Indicates the actual true value. Since the spherical robot performs closed-loop rectangular motion in both scenes, that is, the actual motion returns to the starting point, the true value of the distance between the starting point and the end point γ i is 0, so as long as the distance between the starting point and the end point of the positioning algorithm trajectory is obtained and brought into the RMSE calculation formula, the RMSE can be calculated.

[0285] S4, automatically detect and analyze the stability of the GPS signal in the current environment through the adaptive controller module, and use the fuzzy control selector to adaptively switch the positioning mode according to the real-time situation.

[0286] The adaptive controller module includes: a GPS confidence discrimination model and a fuzzy controller model.

[0287] S41, GPS confidence discrimination model

[0288] The information entropy method is used to analyze the number of GPS satellites, comprehensive precision factor, and horizontal precision factor, and calculate the confidence of GPS. Information entropy is a measure of information uncertainty, which can be used to analyze the complexity and randomness of information, thereby evaluating the reliability and value of information.

[0289] In GPS positioning, the number of satellites, differential state and horizontal precision factor are three important parameters that directly affect the accuracy and reliability of GPS positioning. In order to evaluate the confidence of GPS positioning, the information entropy method can be used to analyze these three parameters to obtain a comprehensive confidence evaluation value.

[0290] Assume that the number of satellites for the main antenna, the horizontal precision factor, and the number of satellites for the secondary antenna are N s , D s and H p , the probability distribution functions are and Then the information entropy can be expressed as:

[0291]

[0292]

[0293]

[0294] Among them, n, h, and d represent the number of satellites in the main antenna, the number of satellites in the secondary antenna, and the value of the horizontal precision factor, respectively. Then, the three information entropies are weighted averaged to calculate the information entropy H of GPS positioning. GPS :

[0295]

[0296] Among them, ω1, ω2, and ω3 are the weight coefficients of the number of main antenna satellites, the number of secondary antenna satellites, and the horizontal precision factor, respectively, and can be adjusted according to actual application conditions.

[0297] Finally, the confidence C of GPS positioning can be calculated based on information entropy GPS , the calculation formula is as follows:

[0298]

[0299] Wherein, N is the number of possible states, which can be determined according to the value range of the number of main antenna satellites, the number of secondary antenna satellites and the horizontal precision factor.

[0300] When H GPS The smaller it is, the more certain the GPS positioning status is, and the corresponding confidence level C GPS The higher the H GPS The larger the value is, the more uncertain the GPS positioning status is, and the corresponding confidence level C GPS The lower.

[0301] In summary, the method of using information entropy to analyze the number of GPS main antenna satellites, the number of secondary antenna satellites and the horizontal precision factor and calculate the GPS confidence has good feasibility and accuracy, and can more accurately evaluate the reliability of GPS positioning.

[0302] S42: adaptively switch the positioning algorithm.

[0303] The switching of the positioning algorithm is based on the value C calculated by the GPS confidence discrimination model in formula 4-6.GPS When the GPS confidence is greater than the threshold P, RTK-GPS positioning is used. When it is less than the threshold, the positioning algorithm based on lidar slam and visual information fusion is used. At the same time, in order to prevent frequent switching, a time threshold T is introduced before switching. Only when the confidence duration is greater than the threshold T, the positioning algorithm is switched. The meanings of parameters P and T are shown in 24:

[0304] Electric heavy trucks use RTK-GPS positioning in areas with strong GPS signals. When they drive into areas with weak GPS, the GPS signal quality deteriorates, and the GPS confidence value P suddenly changes downward. It is then switched to the laser SLAM fusion positioning algorithm based on AprilTag assisted correction. When they drive out of the area with weak GPS, the GPS signal returns to normal, the GPS confidence value P suddenly changes upward, and it is switched back to RTK-GPS positioning.

[0305] To achieve Fig.25 The switching process of the positioning algorithm shown in the figure, this paper uses a fuzzy control-based method to dynamically and adaptively switch between RTK-GPS positioning and laser slam positioning algorithm fused with vision according to the confidence value of GPS signal. This method uses fuzzy reasoning and defuzzification technology to determine the most suitable positioning algorithm according to the input GPS signal confidence value to improve the accuracy and reliability of positioning.

[0306] Fuzzy control algorithm is an intelligent control method based on fuzzy set theory, fuzzy language variables and fuzzy logic reasoning. It is an intelligent control algorithm that imitates human fuzzy reasoning and decision-making process in behavior.

[0307] Fuzzy control first defines the input and output variables and fuzzifies them. The input variables are the GPS signal confidence, duration and current positioning state, which are defined as A, B and C respectively. The GPS mutation conditions (upward mutation and downward mutation) are recorded as A1 and A2 respectively; whether the duration after the mutation exceeds the threshold T is recorded as B1 and B2; whether the current positioning state is the GPS-RTK positioning state or the laser slam positioning state of fusion vision is recorded as C1 and C2. The output variable Z is the selection of whether the positioning algorithm switches parameters, which are recorded as 1 and 0 respectively. The input parameters are recorded as (a, b, c), and the output parameter is Z. The rule base is designed according to the input and output results. A set of rules are designed to determine the switching parameters of the positioning algorithm according to the confidence value, duration and current positioning state of the input GPS signal. The designed fuzzy logic rules are:

[0308]

[0309] The developed fuzzy logic controller is added to the ROS program of the electric heavy truck, which realizes the software module of adaptive switching of the positioning algorithm. The flow chart of the software module is as follows: Fig.26 shown.

[0310] The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should also be regarded as the protection scope of the present invention.

Claims

1. The electric heavy truck adaptive positioning method based on multi-sensor fusion is characterized by: The method comprises the following steps: S1, constructing a sensor data preprocessing module and a spatiotemporal synchronization module to preprocess the data acquired by the sensor; S2, basic positioning of electric heavy trucks based on apriltag positioning algorithm and laser slam positioning algorithm; S3, based on ESKF's apriltag-assisted laser slam correction algorithm, real-time positioning and correction of electric heavy trucks in motion; S4, automatically detect and analyze the stability of the GPS signal in the current environment through the adaptive controller module, and use the fuzzy control selector to adaptively switch the positioning mode according to the real-time situation.

2. The electric heavy truck adaptive positioning method based on multi-sensor fusion according to claim 1 is characterized in that: The sensor data preprocessing module includes: a three-dimensional laser observation model, a camera imaging model, and a GPS positioning model; The camera imaging model uses the translation vector and the rotation matrix Describes the transformation relationship between the camera coordinate system and the world coordinate system; Represents the value of the origin of the camera coordinate system relative to the world coordinate system. Represents the orthogonal rotation transformation of the camera coordinate system relative to the world coordinate system; Assume that the coordinates of the space point A in the world coordinate system and the camera coordinate system are [x w ,y w ,z w ],[x c ,y c ,z c ], then there exists the following homogeneous transformation relationship: Among them, H is called the external parameter matrix of the camera, which is determined by the installation position and posture of the camera in the robot body; assuming that the focal length of the camera, that is, the distance from the origin of the camera plane to the origin of the image plane is f; then according to the triangle similarity principle, point A[x c ,y c ,z c The projection position A′[x′, y′] of the image plane satisfies the following relationship: Assume that the pixel coordinates of point A′ are (u, v), the scaling ratios of the pixels on the x and y axes are a and b, and the image origin translation is (c x ,c y ), the coordinates of the pixel point satisfy the following relationship: Substitute equation 2-3 into equation 2-2, and let f x =af,f y = bf, we get the following coordinate equation: Among them, the K matrix is ​​called the camera intrinsic parameter matrix, which needs to be obtained through calibration; Combining equations 2-1 and 2-4, we can see that the point [x w ,y w ,z w ] is projected onto the pixel point (u,v) on the image. The process satisfies the following relationship:

3. The electric heavy truck adaptive positioning method based on multi-sensor fusion according to claim 1 is characterized in that: The space-time synchronization module includes: time synchronization and space synchronization; The processing method of time synchronization is as follows: in the lidar slam callback function, the timestamp of the LIDAR is used as the reference. When formula 4-1 is satisfied, the GPS and IMU of this group are considered to be time-synchronized data, and then the thread of the GPS / IMU fusion algorithm is entered; |t LIDAR，i -t IMU，j |<0.01s (4-1) Apriltag tag identification and positioning algorithm, in the GPS / vision callback function, takes the GPS timestamp as the reference. When formula 4-2 is satisfied, this set of GPS and camera data is considered to be time-synchronized data, and then the Apriltag tag identification and positioning algorithm is entered; |t GPS，i -t CAMERA，j |<0.01s (4-2); The processing method of spatial synchronization is: converting GPS from WGS-84 coordinate system to UTM. The UTM coordinate system is a coordinate system with three axes xyz perpendicular to each other, just like the Cartesian coordinate system, where xyz corresponds to the three directions of due east, due north, and vertically upward on the ground, respectively, and the first frame of GPS data received is used as the UTM coordinate origin; since GPS and IMU only have a 10cm offset in the y direction, after the IMU performs offset compensation, mileage drawing can also be performed with the UTM coordinate origin as the origin.

4. The electric heavy truck adaptive positioning method based on multi-sensor fusion according to claim 1 is characterized in that: The apriltag tag positioning algorithm includes: adaptive threshold, continuous boundary segmentation, quadrilateral fitting, tag decoding, and tag pose solution; The processing process of the adaptive threshold is as follows: grayscale the input image, divide the image into fixed pixel blocks of 4*4 size, and calculate the minimum and maximum values ​​in each pixel block; in order to prevent the extreme value difference of the boundary pixel block from forming artifacts, the extreme value of each pixel block is determined by the nearby 3x3 pixel blocks; the average of the calculated maximum and minimum values ​​is used as the threshold, and the pixels in the image below the threshold are set to a black value of 255, and the pixels above the threshold are set to a white value of 0, thereby realizing binarization; The processing process of the continuous boundary segmentation is as follows: clustering is completed efficiently by using a hash table, and the elements in each cluster can be indexed by the ID number of the connected domain, thereby realizing boundary segmentation; at the same time, the same white pixel is allowed to appear in two generated clusters, thereby solving the single pixel width problem mentioned earlier that cannot segment two different connected domains; The processing process of the quadrilateral fitting is as follows: by finding a small number of corner points, and then traversing all possible corner point combinations to calculate an approximate segmentation; sorting the contour points by angle around their center in the same rotation direction; through this sorting, performing straight line fitting on the points within a relative range from the center point; traversing all clustered contour point clusters, selecting the peak of the mean square error function to identify the corner points; wherein, the principal component analysis method is used to perform straight line fitting; finally, searching for a corner point combination that can form a quadrilateral in the corner point set, and performing straight line fitting on all sides of the candidate quadrilateral, and selecting four corner points with the smallest straight line fitting error as the final corner points; The processing process of the label decoding is: first detect whether there is a black square frame on the outer layer, and then read the square of the inner dot matrix to see whether there is a valid coding area. According to different coding methods, Apriltag is divided into different types, and the number of dot matrices in its graphic structure is different. The image pixel value is compared with the specified threshold. If it is greater than the threshold, the corresponding area on the label is 1, and if it is less than the threshold, the corresponding area on the label is 0. In order to increase the robustness to illumination, the threshold model of formula 3-1 is defined; the threshold model of black pixels and the threshold model of white pixels are established respectively, and the threshold model is learned using known labels containing black and white pixels. The model parameters can be calculated using least squares regression; the threshold used when decoding the label data bit is the average value of the predicted values ​​of the black and white threshold model; I(x,y)=Ax+Bxy+Cy+D (3-1) The processing process of the tag pose solution is as follows: after obtaining the correct boundary of the tag, combined with the intrinsic reference model of the camera, the four corner points in the tag coordinate system with p1, p2, p3, and p4 as the origin of the tag center, parallel to the adjacent sides of the tag rectangle as the x-axis and y-axis, and perpendicular to the tag plane as the z-axis, whose coordinate values ​​can be calculated from the actual physical distance of the tag, and p1′, p2′, p3′, and p4′ are the corresponding positions projected to the imaging plane. For a pair of matching points, according to the camera model, the following can be obtained: in, Because the four corners of the label are coplanar with the xy axis plane of the label coordinate system, z w is always equal to 0, so it can be sorted into: make Expand to get: Eliminating s gives: Arranged into matrix form, we can get: Because KH and H are equivalent, and h 33 =[0,0,1][t x , t y , t z ] T =t s is not 0, so we can set h 33 =1, then there are 8 unknowns in formula 3-4, each feature point can provide 2 constraint equations, and four pairs of matching points can be used to calculate the H matrix, thereby obtaining the first two columns of the scaled rotation matrix R and the translation vector t; since each column of the rotation matrix is ​​a unit vector, and the label must appear in front of the camera, the scale of the H matrix can be obtained, thereby calculating the first two columns of the actual rotation matrix and the translation vector. At the same time, R is an orthogonal matrix, and its third column can be obtained by cross product of the first two columns, that is, r3=r1×r2.

5. The electric heavy truck adaptive positioning method based on multi-sensor fusion according to claim 1 is characterized in that: The laser SLAM positioning algorithm includes: laser point cloud preprocessing, pose estimation based on feature point cloud matching; The laser point cloud preprocessing process is as follows: removing the point cloud distortion caused by the robot motion through the IMU measurement value, extracting the edge and plane points based on the LOAM geometric feature extraction method, determining the key frame selection strategy, establishing a local map to store feature points, and completing the point cloud preprocessing; The process of removing the point cloud distortion caused by the robot movement is as follows: the position change of the laser radar in each frame scanning process needs to be obtained, and the laser point is converted to the same coordinate system; since the IMU measurement frequency can reach 200Hz, which is 20 times that of the laser radar, the radar position change can be accurately reflected in a short time; therefore, the laser point cloud is dedistorted using the IMU data; the state of the laser radar is positioned as: X i =[p i ,v i ,q i ,b a T ,b ω T ] T Where X i Represents the state of the robot, p i is the position of the robot in the world coordinate system, v i is the speed of the robot in the world coordinate system, q i is the posture of the robot in the world coordinate system, b a is the acceleration drift of IMU, b ω is the angular velocity drift of IMU; The state quantity of the mobile robot in the jth frame is obtained by discretizing the IMU data and the state quantity of the i-th frame. The calculation formula is: in, g w is the gravity vector in the current state, is the linear acceleration vector, is the angular velocity vector; in order to reduce the number of integration times, IMU pre-integration is used to calculate the current state change. The calculation formula is as follows: The frequency of LiDAR is much lower than that of IMU, so there will be multiple IMU data between two LiDAR frames; Assume that the timestamps corresponding to the two frames of lidar data are t k and t k+1 In this paper, the robot is calculated by pre-integration at t k to k+1 Relative pose change over time Assume that the robot is at t∈[t k , t k+1 ] time to make a uniform motion, based on which the linear interpolation can be used to obtain the position transformation of any laser point relative to the first laser point. The calculation formula is: Among them, t i is the time of the i-th laser point in the laser point cloud, T i is the position transformation matrix of the i-th laser point relative to the first laser point, and then the current laser point is converted to the coordinate system corresponding to the first laser point. The calculation formula is: h′ i =T i h i (3-12) where h i is the position of the laser point before distortion correction, h′ i To correct the position of the laser point after distortion, each laser point in the laser point cloud is transformed in position and posture, thus completing the dedistortion of the laser point cloud. The process of establishing a local map storing feature points is as follows: Assuming that the robot posture corresponding to the i-th key frame is X i ; When the robot is relative to X i When the posture change exceeds the set threshold, a new laser radar frame P is selected i+1 Save as a key frame, the corresponding pose is X i+1 ; The non-critical lidar frames between two key frames are discarded. By extracting key frames, the memory resource consumption is reduced while ensuring the map density; the position change Δp and rotation change thresholds Δθ are set to 0.5m and 0.2° respectively.

6. The electric heavy truck adaptive positioning method based on multi-sensor fusion according to claim 5 is characterized in that: The process of posture estimation based on feature point cloud matching includes: initial posture prediction, geometric feature point matching, and posture solution; The processing process of the initial posture prediction is as follows: Assume that the posture at time i is The change in posture from time i to time k is obtained by integrating the IMU data, ΔT ik Including the position and posture changes {ΔX, ΔR}, the posture T′ of the mobile robot at time K is R，K for: T′ R，K =T R，K ΔT ik Multiply the coordinates of the current frame point cloud by T′ R，K The coordinates in the global coordinate system can be obtained, and the optimization formula can be constructed through point cloud matching to iteratively solve the robot posture T R，K ; The processing process of geometric feature point matching is as follows: the point cloud P at time i+1 is i+1 The IMU predicts the pose to the global coordinate system. Let the laser point k be P i+1 An edge feature in the local edge point cloud map is searched using the kd-tree radius search algorithm. Get the points u and v closest to point k, calculate the distance from k to the straight line where points u and v are located, and construct the residual formula d e : Similarly, assuming k is P i+1 A plane feature point is found in the local plane point cloud map through the kd-tree radius search algorithm Search for the nearest point v to point k, and fit the nearest points u and w to v into a plane, calculate the distance from k to the fitting plane, and construct the distance residual d from the point to the plane p : By calculating the distance between point-line and point-plane, point-line and line-plane constraint information is constructed to complete the data association between geometric feature points; The processing process of posture solution is as follows: point cloud matching posture estimation, using laser observation data and IMU measurement values ​​to solve the robot posture, when a frame of new point cloud is received, the posture obtained by IMU integration is linearly interpolated, a frame of point cloud is converted to the scanning starting point, the motion distortion of the point cloud is removed, and the edge and plane feature points are extracted according to the curvature information; by judging whether the current key frame map is empty, it is determined whether the frame is the first frame of point cloud, if it is the first frame of point cloud, it is directly added to the key frame point cloud map, and waits for subsequent point cloud frames to be added, if it is not the first frame of point cloud, then a kd-tree nearest neighbor search tree of the saved key frame point cloud is constructed, and the search tree is used to extract several key frames within 50m of the current posture range to construct a local map, and the map is downsampled; the most relevant feature points of the latest point cloud are searched in the local point cloud, the corresponding geometric constraints are constructed, and the Levenberg-Marquardt method is used to solve and obtain the optimal posture of the robot; it is judged whether the frame meets the key frame judgment condition, if it meets the key frame map, it is added, if not, the next frame is read and looped.

7. The electric heavy truck adaptive positioning method based on multi-sensor fusion according to claim 1 is characterized in that: The process of real-time positioning and correction of the electric heavy truck in mobile state by the ESKF-based apriltag-assisted laser slam correction algorithm is as follows: when the apriltag image is captured, the absolute position observed by the apriltag tag is integrated into the odometer data of the laser SLAM through ESKF to correct the SLAM, thereby improving the positioning accuracy of the robot and correcting the accumulated error of the odometer; The updating process of the integration is as follows: firstly, the nominal state variable x of the system and the error state variable δx are given, and the input is u m , the noise is i: Then the ESKF prediction equation can be written as: F x and F i is the Jacobian matrix of f() with respect to the error and disturbance vectors; Q i is the covariance matrix of the perturbation pulse; Among them, the state transfer matrix F x for: System input Jacobian matrix F i for: The state transfer covariance matrix is: Error Kalman filter update steps: Among them, y is the measurement value, H is the measurement matrix of the error state quantity δx, and since the error state quantity is 0, the nominal state quantity is used as the true value state quantity; The measurement matrix H is: Matrix H x is the Jacobian matrix of the measurement function to the true value state quantity, and its form varies depending on the sensor used; the true value state to error state matrix X δx for: Simplified, we get: in, Update state: In the error Kalman filter, the covariance matrix P also needs to be reset: P←GPG T (3-35)。 8. The electric heavy truck adaptive positioning method based on multi-sensor fusion according to claim 1 is characterized in that: The process of automatically detecting and analyzing the stability of the GPS signal in the current environment by the adaptive controller module is as follows: Assume that the number of satellites for the main antenna, the horizontal precision factor, and the number of satellites for the secondary antenna are N s , D s and H p , and the probability distribution functions are and Then the information entropy can be expressed as: Among them, n, h, and d represent the number of satellites of the main antenna, the number of satellites of the secondary antenna, and the value of the horizontal precision factor respectively; the three information entropies are weighted averaged to calculate the information entropy H of GPS positioning. GPS : Among them, ω1, ω2, and ω3 are the weight coefficients of the number of satellites for the main antenna, the number of satellites for the secondary antenna, and the horizontal precision factor, respectively; Finally, the confidence C of GPS positioning is calculated based on the information entropy. GPS , the calculation formula is as follows: Where N is the number of possible states, which can be determined according to the number of satellites in the main antenna, the number of satellites in the secondary antenna, and the range of the horizontal precision factor; When H GPS The smaller it is, the more certain the GPS positioning status is, and the corresponding confidence level C GPS The higher the H GPS The larger the value is, the more uncertain the GPS positioning status is, and the corresponding confidence level C GPS The lower.

9. The electric heavy truck adaptive positioning method based on multi-sensor fusion according to claim 1 is characterized in that: The process of using the fuzzy control selector to adaptively switch the positioning mode according to the real-time situation is: The switching of the positioning algorithm is based on the value C calculated by the GPS confidence discrimination model in formula 4-6. GPS When the GPS confidence is greater than the threshold P, RTK-GPS positioning is used. When it is less than the threshold, the positioning algorithm based on lidar slam and visual information fusion is used.

10. The electric heavy truck adaptive positioning system based on multi-sensor fusion is characterized by: The system is used to implement the electric heavy truck adaptive positioning method based on multi-sensor fusion as described in any one of claims 1 to 9, and includes a hardware system and a software system; The hardware system consists of a 3D laser radar sensor, a GPS sensor, an IMU inertial measurement sensor and a visual camera sensor; the hardware system is used to provide hardware support for the software system; The 3D laser radar sensor is used to monitor the application scene environment information and obtain three-dimensional point cloud data; the GPS sensor is used to obtain the carrier position, posture, and speed information; the IMU inertial measurement sensor is used to obtain three-dimensional angular velocity, acceleration, magnetic field information and fused three-dimensional azimuth angle, position and speed information; the visual camera sensor is used to observe the application scene environment information; The software system consists of a robot operating system module, an ESKF-based apriltag-assisted laser slam positioning module, and an adaptive controller module; the software system is used to parse the data of the hardware system and complete the adaptive positioning of the electric heavy truck through the operation of the fusion positioning algorithm, and transmit the real-time posture and point cloud map of the electric heavy truck to the terminal display; The robot operating system module is used to provide a software framework; the ESKF-based AprilTag-assisted laser slam positioning module fuses the output posture of the AprilTag tag positioning with the posture output by the laser slam through the error state Kalman filter to complete the real-time estimation of the posture of the electric heavy truck robot; the adaptive controller module is composed of a GPS confidence discrimination model and a fuzzy controller model, and is used to switch the positioning mode.

Images