An outdoor open road unmanned robot control system

Abstract

The application relates to the field of unmanned robot control, and discloses an outdoor open road unmanned robot control system, which comprises a multi-sensor fusion module, a high-speed navigation algorithm module, an environment perception and understanding module, an abnormality processing and feedback module and a cooperation and control module; the multi-sensor fusion module uses a camera, a laser radar, an ultrasonic sensor, an infrared sensor and a GPS to acquire comprehensive data of the environment; the high-speed navigation algorithm module designs an adaptive navigation algorithm based on artificial intelligence and machine learning; the environment perception and understanding module is used for enabling the unmanned robot to accurately identify and classify surrounding objects and regions; the abnormality processing and feedback module is used for ensuring that the unmanned robot can safely cope with and timely report emergencies or faults; and the cooperation and control module is used for realizing the cooperative work among robots and remote monitoring; the application has the advantages of autonomous operation in a complex urban environment and improved navigation and obstacle avoidance efficiency.

Description

Technical Field

[0001] This invention relates to the field of unmanned robot control, specifically to an unmanned robot control system for outdoor open roads. Background Technology

[0002] Outdoor open-road unmanned robots are robotic systems capable of operating autonomously on public roads. These robots typically utilize various advanced technologies, such as sensors, cameras, GPS, LiDAR, and artificial intelligence, to achieve functions like automatic navigation, obstacle avoidance, and task execution. They are widely used in areas such as express delivery, environmental monitoring, security patrols, and urban sanitation. Unmanned robots can regularly inspect and clean public facilities such as parks and squares, removing graffiti from walls, cleaning stains from public benches, and alerting and reporting damaged facilities, thereby maintaining the cleanliness and safety of the urban environment. Although unmanned robots employ many advanced technologies, they may encounter navigation and obstacle avoidance challenges in complex and dynamic urban environments. For example, sudden obstacles, weather changes, and areas with high pedestrian traffic can all affect the robot's normal operation. Therefore, it is necessary to design an outdoor open-road unmanned robot control system with autonomous operation capabilities in complex urban environments, improving the efficiency and reliability of its navigation and obstacle avoidance. Summary of the Invention

[0003] (a) Technical problems to be solved

[0004] To address the shortcomings of existing technologies, this invention provides an outdoor open road unmanned robot control system, which has the advantages of improving the efficiency and reliability of its navigation and obstacle avoidance, and solves the problems mentioned in the background technology.

[0005] (II) Technical Solution

[0006] To achieve the aforementioned goals of improving the efficiency and reliability of navigation and obstacle avoidance, this invention provides the following technical solution: an outdoor open road unmanned robot control system, comprising a multi-sensor fusion module, a high-speed navigation algorithm module, an environmental perception and understanding module, an anomaly handling and feedback module, and a collaboration and control module. The multi-sensor fusion module utilizes cameras, lidar, ultrasonic sensors, infrared sensors, and GPS to acquire comprehensive environmental data. The high-speed navigation algorithm module designs an adaptive navigation algorithm based on artificial intelligence and machine learning, enabling the robot to dynamically adjust its path according to real-time environmental data. The environmental perception and understanding module enables the unmanned robot to accurately identify and classify surrounding objects and areas. The anomaly handling and feedback module ensures the unmanned robot can safely respond to and promptly report any emergencies or malfunctions. The cloud-based collaboration and remote control module enables collaborative work and remote monitoring between robots. The multi-sensor fusion module includes an ultrasonic sensor module, an infrared sensor module, and an IMU module. The ultrasonic sensor module is used for near-range obstacle detection, helping the robot move safely in confined spaces. The infrared sensor module is used to detect heat sources and near-range obstacles. The IMU module provides attitude perception and motion state monitoring, helping the robot maintain balance and navigation.

[0007] According to the above technical solution, the high-speed navigation algorithm module includes a real-time obstacle avoidance module, an environment modeling module, and a speed and steering control module. The real-time obstacle avoidance module uses a reinforcement learning algorithm to help the robot dynamically avoid suddenly appearing obstacles during its movement. The environment modeling module uses SLAM technology to generate and update the environment map in real time. The speed and steering control module calculates the speed and steering angle required by the robot based on the path planning and obstacle avoidance results.

