Guidance system and guidance method for fire early warning and fighting unmanned vehicle

Abstract

The invention relates to a guidance system and guidance method for a fire early warning and fighting unmanned vehicle, the guidance system comprises a processor and a plurality of sensors such as a monocular camera, the monocular camera is used for generating image data along a track relative to the unmanned vehicle, the image data comprises a plurality of features observed in the environment in a plurality of postures of the unmanned vehicle along the trajectory; the plurality of sensors except the monocular camera are used for providing motion data relative to the unmanned vehicle in the environment; the processor is in communication connection with the sensors; and the processor fuses the data acquired by the sensor by using a Kalman filtering algorithm to complete environmental perception, and completes trajectory planning based on a mathematical model of the vehicle. The system can be used for a fire fighting truck, when a fire occurs, manual or automatic tracking cruise can be achieved, an operation instruction is accurately transmitted, the fire fighting truck can quickly reach a fire scene, and the efficiency and accuracy of inspection and fighting are effectively improved.

Description

Technical Field

[0001] The present invention relates to the field of guidance technology, in particular to a guidance system and a guidance method for an unmanned fire warning and fire fighting vehicle, which are applicable to unmanned fire trucks. Background Art

[0002] During fire emergency rescue operations, fire trucks must reach the scene quickly and accurately, adjusting their trajectories in real time to accommodate complex rescue environments. Traditional guidance methods rely on offline maps and GPS positioning technology. This approach cannot provide fire trucks with real-time, high-precision trajectories. In complex fire rescue scenarios, fire trucks may need to navigate congested areas, collapsed buildings, and other obstacles, making traditional guidance technology alone insufficient. Summary of the Invention

[0003] In order to solve the above technical problems, the present invention proposes a guidance system and a guidance method for a fire early warning and fire fighting unmanned vehicle.

[0004] The technical solutions for achieving the purpose of the present invention are:

[0005] In a first aspect, the present invention provides a guidance system for an unmanned fire warning and firefighting vehicle, the guidance system for an unmanned fire warning and firefighting vehicle comprising:

[0006] a monocular camera for generating image data along a trajectory relative to the unmanned vehicle, wherein the image data includes a plurality of features observed in an environment at a plurality of poses of the unmanned vehicle along the trajectory;

[0007] Multiple sensors other than the monocular camera, used to provide motion data of the unmanned vehicle in the environment;

[0008] a processor, communicatively connected to the monocular camera and the plurality of sensors;

[0009] The processor is configured to:

[0010] The data obtained by the multiple sensors and the monocular camera are fused through the Kalman filter algorithm to complete environmental perception, and the fused data is processed based on a mathematical motion model to obtain the trajectory of the unmanned vehicle, wherein the mathematical motion model is based on the Kalman filter algorithm of the Kalman gain calculation network and adopts a hybrid modeling of kinematics and dynamics.

[0011] In some embodiments, fusing the data acquired by the multiple sensors and the monocular camera using a Kalman filter algorithm to complete environmental perception includes:

[0012] The Kalman filter algorithm is used to perform weighted averaging on the data acquired by the multiple sensors and the monocular camera. However, unlike the weighted averaging method, the weight coefficient of the Kalman filter algorithm is determined by the measurement variance, and the two are inversely proportional.

[0013] In some embodiments, implementing the Kalman filter algorithm in environmental perception includes:

[0014]

[0015] Among them, X k is the state vector of the target at time k, X k+1 is the state vector at time k+1, which represents the state estimation of the target at the next moment; φ is the state transfer matrix; w k is the process noise, which has zero mean and positive definite covariance matrix Q k Gaussian noise vector; Z k is the observation vector of the sensor at a certain time; H is the measurement matrix; the observation noise v k is a matrix R with zero mean and positive definite covariance k Gaussian distribution measurement noise vector;

[0016] By the observation vector Z k To estimate the target state vector X k The process includes two processes: time update and measurement update.

[0017] In some embodiments, the time update process is a process of estimating the state at the next moment based on the state at the current moment, as follows:

[0018]

[0019] P k+1,k =φP k,k φ T +Q k

[0020] in, is the target state estimate at time k+1, which is calculated based on the estimated state at time k; is the state estimate at time k; P k+1,k is the estimation error covariance matrix at time k+1, which describes the uncertainty of the estimation error and reflects the accuracy of the state estimation; P k,k is the estimated error covariance matrix at time k, which represents the uncertainty of the target state estimation; Q k is the process noise covariance matrix, which represents the process noise w k The variance of φ describes the uncertainty of the system process noise; Tis the transpose of the matrix φ.

[0021] In some embodiments, the measurement update process is a process of correcting the one-step prediction error based on the current measurement value and the previous one-step prediction error to obtain the estimated value of the current measurement, specifically as follows:

[0022]

[0023] in, is the state estimate at time k-1, which is used to correct the state during the measurement update process;

[0024] is the estimated value of the target state at the current time k; G k is the Kalman gain, which controls the weighting between the measured value and the predicted value. It is calculated by the current estimation error covariance and the measurement noise covariance, indicating the degree of trust in the current measurement data; Z k is the observation value at time k; R k is the measurement noise covariance matrix, which represents the observation noise v k The variance of , describes the noise characteristics of the sensor measurement; The residual is a measure of the deviation between the forecast and the actual situation and is used to adjust the estimate.

[0025] In some embodiments, the second Kalman filter algorithm is divided into three parts: state prediction, Kalman gain calculation, and state update.

