Multi-source fusion tracking path planning equipment

Abstract

The invention relates to the technical field of path planning, and discloses a multi-source fusion tracking path planning device. According to the multi-source fusion tracking path planning equipment, data of various sensors such as a laser radar, an ultrasonic sensor and a high-definition camera are utilized, comprehensiveness and accuracy of environment information are ensured, through a Kalman filtering algorithm and an advanced image processing and point cloud processing technology, the equipment can recognize obstacles and extract road features in real time, and the tracking path planning efficiency is improved. Therefore, efficient path planning and dynamic adjustment are achieved, the flexibility and safety of path planning are improved through the multi-source fusion mode, the adaptability of equipment in a dynamic environment is enhanced, motion control is optimized through an accurate control instruction, and the accuracy and stability of path execution are ensured.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, specifically to a multi-source fusion tracking path planning device. Background Technology

[0002] With the continuous advancement of technology, automation and intelligence are being applied more and more widely in various fields. In particular, in transportation and robotics, intelligent vehicles and mobile robots have gradually become research hotspots. Effective path planning and real-time environmental perception capabilities are important foundations for achieving autonomous driving and autonomous mobility.

[0003] Traditional path planning techniques often rely on a single sensor, resulting in a lack of comprehensive information, an inability to adapt to dynamically changing environments, and deficiencies in real-time feedback mechanisms. Therefore, it is difficult to ensure the accuracy and stability of path execution. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention provides a multi-source fusion path planning device. Utilizing data from multiple sensors, including lidar, ultrasonic sensors, and high-definition cameras, it ensures the comprehensiveness and accuracy of environmental information. Through Kalman filtering algorithms and advanced image processing and point cloud processing technologies, the device can identify obstacles and extract road features in real time, thereby achieving efficient path planning and dynamic adjustment. This multi-source fusion approach not only improves the flexibility and safety of path planning and enhances the device's adaptability in dynamic environments, but also optimizes motion control through precise control commands, ensuring the accuracy and stability of path execution.

[0005] To achieve the above objectives, the present invention provides the following technical solution:

[0006] A multi-source fusion path planning device includes a data acquisition module, a data fusion module, a path planning module, a control module, and an execution module;

[0007] The data acquisition module is used to acquire location data, environmental data, and status data. The location data and status data are acquired through an inertial measurement unit, and the environmental data is acquired through a lidar, an ultrasonic sensor, and a high-definition camera.

[0008] The data fusion module is used to fuse location data, environmental data, and status data to generate unified location information and environmental description. It uses a Kalman filter algorithm to estimate the location data and uses image processing and point cloud processing algorithms to identify obstacles and extract road features.

[0009] The path planning module calculates the optimal path from the starting point to the destination based on the feature information generated by the data fusion module, and performs path safety assessment and dynamic path adjustment.

[0010] The control module converts the optimal path generated by the path planning module into control commands, calculates the steering angle, acceleration and control speed, and monitors the deviation between the equipment and the planned path in real time.

[0011] The execution module is used to perform motion control of the equipment according to the control commands issued by the control module, including acceleration, deceleration and steering, and to provide real-time feedback of the equipment status to the control module to verify the accuracy of path execution.

[0012] Preferably, the formula for estimating the location data is as follows:

[0013]

[0014] In the formula, This represents the current state estimate, including position and velocity. K represents the state estimate from the previous moment. k Indicates the Kalman gain, z k H represents the measurement value at the current moment, and H represents the observation matrix, which maps the state vector to the measurement space.

[0015] Preferably, the algorithm for identifying obstacles is as follows:

[0016] Obstacle=detect(point cloud,threshold)

[0017] In the formula, Obstacle represents the set of detected obstacles, detect represents the obstacle detection function, which processes the point cloud data, point cloud represents the point cloud data generated by the LiDAR, and threshold represents the distance used to distinguish between obstacles and the background.

[0018] Preferably, the algorithm for extracting road features is as follows:

[0019] Road Feature=edge_detect(image,σ)

[0020] In the formula, Road Feature represents the extracted road features, edge_detect represents the edge detection function, image represents the road image data obtained from the high-definition camera, and σ represents the standard deviation of the Gaussian filter.

[0021] Preferably, the formula for calculating the optimal path from the starting point to the ending point is as follows:

[0022]

