Cleaning robot local obstacle avoidance method based on single steering wheel model

Abstract

The invention discloses a cleaning robot local obstacle avoidance method based on a single steering wheel model, particularly relates to the field of robot control, and comprises a map matrix information generation step, a collision cost calculation step, a cost calculation step and a trajectory optimization calculation step. The local obstacle avoidance method disclosed by the invention brings a remarkable effect to the cleaning robot. By comprehensively considering sensor data and environment information of the robot, a comprehensive robot model is constructed, real-time path planning and obstacle avoidance are realized, and the robot can clean safely and efficiently in a complex environment. According to the method, the obstacles in the local environment are effectively identified and predicted, so that the robot can intelligently avoid the obstacles, and the cleaning efficiency and safety are improved. According to the effect, a reliable solution is provided for application of the cleaning robot, and the cleaning robot can complete cleaning more reliably and efficiently in various use scenes.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and more specifically, to a local obstacle avoidance method for a cleaning robot based on a single steering wheel model. Background Art

[0002] Cleaning robots are usually designed to be able to perform cleaning tasks autonomously in indoor environments. However, there are various obstacles in indoor environments, such as furniture, walls, wires, etc. In order to prevent robots from colliding with these obstacles, local obstacle avoidance technology has emerged. This technology enables robots to avoid obstacles by sensing the surrounding environment and taking corresponding actions.

[0003] At present, sweeper robots with local obstacle avoidance have been widely used in many fields, including home and commercial cleaning, for automatically cleaning floors, carpets and other flat surfaces. Sweeping robots can autonomously navigate and clean dust, debris and other dirt, reducing the burden of manual cleaning; hotel and hospital hygiene, sweeping robots are widely used in places such as hotel rooms and hospital wards. They can regularly clean floors, maintain a sanitary environment, and reduce the risk of pathogen transmission; industrial and storage facility cleaning, sweeping robots are used in industrial and storage facilities to clean the ground and remove accumulated debris and garbage. They can work efficiently in large areas of space, improving cleaning efficiency and work efficiency; public places and commercial spaces, sweeping robots are widely used in public places such as airports, shopping malls, restaurants and office buildings. They can perform regular cleaning in high-traffic areas to keep the ground clean and tidy.

[0004] However, there are still some shortcomings in its actual use. For example, the existing local obstacle avoidance methods of cleaning robots are mainly designed for cleaning tasks in home and commercial environments. These methods usually use sensor technology, such as laser sensors and cameras, to perceive obstacles in the local environment, and adopt path planning and obstacle avoidance strategies to avoid obstacles to ensure that the robot can safely perform cleaning work. However, these methods have some defects. First, sensor technology is limited by environmental conditions, such as lighting changes and occlusions, which may lead to reduced accuracy of obstacle detection and recognition. Second, for complex environments and diverse obstacles, current methods may face processing difficulties, resulting in the robot's inability to effectively avoid obstacles. Finally, real-time decision-making capabilities are also a challenge, because robots need to make instant decisions in rapidly changing environments to avoid obstacles, which requires efficient algorithms and computing power, as well as accurate environmental models and prediction capabilities. There are some defects in the current local obstacle avoidance methods of cleaning robots. First, sensor limitations are a major problem, because robots rely on sensor technology to perceive obstacles, but sensors may be limited by environmental conditions, such as lighting changes or occlusions, which reduces the accuracy of obstacle detection and recognition. Secondly, dealing with complex environments is also a challenge, because in a home or commercial environment, the robot needs to deal with many types of obstacles, such as furniture, wires, and toys. Current local obstacle avoidance methods may have difficulties in dealing with complex environments, resulting in the robot being unable to effectively avoid obstacles. Finally, the robot's real-time decision-making ability is also a problem, because it needs to make decisions quickly in a real-time environment to avoid obstacles, which requires efficient algorithms and computing power, as well as accurate environmental models and prediction capabilities. Summary of the invention

[0005] In order to overcome the above-mentioned defects of the prior art, an embodiment of the present invention provides a local obstacle avoidance method of a cleaning robot based on a single steering wheel model, and solves the problems raised in the above-mentioned background technology through the following scheme.

[0006] To achieve the above object, the present invention provides the following technical solutions: A local obstacle avoidance method for a cleaning robot based on a single steering wheel model, comprising: step 1: map matrix information generation: used to generate an environment map matrix to obtain obstacle information;

[0007] Step 2: Collision cost calculation: used to calculate the collision cost of the position after determining whether it collides with an obstacle;

