A robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation

Abstract

An intelligent obstacle avoidance method based on the fusion of artificial potential field method and instance segmentation algorithm includes: collecting obstacle image data of real scenes, using Python script to make instance segmentation data set; importing the data set into Yolact algorithm for training, and making ROS package; using ZED2 camera to extract the depth information of obstacles, fusing the result of instance segmentation with the depth information to obtain the world coordinates, class CLASS and MASK (mask) of obstacles, fusing the above three results into the improved artificial potential field method, defining the world coordinates of the target point, and realizing selective obstacle avoidance in emergency rescue scenes. The present invention solves the problem of delaying the best rescue time due to obstacles in emergency rescue scenes, so that it can reach the rescue point as quickly as possible.

Description

Technical Field

[0001] The invention relates to an intelligent obstacle avoidance method for a robot, and belongs to the technical field of computer vision obstacle avoidance. Background Art

[0002] Using robots for disaster rescue involves many aspects of technology, and how to quickly and accurately avoid all obstacles and find the best path to the rescue point is an urgent problem to be solved. The current mainstream vision-based obstacle avoidance algorithms can only mechanically avoid all obstacles, such as the artificial potential field method and the A* algorithm, which greatly wastes rescue time. Now, with the widespread application of deep learning in image recognition and speech recognition, many technicians have widely applied deep learning models in the field of robotics, and have achieved good results in many scenarios. However, for now, few people have applied deep learning methods to obstacle avoidance and rescue scenarios.

[0003] The artificial potential field method is a robot path planning algorithm. The algorithm regards the target and obstacle as objects that have gravitational and repulsive forces on the robot respectively. The robot moves along the combined force of gravitational and repulsive forces. The algorithm has a simple structure and is convenient for low-level real-time control. It has been widely used in real-time obstacle avoidance and smooth trajectory control.

[0004] Instance segmentation is a field in computer vision. Compared with traditional object detection, instance segmentation is pixel-level segmentation, which can obtain the categories of all objects in the image and their pixel-level masks. Based on this feature, it has been widely used in academia and industry.

[0005] The artificial potential field method has a good effect when applied to ordinary obstacle avoidance scenarios. However, for more urgent disaster relief scenarios, the artificial potential field method has a large lag and cannot find the rescue point quickly and accurately. Summary of the invention

[0006] The present invention aims to overcome the above-mentioned shortcomings of the prior art and proposes a robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation.

[0007] The present invention retrains the Yolact model to obtain all obstacle categories and masks in the scene, obtains the depth information of the obstacle through binocular vision, obtains the world coordinates of the obstacle by using the conversion relationship between the pixel coordinate system and the world coordinate system, and finally uses the artificial potential field method to selectively avoid dangerous obstacles, thereby achieving the fastest arrival at the rescue point and saving rescue time.

[0008] Technical solution: In order to achieve the above purpose, the technical solution adopted by the present invention is:

[0009] A robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation, which collects obstacle image data through ZED2 camera, converts the collected data set into COCO data set, and uses Yolact model to segment objects in the scene in real time to obtain their categories Class and mask MASK. The binocular camera ZED2 is used to obtain the depth information of each obstacle in real time. The depth information matrix and the mask matrix are co-latitude matrices. The two matrices are fused, and the world coordinates of the obstacle can be obtained by using the conversion relationship between the pixel coordinate system and the world coordinate system. In addition, the potential field equation of the artificial potential field method is fused with Class information to obtain the fused potential field. The potential field is used to calculate the gravity and repulsion, and the acceleration is calculated in combination with the mass of the car. Finally, the speed is calculated to obtain the distance that the car needs to travel within the unit sampling time. Finally, the position-speed conversion method is used to obtain the position where the car should travel to at the next sampling moment. At this point, the entire control strategy is completed, which specifically includes the following steps:

[0010] Step 1: Collect the RGB information of obstacles at various viewing angles required for the experiment, perform data enhancement by cropping and rotating, and preprocess the data set:

[0011] Step 1-1: Collect and annotate sample image data of indoor obstacles;