[0026] In some embodiments, the state prediction is to calculate the prior state and the observation of the state at the current moment according to the state equation and the observation equation, as follows:

[0027]

[0028]

[0029] Represents the prior state estimate of the state variable at the current time k, that is, the state predicted based on the posterior state at the previous time k-1 and the input variables; The posterior state at the previous moment k-1 is the optimal state calculated through observation data and state update; u k-1 is the input variable at the previous moment k-1; g(.) is a nonlinear function of state change, which is used to describe how the system derives the prior state at the current moment from the state and input at the previous moment; is the predicted value of the observation at the current moment k, that is, the observation value predicted based on the prior state; h(.) is the observation model function, which describes the relationship between the state and the observation.

[0030] In some embodiments, the Kalman gain is calculated by first calculating the residual of the state estimate and the residual of the observation, and then using the two residuals as inputs of the Kalman gain calculation network to calculate the Kalman gain at the current moment, as follows:

[0031]

[0032] is the residual of the state estimation, that is, the difference between the current prior state and the posterior state at the previous moment; z k is the observation value at the current moment k, the actual observation data obtained from the sensor or other measuring device; Δy is the observation residual, that is, the difference between the actual observation value and the predicted observation value; K is the Kalman gain at the current moment, which is an important parameter for adjusting the state estimation; the Kalman gain calculation network is a network based on deep learning or other algorithms, which is used to input state residuals and observation residuals and output Kalman gain.

[0033] In some embodiments, the processor is further configured to process the sensor data using YOLO V3, which is a deep network comprising 53 convolutional layers and residual connections.

[0034] In a second aspect, the present invention provides a guidance method for an unmanned vehicle for fire early warning and firefighting, the guidance method comprising the following steps:

[0035] Using a processor to receive image data generated by the unmanned vehicle within a trajectory from a monocular camera, wherein the image data includes a plurality of features observed in an environment at a plurality of postures of the unmanned vehicle along the trajectory;

[0036] The processor also receives motion data of the unmanned vehicle in the environment acquired by a monocular camera and a plurality of sensors; the plurality of sensors are multiple sensors other than the monocular camera, and are used to provide motion data of the unmanned vehicle in the environment; the processor is in communication with the monocular camera and the plurality of sensors;

[0037] The processor uses a Kalman filter algorithm to fuse the data obtained by the multiple sensors and the monocular camera to complete environmental perception, and processes the fused data based on a mathematical motion model to obtain the trajectory of the unmanned vehicle, wherein the mathematical motion model is based on a Kalman filter algorithm of a Kalman gain calculation network and adopts a hybrid modeling of kinematics and dynamics.

[0038] Compared with the prior art, the present invention has the following significant advantages:

[0039] (1) The present invention uses the Kalman filtering algorithm to fuse the environmental feature data provided by the monocular camera with the motion data provided by multiple sensors (such as an inertial measurement unit), effectively reducing the errors that may be caused by a single data source and improving the accuracy and robustness of the guidance system of the fire warning and firefighting unmanned vehicle in complex dynamic environments.

[0040] (2) This invention utilizes a mathematical motion model based on a Kalman filter algorithm using a Kalman gain calculation network, combined with kinematic and dynamic hybrid modeling, to more accurately describe the diverse motion characteristics of firefighting vehicles in fire rescue scenarios. Compared to traditional single models, this approach significantly improves the adaptability and response speed of the guidance system for fire warning and firefighting unmanned vehicles in complex dynamic environments.

[0041] (3) The monocular camera in this invention can capture environmental feature information in a variety of postures. Even in fire scenes with dense smoke and strong light interference, the guidance system for the fire warning and firefighting unmanned vehicle can still provide stable environmental perception and guidance capabilities through a multi-source data fusion algorithm. In addition, the optimized configuration of the processor ensures the system's ability to handle complex environmental changes in real time.

[0042] (4) The present invention can achieve accurate perception and rapid response to dynamic obstacles (such as escaping crowds and collapsed buildings) through the processing of mathematical motion models, ensuring that fire trucks can dynamically adjust their trajectories and safely reach the fire scene, thereby improving rescue efficiency.

[0043] (5) The present invention utilizes a fusion algorithm based on a Kalman gain calculation network to effectively filter sensor noise and environmental interference, maintaining stable system operation, particularly in extreme environments such as high temperature and vibration. This design enhances system robustness and meets the high reliability requirements of equipment in fire rescue scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 is a block diagram of a guidance system for a fire warning and firefighting unmanned vehicle in one embodiment of the present invention;

[0045] Figure 2 is a flow chart of a Kalman filter algorithm in one embodiment of the present invention;

[0046] Figure 3 This is a simulation test diagram of the Kalman filter algorithm in one embodiment of the present invention;

[0047] Figure 4 This is a schematic diagram of the structure of YOLO V3 in one embodiment of the present invention;

[0048] Figure 5 is an adaptive threshold segmentation processing diagram provided in one embodiment of the present invention;

[0049] Figure 6 is a processing result image provided in one embodiment of the present invention;

[0050] Figure 7 is a vehicle motion model diagram provided in one embodiment of the present invention;

[0051] Figure 8 is a schematic diagram of vehicle front wheel steering provided in one embodiment of the present invention;

[0052] Figure 9 1 is a diagram of a vehicle monorail model provided in one embodiment of the present invention. DETAILED DESCRIPTION