[0008] Step 3: Cost calculation: used to calculate the orientation cost, collision cost, deviation from the reference path cost and distance cost when there is no collision node;

[0009] Step 4: Trajectory optimization calculation: used to establish a kinematic model to calculate the robot's motion trajectory and optimize it.

[0010] Preferably, the step 1 specifically includes the following steps:

[0011] Step 1.1: Represent the robot model, obstacles, and path boundaries in the form of an image pixel value matrix. First, for obstacles of different heights, it is necessary to distinguish the height of the obstacles to determine whether the cleaning robot can cross them, including higher gray obstacles and lower white obstacles.

[0012] Step 1.2: Use a ground segmentation method to segment the ground and obstacles;

[0013] Step 1.3: Assign different obstacle costs to each pixel based on the distance of the pixel from the central obstacle.

[0014] Preferably, the step 2 specifically includes the following steps:

[0015] Step 2.1: For the cleaning robot motion model, use the offline construction method to calculate the contour error;

[0016] Step 2.2: Determine the collision between the robot and the high obstacle;

[0017] Step 2.3: After determining whether there is a collision with an obstacle, calculate the collision cost at that location.

[0018] Preferably, the step 3 specifically includes the following steps:

[0019] Step 3.1: Calculate the initial position of the robot;

[0020] Step 3.2: Node expansion;

[0021] Step 3.3: Expand node collision judgment;

[0022] Step 3.4: Calculate node collision cost;

[0023] Step 3.5: Calculate the node's deviation from the reference path cost;

[0024] Step 3.6: Node cost calculation, used to calculate the current cost of the node.

[0025] Preferably, the step 4 specifically includes the following steps:

[0026] Step 4.1: Calculation of robot kinematics;

[0027] Step 4.2: Express the output relationship according to the known kinematic model;

[0028] Step 4.3: Ensure the optimization range of the trajectory is within the safe space through collision constraints;

[0029] Step 4.4: When the terrain is relatively flat, the kinematic constraints degenerate into the following standard kinematic constraints;

[0030] Step 4.5: Control output of the cleaning robot.

[0031] Preferably, the error of the Ackerman model with a length of L in calculating the profile in this way is:

[0032]

[0033] Where θ is the angle interval. When θ is 0.25 degrees and L is 2 m, the error is 0.002 m, which is within the acceptable range. L is the constant of the Ackerman motion model.

[0034] The specific method for judging the collision situation of the height obstacle is as follows:

[0035]

[0036] Among them, find max calculates the dot product of matrix A and matrix B to obtain a new matrix to determine whether the maximum value is greater than 255. If it is greater than 255, the robot has collided with the obstacle. If it is less than 255, the robot has no actual collision with the obstacle or the obstacle is inside the robot chassis.

[0037] Preferably, the robot initial position model is as follows:

[0038]

[0039] Among them, t represents the sampling time, which represents the tth time in computer control, θ represents the forward sampling angle, x and y represent the horizontal and vertical axes of the plane projection coordinate system of the sampling point, and v represents the speed of the robot relative to its current forward direction;

[0040] The calculation of the collision cost σ2 of each node is expressed as follows:

[0041]

[0042] Among them, R(x) is the calculation result of the above robot model unit pixel matrix and the environment pixel sub-matrix, that is, the statistics of the collision cost under the actual coverage of the robot model, D x It is the inverse of the distance between the extended node and any obstacle. When this value is greater than the set Th risk The calculation of node collision cost is effective only when , and the collision cost of any other node is 0 under this threshold, that is, it does not participate in the calculation of node cost;

[0043] The calculation model of the node's deviation from the reference path cost is as follows:

[0044]

[0045] Among them, H(x) is the calculation result of the above robot model unit pixel matrix and the path map pixel sub-matrix, that is, the deviation cost of the robot model under the actual coverage path is counted, Dh x It is the inverse of the distance between the extended node and the central reference path. When this value is greater than the set Th head The calculation of the node deviation cost from the reference path is valid only when , and the deviation cost of any other node is 0 under this threshold, that is, it does not participate in the calculation of the node cost;

[0046] The specific mathematical model of the current cost is as follows:

[0047]

[0048] Preferably, the kinematic relationship calculation model is as follows:

[0049]

[0050] Where h1 is the turning radius of a single steering wheel, v d is the driving speed of the robot, v, θ and δ are the speed, heading angle and steering angle of the steering wheel in the x-axis direction in the robot coordinate system, and l is the wheelbase of the robot;

