Dynamic obstacle avoidance method and device, and mobile robot

Abstract

The application relates to a dynamic obstacle avoidance method and device and a mobile robot. The method comprises the following steps: acquiring environment point cloud information; identifying an obstacle scene according to the environment point cloud information to obtain obstacle scene information; performing prediction processing on dynamic obstacle scene information in the obstacle scene information to obtain first obstacle map information, updating the first obstacle map information to the environment point cloud information, and obtaining updated environment point cloud information; performing processing on the updated environment point cloud information based on a VFH obstacle avoidance algorithm to obtain current target direction information; marking an oscillation region of a current running path according to the current target direction information to obtain each oscillation region information; and performing oscillation region constraint on the current running path according to the each oscillation region information to obtain optimized path information. The method can optimize path planning of the mobile robot and improve the obstacle avoidance effect on dynamic obstacles.

Description

Technical Field

[0001] This application relates to the field of mobile robot technology, and in particular to dynamic obstacle avoidance methods, devices and mobile robots. Background Technology

[0002] Mobile robot path planning refers to generating a collision-free path from a starting point to a destination for a mobile robot. For indoor mobile robots, the path planning algorithm determines the level of intelligence in the robot's operation. Based on the degree of environmental awareness, mobile robot path planning algorithms are divided into global path planning algorithms with known environmental information and local obstacle avoidance algorithms for unknown or partially unknown environments.

[0003] Global path planning algorithms, due to known environmental information, only require searching the environmental map to obtain the globally planned path. However, when environmental information changes, the global map needs to be updated and the planning process executed again. Local path planning algorithms, on the other hand, only acquire local map information and can react quickly when environmental information changes. Local path planning algorithms only need to calculate the local environmental map, effectively reducing computational cost and response time compared to global path planning algorithms. However, current local path planning algorithms rarely consider the movement characteristics of dynamic obstacles; when indoor objects move rapidly, collisions may occur. In summary, existing mobile robots exhibit poor obstacle avoidance performance and low path planning optimization rates during obstacle avoidance and path planning processes. Summary of the Invention

[0004] Therefore, it is necessary to provide a dynamic obstacle avoidance method, device, and mobile robot to address the problems of poor obstacle avoidance performance and low path planning optimization rate of existing mobile robots in the obstacle avoidance and path planning process.

[0005] To achieve the above objectives, embodiments of the present invention provide a dynamic obstacle avoidance method applied to a mobile robot, comprising the following steps:

[0006] Acquire environmental point cloud information;

[0007] Based on environmental point cloud information, obstacle scene is identified to obtain obstacle scene information;

[0008] The dynamic obstacle scene information in the obstacle scene information is predicted and processed to obtain the first obstacle map information, and the first obstacle map information is updated to the environmental point cloud information to obtain the updated environmental point cloud information.

[0009] Based on the VFH obstacle avoidance algorithm, the updated environmental point cloud information is processed to obtain the current target direction information; based on the current target direction information, the oscillation region is marked on the current running path to obtain the information of each oscillation region; based on the information of each oscillation region, the oscillation region constraint is applied to the current running path to obtain the optimized path information.

[0010] In one embodiment, the step of identifying the obstacle scene further includes:

[0011] When the obstacle scene information includes special obstacle scene information, a second obstacle map information corresponding to the special obstacle scene information is generated, and the second obstacle map information is updated to the updated environmental point cloud information.

[0012] In one embodiment, the step of identifying the obstacle scene further includes:

[0013] When the obstacle scene information includes static obstacle scene information, a third obstacle map information corresponding to the static obstacle scene information is generated; and the third obstacle map information is updated to the updated environmental point cloud information.

[0014] In one embodiment, the step of acquiring environmental point cloud information includes:

[0015] Camera images are captured to obtain the direction of movement of the mobile robot;

[0016] The camera-acquired images are processed for camera calibration to obtain camera intrinsic parameters and distortion parameters;

[0017] Based on the camera's intrinsic parameters and distortion parameters, the images acquired by the camera are processed to obtain environmental point cloud information.

[0018] In one embodiment, the step of performing camera calibration processing on images acquired by the camera includes:

[0019] Based on Zhang's calibration algorithm, the intrinsic parameters of the camera are calibrated on the images acquired by the camera to obtain the camera intrinsic parameters.

[0020] Distortion correction is performed on the images acquired by the camera to obtain distortion parameters.

[0021] In one embodiment, the step of processing the updated environmental point cloud information includes:

[0022] Based on the VFH obstacle avoidance algorithm, the updated environmental point cloud information is processed to obtain a binary obstacle density histogram.

[0023] The binary obstacle density histogram is processed to obtain candidate valley information;

[0024] The candidate trough information is weighted to obtain the weighted candidate trough information.

[0025] Based on the weighted candidate trough information and the preset final target direction information, the current target direction information is obtained.

[0026] In one embodiment, the step of marking the oscillation region of the current running path includes:

[0027] Retrieve pre-stored trajectory information;

[0028] The current target direction information is compared with the pre-stored trajectory information, and the weighted Euclidean distance difference between the current running path point and the next adjacent running path point is calculated based on the comparison result to obtain the first value.

[0029] When the first value falls within the preset threshold range, the next running path point is marked with an oscillation mark to obtain the oscillation area information.

[0030] In one embodiment, the step of predicting dynamic obstacle scene information in obstacle scene information includes:

[0031] Initialize the KF parameters of each dynamic obstacle point in the environmental point cloud information;