[0023] In the formula, C(p) represents the total cost of the path, m represents the number of path segments, and d... i Let represent the length of the i-th segment, t represent the time cost of the i-th segment, and s represent the time cost of the i-th segment. i w represents the security risk level of the i-th segment. d w t w s This represents the weighting coefficient of each cost item, and i represents the counting subscript.

[0024] Preferably, the formula for performing the path security assessment is as follows:

[0025]

[0026] In the formula, S represents the safety score of the path, m represents the number of obstacles encountered on the path, and d obs,j This represents the minimum distance between the j-th obstacle and the path, where j represents the index.

[0027] Preferably, the formula for the dynamic path adjustment is as follows:

[0028] d'=d+K e *e+K v *v

[0029] In the formula, d' represents the adjusted path distance, d represents the original path distance, and K... e K represents the gain coefficient related to the deviation, e represents the distance between the target path and the actual path, and K represents the distance between the target path and the actual path. v This represents the gain coefficient related to speed, where v represents the current speed of the device.

[0030] Preferably, the formula for calculating the steering angle is as follows:

[0031]

[0032] In the formula, θ represents the turning angle between the target direction and the current direction, (x target ,y target (x) represents the coordinates of the target point. current ,y current ) represents the coordinates of the current point.

[0033] Preferably, the formula for calculating acceleration is as follows:

[0034]

[0035] In the formula, 'a' represents acceleration, and 'v' represents acceleration. f v represents the final velocity. i Δt represents the initial velocity, and Δt represents the time interval.

[0036] Preferably, the formula for calculating the control speed is as follows:

[0037]

[0038] In the formula, v represents the current velocity, v max DC represents the device's maximum speed, and DC represents the current distance to the nearest obstacle. safe This indicates the safe distance threshold.

[0039] Compared with the prior art, the present invention provides a multi-source fusion path planning device, which has the following beneficial effects:

[0040] This invention utilizes data from multiple sensors, including lidar, ultrasonic sensors, and high-definition cameras, to ensure the comprehensiveness and accuracy of environmental information. Through Kalman filtering algorithms and advanced image processing and point cloud processing technologies, the device can identify obstacles and extract road features in real time, thereby achieving efficient path planning and dynamic adjustment. This multi-source fusion approach not only improves the flexibility and safety of path planning and enhances the device's adaptability in dynamic environments, but also optimizes motion control through precise control commands, ensuring the accuracy and stability of path execution. Attached Figure Description

[0041] Figure 1 This is a schematic diagram of the device module process of the present invention. Detailed Implementation

[0042] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0043] Traditional path planning technologies often rely on single sensors, resulting in incomplete information, inability to adapt to dynamically changing environments, and deficiencies in real-time feedback mechanisms, thus making it difficult to ensure the accuracy and stability of path execution. Therefore, a multi-source fusion tracking path planning device is proposed. (See [link to relevant documentation]). Figure 1 The device includes a data acquisition module, a data fusion module, a path planning module, a control module, and an execution module.

[0044] The data acquisition module is responsible for acquiring various key information needed for the device to move in the environment, in order to ensure smooth navigation and path planning. This module mainly consists of the following parts:

[0045] Position and status data are acquired through an inertial measurement unit (IMU), a sensor system that integrates an accelerometer, gyroscope, and magnetometer. It can measure the linear acceleration and angular velocity of a device in real time, and then calculate its velocity and position information by integration. For example, the accelerometer measures the acceleration of the device in various directions, while the gyroscope provides the rotation information of the device. By combining these data, we can deduce the precise pose of the device in three-dimensional space. In addition, the IMU can also provide attitude information, which is especially important for stable navigation, especially when GPS signals are weak or unavailable.

[0046] Environmental data collection focuses on the use of devices such as LiDAR, ultrasonic sensors, and high-definition cameras. LiDAR constructs a high-precision 3D point cloud of the surrounding environment by emitting laser beams and measuring their return time, providing an effective data foundation for real-time obstacle detection and environmental modeling. Its accuracy enables the device to obtain the distance, shape, and position of obstacles, and it has a high environmental perception capability.

[0047] Ultrasonic sensors are mainly used for short-range measurements. They measure distance by emitting ultrasonic waves and detecting the echoes. They are particularly suitable for detecting obstacles that may be close and small objects in complex environments, thereby enhancing the safety and reliability of the entire system.