[0008] According to the above technical solution, the environmental perception and understanding module includes a semantic segmentation module and a dynamic object tracking module. The semantic segmentation module uses semantic segmentation technology to divide the environmental image into different regions and objects, providing detailed environmental information for path planning and obstacle avoidance. The dynamic object tracking module uses computer vision and sensor data to track moving objects in real time and predict their trajectories.

[0009] According to the above technical solution, the anomaly handling and feedback module includes a self-diagnosis module, an emergency handling module, and a fault reporting module. The self-diagnosis module monitors the various functional states of the robot in real time, detects and diagnoses potential faults, the emergency handling module is used to quickly respond to emergencies and protect the safety of the robot and its surrounding environment, and the fault reporting module is used to provide detailed fault information to help maintenance personnel quickly locate and solve problems.

[0010] According to the above technical solution, the collaboration and control module includes a collaborative work module and a remote control module. The collaborative work module is used to coordinate the collaborative work between multiple robots, assign tasks and optimize workflows. The remote control module allows operators to remotely control the robots through a cloud system to perform emergency operations or adjust task parameters.

[0011] A control method for an unmanned robot on an open outdoor road, characterized by comprising the following steps:

[0012] S1: Use various sensors to perceive the surrounding environment and obstacles, use SLAM technology to build and update the environmental map, and determine the robot's position at the same time;

[0013] S2: Calculate the robot's path using a path planning algorithm based on the target location and the environment map;

[0014] S3: Based on path planning, use obstacle avoidance algorithms to handle obstacles;

[0015] S4: Controls the robot's speed and direction, navigates along the planned path, and adjusts its posture and motion strategy in real time;

[0016] S5: Provides remote monitoring and control functions, allowing users to view robot status and adjust task parameters in real time.

[0017] According to the above technical solution, S1 further includes:

[0018] The map is initialized using feature points to create an initial environmental map. Based on the attitude and acceleration information provided by the IMU, the robot's motion trajectory and attitude changes are calculated. When new sensor data arrives, feature point matching technology is used to match the current data with the previous map data to determine the robot's position and orientation. SLAM algorithm is used for path reasoning. The robot's position on the map is updated based on continuous sensor data. Errors in the map are detected and corrected through a closed-loop detection mechanism. The robot's real-time position information on the map is fed back to the navigation and control system.

[0019] According to the above technical solution, S2 further includes constructing a map of the target location and environment, dividing the environment into grids, each grid representing whether the area is occupied by obstacles, and using the nodes and edges of the graph to represent the location and path in the environment;

[0020] We choose Dijkstra's algorithm to calculate the shortest path from the starting point to all other nodes. We set the distance to the starting point to 0, i.e., dist(start) = 0, and the distances to all other nodes to infinity, i.e., dist(v) = ∞. For all nodes v ≠ start, we use a priority queue to store nodes and their distance values. We retrieve the node u with the smallest distance value from the priority queue and mark node u as visited. For each adjacent node v, if the path from u to v is shorter than the currently known shortest distance, we update the distance using the following formula:

[0021] dist(v)=min(dist(v),dist(u)+w(u,v))

[0022] Where w(u, v) is the weight of the edge from u to v;

[0023] Repeat the process of selecting nodes and updating distances until the priority queue is empty. The shortest path for all nodes has been calculated, and the results are stored in the dist array.

[0024] The initial path is smoothed using Bézier curves to adapt to complex road conditions, while also taking into account dynamic obstacles and real-time adjustment of control points. The velocity and acceleration of the path are calculated using the derivative of the Bézier curve.

[0025] Using a cubic Bézier curve, the formula is as follows:

[0026] B(t)=(1-t) 3 P0+3(1-t) 2 tP1+3(1-t)t 2 P2+t 3 P3, t∈[0,1]

[0027] The derivative of a Bézier curve (i.e., velocity) can be obtained by differentiating the curve's formula:

[0028]

[0029] For cubic Bézier curves, the derivative formula is:

[0030] B′(t)=3(1-t) 2 (P1-P0)+6(1-t)t(P2-P1)+3t 2 (P3-P2)

[0031] The second derivative of the Bézier curve (i.e., acceleration) can be obtained by differentiating the velocity formula:

