A local obstacle avoidance path planning method for mobile robots in unknown environment

Abstract

The application discloses a kind of unknown environment under mobile robot local obstacle avoidance path planning method, comprising the following steps: S1, determine initial position information, mobile robot obtains the position information of initial state by relevant sensor;S2, set starting point and target point, with the initial position of mobile robot as starting point, target point is designated by artificial setting mode, and the target point is the target point that mobile robot moves;S3, judge whether need to build map;S4, mobile robot uses costmap converter component;S5, use the soft constraint of factor graph to optimize TEB, use factor graph to optimize TEB;S6, carry out map information storage.The application provides a kind of unknown environment under mobile robot local obstacle avoidance path planning method, improves the shortcomings that mobile robot is long in unknown environment navigation calculation time, high in calculation cost, cannot find optimal path, so that it can also be carried out smooth navigation in low-performance robot.

Description

Technical Field

[0001] This invention relates to the field of mobile robot path planning algorithm technology, and in particular to a method for local obstacle avoidance path planning for mobile robots in unknown environments. Background Technology

[0002] Currently, path planning can be broadly divided into global path planning and local path planning. Global path planning includes algorithms such as Dijkstra's algorithm, A* algorithm, and PRM, while local path planning includes algorithms such as DWA algorithm, TEB algorithm, and simulated annealing. When a mobile robot performs path planning, the global path algorithm first calculates the globally optimal solution. During the journey, the local path algorithm performs real-time obstacle avoidance adjustments until the mobile robot reaches the target navigation point.

[0003] Currently, most mobile robots plan their paths in fully known environments, while navigation in unknown or semi-unknown environments is usually based on sensor data. Most sensor data-based methods focus on finding a path that can reach the target, but this path is usually not the optimal path. Furthermore, navigation in unknown environments can lead to problems such as high computational costs and long computation times. Summary of the Invention

[0004] The purpose of this invention is to provide a local obstacle avoidance path planning method for mobile robots in unknown environments, which improves the shortcomings of long navigation calculation time, high calculation cost and inability to find the optimal path in unknown environments, so that it can perform smooth navigation even in low-performance robots.

[0005] The technical solution adopted in the local obstacle avoidance path planning method for mobile robots in unknown environments disclosed in this invention is:

[0006] A method for local obstacle avoidance path planning for a mobile robot in an unknown environment includes the following steps:

[0007] S1, Determine the initial position information. The mobile robot obtains the initial position information through relevant sensors.

[0008] S2, Set the starting point and target point. Starting from the initial position of the mobile robot, specify the target point manually. This target point is the target point that the mobile robot will move to.

[0009] S3: Determine whether map construction is needed. The mobile robot determines whether it is in a known environment based on the initial state position information obtained in S1. If it is in a known environment, map information does not need to be constructed and can be obtained by retrieving pre-stored map information or loading map information. If it is in an unknown environment, ORB-SLAM3 technology is used to construct the map in real time.

[0010] S4. The mobile robot uses the costmap converter component to convert the map into a set of points, lines and polygons through the binocular vision camera to prepare the TEB. Based on the current position of the mobile robot and the target position, the mobile robot's TEB is updated for one planning cycle. Then, a new TEB is built using the current global plan. Finally, additional experimental points are added through the JPS algorithm to search for candidate paths and retain trajectories without homologous trajectories.

[0011] S5, using the soft constraints of the factor graph to optimize TEB, using the factor graph to optimize TEB: the edge cost of the factor graph is a soft constraint, so it has the effect of multi-objective optimization with adjustable weights.

[0012] S6 performs map information storage, performs feature matching on the image frames of TEB for real-time obstacle avoidance, establishes the correspondence between images, and stores the explored map information.

[0013] As a preferred option, in S1, when the mobile robot determines its position information, the sensors used include odometers, inertial measurement units, and lidar.

[0014] As a preferred option, in S2, the method of specifying the target point includes inputting location coordinate information or selecting from pre-constructed map information.

[0015] As a preferred option, the specific steps for real-time map construction using ORB-SLAM3 technology in S3 are as follows:

[0016] A. Initialize the ROS system and start the Cure-related threads;

