A mobile robot autonomous navigation method
By using array cache and multimode probability functions in mobile robot navigation, combining bit operations and Bresenham algorithm, the shortcomings in real-time and efficiency of traditional storage methods are solved, efficient navigation and path planning are achieved, and robot task completion is improved success rate and robustness.
Patent Information
- Application Number
- CN202211006654.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-08-22
- Publication Date
- 2024-12-20
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
In scenarios where real-time requirements are high, traditional quad-tree or octree storage methods are difficult to effectively process large amounts of data, resulting in low map building efficiency and affecting navigation accuracy and task completion.
Using array cache combined with bit operations and multimode probability functions to quickly establish and query the map that the robot can understand, and update the map through the Bresenham algorithm and Gaussian probability model, optimize path planning, and smooth the B-spline to find the optimal path .
It improves the robot's perception speed of the environment and task success rate, realizes the scale and automated deployment of navigation tasks, simplifies map query and path planning, and improves the robustness and efficiency of navigation tasks.
Smart Images
Figure CN115371678B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of robots, and in particular to an autonomous navigation method for a mobile robot. Background Art
[0002] In the past few decades, mobile robot navigation technology has been developing day by day, allowing mobile robots to complete some basic tasks. With the continuous improvement of automation technology, people's requirements for mobile robots are also increasing. For example, the accuracy requirements on the way to complete the task and the degree of completion of the task are constantly improving.
[0003] One of the prerequisites for the robot to complete these tasks is to have accurate perception of the surrounding environment and store the environment for a certain period of time for the next task decision. This process is generally to build a map that the robot can understand. The accuracy of the map directly determines the accuracy of the robot's execution of a given task. The usual mapping method uses the Inverse Sensor Model method to build a sensor data fusion model to avoid complex calculations. When the amount of data is large, quadtrees or octrees are generally used to store data. However, if the real-time requirements are high, such a storage method may not be competent.
[0004] Therefore, a mobile robot autonomous navigation method is needed to solve the above problems. Summary of the invention
[0005] The purpose of the present invention is to provide an autonomous navigation method for a mobile robot. After the information received by the sensor is processed, an array cache is used to establish a local map that the robot can understand. The bit operation of bytes is used to simplify the cost of querying the corresponding position on the map and improve the query efficiency.
[0006] In order to solve the above technical problems, the present invention provides a mobile robot autonomous navigation method, comprising the steps of:
[0007] Map search query based on bit operation, saving the map in a cache array;
[0008] The multi-mode probability function is applied to approximate the objective function in the feasible region and find the optimal path.
[0009] Furthermore, a map is built by accepting external data and updating the distance between the robot and each obstacle.
[0010] Furthermore, the external data is sensor data.
[0011] Furthermore, the cache array is a continuous array with a length of N.
[0012] Furthermore, the position of the spatial point is rasterized to obtain an integer index x of the sensing point, a compensation index is defined as the starting position of the robot's local coordinate system, and a transformation relationship from the global coordinate to the robot's local coordinate is established.
[0013] Furthermore, whether the sensor data is on the map is obtained by formula (1):
[0014] Is it on the map (x) = 0 < = xo <N (1)
[0015] Among them, x is a positive integer, N is a positive integer, and o is the origin of the coordinate system.
[0016] Furthermore, if the sensor data is located on the cache array of the map, the specific position of the data point on the cache array is determined by formula (2):
[0017] Array address (x) = (xo) mod N (2)
[0018] Among them, x is a positive integer, N is a positive integer, o is the origin of the coordinate system, and mod is the remainder operation.
[0019] Furthermore, the specific position of the sensor data is determined by bit operation, and is determined by formula (3) and formula (4):
[0020] Is it on the map (x) =! ((xo)&(~(2 p -1))) (3)
[0021] Array address (x) = (xo) & (2 p -1) (4)
[0022] Where x is a positive integer, N = 2 p is the length of the array, o is the origin of the coordinate system, and ~ is the negation operation.
[0023] Furthermore, a corresponding ray tracing operation is performed on the position between the sensor and the data point according to the Bresenham algorithm to update the map.
[0024] Furthermore, when performing ray tracing, the starting point is selected as the sensor position;
[0025] The data points of the sensor are modeled using Gaussian probability, and when the collected position is the position with the highest initial probability, the probability of the data points within a certain range around it being the target point is updated according to the status of the surrounding collection points;
[0026] Select the point with the highest probability as the end point and perform ray tracing.
[0027] Furthermore, the status of the surrounding collection points includes whether they are visible or blocked.
[0028] Furthermore, the data points of all sensors in each frame are updated to draw the entire map.
[0029] Furthermore, after the map is constructed, the distance between the robot and each obstacle is updated, including the steps of:
[0030] Use a cache array of the same length to store the distance of each data point to the nearest obstacle;
[0031] After each map status update, save all points marked as obstacles, mark their distance from the obstacles as 0, and save all points re-marked as free, marking their values in the cache array as infinity;
[0032] Starting from each obstacle point, update the distances to its surrounding points;
[0033] If the distance between the spatial point and the obstacle is shorter than the distance previously stored in the distance cache array, add this new point to a queue;
[0034] When the points in the queue are empty, it means that the distances of all points on the map have been updated, and the map status is now available.
[0035] Furthermore, the multi-mode probability function is applied to approximate the objective function in the feasible region and find the optimal path, including the following steps:
[0036] Assume that the trajectory follows the following probability distribution:
[0037]
[0038] Where F represents a decreasing function, C represents the cost value, Z represents the partition function, and ε refers to a specific trajectory;
[0039] Through resampling and importance sampling methods, it is converted into an easy-to-sample function. The multimodal distribution function is expressed by a Gaussian mixture model as follows:
[0040]
[0041] Through the Gaussian mixture model (6), we can get d C The approximate value of (ε) is obtained, and then the trajectory ε representing the minimum cost value is obtained.
[0042] Further, a smoothing process is performed, including the steps of:
[0043] The obtained trajectory is represented by a B-spline curve, as shown in the following formula:
[0044]
[0045] Among them, P i refers to the i-th at time t i The control point, B i,k is the basis function of the B-spline curve;
[0046] The cost function is defined as shown in formula (8):
[0047] E total =E c +E q +E l (8)
[0048] Among them, E total To define the cost function, E c represents the collision cost, E q represents smoothness, E l Represents constraints on the various derivatives of the robot state;
[0049] The path is approximated by spline curves and converted into a trajectory. The control amount of the robot at the next moment is calculated by finding the point closest to the trajectory at the current moment, and the robot is controlled to move toward the target point.
[0050] Compared with the prior art, the beneficial effects of the present invention are mainly reflected in: a navigation method for a mobile robot with known positioning is proposed, which combines the mapping and path planning methods, so that the robot can successfully complete the navigation task. This method not only improves the robot's perception speed of the surrounding environment, but also improves the success rate of the robot in completing the specified task. The large-scale and automated deployment of navigation tasks is realized. BRIEF DESCRIPTION OF THE DRAWINGS
[0051] Figure 1 is a flow chart of the autonomous navigation method of a mobile robot of the present invention;
[0052] Figure 2 A flowchart of a method for constructing a map according to the present invention. DETAILED DESCRIPTION
[0053] The mobile robot autonomous navigation method of the present invention will be described in more detail below in conjunction with a schematic diagram, wherein a preferred embodiment of the present invention is shown, and it should be understood that those skilled in the art may modify the present invention described herein while still achieving the advantageous effects of the present invention. Therefore, the following description should be understood as being widely known to those skilled in the art, and not as a limitation of the present invention.
[0054] The present invention is described in more detail in the following paragraphs by way of example with reference to the accompanying drawings. The advantages and features of the present invention will become more apparent from the following description and claims. It should be noted that the drawings are in very simplified form and in non-precise proportions, and are only used to facilitate and clearly assist in illustrating the purpose of the embodiments of the present invention.
[0055] The present invention proposes a mobile robot autonomous navigation method, comprising the steps of:
[0056] Map search query based on bit operation, saving the map in a cache array;
[0057] The multi-mode probability function is applied to approximate the objective function in the feasible region and find the optimal path.
[0058] Specifically, a map is established by receiving external data, and the distance between the robot and each obstacle is updated. Specifically, a map is established by receiving external data, and the distance between the robot and each obstacle is updated. The cache array is a continuous array with a length of N.
[0059] The main content of this application is to propose an autonomous navigation method when the mobile robot knows its position. Specifically, Figure 1 As shown in the figure, this method includes two steps: first, a map that the robot can understand is established by receiving sensor data, and the distance between the robot and each obstacle is updated. Secondly, the map information is used to plan a safe and feasible path for the next movement.
[0060] First, please refer to Figure 2 , build the map:
[0061] Unlike other methods that use quadtree or octree data types to build maps, this method uses array cache to save map data. The robot will receive sensor data and rasterize the position of the spatial point to obtain an integer index x of the sensing point. The cache array is a continuous array of length N, representing the environment within a certain range around the robot. In order to map this cache array to the local coordinate system around the robot, it is necessary to define a compensation index to represent the starting position of this local coordinate system, thereby establishing a conversion relationship from the global coordinate to the robot's local coordinate.
[0062] Next, each sensor data is converted to local coordinates. Since the length of the cache array is known, (1) can be used to determine whether the sensor data is on the map.
[0063] Is it on the map (x) = 0 < = xo <N(1)
[0064] Where x is a positive integer, N is a positive integer, and o is the origin of the coordinate system. If the data point is in the map array cache, (2) can also be used to determine the specific location of the array where the data point should be located:
[0065] Array address (x) = (xo) mod N (2)
[0066] Where x is a positive integer, N is a positive integer, o is the origin of the coordinate system, and mod is the remainder operation. Or use bitwise operations to perform the above two steps:
[0067] Is it on the map (x) =! ((xo)&(~(2 p -1))) (3)
[0068] Array address (x) = (xo) & (2 p -1) (4)
[0069] Where x is a positive integer, N = 2 p is the length of the array, o is the origin of the coordinate system, and ~ is the negation operation.
[0070] In this way, through bit operations, it is possible to quickly determine whether each sensor data point is on the map. If so, its corresponding position on the array is found, and then the corresponding ray tracing operation is performed from the sensor to the data point according to the Bresenham algorithm to update the map.
[0071] When performing ray tracing, the starting point is selected as the sensor position, and the end point is different from general ray tracing and is not a sensor data point. The sensor data points are modeled using Gaussian probability. When the collected position is the position with the highest initial probability, the probability of the points within a certain range around it being the target point is updated according to the status of the surrounding collection points, including whether it is visible or blocked. Then the point with the highest probability is selected as the end point and ray tracing is performed. In this way, all sensor data points in each frame are updated, and the entire map can be updated smoothly.
[0072] After getting a new map, you need to update the new distance from each point on the map to the obstacle. Similar to the map status update, an array cache of the same length is used to save the distance of each point to the nearest obstacle. After each map status update, you only need to save all points marked as obstacles, mark their distance to the obstacle as 0, and save all points that have been re-marked as free, marking their values in the array as infinity.
[0073] After that, starting from each obstacle point, update the distance of the points around it. If the distance of a spatial point from the obstacle is shorter than the distance previously saved in the distance array, then add this new point to a queue. When the points in the queue are empty, it means that the distances of all points on the map have been updated, and the map status is now available. In this way, each map update moves with the robot, but all data is stored in a fixed-size array cache.
[0074] The above mapping method greatly reduces the time it takes for the robot to find a spatial point on the map, and the cost is not complicated, which means that the robot can complete this step in real-time tasks. When each map update is completed, it means that the robot can pass it to the planning module for the next task.
[0075] Secondly, the trajectory planning module:
[0076] For a mobile robot, after obtaining a new map, the main purpose of trajectory planning is generally to find a collision-free and safe path from the starting point to the end point. In order to quickly find this path, a cost value can be assigned to each path. In this way, the final problem becomes finding a path with the minimum cost value so that the robot can move safely to the end point.
[0077] Next, the cost function is designed by considering smoothness, obstacle avoidance, and time optimization so that all paths can be represented. Generally speaking, the cost function designed by considering multiple conditions is non-convex in most cases. This makes it relatively difficult to find the optimal value because the cost function exhibits a certain degree of multimodality.
[0078] The present invention considers converting the multimodality of the cost function into a density function of the trajectory, and approximating the multimodality of the cost function by estimating the distribution of the density function. In this way, by approximating a multimodal distribution function, multiple trajectories can be obtained. These trajectories represent candidates for the optimal trajectory, and the optimal trajectory can be obtained through optimization techniques such as gradient descent, which can simultaneously meet some preset constraints.
[0079] The main problem then becomes how to find such a multi-mode density function that can represent the trajectory. Assume that the trajectory obeys the following probability distribution:
[0080]
[0081] Where F represents a decreasing function and C represents the cost value. Z represents a partitioning function and ε refers to a specific trajectory. Generally speaking, it is impossible to directly sample such a probability function, so it can be converted into a function that is easier to sample through resampling and importance sampling methods. Finally, the multimodal distribution function can be represented by a Gaussian mixture model as follows:
[0082]
[0083] By sampling the simple Gaussian mixture model (6), we can obtain d C In this way, we can get the trajectory ε that represents the minimum cost value.
[0084] Next, in order to make the robot move more smoothly, the trajectory needs to be further smoothed. This is because although a trajectory with the lowest cost can be obtained through density estimation and resampling technology, such a trajectory cannot meet certain constraints in some cases. Therefore, smoothing is necessary for the robot to move smoothly. Here, a B-spline curve is used to represent the obtained trajectory, as shown in the following formula:
[0085]
[0086] Among them, P i refers to the i-th at time t i The control point, B i,k is the basis function of the B-spline curve. On this basis, the cost function can be defined as shown in formula (8):
[0087] E total =E c +E q +E l (8)
[0088] Among them, E total To define the cost function, E c represents the collision cost, which is used to punish the collision between the robot and the obstacle, E q represents smoothness, which means that the derivatives of the robot's motion state need to be as small as possible, thus ensuring the smoothness of the robot's movement. l It represents a soft constraint, indicating that the derivatives of the robot's motion state must not exceed the set amount. Otherwise, it will be punished. This ensures the availability of the trajectory. The path is approximated by a spline curve. The path can be converted into a trajectory. By finding the point closest to the trajectory at the current moment, the control amount of the robot at the next moment is calculated. In this way, the robot is continuously controlled to move towards the target point.
[0089] The present invention uses array cache to store data when building a map, and each query will only take O(1) time, which will greatly improve the time cost compared with the traditional quadtree and octree. At the same time, the present invention does not reduce the time cost when modifying data.
[0090] After solving the map problem, robot trajectory planning is another major difficulty in the navigation task. The traditional method is to have a global path in advance, and intercept a section of the global path each time to perform local path planning. This is generally not a big problem in a static environment, but for a dynamic environment, this will cause the robot to ignore other better paths and fall into a local optimum. The present invention will avoid this problem by designing a new local path planning method. Thereby ensuring the robustness of navigation and improving the success rate of the robot completing the task.
[0091] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.
Claims
1. A mobile robot autonomous navigation method, characterized in that: Includes steps: Map retrieval query based on bit operation, storing the map in a cache array; applying a multi-modal probability function to approximate the target function in a feasible area to find the optimal path; The map search query based on bit operation saves the map in the cache array including: Rasterize the position of the space point to obtain an integer index x of the sensing point, define a compensation index as the starting position of the robot's local coordinate system, and establish the transformation relationship from the global coordinate to the robot's local coordinate; According to the Bresenham algorithm, the corresponding ray tracing operation is performed between the sensor and the data point to update the map; The ray tracing operation includes: Select the sensor position as the starting point; The data points of the sensor are modeled using Gaussian probability, and when the collected position is the position with the highest initial probability, the probability of the data points within a certain range around it being the target point is updated according to the status of the surrounding collection points; Select the point with the highest probability as the end point and perform ray tracing.
2. The mobile robot autonomous navigation method according to claim 1, characterized in that: Build a map by accepting external data and updating the distance between the robot and each obstacle.
3. The mobile robot autonomous navigation method according to claim 2, characterized in that: The external data is sensor data.
4. The mobile robot autonomous navigation method according to claim 3, characterized in that: The cache array is a continuous array with a length of N.
5. The mobile robot autonomous navigation method according to claim 1, characterized in that: Whether the sensor data is on the map is obtained by formula (1): Is it on the map (x) = 0 < = xo <N (1) Among them, x is a positive integer, N is a positive integer, and o is the origin of the coordinate system.
6. The mobile robot autonomous navigation method according to claim 5, characterized in that: If the sensor data is located on the cache array of the map, the specific position of this data point on the cache array is determined by formula (2): Array address (x) = (xo) mod N (2) Among them, x is a positive integer, N is a positive integer, o is the origin of the coordinate system, and mod is the remainder operation.
7. The mobile robot autonomous navigation method according to claim 1, characterized in that: The specific position of the sensor data is determined by bit operation, and is determined by formula (3) and formula (4): Is it on the map (x) =! ((xo)&(~(2 p -1)))(3) Array address (x) = (xo) & (2 p -1)(4) Where x is a positive integer, N = 2 p is the length of the array, o is the origin of the coordinate system, and ~ is the negation operation.
8. The mobile robot autonomous navigation method according to claim 1, characterized in that: The status of the surrounding collection points includes whether they are visible or blocked.
9. The mobile robot autonomous navigation method according to claim 1, characterized in that: The data points of all sensors are updated in each frame to draw the entire map.
10. The mobile robot autonomous navigation method according to claim 4, characterized in that: After the map is constructed, the distance between the robot and each obstacle is updated, including the following steps: Use a cache array of the same length to store the distance of each data point to the nearest obstacle; After each map status update, save all points marked as obstacles, mark their distance from the obstacles as 0, and save all points re-marked as free, marking their values in the cache array as infinity; Starting from each obstacle point, update the distances to its surrounding points; If the distance between the spatial point and the obstacle is shorter than the distance previously stored in the distance cache array, add this new point to a queue; When the points in the queue are empty, it means that the distances of all points on the map have been updated, and the map status is now available.
11. The mobile robot autonomous navigation method according to claim 1, characterized in that: Apply the multi-mode probability function to approximate the objective function in the feasible region and find the optimal path, including the following steps: Assume that the trajectory follows the following probability distribution: Where F represents a decreasing function, C represents the cost value, Z represents the partition function, and ε refers to a specific trajectory; Through resampling and importance sampling methods, it is converted into an easy-to-sample function. The multimodal distribution function is expressed by a Gaussian mixture model as follows: Through the Gaussian mixture model (6), we can get d C The approximate value of (ε) is obtained, and then the trajectory ε representing the minimum cost value is obtained.
12. The mobile robot autonomous navigation method according to claim 11, characterized in that: The smoothing process includes the following steps: The obtained trajectory is represented by a B-spline curve, as shown in the following formula: Among them, P i refers to the i-th at time t i The control point, B i,k is the basis function of the B-spline curve; The cost function is defined as shown in formula (8): AND total =And c +E q +E l (8) Among them, E total To define the cost function, E c represents the collision cost, E q represents smoothness, E l Represents constraints on the various derivatives of the robot state; The path is approximated by spline curves and converted into a trajectory. The control amount of the robot at the next moment is calculated by finding the point closest to the trajectory at the current moment, and the robot is controlled to move toward the target point.