[0032]

[0033] For a cubic Bézier curve, the formula for the second derivative is as follows:

[0034] B ″ (t)=6(1-t)(P2-2P1+P0)+6t(P3-2P2+P1)

[0035] When a dynamic obstacle is detected, the control points of the Bézier curve can be adjusted to move closer to the obstacle and avoid it. Then, the Bézier curve and its derivative can be recalculated.

[0036] According to the above technical solution, S3 further includes considering the robot's speed and turning in the next short period of time, and selecting a movement that both avoids obstacles and moves toward the target. When using DWA, the speed and turning are selected according to the dynamic window at each time step, and then the path is updated.

[0037] The robot model formula is as follows:

[0038] x′=vcos(θ)

[0039] y′=v]sin(θ)

[0040] θ′= w

[0041] Where v is linear velocity and w is angular velocity;

[0042] The velocity space constraint formula is as follows:

[0043] V = {(v, w)|v min ≤v≤v max w min ≤w≤w max}

[0044] The formula for dynamic windows is as follows:

[0045] V d ={(v, w)|v min ≤v+v′Δt≤v max w min ≤w+w′Δt≤w max}

[0046] Where Δt is the time step;

[0047] The evaluation function formula is as follows:

[0048] G(v,w)=α*heading(v,w)+β*dist(v,w)+γ*velocity(v)

[0049] Where heading is the cost function for heading toward the target, dist is the cost function for distance from obstacles, velocity is the cost function for velocity, and α, β, and γ are weighting coefficients.

[0050] (III) Beneficial Effects

[0051] Compared with the prior art, the present invention provides an outdoor open road unmanned robot control system, which has the following beneficial effects:

[0052] This outdoor open-road unmanned robot control system calculates the robot's path using a path planning algorithm based on the target location and environmental map. It controls the robot's speed and steering, navigates along the planned path, adjusts its posture and motion strategy in real time, and uses Bézier curves to smooth the initial path to adapt to complex road conditions. It also considers dynamic obstacles and adjusts control points in real time, using the derivative of the Bézier curve to calculate the path's speed and acceleration. This system enables the unmanned robot to operate autonomously in complex urban environments, improving its navigation and obstacle avoidance efficiency and reliability. Attached Figure Description

[0053] Figure 1 This is a schematic diagram of the structure of the present invention. Detailed Implementation

[0054] 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.

[0055] Please see Figure 1

[0056] This invention provides a technical solution: an outdoor open road unmanned robot control system, comprising a multi-sensor fusion module, a high-speed navigation algorithm module, an environmental perception and understanding module, an anomaly handling and feedback module, and a collaboration and control module. The multi-sensor fusion module utilizes cameras, LiDAR, ultrasonic sensors, infrared sensors, and GPS to acquire comprehensive environmental data. The high-speed navigation algorithm module designs an adaptive navigation algorithm based on artificial intelligence and machine learning, enabling the robot to dynamically adjust its path according to real-time environmental data. The environmental perception and understanding module enables the unmanned robot to accurately identify and classify surrounding objects and areas. The anomaly handling and feedback module ensures the unmanned robot can safely respond to and promptly report any emergencies or malfunctions. The cloud-based collaboration and remote control module enables collaborative work and remote monitoring between robots. The multi-sensor fusion module includes an ultrasonic sensor module, an infrared sensor module, and an IMU module. The ultrasonic sensor module is used for near-range obstacle detection, helping the robot move safely in confined spaces. The infrared sensor module is used to detect heat sources and near-range obstacles. The IMU module provides attitude perception and motion state monitoring, helping the robot maintain balance and navigation.

[0057] The high-speed navigation algorithm module includes a real-time obstacle avoidance module, an environment modeling module, and a speed and steering control module. The real-time obstacle avoidance module uses a reinforcement learning algorithm to help the robot dynamically avoid suddenly appearing obstacles during its movement. The environment modeling module uses SLAM technology to generate and update the environment map in real time. The speed and steering control module calculates the speed and steering angle required by the robot based on the path planning and obstacle avoidance results.