[0032] The dynamic obstacle points in two adjacent frames are translated and registered to obtain the movement information of the corresponding dynamic obstacle points;

[0033] The matched dynamic obstacle points are fed into the KF prior estimation and posterior update model for KF prediction. Unmatched dynamic obstacle points are considered to have stopped moving, and the KF parameters of the corresponding unmatched dynamic obstacle points are initialized.

[0034] Based on the results of KF prediction processing, the first obstacle map information is obtained.

[0035] On the other hand, embodiments of the present invention also provide a dynamic obstacle avoidance device applied to a mobile robot, comprising:

[0036] Point cloud information acquisition unit, used to acquire environmental point cloud information;

[0037] The obstacle recognition unit is used to identify obstacle scenes based on environmental point cloud information to obtain obstacle scene information;

[0038] The point cloud information update unit is used to predict and process the dynamic obstacle scene information in the obstacle scene information to obtain the first obstacle map information, and update the first obstacle map information to the environmental point cloud information to obtain the updated environmental point cloud information.

[0039] The obstacle avoidance optimization unit is used to process the updated environmental point cloud information based on the VFH obstacle avoidance algorithm to obtain the current target direction information; based on the current target direction information, it marks the oscillation region of the current running path to obtain the information of each oscillation region; based on the information of each oscillation region, it constrains the current running path to obtain the optimized path information.

[0040] On the other hand, embodiments of the present invention also provide a mobile robot, including a mobile robot body and a controller disposed on the mobile robot; the controller is used to execute the dynamic obstacle avoidance method described above.

[0041] The above-mentioned technical solutions have the following advantages and beneficial effects:

[0042] In each embodiment of the above dynamic obstacle avoidance method, environmental point cloud information is acquired; obstacle scenes are identified based on the environmental point cloud information to obtain obstacle scene information; dynamic obstacle scene information in the obstacle scene information is predicted to obtain first obstacle map information, and the first obstacle map information is updated to the environmental point cloud information to obtain updated environmental point cloud information; the updated environmental point cloud information is processed based on the vector field histogram obstacle avoidance algorithm to obtain current target direction information; oscillation regions are marked on the current running path based on the current target direction information to obtain oscillation region information; oscillation region constraints are applied to the current running path based on the oscillation region information to obtain optimized path information. This application classifies obstacles by type and identifies them using the acquired environmental point cloud information. When a dynamic obstacle is identified, the dynamic obstacle scene information is predicted and processed, and the generated first obstacle map information is updated to the environmental point cloud information. Combined with the VFH obstacle avoidance algorithm, obstacle avoidance processing is performed on dynamic obstacles, solving the obstacle avoidance problem under multiple dynamic obstacles. In addition, by marking and constraining the oscillation area of ​​the current running path, the mobile robot is kept away from the oscillation area, which can effectively prevent the mobile robot from getting stuck in the dead zone, thereby optimizing the path planning of the mobile robot and improving the obstacle avoidance effect of dynamic obstacles. Attached Figure Description

[0043] Figure 1 This is a schematic diagram illustrating the application environment of the dynamic obstacle avoidance method in one embodiment.

[0044] Figure 2 This is a schematic diagram of the first process of a dynamic obstacle avoidance method in one embodiment;

[0045] Figure 3 This is a flowchart illustrating the steps for obtaining environmental point cloud information in one embodiment;

[0046] Figure 4This is a flowchart illustrating the steps for processing updated environmental point cloud information in one embodiment.

[0047] Figure 5 This is a schematic diagram illustrating the obstacle avoidance performance of an existing VFH (left figure) and an improved VFH (right figure) in one embodiment;

[0048] Figure 6 This is a flowchart illustrating the step of marking the oscillation region in the current running path in one embodiment;

[0049] Figure 7 This is a schematic diagram of an oscillation scenario in one embodiment;

[0050] Figure 8 This is a schematic diagram of a process for predicting dynamic obstacle scene information based on the Kalman filter algorithm in one embodiment.

[0051] Figure 9 This is a schematic diagram illustrating the effect of optimized path information in one embodiment;

[0052] Figure 10 This is a block diagram of a dynamic obstacle avoidance device in one embodiment. Detailed Implementation

[0053] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application are clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present application, and not all of them. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present application.

[0054] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of this application described herein. Furthermore, the terms "comprising," "having," and any variations thereof are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may also include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0055] In addition, the term "multiple" should mean two or more.

[0056] In existing technologies, there are currently two main approaches to handling dynamic obstacle scenarios. One approach is to improve algorithm efficiency by adding constraints, improving algorithm structure, and combining optimization algorithms, thereby enhancing the replanning capability of path planning algorithms in dynamic scenarios. However, when obstacles change rapidly, replanning still suffers from problems such as excessively long convergence times and inability to react promptly to moving obstacles, leading to collisions. The second approach is to predict the trajectory of moving obstacles and use the predicted grid map for obstacle avoidance and path planning, but this approach suffers from the problem of failing to effectively match obstacles when multiple obstacles are moving. In summary, existing mobile robots exhibit poor obstacle avoidance performance and low path planning optimization rates in dynamic obstacle scenarios. To address these issues, this application provides the following dynamic obstacle avoidance method.

[0057] The dynamic obstacle avoidance method provided in this application can be applied to, for example... Figure 1 The application environment shown is illustrated. The mobile robot includes a main body 104 and a controller 102 connected to it. The mobile robot can be, but is not limited to, a sweeping robot, a mopping robot, or a service robot, and possesses self-propelled capabilities. The mobile robot also includes a camera, which can be used to acquire environmental images with corresponding environmental point cloud information. The controller can then process the environmental images acquired by the camera to obtain the corresponding environmental point cloud information.