[0012] Step 1-2: Perform feature enhancement on the labeled sample dataset to obtain the COCO dataset;

[0013] Step 1-3: Build the Yolact model;

[0014] Step 1-4: Use the COCO dataset to train the Yolact model to obtain target parameters and target weights of obstacle samples, and use the target parameters and target weights as parameters and weights of the trained Yolact model;

[0015] Step 1-5: Use the trained Yolact model to detect the standardized obstacle sample image and output the category Class and MASK matrix M of the corresponding obstacle sample. w×h , (w, h) represents the input dimension of the RGB image, and defines Class safe For security categories, Class danger is a hazard category;

[0016] Step 2: Obtain the depth information matrix D of the obstacle through the ZED2 camera w×h , and through the camera's rotation matrix R 3×3 With the translation matrix T 3×1Get the 3D position information of the obstacle relative to the camera, and establish the transformation relationship matrix H between the car's world coordinate system base_link and the camera's world coordinate system zed2_link 3×1 :

[0017] Step 2-1: According to the equidistant sampling algorithm, obtain the depth value z and the corresponding pixel coordinates (x, y) of the pixel position in each area of ​​the obstacle sample;

[0018] Step 2-3: Obtain the intrinsic parameter matrix Q through camera calibration, which can be expressed by the following formula:

[0019]

[0020] Among them, f x 、f y Respectively represent the focal lengths in the x and y coordinate axes in the camera coordinate system, and u0 and v0 represent the pixel centers of the u and v axes in the pixel coordinate system;

[0021] Step 2-4: Convert the obstacle’s pixel coordinates (u, v) and its corresponding depth value z to the camera’s world coordinates (X ω , Y ω , Z ω ), the conversion formula is as follows:

[0022]

[0023] Where R and T are rotation matrix and translation matrix respectively;

[0024] Step 2-5: Based on the transformation relationship matrix H between the car world coordinate system base_link and the camera world coordinate system zed2_link given in step 2 3×1 It can be obtained that the coordinates of the obstacle in the world coordinate system of the car are given by the following formula:

[0025]

[0026] in, X offset , Y offset , Z offset Respectively represent the relative displacement between the camera and the car in space.

[0027] Step 3: Determine the coordinates of the target point and build a model based on the artificial potential field method. Based on steps 1 and 2, we can obtain the world coordinates of the obstacle relative to the car and its corresponding category (X ω , Y ω , Z ω , Class), in this paper, we do not consider Z ωCoordinates, we determine a target point coordinate (X g , Y g ), in order to establish the potential field model:

[0028] Step 3-1: Establish an artificial potential field, abstract the movement of the car in the environment around it into the movement in the gravitational field. The target exerts gravitational force on the car, and the obstacle exerts repulsive force on the car. The direction of the resultant force is calculated according to the superposition principle, which is the direction of movement of the car.

[0029] Step 3-2: Define the gravity formula at the target point:

[0030]

[0031] Where α is the gravitational gain, d(q,q goal ) is the current position of the car and the target point q goal :(X g , Y g ) between the two sides;

[0032] Step 3-3: The instance segmentation algorithm Yolact will calculate the obstacle category Class in real time. When the obstacle category belongs to the Class in step 1-5 danger When there is repulsion at the obstacle of this category, when the obstacle category belongs to Class safe When , the repulsion is ignored, so the repulsion formula is distributed as follows:

[0033]

[0034] Among them, d(q,q obs ) is the distance between the current point of the car and the obstacle, β is the repulsion gain, Q is the distance threshold of the obstacle. When the distance between the current point and the obstacle is greater than this threshold, the obstacle will not repel the car.

[0035] Step 3-4: According to steps 3-2 and 3-3, the gravity and repulsion of the drone at any position can be calculated, where the gravity is given by the following formula:

[0036]

[0037] The repulsive force is given by the following formula:

[0038]

[0039] The current resultant force of the car is:

[0040] F(q)=F att (q)+F rep(q) (8)

[0041] Step 3-5: Determine the real-time steering angle θ of the car based on the odometer time ;