[0053] The present invention provides a guidance system and a guidance method for an unmanned fire warning and fire fighting vehicle. The guidance system for an unmanned fire warning and fire fighting vehicle can be used for an unmanned fire truck. When a fire occurs, the unmanned fire truck can cruise manually or automatically, and the operating instructions are accurately transmitted. The unmanned fire truck can quickly reach the fire scene, effectively improving the efficiency and accuracy of inspection and fire fighting.

[0054] Sensor Type:

[0055] 1. The first type of sensor provides direct measurements of quantities related to motion. For example, a wheel encoder provides data based on the rotation of the wheel. Other examples include speedometers, Doppler radars, gyroscopes, accelerometers, airspeed sensors, and global positioning systems (GPS).

[0056] 2. The second type of sensor outputs data that can be used to infer motion after processing. This second type of sensor can also be called a motion inference sensor. For example, by analyzing a series of camera images, relative motion can be inferred. In addition to cameras (single or multiple), other motion inference sensor embodiments include photoelectric sensors, sonars, and radars. In addition, data can also be derived based on a statistical probability model of motion (i.e., a motion tracking model). For example, relevant data can be obtained by analyzing the motion pattern of a reference system (such as a fire truck) rushing to the scene of a fire. In addition, various kinematic models or dynamic models, or models that integrate kinematic models and dynamic models, can also be used to describe motion.

[0057] An inertial measurement unit (IMU) can be used to generate an estimate of the motion of the reference frame at each time sampling point. In addition, a monocular camera module can be used to detect the environment.

[0058] Data collection:

[0059] like Figure 1As shown, in some embodiments, a guidance system 100 for a fire warning and fire fighting unmanned vehicle is provided. The guidance system 100 for a fire warning and fire fighting unmanned vehicle includes a first sensor 110 and a second sensor 120. In one embodiment, the first sensor 110 includes a feature tracker, and the second sensor 120 includes a motion sensor, a motion inference sensor, or a motion tracking model. The first sensor 110 and the second sensor 120 can be combined and implemented as a single sensor, such as a camera or a radar system, which distributes motion perception and feature tracking functions through time, space, or other parameters. In addition, more than two sensors can be equipped. The first sensor 110 can be a monocular camera, which is used to generate image data along a trajectory within an environment, wherein the image data includes multiple features observed in the environment by multiple postures of the vehicle along the trajectory.

[0060] The guidance system 100 for an unmanned fire warning and firefighting vehicle utilizes data derived from two perception modalities (corresponding to a first sensor 110 and a second sensor 120). The first perception modality involves feature observation, whose functionality is related to specific features and poses. The second perception modality detects the motion of a reference frame relative to the environment and expresses constraints on continuous pose and motion measurements. Motion can be sensed using direct motion sensors, motion tracking sensors, motion inference sensors, or based on feature observations.

[0061] 1. Dataset Creation

[0062] Experiments were conducted by creating three data sets, namely a data set with good lighting, a data set with dark lighting, and a data set with interference on rainy days. Each data set (video frames extracted from video files obtained by a camera) contained 8,000 training samples and 2,000 test samples, and the algorithms of the present invention were used for experiments respectively.

[0063] 2. Perspective Transformation

[0064] In the camera coordinate system, the indicator line will be distorted, so a sliding window polynomial fitting is used to cut off most of the invalid parts and better extract the indicator line. Adaptive threshold segmentation effectively eliminates the interference of shadows and completes the accurate extraction of the indicator line.

[0065] Experiments were conducted by creating three data sets, namely a data set with good lighting, a data set with dark lighting, and a data set with interference on rainy days. Each data set (video frames extracted from video files obtained by a camera) contained 8,000 training samples and 2,000 test samples, and the algorithms of the present invention were used for experiments respectively.

[0066] like Figure 5As shown in the figure, the adaptive threshold segmentation effectively eliminates the interference of shadows and completes the accurate extraction of indicator lines. Figure 6 As shown, the image processing results

[0067] Data processing:

[0068] The guidance system 100 for an unmanned fire warning and firefighting vehicle includes a processor for receiving and processing data from one or more sensors. In one embodiment, the processor includes instructions for implementing an algorithm to process the data and derive guidance information. For example, the processor may implement a filtering algorithm such as a Kalman filter or an extended Kalman filter (EKF).

[0069] The processor processes the data obtained by the feature tracking sensor and the motion sensor (or motion inference sensor, motion tracking model), and the algorithm complexity is linearly related to the number of tracked features.

[0070] The processor is communicatively connected to all sensors.

[0071] The processor is configured to: fuse the data obtained by the multiple sensors and the monocular camera through the Kalman filter algorithm to complete environmental perception, and process the fused data based on the mathematical motion model to obtain the trajectory of the reference system. The mathematical motion model is based on the Kalman filter algorithm of the Kalman gain calculation network and adopts kinematic and dynamic hybrid modeling.

[0072] Multi-sensor data fusion, also known as information fusion, is an information processing process that combines data from multiple sensors through automated analysis and comprehensive optimization using computer technology to achieve the necessary decision-making and estimation tasks under certain criteria. Depending on the data processing method, the topology of multi-sensor data fusion is mainly divided into centralized and distributed.

[0073] Centralized multi-sensor data fusion, also known as front-sensor information fusion, builds an information fusion center, in which the information fusion center does not process the original information of multiple sensors but directly performs data fusion processing. The advantage of this fusion method is that it can achieve real-time data fusion and has high data processing accuracy. However, due to the large amount of data to be processed, the requirements for the processor are high, so its reliability is low, and it is generally used in environments where information fusion is relatively simple.

