A method, system and storage medium for driving control of unmanned fire truck based on positioning fusion

Abstract

The present invention discloses a method, system and storage medium for controlling the driving of an unmanned fire truck based on positioning fusion, and belongs to the technical field of unmanned driving control. The method comprises: obtaining real-time multi-source positioning related parameters of a fire truck and preprocessing the multi-source positioning related parameters; respectively obtaining indoor and outdoor positioning information of the fire truck according to the preprocessed multi-source positioning related parameters; obtaining the positioning fusion information of the fire truck at the current moment according to the indoor and outdoor positioning information; obtaining the steering angle prediction information and acceleration and deceleration prediction information of the fire truck at the next moment based on the positioning fusion information and a model prediction control algorithm; controlling the driving state of the fire truck at the next moment according to the steering angle control prediction information and the acceleration and deceleration control prediction information, obtaining the fused positioning information through the multi-source positioning related parameters, and inputting the fused positioning information into the model prediction control algorithm, thereby improving the overall positioning accuracy and the control performance of the fire truck under high-speed driving conditions, and reducing the control error of the fire truck.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned driving control technology, and in particular relates to a driving control method, system and storage medium for an unmanned fire truck based on positioning fusion. Background Art

[0002] In the field of fire truck control, accurate positioning technology is crucial to ensure that vehicles can perform tasks safely and efficiently in complex environments. At present, fire trucks mainly use RTK (Real-Time Kinematic) technology for positioning, which relies on satellite signals and achieves high-precision positioning through differential GPS. However, the reliability and accuracy of RTK technology will be seriously affected in environments with limited GNSS signals such as indoors or underground garages, resulting in inaccurate positioning information, which in turn affects the overall control performance of the fire truck. In these scenarios, GNSS signals may become unreliable or even completely lost, making it impossible for RTK positioning systems that rely on satellite signals to provide continuous and reliable positioning data. In addition, the traditional PID (proportional-integral-differential) control method is prone to large control errors when driving at high speeds due to the limitations of response speed and control accuracy, affecting the operational stability and safety of the fire truck.

[0003] To address these issues, developing a fire truck control method that can provide reliable positioning in a GNSS signal-limited environment and maintain precise control when driving at high speeds is of great significance to improving the operational efficiency and safety of fire trucks. Summary of the invention

[0004] The present invention provides a driving control method, system and storage medium for an unmanned fire truck based on positioning fusion, which realizes indoor and outdoor positioning fusion through multi-source parameters, and then uses the fused positioning information as the control input of the unmanned fire truck, thereby solving the problem that the limited GNSS signal affects the overall positioning accuracy in indoor or underground garage scenarios. The combination of model predictive control and traditional PID control reduces the control error of the whole vehicle, thereby improving the operating efficiency and safety of the unmanned fire truck.

[0005] To achieve the above object, the present invention is implemented by adopting the following technical solutions:

[0006] A driving control method for an unmanned fire truck based on positioning fusion, comprising:

[0007] Acquire real-time multi-source positioning related parameters of the fire truck and pre-process the multi-source positioning related parameters;

[0008] The indoor and outdoor positioning information of the fire truck is obtained respectively according to the pre-processed multi-source positioning related parameters;

[0009] Acquire the current positioning fusion information of the fire truck according to the indoor and outdoor positioning information;

[0010] Based on the positioning fusion information and the model predictive control algorithm, the steering angle prediction information and the acceleration and deceleration prediction information of the fire truck at the next moment are obtained;

[0011] The driving state of the fire truck at the next moment is controlled according to the steering angle control prediction information and the acceleration and deceleration control prediction information.

[0012] Optionally, the obtaining of the real-time multi-source positioning related parameters of the fire truck includes: respectively collecting real-time point cloud data, IMU data and RTK positioning data of the fire truck during its driving process through a laser radar, an IMU inertial measurement unit and an RTK positioning device installed on the fire truck.

[0013] Optionally, the preprocessing the multi-source positioning related parameters includes:

[0014] Performing spatiotemporal synchronization on the IMU data and the point cloud data to obtain synchronized IMU data;

[0015] The point cloud data is subjected to filtering for abnormal point clouds, and the distortion of the filtered point clouds is corrected by using the synchronized IMU data to obtain a valid point cloud set.

[0016] Optionally, the indoor and outdoor positioning information of the fire truck is obtained respectively according to the preprocessed multi-source positioning related parameters, including: obtaining the indoor positioning information of the fire truck according to the observation point cloud and IMU data, and obtaining the outdoor positioning information of the fire truck according to the RTK satellite positioning data.