[0042] Step 3-6: According to the F given in 3-4 att (q), F rep(q) Then we can get the resultant force F com and the angle θ relative to the world coordinate system expect , according to the mass M of the car and the acceleration formula, we can get its expected speed v control , according to the sampling time t, the expected coordinate (x expect ,y expect ), where v expect 、x expect ,y expect It is given by the following formula:

[0043]

[0044] x expect =x0+v expect ·t·cos θ expect (10)

[0045] y expect =y0+v expect t sinθ expect (11)

[0046] Among them, (x0, y0) represents the initial coordinates of the car;

[0047] Step 4: Conversion of relative pose to velocity model:

[0048] Step 4-1: Given the control coefficients of linear velocity v and angular velocity ω (k v , k ω ) and the initial velocity value (v0, ω0), according to the θ given in step 3 expect 、x expect ,y expect And the current position information of the car θ time 、x time ,y time Establish the speed conversion model, then its error matrix E robot , translation matrix T robot It can be given by the following formula:

[0049]

[0050]

[0051] Then the speed control matrix C robot =T robot ·E robot;

[0052] Step 4-2: Calculate the real-time speed of the car. We define the speed control matrix in step 4-1 as:

[0053]

[0054] Then the target linear velocity of the car is v goal 、Target angular velocity ω goal It can be given by the following formula:

[0055] v goal =k v ·c1+v0·cos c3 (15)

[0056] ω goal =k ω ·sin c3+v0·c2+ω0 (16)

[0057] Preferably, the RGB image information of each viewing angle collected in step 1 is limited, so there is still a problem of unrecognizable categories in real scenes.

[0058] Preferred: Class described in steps 1-5 safe With Class danger It is artificially defined at the algorithm level and has a certain degree of randomness. Considering the real scene, Class safe Should be as safe as possible.

[0059] Optimum: The transformation relationship matrix H between the car world coordinate system base_link established in step 2 and the camera world coordinate system zed2-link 3×1 The value of changes with the relative position between the camera and the car.

[0060] Preferably: the rotation matrix R determined in steps 2-4 and the translation matrix T are composed of It is the camera external parameter and will change with the changes of obstacles and camera pose.

[0061] Preferably: In step 3-5, the real-time steering angle θ is determined according to the odometer time Cumulative errors will occur during long-term operation.

[0062] Preferably: the initial coordinates (x0, y0) of the car in step 3-6 will return to zero when the car is restarted.

[0063] Preferably: the control coefficient (k v , k ω ) should be determined according to the upper limit of the trolley speed.

[0064] Preferably: To ensure the continuity and stability of speed control, the initial linear velocity value v0 given in step 4-1 should be as small as possible, and the initial angular velocity value ω0 should be set to 0.

[0065] Compared with the prior art, the present invention has the following beneficial effects:

[0066] 1. The application of instance segmentation technology to obstacle avoidance algorithm may be proposed for the first time in this invention;

[0067] 2. Compared with the simple obstacle avoidance algorithm, this paper proposes a robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation, which improves the work efficiency in rescue scenarios;

[0068] 3. The present invention proposes different path planning methods when the car encounters different obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 Flowchart of the robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation

[0070] Figure 2 It is the conversion relationship diagram between pixel coordinate system and world coordinate system;

[0071] Figure 3 This is a schematic diagram of the artificial potential field algorithm;

[0072] Figure 4 This is the framework diagram of the intelligent obstacle avoidance method. DETAILED DESCRIPTION

[0073] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention is clearly and completely described below in conjunction with the accompanying drawings and specific embodiments. Obviously, the described embodiments are 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.

[0074] A robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation, such as Figure 1 As shown, it includes the input of RGB information and depth information, data set preparation and preprocessing, the fusion of depth information and instance segmentation results, the acquisition of the world coordinates of the obstacle, the fusion and modification of the artificial potential field method to start the experiment, and finally reach the target point.

[0075] The specific steps are as follows:

[0076] Step 1: Collect the RGB information of obstacles at various viewing angles required for the experiment, perform data enhancement by cropping and rotating, and preprocess the data set:

[0077] Step 1-1: Collect and annotate sample image data of indoor obstacles;

[0078] Step 1-2: Perform feature enhancement on the labeled sample dataset to obtain the COCO dataset;

[0079] Step 1-3: Build the Yolact model;

[0080] Step 1-4: Use the COCO dataset to train the Yolact model to obtain target parameters and target weights of obstacle samples, and use the target parameters and target weights as parameters and weights of the trained Yolact model;

[0081] Step 1-5: Use the trained Yolact model to detect the standardized obstacle sample image and output the category Class and MASK matrix M of the corresponding obstacle sample. w×h , (w, h) represents the input dimension of the RGB image, and defines Class safe For security categories, Class danger is a hazard category;

[0082] Step 2: Obtain the depth information matrix D of the obstacle through the ZED2 camera w×h , establish the transformation relationship between the pixel coordinate system and the world coordinate system, such as Figure 2 As shown, through the camera's rotation matrix R 3×3 With the translation matrix T 3×1 Get the 3D position information of the obstacle relative to the camera, and establish the transformation relationship matrix H between the car's world coordinate system base_link and the camera's world coordinate system zed2_link 3×1 :

[0083] Step 2-1: According to the equidistant sampling algorithm, obtain the depth value z and the corresponding pixel coordinates (x, y) of the pixel position in each area of ​​the obstacle sample;

[0084] Step 2-3: Obtain the intrinsic parameter matrix Q through camera calibration, which can be expressed by the following formula:

[0085]

[0086] Among them, f x 、f y Respectively represent the focal lengths in the x and y coordinate axes in the camera coordinate system, and u0 and v0 represent the pixel centers of the u and v axes in the pixel coordinate system;

[0087] Step 2-4: Convert the obstacle’s pixel coordinates (u, v) and its corresponding depth value z to the camera’s world coordinates (X ω , Yω , Z ω ), the conversion formula is as follows:

[0088]

[0089] Where R and T are rotation matrix and translation matrix respectively;

[0090] Step 2-5: Based on the transformation relationship matrix H between the car world coordinate system base_link and the camera world coordinate system zed2_link given in step 2 3×1 It can be obtained that the coordinates of the obstacle in the world coordinate system of the car are given by the following formula:

[0091]

[0092] in, X offset , Y offset , Z offset Respectively represent the relative displacement between the camera and the car in space.

[0093] Step 3: Determine the coordinates of the target point and establish a model based on the artificial potential field method, such as Figure 3 As shown, based on steps 1 and 2, we can get the world coordinates of the obstacle relative to the car and its corresponding category (X ω , Y ω , Z ω , Class), in this paper, we do not consider Z ω Coordinates, we determine a target point coordinate (X g , Y g ), in order to establish the potential field model:

[0094] Step 3-1: Establish an artificial potential field, abstract the movement of the car in the environment around it into the movement in the gravitational field. The target exerts gravitational force on the car, and the obstacle exerts repulsive force on the car. The direction of the resultant force is calculated according to the superposition principle, which is the direction of movement of the car.

[0095] Step 3-2: Define the gravity formula at the target point:

[0096]

[0097] Where α is the gravitational gain, d(q,q goal ) is the current position of the car and the target point q goal :(X g , Y g ) between the two sides;

[0098] Step 3-3: The instance segmentation algorithm Yolact will calculate the obstacle category Class in real time. When the obstacle category belongs to the Class in step 1-5 danger When there is repulsion at the obstacle of this category, when the obstacle category belongs to Class safe When the repulsive force is ignored, an intelligent obstacle avoidance model is established based on this, such as Figure 4 As shown, under this model, the repulsion formula is distributed as follows:

[0099]

[0100] Among them, d(q,q obs ) is the distance between the current point of the car and the obstacle, β is the repulsion gain, Q is the distance threshold of the obstacle. When the distance between the current point and the obstacle is greater than this threshold, the obstacle will not repel the car.

[0101] Step 3-4: According to steps 3-2 and 3-3, the gravity and repulsion of the drone at any position can be calculated, where the gravity is given by the following formula:

[0102]

[0103] The repulsive force is given by the following formula:

[0104]

[0105] The current resultant force of the car is:

[0106] F(q)=F att (q)+F rep(q) (8)

[0107] Step 3-5: Determine the real-time steering angle θ of the car based on the odometer time ;

[0108] Step 3-6: Based on the F given in step 3-4 att (q), F rep(q) Then we can get the resultant force F com and the angle θ relative to the world coordinate system expect , according to the mass M of the car and the acceleration formula, we can get its expected speed v control , according to the sampling time t, the expected coordinate (x expect ,y expect ), where v expect 、x expect ,y expect It is given by the following formula:

[0109]

[0110] xexpect =x0+v expect t cosθ expect (10)

[0111] y expect =y0+v expect t sinθ expect (11)

[0112] Among them, (x0, y0) represents the initial coordinates of the car;

[0113] Step 4: Conversion of relative pose to velocity model:

[0114] Step 4-1: Given the control coefficients of linear velocity v and angular velocity ω (k v , k ω ) and the initial velocity value (v0, ω0), according to the θ given in step 3 expect 、x expect ,y expect And the current position information of the car θ time 、x time ,y time Establish the speed conversion model, then its error matrix E robot , translation matrix T robot It can be given by the following formula:

[0115]

[0116]

[0117] Then the speed control matrix C robot =T robot ·E robot ;

[0118] Step 4-2: Calculate the real-time speed of the car. We define the speed control matrix in step 4-1 as:

[0119]

[0120] Then the target linear velocity of the car is v goal 、Target angular velocity ω goal It can be given by the following formula:

[0121] v goal =k v ·c1+v0·cos c3(15)

[0122] ω goal =k ω ·sin c3+v0·c2+ω0 (16)

[0123] The present invention is more complicated than the traditional visual obstacle avoidance method, but has better speed and accuracy in obstacle avoidance and is more practical in rescue and other scenarios.

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

Claims

