A control method based on obstacle avoidance robot

Abstract

The present invention discloses a control method based on an obstacle avoidance robot, the method comprising: obtaining the working mode of the obstacle avoidance robot, the working mode comprising: normal mode, crawling mode and forward and backward falling mode; using an artificial potential field method to plan the motion trajectory of the obstacle avoidance robot, the artificial potential field method constructs a virtual potential field, the virtual potential field comprises: obstacles, robot coordinates, destination coordinates; the obstacle avoidance robot controls driving according to the working mode and the motion trajectory. By adopting the present invention, the obstacle avoidance capability of the robot can be adjusted according to different working modes, so that it can smoothly avoid obstacles and is not easy to fall into a dead point.

Description

Technical Field

[0001] The present invention relates to the field of obstacle avoidance, and in particular to a control method based on an obstacle avoidance robot. Background Art

[0002] Robots are used in more and more industries, such as home furnishing industry and manufacturing industry. Robots in the prior art usually have poor obstacle avoidance effect, which easily causes the robot to get stuck in a dead spot and unable to pass. The obstacle crossing ability of robots in different working modes is different, and the prior art cannot adjust the obstacle crossing ability of robots according to different working modes. Therefore, the inventor of the present invention provides a control method based on an obstacle avoidance robot to solve the above problems. Summary of the invention

[0003] In order to solve the above problems, the purpose of the present invention is to provide a control method based on an obstacle avoidance robot, which can adjust the robot's obstacle avoidance ability according to different working modes, so that it can avoid obstacles smoothly and is not easy to fall into dead spots.

[0004] Based on this, the present invention provides a control method based on an obstacle avoidance robot, the method comprising:

[0005] Obtaining a working mode of the obstacle avoidance robot, wherein the working modes include: a normal mode, a crawling mode, and a forward and backward falling mode;

[0006] The motion trajectory of the obstacle-avoiding robot is planned by using an artificial potential field method, wherein the artificial potential field method constructs a virtual potential field, and the virtual potential field includes: obstacles, robot coordinates, and destination coordinates;

[0007] The obstacle avoidance robot controls its movement according to the working mode and the motion trajectory.

[0008] Wherein, the construction of the virtual potential field includes:

[0009] Using the depth camera on the obstacle avoidance robot to collect the original depth image;

[0010] Obtaining the depth values ​​of pixels in the original depth image by using the callback function published by the depth camera, thereby obtaining a two-dimensional depth data matrix of the original depth image;

[0011] The three-dimensional space coordinate point is obtained by adopting a preset mapping method according to the alignment function published by the depth camera, the x and y coordinates in the two-dimensional depth data matrix, and the depth value.

[0012] Wherein, the use of the artificial potential field method to plan the motion trajectory of the obstacle avoidance robot includes:

[0013] Calculating a total potential field of the obstacle avoiding robot's operating space, where the total potential field is a superposition of a gravitational potential field of a target point on the obstacle avoiding robot and a repulsive potential field of an obstacle on the obstacle avoiding robot;

[0014] The total potential field is solved by using a preset trigonometric function, thereby controlling the movement of the obstacle avoiding robot.

[0015] The calculating of the total potential field of the running space of the obstacle avoiding robot, wherein the total potential field is the superposition of the gravitational potential field of the target point on the obstacle avoiding robot and the repulsive potential field of the obstacle on the obstacle avoiding robot, specifically includes:

[0016] F(x)=F att (x)+F rep (x)

[0017] U att (x) = k||xx g || 2 / 2

[0018] F att (x) = -grad[U att (x)]=-k||xx g ||

[0019]

[0020] The gravitational field generated by the target point on the obstacle avoiding robot is U att , the gravitational force generated by the target point on the obstacle avoiding robot is F att , the repulsive field generated by the obstacle on the obstacle avoiding robot is U rep , the repulsive force generated by the obstacle on the obstacle avoiding robot is F rep , wherein the F att With the F rep The gravitational force F(x) is synthesized, k is the proportional gain coefficient of the gravitational potential field, ‖xx g ‖ is the relative distance between the obstacle avoiding robot and the target point, p is the proportional gain coefficient of the repulsive field Urep, d is the distance between the robot and the obstacle, and d0 is the distance threshold at which the repulsive potential field affects the obstacle avoiding robot, that is, the radius of action on the robot.

[0021] Wherein, the using of a preset trigonometric function to solve the total potential field comprises:

[0022] The coordinates of the obstacle avoiding robot are (x, y), the coordinates of the center point of the obstacle are (X0, Y0), and the coordinates of the destination are (X, Y). The components of force in the x and y directions on the obstacle avoiding robot and the required forward angle θ of the obstacle avoiding robot are obtained by mathematical calculation; the process of obtaining the forward angle θ is as follows:

[0023]

[0024] The repulsive force of the obstacle in the X direction is F rep cosθ, the repulsive force of the obstacle in the Y direction is F rep sinθ.

[0025] Wherein, the method further comprises:

[0026] Assume that the dead point distance of the obstacle avoidance robot to the obstacle is h0, the actual distance to the obstacle is d, and the safe distance to the boundary is d s , the actual distance from the boundary to the boundary is d t , the destination coordinates are (X, Y), the current obstacle center coordinates are (X0, Y0), and the current obstacle avoidance robot body coordinates are (x, y). Then θ * =arctan((Yy) / (Xx)), the angle between the body and the target point and the angle between the obstacle center and the target point are obtained as θ1 and θ2 respectively;

[0027] If θ1<θ2, then the deflection is forced to be 45 degrees, otherwise it is forced to be deflected to negative 45 degrees;

[0028] The actual distance d at the boundary t Greater than the safety distance d of the boundary s Under the condition of, the obstacle avoiding robot is forced to move forward a preset distance to escape from the dead point, otherwise, the forced deflection continues.

[0029] The dead point distance is set to one fifth of the distance threshold.

[0030] In the process of obstacle avoidance of the original robot, due to the limited field of view of the depth camera, the obstacle points in the real-time constructed map are limited, and because the obstacles photographed by the camera appear directly in front, it is easy to fall into the problem of local minimum solution when calculating the total potential field. The present invention improves the original algorithm to make the robot escape from the dead point, and controls the movement trajectory of the robot by constructing an artificial virtual potential field in the robot's operating environment space and calculating the force of the robot in the artificial potential field, so as to realize the robot's autonomous obstacle avoidance. The walking route of the robot is well planned, and the obstacle avoidance effect is good. The robot will not hit obstacles and will not fall into a dead point and be unable to move forward. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0032] Figure 1 is a schematic diagram of a control method based on an obstacle avoidance robot provided in an embodiment of the present invention;

[0033] Figure 2 is a schematic diagram of an artificial potential field provided by an embodiment of the present invention;

[0034] Figure 3 It is a schematic diagram of obstacle ranging provided by an embodiment of the present invention. DETAILED DESCRIPTION

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

[0036] Figure 1 is a schematic diagram of a control method based on an obstacle avoidance robot provided by an embodiment of the present invention, the method comprising:

[0037] S101, obtaining a working mode of the obstacle avoidance robot, wherein the working modes include: a normal mode, a crawling mode, and a front and back falling mode;

[0038] The working modes of the obstacle avoidance robot basically include normal mode, crawling mode and front and rear falling mode. The obstacle avoidance robot is equipped with a three-axis tilt sensor, and adopts a high-performance microprocessor and advanced dynamics solution and Kalman dynamic filtering algorithm. It can quickly solve the current real-time motion posture of the module and provide various posture angle information for the subsequent mobile robot's obstacle avoidance. When the sensor detects that the robot's front tilt angle increases, the robot determines that it is climbing, and will gradually control the robot's driving arm to crawl downward and fall forward, so as to control the center of gravity downward and move forward to avoid the robot from falling backward when the elevation angle is too large. Similarly, when the elevation angle behind the robot is too large, the robot will determine that it is going downhill and control the center of gravity to move backward to avoid the phenomenon of falling forward.

[0039] S102, planning the motion trajectory of the obstacle-avoiding robot by using an artificial potential field method, wherein the artificial potential field method constructs a virtual potential field, and the virtual potential field includes obstacles, robot coordinates, and destination coordinates;

[0040] Figure 3 is a schematic diagram of obstacle ranging provided by an embodiment of the present invention, Figure 3 middle

[0041]

[0042] Among them, Target_xy_pixel is the coordinate of the pixel point in the lens, Target_xy_ture is the actual coordinate (the coordinate in the field), Ppx, ppy are the coordinates of the pixel point projected on the horizontal plane (this point is introduced only for ease of understanding and will not be involved in the calculation), Fx is the depth of the pixel point projected onto the horizontal plane from the robot, and Target_depth is the actual depth value returned by the camera.

[0043] The construction of the virtual potential field includes:

[0044] The depth camera used in the present invention is the Intel D435i camera, which is mainly used to realize the feedback of the depth value and color image of the object. Here, the depth camera is only responsible for providing the image, and the recognition step is implemented in the YOLOv4 model later. It is particularly emphasized that the depth information and depth value of the image usually refer to the distance from the camera to the object in the depth image. The camera can return the depth data of each pixel in the image.

[0045] By calling the callback function of / camera / aligned_depth_to_color / image_raw released by the camera, the original depth image is converted into a format through cv_bridge in the callback function, and then the depth value of a certain pixel can be obtained through the depth_pic.at function. The depth value here represents the distance between the point in the image and the camera. In this way, the two-dimensional depth data matrix of the image can be obtained.

[0046] The depth camera will release a function to align the depth image with the color image: / camera / aligned_depth_to_color / image_raw (depth flow to color flow alignment). This function can be used to obtain the corresponding three-dimensional coordinates, which also need to be transformed by the intrinsic parameter matrix of the depth camera. Figure 3 This is a mapping method. If we know the x, y coordinates and depth fx of any pixel point in the two-dimensional matrix, we can map the three points to the three-dimensional space coordinate point. In this way, we can create data in the artificial potential field.

[0047] Figure 2 is a schematic diagram of an artificial potential field provided by an embodiment of the present invention, wherein the calculating of the total potential field of the operating space of the obstacle avoiding robot, the total potential field being the superposition of the gravitational potential field of the target point on the obstacle avoiding robot and the repulsive potential field of the obstacle on the obstacle avoiding robot, specifically comprises:

[0048] F(x)=F att (x)+F rep (x)

[0049] U att (x) = k||xx g || 2 / 2

[0050] F att (x) = -grad[U att (x)]=-k||xx g ||

[0051]

[0052] The gravitational field generated by the target point on the obstacle avoiding robot is U att , the gravitational force generated by the target point on the obstacle avoiding robot is F att , the repulsive field generated by the obstacle on the obstacle avoiding robot is U rep , the repulsive force generated by the obstacle on the obstacle avoiding robot is F rep , wherein the F att With the F rep The gravitational force F(x) is synthesized, k is the proportional gain coefficient of the gravitational potential field, ‖xx g ‖ is the relative distance between the obstacle avoiding robot and the target point, p is the proportional gain coefficient of the repulsive field Urep, d is the distance between the robot and the obstacle, and d0 is the distance threshold at which the repulsive potential field affects the obstacle avoiding robot, that is, the radius of action on the robot.

[0053] Wherein, the distance threshold is a preset constant.

[0054] Calculating a total potential field of the obstacle avoiding robot's operating space, where the total potential field is a superposition of a gravitational potential field of a target point on the obstacle avoiding robot and a repulsive potential field of an obstacle on the obstacle avoiding robot;

[0055] The total potential field is solved by using a preset trigonometric function, thereby controlling the movement of the obstacle avoiding robot.

[0056] According to the distance measurement result and the detection result, calculating the obstacle size under the distance threshold specifically includes:

[0057] Wherein, the using of a preset trigonometric function to solve the total potential field comprises:

[0058] The coordinates of the obstacle avoiding robot are (x, y), the coordinates of the center point of the obstacle are (X0, Y0), and the coordinates of the destination are (X, Y). The components of force in the x and y directions on the obstacle avoiding robot and the required forward angle θ of the obstacle avoiding robot are obtained by mathematical calculation; the process of obtaining the forward angle θ is as follows:

[0059]

[0060] The repulsive force of the obstacle in the X direction is F rep cosθ, the repulsive force of the obstacle in the Y direction is F rep sinθ.

[0061] In order to obtain the parameters of the learning model, we will use a depth camera to capture photos of common daily necessities and building materials such as stone and steel bars in advance. These photos will always have problems such as low quality due to changes in lighting, so these pre-obtained images need to be processed. Labelimage is an online deep learning image segmentation and annotation tool that can basically meet common image annotation tasks. It has annotation methods such as rectangles and polygons. Through this software, we process the obtained data set, divide the recognition targets, and then put these processed photos into the model, namely the YOLOv4 model.

[0062] The YOLOv4 model is used to identify and analyze objects in the field of view. Since the robot's target is only a distant point or a target object, the recognition part will only affect the selection of the target and will not affect a series of operations such as obstacle avoidance. If you want the robot to find a specific object, you can set this object as the target and allow the robot to automatically find its way to the target.