[0017] Optionally, acquiring indoor positioning information of the fire truck according to the observation point cloud and IMU data includes:

[0018] The initial value of the registration between the observed point cloud and the global point cloud map is obtained through the IMU pre-integration results;

[0019] Associating the current frame point cloud with the historical frame point cloud according to the valid point cloud set, and registering the associated current frame point cloud with the historical frame point cloud to obtain a corrected global point cloud map;

[0020] The laser odometer is solved by registering the current frame point cloud with the global point cloud map, and the indoor positioning information of the fire truck is obtained according to the registration initial value and the laser odometer.

[0021] Optionally, the acquiring the current positioning fusion information of the fire truck according to the indoor and outdoor positioning information includes:

[0022] While solving the laser odometer by registering the current frame point cloud with the global point cloud map, the RTK satellite search status is determined according to the RTK satellite positioning data;

[0023] Determine whether the laser odometer obtained is a fixed solution. If it is not a fixed solution, update the global point cloud map. If it is a fixed solution, construct a factor graph and fuse the laser odometer with the RTK positioning information based on the factor graph optimization.

[0024] Optionally, the obtaining of the steering angle prediction information and acceleration / deceleration prediction information of the fire truck at the next moment based on the positioning fusion information and the model predictive control algorithm includes:

[0025] The positioning fusion information is input into the MPC-PID model predictive control lateral control module in the industrial computer to obtain the steering angle signal of the fire truck at the next moment;

[0026] The positioning fusion information is input into the PID model predictive control longitudinal control module in the industrial computer to obtain the acceleration and deceleration signal of the fire truck at the next moment.

[0027] Optionally, controlling the driving state of the fire truck at the next moment according to the steering angle control prediction information and the acceleration / deceleration control prediction information includes:

[0028] The steering angle signal is sent to the chassis of the fire truck through the industrial computer to drive the fire truck to turn, and the state quantity related to the steering is fed back to the MPC-PID model predictive control lateral control module;

[0029] The acceleration and deceleration signals are sent to the chassis of the fire truck through the industrial computer to drive the fire truck to accelerate and decelerate, and the state quantities related to acceleration and deceleration are fed back to the PID model predictive control longitudinal control module.

[0030] In a second aspect, the present invention provides an unmanned fire truck driving control system based on positioning fusion, comprising:

[0031] Multi-source positioning related parameter acquisition module: used to obtain the real-time multi-source positioning related parameters of the fire truck and pre-process the multi-source positioning related parameters;

[0032] Indoor and outdoor positioning information acquisition module: used to obtain the indoor and outdoor positioning information of the fire truck according to the pre-processed multi-source positioning related parameters;

[0033] Positioning fusion information acquisition module: used to obtain the current positioning fusion information of the fire truck according to the indoor and outdoor positioning information;

[0034] Fire truck driving state prediction module: used to obtain the steering angle prediction information and acceleration and deceleration prediction information of the fire truck at the next moment based on the positioning fusion information and the model prediction control algorithm;

[0035] Fire truck driving control module: used to control the driving state of the fire truck at the next moment according to the steering angle control prediction information and the acceleration and deceleration control prediction information.

[0036] In a third aspect, the present invention provides a computer storage medium having a computer program stored thereon. When the computer program is executed by a processor, the unmanned fire truck driving control method based on positioning fusion as described in any step of the first aspect is implemented.

[0037] Compared with the prior art, the beneficial effects achieved by the present invention are as follows: through multi-sensor fusion positioning, seamless switching and fusion of indoor and outdoor positioning is realized, the overall positioning accuracy is improved, and by adopting advanced control strategies, combining model predictive control with traditional PID control methods, the dynamic behavior of the vehicle can be better predicted, and the steering angle and acceleration and deceleration of the fire truck can be accurately controlled based on the fused positioning information, thereby improving the control performance under high-speed driving conditions and reducing the control error of the fire truck. Through multi-sensor fusion positioning technology and model predictive control algorithm, the intelligence level of the fire truck is improved, so that it can better adapt to the complex and changeable fire environment, and improve the fire truck at various speeds and The control stability and safety in various environments provide strong technical support for firefighting and rescue; using the attitude and speed information provided by the inertial measurement unit IMU and the surrounding environment information provided by the lidar, combined with the RTK technology, this fusion positioning method can improve the accuracy and reliability of positioning through data supplemented by other sensors when the GNSS signal is unstable, and also enable the fire truck to maintain efficient operating performance in various environments; through the application of the MPC-PID model predictive control algorithm, the lateral and longitudinal control of the fire truck is optimized, so that the fire truck can track the predetermined path more smoothly and accurately, and the control response speed and accuracy are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 The figure is a flow chart of a driving control method of an unmanned fire truck based on positioning fusion in one embodiment of the present invention;