[0051] The output relationship can be expressed as:

[0052]

[0053] Among them, r d is the driving wheel radius, w d is the driving wheel angular velocity;

[0054] The collision constraints are as follows:

[0055]

[0056]

[0057] in, To pass through a path point below the chassis obstacle, X buffer The amount of space left in the chassis for obstacles;

[0058] The kinematic constraints are as follows:

[0059]

[0060] Among them, α(t) is the slope of the plane where the trajectory point is located at time t, which is obtained according to the three-dimensional topographic map and the change of the z value of the trajectory point in the previous step. The z value is determined by the state quantity, which is the wheelbase of the front and rear wheels of the Ackerman model, and which is the direction angle projected to the XY plane;

[0061] The control quantity output of the cleaning robot is as follows:

[0062]

[0063] Preferably, the method is as follows:

[0064] The point cloud with a value of 0 after segmentation is the ground, and the point cloud with a value other than 0 is an obstacle of different heights. The height of the obstacle relative to the ground is extracted through ground segmentation. The height information of the obstacle is then projected to obtain a pixel matrix. Dark colors represent obstacles above the chassis, and light colors represent obstacles below the chassis. Its mathematical representation is an environment map pixel matrix, and the heights of different obstacles have different values. Values ​​greater than 255 represent obstacles above the robot chassis, and values ​​less than 255 represent obstacles below the robot chassis. Obstacle information is obtained through this environment map matrix.

[0065] Preferably, a robot model matrix is ​​generated every 0.25 degrees with the robot as the center, and the robot models of 1440 different angles generated are stored as a matrix array. During collision detection, the corresponding robot pixel matrix is ​​selected according to the different angle information of the path points in the path. The chassis and tires are represented by different numerical values, 255 represents the tire, and 10 represents the chassis.

[0066] Preferably, in the path search phase, an area of ​​the same size as the robot model matrix is ​​selected on the obstacle map according to the position of the path point, and a model matrix in the offline robot model matrix library is selected according to the angle of the path point.

[0067] Preferably, the robot model is projected into a unit matrix in the same way, and the model unit matrix in the offline robot model matrix library is selected according to the angle of the path point. According to the position of the path point, select the regional matrix C of the same size as the robot model matrix on the obstacle map. The collision cost of the path point is obtained by the tensor product of the two matrices. Similarly, when calculating the cost of the current path point deviating from the reference path, select the model unit matrix in the offline robot model matrix library according to the angle of the path point. According to the position of the path point, a regional submatrix P of the same size as the robot model matrix is ​​selected on the path map, and the deviation path cost of the path point is obtained by the tensor product of the two matrices.

[0068] Preferably, the Hybrid A* path planning algorithm is introduced in the front-end path search stage by calculating the robot model matrix, and the kinematic constraints of the cleaning robot are used to search on the obstacle map. The search space includes position and angle (x, y, θ). Different from the traditional A *, searching in a discrete state grid, but its discrete grid is associated with a continuous state. * It is a segmented line segment. Since its node expansion method follows the kinematic constraints, the path it generates can be directly followed by the Ackerman model. Because it combines the continuous coordinate states that occupy the same state grid in the continuous discrete space, the algorithm cannot guarantee the minimum cost solution. However, in actual practice, the results of the search algorithm are usually near the global optimal solution. The present invention uses its results as the initial guess of the nonlinear trajectory optimization stage, which is conducive to accelerating the convergence of the trajectory optimization stage.

[0069] Preferably, when the node is extended in the direction, it is necessary to set different cost penalties for the extended nodes that turn, and combine the Reed-Shepp curve to connect the end points at a certain frequency to increase the search speed. Each node obtains the cost of the node by calculating the kinematic model matrix of the cleaning robot and the obstacle map matrix, as described in the previous section. A calculation method is used for the heuristic function of the path search algorithm, that is, the distance from the current position to the target position is considered kinematics while ignoring the obstacles. In the program, the present invention directly uses the length of the Reed-Shepp curve as the heuristic function. Because a discrete state space is used during the search, this means that the search will never reach the precise continuous coordinate target state, and its accuracy depends on the resolution of the grid. In order to solve the accuracy problem and further improve the search speed, the search is enhanced based on the Reed-Shepp curve. An additional child node is generated by calculating the optimal Reed-Shepp path from the current state to the target. Then check whether the Reed-Shepp path collides with the current obstacle, and if there is no collision, add the child node to the search tree. In order to reduce the amount of calculation, it is not desirable to apply the Reed-Shepp curve to each node, especially the node far away from the target, because most such paths may collide. In the implementation, a simple selection rule is used, where the Reed-Shepp curve is applied to one of every N nodes, where N decreases as the heuristic function decreases, i.e., the Reed-Shepp curve is used more frequently when the current node is closer to the target.