[0058] like Figure 2 As shown, in one embodiment, it is applied to Figure 1 Taking the controller in the example, the method includes:

[0059] Step S210: Obtain environmental point cloud information.

[0060] Among them, environmental point cloud information refers to the point cloud information of the environment image in which the mobile robot is located. Point cloud information refers to a set of vectors in a three-dimensional coordinate system. These vectors can be represented in three-dimensional coordinate form.

[0061] For example, a mobile robot is equipped with a camera, which can capture environmental images in the direction the robot is moving and transmit the captured environmental images to a controller. The controller can then process the received environmental images to obtain environmental point cloud information.

[0062] Step S220: Based on the environmental point cloud information, identify the obstacle scene to obtain obstacle scene information.

[0063] The controller can identify and process obstacle scenes in the environmental point cloud information based on a preset obstacle database to obtain obstacle scene information. This obstacle scene information includes general obstacle scene information and special obstacle scene information. General obstacle scene information is further divided into static obstacle scene information and dynamic obstacle scene information based on whether the objects are moving. Static obstacle scene information includes stationary obstacles, such as tables and chairs in an indoor home scene. Dynamic obstacle scene information includes movable obstacles, such as pets like cats and dogs. Special obstacle scene information includes, but is not limited to, obstacles such as slopes, ditches, and shoelaces.

[0064] For example, based on the application scenario of the mobile robot, obstacles can be divided into ordinary obstacles that can be successfully detected by normal radar and special obstacles that cannot be detected by radar, and then obstacle scenarios can be divided into ordinary obstacle scenarios and special obstacle scenarios.

[0065] Step S230: Perform predictive processing on the dynamic obstacle scene information in the obstacle scene information to obtain the first obstacle map information, and update the first obstacle map information to the environmental point cloud information to obtain the updated environmental point cloud information.

[0066] The first obstacle map information can be the corresponding grid map information.

[0067] The controller classifies the obstacle scene based on obstacles, determines the type of obstacle scene information, and when the obstacle scene information includes dynamic obstacle scene information, it performs predictive processing on the dynamic obstacle scene information to obtain the first obstacle map information. The controller can then update the environmental point cloud information with the processed first obstacle map information to obtain the updated environmental point cloud information.

[0068] For example, the controller acquires obstacle scene information from at least two frames, determines whether the obstacle coordinates in the obstacle scene information have changed, and identifies obstacles whose coordinates have changed as dynamic obstacles.

[0069] It should be noted that the Kalman Filter (KF) algorithm used in this embodiment can be used to predict dynamic obstacles. In other embodiments, the Extended Kalman Filter algorithm, Unscented Kalman Filter algorithm, or Particle Filter algorithm can also be used to predict dynamic obstacles.

[0070] Step S240: Based on the Vector Field Histogram (VFH) obstacle avoidance algorithm, the updated environmental point cloud information is processed to obtain the current target direction information; based on the current target direction information, the oscillation region of the current running path is marked to obtain the information of each oscillation region; based on the information of each oscillation region, the oscillation region constraint of the current running path is applied to obtain the optimized path information.

[0071] The controller employs a vector field histogram obstacle avoidance algorithm to process the updated environmental point cloud information, calculate the corresponding target direction, and thus obtain the current target direction information. Based on the current target direction information, the controller detects whether oscillations occur in the current running path and marks the oscillation regions on the current running path according to the detection results, thereby obtaining the corresponding oscillation region information. The controller adds constraints to the corresponding oscillation regions to avoid or move away from them, thus obtaining optimized path information. This effectively prevents the mobile robot from getting stuck in dead zones, achieving dynamic obstacle avoidance for the mobile robot.

[0072] In the above embodiments, based on the classification of obstacle types, obstacle identification is performed on the acquired environmental point cloud information. When a dynamic obstacle is identified, the dynamic obstacle scene information is predicted, and the generated first obstacle map information is updated to the environmental point cloud information. Combined with the vector field histogram obstacle avoidance algorithm, obstacle avoidance processing is performed on dynamic obstacles, solving the obstacle avoidance problem under multiple dynamic obstacles. In addition, by marking and constraining the oscillation region of the current running path, the mobile robot is kept away from the oscillation region, which can effectively prevent the mobile robot from getting stuck in the dead zone, thereby optimizing the path planning of the mobile robot and improving the obstacle avoidance effect of dynamic obstacles.

[0073] In one embodiment, the step of identifying the obstacle scene further includes:

[0074] When the obstacle scene information includes special obstacle scene information, a second obstacle map information corresponding to the special obstacle scene information is generated, and the second obstacle map information is updated to the updated environmental point cloud information.

[0075] The second obstacle map information can be the corresponding grid map information.

[0076] The controller classifies the obstacle scene based on obstacles and determines the type of obstacle scene information. When the obstacle scene information includes special obstacle scene information, it generates a second obstacle map information corresponding to the special obstacle scene information. The controller can then update the environmental point cloud information with the processed third obstacle map information to obtain the updated environmental point cloud information.

[0077] It should be noted that in some application scenarios, special obstacles are not limited to ditches, slopes, and shoelaces. In other application scenarios, objects that cannot be detected by LiDAR and pose obstacles to mobile robots can also be considered special obstacles.

[0078] In one embodiment, the step of identifying the obstacle scene further includes:

[0079] When the obstacle scene information includes static obstacle scene information, a third obstacle map information corresponding to the static obstacle scene information is generated, and the third obstacle map information is updated to the updated environmental point cloud information.

