Mine vehicle intelligent navigation and safe driving method and system based on inertial navigation
Through multi-sensor data fusion and path planning algorithms, the navigation accuracy and stability of mine vehicles are solved, high-precision navigation and safe driving are achieved, and the intelligence and operation efficiency of mine vehicles are improved.
Patent Information
- Application Number
- CN202411077724.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-07
- Publication Date
- 2024-11-22
AI Technical Summary
The existing mining vehicle navigation systems have problems such as insufficient navigation accuracy, poor system stability and limited environmental perception capabilities in complex mining environments, which are difficult to meet the requirements of high accuracy and high reliability.
Using intelligent navigation and safe driving methods based on inertial navigation, the multi-sensor data fusion of inertial measurement unit (IMU), global positioning system (GPS), lidar (LiDAR), ultrasonic sensors, vision sensors and other multi-sensor data, combined with an extended Kalman filter and path planning algorithms to realize high-precision navigation and safe driving of vehicles.
It improves the vehicle's positioning accuracy and navigation stability, detects and recognizes sensor abnormalities in real time, ensures system safety and reliability, automatically avoids obstacles, optimizes driving paths, and improves the intelligence level and operation efficiency of mining vehicles.
Smart Images

Figure CN119002482A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of intelligent navigation technology, and in particular to an intelligent navigation and safe driving method and system for mining vehicles based on inertial navigation. Background Art
[0002] The mining environment is complex and the working conditions are harsh, which places extremely high demands on the navigation and driving of mining vehicles. Traditional mining vehicle navigation systems mainly rely on GPS for positioning and navigation. However, in environments such as mines or underground mines where GPS signals are weak or completely lost, navigation methods that rely solely on GPS are difficult to meet the requirements of high precision and high reliability. In order to improve the navigation accuracy and safety of mining vehicles, multi-sensor fusion technology must be used, combined with advanced navigation and control algorithms, to achieve accurate navigation and intelligent driving of mining vehicles.
[0003] Inertial navigation technology (INS) measures the acceleration and angular velocity of the vehicle and uses integration to calculate the position, velocity and attitude of the vehicle. However, the inertial navigation system has the problem of cumulative error, that is, as time goes by, the positioning error will continue to accumulate, resulting in a decrease in accuracy. In order to overcome this problem, the inertial navigation system usually fuses data with other sensors (such as GPS, LiDAR, visual sensors, etc.), using multi-source information to complement and correct each other, thereby improving the accuracy and stability of the overall navigation system.
[0004] In recent years, with the rapid development of sensor technology, computer technology and artificial intelligence technology, multi-sensor fusion navigation systems have been widely studied and applied in the field of mining vehicles. Typical multi-sensor fusion methods include Kalman Filter, Extended Kalman Filter (EKF) and Particle Filter. These methods can effectively reduce the error of a single sensor and improve the robustness and reliability of the navigation system by weighted fusion of data from different sensors.
[0005] In the actual application of mining vehicles, environmental perception and path planning are also crucial. Environmental perception mainly uses devices such as LiDAR, ultrasonic sensors and visual sensors to achieve real-time perception of the surrounding environment, detect and identify obstacles, road boundaries and other important information. Path planning generates the optimal driving path based on known environmental information and target location, guiding the vehicle to reach the destination safely and efficiently.
[0006] In addition, safe driving of mining vehicles is also a key issue. Due to the complex and changeable mining environment, vehicles may encounter various emergencies during driving, such as road collapse, sudden appearance of obstacles, etc. Therefore, the mining vehicle navigation system needs to have real-time anomaly detection and fault diagnosis capabilities to promptly detect and handle potential dangers and ensure the safety of vehicles and operators.
[0007] In summary, although the existing mining vehicle navigation system can meet the basic navigation needs to a certain extent, it still has problems such as insufficient navigation accuracy, poor system stability, and limited environmental perception in complex mining environments. Therefore, designing an intelligent navigation and safe driving system for mining vehicles based on inertial navigation, making full use of multi-sensor fusion technology and advanced control algorithms, and improving navigation accuracy, system stability and safety is the key to solving the intelligent navigation and safe driving of mining vehicles. Summary of the invention
[0008] In order to solve the technical problems of insufficient navigation accuracy, poor system stability and limited environmental perception capability in the prior art, the present invention provides an intelligent navigation and safe driving method and system for mining vehicles based on inertial navigation.
[0009] The technical solution provided by the present invention is as follows:
[0010] First aspect:
[0011] The invention provides a method for intelligent navigation and safe driving of mining vehicles based on inertial navigation, comprising:
[0012] S1, obtain the initial position, speed and attitude data of the mining vehicle, and initialize the initial position P0, initial speed v0 and attitude quaternion q0 through the global positioning system (GPS) and inertial measurement unit (IMU);
[0013] S2. The inertial navigation system uses the acceleration and angular velocity data provided by the IMU and the inertial navigation equation to update the position, velocity and attitude of the vehicle in real time. The position update formula is as follows:
[0014]
[0015] In the above formula, P k represents the position at time k, v k represents the speed at time k, a k represents the acceleration at time k, Δt represents the time step, and the velocity update formula is as follows:
[0016] v k+1 =v k +a k Δt
[0017] The posture update formula is as follows:
[0018]
[0019] In the above formula, Δq represents the quaternion increment obtained by integrating the angular velocity. Represents quaternion multiplication, and the calculation formula of the quaternion increment Δq is as follows:
[0020]
[0021] In the above formula, ω k represents the angular velocity at time k;
[0022] S3, using an extended Kalman filter (EKF) to fuse the IMU data and other sensor data to improve navigation accuracy, the other sensors include the GPS, LiDAR, ultrasonic sensor, visual sensor, magnetometer and odometer, the EKF includes two steps of prediction and update;
[0023] S4, using the mine road map information and the current status of the vehicle, to plan the route, using A * The algorithm performs path planning, and the main steps are as follows:
[0024] Calculation of the heuristic function h(n):
[0025]
[0026] In the above formula, n represents the current node, (x n ,y n ) represents the coordinates of node n, (x goal ,y goal ) represents the coordinates of the target node;
[0027] Calculation of the cost function g(n):
[0028] g(n)=g(n parent )+cost(n parent , n)
[0029] In the above formula, n parent represents the parent node of node n, cost(n parent , n) represents the cost from the parent node to the current node;
[0030] Calculation of the total estimated cost f(n):
[0031] f(n)=g(n)+h(n)
[0032] Node expansion and path selection:
[0033] Repeatedly expand all child nodes of the current node, calculate the value of its heuristic function f(n), and select the node with the smallest f(n) value for the next expansion until the target node is reached;
[0034] S5. Combine path planning and environment perception to perform obstacle avoidance in real time. The obstacle avoidance strategies include dynamic window method (DWA) and artificial potential field method (APF).
[0035] S6. Use a PID controller or model predictive control (MPC) to control the vehicle.
[0036] Second aspect:
[0037] The inertial navigation-based intelligent navigation and safe driving system for mining vehicles provided by the present invention comprises:
[0038] A sensor module, a data processing module, a control module, a status monitoring and self-diagnosis module, and a communication module. The sensor module is used to collect data required during the operation of the vehicle. The sensor module includes an inertial measurement unit (IMU), a global positioning system (GPS), a laser radar (LiDAR), an ultrasonic sensor, a visual sensor, a magnetometer, and an odometer. The IMU is used to obtain acceleration and angular velocity data of the vehicle. The GPS is used to provide location information of the vehicle. The laser radar is used for environmental perception and obstacle detection. The ultrasonic sensor is used for close-range obstacle detection. The visual sensor is used to identify road signs, detect obstacles, and assist in navigation. The magnetometer is used to provide orientation information of the vehicle. The odometer is used to measure the travel distance of the vehicle. The data collected by the sensor module is transmitted to the data processing module.
[0039] The data processing module is used to fuse and process the sensor data to generate high-precision position information and motion status of the vehicle, and the data processing module transmits the processed results to the control module and the status monitoring and self-diagnosis module;
[0040] The control module generates a vehicle control instruction according to the navigation information output by the data processing module to realize intelligent navigation and safe driving, and the control module transmits the control instruction generated according to the information of the data processing module to the vehicle actuator;
[0041] The state monitoring and self-diagnosis module is used to monitor the running state of the vehicle in real time to ensure the stability and safety of the system. The state monitoring and self-diagnosis module transmits the monitoring results to the communication module;
[0042] The communication module is used for data communication between the vehicle and the control center to realize remote monitoring and operation, including wireless communication and data storage. The communication module transmits data to the remote control center through wireless communication to realize remote monitoring and operation.
[0043] The beneficial effects brought about by the technical solution provided by the present invention include at least:
[0044] (1) In the present invention, by fusing multiple sensor data such as inertial measurement unit (IMU), global positioning system (GPS), laser radar (LiDAR), ultrasonic sensor and visual sensor, and adopting extended Kalman filter (EKF) for data fusion, the positioning accuracy and navigation stability of the vehicle are greatly improved. Especially when the GPS signal is weak or fails, the system can still rely on the comprehensive data of inertial navigation and other sensors to provide continuous and reliable position information, ensuring the stable operation of the vehicle in a complex mining environment;
[0045] (2) In the present invention, by introducing mean shift detection and Kalman filter residual analysis algorithm, the system can detect and identify abnormal conditions in sensor data in real time, preventing erroneous data from affecting vehicle navigation and control. At the same time, the status monitoring and self-diagnosis module monitors the operating status of the vehicle in real time, including battery power, sensor status, etc., and can diagnose and alarm in time when a fault occurs, thereby improving the safety and reliability of the system and ensuring the smooth progress of mining operations;
[0046] (3) In the present invention, the system has built-in path planning algorithms and advanced control algorithms (such as PID controller and model predictive control MPC), which can intelligently plan the vehicle's driving path and perform precise speed and steering control based on real-time perceived environmental information and predetermined goals. The system can automatically avoid obstacles and optimize the driving path in a mining environment, thereby improving operating efficiency and reducing the need for manual intervention, greatly improving the intelligence level and operating efficiency of mining vehicles. BRIEF DESCRIPTION OF THE DRAWINGS
[0047] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings in the following description are only some embodiments of the present invention. For ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without paying creative work.
[0048] Figure 1 A schematic diagram of a flow chart of a method for intelligent navigation and safe driving of a mining vehicle based on inertial navigation provided by an embodiment of the present invention;
[0049] Figure 2A schematic diagram of the structure of an intelligent navigation and safe driving system for mining vehicles based on inertial navigation provided in an embodiment of the present invention. DETAILED DESCRIPTION
[0050] The technical solution of the present invention is described below in conjunction with the accompanying drawings.
[0051] In the embodiments of the present invention, words such as "exemplarily" and "for example" are used to indicate examples, illustrations or explanations. Any embodiment or design described as "example" in the present invention should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of the word "example" is intended to present the concept in a specific way. In addition, in the embodiments of the present invention, the meaning expressed by "and / or" can be both, or it can be either of the two.
[0052] In the embodiments of the present invention, "image" and "picture" can sometimes be used interchangeably. It should be noted that when the difference between them is not emphasized, the meanings they intend to express are the same. "of", "corresponding, relevant" and "corresponding" can sometimes be used interchangeably. It should be noted that when the difference between them is not emphasized, the meanings they intend to express are the same.
[0053] In the embodiments of the present invention, sometimes a subscript such as W1 may be mistakenly written as a non-subscript such as W1. When the difference is not emphasized, the meanings to be expressed are consistent.
[0054] In order to make the technical problems, technical solutions and advantages to be solved by the present invention more clear, a detailed description will be given below with reference to the accompanying drawings and specific embodiments.
[0055] Reference Manual Attached Figure 1 , showing a flow chart of the method for intelligent navigation and safe driving of a mining vehicle based on inertial navigation provided by an embodiment of the present invention.
[0056] The embodiment of the present invention provides a method for intelligent navigation and safe driving of a mining vehicle based on inertial navigation. The processing flow of the method may include the following steps:
[0057] S1, obtain the initial position, speed and attitude data of the mining vehicle, and initialize the initial position P0, initial speed v0 and attitude quaternion q0 through the global positioning system (GPS) and inertial measurement unit (IMU);
[0058] S2. The inertial navigation system uses the acceleration and angular velocity data provided by the IMU and the inertial navigation equation to update the position, velocity and attitude of the vehicle in real time. The position update formula is as follows:
[0059]
[0060] In the above formula, P k represents the position at time k, v k represents the speed at time k, a k represents the acceleration at time k, Δt represents the time step, and the velocity update formula is as follows:
[0061] v k+1 =v k +a k Δt
[0062] The posture update formula is as follows:
[0063]
[0064] In the above formula, Δq represents the quaternion increment obtained by integrating the angular velocity. Represents quaternion multiplication, and the calculation formula of the quaternion increment Δq is as follows:
[0065]
[0066] In the above formula, ω k represents the angular velocity at time k;
[0067] S3. Use the extended Kalman filter (EKF) to fuse IMU data with other sensor data to improve navigation accuracy. Other sensors include GPS, LiDAR, ultrasonic sensor, visual sensor, magnetometer and odometer. EKF includes two steps: prediction and update.
[0068] S4. Use the map information of the mine road and the current status of the vehicle to plan the path. The A* algorithm is used for path planning. The main steps are as follows:
[0069] Calculation of the heuristic function h(n):
[0070]
[0071] In the above formula, n represents the current node, (x n ,y n ) represents the coordinates of node n, (x goal ,y goal ) represents the coordinates of the target node;
[0072] Calculation of the cost function g(n):
[0073] g(n)=g(n parent )+coSt(n parent , n)
[0074] In the above formula, n parentrepresents the parent node of node n, cost(n parent , n) represents the cost from the parent node to the current node;
[0075] Calculation of the total estimated cost f(n):
[0076] f(n)=g(n)+h(n)
[0077] Node expansion and path selection:
[0078] Repeatedly expand all child nodes of the current node, calculate the value of its heuristic function f(n), and select the node with the smallest f(n) value for the next expansion until the target node is reached;
[0079] S5. Combine path planning and environment perception to perform obstacle avoidance in real time. The obstacle avoidance strategies include dynamic window method (DWA) and artificial potential field method (APF).
[0080] S6. Use a PID controller or model predictive control (MPC) to control the vehicle.
[0081] It should be further explained that the EKF in S3 includes:
[0082] Prediction Steps:
[0083] The state vector x k =[P k , v k ,q k , b a , b ω ], where b a and b ω Respectively represent the bias of acceleration and angular velocity, the state prediction equation:
[0084] x k|k-1 =f(x k-1 ,u k )
[0085] In the above formula, u k =[a k ,ω k ] indicates control input,
[0086] Prediction covariance:
[0087]
[0088] In the above formula, F k represents the state transfer matrix, Q k represents the process noise covariance matrix;
[0089] Update steps:
[0090] Observation equation:
[0091] z k =h(x k )+v k
[0092] In the above formula, z k represents the observed value, v k Represents observation noise, and for N sensors, provides observation values The observation model is:
[0093]
[0094] The fusion of observations is performed in the following steps:
[0095] Kalman gain calculation:
[0096]
[0097] In the above formula, H k represents the observation matrix, R k represents the observation noise covariance matrix;
[0098] Status Update:
[0099] x k =x k|k-1 +K k (z k -h(x k|k-1 ))
[0100] In the above formula, Represents the observation value vector of all sensors;
[0101] Update the covariance:
[0102] P k =(IK k H k ) k|k-1
[0103] Through the above sensor data fusion process, high-precision positioning and navigation of mining vehicles in complex environments can be achieved.
[0104] Assuming there are three sensors: IMU, GPS and LiDAR, the specific observation equation and fusion process are as follows:
[0105] IMU data:
[0106]
[0107] GPS Data:
[0108]
[0109] LiDAR Data:
[0110]
[0111] Fusion process:
[0112] Kalman gain:
[0113]
[0114] In the above formula,
[0115] Status Update:
[0116]
[0117] Update the covariance:
[0118] P k =(IK k H k ) k|k-1
[0119] Through the above complex and precise sensor data fusion process, high-precision positioning and navigation of mining vehicles in complex environments can be achieved.
[0120] In a possible implementation, the dynamic window method in S5 includes:
[0121] Search for the optimal speed that satisfies the dynamic constraints in the speed space, so that obstacles can be avoided and the target can be approached within a given time window. The objective function is:
[0122] G(v,ω)=α·heading(v,ω)+β·distance(v,ω)+γ·velocity(v,ω)
[0123] In the above formula, α, β and γ represent weight coefficients.
[0124] In a possible implementation, the artificial potential field method in S5 includes:
[0125] Calculate the resultant force on the vehicle, which is composed of the target attraction and the obstacle repulsion. The attraction is:
[0126] F att =-k att (PP goal )
[0127] Repulsion:
[0128]
[0129] In the above formula, P represents the vehicle position, O represents the obstacle position, d(P, O) represents the distance from the vehicle to the obstacle, d0 represents the impact range, and k att and k rep represents a constant, and the resultant force is F = F att +F rep .
[0130] In a possible implementation, the PID controller in S6 includes:
[0131] Speed Control:
[0132]
[0133] In the above formula, v ref represents the reference speed, v represents the actual speed, K p , K i and K d Represents the proportional, integral, and derivative gains of the PID controller;
[0134] Steering Control:
[0135]
[0136] In the above formula, θ ref represents the reference steering angle, θ represents the actual steering angle, K p , K i and K d Represents the proportional, integral, and derivative gains of the PID controller.
[0137] In a possible implementation, the model predictive control in S6 includes:
[0138] Set the prediction time domain N and define the cost function:
[0139]
[0140] In the above formula, x(k+i|k) represents the state at the i-th moment predicted at time k, and x ref represents the reference state, u(k+i|k) represents the control input, Q and R represent the weight matrices of the state and control input respectively;
[0141] Optimizing control input:
[0142]
[0143] Constraints:
[0144] x(k+i+1lk)=A x (k+i|k)+Bu(k+i|k)
[0145] umin ≤u(k+i|k)≤u max .
[0146] In a possible implementation, it also includes status monitoring and self-diagnosis:
[0147] Real-time monitoring of vehicle status to ensure stable system operation. Monitoring parameters include battery power and sensor working status. By analyzing sensor data, faults can be detected and diagnosed to ensure safe operation of the vehicle. Battery power monitoring:
[0148]
[0149] In the above formula, SOC represents the battery state of charge, Q remaining Indicates the remaining power, Q total Indicates the total battery charge;
[0150] Sensor working status monitoring:
[0151] By periodically checking the sensor output, it detects whether there is an abnormality. The anomaly detection algorithm includes mean drift detection and Kalman filter residual analysis.
[0152] It should be further explained that mean shift detection includes:
[0153] Mean shift detection identifies anomalies by detecting whether the mean of the sensor output changes significantly. The sensor data output data is {x1, x2, …, x n}, the specific steps of mean shift detection are as follows:
[0154] Calculate the sliding window mean, select a sliding window of length w, calculate the mean of the data in each window, for the i-th window, the mean is:
[0155]
[0156] Calculate the mean change, calculate the change of the mean of adjacent sliding windows, and define the mean change as:
[0157] Δμ i =μ i+1 -μ i
[0158] Set a threshold, set a threshold δ, if |Δμ i | If it exceeds δ, it is considered abnormal;
[0159] Detect anomalies and calculate Δμ for each window i , if |Δμ i |>δ, an abnormality is detected.
[0160] It should be further explained that the Kalman filter residual analysis includes:
[0161] Kalman filter residual analysis identifies anomalies by detecting the residual of the filter. The residual of the filter is the difference between the observed value and the predicted value. The system state vector is x k , the observation vector is z k , the specific steps of residual analysis of Kalman filter are as follows:
[0162] Prediction step, predicting the current state and observed values:
[0163] x k|k-1 =Ax k-1|k-1 +Bu k
[0164] z k|k-1 =Hx k|k-1
[0165] Compute the residuals, which are defined as the difference between the observed and predicted observed values:
[0166] r k =z k -z k|k-1
[0167] Calculate the residual covariance, which is defined as:
[0168] S k =HP k|k-1 H T +R
[0169] In the above formula, P k|k-1 represents the prediction error covariance, and R represents the observation noise covariance;
[0170] Set the anomaly detection threshold γ, which is set according to the chi-square distribution. The threshold value depends on the dimension of the residual and the expected confidence level;
[0171] Detect anomalies and compute standardized residuals:
[0172]
[0173] If the standardized residual is greater than the threshold γ, it is determined that an abnormality is detected.
[0174] Assume that there is an IMU and GPS data of a mining vehicle, and the observation data of the two are fused through Kalman filtering, and anomaly detection is performed at the same time:
[0175] Kalman filter initialization, state vector:
[0176]
[0177] State transition matrix:
[0178]
[0179] Observation matrix:
[0180] H =
[10]
[0181] Fusion Process and Anomaly Detection:
[0182] Prediction Steps:
[0183] x k|k-1 =Ax k-1|k-1 +Bu k
[0184] P k|k-1 =AP k - 1|k -1A T +Q
[0185] Compute the residual:
[0186] Z k|k-1 =Hx k|k-1
[0187] r k =z k -Z k|k-1
[0188] Compute the residual covariance:
[0189] S k =HP k|k-1 H T +R
[0190] Detecting anomalies:
[0191] Compute the standardized residuals:
[0192]
[0193] If the standardized residual is greater than the threshold γ, it is determined that an abnormality is detected.
[0194] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:
[0195] (1) In the present invention, by fusing multiple sensor data such as inertial measurement unit (IMU), global positioning system (GPS), laser radar (LiDAR), ultrasonic sensor and visual sensor, and adopting extended Kalman filter (EKF) for data fusion, the positioning accuracy and navigation stability of the vehicle are greatly improved. Especially when the GPS signal is weak or fails, the system can still rely on the comprehensive data of inertial navigation and other sensors to provide continuous and reliable position information, ensuring the stable operation of the vehicle in a complex mining environment;
[0196] (2) In the present invention, by introducing mean shift detection and Kalman filter residual analysis algorithm, the system can detect and identify abnormal conditions in sensor data in real time, preventing erroneous data from affecting vehicle navigation and control. At the same time, the status monitoring and self-diagnosis module monitors the operating status of the vehicle in real time, including battery power, sensor status, etc., and can diagnose and alarm in time when a fault occurs, thereby improving the safety and reliability of the system and ensuring the smooth progress of mining operations;
[0197] (3) In the present invention, the system has built-in path planning algorithms and advanced control algorithms (such as PID controller and model predictive control MPC), which can intelligently plan the vehicle's driving path and perform precise speed and steering control based on real-time perceived environmental information and predetermined goals. The system can automatically avoid obstacles and optimize the driving path in a mining environment, thereby improving operating efficiency and reducing the need for manual intervention, greatly improving the intelligence level and operating efficiency of mining vehicles.
[0198] Reference Manual Attached Figure 2 , shows a schematic structural diagram of an intelligent navigation and safe driving system for mining vehicles based on inertial navigation provided by an embodiment of the present invention.
[0199] The present invention also provides an intelligent navigation and safe driving system for a mining vehicle based on inertial navigation, which is applied to the above-mentioned intelligent navigation and safe driving method for a mining vehicle based on inertial navigation, and comprises:
[0200] Sensor module, data processing module, control module, status monitoring and self-diagnosis module, and communication module. The sensor module is used to collect data required during the operation of the vehicle. The sensor module includes an inertial measurement unit (IMU), a global positioning system (GPS), a laser radar (LiDAR), an ultrasonic sensor, a visual sensor, a magnetometer, and an odometer. The IMU is used to obtain the acceleration and angular velocity data of the vehicle. The GPS is used to provide the location information of the vehicle. The laser radar is used for environmental perception and obstacle detection. The ultrasonic sensor is used for close-range obstacle detection. The visual sensor is used to identify road signs, detect obstacles and assist navigation. The magnetometer is used to provide the vehicle's orientation information. The odometer is used to measure the vehicle's travel distance.
[0201] The data processing module is used to fuse and process sensor data to generate high-precision location information and motion status of the vehicle, including data preprocessing, sensor data fusion and anomaly detection. Data preprocessing is to filter and denoise sensor data. Sensor data fusion is to use extended Kalman filter (EKF) to fuse IMU, GPS, LiDAR and other sensor data to improve positioning accuracy. Anomaly detection is to use mean drift detection and Kalman filter residual analysis algorithm to detect anomalies in sensor data and ensure data reliability.
[0202] The control module generates vehicle control instructions based on the navigation information output by the data processing module to achieve intelligent navigation and safe driving, including path planning, speed control and steering control. Path planning is to plan the vehicle's driving path based on environmental perception information and predetermined goals. Speed control is to adjust the vehicle speed using a PID controller or model predictive control (MPC). Steering control is to adjust the vehicle's steering angle based on the path planning results.
[0203] The status monitoring and self-diagnosis module is used to monitor the operating status of the vehicle in real time to ensure the stability and safety of the system, including battery monitoring, sensor status monitoring and fault diagnosis. Battery monitoring is to monitor the battery power in real time and calculate the battery state of charge (SOC). Sensor status monitoring is to regularly check the sensor output to detect whether there is an abnormality. Fault diagnosis is to monitor and diagnose system faults by analyzing sensor data.
[0204] The communication module is used for data communication between the vehicle and the control center to achieve remote monitoring and operation, including wireless communication and data storage. Wireless communication is used to achieve data transmission between the vehicle and the control center, and data storage is used to store vehicle operation data for subsequent analysis and backtracking.
[0205] The workflow of the above system is as follows:
[0206] Data collection: The sensor module collects vehicle acceleration, angular velocity, position information, environmental perception information and other data.
[0207] Data preprocessing: The data processing module filters and denoises the collected data.
[0208] Data fusion: Extended Kalman filter (EKF) is used to fuse IMU, GPS, LiDAR and other sensor data to generate high-precision position information and motion status.
[0209] Anomaly Detection: Detect anomalies in sensor data using mean shift detection and Kalman filter residual analysis algorithms.
[0210] Path planning: The control module plans the vehicle's driving path based on environmental perception information and predetermined goals.
[0211] Speed and steering control: Use PID controller or model predictive control (MPC) to generate vehicle speed and steering control commands.
[0212] Condition monitoring and self-diagnosis: The condition monitoring and self-diagnosis module monitors the vehicle's operating status in real time, detects and diagnoses faults, and ensures the stability and safety of the system.
[0213] Data communication: The communication module realizes data transmission between the vehicle and the control center, supporting remote monitoring and operation.
[0214] Through the above modules and processes, the intelligent navigation and safe driving system for mining vehicles based on inertial navigation can achieve high-precision navigation and safe driving of mining vehicles, ensuring reliable operation in complex environments.
[0215] The sensor module is connected to the data processing module. The sensor module is the data source of the system and is responsible for collecting various necessary sensor data. The sensor module transmits data to the data processing module through a data bus or wireless connection. The data processing module performs preprocessing, fusion and anomaly detection on these data. The connection method is a wired data bus (such as CAN bus) or a wireless connection. The data flow is from the sensor module to the data processing module.
[0216] The data processing module is connected to the control module. The data processing module transmits the fused high-precision position information and motion status to the control module. The control module performs path planning and vehicle control based on this information. The connection mode is memory sharing or data bus. The data flow direction is from the data processing module to the control module.
[0217] The control module is connected to the vehicle actuators. The control instructions generated by the control module need to be passed to the vehicle actuators (such as motor controllers and steering devices). These actuators perform specific operations according to the control instructions. The connection method is wired connection (such as CAN bus). The data flow is from the control module to the vehicle actuator.
[0218] The data processing module is connected to the state monitoring and self-diagnosis module. The data processing module will pass the processed data and detected abnormal information to the state monitoring and self-diagnosis module. The state monitoring and self-diagnosis module monitors the system status and diagnoses and alarms when a fault is detected. The connection mode is memory sharing or data bus. The data flow is from the data processing module to the state monitoring and self-diagnosis module.
[0219] The status monitoring and self-diagnosis module is connected to the communication module. The status monitoring and self-diagnosis module transmits the monitored system status information and diagnosis results to the communication module. The communication module is responsible for transmitting this information to the remote control center to support remote monitoring and operation. The connection method is wired or wireless. The data flow is from the status monitoring and self-diagnosis module to the communication module.
[0220] The communication module is connected to the control center. The communication module transmits the vehicle's status information, diagnostic results, and sensor data to the remote control center through wireless communication. The control center can monitor and remotely control the vehicle. The connection method is wireless communication (such as 4G / 5G network). The data flow is from the communication module to the control center.
[0221] The inertial navigation-based intelligent navigation and safe driving system for mining vehicles provided by the present invention can execute the above-mentioned inertial navigation-based intelligent navigation and safe driving method for mining vehicles, and achieve the same or similar technical effects. To avoid repetition, the present invention will not go into details.
[0222] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:
[0223] In the present invention, by fusing multiple sensor data such as an inertial measurement unit (IMU), a global positioning system (GPS), a laser radar (LiDAR), an ultrasonic sensor, and a visual sensor, and adopting an extended Kalman filter (EKF) for data fusion, the positioning accuracy and navigation stability of the vehicle are greatly improved. In particular, when the GPS signal is weak or fails, the system can still rely on the comprehensive data of inertial navigation and other sensors to provide continuous and reliable position information, thereby ensuring the stable operation of the vehicle in a complex mining environment.
[0224] (2) In the present invention, by introducing mean shift detection and Kalman filter residual analysis algorithm, the system can detect and identify abnormal conditions in sensor data in real time, preventing erroneous data from affecting vehicle navigation and control. At the same time, the status monitoring and self-diagnosis module monitors the operating status of the vehicle in real time, including battery power, sensor status, etc., and can diagnose and alarm in time when a fault occurs, thereby improving the safety and reliability of the system and ensuring the smooth progress of mining operations;
[0225] (3) In the present invention, the system has built-in path planning algorithms and advanced control algorithms (such as PID controller and model predictive control MPC), which can intelligently plan the vehicle's driving path and perform precise speed and steering control based on real-time perceived environmental information and predetermined goals. The system can automatically avoid obstacles and optimize the driving path in a mining environment, thereby improving operating efficiency and reducing the need for manual intervention, greatly improving the intelligence level and operating efficiency of mining vehicles.
[0226] The above contents are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art who is familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the above claims.
[0227] There are a few points to note:
[0228] (1) The drawings of the embodiments of the present invention only relate to the structures related to the embodiments of the present invention, and other structures may refer to the general design.
[0229] (2) For the sake of clarity, in the drawings used to describe the embodiments of the present invention, the thickness of the layers or regions is exaggerated or reduced, that is, these drawings are not drawn according to the actual scale. It is understood that when an element such as a layer, film, region or substrate is referred to as being "on" or "under" another element, the element may be "directly" "on" or "under" the other element or there may be intermediate elements.
[0230] (3) In the absence of conflict, the embodiments of the present invention and the features therein may be combined with each other to obtain new embodiments.
[0231] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. The protection scope of the present invention shall be based on the protection scope of the claims.
Claims
1. A method for intelligent navigation and safe driving of mining vehicles based on inertial navigation, characterized in that: include: S1, obtain the initial position, speed and attitude data of the mining vehicle, and initialize the initial position P0, initial speed v0 and attitude quaternion q0 through the global positioning system (GPS) and inertial measurement unit (IMU); S2. The inertial navigation system uses the acceleration and angular velocity data provided by the IMU and the inertial navigation equation to update the position, velocity and attitude of the vehicle in real time. The position update formula is as follows: In the above formula, P k represents the position at time k, v k represents the speed at time k, a k represents the acceleration at time k, Δt represents the time step, and the velocity update formula is as follows: v k+1 =v k +a k Δt The posture update formula is as follows: In the above formula, Δq represents the quaternion increment obtained by integrating the angular velocity. Represents quaternion multiplication, and the calculation formula of the quaternion increment Δq is as follows: In the above formula, ω k represents the angular velocity at time k; S3, using an extended Kalman filter (EKF) to fuse the IMU data and other sensor data to improve navigation accuracy, the other sensors include the GPS, LiDAR, ultrasonic sensor, visual sensor, magnetometer and odometer, the EKF includes two steps of prediction and update; S4, using the mine road map information and the current status of the vehicle, to plan the route, using A * The algorithm performs path planning, and the main steps are as follows: Calculation of the heuristic function h(n): In the above formula, n represents the current node, (x n ,y n ) represents the coordinates of node n, (x goal ,y goal ) represents the coordinates of the target node; Calculation of the cost function g(n): g(n)=g(n parent )+cost(n parent ,n) In the above formula, n parent represents the parent node of node n, cost(n parent ,n) represents the cost from the parent node to the current node; Calculation of the total estimated cost f(n): f(n)=g(n)+h(n) Node expansion and path selection: Repeatedly expand all child nodes of the current node, calculate the value of its heuristic function f(n), and select the node with the smallest f(n) value for the next expansion until the target node is reached; S5. Combine path planning and environment perception to perform obstacle avoidance in real time. The obstacle avoidance strategies include dynamic window method (DWA) and artificial potential field method (APF). S6. Use a PID controller or model predictive control (MPC) to control the vehicle.
2. The method for intelligent navigation and safe driving of mining vehicles based on inertial navigation according to claim 1 is characterized in that: The EKF in S3 includes: Prediction Steps: The state vector x k =[P k ,v k ,q k ,b a ,b ω ], where b a and b ω Respectively represent the bias of acceleration and angular velocity, the state prediction equation: x k|k-1 =f(x k-1 ,u k ) In the above formula, u k =[a k ,ω k ] indicates control input, Prediction covariance: In the above formula, F k represents the state transfer matrix, Q k represents the process noise covariance matrix; Update steps: Observation equation: z k =h(x k )+v k In the above formula, z k represents the observed value, v k Represents observation noise, and for N sensors, provides observation values The observation model is: The fusion of observations is performed in the following steps: Kalman gain calculation: In the above formula, H k represents the observation matrix, R k represents the observation noise covariance matrix; Status Update: x k =x k|k-1 +K k (z k -h(x k|k-1 )) In the above formula, Represents the observation value vector of all sensors; Update the covariance: P k =(I-K k H k )P k|k-1 Through the above sensor data fusion process, high-precision positioning and navigation of mining vehicles in complex environments can be achieved.
3. The method for intelligent navigation and safe driving of mining vehicles based on inertial navigation according to claim 1 is characterized in that: The dynamic window method in S5 includes: Search for the optimal speed that satisfies the dynamic constraints in the speed space, so that obstacles can be avoided and the target can be approached within a given time window. The objective function is: G(v,ω)=α·heading(v,ω)+β·distance(v,ω)+γ·velocity(v,ω) In the above formula, α, β and γ represent weight coefficients.
4. The method for intelligent navigation and safe driving of mining vehicles based on inertial navigation according to claim 1, characterized in that: The artificial potential field method in S5 includes: Calculate the resultant force on the vehicle, which is composed of the target attraction and the obstacle repulsion. The attraction is: F att =-k att (P-P goal ) Repulsion: In the above formula, P represents the vehicle position, O represents the obstacle position, d(P,O) represents the distance from the vehicle to the obstacle, d0 represents the impact range, and k att and k rep represents a constant, and the resultant force is F = F att +F rep .
5. The method for intelligent navigation and safe driving of mining vehicles based on inertial navigation according to claim 1, characterized in that: The PID controller in S6 includes: Speed Control: In the above formula, v ref represents the reference speed, v represents the actual speed, K p , K i and K d Represents the proportional, integral, and derivative gains of the PID controller; Steering Control: In the above formula, θ ref represents the reference steering angle, θ represents the actual steering angle, K p , K i and K d Represents the proportional, integral, and derivative gains of the PID controller.
6. The method for intelligent navigation and safe driving of mining vehicles based on inertial navigation according to claim 1, characterized in that: The model predictive control in S6 includes: Set the prediction time domain N and define the cost function: In the above formula, x(k+i|k) represents the state at the i-th moment predicted at time k, and x ref represents the reference state, u(k+i|k) represents the control input, Q and R represent the weight matrices of the state and control input respectively; Optimizing control input: Constraints: x(k+i+1|k)=A x (k+i|k)+Bu(k+i|k) u min ≤u(k+i|k)≤u max 。 7. The method for intelligent navigation and safe driving of mining vehicles based on inertial navigation according to claim 1, characterized in that: Also includes condition monitoring and self-diagnostics: Real-time monitoring of vehicle status to ensure stable system operation. Monitoring parameters include battery power and sensor working status. By analyzing sensor data, faults can be detected and diagnosed to ensure safe operation of the vehicle. Battery power monitoring: In the above formula, SOC represents the battery state of charge, Q remaining Indicates the remaining power, Q total Indicates the total battery charge; Sensor working status monitoring: By periodically checking the sensor output, it detects whether there is an abnormality. The anomaly detection algorithm includes mean drift detection and Kalman filter residual analysis.
8. The method for intelligent navigation and safe driving of mining vehicles based on inertial navigation according to claim 7 is characterized in that: The mean shift detection includes: Mean shift detection identifies anomalies by detecting whether the mean of the sensor output changes significantly. The sensor data output data is {x1,x2,…,x n }, the specific steps of mean shift detection are as follows: Calculate the sliding window mean, select a sliding window of length w, calculate the mean of the data in each window, for the i-th window, the mean is: Calculate the mean change, calculate the change of the mean of adjacent sliding windows, and define the mean change as: Dm i =μ i+1 -m i Set a threshold, set a threshold δ, if |Δμ i | If it exceeds δ, it is considered abnormal; Detect anomalies and calculate Δμ for each window i , if |Δμ i |>δ, an anomaly is detected.
9. The method for intelligent navigation and safe driving of mining vehicles based on inertial navigation according to claim 7, characterized in that: The Kalman filter residual analysis includes: Kalman filter residual analysis identifies anomalies by detecting the residual of the filter, which is the difference between the observed value and the predicted value. The system state vector is x k , the observation vector is z k , the specific steps of residual analysis of Kalman filter are as follows: Prediction step, predicting the current state and observed values: x k|k-1 =Ax k-1|k-1 +Bu k z k|k-1 =Hx k|k-1 Compute the residuals, which are defined as the difference between the observed and predicted observed values: r k =z k -z k|k-1 Calculate the residual covariance, which is defined as: S k =HP k|k-1 H T +R In the above formula, P k|k-1 represents the prediction error covariance, and R represents the observation noise covariance; Set the anomaly detection threshold γ, which is set according to the chi-square distribution. The threshold value depends on the dimension of the residual and the expected confidence level; Detect anomalies and compute standardized residuals: If the standardized residual is greater than the threshold γ, it is determined that an abnormality is detected.
10. The intelligent navigation and safe driving system for mining vehicles based on inertial navigation is characterized by: include: A sensor module, a data processing module, a control module, a status monitoring and self-diagnosis module, and a communication module. The sensor module is used to collect data required during the operation of the vehicle. The sensor module includes an inertial measurement unit (IMU), a global positioning system (GPS), a laser radar (LiDAR), an ultrasonic sensor, a visual sensor, a magnetometer, and an odometer. The IMU is used to obtain acceleration and angular velocity data of the vehicle. The GPS is used to provide location information of the vehicle. The laser radar is used for environmental perception and obstacle detection. The ultrasonic sensor is used for close-range obstacle detection. The visual sensor is used to identify road signs, detect obstacles, and assist in navigation. The magnetometer is used to provide orientation information of the vehicle. The odometer is used to measure the travel distance of the vehicle. The data collected by the sensor module is transmitted to the data processing module. The data processing module is used to fuse and process the sensor data to generate high-precision position information and motion status of the vehicle, and the data processing module transmits the processed results to the control module and the status monitoring and self-diagnosis module; The control module generates a vehicle control instruction according to the navigation information output by the data processing module to realize intelligent navigation and safe driving, and the control module transmits the control instruction generated according to the information of the data processing module to the vehicle actuator; The state monitoring and self-diagnosis module is used to monitor the running state of the vehicle in real time to ensure the stability and safety of the system. The state monitoring and self-diagnosis module transmits the monitoring results to the communication module; The communication module is used for data communication between the vehicle and the control center to realize remote monitoring and operation, including wireless communication and data storage. The communication module transmits data to the remote control center through wireless communication to realize remote monitoring and operation.
Citation Information
Cited By
Real-time intelligent sensing and real-time obstacle avoidance system of unmanned ship at sea and operation method
CN119179328A
Intelligent whistle limiting method and system for rail locomotive and electronic equipment
CN119319864A
Implementation method, system, medium and equipment for accurately detecting right turning of vehicle
CN119357906A
A method, system, medium and device for accurately detecting right turn of vehicle
CN119357906B
Electric power material warehouse-in and warehouse-out intelligent management and control method and device based on intelligent carrying
CN119692907A