[0063] The construction process of the YOLOv4 model includes: first, the selection of a deep learning framework. Since the deep learning model used in this scenario has certain requirements for accuracy and recognition speed, and the deep learning model trained by the YOLOv4 neural network algorithm has good results in both recognition speed and accuracy, this product uses the neural network algorithm to train the model. YOLOv4 is mainly composed of three parts: BackBone, Neck and Head.

[0064] After the framework selection is completed, we start to collect image data of the robot's working environment. After collection, we preprocess the image data and use labelimg to mark the obstacles (such as stones and common daily necessities) in the collected image information. After marking, the original image needs to be scaled to 416*416 in length and width. The specific scaling method is as follows: the ratio of the maximum value of the length and width of the original image to 416 is used as the scaling ratio. After scaling according to this ratio, the scaled image is placed in the center of the input image. The insufficient part of the short side of the original image after scaling is filled with gray (RGB is (128, 128, 128)). After the above operations are completed, the final data set is obtained.

[0065] The prepared data set is put into the YOLOv4 model for model training. For the training process, each image in the data set has been labeled, that is, the prior information such as the target position and category on each image is already known. The model parameters are random at the beginning. It is necessary to calculate the loss of the network prediction result and the real prior information, and then continuously calculate the gradient through the back propagation algorithm to reduce the loss and optimize the model parameters. In terms of the definition of the loss function, the loss part of YOLOv4 is divided into three parts, namely the loss of the regression box, the confidence loss and the classification loss. The loss of the regression box is divided into the loss of the coordinate point and the width and height loss of the regression box. The loss of the regression box can be replaced by the CIOU loss, and the confidence loss and classification loss directly use the cross entropy loss. When the loss is small enough, it means that the training effect of the model is relatively good, and the model and model parameters are saved for prediction of pictures.

[0066] S103: The obstacle avoidance robot controls its movement according to the working mode and the motion trajectory.

[0067] That is to say, the working mode of the obstacle avoidance robot is only controlled by the gyroscope. After the robot calculates the resultant force and direction according to the potential field, the switching of the working mode allows the robot to automatically adjust its center of gravity during movement and pass through terrain with a relatively large slope.

[0068] Wherein, the method further comprises:

[0069] Assume that the dead point distance of the obstacle avoidance robot to the obstacle is h0, the actual distance to the obstacle is d, and the safe distance to the boundary is d s , the actual distance from the boundary to the boundary is d t , the destination coordinates are (X, Y), the current obstacle center coordinates are (X0, Y0), and the current obstacle avoidance robot body coordinates are (x, y), then θ * =arctan((Yy) / (Xx)), the angle between the body and the target point and the angle between the obstacle center and the target point are obtained as θ1 and θ2 respectively;

[0070] If θ1<θ2, then the deflection is forced to be 45 degrees, otherwise it is forced to be deflected to negative 45 degrees;

[0071] The actual distance d at the boundary t Greater than the safety distance d of the boundary s Under the condition of , the obstacle avoidance robot is forced to move forward a preset distance to escape from the dead point. Otherwise, the forced deflection continues.

[0072] The dead point is the depth information collected by the obstacle avoidance robot in front of the robot by the depth camera at all times and mapped to a three-dimensional plane. Two points in the image (one quarter of the x-axis and three quarters of the x-axis, and one quarter of the height of the y-axis) are taken to obtain the passable size information in front of the robot. After pre-measuring the size of the robot in different extended states, it is saved as a data set. If the passable size in front of the robot is smaller than the current actual minimum size of the robot, it means that the robot cannot pass through, and this place is set as a dead point, and the robot turns to disengage in the next dead point disengagement procedure. Otherwise, after forcing the driving arms on both sides of the robot to retract, pass through the front area.

[0073] The boundary refers to the boundary of the constructed artificial potential field, i.e., the virtual potential field, which is the boundary value of the image matrix returned by the depth camera, mapped to the position in the field. Because the robot is likely to lose the target in the process of leaving the dead zone, in order to prevent the target from being lost directly, in the process of leaving the dead zone, the robot needs to stop processing the camera data for a short period of time and walk a distance in a straight line. After being forced to move a distance, if the target can still be observed and identified in the depth camera, the data processing is resumed, the potential field is continuously refreshed, and the robot is guided. Otherwise, it is considered that the target is temporarily lost. After resuming data processing, the gravitational point in the potential field is removed, and only the repulsive force of the obstacle is used to guide the robot to avoid the obstacle, and it does not move to the previous target. Move to the target after discovering a new target next time.