[0058] The environmental perception and understanding module includes a semantic segmentation module and a dynamic object tracking module. The semantic segmentation module uses semantic segmentation technology to divide the environmental image into different regions and objects, providing detailed environmental information for path planning and obstacle avoidance. The dynamic object tracking module uses computer vision and sensor data to track moving objects in real time and predict their trajectories.

[0059] The anomaly handling and feedback module includes a self-diagnosis module, an emergency handling module, and a fault reporting module. The self-diagnosis module monitors the robot's various functional statuses in real time, detects and diagnoses potential faults, the emergency handling module is used to quickly respond to emergencies and protect the safety of the robot and its surrounding environment, and the fault reporting module is used to provide detailed fault information to help maintenance personnel quickly locate and resolve problems.

[0060] A control method for an unmanned robot on an open outdoor road, characterized by comprising the following steps:

[0061] S1: Use various sensors to perceive the surrounding environment and obstacles, use SLAM technology to build and update the environmental map, and determine the robot's position at the same time;

[0062] S2: Calculate the robot's path using a path planning algorithm based on the target location and the environment map;

[0063] S3: Based on path planning, use obstacle avoidance algorithms to handle obstacles;

[0064] S4: Controls the robot's speed and direction, navigates along the planned path, and adjusts its posture and motion strategy in real time;

[0065] S5: Provides remote monitoring and control functions, allowing users to view robot status and adjust task parameters in real time.

[0066] S1 further includes:

[0067] The map is initialized using feature points to create an initial environmental map. Based on the attitude and acceleration information provided by the IMU, the robot's motion trajectory and attitude changes are calculated. When new sensor data arrives, feature point matching technology is used to match the current data with the previous map data to determine the robot's position and orientation. SLAM algorithm is used for path reasoning. The robot's position on the map is updated based on continuous sensor data. Errors in the map are detected and corrected through a closed-loop detection mechanism. The robot's real-time position information on the map is fed back to the navigation and control system.

[0068] S2 further includes constructing a map of the target location and environment, dividing the environment into grids, with each grid representing whether the area is occupied by obstacles, and using the nodes and edges of the graph to represent the location and path in the environment;

[0069] We choose Dijkstra's algorithm to calculate the shortest path from the starting point to all other nodes. We set the distance to the starting point to 0, i.e., dist(start) = 0, and the distances to all other nodes to infinity, i.e., dist(v) = ∞. For all nodes v ≠ start, we use a priority queue to store nodes and their distance values. We retrieve the node u with the smallest distance value from the priority queue and mark node u as visited. For each adjacent node v, if the path from u to v is shorter than the currently known shortest distance, we update the distance using the following formula:

[0070] dist(v)=min(dist(v),dist(u)+w(u,v))

[0071] Where w(u, v) is the weight of the edge from u to v;

[0072] Repeat the process of selecting nodes and updating distances until the priority queue is empty. The shortest path for all nodes has been calculated, and the results are stored in the dist array.

[0073] The initial path is smoothed using Bézier curves to adapt to complex road conditions, while also taking into account dynamic obstacles and real-time adjustment of control points. The velocity and acceleration of the path are calculated using the derivative of the Bézier curve.

[0074] Using a cubic Bézier curve, the formula is as follows:

[0075] B(t)=(1-t) 3 P0+3(1-t) 2 tP1+3(1-t)t 2 P2+t 3 P3, t∈[0,1]

[0076] The derivative of a Bézier curve (i.e., velocity) can be obtained by differentiating the curve's formula:

[0077]

[0078] For cubic Bézier curves, the derivative formula is:

[0079] B′(t)=3(1-t) 2 (P1-P0)+6(1-t)t(P2-P1)+3t 2 (P3-P2)

[0080] The second derivative of the Bézier curve (i.e., acceleration) can be obtained by differentiating the velocity formula:

[0081]

[0082] For a cubic Bézier curve, the formula for the second derivative is as follows:

[0083] B ″ (t)=6(1-t)(P2-2P1+P0)+6t(P3-2P2+P1)

[0084] When a dynamic obstacle is detected, the control points of the Bézier curve can be adjusted to move closer to the obstacle and avoid it. Then, the Bézier curve and its derivative can be recalculated.