[0048] High-definition cameras acquire images in real time to perform visual recognition and analysis of the surrounding environment. Through image processing algorithms, such as edge detection and target recognition technology, cameras can identify and extract information such as road features, traffic signs, and pedestrians. When these visual data are combined with laser data, they help to build a more accurate environmental model, form a richer scene understanding, and support intelligent decision-making in complex environments.

[0049] The main function of the data fusion module is to effectively integrate data from different sensors to generate a unified location information and environmental description. This process not only improves the reliability and accuracy of the data, but also provides a solid data foundation for subsequent path planning.

[0050] First, the fusion of location data mainly relies on the Kalman filter algorithm, which is suitable for dynamic systems to continuously estimate the state of an object from sensor measurements. Its core formula is:

[0051]

[0052] In this formula, This represents the current state estimate (including position and velocity), while This is the state estimate from the previous time step, and the Kalman gain K. kBy controlling the balance between measured and predicted values, the gain decreases for measurements with high uncertainty, thereby reducing dependence on the measurement, and vice versa. This significantly improves the accuracy of real-time position estimation. In this way, we can effectively combat noise and interference, improve the accuracy of position and state estimation, and thus reduce the errors that may occur in actual path planning.

[0053] The fusion of environmental data is achieved through image processing and point cloud processing algorithms. Using image processing technology, the device can capture images of the surrounding environment from a high-definition camera and quickly identify obstacles through edge detection, feature extraction and other algorithms. This process helps to extract important information such as road features, traffic signs and pedestrians. By analyzing the images, we can obtain a detailed description of the environment, thereby improving the basis for navigation decisions.

[0054] Meanwhile, point cloud processing algorithms use LiDAR data to transform 3D environmental data into an analyzable format, identify obstacles and their geometric features. These algorithms typically involve techniques such as clustering, classification, and surface reconstruction to extract meaningful information from the point cloud, enabling the device to accurately identify the location and size of obstacles and provide real-time feedback for dynamic path planning.

[0055] By combining location data, environmental data, and status data, the data fusion module can generate a unified environmental description. This description not only provides the device's current precise location information but also includes detailed features of the surrounding environment, providing rich background information for the path planning module so that it can calculate the optimal path and make effective dynamic adjustments.

[0056] In summary, the data fusion module integrates advanced technologies such as Kalman filtering, image processing, and point cloud processing to effectively integrate and analyze data from various sensors. This fusion not only improves the device's navigation capabilities in complex environments but also enhances the overall system's stability and security, laying a solid big data foundation for achieving efficient loop path planning.

[0057] The main task of the path planning module is to calculate the best path from the starting point to the destination based on the feature information provided by the data fusion module, and at the same time perform path safety assessment and dynamic path adjustment. This module achieves efficient navigation through precise algorithms, ensuring that the device can drive safely and reliably in complex environments.

[0058] During the optimal path calculation process, the path planning module uses the comprehensive cost calculation formula:

[0059]

[0060] In this formula, C(p) represents the total cost of the path, encompassing multiple influencing factors, the parameter n is the number of segments in the path, and d... i t i and s i Let w represent the length, time cost, and security risk of the i-th segment, respectively, with weight w. d w s w t This reflects the importance of each factor in the overall path evaluation. In this way, the device can select a path that achieves the best balance between distance, time and safety based on the optimization goal. This comprehensive evaluation ensures that the path selection is both efficient and safe, and helps the device respond quickly to various changes in a dynamic environment.

[0061] To further ensure the safety of the path, the path planning module uses the following formula for safety assessment:

[0062]

[0063] Here, S represents the safety score of the path, m is the number of obstacles encountered on the path, and d obs,j It is the minimum distance between the j-th obstacle and the path. Through this evaluation, the module can identify potential danger zones and obstacles, thereby taking timely measures to avoid potential collision risks and improve the overall safety of the equipment.

[0064] Regarding dynamic path adjustment, the path planning module may encounter unexpected situations, such as the appearance of new obstacles or the route becoming impassable. Therefore, a dynamic adjustment formula is introduced:

[0065] d'=d+K e *e+K v *v

[0066] This formula shows that the adjusted path distance d' is updated based on the original path distance d, where K e and K v It is a gain coefficient related to the path deviation e and the current speed v of the device. It monitors the deviation in real time and adjusts it according to the speed of the device, so that the device can flexibly respond to environmental changes and ensure that the path is always optimal and safe. This dynamic adjustment capability enables the system to adapt quickly when facing complex and uncertain situations, which significantly improves the level of navigation intelligence.