[0070] Preferably, the state of the node, i.e. (x, y, θ), is obtained by node expansion, a submatrix of the same size as the robot model pixel matrix is ​​selected from the above-mentioned environment pixel matrix according to the node position, and the corresponding robot model pixel matrix is ​​selected from the offline robot model pixel matrix library according to the orientation information, and whether the nodes collide is determined by the above-mentioned matrix calculation method.

[0071] Preferably, for the searched trajectory, trajectory optimization is required, and the front-end path that meets the kinematic constraints is used as the initial guess of the nonlinear optimization framework to accelerate the convergence speed. In the construction of the safety corridor, when the initial path passes through an obstacle below the chassis without colliding with the obstacle, the safety corridor of this part of the path point is limited to around the low obstacle, so that the optimization range of the trajectory is guaranteed to be in the safe space.

[0072] Technical effects and advantages of the present invention:

[0073] 1. The local obstacle avoidance method of the present invention adopts a full-model strategy, which comprehensively considers information from multiple aspects. First, the robot's sensor system obtains data in the environment, including distance, depth and other information. Secondly, these data are used to build a comprehensive and accurate robot model, including the robot's geometry, motion capabilities and perception range. Based on this full model, the scheme realizes real-time path planning and obstacle avoidance. The robot can use the model to analyze and predict the current environment, identify potential obstacles, and make intelligent decisions based on information such as their location, size and movement trend. By dynamically adjusting the path and speed, the robot can avoid obstacles to ensure the smooth completion of the cleaning task;

[0074] 2. The present invention can provide a more comprehensive and accurate environmental perception capability by comprehensively considering the data of multiple sensors, thereby enhancing the obstacle avoidance effect. Secondly, the robot model is used for path planning and decision-making, so that countermeasures can be made more flexibly when encountering complex and dynamic obstacles. In addition, the solution is also real-time, and the robot can respond quickly in a changing environment, improving cleaning efficiency and safety. The local obstacle avoidance solution based on the full robot model proposed in this patent makes full use of sensor data and environmental information to build a comprehensive robot model, and realizes intelligent path planning and obstacle avoidance, providing a reliable solution for the application of cleaning robots in complex environments;

[0075] 3. The present invention is applicable to various cleaning robot usage scenarios, including household cleaning, commercial cleaning, public area cleaning and industrial cleaning, etc. Through the local obstacle avoidance method proposed in this patent, the robot can avoid collision with obstacles in complex environments, achieve safe and efficient cleaning tasks, protect the integrity of facilities, and improve the cleaning effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 It is the main algorithm flow chart of the present invention.

[0077] Figure 2 The environment pixel sub-matrix is ​​selected according to the position of the path point in the environment matrix of the present invention.

[0078] Figure 3 It is the node expansion method and Reed-Shepp curve of the path search in the present invention.

[0079] Figure 4 It is a safety corridor generated based on low obstacles that can pass through the robot chassis in the present invention. DETAILED DESCRIPTION

[0080] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0081] As attached Figure 1 , Attachment Figure 2 , Attachment Figure 3 and attached Figure 4 A local obstacle avoidance method for a cleaning robot based on a single steering wheel model is shown, comprising:

[0082] Step 1: Map matrix information generation: used to generate the environment map matrix to obtain obstacle information;

[0083] Step 1.1: Represent the robot model, obstacles, and path boundaries in the form of an image pixel value matrix. First, for obstacles of different heights, it is necessary to distinguish the height of the obstacles to determine whether the cleaning robot can cross them, including higher gray obstacles and lower white obstacles.

[0084] Step 1.2: Use a ground segmentation method to segment the ground and obstacles. The method is as follows:

[0085] The point cloud with a value of 0 after segmentation is the ground, and the point cloud with a value other than 0 is an obstacle of different heights. The height of the obstacle relative to the ground is extracted through ground segmentation. The height information of the obstacle is then projected to obtain a pixel matrix. Dark colors represent obstacles above the chassis, and light colors represent obstacles below the chassis. Its mathematical representation is an environment map pixel matrix, and the heights of different obstacles have different values. Values ​​greater than 255 represent obstacles above the robot chassis, and values ​​less than 255 represent obstacles below the robot chassis. Obstacle information is obtained through this environment map matrix.