[0039] Figure 2 The figure shows a system architecture diagram of a travel control entity in one embodiment of the present invention;

[0040] Figure 3 The figure is a flow chart of a laser radar-IMU fusion positioning method in one embodiment of the present invention;

[0041] Figure 4 The figure shows a flow chart of an RTK-lidar-IMU fusion positioning method in one embodiment of the present invention;

[0042] Figure 5Shown is a flow chart of a fire truck model prediction lateral control method in one embodiment of the present invention;

[0043] Figure 6 Shown is a flow chart of a fire truck model predictive longitudinal control method in an embodiment of the present invention. DETAILED DESCRIPTION

[0044] The present invention will be further described below in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and cannot be used to limit the protection scope of the present invention.

[0045] Example 1

[0046] like Figure 1 As shown, this embodiment provides a driving control method for an unmanned fire truck based on positioning fusion, including:

[0047] Acquire real-time multi-source positioning related parameters of the fire truck and pre-process the multi-source positioning related parameters;

[0048] The indoor and outdoor positioning information of the fire truck is obtained respectively according to the pre-processed multi-source positioning related parameters;

[0049] Acquire the current positioning fusion information of the fire truck according to the indoor and outdoor positioning information;

[0050] Based on the positioning fusion information and the model predictive control algorithm, the steering angle prediction information and the acceleration and deceleration prediction information of the fire truck at the next moment are obtained;

[0051] The driving state of the fire truck at the next moment is controlled according to the steering angle control prediction information and the acceleration and deceleration control prediction information.

[0052] Example 2

[0053] Based on Example 1, this example also makes the following design.

[0054] On the one hand, this embodiment uses the fusion of RTK-lidar-IMU data for positioning as the control input of the fire truck, thereby solving the problem that GNSS may become unreliable or completely lost in indoor or underground garage scenarios, affecting the overall positioning accuracy and further affecting the control error of the entire vehicle. On the other hand, through the lateral and longitudinal decoupling control of the fire truck, the lateral control adopts the kinematic MPC+PID control algorithm, and the longitudinal controller adopts the PID control algorithm to solve the problem of large control error caused by simply using PID when the vehicle speed is high.

[0055] Figure 2This is a diagram of the physical system architecture of the driving control. The physical control system installed on the unmanned fire truck realizes the driving control method of the unmanned fire truck. The control system is divided into: 1. Hardware part: IMU, laser radar, positioning antenna, RTK hardware, industrial computer, fire truck chassis; 2. Software part: laser radar-IMU fusion positioning algorithm, RTK positioning algorithm, RTK-laser radar-IMU fusion positioning algorithm, model predictive control algorithm. Among them, IMU is used to collect posture information of fire trucks during driving, positioning antenna is used to collect vehicle information and satellite positioning signals, and lidar is used to collect point cloud data during driving of fire trucks; RTK hardware is used to run RTK positioning algorithm, which is mainly responsible for processing the collected satellite signals and calculating the specific position of the fire truck; industrial computer is mainly used to run lidar-IMU fusion positioning algorithm, RTK-lidar-IMU fusion positioning algorithm and model predictive control algorithm; the fire truck chassis is used to execute the instructions output by the model predictive control algorithm to drive the vehicle to accelerate, decelerate and turn; after the system realizes indoor positioning through the lidar-IMU fusion positioning algorithm, it sends the indoor positioning signal to the RTK-lidar-IMU fusion positioning algorithm module for processing to obtain fusion positioning information, and finally sends the fusion positioning information to the model predictive control algorithm to realize the lateral and longitudinal control of the fire truck.

[0056] Figure 3 The flowchart of the laser radar-IMU fusion positioning of the present invention first executes step 1-time and space calibration: responsible for time and space synchronization of the observed point cloud and the IMU raw data; then, execute step 2-point cloud filtering: mainly through the filtering algorithm to process the abnormal points of the point cloud; secondly, execute step 3-distortion correction: use the time and space aligned IMU data and IMU odometer to correct the motion distortion caused by the relative motion between the laser radar and the measured object, and use the IMU pre-integration result to provide the initial value for the registration of the observed point cloud and the global point cloud map, so that the iterative equation converges quickly; again, execute step 4-loop detection: based on the spatial nearest neighbor rule, find the historical frame associated with the current frame. If the match is successful, the point cloud is registered based on the normal distribution transformation algorithm, combined with the factor graph Optimize, update and correct the global map; again, execute step 5 - front-end registration: extract historical frames that are close in time and space to the current frame from the global point cloud map to form a local map, construct point-line and point-surface residual equations based on the corner and surface features of the current frame, and use the Levenberg iterative algorithm to solve the laser odometer to complete a single point cloud registration; again, execute step 6 - back-end optimization: construct a factor graph based on the laser odometer, IMU odometer, and loop detection factor, and optimize and correct the laser odometer and IMU zero bias to improve the quality of global mapping and the solution accuracy of the IMU odometer; finally, execute step 7 - vehicle positioning: through the front-end registration of the current frame with the global point cloud map, and combined with the back-end optimization and fusion of RTK positioning information, the output of the vehicle positioning information is completed.