[0080] The third obstacle map information can be the corresponding grid map information.

[0081] The controller divides the obstacle scene based on obstacles and determines the type of obstacle scene information. When the obstacle scene information includes static obstacle scene information, it directly generates the third obstacle map information corresponding to the static obstacle scene information and updates the environmental point cloud information with the generated third obstacle map information to obtain the updated environmental point cloud information.

[0082] For example, the controller can acquire at least two frames of obstacle scene information, determine whether the obstacle coordinates in the obstacle scene information have changed, and identify obstacles whose coordinates have not changed as static obstacles.

[0083] In one embodiment, such as Figure 3 As shown, the steps for obtaining environmental point cloud information include:

[0084] Step S310: Acquire camera images of the mobile robot's forward direction.

[0085] In this context, camera image acquisition refers to the capture of environmental images along the direction the mobile robot is moving. The camera is mounted on the mobile robot, and during its operation, it scans the current environment to obtain the captured images.

[0086] It should be noted that the camera can be a 3D TOF camera, and the images captured by the 3D TOF camera correspond to TOF images. In some embodiments, the camera can also be replaced by other depth cameras, such as binocular cameras, structured light cameras, etc.

[0087] Step S320: Perform camera calibration processing on the images acquired by the camera to obtain camera intrinsic parameters and distortion parameters.

[0088] Camera intrinsic parameters refer to the parameters obtained by transforming the camera coordinate system to the image coordinate system, i.e., from 3D points to 2D points. Distortion parameters include radial distortion parameters and tangential distortion parameters. It should be noted that the camera parameters solved during the camera calibration process are the geometric model parameters of camera imaging that must be established to determine the relationship between the 3D geometric position of a point on the surface of a spatial object and its corresponding point in the image.

[0089] For example, the steps of performing camera calibration processing on images acquired by the camera include:

[0090] Based on Zhang's (Zhang Zhengyou) calibration algorithm, the intrinsic parameters of the camera-acquired images are calibrated to obtain the camera's intrinsic parameters; distortion correction is performed on the camera-acquired images to obtain distortion parameters.

[0091] Among them, Zhang's calibration algorithm is a camera calibration algorithm based on a single-plane chessboard grid.

[0092] Step S330: Process the camera-acquired images according to the camera intrinsic parameters and distortion parameters to obtain environmental point cloud information.

[0093] The controller calibrates and corrects the camera-acquired images based on the processed camera intrinsic parameters and distortion parameters, obtaining processed camera-acquired images. These processed images are then converted into environmental point cloud information. This allows the camera to capture areas that are inaccessible to ordinary radar, compensating for radar blind spots. In one embodiment, such as... Figure 4 As shown, the steps for processing the updated environmental point cloud information include:

[0094] Step S410: Based on the vector field histogram obstacle avoidance algorithm, the updated environmental point cloud information is processed to obtain a binary obstacle density histogram.

[0095] The controller uses a vector field histogram obstacle avoidance algorithm to process the updated environmental point cloud information, calculate the corresponding obstacle density, and then obtain a binary obstacle density histogram.

[0096] Step S420: Process the binary obstacle density histogram to obtain candidate valley information.

[0097] The controller calculates the candidate valley direction based on the processed binary obstacle density histogram, and then obtains the candidate valley information.

[0098] It should be noted that candidate valley information can be used to represent obstacle information.

[0099] Step S430: The candidate valley information is weighted to obtain weighted candidate valley information.

[0100] The controller can perform weighted processing on the candidate valley information according to preset weights, and then obtain the weighted candidate valley information.

[0101] Step S440: Obtain the current target direction information based on the weighted candidate valley information and the preset final target direction information.

[0102] The controller calculates the current target direction based on the preset final target direction information and the processed weighted candidate valley information, and thus obtains the current target direction information.

[0103] For example, the static obstacle avoidance performance of existing VFH (or original VFH) and improved VFH algorithms is as follows: Figure 5 As shown, circles represent the mobile robot, squares represent the active window for obstacle avoidance algorithm calculations, dots represent obstacles, and stars represent target points. The position of the mobile robot at each step is saved to form the motion trajectory shown in the figure. When the target point is behind an obstacle, as... Figure 5 As shown in the left figure, the existing VFH algorithm causes the mobile robot to oscillate during its back-and-forth movement. After adopting the oscillation detection and suppression method step S240 of the above embodiment, Figure 5 In the right image, after the mobile robot moves back and forth once, the system detects that the mobile robot has fallen into oscillation. After adding oscillation suppression conditions, it moves away from the oscillation area.

[0104] In one embodiment, such as Figure 6 and 7 As shown, the steps for marking oscillation regions in the current running path include:

[0105] Step S610: Obtain the pre-stored trajectory information.

[0106] Step S620: Compare the current target direction information with the pre-stored trajectory information, and calculate the weighted Euclidean distance difference between the current running path point and the next adjacent running path point based on the comparison result to obtain the first value.

[0107] Step S630: When the first value falls within the preset threshold range, the next running path point is marked with an oscillation mark to obtain oscillation area information.

[0108] Specifically, by comparing the current target direction information with the pre-stored trajectory information and calculating the weighted Euclidean distance difference between adjacent points, the system determines whether the current running path oscillates and the type of oscillation based on the calculated first value. Then, based on different oscillation types, the system constrains and restricts the travel direction of the mobile robot to stay away from the oscillation area, thereby effectively preventing the mobile robot from getting stuck in the dead zone.