[0017] B. Create the SLAM system and prepare to generate frame images. Here, the constructor System::System() of the system is called.

[0018] C. Subscribe to topics to obtain color images from the visual camera;

[0019] D. Convert the subscribed color image into a matrix type, push the RGB image and timestamp parameters into img0Buf, and pass it to the TrackStereo function in System for tracking, while completing the alignment process with the IMU sensor.

[0020] As a preferred solution, in S3, the visual map method is used to construct map information from the image of the previous step. The mobile robot is used as a point and the obstacles are polygons. Then, the starting point S, the ending point G, and the vertices V of the polygons are connected, and the line between two points does not penetrate the obstacle. The distance of the line can be used as the weight of the line, thus establishing a visual map.

[0021] As a preferred option, in S3, when the mobile robot receives the instruction of the target point information, it first determines whether there is a known map in the current environment. If there is a known map, it then uses the global path planning algorithm JPS to calculate an initial path.

[0022] The beneficial effects of the local obstacle avoidance path planning method for mobile robots in unknown environments disclosed in this invention are as follows: Factor graph optimization of TEB is used. Factor graph is an open-source C++ framework based on graph-based nonlinear least squares optimization. Because many problems in SLAM involve minimizing nonlinear error functions that can be represented graphically, graph representation allows TEB to use various soft constraints to optimize the trajectory. Using factor graph effectively solves the sparsity problem of the TEB algorithm, improves the computational speed of path planning, and overcomes the shortcomings of long navigation computation time, high computational cost, and inability to find the optimal path for mobile robots in unknown environments, enabling smooth navigation even in low-performance robots. Attached Figure Description

[0023] Figure 1 This is a flowchart of a local obstacle avoidance path planning method for a mobile robot in an unknown environment, according to the present invention.

[0024] Figure 2 It is an openlist node of the JPS algorithm.

[0025] Figure 3 It is an openlist node of the A* algorithm. Detailed Implementation

[0026] The present invention will be further described and illustrated below with reference to specific embodiments and the accompanying drawings:

[0027] Please refer to Figure 1 A method for local obstacle avoidance path planning for a mobile robot in an unknown environment includes the following steps:

[0028] S1, Determine the initial position information. The mobile robot obtains the position information of the initial state through relevant sensors. In S1, the sensors used by the mobile robot to determine the position information include odometry, inertial measurement unit and lidar.

[0029] S2, set the starting point and target point. Starting from the initial position of the mobile robot, the target point is specified manually. This target point is the target point that the mobile robot will move to. In S2, the target point can be specified by inputting position coordinate information or by selecting from the pre-built map information.

[0030] S3. Determine whether a map needs to be built. The mobile robot determines whether it is in a known environment based on the initial position information obtained in S1. If it is in a known environment, map information does not need to be built and can be obtained by retrieving pre-stored map information or loading map information. If it is in an unknown environment, ORB-SLAM3 technology is used to build the map in real time.

[0031] In S3, the specific steps for real-time map building using ORB-SLAM3 technology are as follows:

[0032] A. Initialize the ROS system and start the Cure-related threads;

[0033] B. Create the SLAM system and prepare to generate frame images. Here, the constructor System::System() of the system is called.

[0034] C. Subscribe to topics to obtain color images from the visual camera;

[0035] D. Convert the subscribed color image into a matrix type, push the RGB image and timestamp parameters into img0Buf, and pass it to the TrackStereo function in System for tracking, while completing the alignment process with the IMU sensor.

[0036] The visualization method is used to construct a map from the image in the previous step. The mobile robot is represented as a point and the obstacles as polygons. Then, the starting point S, the ending point G, and the vertices V of the polygons are connected, and the line connecting the two points does not penetrate the obstacles. The distance of the line can be used as the weight of the line, thus creating a visualization.

[0037] In S3, when the mobile robot receives the instruction of the target point information, it first determines whether there is a known map in the current environment. If there is a known map, it then uses the global path planning algorithm JPS to calculate an initial path.