[0057] Figure 4 This is the RTK-lidar-IMU fusion positioning logic diagram of the fire truck of the present invention. First, execute step 1 - enter the positioning link; then, execute step 2 - solve the laser odometer by real-time observation point cloud and global point cloud map registration; secondly, step 2 is executed at the same time - determine the RTK star search status; step 3 - determine whether it is a fixed solution. If it is a fixed solution, execute step 4 - perform positioning information fusion through factor graph optimization / Kalman filtering to improve the robustness and reliability of the positioning system. If it is not a fixed solution, execute step 5 and enable the synchronous update map module to update the global map; the sixth step sends the result of the positioning information fusion to the model predictive control algorithm, and finally the model predictive control algorithm drives the fire truck chassis to move after running.

[0058] In this embodiment, the fire truck model predictive control is divided into model predictive control lateral control and longitudinal control.

[0059] Figure 5 The lateral control logic diagram of the fire truck model of the present invention is predicted. The lateral control adopts the kinematic MPC-PID control algorithm. First, Figure 3 The signal after fusion positioning is sent to the MPC-PID model predictive control lateral control module, and the calculated steering angle signal is sent to the unmanned fire truck chassis, thereby driving the unmanned fire truck to turn. At the same time, the state quantity related to steering is fed back to the model predictive control lateral control module through the state estimator to form a closed-loop fire truck chassis steering control.

[0060] Figure 6 The longitudinal control logic diagram of the fire truck model prediction of the present invention is shown in FIG. 1 . The longitudinal controller adopts the PID control algorithm. First, Figure 3 The signal after fusion positioning is sent to the PID longitudinal controller module, and the calculated acceleration and deceleration signals are sent to the unmanned fire truck chassis, thereby driving the unmanned fire truck to accelerate and decelerate. At the same time, the state quantities related to acceleration and deceleration are fed back to the model predictive control longitudinal control module through the state estimator to form a closed-loop fire truck chassis acceleration and deceleration control.

[0061] Example 3

[0062] This embodiment provides an unmanned fire truck driving control system based on positioning fusion, including:

[0063] Multi-source positioning related parameter acquisition module: used to obtain the real-time multi-source positioning related parameters of the fire truck and pre-process the multi-source positioning related parameters;

[0064] Indoor and outdoor positioning information acquisition module: used to obtain the indoor and outdoor positioning information of the fire truck according to the pre-processed multi-source positioning related parameters;

[0065] Positioning fusion information acquisition module: used to obtain the current positioning fusion information of the fire truck according to the indoor and outdoor positioning information;

[0066] Fire truck driving state prediction module: used to obtain the steering angle prediction information and acceleration and deceleration prediction information of the fire truck at the next moment based on the positioning fusion information and the model prediction control algorithm;

[0067] Fire truck driving control module: used to control the driving state of the fire truck at the next moment according to the steering angle control prediction information and the acceleration and deceleration control prediction information.

[0068] Example 4

[0069] This embodiment provides a computer storage medium on which a computer program is stored. When the computer program is executed by a processor, the unmanned fire truck driving control method based on positioning fusion as described in any step of Example 2 is implemented.

[0070] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present application may adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes.

[0071] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0072] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0073] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0074] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the enlightenment of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the purpose of the present invention and the claims, which all fall within the protection of the present invention.

Claims

1. A driving control method for an unmanned fire truck based on positioning fusion, characterized in that: include: Acquire the real-time multi-source positioning related parameters of the fire truck and pre-process the multi-source positioning related parameters; The indoor and outdoor positioning information of the fire truck is obtained respectively according to the pre-processed multi-source positioning related parameters; Acquire the current positioning fusion information of the fire truck according to the indoor and outdoor positioning information; Based on the positioning fusion information and the model predictive control algorithm, the steering angle prediction information and the acceleration and deceleration prediction information of the fire truck at the next moment are obtained; The driving state of the fire truck at the next moment is controlled according to the steering angle control prediction information and the acceleration / deceleration control prediction information.