[0086] Step 1.3: Assign different obstacle costs to each pixel based on the distance of the pixel from the central obstacle;

[0087] What needs to be specifically explained in this embodiment is that the cost value of a pixel is inversely proportional to the distance from the center of the obstacle, that is, the larger the pixel value, the closer to the obstacle, and the higher the cost. The reference path at the front end is used as the center to generate the cost pixel matrix of the path. The cost value of the pixel is inversely proportional to the distance from the center of the reference path. The farther the node is from the center of the reference path, the higher the cost. This map will serve as the basic criterion for path planning.

[0088] Step 2: Collision cost calculation: used to calculate the collision cost of the position after determining whether it collides with an obstacle;

[0089] Step 2.1: For the cleaning robot motion model, use the offline construction method to calculate the contour error;

[0090] What needs to be specifically explained in this embodiment is that: a robot model matrix is ​​generated every 0.25 degrees with the robot as the center, and the generated robot models of 1440 different angles are stored as a matrix array. During collision detection, the corresponding robot pixel matrix is ​​selected according to the different angle information of the path points in the path. The chassis and tires are represented by different numerical values, 255 represents the tire, and 10 represents the chassis. The error of the Ackerman model with a length of L in this way is calculated as:

[0091]

[0092] Where θ is the angle interval. When θ is 0.25 degrees and L is 2 meters, the error is 0.002 meters, which is within the acceptable range. L is the constant of the Ackerman motion model.

[0093] The model can be generated directly through the top view projection of the robot, which is a good way to deal with robots with irregular contours. When the robot itself has an irregular contour, generating the robot model pixel matrix in this way can accurately detect collisions between obstacles and the robot.

[0094] Step 2.2: Determine the collision between the robot and the high obstacle;

[0095] What needs to be specifically explained in this embodiment is that in the path search stage, an area of ​​the same size as the robot model matrix is ​​selected on the obstacle map according to the position of the path point, and a model matrix in the offline robot model matrix library is selected according to the angle of the path point. The specific method for determining the collision situation of the height obstacle is:

[0096]

[0097] Among them, find max calculates the dot product of matrix A and matrix B to obtain a new matrix to determine whether the maximum value is greater than 255. If it is greater than 255, the robot has collided with the obstacle. If it is less than 255, the robot has no actual collision with the obstacle or the obstacle is inside the robot chassis.

[0098] Step 2.3: After determining whether there is a collision with an obstacle, calculate the collision cost at that location.

[0099] In this embodiment, it should be specifically explained that the robot model is projected into a unit matrix in the same way, and the model unit matrix in the offline robot model matrix library is selected according to the angle of the path point. According to the position of the path point, select the regional matrix C of the same size as the robot model matrix on the obstacle map. The collision cost of the path point is obtained by the tensor product of the two matrices. Similarly, when calculating the cost of the current path point deviating from the reference path, select the model unit matrix in the offline robot model matrix library according to the angle of the path point. According to the position of the path point, a regional submatrix P of the same size as the robot model matrix is ​​selected on the path map, and the deviation path cost of the path point is obtained by the tensor product of the two matrices.

[0100] Step 3: Cost calculation: used to calculate the orientation cost, collision cost, deviation from the reference path cost and distance cost when there is no collision node;

[0101] Step 3.1: Calculate the initial position of the robot;

[0102] What needs to be specifically explained in this embodiment is that the Hybrid A* path planning algorithm is introduced in the front-end path search stage by calculating the robot model matrix, and the kinematic constraints of the cleaning robot are used to search on the obstacle map. The search space includes position and angle (x, y, θ). Different from the traditional A * , searching in a discrete state grid, but its discrete grid is associated with a continuous state. * It is a segmented line segment. Since its node expansion method follows the kinematic constraints, the path it generates can be directly followed by the Ackerman model. Because it combines the continuous coordinate states that occupy the same state grid in the continuous discrete space, the algorithm cannot guarantee the minimum cost solution. However, in actual practice, the results of the search algorithm are usually near the global optimal solution. The present invention uses its results as the initial guess of the nonlinear trajectory optimization stage, which is conducive to accelerating the convergence of the trajectory optimization stage; the robot initial position model is as follows:

[0103]