[0109] In the above embodiments, by improving the VFH algorithm, the problem of oscillations that easily occur near the target point close to the obstacle and in the area of ​​symmetrical obstacles is solved, the path stability of the VFH algorithm is improved, and the Kalman filter algorithm is combined to avoid multiple dynamic obstacles in complex environments, thereby optimizing the path planning of the mobile robot and improving the obstacle avoidance effect of dynamic obstacles.

[0110] In one embodiment, such as Figure 8 As shown, the steps for predicting dynamic obstacle scene information in obstacle scene information include:

[0111] Step S810: Initialize the KF (Kalman Filter) parameters of each dynamic obstacle point in the environmental point cloud information.

[0112] Step S820: Perform translation registration on the dynamic obstacle points in two adjacent frames to obtain the movement information of the corresponding dynamic obstacle points.

[0113] Step S830: The matched dynamic obstacle points are connected to the KF algorithm prior estimation and posterior update model for KF prediction processing. Unmatched dynamic obstacle points are considered to have stopped moving, and the KF parameters of the corresponding unmatched dynamic obstacle points are initialized.

[0114] Step S840: Obtain the first obstacle map information based on the KF algorithm prediction processing result.

[0115] Specifically, the controller initializes the KF parameters of each dynamic obstacle point in the environment map, performs translational registration on the dynamic obstacle points in two adjacent frames, and obtains the movement information of the corresponding dynamic obstacle points; it then inputs the matched dynamic obstacle points into the KF algorithm prior estimation and posterior update model for KF prediction processing, considers unmatched dynamic obstacle points as stopped moving, initializes the corresponding KF parameters, and outputs the next dynamic obstacle point map information predicted by the KF algorithm, thereby obtaining the first obstacle map information.

[0116] For example, the VFH algorithm's dynamic obstacle prediction performance based on the KF algorithm for multi-point obstacle prediction is as follows: Figure 9As shown, the two rectangles represent two activity windows at adjacent time points, the dense points are the moving obstacle points detected at the current time, the triangular points are the obstacle points predicted by the KF algorithm, the dashed arrows above the obstacles represent the current movement direction of the dynamic obstacles, and the cross-shaped points are the center points of the mobile robot. When passing through the illustrated area, since the obstacles are in a moving state, the VFH planning direction may collide with the obstacles in the next time point. The KF algorithm is used to predict the obstacle's trajectory at multiple points, and the predicted trajectory is added to the local map for the next VFH planning step to avoid dynamic obstacles. Obstacle map information generated for different obstacle scenarios is updated to the environmental point cloud information. Translation registration is performed using adjacent two-frame maps, and the KF algorithm is used to predict the trajectory of multiple dynamic obstacles. The VFH algorithm is then used for obstacle avoidance, solving the obstacle avoidance problem under multiple dynamic obstacles. By detecting whether oscillations occur during the VFH planning process, constraints are added to the marked oscillation areas. Moving away from the oscillation areas effectively prevents the robot from getting stuck in dead zones, thereby optimizing the mobile robot's path planning and improving the obstacle avoidance effect for dynamic obstacles.

[0117] In a specific example, based on a 3D ToF camera, this method is applied to... Figure 1 Taking the controller in the example, the specific steps are as follows:

[0118] Step 1: Calibrate and correct the ToF camera, and acquire ToF images to generate point cloud information.

[0119] The specific implementation process of step 1 is divided into steps 1.1, 1.2 and 1.3.

[0120] Step 1.1: Take 10 images of the printed checkerboard pattern from different angles using a ToF camera, and perform Zhang Zhengyou calibration using MATLAB's Camera Calibrator toolbox to obtain the ToF camera's intrinsic parameter matrix as follows:

[0121]

[0122] in f is the camera focal length, d x ,d y These represent the pixel scales in the x and y directions of the pixel coordinate system, respectively. x and c y These are the coordinates of the origin of the pixel image coordinate system in the pixel coordinate system.

[0123] The actual internal parameters obtained from calibration (in mm) are as follows:

[0124]

[0125] The distortion factors obtained from calibration are: k1 = -0.05526, k2 = 0.2604, k3 = -0.44089.

[0126] Step 1.2: Use a ToF camera to acquire images and correct image distortion. The correction method is as follows:

[0127] x distorted =x+[2p1xy+p2(r) 2 +2x 2 )]

[0128] y distorted =y+[2p2xy+p1(r) 2 +2y 2 )]

[0129]

[0130] Step 1.3: Convert the corrected image into a point cloud using intrinsic parameters. The conversion relationship is as follows:

[0131]

[0132] Where (u,v) is the pixel coordinate system, (x) is the pixel coordinate system. c ,y c ,z c () is the world coordinate system.

[0133] Step 2: Based on the environmental point cloud information, divide the obstacle scene into ordinary obstacle scenes and special scenes (such as slopes, ditches, shoelaces, etc.), and update the obstacle map in a corresponding way according to different scenes.

[0134] The specific implementation process of step 2 is divided into steps 2.1, 2.2, 2.3 and 2.4.

[0135] Step 2.1: Input the point cloud information obtained in Step 1, traverse the point cloud, filter out point cloud information larger than 20cm, and determine if there are special obstacles such as slopes, shoelaces, and ditches. For ditches, only the part above the ground needs to be filtered out. If there are ditches below the ground, take the average value of the ditches to obtain their approximate center coordinates. Calculate the minimum distance r between the approximate center coordinates and each ditches. When 2r > d (d is the diameter of the wheel), it is determined that the wheel cannot pass. For slopes, first filter out the part below the ground, detect the location where the obstacle height changes abruptly. If the slope at the change location is less than 30°, mark the corresponding area as a passable area; otherwise, mark it as an impassable area.