[0067] The main function of the control module is to convert the optimal path generated by the path planning module into specific control commands to achieve effective navigation of the equipment. This module achieves precise steering, acceleration and speed control through a series of complex algorithms and technologies, while monitoring the deviation between the equipment and the planned path in real time to ensure smooth and efficient driving.

[0068] Regarding steering control, the control module uses the following formula to calculate the required steering angle:

[0069]

[0070] This formula calculates the angle difference θ between the target point and the current point, which can accurately guide the device towards the target direction by calculating the device's current position (x) in real time. target ,y target ) and target localization (x current ,y current The relationship between the two is such that the control module can provide accurate steering commands to ensure that the equipment can turn and make U-turns smoothly. The advantage of this calculation method is that it can quickly react to environmental changes and optimize the steering trajectory to reduce time loss and energy consumption during the steering process.

[0071] In terms of acceleration control, the control module calculates the required acceleration using the following formula based on the initial velocity and target velocity of the device:

[0072]

[0073] Where a is acceleration, v f It is the expected final velocity, v i Δt is the current speed of the equipment and Δt is the time interval. Through this calculation, the control module can smoothly adjust the acceleration or deceleration of the equipment according to the real-time situation, ensuring the stability of the equipment during acceleration and reducing the risk of loss of control caused by sudden acceleration. This is crucial for improving ride comfort and enhancing safety.

[0074] Regarding speed control, the control module performs speed control operations based on real-time feedback and the distance between the device and the planned path, using the following formula:

[0075]

[0076] Here, v is the current speed, v max It is the maximum speed allowed by the device, DC is the distance between the device and the nearest obstacle, and DC... safe This is the safe distance threshold set by the system. This formula ensures that the device will automatically reduce its speed when approaching an obstacle, thereby increasing driving safety and reducing the risk of collision. Through this dynamic adjustment mechanism, the control module can maintain the optimal driving speed according to changes in the current environment, providing users with a more reliable operating experience.

[0077] Finally, the control module also has the function of real-time monitoring of the deviation between the device and the planned path. By working in conjunction with the path planning module, the system can use deviation feedback to continuously adjust the control commands to achieve accurate navigation and effective correction. The design and implementation of this module not only improves the intelligence and responsiveness of the device, but also ensures the safety and comfort of driving in complex environments, providing users with great convenience and protection.

[0078] The execution module is responsible for actually executing the control commands issued by the control module to achieve precise motion control of the equipment, including acceleration, deceleration and steering. This module is designed with a series of advanced technologies and control algorithms to ensure that the equipment can navigate smoothly and accurately in dynamic environments.

[0079] The execution module uses a high-performance motor drive system to accelerate and decelerate the equipment. The motor controller can adjust the motor's output power in a short time according to the acceleration command issued by the control module to achieve a smooth acceleration and deceleration process. The system typically uses a PID control algorithm to monitor the difference between the motor speed and the target speed in real time, and then responds quickly to the needs by adjusting the input voltage. By adopting this closed-loop control mode, the execution module can achieve precise speed adjustment, ensuring the stability of the equipment during acceleration and avoiding safety hazards caused by sudden acceleration, thus providing users with a more comfortable riding experience.

[0080] In terms of rotation control, the execution module uses high-precision servo motors or steering wheels to achieve equipment steering. The steering angle calculated by the control module is converted into specific input signals for the motor to ensure that the equipment can turn precisely along the set trajectory. In this process, the execution module often combines feedback sensors, such as encoders or angle sensors, to monitor the deviation between the current orientation of the equipment and the target direction in real time. This feedback can adjust the control signal in a timely manner, thereby achieving smooth steering and significantly reducing the risk of path deviation.

[0081] In addition, the execution module also has a real-time status feedback function. It collects the operating status of the equipment in a timely manner by connecting to different sensors (such as speed sensors, acceleration sensors and angle sensors) and feeds this information back to the control module. This information includes the current position, current speed, acceleration and steering angle. This real-time monitoring mechanism enables the control module to verify the accuracy of the path execution in a timely manner and ensure that the equipment always travels according to the planned path.