[0038] S4. The mobile robot uses the costmap converter component to convert the map into a set of points, lines and polygons through the binocular vision camera to prepare the TEB. Based on the current position of the mobile robot and the target position, the mobile robot's TEB is updated for one planning cycle. Then, a new TEB is built using the current global plan. Finally, additional experimental points are added through the JPS algorithm to search for candidate paths and retain trajectories without homologous trajectories.

[0039] S5, using the soft constraints of the factor graph to optimize TEB, using the factor graph to optimize TEB: the edge cost of the factor graph is a soft constraint, so it has the effect of multi-objective optimization with adjustable weights.

[0040] S6 performs map information storage, performs feature matching on the image frames of TEB for real-time obstacle avoidance, establishes the correspondence between images, and stores the explored map information.

[0041] In the above scheme, (1) Environmental information construction: Simultaneous localization and mapping (SLAM) technology is mainly used to construct a real-time environmental map. This invention adopts visual SLAM technology, using a 640*320 resolution camera, a lidar as a distance sensor, and a MEMS inertial measurement unit. The ORB-SLAM3 system is run during navigation. This system is the first feature-based tightly coupled visual inertial odometry system, which has significant advantages over other systems in unknown environments: it can run robustly in real time regardless of the complexity and size of the scene, and its accuracy is several times higher than that of ORB-SLAM3. Since ORB-SLAM3 has improved the new relocalization module of recall to build the map, it can also run smoothly for a long time in scenes with moving obstacles.

[0042] (2) Global Path Planning: This invention employs the Jump Point Search (JPS) algorithm for global path planning. The reason for choosing JPS over A* is that JPS is almost an order of magnitude faster, maintaining both speed and integrity while achieving the optimal solution. For example... Figure 2 and Figure 3 As shown, the A* algorithm considers all neighboring nodes of a node, which greatly reduces the search efficiency of path planning. However, in the case of no occlusion, if only one path is taken from the starting point to the ending point, there is no need to put other paths into the open list. This can save many meaningless nodes. Therefore, the nodes searched by the JPS algorithm are all critical nodes, nodes where the walking direction needs to be changed. The JPS+ algorithm adds preprocessing optimization to the JPS algorithm, which can make path search faster. The specific process is as follows: First, the JPS+ algorithm will determine the jump points of the nodes on the current map and find all the jump points on the current map; then, it will determine the straight-line reachability of these jump points and store the straight-line reachability of each jump point. If it is reachable, it will also record the straight-line distance; the same method will be used to determine the diagonal distance of the jump points. When all eight directions have been processed, the preprocessing is also completed.

[0043] (3) Local Path Planning: This invention employs the Timed Elastic Band (TEB) algorithm for local path planning. TEB is generally considered a nonlinear optimization problem, with constraint parameters including maximum speed, acceleration, minimum turning radius, and minimum distance from obstacles. TEB optimizes a trajectory rather than a path. During local navigation, the starting and target points are specified by the global planner, and several control points controlling the shape of the elastic band are inserted in between. Motion time is defined between these points to ensure that the movement of the mobile robot is real-time. The actual operation of TEB can be understood as placing a rubber band on the initially planned path, then tightening the rubber band between the starting and target points until there is no slack in the path, creating a smooth trajectory while maintaining a safe distance from obstacles. This invention uses factor graphs to optimize TEB. Factor graphs are an open-source C++ framework for graph-based nonlinear least squares optimization. Because many problems in SLAM involve minimizing nonlinear error functions that can be represented graphically, graph representation allows TEB to use various soft constraints to optimize the trajectory. Using factor graphs effectively solves the sparsity problem of the TEB algorithm and improves the computational speed of path planning. The following details the steps for optimizing the TEB: ① Updating obstacle representation: The first step is to convert the local environment into a set of obstacles to fill the edge structure of the factor graph. However, sometimes too many obstacles can lengthen the optimization time. Therefore, this invention uses a costmap converter component to convert the map into a set of points, lines, and polygons. ② TEB preprocessing: The second step is to prepare the TEB. First, update the robot's TEB from the previous planning cycle using the current robot position and the target position. Then, create a new TEB using the current global plan. Finally, add additional experimental points using the JPS algorithm to search for candidate paths. This search may generate many candidate trajectories, retaining those without homologous trajectories. ③ Optimizing the TEB: Optimizing the TEB using factor graphs: The edge cost of the factor graph is a soft constraint, so it has an adjustable weight multi-objective optimization effect.