2. The unmanned fire truck driving control method based on positioning fusion according to claim 1 is characterized in that: The method of obtaining the real-time multi-source positioning related parameters of the fire truck includes: collecting real-time point cloud data, IMU data and RTK positioning data of the fire truck during driving by using a laser radar, an IMU inertial measurement unit and an RTK positioning device installed on the fire truck.

3. The unmanned fire truck driving control method based on positioning fusion according to claim 2 is characterized in that: The preprocessing of the multi-source positioning related parameters includes: Performing spatiotemporal synchronization on the IMU data and the point cloud data to obtain synchronized IMU data; The point cloud data is subjected to filtering for abnormal point clouds, and the distortion of the filtered point clouds is corrected by using the synchronized IMU data to obtain a valid point cloud set.

4. The unmanned fire truck driving control method based on positioning fusion according to claim 2 or 3 is characterized in that: The method of respectively obtaining the indoor and outdoor positioning information of the fire truck according to the preprocessed multi-source positioning related parameters includes: obtaining the indoor positioning information of the fire truck according to the observation point cloud and IMU data, and obtaining the outdoor positioning information of the fire truck according to the RTK satellite positioning data.

5. The method for controlling the driving of an unmanned fire truck based on positioning fusion according to claim 4 is characterized in that: The method of obtaining indoor positioning information of the fire truck according to the observation point cloud and IMU data includes: The initial value of the registration between the observed point cloud and the global point cloud map is obtained through the IMU pre-integration results; Associating the current frame point cloud with the historical frame point cloud according to the valid point cloud set, and registering the associated current frame point cloud with the historical frame point cloud to obtain a corrected global point cloud map; The laser odometer is solved by registering the current frame point cloud with the global point cloud map, and the indoor positioning information of the fire truck is obtained according to the registration initial value and the laser odometer.

6. The method for controlling the driving of an unmanned fire truck based on positioning fusion according to claim 5 is characterized in that: The obtaining the current positioning fusion information of the fire truck according to the indoor and outdoor positioning information includes: While solving the laser odometer by registering the current frame point cloud with the global point cloud map, the RTK satellite search status is determined according to the RTK satellite positioning data; Determine whether the laser odometer obtained is a fixed solution. If it is not a fixed solution, update the global point cloud map. If it is a fixed solution, construct a factor graph and fuse the laser odometer with the RTK positioning information based on the factor graph optimization.

7. The unmanned fire truck driving control method based on positioning fusion according to claim 1 is characterized in that: The method of obtaining the steering angle prediction information and acceleration / deceleration prediction information of the fire truck at the next moment based on the positioning fusion information and the model prediction control algorithm includes: The positioning fusion information is input into the MPC-PID model predictive control lateral control module in the industrial computer to obtain the steering angle signal of the fire truck at the next moment; The positioning fusion information is input into the PID model predictive control longitudinal control module in the industrial computer to obtain the acceleration and deceleration signal of the fire truck at the next moment.

8. The method for controlling the driving of an unmanned fire truck based on positioning fusion according to claim 7 is characterized in that: The controlling of the driving state of the fire truck at the next moment according to the steering angle control prediction information and the acceleration / deceleration control prediction information includes: The steering angle signal is sent to the chassis of the fire truck through the industrial computer to drive the fire truck to turn, and the state quantity related to the steering is fed back to the MPC-PID model predictive control lateral control module; The acceleration and deceleration signals are sent to the chassis of the fire truck through the industrial computer to drive the fire truck to accelerate and decelerate, and the state quantities related to acceleration and deceleration are fed back to the PID model predictive control longitudinal control module.

9. An unmanned fire truck driving control system based on positioning fusion, characterized in that: include: Multi-source positioning related parameter acquisition module: used to obtain the real-time multi-source positioning related parameters of the fire truck and pre-process the multi-source positioning related parameters; Indoor and outdoor positioning information acquisition module: used to obtain the indoor and outdoor positioning information of the fire truck according to the pre-processed multi-source positioning related parameters; Positioning fusion information acquisition module: used to obtain the current positioning fusion information of the fire truck according to the indoor and outdoor positioning information; Fire truck driving state prediction module: used to obtain the steering angle prediction information and acceleration and deceleration prediction information of the fire truck at the next moment based on the positioning fusion information and the model prediction control algorithm; Fire truck driving control module: used to control the driving state of the fire truck at the next moment according to the steering angle control prediction information and the acceleration and deceleration control prediction information.

10. A computer storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the unmanned fire truck driving control method based on positioning fusion as described in any one of claims 1 to 8 is implemented.

Images