[0082] The comprehensive application of this system not only improves the flexibility and safety of path planning and enhances the adaptability of equipment in dynamic environments, but also optimizes motion control through precise control commands, ensuring the accuracy and stability of path execution.

[0083] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A multi-source fusion path planning device, characterized in that: It includes a data acquisition module, a data fusion module, a path planning module, a control module, and an execution module; The data acquisition module is used to acquire location data, environmental data, and status data. The location data and status data are acquired through an inertial measurement unit, and the environmental data is acquired through a lidar, an ultrasonic sensor, and a high-definition camera. The data fusion module is used to fuse location data, environmental data, and status data to generate unified location information and environmental description. It uses a Kalman filter algorithm to estimate the location data and uses image processing and point cloud processing algorithms to identify obstacles and extract road features. The path planning module calculates the optimal path from the starting point to the destination based on the feature information generated by the data fusion module, and performs path safety assessment and dynamic path adjustment. The control module converts the optimal path generated by the path planning module into control commands, calculates the steering angle, acceleration and control speed, and monitors the deviation between the equipment and the planned path in real time. The execution module is used to perform motion control of the equipment according to the control commands issued by the control module, including acceleration, deceleration and steering, and to provide real-time feedback of the equipment status to the control module to verify the accuracy of path execution.

2. The multi-source fusion path planning device according to claim 1, characterized in that: The formula for estimating the location data is as follows: In the formula, This represents the current state estimate, including position and velocity. K represents the state estimate from the previous moment. k Indicates the Kalman gain, z k H represents the measurement value at the current moment, and H represents the observation matrix, which maps the state vector to the measurement space.

3. The multi-source fusion path planning device according to claim 2, characterized in that: The algorithm for identifying obstacles is as follows: Obstacle=detect(point cloud,threshold) In the formula, Obstacle represents the set of detected obstacles, detect represents the obstacle detection function, which processes the point cloud data, point cloud represents the point cloud data generated by the LiDAR, and threshold represents the distance used to distinguish between obstacles and the background.

4. The multi-source fusion path planning device according to claim 3, characterized in that: The algorithm for extracting road features is as follows: Road Feature=edge_detect(image,σ) In the formula, Road Feature represents the extracted road features, edge_detect represents the edge detection function, image represents the road image data obtained from the high-definition camera, and σ represents the standard deviation of the Gaussian filter.

5. The multi-source fusion path planning device according to claim 4, characterized in that: The formula for calculating the optimal path from the starting point to the destination is as follows: In the formula, C(p) represents the total cost of the path, n represents the number of path segments, and d i Let represent the length of the i-th segment, t represent the time cost of the i-th segment, and s represent the time cost of the i-th segment. i w represents the security risk level of the i-th segment. d w t w s This represents the weighting coefficient of each cost item, and i represents the counting subscript.

6. The multi-source fusion path planning device according to claim 5, characterized in that: The formula for performing path security assessment is as follows: In the formula, S represents the safety score of the path, m represents the number of obstacles encountered on the path, and d obs,j This represents the minimum distance between the j-th obstacle and the path, where j represents the index.

7. The multi-source fusion path planning device according to claim 6, characterized in that: The formula for dynamic path adjustment is as follows: d′=d+K e *e+K v *v In the formula, d' represents the adjusted path distance, d represents the original path distance, and K... e K represents the gain coefficient related to the deviation, e represents the distance between the target path and the actual path, and K represents the distance between the target path and the actual path. v This represents the gain coefficient related to speed, where v represents the current speed of the device.

8. The multi-source fusion path planning device according to claim 7, characterized in that: The formula for calculating the steering angle is as follows: In the formula, θ represents the turning angle between the target direction and the current direction, (x target ,y target (x) represents the coordinates of the target point. current ,y current ) represents the coordinates of the current point.

9. The multi-source fusion path planning device according to claim 8, characterized in that: The formula for calculating acceleration is as follows: In the formula, 'a' represents acceleration, and 'v' represents acceleration. f v represents the final velocity. i Δt represents the initial velocity, and Δt represents the time interval.

10. A multi-source fusion path planning device according to claim 9, characterized in that: The formula for calculating the control speed is as follows: In the formula, v represents the current velocity, v max DC represents the device's maximum speed, and DC represents the current distance to the nearest obstacle. safe This indicates the safe distance threshold.

Images