1. A robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation, characterized in that: The following steps are involved: Step 1: Collect the RGB information of obstacles at various viewing angles required for the experiment, perform data enhancement by cropping and rotating, and preprocess the data set: Step 1-1: Collect and annotate sample image data of indoor obstacles; Step 1-2: Perform feature enhancement on the labeled sample dataset to obtain the COCO dataset; Step 1-3: Build the Yolact model; Step 1-4: Use the COCO dataset to train the Yolact model to obtain target parameters and target weights of obstacle samples, and use the target parameters and target weights as parameters and weights of the trained Yolact model; Step 1-5: Use the trained Yolact model to detect the standardized obstacle sample image and output the category Class and MASK matrix M of the corresponding obstacle sample. w×h , (w,h) represents the input dimension of RGB image, and defines Class safe For security categories, Class danger is a hazard category; Step 2: Obtain the depth information matrix D of the obstacle through the ZED2 camera w×h , and through the camera's rotation matrix R 3×3 With the translation matrix T 3×1 Get the 3D position information of the obstacle relative to the camera, and establish the transformation relationship matrix H between the car's world coordinate system base_link and the camera's world coordinate system zed2_link 3×1 : Step 2-1: According to the equidistant sampling algorithm, obtain the depth value z and the corresponding pixel coordinates (u, v) of the pixel position in each area of ​​the obstacle sample; Step 2-3: Obtain the intrinsic parameter matrix Q through camera calibration. Q can be expressed by the following formula: Among them, f x 、f y Respectively represent the focal lengths in the x and y coordinate axes in the camera coordinate system, and u0 and v0 represent the pixel centers of the u and v axes in the pixel coordinate system; Step 2-4: Convert the obstacle's pixel coordinates (u, v) and its corresponding depth value z to the camera's world coordinates (X ω ,Y ω ,Z ω ), the conversion formula is as follows: Where R and T are rotation matrix and translation matrix respectively; Step 2-5: Based on the transformation relationship matrix H between the car world coordinate system base_link and the camera world coordinate system zed2_link given in step 2 3×1 It can be obtained that the coordinates of the obstacle in the world coordinate system of the car are given by the following formula: in, X offset , Y offset , Z offset Respectively represent the relative displacement between the camera and the car in space; Step 3: Determine the coordinates of the target point, establish a model based on the artificial potential field method, and obtain the world coordinates of the obstacle relative to the car and its corresponding category (X car ,Y car ,Z car ,Class), without considering Z car Coordinates, determine a target point coordinate (X g ,Y g ), in order to establish the potential field model: Step 3-1: Establish an artificial potential field, abstract the movement of the car in the environment around it into the movement in the gravitational field. The target exerts gravitational force on the car, and the obstacle exerts repulsive force on the car. The direction of the resultant force is calculated according to the superposition principle, which is the direction of movement of the car. Step 3-2: Define the gravity formula at the target point: Where α is the gravitational gain, d(q,q goal ) is the current position of the car and the target point q goal (X g , Y g ) between the two sides; Step 3-3: The instance segmentation algorithm Yolact will calculate the obstacle category Class in real time. When the obstacle category belongs to the Class in step 1-5 danger When there is repulsion at the obstacle of this category, when the obstacle category belongs to Class safe When , the repulsion is ignored, so the repulsion formula is distributed as follows: Among them, d(q,q obs ) is the distance between the current point of the car and the obstacle, β is the repulsion gain, Q is the distance threshold of the obstacle. When the distance between the current point and the obstacle is greater than this threshold, the obstacle will not repel the car. Step 3-4: According to steps 3-2 and 3-3, the attraction and repulsion of the car at any position can be calculated, where the attraction is given by the following formula: The repulsive force is given by the following formula: The current resultant force of the car is: F(q)=F att (q)+F rep(q) #(8) Step 3-5: Determine the real-time steering angle θ of the car based on the odometer time ; Step 3-6: According to the F given in 3-4 att (q), F rep(q) Then we can get the resultant force F com and the angle θ relative to the world coordinate system expect , according to the mass M of the car and the acceleration formula, we can get its expected speed v expect , according to the sampling time t, the expected coordinate (x expect ,y expect ), where v expect 、x expect ,y expect It is given by the following formula: x expect =x0+v expect ·t·cosθ expect #(10) y expect =y0+v expect ·t·sinθ expect #(11) Among them, (x0, y0) represents the initial coordinates of the car; Step 4: Convert relative pose to velocity model: Step 4-1: Given the control coefficients of linear velocity v and angular velocity ω (k v ,k ω ) and the initial velocity value (v0,ω0), according to the θ given in step 3 expect 、x expect ,y expect And the current position information of the car θ time 、x time ,y time Establish the speed conversion model, then its error matrix E robot , translation matrix T robot It can be given by the following formula: Then the speed control matrix C robot =T robot ·E robot ; Step 4-2: Calculate the real-time speed of the car. We define the speed control matrix in step 4-1 as: Then the target linear velocity of the car is v goal 、Target angular velocity ω goal It can be given by the following formula: v goal =k v ·c1+v0·cos c3#(15) oh goal =k ω sinc3+v0 c2+ω0#(16).

2. The robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation according to claim 1, characterized in that: The transformation relationship matrix H between the car world coordinate system base_link established in step 2 and the camera world coordinate system zed2_link 3×1 The value of changes with the relative position between the camera and the car.

3. The robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation according to claim 1, characterized in that: The rotation matrix R determined in steps 2-4 and the translation matrix T are composed of It is the camera external parameter and will change with the changes of obstacles and camera pose.

4. The robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation according to claim 1, characterized in that: The initial coordinates (x0, y0) of the car in steps 3-6 will return to zero when the car is restarted.

5. The robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation according to claim 1, characterized in that: The control coefficient (k v ,k ω ) should be determined according to the upper limit of the trolley speed.

6. The robot intelligent obstacle avoidance method based on artificial potential field method and instance segmentation according to claim 1, characterized in that: To ensure the continuity and stability of speed control, the initial linear velocity value v0 given in step 4-1 should be close to 0, and the initial angular velocity value ω0 should be set to 0.

Images