[0104] Among them, t represents the sampling time, which represents the tth time in computer control, θ represents the forward sampling angle, x and y represent the horizontal and vertical axes of the plane projection coordinate system of the sampling point, and v represents the speed of the robot relative to its current forward direction.

[0105] Step 3.2: Node expansion;

[0106] What needs to be specifically explained in this embodiment is that when the node is extended in the direction, it is necessary to set different cost penalties for the extended nodes that turn, and combine the Reed-Shepp curve to connect the end points at a certain frequency to increase the search speed. Each node obtains the cost of the node by calculating the kinematic model matrix of the cleaning robot and the obstacle map matrix, as described in the previous section. A calculation method is used for the heuristic function of the path search algorithm, that is, considering the kinematics from the current position to the target position and ignoring the distance of the obstacles. In the program, the present invention directly uses the length of the Reed-Shepp curve as the heuristic function. Because a discrete state space is used during the search, this means that the search will never reach the precise continuous coordinate target state, and its accuracy depends on the resolution of the grid. In order to solve the accuracy problem and further improve the search speed, the search is enhanced based on the Reed-Shepp curve. An additional child node is generated by calculating the optimal Reed-Shepp path from the current state to the target. Then check whether the Reed-Shepp path collides with the current obstacle, and if there is no collision, add the child node to the search tree. In order to reduce the amount of computation, it is not desirable to apply the Reed-Shepp curve to every node, especially nodes far from the target, because most of such paths may collide. In the implementation, a simple selection rule is used, where the Reed-Shepp curve is applied to one of every N nodes, where N decreases as the heuristic function decreases, i.e., the Reed-Shepp curve is used more frequently when the current node is closer to the target.

[0107] Step 3.3: Expand node collision judgment;

[0108] What needs to be specifically explained in this embodiment is that the state of the node, i.e. (x, y, θ), is obtained by node expansion, a submatrix of the same size as the robot model pixel matrix is ​​selected from the above-mentioned environment pixel matrix through the node position, and the corresponding robot model pixel matrix is ​​selected from the offline robot model pixel matrix library through the orientation information, and whether the nodes collide is determined by the above-mentioned matrix calculation method.

[0109] Step 3.4: Calculate node collision cost;

[0110] When expanding a collision-free node, the current cost needs to be calculated. The following mainly calculates the orientation cost, collision cost, deviation from the reference path cost and distance cost. In addition, the value of the above heuristic function is added to form the overall cost of the node. The calculation of the orientation cost is simple, that is, the more the angle of the expanded node deviates from the current orientation, the higher the cost. In this algorithm, the orientation of the node is expanded into 5 directions according to the steering angle of the cleaning robot model. In addition to the same orientation as the current orientation, a steering cost σ1 is added to other orientations.

[0111] The calculation of the collision cost σ2 of each node is expressed as follows:

[0112]

[0113] Among them, R(x) is the calculation result of the above robot model unit pixel matrix and the environment pixel sub-matrix, that is, the statistics of the collision cost under the actual coverage of the robot model, D x It is the inverse of the distance between the extended node and any obstacle. When this value is greater than the set Th risk The calculation of node collision cost is effective only when , and the collision cost of any other node is 0 under this threshold, that is, it does not participate in the calculation of node cost.

[0114] It should be specifically explained in this embodiment that the reason for using this cost function is that, first, it reduces unnecessary calculations. When the node is far away from the obstacle, the collision risk is close, and the collision cost is meaningful only when it is a certain distance away from the obstacle. Second, it is hoped that there will be a buffer zone between the cleaning robot and other surrounding objects to minimize the risk of collision.

[0115] Step 3.5: Calculate the node's deviation from the reference path cost;

[0116] In addition, the deviation cost σ3 of each node from the reference path penalizes the distance the robot deviates from the reference path. In the autonomous exploration system, this ensures that the obstacle avoidance path cannot deviate too much from the original reference path to protect the original environmental gain information. The calculation method is as follows. To a certain extent, it ensures the smoothness of the generated path, rather than always sticking to the center of the reference path. The calculation model of the node's deviation from the reference path cost is as follows:

[0117]

[0118] Among them, H(x) is the calculation result of the above robot model unit pixel matrix and the path map pixel sub-matrix, that is, the deviation cost of the robot model under the actual coverage path is counted, Dh x It is the inverse of the distance between the extended node and the central reference path. When this value is greater than the set Th headThe calculation of the node deviation cost from the reference path is valid only when , and the deviation cost of any other node is 0 under this threshold, that is, it does not participate in the calculation of the node cost.