[0074] Distributed multi-sensor information fusion, also known as post-sensor information fusion, involves a structure in which the sensor's own processor pre-processes the raw data locally before sending the processed results to the information fusion center. This fusion approach has the advantages of low communication channel requirements, high structural redundancy of the fused information, and the ability to rationally calculate and distribute loads. However, because the information to be fused has already been locally processed by each sensor, some of the original information may be lost, preventing real-time data fusion and introducing errors and other impacts to subsequent fusion.

[0075] The guidance system 100 for the fire warning and firefighting unmanned vehicle uses a hybrid multi-sensor fusion approach, which combines the advantages of centralized and distributed architectures, resulting in a more reliable and effective structure. This information fusion approach has the advantage of achieving better information fusion results even in complex environments.

[0076] The details of multi-sensor information fusion are as follows:

[0077] Based on statistical characteristics, the Kalman filter recursion is used to fuse specific data. Based on the optimal estimate of the statistical characteristics of the measurement model, it mainly performs low-level fusion of redundant real-time data. For systems with linear models and white noise models where both system noise and sensor measurement noise conform to Gaussian distributions, the Kalman filter will provide a unique statistically optimal estimate for the fused data. Its algorithm is as follows:

[0078]

[0079] Among them, X k is the state vector of the target at time k, X k+1 is the state vector at time k+1, which represents the state estimation of the target at the next moment; φ is the state transfer matrix; w k is the process noise, which has zero mean and positive definite covariance matrix Q k Gaussian noise vector; Z k is the observation vector of the sensor at a certain time; H is the measurement matrix; the observation noise v k is a matrix R with zero mean and positive definite covariance k Gaussian distribution measurement noise vector.

[0080] By the observation vector Z k To estimate the target state vector X k The process includes two processes: time update and measurement update. The time update process is the process of estimating the state at the next moment based on the state at the current moment. The process is as follows:

[0081]

[0082] P k+1,k =φP k,k φ T +Q k

[0083] in, is the target state estimate at time k+1, which is calculated based on the estimated state at time k; is the state estimate at time k; P k+1,k is the estimation error covariance matrix at time k+1, which describes the uncertainty of the estimation error and reflects the accuracy of the state estimation; P k,k is the estimated error covariance matrix at time k, which represents the uncertainty of the target state estimation; Q k is the process noise covariance matrix, which represents the process noise w k The variance of φ describes the uncertainty of the system process noise; T is the transpose of the matrix φ.

[0084] Similarly, the measurement update process is to correct the one-step prediction error based on the current measurement value and the previous one-step prediction error and obtain the estimated value of this measurement. The process is as follows:

[0085]

[0086] in, is the state estimate at time k-1, which is used to correct the state during the measurement update process; is the estimated value of the target state at the current time k; G k is the Kalman gain, which controls the weighting between the measured value and the predicted value. It is calculated by the current estimation error covariance and the measurement noise covariance, indicating the degree of trust in the current measurement data; Z k is the observation value at time k; R k is the measurement noise covariance matrix, which represents the observation noise v k The variance of , describes the noise characteristics of the sensor measurement; The residual is a measure of the deviation between the forecast and the actual situation and is used to adjust the estimate.

[0087] The essence of the Kalman filter algorithm to achieve information fusion is also to perform weighted averaging on the data measured by the sensors. However, unlike the weighted averaging method, its weight coefficient is determined by the measurement variance, and the two are inversely proportional. Therefore, if you want to change the weight coefficient in the fusion process, you only need to change the measurement variance of the corresponding sensor. Since the Kalman filter has the ability to use recursive methods to fuse data, it does not require high storage and calculation of large amounts of data, so it can well meet the control needs of this reference system. Figure 3 As shown, simulation tests demonstrated good control results. This Kalman filter algorithm ensures that unmanned fire trucks can travel quickly and safely in complex logistics center environments. Field tests were conducted to adjust and optimize guidance parameters to ensure system stability and accuracy.

[0088] Processing the fused data according to a mathematical motion model based on a Kalman filter algorithm of a Kalman gain calculation network and using a hybrid of kinematics and dynamics modeling;

[0089] Vehicle kinematics modeling:

[0090] The vehicle steering motion model is in the inertial coordinate system OXY, (X r ,Y r ) and (X f ,Y f ) are the coordinates of the rear axle and front axle centers of the vehicle, is the yaw angle (heading angle) of the vehicle body, δ f is the front wheel deflection angle, v r is the center speed of the vehicle's rear axle, v f is the center speed of the vehicle's front axle, and l is the wheelbase (note that the variable subscript f represents front, and r represents rear, the same below).

[0091] During a vehicle's steering process, R is the rear wheel turning radius, P is the vehicle's instantaneous center of rotation, M is the center of the rear axle, and N is the center of the front axle. It is assumed here that the vehicle's slip angle remains constant during steering; that is, the vehicle's instantaneous turning radius is the same as the road's curvature radius.

[0092] like Figure 7 As shown in FIG8, at the rear axle driving axis (X r ,Y r ), the speed is:

[0093]

[0094] Respectively represent the rear axle axis velocity components;

[0095] The kinematic constraints of the front and rear axles are:

[0096]

[0097] Combining equations (2.1) and (2.2), we can get:

[0098]

[0099] According to the geometric relationship between the front and rear wheels:

[0100]

[0101] Substituting equations (2.3) and (2.4) into equation (2.2), we can obtain the yaw angular velocity ω:

[0102]