[0136] Step 2.2: Filter out the special obstacles detected in Step 2.1, find the intersection of the grid maps at adjacent time points, and perform difference processing on the intersection of the grid maps at adjacent time points in their respective regions. If there is a region with a non-zero value, it is judged as a dynamic obstacle; otherwise, it is a static obstacle.

[0137] Step 2.3: For static obstacles in Step 2.2, output a raster map. For dynamic obstacles in Step 2.2, perform the following KF prediction process:

[0138] Acquire the environmental grid map and filter out static obstacle areas in the grid map. Initialize the KF parameters of the dynamic obstacle points as follows:

[0139]

[0140] in X1 represents the prior calculated value of the position vector at the initial time, and X2 represents the posterior updated value of the position vector at the initial time. Let P0 be the initial time-prediction error covariance matrix, and P0 be the initial time-posterior error covariance matrix.

[0141] The motion points in two adjacent map frames are translated and registered. First, the intersection of the two adjacent map frames is calculated. The intersection is then subtracted, with positions greater than 0 marked as "+" and positions less than 0 marked as "-". The nearest "+" point to each "-" marked point is used as the matching point before and after the movement, thus obtaining the movement status of the corresponding motion point.

[0142] The matching points before and after the dynamic obstacle point moves are substituted into the KF algorithm for updating and prediction, as shown in the following formula:

[0143] The prior estimate is:

[0144] The prior prediction error covariance is:

[0145] The Kalman gain is:

[0146] The posterior update is as follows:

[0147] The posterior error covariance is:

[0148] Where X is the position vector, X = [x, y, v] x ,v y ], k is time, u k Let y be the process noise, A and B be the prior update parameter matrices, Q be the process noise covariance, R be the measurement noise covariance, and C be the transformation matrix between the measured value y and the position X.

[0149] Predict the next trajectory of the moving obstacle using prior estimation and output a raster map of the predicted trajectory.

[0150] Step 2.4: Overlay and output the grid map of special obstacles, the static obstacle grid map, the dynamic obstacle grid map, and the predicted trajectory grid map.

[0151] Step 3: Use the VFH algorithm to calculate the target direction, and detect whether oscillations occur based on the running path. Avoid oscillation regions by adding constraints.

[0152] The specific implementation process of step 3 is divided into steps 3.1, 3.2, 3.3 and 3.4.

[0153] Step 3.1: First, divide the raster map output in Step 2 into partitions of 10° each. Calculate the obstacle density within each partition n. The calculation steps are as follows:

[0154] For each obstacle point (x) i ,y i The robot's center point is (x0, y0), and its corresponding angle is:

[0155]

[0156] The corresponding obstacle density is:

[0157]

[0158] Where c i,j For obstacle point (x) i ,y i The probability values ​​of the occurrence of obstacles, where a and b are constants, and d i,j For obstacle point (x) i ,y i The distance between (x0, y0) and (x0, y0).

[0159] The obstacle density of partition n is:

[0160]

[0161] The obstacle density is calculated for each partition to obtain an obstacle density histogram, and then a threshold τ is set. h and τ l The formula for converting a rectangular density histogram to a binary density histogram is as follows:

[0162]

[0163] Where k represents time.

[0164] Step 3.2: Using a binary obstacle density histogram, regions with an obstacle density of 0 are selected as candidate regions, regions with an obstacle density greater than 0 are selected as impassable regions, and regions with a continuous density of 0 are selected as candidate valleys.

[0165] Step 3.3: Weight the candidate troughs and calculate the current target direction based on the final target direction.

[0166] Step 3.4: Compare the target direction with the stored trajectory, and calculate the weighted Euclidean distance difference between adjacent points. The calculation formula is as follows:

[0167] Point (x) i ,y i Distance from target point (x) goal ,y goal ) and initial starting point (x) ini ,y ini The sum of the Euclidean distances of ) is:

[0168]

[0169] Where α and β are the corresponding weighting factors.

[0170] Weighted Euclidean distance difference is divided into:

[0171]

[0172] Oscillation types and their difference curves are as follows Figure 7 As shown, the occurrence of oscillation and its type can be determined based on the corresponding difference curve. The specific process is as follows:

[0173] Record the planned target point output by the VFH algorithm each time; calculate and record the weighted Euclidean distance from the previous step each time the planned target point is reached; plot the weighted Euclidean distance with time k on the horizontal axis and the weighted Euclidean distance obtained at time k on the vertical axis based on the recorded weighted Euclidean distance; determine the number of times n that the zero point is crossed in 5 adjacent time points; when n≥4, it is linear oscillation; when 2≤n<4, it is triangular oscillation; when n<2, no oscillation occurs.

[0174] Step 3.5: Restrict the passable direction according to different oscillation types, and keep away from the oscillation area.

[0175] Among them, for Figure 7 The first type of oscillation can be limited by restricting the robot's turning angle, ensuring that the difference between the direction of the planned target point and the current running direction is less than 3 zones; for Figure 7 For the second type of oscillation, or even more complex types, it is necessary to obtain the region trapped in the oscillation through the distance difference curve, increase the obstacle density in the corresponding region, and move away from the oscillation region.