[0119] Step 3.6: Node cost calculation, used to calculate the current cost of the node;

[0120] Finally, the distance from the robot position to the current node is used as the distance cost σ4 of the node. The four different costs are assigned different weights according to the actual scene needs and used together as the current cost of the node. The specific mathematical model of the current cost is as follows:

[0121]

[0122] Step 4: Trajectory optimization calculation: used to establish a kinematic model to calculate the robot's motion trajectory and optimize it;

[0123] Step 4.1: Calculation of robot kinematics;

[0124] First, we need to establish the kinematic model of the sweeper. XY is the global coordinate system, and xy is the local coordinate system, that is, the sweeper robot coordinate system. Assume that the robot does not slide during movement. ICR is the instantaneous rotation center of the robot, and the kinematic relationship calculation model is as follows:

[0125]

[0126] Where h1 is the turning radius of a single steering wheel, v d is the driving speed of the robot, v, θ and δ are the speed, heading angle and steering angle of the steering wheel in the x-axis direction in the robot coordinate system, and l is the wheelbase of the robot.

[0127] Step 4.2: Express the output relationship according to the known kinematic model;

[0128] According to the known kinematic model, it is necessary to derive the input-output relationship of the control quantity, which can be expressed as:

[0129]

[0130] Among them, r d is the driving wheel radius, w d is the driving wheel angular velocity.

[0131] Step 4.3: Ensure the optimization range of the trajectory is within the safe space through collision constraints;

[0132] For the searched trajectory, trajectory optimization is required. The front-end path generated in accordance with the kinematic constraints is used as the initial guess of the nonlinear optimization framework to accelerate the convergence speed. In the construction of the safety corridor, when the initial path passes through an obstacle below the chassis without colliding with the obstacle, the safety corridor of this part of the path point is limited to the low obstacle, so that the optimization range of the trajectory is guaranteed to be in the safe space. The collision constraints are as follows:

[0133]

[0134] in, To pass through a path point below the chassis obstacle, X buffer The amount of space left in the chassis for obstacles.

[0135] Step 4.4: When the terrain is relatively flat, the kinematic constraints degenerate into the following standard kinematic constraints. The kinematic constraints are as follows:

[0136]

[0137] Among them, α(t) is the slope of the plane where the trajectory point is located at time t, which is obtained according to the three-dimensional topographic map and the change of the z value of the trajectory point in the previous step. The z value is determined by the state quantity, which is the wheelbase of the front and rear wheels of the Ackerman model, and the other is the direction angle projected to the XY plane.

[0138] Step 4.5: Control output of the cleaning robot;

[0139] The control quantity output of the cleaning robot is as follows:

[0140]

[0141] One part of the objective function is to make the optimized trajectory point as close as possible to the front-end search path to ensure that the environmental exploration gain is not lost. In addition, it is hoped that the change in the control amount is as small as possible to make the trajectory smooth and save energy. The other part is to make the trajectory conform to the kinematic constraints and not exceed the kinematic capabilities of the robot. Finally, the optimized trajectory is iteratively solved by a nonlinear solver.

[0142] Secondly: In the drawings of the embodiments disclosed in the present invention, only the structures related to the embodiments disclosed in the present invention are involved, and other structures can refer to the general design. In the absence of conflict, the same embodiment and different embodiments of the present invention can be combined with each other;

[0143] Finally: The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A local obstacle avoidance method for a cleaning robot based on a single steering wheel model, characterized in that: include: Step 1: Map matrix information generation: used to generate the environment map matrix to obtain obstacle information; Step 2: Collision cost calculation: used to calculate the collision cost of the position after determining whether it collides with an obstacle; Step 3: Cost calculation: used to calculate the orientation cost, collision cost, deviation from the reference path cost and distance cost when there is no collision node; Step 4: Trajectory optimization calculation: used to establish a kinematic model to calculate the robot's motion trajectory and optimize it.

2. The local obstacle avoidance method of a cleaning robot based on a single steering wheel model according to claim 1, characterized in that: The step 1 specifically includes the following steps: Step 1.1: Represent the robot model, obstacles, and path boundaries in the form of an image pixel value matrix. First, for obstacles of different heights, it is necessary to distinguish the height of the obstacles to determine whether the cleaning robot can cross them, including higher gray obstacles and lower white obstacles. Step 1.2: Use a ground segmentation method to segment the ground and obstacles; Step 1.3: Assign different obstacle costs to each pixel based on the distance of the pixel from the central obstacle.