[0103] Where ω is the vehicle's yaw rate; l is the wheelbase of the front and rear axles; at the same time, ω and vehicle speed v r The turning radius R and front wheel deflection angle δ can be obtained f :

[0104]

[0105] From equations (2.3) and (2.5), the vehicle kinematic model can be obtained as:

[0106]

[0107] The model can be further expressed in a more general form:

[0108]

[0109] Among them, the state quantity Control quantity u kin =[v r ,δ f ] T , f kin (ξ kin ,u kin ) is the functional relationship between the state quantity and the control quantity. In the path tracking control process of unmanned vehicles, it is often hoped to use [v r ,ω] as the control quantity, and substituting Equation (2.5) into Equation (2.7), the kinematic model of the vehicle can be converted into the following form:

[0110]

[0111] Vehicle dynamics modeling

[0112] The vehicle dynamics model generally includes a mass-spring-damper model for analyzing the vehicle's smoothness and a vehicle-tire model for analyzing the vehicle's handling stability. The two have different research focuses. The focus of the smoothness analysis is the vehicle's suspension characteristics, while the focus of the vehicle's handling stability analysis is the vehicle's longitudinal and lateral dynamic characteristics. The main research goal of the present invention is to enable the vehicle to track the desired path quickly and stably, which belongs to the vehicle's handling stability problem. Therefore, the suspension characteristics are not explored in depth: At the same time, the dynamic model established by the present invention is mainly used as a prediction model in the model predictive controller. It needs to be simplified as much as possible on the basis of a relatively accurate description of the vehicle's dynamics process to reduce the amount of calculation of the control algorithm.

[0113] Based on the above analysis, the following idealized assumptions are made when modeling vehicle dynamics:

[0114] ① Assume that the unmanned vehicle is driving on a flat road and ignore the vertical motion of the vehicle.

[0115] ②) The suspension system and vehicle are rigid, ignoring the suspension motion and its effect on the coupling relationship

[0116] ③Only consider the pure sideways tire characteristics, ignoring the longitudinal and lateral coupling relationship of the tire force

[0117] ④) Use a single track model to describe vehicle motion without considering the left-right transfer of load.

[0118] ⑤ Assuming that the vehicle speed changes slowly and ignoring the load transfer between the front and rear axles,

[0119] ⑥ Ignore longitudinal and lateral aerodynamics.

[0120] Based on the above six ideal assumptions, a plane motion vehicle has only three directions of motion, namely longitudinal, lateral and yaw motion. Assume that the vehicle is front-wheel drive. The coordinate system oxyz is the vehicle coordinate system fixed to the vehicle body. xoz is in the plane of left-right symmetry of the vehicle, the point where the center of mass of the vehicle is located is the coordinate origin o, the x-axis is along the longitudinal axis of the vehicle, the y-axis is perpendicular to the longitudinal axis of the vehicle, and the z-axis satisfies the right-hand rule, is perpendicular to xoy and points upward. The coordinate system OXY is an inertial coordinate system fixed to the ground, which also satisfies the right-hand rule. The tire force is defined as follows:

[0121] F lf ,F lr : Longitudinal force on the front and rear tires.

[0122] F ef ,F er : Lateral force on the front and rear tires.

[0123] F xf ,F xr: Force in the x direction acting on the front and rear tires.

[0124] F yf ,F yr : Force in the y direction acting on the front and rear tires.

[0125] According to Newton's second law, the force equilibrium equations along the x-axis, y-axis and around the z-axis are obtained respectively.

[0126] like Figure 9 As shown, in the x-axis direction:

[0127]

[0128] is the acceleration of the vehicle's center of mass in the x-axis direction; is the velocity of the vehicle's center of mass in the y-axis direction; is the angular velocity of the vehicle around the z axis;

[0129] In the y-axis direction

[0130]

[0131] is the acceleration of the vehicle's center of mass in the y-axis direction; is the velocity of the vehicle's center of mass in the x-axis direction;

[0132] Around the z-axis

[0133]

[0134] is the angular acceleration of the vehicle around the z-axis

[0135] Where a and b are the distances from the center of mass to the front and rear axles, respectively; m is the vehicle's curb weight; l z is the vehicle's moment of inertia around the z-axis.

[0136] The conversion relationship between the resultant force on the tire in the x-direction and y-direction and the longitudinal and lateral forces is as follows:

[0137] F xf =F lf cosδ f -F ef sinδ f (2.13)

[0138] F xr =F lr cosδ r -F er sinδ r (2.14)

[0139] Fyf =F lf sinδ f +F ef cosδ f (2.15)

[0140] F yr =F lr sinδ r +F er cosδ r (2.16)

[0141] The longitudinal force and lateral force of the tire can be expressed as a complex function of parameters such as tire slip angle, slip rate, road friction coefficient and vertical load:

[0142] F l =f l (α,s,μ,F z ) (2.17)

[0143] F e =f e (α,s,μ,F z ) (2.18)

[0144] Where α is the tire slip angle, s is the slip rate, μ is the road friction coefficient, and F z is the vertical load on the tire.

[0145] Taking into account the respective defects of traditional kinematic models and dynamic models, the guidance system 100 for fire warning and firefighting unmanned vehicles integrates the above two models, that is, adopts hybrid kinematics and dynamics modeling (Hybrid Kinematics-Dynamics Model) to obtain a mathematical motion model. The mathematical motion model is based on the Kalman filter algorithm of the Kalman gain calculation network and integrates the prior knowledge obtained from the two models.