[0085] S3 further includes considering the robot's speed and turning in the near future and selecting a motion that both avoids obstacles and moves toward the target. When using DWA, the speed and turning are selected according to the dynamic window at each time step, and then the path is updated.

[0086] The robot model formula is as follows:

[0087] x′=vcos(θ)

[0088] y′=vsin(θ)

[0089] θ′= w

[0090] Where v is linear velocity and w is angular velocity;

[0091] The velocity space constraint formula is as follows:

[0092] V = {(v, w)|v min ≤v≤v max w min ≤w≤w max}

[0093] The formula for dynamic windows is as follows:

[0094] V d ={(v, w)|v min ≤v+v′Δt≤v max w min ≤w+w′Δt≤w max}

[0095] Where Δt is the time step;

[0096] The evaluation function formula is as follows:

[0097] G(v,w)=α*heading(v,w)+β*dist(v,w)+γ*velocity(v)

[0098] Where heading is the cost function for heading toward the target, dist is the cost function for distance from obstacles, velocity is the cost function for velocity, and α, β, and γ are weighting coefficients.

[0099] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0100] 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 control system for an unmanned robot on an outdoor open road, characterized in that... The system includes a multi-sensor fusion module, a high-speed navigation algorithm module, an environmental perception and understanding module, an anomaly handling and feedback module, and a collaboration and control module. The multi-sensor fusion module utilizes cameras, LiDAR, ultrasonic sensors, infrared sensors, and GPS to acquire comprehensive environmental data. The high-speed navigation algorithm module designs an adaptive navigation algorithm based on artificial intelligence and machine learning, enabling the robot to dynamically adjust its path according to real-time environmental data. The environmental perception and understanding module enables the unmanned robot to accurately identify and classify surrounding objects and areas. The anomaly handling and feedback module ensures the unmanned robot can safely respond to emergencies or malfunctions and report them promptly. The collaboration and control module enables collaborative work between robots and remote monitoring. The multi-sensor fusion module includes an ultrasonic sensor module, an infrared sensor module, and an IMU module. The ultrasonic sensor module is used for near-range obstacle detection, helping the robot move safely in confined spaces. The infrared sensor module is used to detect heat sources and near-range obstacles. The IMU module provides attitude perception and motion state monitoring, helping the robot maintain balance and navigation.

2. The outdoor open road unmanned robot control system according to claim 1, characterized in that, The high-speed navigation algorithm module includes a real-time obstacle avoidance module, an environment modeling module, and a speed and steering control module. The real-time obstacle avoidance module uses a reinforcement learning algorithm to help the robot dynamically avoid suddenly appearing obstacles during its movement. The environment modeling module uses SLAM technology to generate and update the environment map in real time. The speed and steering control module calculates the required speed and steering angle for the robot based on the path planning and obstacle avoidance results.

3. The outdoor open road unmanned robot control system according to claim 1, characterized in that, The environmental perception and understanding module includes a semantic segmentation module and a dynamic object tracking module. The semantic segmentation module uses semantic segmentation technology to divide the environmental image into different regions and objects, providing detailed environmental information for path planning and obstacle avoidance. The dynamic object tracking module uses computer vision and sensor data to track moving objects in real time and predict their trajectories.

4. The outdoor open road unmanned robot control system according to claim 1, characterized in that, The anomaly handling and feedback module includes a self-diagnosis module, an emergency handling module, and a fault reporting module. The self-diagnosis module monitors the robot's various functional statuses in real time, detects and diagnoses potential faults, the emergency handling module is used to quickly respond to emergencies and protect the safety of the robot and its surrounding environment, and the fault reporting module is used to provide detailed fault information to help maintenance personnel quickly locate and resolve problems.

5. The outdoor open road unmanned robot control system according to claim 1, characterized in that, The collaboration and control module includes a collaborative work module and a remote control module. The collaborative work module is used to coordinate the collaborative work between multiple robots, assign tasks and optimize workflows. The remote control module allows operators to remotely control the robots through a cloud system to perform emergency operations or adjust task parameters.