3. The local obstacle avoidance method of a cleaning robot based on a single steering wheel model according to claim 1, characterized in that: The step 2 specifically includes the following steps: Step 2.1: For the cleaning robot motion model, use the offline construction method to calculate the contour error; Step 2.2: Determine the collision between the robot and the high obstacle; Step 2.3: After determining whether there is a collision with an obstacle, calculate the collision cost at that location.

4. The local obstacle avoidance method of a cleaning robot based on a single steering wheel model according to claim 1, characterized in that: The step 3 specifically includes the following steps: Step 3.1: Calculate the initial position of the robot; Step 3.2: Node expansion; Step 3.3: Expand node collision judgment; Step 3.4: Calculate node collision cost; Step 3.5: Calculate the node's deviation from the reference path cost; Step 3.6: Node cost calculation, used to calculate the current cost of the node.

5. The local obstacle avoidance method of a cleaning robot based on a single steering wheel model according to claim 1, characterized in that: The step 4 specifically includes the following steps: Step 4.1: Calculation of robot kinematics; Step 4.2: Express the output relationship according to the known kinematic model; Step 4.3: Ensure the optimization range of the trajectory is within the safe space through collision constraints; Step 4.4: When the terrain is relatively flat, the kinematic constraints degenerate into the following standard kinematic constraints; Step 4.5: Control output of the cleaning robot.

6. The local obstacle avoidance method of a cleaning robot based on a single steering wheel model according to claim 3, characterized in that: The error of the Ackerman model with length L in calculating the contour in this way is: Where θ is the angle interval. When θ is 0.25 degrees and L is 2 m, the error is 0.002 m, which is within the acceptable range. L is the constant of the Ackerman motion model. The specific method for judging the collision situation of the height obstacle is as follows: Among them, find max calculates the dot product of matrix A and matrix B to obtain a new matrix to determine whether the maximum value is greater than 255. If it is greater than 255, the robot has collided with the obstacle. If it is less than 255, the robot has no collision with the obstacle or the obstacle is inside the robot chassis.

7. The local obstacle avoidance method of a cleaning robot based on a single steering wheel model according to claim 4, characterized in that: The robot initial position model is as follows: Among them, t represents the sampling time, which represents the tth time in computer control, θ represents the forward sampling angle, x and y represent the horizontal and vertical axes of the plane projection coordinate system of the sampling point, and v represents the speed of the robot relative to its current forward direction; The calculation of the collision cost σ2 of each node is expressed as follows: Among them, R(x) is the calculation result of the above robot model unit pixel matrix and the environment pixel sub-matrix, that is, the statistics of the collision cost under the actual coverage of the robot model, D x It is the inverse of the distance between the extended node and any obstacle. When this value is greater than the set Th risk The calculation of node collision cost is effective only when , and the collision cost of any other node is 0 under this threshold, that is, it does not participate in the calculation of node cost; The calculation model of the node's deviation from the reference path cost is as follows: Among them, H(x) is the calculation result of the above robot model unit pixel matrix and the path map pixel sub-matrix, that is, the deviation cost of the robot model under the actual coverage path is counted, Dh x It is the inverse of the distance between the extended node and the central reference path. When this value is greater than the set Th head The calculation of the node deviation cost from the reference path is valid only when , and the deviation cost of any other node is 0 under this threshold, that is, it does not participate in the calculation of the node cost; The specific mathematical model of the current cost is as follows:

8. The local obstacle avoidance method of a cleaning robot based on a single steering wheel model according to claim 5, characterized in that: The kinematic relationship calculation model is as follows: Where h1 is the turning radius of a single steering wheel, v d is the driving speed of the robot, v, θ and δ are the speed, heading angle and steering angle of the steering wheel in the x-axis direction in the robot coordinate system, and l is the wheelbase of the robot; The output relationship can be expressed as: Among them, r d is the driving wheel radius, w d is the driving wheel angular velocity; The collision constraints are as follows: in, To pass through a path point below the chassis obstacle, X buffer The amount of space left in the chassis for obstacles; The kinematic constraints are as follows: Among them, α(t) is the slope of the plane where the trajectory point is located at time t, which is obtained according to the three-dimensional topographic map and the change of the z value of the trajectory point in the previous step. The z value is determined by the state quantity, which is the wheelbase of the front and rear wheels of the Ackerman model, and which is the direction angle projected to the XY plane; The control quantity output of the cleaning robot is as follows:

Images