[0176] The static obstacle avoidance performance of traditional VFH and improved VFH algorithms is as follows: Figure 5 As shown, circles represent the mobile robot, squares represent the active window for obstacle avoidance algorithm calculations, dots represent obstacles, and stars represent target points. The position of the mobile robot at each step is saved to form the motion trajectory shown in the figure. When the target point is behind an obstacle, as... Figure 5 As shown in the left figure, the existing VFH algorithm causes the mobile robot to oscillate during its back-and-forth movement. After adopting the oscillation detection method in step 3.4 and the oscillation suppression method in step 3.5, Figure 5 In the right figure, after the mobile robot moves back and forth once, the system detects that the mobile robot has fallen into oscillation. After adding oscillation suppression conditions, it moves away from the oscillation area.

[0177] The VFH algorithm, which performs multi-point obstacle prediction based on the KF algorithm, demonstrates the following dynamic obstacle prediction performance: Figure 9 As shown, the two rectangles represent two activity windows at adjacent time points, the dense points are the moving obstacle points detected at the current time, the triangular points are the obstacle points predicted by KF, the dashed arrows above the obstacles represent the current movement direction of the dynamic obstacles, and the cross-shaped points are the center points of the mobile robot. When passing through the area shown, because the obstacles are in a moving state, the VFH planned direction may collide with the obstacles in the next time point. Using the KF algorithm to predict the obstacle's trajectory at multiple points and adding the predicted trajectory to the local map allows for further VFH planning to avoid dynamic obstacles.

[0178] It should be understood that, although Figures 2-4 The steps in flowcharts 6 and 8 are shown sequentially as indicated by the arrows; however, these steps are not necessarily executed in the exact order indicated by the arrows. Unless otherwise specified herein, there is no strict order requirement for the execution of these steps, and they can be executed in other orders. Furthermore, Figures 2-4 At least some of the steps in 6 and 8 may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but may be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but may be executed in turn or alternately with at least some of the sub-steps or stages of other steps or other steps.

[0179] In one embodiment, such as Figure 10 As shown, a dynamic obstacle avoidance device for mobile robots is also provided, comprising:

[0180] The point cloud information acquisition unit 110 is used to acquire environmental point cloud information.

[0181] The obstacle recognition unit 120 is used to recognize the obstacle scene based on the environmental point cloud information and obtain obstacle scene information.

[0182] The point cloud information update unit 130 is used to predict and process the dynamic obstacle scene information in the obstacle scene information to obtain the first obstacle map information, and update the first obstacle map information to the environmental point cloud information to obtain the updated environmental point cloud information.

[0183] The obstacle avoidance optimization unit 140 is used to process the updated environmental point cloud information based on the vector field histogram obstacle avoidance algorithm to obtain the current target direction information; mark the oscillation region of the current running path according to the current target direction information to obtain the information of each oscillation region; and constrain the oscillation region of the current running path according to the information of each oscillation region to obtain the optimized path information.

[0184] For specific limitations regarding the dynamic obstacle avoidance device, please refer to the limitations of the dynamic obstacle avoidance method for mobile robots mentioned above, which will not be repeated here. Each module in the aforementioned dynamic obstacle avoidance device can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in hardware or independent of the controller in the mobile robot, or stored in software in the mobile robot's memory, so that the controller can call and execute the corresponding operations of each module.

[0185] In one embodiment, a mobile robot is also provided, including a mobile robot body and a controller disposed on the mobile robot; the controller is used to execute the dynamic obstacle avoidance method described above.

[0186] The mobile robot can be a cleaning robot with functions such as sweeping and mopping. The main body of the mobile robot is equipped with a camera, which is used to capture images of the robot's forward direction, thus obtaining the images captured by the camera.

[0187] The controller is used to execute the following steps of the dynamic obstacle avoidance method:

[0188] The process involves: acquiring environmental point cloud information; identifying obstacle scenes based on the environmental point cloud information to obtain obstacle scene information; predicting and processing dynamic obstacle scene information within the obstacle scene information to obtain first obstacle map information, and updating the environmental point cloud information with the first obstacle map information to obtain updated environmental point cloud information; processing the updated environmental point cloud information based on a vector field histogram obstacle avoidance algorithm to obtain current target direction information; marking oscillation regions on the current running path based on the current target direction information to obtain information on each oscillation region; and applying oscillation region constraints to the current running path based on the information on each oscillation region to obtain optimized path information.

[0189] In the above embodiments, based on the classification of obstacle types, obstacle identification is performed on the acquired environmental point cloud information. When a dynamic obstacle is identified, the dynamic obstacle scene information is predicted, and the generated first obstacle map information is updated to the environmental point cloud information. Combined with the vector field histogram obstacle avoidance algorithm, obstacle avoidance processing is performed on dynamic obstacles, solving the obstacle avoidance problem under multiple dynamic obstacles. In addition, by marking and constraining the oscillation region of the current running path, the mobile robot is kept away from the oscillation region, which can effectively prevent the mobile robot from getting stuck in the dead zone, thereby optimizing the path planning of the mobile robot and improving the obstacle avoidance effect of dynamic obstacles.

[0190] In one embodiment, a computer-readable storage medium is also provided, on which a computer program is stored, which, when executed by a processor, implements the steps of any of the above-described dynamic obstacle avoidance methods.

[0191] In one example, when a computer program is executed by a processor, it performs the following steps:

[0192] The process involves: acquiring environmental point cloud information; identifying obstacle scenes based on the environmental point cloud information to obtain obstacle scene information; predicting dynamic obstacle scene information when it includes dynamic obstacle scene information to obtain a first obstacle map information, and updating the environmental point cloud information with the first obstacle map information to obtain updated environmental point cloud information; processing the updated environmental point cloud information using a vector field histogram obstacle avoidance algorithm to obtain current target direction information; marking oscillation regions on the current running path based on the current target direction information to obtain oscillation region information; and constraining the current running path based on the oscillation region information to obtain optimized path information.