[0146] like Figure 2 As shown in Figure 1, the overall Kalman filter algorithm consists of three parts: state prediction, Kalman gain calculation, and state update. State prediction calculates the current prior state and state observation based on the state equation and observation equation. Kalman gain calculation first calculates the residual of the state estimate and the residual of the observation. These two residuals serve as inputs to the Kalman gain calculation network to calculate the current Kalman gain. State update calculates the current posterior state based on the Kalman gain and the residual of the observation, completing the iterative process from k-1 to k.

[0147] Among them, the state prediction is:

[0148]

[0149] Represents the prior state estimate of the state variable at the current time k, that is, the state predicted based on the posterior state at the previous time k-1 and the input variables; The posterior state at the previous moment k-1 is the optimal state calculated through observation data and state update; u k-1 is the input variable at the previous moment k-1, such as control signal, external input force, etc.; g(.) is a nonlinear function of state change, which is used to describe how the system derives the prior state at the current moment from the state and input at the previous moment; is the predicted value of the observation at the current moment k, that is, the observation value predicted based on the prior state; h(.) is the observation model function, which describes the relationship between the state and the observation, and may be a linear or nonlinear function.

[0150] Kalman gain calculation:

[0151]

[0152] is the residual of the state estimation, that is, the difference between the current prior state and the posterior state at the previous moment; z k is the observation value at the current moment k, the actual observation data obtained from the sensor or other measuring device; Δy is the observation residual, that is, the difference between the actual observation value and the predicted observation value; K is the Kalman gain at the current moment, which is an important parameter for adjusting the state estimation; the Kalman gain calculation network is a network based on deep learning or other algorithms, which is used to input state residuals and observation residuals and output Kalman gain.

[0153] Status Update:

[0154]

[0155] is the posterior state at the current moment k, which is the optimal state estimate obtained by combining the prior state and the observed data; is the prior state at the current moment (see the state prediction section); KΔy is the adjustment amount based on the Kalman gain and the observation residual, which corrects the prior state to make it closer to the actual state.

[0156] The mathematical motion model adopts the hybrid modeling of kinematics and dynamics, which has the following advantages:

[0157] 1. Improve the interpretability of estimation: Hybrid modeling combines kinematic or dynamic mathematical models with artificial intelligence technology, which can improve the interpretability of estimation methods to a certain extent.

[0158] 2. Improved Model Accuracy and Generalization: Hybrid modeling integrates physical laws and data-driven models to build a highly accurate and generalizable vehicle dynamics model. This model combines the accuracy of physical models with the flexibility of data-driven models, enabling the model to maintain good generalization performance even in untrained conditions.

[0159] 3. Reduce modeling difficulty: For some components that are difficult to establish finite element models, hybrid modeling technology can use experimental test data instead, thereby greatly reducing the difficulty of NVH (noise, vibration and harshness) modeling.

[0160] 4. Improve simulation accuracy: During the hybrid modeling process, components that are difficult to model using finite elements or difficult to obtain accurate parameters can be replaced by experimental test data, which greatly improves the NVH simulation accuracy of complex models.

[0161] 5. Improve simulation efficiency: Experimental test data is used instead in complex models and large-scale models. Compared with full-scale finite element simulation models, the degree of freedom of experimental test data is greatly reduced, thereby greatly improving the simulation efficiency of system-level NVH models.

[0162] 6. Improved simulation depth: Hybrid modeling technology enables system-level NVH analysis and optimization of complete vehicles and complete machines, thereby improving the overall simulation depth of system-level NVH models.

[0163] 7. Make full use of existing data: Hybrid modeling technology can make full use of existing simulation and test data to achieve rapid analysis and optimization of vehicle NVH, saving development costs, shortening development cycles, and improving vehicle NVH performance.

[0164] 8. Combining the advantages of physical models and data-driven models: Hybrid models combine the clear physical meaning of physical models with the flexibility of data-driven models, promoting the improvement of the overall accuracy and reliability of the model.

[0165] 9. Improve the adaptability of the model: The hybrid model has better adaptability. When the internal physical mechanism of the research object changes dynamically, the updated model can be obtained by collecting new samples and retraining, avoiding repeated research on the mechanism process.

[0166] 10. Reduce the number of model estimation parameters and reduce the risk of overfitting: The prior knowledge contained in the mechanism model can narrow the parameter space search domain, reduce the number of model estimation parameters, and reduce the risk of overfitting.

[0167] In summary, mathematical motion models provide an accurate and flexible solution in the field of intelligent driving, which can improve the accuracy and reliability of vehicle control while reducing development costs and time.

[0168] The processor is configured to process the fused data of the multiple sensors according to a mathematical motion model.

[0169] The processor outputs the result to the output device 140. Examples of the output device 140 include a storage device, a visual display, a printer, an actuator (for manipulating a hardware device), or a controller (for controlling other systems).

[0170] Many output results are contemplated. For example, the algorithm can be configured to determine the position of a particular feature. This feature is one of the features tracked by one of the sensors, and its position is described as a point in three-dimensional space. The results can include guidance information for the reference frame. For example, position, attitude, orientation, velocity, acceleration, or other higher-order derivatives with respect to time can be calculated. In one embodiment, the results include the pose of the reference frame. Pose includes descriptions of both position and attitude. Orientation refers to a single degree of freedom, commonly referred to as heading. Attitude, on the other hand, includes three dimensions: roll, pitch, and yaw.

[0171] Outputs can include position, orientation, velocity (linear or rotational), acceleration (linear or rotational), or higher-order derivatives of position with respect to time.

