Method for determining route planning based on azimuth angle
Through azimuth-based route planning method, combined with environmental information and obstacle locations, the real-time and accuracy problems of route planning in complex and dynamic environments in the prior art are solved, and more flexible and high-precision route planning is achieved.
Patent Information
- Application Number
- CN202411727540.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-11-28
- Publication Date
- 2025-03-18
AI Technical Summary
Existing route planning methods are limited in real-time and accuracy in complex and dynamic environments, and are particularly difficult to respond to obstacles and emergencies in unknown or irregular environments.
The azimuth-based route planning method is adopted, and the azimuth between the starting point and the target position is calculated, and the path is dynamically adjusted to adapt to environmental changes by combining environmental information and obstacle positions.
This method simplifies the path planning process, reduces dependence on complex map data, improves the flexibility and accuracy of route planning, and is suitable for application scenarios with high precision requirements such as autonomous driving and drone navigation.
Smart Images
Figure CN119642797A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of route planning, and in particular to a method for determining route planning based on azimuth. Background Art
[0002] Existing route planning methods mostly use algorithms based on map data, obstacle perception, path search, etc. However, traditional path planning methods often rely on complex map information and geographic location data, and are not flexible enough to respond to obstacles or emergencies in dynamic environments, especially in unknown or irregular environments. The real-time and accuracy of path planning will be greatly limited.
[0003] In the existing technology, there are few applications of using azimuth (azimuth refers to the direction angle from the observer's position to the target position) to assist in determining the path, especially for path planning methods in complex environments and dynamically changing scenes, which still need to be further optimized.
[0004] Therefore, developing an azimuth-based route planning method that can provide fast, flexible and accurate route planning in dynamic and complex environments has important practical value in the fields of autonomous driving, drone navigation, intelligent robots, etc. Summary of the invention
[0005] The purpose of the present invention is to solve the shortcomings in the prior art and to propose a method for determining route planning based on azimuth.
[0006] To achieve the above object, the present invention adopts the following technical solution: a method for determining route planning based on azimuth, the method comprises the following steps: S1: Calculate the azimuth between the starting point and the target position; S2: Obtain environmental information and identify obstacle locations; S3: Correct the azimuth angle and adjust the path according to the obstacle position; S4: Optimize the path and determine the final route; S5: Execute the planned route.
[0007] Preferably, in step S2, the environmental information is acquired in real time through sensors, lidar, cameras or other environmental perception devices.
[0008] Preferably, in step S3, the identification of obstacles is based on real-time analysis of environmental information, the size, position and dynamic state of the obstacles.
[0009] Preferably, in step S4, the change of azimuth angle during path optimization takes into account the influence of terrain, obstacles and other dynamic factors.
[0010] Preferably, in step S4, the path optimization uses a path search algorithm, including an A* algorithm, a Dijkstra algorithm or a variant thereof.
[0011] Preferably, a device for determining a route planning method based on azimuth comprises: an azimuth calculation unit, used to calculate the azimuth between a starting point and a target position; an environment perception unit, used to obtain environment information in real time and identify obstacles; a path correction unit, used to correct the azimuth and adjust the path according to obstacle information; a path optimization unit, used to optimize the path and determine the final route; and an execution unit, used to execute the planned route.
[0012] Preferably, the environmental perception unit includes a sensor, a lidar, a camera or other environmental information collection device.
[0013] Compared with the prior art, the beneficial effects of the present invention are: the method of the present invention simplifies the path planning process: by performing preliminary path planning based on azimuth, the dependence on complex map data is reduced, the path planning process is simplified, and it is particularly suitable for complex and unknown environments.
[0014] The present invention can monitor obstacles in the environment in real time and correct the path according to the adjustment of the azimuth, so that route planning can dynamically adapt to environmental changes and improve the flexibility and accuracy of path planning. It can also achieve more accurate navigation by optimizing azimuth calculation and adjusting the path, which is particularly suitable for application scenarios with high precision requirements such as automatic driving and drone navigation. The present invention is not only suitable for intelligent devices such as vehicles and robots, but can also be widely used in other fields, such as aerospace, logistics distribution, agricultural drones and other scenarios. BRIEF DESCRIPTION OF THE DRAWINGS
[0015] Figure 1 Simple diagram for azimuth optimized routes. DETAILED DESCRIPTION
[0016] In order to provide a further understanding of the purpose, structure, features, and functions of the present invention, the following detailed description is given in conjunction with the embodiments.
[0017] Please refer to Figure 1 The present invention provides a method for determining route planning based on azimuth, and the method comprises the following steps: S1: Calculate the azimuth between the starting point and the target position; The calculation of the azimuth is determined by the relative position between the geographic coordinates of the starting point and the target. The longitude and latitude of the starting point and the end point are obtained through the GPS positioning software, which are the starting point (lon1, lat1) and the end point (lon2, lat2). The specific calculation formula is as follows: rad1 = lat1 / 180.0*π; rad2 = lat2 / 180.0*π; longDiff = (lon2-lon1) / 180.0*π; y=sin (longDiff )*cos(lon2); x=cos(rad1)*sin(rad2)-sin(rad1)*cos(rad2)*cos(longDiff); bear = (atan2(x,y)*180.0 / π+360)%360; rad1, rad2 are the radians of the coordinate latitude; longDiff is the radian of the longitude difference of the coordinates; x, y coordinate quadrant; bear azimuth (the angle between the line connecting the start and end points and true north); This algorithm is used to calculate the direction (i.e. azimuth) between two geographic coordinate points; taking into account the fact that the earth is spherical, the azimuth on the shortest path from one point to another is calculated using the formula of the great circle route.
[0018] S2: Obtain environmental information and identify obstacle locations; The environmental information is acquired in real time through sensors, lidar, cameras or other environmental perception devices.
[0019] Obtain environmental information and identify obstacles, collect environmental data information through sensors; after obtaining environmental data information, the original data needs to be processed to identify and locate obstacles; then identify and locate obstacles; then visualize the location of obstacles and plan paths; finally, system integration and real-time feedback.
[0020] The processing of the raw data includes data preprocessing and obstacle identification and positioning.
[0021] The data preprocessing is to perform filtering, noise removal and other processing on the sensor data to improve the data quality.
[0022] Obstacle recognition is to identify obstacles through point cloud segmentation and clustering (such as plane extraction based on RANSAC and DBSCAN algorithm).
[0023] Depth image processing is to detect objects closer to the sensor by performing threshold segmentation or contour extraction on the depth map, thereby identifying obstacles.
[0024] You can also use computer vision toolkits such as OpenCV for image processing and combine depth information to extract obstacles.
[0025] Obstacle detection and classification uses feature-based methods or deep learning models to identify the types of obstacles in the environment (such as static obstacles, dynamic obstacles).
[0026] This step may be combined with machine learning, especially deep learning models such as convolutional neural networks (CNNs) for more accurate obstacle classification.
[0027] Obstacle localization obtains the timing information of sensor data and combines it with posture estimation (such as SLAM algorithm) to obtain the position of the obstacle relative to the vehicle.
[0028] By fusing data from different sensors (such as LiDAR and cameras), the accuracy and robustness of obstacle recognition can be improved.
[0029] Then use graphical tools (such as RViz, Matplotlib, etc.) to visualize environmental information and obstacle locations. Through the graphical interface, users can view the location of the vehicle and the distribution of surrounding obstacles. By calculating the location of obstacles, obstacle avoidance paths can be planned for the autonomous driving system.
[0030] All sensor data processing, obstacle identification and path planning usually need to be performed in real time to ensure that the vehicle can respond to new environmental information.
[0031] This is usually achieved through embedded systems or efficient computing hardware.
[0032] The process of obstacle detection and positioning is not completed in one go. The vehicle will continuously receive new sensor data and adjust its own movement trajectory.
[0033] S3: Correct the azimuth according to the obstacle position and adjust the path; Usually used in autonomous driving systems; it detects surrounding obstacles through sensors and adjusts the vehicle's direction and path according to the location of the obstacles to ensure that the vehicle can reach the target location safely and smoothly.
[0034] Obstacle identification is based on real-time analysis of environmental information, obstacle size, location and dynamic state.
[0035] Use sensors to sense surrounding obstacles and obtain obstacle information in the surrounding environment, including the location, size, and direction of the obstacles. Based on the obstacle detection data, the vehicle needs to replan the path. The path planning algorithm will recalculate the optimal driving path based on the current position, target position, and detected obstacle positions.
[0036] When the robot detects an obstacle, it needs to adjust its direction of travel. Common methods to avoid collision include: Local obstacle avoidance: Avoid obstacles ahead by adjusting the azimuth angle in real time. The obstacles generate repulsive force, the target point generates attractive force, and the new direction of travel is determined by calculating the combined force.
[0037] Target-based direction correction: When facing an obstacle, the vehicle will adjust its direction based on the target's orientation and the relative position of the obstacle. For example, after calculating the obstacle avoidance path using an obstacle avoidance algorithm (such as A* or VFH), the vehicle will update the azimuth based on the new path.
[0038] After the path is planned and the direction is adjusted, the vehicle needs to smoothly follow the path. Use a PID controller: This is used to adjust the speed and direction of the vehicle so that it follows the calculated path.
[0039] S4: Optimize the path and determine the final route; Determine the optimization objectives and constraints.
[0040] For example, the objective can be the shortest path, minimum time, or minimum cost; constraints may include traffic restrictions, road types, traffic signals, etc.
[0041] Choose an appropriate optimization algorithm based on the scale and requirements of the specific problem.
[0042] Convert the actual problem into a graph model. In the graph model, locations or nodes represent individual sites (such as cities, warehouses, intersections, etc.), and edges represent the connections between these locations (such as roads, tracks, etc.). Each edge has a certain weight, which represents the distance, time or cost from one node to another.
[0043] By selecting a suitable algorithm, the graph is processed to find the shortest path or the optimal path that meets other conditions.
[0044] The final path can be determined by calculating the shortest path and the optimal path. At this point, the path output by the algorithm needs to be verified and adjusted to ensure that it is feasible in actual operation and meets all constraints.
[0045] S5: Execute the planned route.
[0046] Preferably, in step S4, the change of azimuth angle during path optimization takes into account the influence of terrain, obstacles and other dynamic factors.
[0047] Preferably, in step S4, the path optimization uses a path search algorithm, including an A* algorithm, a Dijkstra algorithm or a variant thereof.
[0048] Preferably, a device for determining a route planning method based on azimuth comprises: an azimuth calculation unit, used to calculate the azimuth between a starting point and a target position; an environment perception unit, used to obtain environment information in real time and identify obstacles; a path correction unit, used to correct the azimuth and adjust the path according to obstacle information; a path optimization unit, used to optimize the path and determine the final route; and an execution unit, used to execute the planned route.
[0049] Preferably, the environmental perception unit includes a sensor, a lidar, a camera or other environmental information collection device.
[0050] First, the system needs to calculate the azimuth between the starting point and the target position, that is, the direction between the two points. This step provides the basic orientation for subsequent path planning, ensuring that the system can navigate toward the target when starting from the starting point; the surrounding environmental data is obtained in real time through environmental perception devices such as sensors, lidar, and cameras. This information is used to identify obstacles, terrain changes, and dynamic factors (such as pedestrians, vehicles, etc.) in the path. The identification of obstacles is not only based on their static position, but also takes into account their size and possible dynamic state, such as moving obstacles; after real-time perception of environmental information, the system adjusts the original azimuth according to the specific location of the obstacle. If an obstacle appears on the path, the system will calculate a new azimuth to avoid the obstacle and dynamically adjust the path. This process ensures that the system can flexibly respond to emergencies and avoid collisions in complex environments; after correcting the azimuth, the system uses a path optimization algorithm to further improve the route. Common path search algorithms such as the A* algorithm or the Dijkstra algorithm will optimize the path based on terrain features, obstacle distribution, and other dynamic factors (such as traffic flow) to ensure that the route is the shortest, safest, or most efficient. Finally, the system performs navigation according to the optimized path, automatically moves along the planned route, and completes the automatic navigation task of the target.
[0051] By acquiring environmental information and identifying obstacles in real time, the system can dynamically adjust the path according to changes in obstacles, effectively avoid collisions, and ensure driving safety. Path optimization not only takes obstacles into consideration, but also comprehensively considers multiple factors such as terrain and traffic, making the final path more efficient, saving time and energy. This method can handle various dynamic factors in complex environments in real time and adapt to a variety of complex scenarios, such as urban roads and indoor navigation. Azimuth correction and path optimization can improve the accuracy of overall navigation, especially in complex or unknown environments, and can provide more reliable and accurate route planning.
[0052] Use azimuth to plan routes to avoid inconvenient routes during container transport; take one point as the coordinate origin as shown in the following figure, the azimuth angle from point O to point A minus the azimuth angle from point O to point B is an acute angle, which meets business needs.
[0053] In general, this azimuth-based route planning method can effectively improve the intelligence level of the automatic navigation system and enhance its adaptability in dynamic environments. It is widely used in unmanned driving, automatic robots, logistics and distribution and other fields.
[0054] The present invention has been described by the above-mentioned relevant embodiments, however, the above-mentioned embodiments are only examples for implementing the present invention. It must be pointed out that the disclosed embodiments do not limit the scope of the present invention. On the contrary, changes and modifications made without departing from the spirit and scope of the present invention are all within the scope of patent protection of the present invention.
Claims
1. A method for determining route planning based on azimuth, characterized in that: The method comprises the following steps: S1: Calculate the azimuth between the starting point and the target position; S2: Obtain environmental information and identify obstacle locations; S3: Correct the azimuth angle and adjust the path according to the obstacle position; S4: Optimize the path and determine the final route; S5: Execute the planned route.
2. A method for determining route planning based on azimuth as claimed in claim 1, characterized in that: In step S2, the environmental information is acquired in real time through sensors, lidar, cameras or other environmental perception devices.
3. The method for determining route planning based on azimuth as claimed in claim 1, characterized in that: In step S3, the identification of obstacles is based on real-time analysis of environmental information, the size, position and dynamic state of the obstacles.
4. The method for determining route planning based on azimuth as claimed in claim 1, characterized in that: In step S4, the change of azimuth angle during path optimization takes into account the influence of terrain, obstacles and other dynamic factors.
5. The method for determining route planning based on azimuth as claimed in claim 1, characterized in that: In step S4, the path optimization uses a path search algorithm, including an A* algorithm, a Dijkstra algorithm or a variant thereof.
6. A device for determining a route planning method based on an azimuth angle, characterized in that: Include: An azimuth calculation unit, used to calculate the azimuth between the starting point and the target position; Environmental perception unit, used to obtain environmental information in real time and identify obstacles; A path correction unit, used to correct the azimuth and adjust the path according to the obstacle information; A path optimization unit, used to optimize the path and determine the final route; The execution unit is used to execute the planned route.
7. The device for determining route planning based on azimuth as claimed in claim 5, characterized in that: The environment perception unit includes a sensor, a laser radar, a camera or other environment information collection devices.