[0074] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and substitutions can be made without departing from the technical principles of the present invention. These improvements and substitutions should also be regarded as the scope of protection of the present invention.

Claims

1. A control method based on an obstacle avoidance robot, characterized in that: include: Obtaining a working mode of the obstacle avoidance robot, wherein the working modes include: a normal mode, a crawling mode, and a forward and backward falling mode; The motion trajectory of the obstacle avoiding robot is planned by using an artificial potential field method, and the artificial potential field method constructs a virtual potential field, and the virtual potential field includes: obstacle coordinates, robot coordinates, and destination coordinates. The method of planning the motion trajectory of the obstacle avoiding robot by using the artificial potential field method includes: calculating the total potential field of the operating space of the obstacle avoiding robot, and the total potential field is the superposition of the gravitational potential field of the target point for the obstacle avoiding robot and the repulsive potential field of the obstacle for the obstacle avoiding robot; using a preset trigonometric function to solve the total potential field, and then controlling the movement of the obstacle avoiding robot; the method of calculating the total potential field of the operating space of the obstacle avoiding robot, and the total potential field is the superposition of the gravitational potential field of the target point for the obstacle avoiding robot and the repulsive potential field of the obstacle for the obstacle avoiding robot specifically includes: F(x)=F att (x)+F rep (x) U att (x)=k||x-x g || 2 / 2 F att (x)=-grad[U att (x)]=-k||x-x g || The gravitational field generated by the target point on the obstacle avoiding robot is U att , the gravitational force generated by the target point on the obstacle avoiding robot is F att , the repulsive field generated by the obstacle on the obstacle avoiding robot is U rep , the repulsive force generated by the obstacle on the obstacle avoiding robot is F rep , wherein the F att With the F rep The gravitational force F(x) is synthesized, k is the proportional gain coefficient of the gravitational potential field, ‖xx g ‖ is the relative distance between the obstacle avoiding robot and the target point, p is the proportional gain coefficient of the repulsive field Urep, d is the distance between the robot and the obstacle, and d0 is the distance threshold at which the repulsive potential field affects the obstacle avoiding robot, i.e., the radius of action on the robot; The obstacle avoidance robot controls the movement according to the working mode and the motion trajectory; The method further comprises: Assume that the dead point distance of the obstacle avoidance robot to the obstacle is h0, the actual distance to the obstacle is d, and the safe distance to the boundary is d s , the actual distance from the boundary to the boundary is d t , the destination coordinates are (X, Y), the current obstacle center coordinates are (X0, Y0), and the current obstacle avoidance robot body coordinates are (x, y). Then θ * =arctan((Yy) / (Xx)), the angle between the body and the target point and the angle between the obstacle center and the target point are obtained as θ1 and θ2 respectively; If θ1<θ2, then the deflection is forced to be 45 degrees, otherwise it is forced to be deflected to negative 45 degrees; The actual distance d at the boundary t Greater than the safety distance d of the boundary s Under the condition of, the obstacle avoiding robot is forced to move forward a preset distance to escape from the dead point, otherwise, the forced deflection continues.

2. The control method based on the obstacle avoidance robot according to claim 1, characterized in that: The construction of the virtual potential field includes: Using the depth camera on the obstacle avoidance robot to collect the original depth image; Obtaining the depth values ​​of pixels in the original depth image by using the callback function published by the depth camera, thereby obtaining a two-dimensional depth data matrix of the original depth image; The three-dimensional space coordinate point is obtained by adopting a preset mapping method according to the alignment function published by the depth camera, the x and y coordinates in the two-dimensional depth data matrix, and the depth value.

3. The control method based on the obstacle avoidance robot according to claim 1, characterized in that: The use of a preset trigonometric function to solve the total potential field comprises: The coordinates of the obstacle avoiding robot are (x, y), the coordinates of the center point of the obstacle are (X0, Y0), and the coordinates of the destination are (X, Y). The components of force in the x and y directions on the obstacle avoiding robot and the required forward angle θ of the obstacle avoiding robot are obtained by mathematical calculation; the process of obtaining the forward angle θ is as follows: The repulsive force of the obstacle in the X direction is F rep cosθ, the repulsive force of the obstacle in the Y direction is F rep sinθ.

4. The control method based on the obstacle avoidance robot according to claim 1, characterized in that: The dead point distance is set to one fifth of the distance threshold.

Images