[0172] The output can be of any dimension, including 1D, 2D, or 3D.

[0173] For example, the reference frame can include a vehicle (e.g., an unmanned fire truck) or a pedestrian. The sensor communicates with the processor and provides data relative to the reference frame. For example, a particular sensor can have multiple components, with one part fixed to the reference frame and another part fixed to the environment.

[0174] In other embodiments, a portion of the sensor is decoupled from the reference frame and provides data to a remote processor.

[0175] In one embodiment, a feature in space describes a specific point; therefore, its location in the environment can be identified using three degrees of freedom. In contrast to a feature, a reference frame can be viewed as a rigid body with six degrees of freedom. Specifically, the reference frame's degrees of freedom can be described as movement up and down, side to side, forward and backward, tilting up and down (pitch), turning left and right (yaw), and tilting left and right (roll).

[0176] The second sensor 120 includes an infrared thermal imager, which converts the received thermal radiation energy in the infrared band into an electrical signal. After amplification and shaping, the signal is converted into a digital signal after digital / analog conversion, which is displayed as an image on the monitor. The grayscale value of each point in the image corresponds to the radiation energy emitted by that point on the object being measured and reaching the photoelectric conversion device. After calculation, the radiation temperature value of each point on the surface of the object being measured can be read from the infrared thermal imager image, and this value will trigger a high temperature warning. In this way, the unmanned fire truck, as a reference system, can perceive the temperature status of objects around the fire scene, especially in low temperatures or at night.

[0177] The processor uses YOLO V3 to process sensor data, that is, uses the data of multiple sensors as input to YOLO V3 to enhance the environmental perception, target recognition and decision-making capabilities of the guidance system 100 for the fire warning and firefighting unmanned vehicle.

[0178] like Figure 4 As shown, YOLO V3 marks the first major evolution in the YOLO family. Its network architecture shifts from a small, single, shallow network to a deeper one consisting of 53 convolutional layers and residual connections. Residual connections are used to prevent gradient explosion, and stride-2 convolutions replace the max pooling operations found in YOLO V1 and YOLO V2. This network, called DarkNet-53, incorporates residual connections to optimize training. It also employs an efficient single-stage detector architecture, ensuring a balance between speed and accuracy, making it ideal for real-time applications. Following DarkNet-53, a spatial pyramid pooling (SPP) module is embedded, resulting in this version being named YOLO V3-spp. In addition to its deeper network, YOLO V3 recognizes that the primary weakness of previous YOLO versions was their single prediction model. Consequently, a major advancement is the incorporation of the multi-scale feature of FPN into the YOLO family. This feature performs predictions on grids of multiple scales, supporting object detection at multiple scales and enhancing detection capabilities for both small and large objects. This multi-scale detection approach offers significant advantages for multi-object perception in complex environments. Based on this improvement, the structure of the YOLO series of object detectors can be more precisely divided into three parts: the backbone network, the neck, and the detection head. Furthermore, YOLO V3 no longer uses the Softmax classifier. Instead, it uses binary cross-entropy to train an independent logistic classifier, transforming the detection problem into a multi-label classification problem. It can simultaneously detect objects in multiple categories and assign multiple attribute labels to each object. The loss function also replaces the classification loss with a binary cross-entropy loss. Because the new structural changes lead to inaccurate bounding box error calculations, the square roots of the height and width are added to the loss function to improve the prediction accuracy of the bounding box size.

[0179] In some embodiments, a guidance method for a fire warning and firefighting unmanned vehicle is provided, comprising the following steps:

[0180] Using a processor to receive image data generated by the monocular camera within an environment trajectory by the unmanned vehicle, wherein the image data includes multiple features observed in the environment by the unmanned vehicle in multiple postures along the environment trajectory;

[0181] The processor also receives motion data of the unmanned vehicle in the environment obtained by the monocular camera and multiple sensors; the multiple sensors are multiple sensors other than the monocular camera, which are used to provide motion data of the unmanned vehicle in the environment; the processor is in communication with the monocular camera and the multiple sensors;

[0182] The processor uses the Kalman filter algorithm to fuse the data obtained by multiple sensors and the monocular camera to complete environmental perception, and processes the fused data based on a mathematical motion model to obtain the trajectory of the unmanned vehicle. The mathematical motion model is based on the Kalman filter algorithm of the Kalman gain calculation network and adopts a hybrid modeling of kinematics and dynamics.

[0183] The technical solutions involved in the present invention include not only the technical solutions disclosed above, but also technical solutions composed of equivalent replacements of the technical features in the technical solutions involved in the present invention. Matters not covered in the present invention belong to the common knowledge of those skilled in the art.

Claims

1. A guidance system for an unmanned vehicle for fire early warning and fire fighting, characterized in that: include: a monocular camera for generating image data along a trajectory relative to the unmanned vehicle, wherein the image data includes a plurality of features observed in an environment at a plurality of poses of the unmanned vehicle along the trajectory; Multiple sensors other than the monocular camera, used to provide motion data of the unmanned vehicle in the environment; a processor, communicatively connected to the monocular camera and the plurality of sensors; The processor is configured to: The data obtained by the multiple sensors and the monocular camera are fused through the Kalman filter algorithm to complete environmental perception, and the fused data is processed based on a mathematical motion model to obtain the trajectory of the unmanned vehicle, wherein the mathematical motion model is based on the Kalman filter algorithm of the Kalman gain calculation network and adopts a hybrid modeling of kinematics and dynamics.