[0193] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by computer program instructions and related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0194] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0195] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A dynamic obstacle avoidance method applied to a mobile robot, characterized in that, Includes the following steps: Acquire environmental point cloud information; Based on the environmental point cloud information, obstacle scene is identified to obtain obstacle scene information; The dynamic obstacle scene information in the obstacle scene information is predicted and processed to obtain the first obstacle map information, and the first obstacle map information is updated to the environmental point cloud information to obtain the updated environmental point cloud information. Based on the VFH obstacle avoidance algorithm, the updated environmental point cloud information is processed to obtain the current target direction information; Based on the current target direction information, the oscillation region is marked on the current running path to obtain the oscillation region information; Based on the oscillation region information, the current running path is constrained to obtain optimized path information; The step of processing the updated environmental point cloud information includes: processing the updated environmental point cloud information based on the VFH obstacle avoidance algorithm to obtain a binary obstacle density histogram; processing the binary obstacle density histogram to obtain candidate valley information; weighting the candidate valley information to obtain weighted candidate valley information; and obtaining the current target direction information based on the weighted candidate valley information and preset final target direction information. The step of marking the oscillation region of the current running path includes: acquiring pre-stored trajectory information; comparing the current target direction information with the pre-stored trajectory information, and calculating the weighted Euclidean distance difference between the current running path point and the adjacent next running path point based on the comparison result to obtain a first value; when the first value falls within a preset threshold range, marking the next running path point with oscillation to obtain the oscillation region information.

2. The dynamic obstacle avoidance method according to claim 1, characterized in that, Following the step of identifying the obstacle scene, the method further includes: When the obstacle scene information includes special obstacle scene information, a second obstacle map information corresponding to the special obstacle scene information is generated, and the second obstacle map information is updated to the updated environmental point cloud information.

3. The dynamic obstacle avoidance method according to claim 1, characterized in that, Following the step of identifying the obstacle scene, the method further includes: When the obstacle scene information includes static obstacle scene information, a third obstacle map information corresponding to the static obstacle scene information is generated, and the third obstacle map information is updated to the updated environmental point cloud information.

4. The dynamic obstacle avoidance method according to claim 1, characterized in that, The steps for acquiring environmental point cloud information include: Camera images are captured to obtain the direction of movement of the mobile robot; The images acquired by the camera are subjected to camera calibration processing to obtain camera intrinsic parameters and distortion parameters; The images acquired by the camera are processed based on the camera intrinsic parameters and the distortion parameters to obtain the environmental point cloud information.

5. The dynamic obstacle avoidance method according to claim 4, characterized in that, The step of performing camera calibration processing on the images acquired by the camera includes: Based on Zhang's calibration algorithm, the intrinsic parameters of the camera are calibrated on the images acquired by the camera to obtain the camera's intrinsic parameters. Distortion correction is performed on the images acquired by the camera to obtain the distortion parameters.

6. The dynamic obstacle avoidance method according to claim 1, characterized in that, The step of predicting and processing the dynamic obstacle scene information in the obstacle scene information includes: Initialize the KF parameters of each dynamic obstacle point in the environmental point cloud information; The dynamic obstacle points in two adjacent frames are translated and registered to obtain the movement information of the corresponding dynamic obstacle points; The matched dynamic obstacle points are fed into the KF prior estimation and posterior update model for KF prediction. Unmatched dynamic obstacle points are considered to have stopped moving, and the KF parameters of the corresponding unmatched dynamic obstacle points are initialized. Based on the results of KF prediction processing, the first obstacle map information is obtained.

7. A dynamic obstacle avoidance device, applied to a mobile robot, characterized in that, include: Point cloud information acquisition unit, used to acquire environmental point cloud information; An obstacle recognition unit is used to recognize obstacle scenes based on the environmental point cloud information to obtain obstacle scene information; The point cloud information update unit is used to perform predictive processing on the dynamic obstacle scene information in the obstacle scene information to obtain the first obstacle map information, and update the first obstacle map information to the environmental point cloud information to obtain the updated environmental point cloud information. The obstacle avoidance optimization unit is used to process the updated environmental point cloud information based on the VFH obstacle avoidance algorithm to obtain the current target direction information; Based on the current target direction information, the oscillation region is marked on the current running path to obtain the oscillation region information; Based on the oscillation region information, the current running path is constrained to obtain optimized path information; The step of processing the updated environmental point cloud information includes: processing the updated environmental point cloud information based on the VFH obstacle avoidance algorithm to obtain a binary obstacle density histogram; processing the binary obstacle density histogram to obtain candidate valley information; weighting the candidate valley information to obtain weighted candidate valley information; and obtaining the current target direction information based on the weighted candidate valley information and preset final target direction information. The step of marking the oscillation region of the current running path includes: acquiring pre-stored trajectory information; comparing the current target direction information with the pre-stored trajectory information, and calculating the weighted Euclidean distance difference between the current running path point and the adjacent next running path point based on the comparison result to obtain a first value; when the first value falls within a preset threshold range, marking the next running path point with oscillation to obtain the oscillation region information.

8. A mobile robot, characterized in that, It includes a mobile robot body and a controller mounted on the mobile robot; the controller is used to execute the dynamic obstacle avoidance method according to any one of claims 1 to 6.

Images