[0044] The costmap converter component is an existing technology component, and related information can be found at http: / / wiki.ros.org / costmap_converter.

[0045] This invention provides a local obstacle avoidance path planning method for mobile robots in unknown environments. It uses factor graph optimization (TEB), an open-source C++ framework based on graph-based nonlinear least squares optimization. Since many problems in SLAM involve minimizing nonlinear error functions that can be represented graphically, graph representation allows TEB to use various soft constraints to optimize the trajectory. Using factor graphs effectively solves the sparsity problem of the TEB algorithm, improves the computational speed of path planning, and overcomes the shortcomings of long navigation computation time, high computational cost, and inability to find the optimal path in unknown environments for mobile robots, enabling smooth navigation even in low-performance robots.

[0046] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the essence and scope of the technical solutions of the present invention.

Claims

1. A method for local obstacle avoidance path planning for a mobile robot in an unknown environment, characterized in that, Includes the following steps: S1, Determine the initial position information. The mobile robot obtains the initial position information through relevant sensors. S2, Set the starting point and target point. Starting from the initial position of the mobile robot, specify the target point manually. This target point is the target point that the mobile robot will move to. S3: Determine whether map construction is needed. The mobile robot determines whether it is in a known environment based on the initial state position information obtained in S1. If it is in a known environment, map information does not need to be constructed and can be obtained by retrieving pre-stored map information or loading map information. If it is in an unknown environment, ORB-SLAM3 technology is used to construct the map in real time. S4. The mobile robot uses the costmap converter component to convert the map into a set of points, lines and polygons through the binocular vision camera to prepare the TEB. Based on the current position of the mobile robot and the target position, the mobile robot's TEB is updated for one planning cycle. Then, a new TEB is built using the current global plan. Finally, additional experimental points are added through the JPS algorithm to search for candidate paths and retain trajectories without homologous trajectories. S5, using the soft constraints of the factor graph to optimize TEB, using the factor graph to optimize TEB: the edge cost of the factor graph is a soft constraint, so it has the effect of multi-objective optimization with adjustable weights. S6 performs map information storage, performs feature matching on the image frames of TEB for real-time obstacle avoidance, establishes the correspondence between images, and stores the explored map information.

2. The method for local obstacle avoidance path planning of a mobile robot in an unknown environment as described in claim 1, characterized in that, In S1, the sensors used by the mobile robot to determine its location include an odometer, an inertial measurement unit, and a lidar.

3. The method for local obstacle avoidance path planning of a mobile robot in an unknown environment as described in claim 1, characterized in that, In S2, the target point can be specified by inputting location coordinates or by selecting from pre-built map information.

4. The method for local obstacle avoidance path planning of a mobile robot in an unknown environment as described in claim 1, characterized in that, In S3, the specific steps for real-time map building using ORB-SLAM3 technology are as follows: A. Initialize the ROS system and start the Cure-related threads; B. Create the SLAM system and prepare to generate frame images. Here, the constructor System::System() of the system is called. C. Subscribe to topics to obtain color images from the visual camera; D. Convert the subscribed color image into a matrix type, push the RGB image and timestamp parameters into img0Buf, and pass it to the TrackStereo function in System for tracking, while completing the alignment process with the IMU sensor.

5. The method for local obstacle avoidance path planning of a mobile robot in an unknown environment as described in claim 4, characterized in that, In S3, the visual map method is used to construct map information from the image in the previous step. The mobile robot is used as a point and the obstacles are polygons. Then, the starting point S, the ending point G, and the vertices V of the polygons are connected, and the line between two points does not penetrate the obstacle. The distance of the line is used as the weight of the line, thus establishing a visual map.

6. The method for local obstacle avoidance path planning of a mobile robot in an unknown environment as described in claim 1, characterized in that, In S3, when the mobile robot receives the instruction of the target point information, it first determines whether there is a known map in the current environment. If there is a known map, it then uses the global path planning algorithm JPS to calculate an initial path.

Images