2. The guidance system for a fire early warning and fire fighting unmanned vehicle according to claim 1 is characterized in that: The Kalman filter algorithm is used to fuse the data obtained by the multiple sensors and the monocular camera to complete the environmental perception, including: The Kalman filter algorithm is used to perform weighted averaging on the data acquired by the multiple sensors and the monocular camera. However, unlike the weighted averaging method, the weight coefficient of the Kalman filter algorithm is determined by the measurement variance, and the two are inversely proportional.

3. The guidance system for a fire early warning and fire fighting unmanned vehicle according to claim 2, characterized in that: The Kalman filter algorithm used to implement environmental perception includes: Among them, X k is the state vector of the target at time k, X k+1 is the state vector at time k+1, which represents the state estimation of the target at the next moment; φ is the state transfer matrix; w k is the process noise, which has zero mean and positive definite covariance matrix Q k Gaussian noise vector; Z k is the observation vector of the sensor at a certain time; H is the measurement matrix; the observation noise v k is a matrix R with zero mean and positive definite covariance k Gaussian distribution measurement noise vector; By the observation vector Z k To estimate the target state vector X k The process includes two processes: time update and measurement update.

4. The guidance system for a fire early warning and fire fighting unmanned vehicle according to claim 3 is characterized in that: The time update process is a process of estimating the state at the next moment based on the state at the current moment, specifically as follows: in, is the target state estimate at time k+1, which is calculated based on the estimated state at time k; is the state estimate at time k; P k+1,k is the estimation error covariance matrix at time k+1, which describes the uncertainty of the estimation error and reflects the accuracy of the state estimation; P k,k is the estimated error covariance matrix at time k, which represents the uncertainty of the target state estimation; Q k is the process noise covariance matrix, which represents the process noise w k The variance of φ describes the uncertainty of the system process noise; T is the transpose of the matrix φ.

5. The guidance system for the fire early warning and fire fighting unmanned vehicle according to claim 4 is characterized in that: The measurement update process is a process of correcting the one-step prediction error based on the current measurement value and the previous one-step prediction error to obtain the estimated value of this measurement, which is specifically as follows: in, is the state estimate at time k-1, which is used to correct the state during the measurement update process; is the estimated value of the target state at the current time k; G k is the Kalman gain, which controls the weighting between the measured value and the predicted value. It is calculated by the current estimation error covariance and the measurement noise covariance, indicating the degree of trust in the current measurement data; Z k is the observation value at time k; R k is the measurement noise covariance matrix, which represents the observation noise v k The variance of , describes the noise characteristics of the sensor measurement; The residual is a measure of the deviation between the forecast and the actual situation and is used to adjust the estimate.

6. The guidance system for a fire early warning and fire fighting unmanned vehicle according to claim 1, characterized in that: The Kalman filter algorithm is divided into three parts: state prediction, Kalman gain calculation and state update.

7. The guidance system for the fire early warning and fire fighting unmanned vehicle according to claim 6, characterized in that: The state prediction is to calculate the prior state and state observation at the current moment according to the state equation and observation equation, as follows: Represents the prior state estimate of the state variable at the current time k, that is, the state predicted based on the posterior state at the previous time k-1 and the input variables; The posterior state at the previous moment k-1 is the optimal state calculated through observation data and state update; u k-1 is the input variable at the previous moment k-1; g(.) is a nonlinear function of state change, which is used to describe how the system derives the prior state at the current moment from the state and input at the previous moment; is the predicted value of the observation at the current moment k, that is, the observation value predicted based on the prior state; h(.) is the observation model function, which describes the relationship between the state and the observation.

8. The guidance system for the fire early warning and fire fighting unmanned vehicle according to claim 6 is characterized in that: The Kalman gain calculation is to first calculate the residual of the state estimate and the residual of the observation, and then use the two residuals as the input of the Kalman gain calculation network to calculate the Kalman gain at the current moment, as follows: is the residual of the state estimation, that is, the difference between the current prior state and the posterior state at the previous moment; z k is the observation value at the current moment k, the actual observation data obtained from the sensor or other measuring device; Δy is the observation residual, that is, the difference between the actual observation value and the predicted observation value; K is the Kalman gain at the current moment, which is an important parameter for adjusting the state estimation; the Kalman gain calculation network is a network based on deep learning or other algorithms, which is used to input state residuals and observation residuals and output Kalman gain.

9. The guidance system for a fire early warning and fire fighting unmanned vehicle according to claim 1, characterized in that: The processor is further configured to process the sensor data using YOLO V3, which is a deep network including 53 convolutional layers and residual connections.

10. A guidance method for an unmanned vehicle for fire early warning and fire fighting, characterized in that: The following steps are involved: Using a processor to receive image data generated by the unmanned vehicle within a trajectory from a monocular camera, wherein the image data includes a plurality of features observed in an environment at a plurality of postures of the unmanned vehicle along the trajectory; The processor also receives motion data of the unmanned vehicle in the environment acquired by a monocular camera and a plurality of sensors; the plurality of sensors are multiple sensors other than the monocular camera, and are used to provide motion data of the unmanned vehicle in the environment; the processor is in communication with the monocular camera and the plurality of sensors; The processor uses a Kalman filter algorithm to fuse the data obtained by the multiple sensors and the monocular camera to complete environmental perception, and processes the fused data based on a mathematical motion model to obtain the trajectory of the unmanned vehicle, wherein the mathematical motion model is based on a Kalman filter algorithm of a Kalman gain calculation network and adopts a hybrid modeling of kinematics and dynamics.

Images