6. A control method for an unmanned robot on an open outdoor road, applied to an unmanned robot control system as described in any one of claims 1-5, characterized in that, Includes the following steps: S1: Use various sensors to perceive the surrounding environment and obstacles, use SLAM technology to build and update the environmental map, and determine the robot's position at the same time; S2: Calculate the robot's path using a path planning algorithm based on the target location and the environment map; S3: Based on path planning, use obstacle avoidance algorithms to handle obstacles; S4: Controls the robot's speed and direction, navigates along the planned path, and adjusts its posture and motion strategy in real time; S5: Provides remote monitoring and control functions, allowing users to view robot status and adjust task parameters in real time.

7. The method for controlling an unmanned robot on an outdoor open road according to claim 1, characterized in that, S1 further includes: The map is initialized using feature points to create an initial environmental map. Based on the attitude and acceleration information provided by the IMU, the robot's motion trajectory and attitude changes are calculated. When new sensor data arrives, feature point matching technology is used to match the current data with the previous map data to determine the robot's position and orientation. SLAM algorithm is used for path reasoning. The robot's position on the map is updated based on continuous sensor data. Errors in the map are detected and corrected through a closed-loop detection mechanism. The robot's real-time position information on the map is fed back to the navigation and control system.

8. The method for controlling an unmanned robot on an outdoor open road according to claim 1, characterized in that, S2 further includes: Construct a map of the target location and environment, dividing the environment into a grid, with each grid representing whether the area is occupied by obstacles, and using the nodes and edges of the graph to represent the location and path in the environment; We choose Dijkstra's algorithm to calculate the shortest path from the starting point to all other nodes. We set the distance to the starting point to 0, i.e., dist(start) = 0, and the distances to all other nodes to infinity, i.e., dist(v) = ∞. For all nodes v ≠ start, we use a priority queue to store nodes and their distance values. We retrieve the node u with the smallest distance value from the priority queue and mark node u as visited. For each adjacent node v, if the path from u to v is shorter than the currently known shortest distance, we update the distance using the following formula: dist(v)=min(dist(v),dist(u)+w(u,v)) Where w(u,v) is the weight of the edge from u to v; Repeat the process of selecting nodes and updating distances until the priority queue is empty. The shortest path for all nodes has been calculated, and the results are stored in the dist array. The initial path is smoothed using Bézier curves to adapt to complex road conditions, while also taking into account dynamic obstacles and real-time adjustment of control points. The velocity and acceleration of the path are calculated using the derivative of the Bézier curve. Using a cubic Bézier curve, the formula is as follows: B(t)=(1-t) 3 P0+3(1-t) 2 tP1+3(1-t)t 2 P2+t 3 P3,t∈[0,1] The derivative of a Bézier curve (i.e., velocity) can be obtained by differentiating the curve's formula: For cubic Bézier curves, the derivative formula is: B′(t)=3(1-t) 2 (P1-P0)+6(1-t)t(P2-P1)+3t 2 (P3-P2) The second derivative of the Bézier curve (i.e., acceleration) can be obtained by differentiating the velocity formula: For a cubic Bézier curve, the formula for the second derivative is as follows: B″(t)=6(1-t)(P2-2P1+P0)+6t(P3-2P2+P1) When a dynamic obstacle is detected, the control points of the Bézier curve can be adjusted to move closer to the obstacle and avoid it. Then, the Bézier curve and its derivative can be recalculated.

9. The method for controlling an unmanned robot on an outdoor open road according to claim 1, characterized in that, S3 further includes: Considering the robot's speed and turning in the near future, and selecting a move that both avoids obstacles and moves toward the target, when using DWA, the speed and turning are selected according to the dynamic window at each time step, and then the path is updated; The robot model formula is as follows: x′=vcos(θ) y′=vsin(θ) θ′=w Where v is linear velocity and w is angular velocity; The velocity space constraint formula is as follows: V={(v,w)|v min ≤v≤v max ,w min ≤w≤w max } The formula for dynamic windows is as follows: V d ={(v,w)|v min ≤v+v′Δt≤v max ,w min ≤w+w′Δt≤w max } Where Δt is the time step; The evaluation function formula is as follows: G(v,w)=α*heading(v,w)+β*dist(v,w)+γ*velocity(v) Where heading is the cost function for heading toward the target, dist is the cost function for distance from obstacles, velocity is the cost function for velocity, and α, β, and γ are weighting coefficients.

Images