CN108858199B - Method for grabbing target object by service robot based on vision - Google Patents

Abstract

The invention relates to the technical field of service robots, in particular to a vision-based method for a service robot to grab a target object, aiming to solve the problem of robot grabbing in the presence of obstacles that prevent the target object from being directly grabbed. The method for grabbing a target object in the present invention includes: acquiring a color image and original three-dimensional point cloud data in a camera coordinate system; detecting the target object, acquiring point cloud data of the target object, and obtaining environmental point cloud data around the target object , transform the above point cloud data into the manipulator coordinate system; fit the plane equation of the plane where the target object is located in the manipulator coordinate system; on this basis, obtain the position and size information of each obstacle, as well as the target object's Position and size information; based on the above position and size information, first move the obstacles that prevent the target object from being directly grasped, and then complete the grasp of the target object. The invention effectively improves the grasping ability of the service robot.

Description

A vision-based approach to grasping target objects for service robots

technical field

The invention relates to the technical field of service robots, in particular to a method for a vision-based service robot to grab a target object.

Background technique

With the continuous development of service robot and artificial intelligence technology, its application fields are also expanding. In addition to environmental perception, positioning, and navigation, the core technology of intelligent service robots is also very important for object grasping technology, which is the premise for service robots to provide high-quality services. For example, in environments such as home, office, and medical care, services such as picking up items require service robots to have the ability to grab objects. In order for the service robot to grasp objects, the robotic arm needs to be installed on the mobile platform of the service robot. After the mobile platform reaches the designated operating area, the service robot will control the robotic arm to implement grasping. The grasping of service robots involves the detection of objects and the motion planning of robotic arms, among which the research on the method of grasping objects based on vision is the most common.

Domestic and foreign researchers have carried out in-depth research on vision-based robotic grasping. Object detection is the premise of service robot grasping. Traditional object detection methods usually require hand-designed features, which have poor generalization ability and poor robustness. In recent years, deep learning has attracted attention, and its advantage is that the feature extraction process does not require manual design of features, but by using large-scale data sets to train the model, the features of the target object can be learned, which has strong robustness. At present, the common target detection methods based on deep learning include RCNN, Fast-RCNN, Faster-RCNN, YOLO, SSD, etc. Among them, SSD combines the advantages of YOLO's fast speed and Faster-RCNN's high detection accuracy and has attracted widespread attention. After completing the detection of the target object, the service robot can plan the motion of its own robotic arm, and then complete the grasping of the target object. A common way to plan the motion of the robotic arm is to call MoveIt! Feature pack. MoveIt! The function package integrates the forward and inverse kinematics solution, motion planning, collision detection and other functions of the robotic arm. The motion planning uses the open source motion planning library (OpenMotionPlanning Library, OMPL). Currently, MoveIt! The function package has been widely used in the mainstream robotic arm motion planning.

The existing robot grasping technology usually directly grasps the target object after completing the detection of the specified target object. In fact, in real-world environments, there are sometimes obstacles around the target object that prevent the robotic arm of the service robot from grabbing directly. Therefore, it is necessary to conduct more in-depth research on the existing robot grasping technology, in order to solve the problem that the existing technology is difficult to satisfy the robot grasping problem under the situation of obstacles that prevent the target object from being directly grasped.

SUMMARY OF THE INVENTION

In order to solve the above problems in the prior art, the present invention proposes a vision-based method for a service robot to grasp a target object, which can realize the service robot's ability to grasp the target object in the presence of obstacles that prevent the target object from being directly grasped. The grasping of objects improves the grasping ability of the service robot.

The present invention provides a method for a vision-based service robot to grab a target object, comprising the following steps:

Step S1, obtain the color image Is and the original three-dimensional point cloud data D _s under the camera coordinate system O _c X _c Y _c Z _c through the Kinect sensor installed on the service robot _;

Step S2, based on the color image I _s , use a deep learning SSD (Single Shot multiBox Detector) target detection method to detect the target object, and then obtain the first point cloud of the target object from the original three-dimensional point cloud data D _s data D _to ; remove the first point cloud data D _to of the target object from the original three-dimensional point cloud data D _s to obtain the first environmental point cloud data D _rs ;

Step S3, according to the coordinate transformation relationship from the camera coordinate system O _c X _c Y _c Z _c to the manipulator coordinate system _Or X _r Y _r Z _r of the service robot, transform the first environmental point cloud data _Drs Under the coordinate system Or X _r Y _r Z _r of the robot arm, the second environmental point cloud data D _r is obtained _; the first point cloud data D _to of the target object is converted to the coordinate system _Or of the robot arm Under X _r Y _r Z _r , obtain the second point cloud data D _t of the target object;

Step S4, in combination with the working space S _rm of the robotic arm on the X _r Y _r plane, for the second environmental point cloud data D _r , remove the data whose values in the Y _r axis direction are outside the range of [y _min , y _max ] , and remove the data whose values in the X _r axis direction are outside the range of [x _min , x _max ] to obtain the third environmental point cloud data D _f ; wherein, the coordinates of the lower left corner and the upper right corner of the workspace S _rm are respectively (x _min , y _min ) and (x _max , y _max ), x _min , y _min , x _max and y _max are all preset thresholds;

Step S5, based on the third environmental point cloud data D _f , using a random sampling consistency (Random Sample Consensus, RANSAC) algorithm to fit the plane where the target object is located, to obtain a plane equation of the plane where the target object is located;

Step S6, in combination with the plane equation of the plane where the target object is located, remove the data corresponding to the plane and the point below the plane in the third environmental point cloud data D _f to obtain the fourth environmental point cloud data D _o , Then perform point cloud cluster clustering, and obtain the position and size information of obstacles corresponding to each point cloud cluster;

Step S7, in combination with the plane equation, remove the plane and the data corresponding to the point below the plane in the second point cloud data D _t of the target object to obtain the third point cloud data D _tr of the target object , and obtain the position and size information of the target object;

Step S8, based on the position and size information of the target object and the position and size information of each obstacle, first move the obstacles that prevent the target object from being directly grasped, and then complete the grasping of the target object. Pick.

Preferably, the coordinate transformation relationship from the camera coordinate system O _c X _c Y _c Z _c to the robotic arm coordinate system _Or X _r Y _r Z _r of the service robot in step S3 is:

Wherein, (x _cp , y _cp , z _cp ) ^T and (x _rp , y _rp , z _rp ) ^T are respectively the points in the original 3D point cloud data D _s in the camera coordinate system O _c X _c Y _c Z _c and the coordinates in the robotic arm coordinate system _Or X _r Y _r Z _r , T _m is the hand-eye relationship matrix;

The calculation method of the hand-eye relationship matrix includes:

和

对应的齐次坐标分别为

和

其中，n_g为预设的常数，t＝1,2,…,n_g；Control the end of the manipulator to move to different positions, and obtain the n _g of the end of the manipulator in the camera coordinate system O _c X _c Y _c Z _c and the manipulator coordinate system _Or X _r Y _r Z _r respectively three-dimensional coordinates

and

The corresponding homogeneous coordinates are

and

Among them, n _g is a preset constant, t=1,2,...,n _g ;

The hand-eye relationship matrix is calculated according to the following formula:

表示求取矩阵

的伪逆。in,

Indicates to find a matrix

The pseudo-inverse of .

Preferably, the method for obtaining the coordinates of the end of the robotic arm in the camera coordinate system O _c X _c Y _c Z _c is as follows:

A red ball is fixed at the end of the robotic arm, and based on the color image provided by the Kinect sensor, the target recognition algorithm based on color features is used to identify the red ball, so as to obtain the center point of the red ball at the camera coordinates The coordinates (x _c , y _c , z _c ) ^T in the system O _c X _c Y _c Z _c are taken as the coordinates of the end of the robotic arm in the camera coordinate system O _c X _c Y _c Z _c .

Preferably, the method for obtaining the coordinates of the end of the manipulator in the coordinate system of the manipulator _Or X _r Y _r Z _r is as follows:

Take advantage of MoveIt! The function package obtains the three-dimensional coordinates (x _{r , y r , z r ) T of the end of the manipulator in the coordinate system Or X r Y r Z r of the manipulator} ^through _{forward kinematics solution} .

Preferably, in step S5 "based on the third environmental point cloud data D _f , a random sampling consistency algorithm is used to fit the plane where the target object is located to obtain a plane equation of the plane where the target object is located", which specifically includes :

Step S51, select 3 data from the third environmental point cloud data D _f , and calculate a plane according to the corresponding points of the 3 data;

Step S52, obtain the distance from the point corresponding to the data other than the 3 data in the third environmental point cloud data D _f to the plane, and use the point within the range of d _h as the inner point to form the plane corresponding to The interior point set of , where d _h is the preset distance threshold;

Step S53, repeating steps S51-S52, until the ratio of the number of inliers in the inlier set to the number of all data in the third environmental point cloud data _Df exceeds λ or reaches a preset number of iterations _MK ; where λ is the threshold of the preset quantity ratio;

Step S54, the plane equation corresponding to the maximum number of interior points is used as the plane equation of the plane where the target object is located, and the plane equation is expressed in the robotic arm coordinate system _Or X _r Y _r Z _r as:

A _op x+B _op y+C _op z+D _op =0

Among them, A _op , B _op , C _op , and D _op are the four coefficients of the plane equation.

Preferably, in step S6 "combining the plane equation of the plane where the target object is located, remove the data corresponding to the plane and the points below the plane in the third environmental point cloud data D _f to obtain the fourth environmental point cloud Data D _o , and then perform point cloud cluster clustering, and obtain the position and size information of obstacles corresponding to each point cloud cluster, including:

Step S61, in conjunction with the plane equation of the plane where the target object is located, remove the data corresponding to the plane and the point below the plane in the third environmental point cloud data D _f to obtain the fourth environmental point cloud data D _o ;

Step S62, based on the fourth environmental point cloud data D _o , use the Euclidean clustering algorithm to perform point cloud cluster clustering, and obtain the position and size information of obstacles corresponding to each point cloud cluster;

Wherein, step S62 specifically includes:

Step _S621 , for the fourth environmental point cloud data Do, set a cluster set C and a queue Q, initially the cluster set C and the queue Q are empty sets, and _mark each data in Do is unprocessed;

Step S622, judging the point p _i corresponding to the data in the fourth environment point cloud data D _o , if the data corresponding to the point p _i in the fourth environment point cloud data D _o is in the processed state, then Continue to judge the next point of point p _i ; when all the data in the fourth environment point cloud data D _o are in the processed state, go to step S626; if point p _i is in the fourth environment point cloud data D o If the corresponding data in _o is in an unprocessed state, go to step S623; wherein, i=0, 1..., N _d -1, and N _d is the total number of data in D _o ;

Step S623, set the queue Q as an empty set, put the point p _i in the queue Q, and mark the data corresponding to the point in the fourth environment point cloud data D _o as a processed state;

Step S624, read a point from the queue Q, denoted as q, and obtain the distance between the point and the point q corresponding to all the data in the unprocessed state in the fourth environment point cloud data D _o , when When the distance does not exceed _{d th} , the corresponding point is added to the queue Q, and the data corresponding to the point in Do is marked as a processed state; the execution is repeated until all points in the queue Q are traversed. Complete and no new points are added to the queue Q; wherein, d _th is a preset threshold;

Step S625, if the number of points in the queue Q is greater than N _min , record all the points in the queue Q as a point cloud cluster and put it into the cluster set C; go to step S622; wherein, N _min is the preset threshold;

Step S626, determine whether the cluster set C is an empty set, if not, calculate the position and size information of the obstacle corresponding to each point cloud cluster.

Preferably, in step S8 "based on the position and size information of the target object and the position and size information of each obstacle, first move the obstacles that prevent the target object from being directly grasped, and then complete the Grab the target object", including:

Step S81, based on the position and size information of the target object and the position and size information of each obstacle, use MoveIt! The function package determines whether the target object can be directly grabbed, and if so, according to MoveIt! The planning result of the function package implements the grasping of the target object; otherwise, step S82 is performed; wherein, the grasping direction of the end of the mechanical arm is vertically downward grasping along the Z _r axis;

In step S82, the obstacles that prevent the target object from being directly grasped are moved, so as to complete the grasping of the target object.

Preferably, in step S82, "moving obstacles that prevent the target object from being directly grasped, so as to complete the grasping of the target object", specifically includes:

Step S821, in the third point cloud data D _tr of the target object, select all the data whose value is within [z _max -z _h , z _max ] in the direction of the Z _r axis as the target object grasping area The point cloud data D _gr ; wherein, z _max is the maximum value of each data in D _tr in the direction of the Z _r axis, and z _h is a preset threshold;

Step S822, for each obstacle, obtain the minimum value of the distance between each point in the point set corresponding to the point cloud data D _gr of the target object grasping area and each point in the corresponding point cloud cluster of the obstacle. , the minimum value is recorded as the distance from the obstacle to the target object, and the distance from the obstacle to the target object and the ID number of the obstacle are combined to form an obstacle influence sequence;

Step S823, divide the working space S _rm of the robotic arm on the X _r Y _r plane into a grid to obtain a grid map of the working space S _rm ; respectively divide the third point cloud data D _tr of the target object corresponding to All points and point cloud clusters corresponding to obstacles are projected onto the X _r Y _{r plane of the robotic arm coordinate system Or X r Y r Z r , thereby} obtaining the corresponding circumscribed rectangle;

Step S824, according to the distance to the target object corresponding to each obstacle in the obstacle influence sequence, select the obstacle with the smallest distance to the target object as the obstacle to be moved O _{bs_mov} ;

Step S825, assigning a value to each grid in the workspace S _rm to generate a grid map of the gravitational potential field corresponding to the obstacle to be moved O _{bs_mov} ;

Step S826, for each other object, reassign each grid in the workspace S _rm to generate a grid map of the repulsion potential field corresponding to the object; wherein, the other objects include: in addition to the to-be-moved Obstacles other than the obstacle O _{bs_mov} and the target object;

Step S827, superimpose the gravitational potential field grid map corresponding to the obstacle to be moved O _{bs_mov} and the repulsive force potential field grid map of the other objects to generate the final potential field grid map; select the potential field grid map The position corresponding to the grid with the smallest numerical value in the figure is used as the position to be placed of the obstacle to be moved O _{bs_mov} ; if the number of grids with the smallest numerical value is greater than 1, then select the distance from the obstacle to be moved O _{bs_mov} at X _r Y The position corresponding to the grid closest to the center point of the circumscribed rectangle projected on the _r plane is used as the position to be placed of the obstacle to be moved O _{bs_mov} ;

Step S828, based on the current position and size information of the obstacle to be moved _{Obs_mov} , the position to be placed of the obstacle to be moved _{Obs_mov} , other obstacles and the position and size information of the target object, use MoveIt! The function package performs the motion planning of the manipulator, and completes the transfer of the obstacle to be moved _{Obs_mov} ; the obstacle to be moved _{Obs_mov} is renamed the obstacle to be moved _{Obs_mov} , and the to-be-placed position of the obstacle Renamed as a new position; move the moved obstacle O _{bs_mov} out of the obstacle influence sequence, the position of the center point of the circumscribed rectangle obtained by the projection of the obstacle on the X _r Y _r plane is used after the obstacle has been moved. be updated with the new location;

Step S829, based on the position and size information of the target object, the new position and size information of the moved obstacle O _{bs_mov} , and the current position and size information of other obstacles, through MoveIt! The function package judges whether the target object can be directly grabbed, and if so, use MoveIt! The function package performs motion planning of the robotic arm, and then grasps the target object; otherwise, go to step S824.

Preferably, in step S825, "assigning each grid in the workspace S _rm to generate a grid map of the gravitational potential field corresponding to the obstacle to be moved O _{bs_mov} " specifically includes:

The numerical value of all grids in the circumscribed rectangle obtained by the projection of the obstacle to be moved O _{bs_mov} on the X _r Y _r plane is assigned as the height of the obstacle along the Z _r axis direction;

Based on the circumscribed rectangle obtained by the projection of the obstacle to be moved O _{bs_mov} on the X _r Y _r plane, expand outward in the unit of grid, each time in the positive direction of the X _r axis, the positive direction of the Y _r axis, and the negative direction of the X _r axis A grid is simultaneously expanded in the negative direction of the Y _r -axis, thereby generating a series of expanded rectangles until the outermost expanded rectangle contains all the grids in the workspace S _rm , wherein each expanded rectangle uses The number of expansions when the expanded rectangle is generated is characterized, and is located outside the circumscribed rectangle obtained by the projection of the obstacle to be moved O _{bs_mov} on the X _r Y _r plane and within the working space S _rm The grid G _at Values are assigned according to the following formula:

F _at =n _at · _ka

Among them, F _at is the value of the grid Ga _at , _ka is the preset gravitational field coefficient, n _at is the expansion times corresponding to the expansion rectangle containing the minimum area of the grid Ga _at , n _at =1,2,3 ,….

Preferably, in step S826, "for each other object, reassign each grid in the workspace S _rm to generate a grid map of the repulsive force potential field corresponding to the object" specifically includes:

The value of all grids in the circumscribed rectangle obtained by projecting the object on the X _r Y _r plane is assigned as the height of the object along the Z _r axis;

Based on the circumscribed rectangle projected by the object on the X _r Y _{r plane} , it _expands outward in units of _{grids .} A grid is simultaneously expanded in the direction, thereby generating a series of expanded rectangles, until the outermost expanded rectangle contains all the grids in the workspace S _rm , wherein each expanded rectangle uses the The number of expansions is used to characterize, and the value of the grid G _re located outside the circumscribed rectangle obtained by the projection of the object on the X _r Y _r plane and within the working space S _rm is assigned according to the following formula:

Among them, F _re is the value of the grid _Gre , k _r is the preset repulsion field coefficient, O _h is the height of the object, n _re is the expansion times corresponding to the expansion rectangle containing the minimum area of the grid _Gre , n _re = 1,2,3,..., μ is the mean of the given Gaussian distribution, σ is the standard deviation of the given Gaussian distribution.

By adopting the grasping method of the present invention, when there is an obstacle that prevents the target object from being grasped directly, the obstacle removal operation can be performed first, and then the grasping of the target object can be completed, which is useful for the service robot in environments such as home, office, and medical care. Technical support is provided for the crawling jobs under.

Description of drawings

1 is a schematic flowchart of an embodiment of a method for grasping a target object by a vision-based service robot of the present invention;

2a-2e are schematic diagrams of a method for generating a grid map of a gravitational potential field corresponding to an obstacle to be moved according to an embodiment of the present invention.

Detailed ways

Preferred embodiments of the present invention are described below with reference to the accompanying drawings. It should be understood by those skilled in the art that these embodiments are only used to explain the technical principle of the present invention, and are not intended to limit the protection scope of the present invention.

Referring to FIG. 1 , FIG. 1 shows a schematic flowchart of an embodiment of a method for grasping a target object by a vision-based service robot of the present invention. As shown in Figure 1, the grabbing method of this embodiment includes the following steps:

Step S1, the service robot obtains the color image Is and the original three-dimensional point cloud data D _s under the camera coordinate system O _c X _c Y _c Z _c through the Kinect sensor installed on itself, wherein the Kinect sensor is installed obliquely downward _; the camera coordinate system O _c X _c Y _c Z _c is a right-handed system, the origin O _c of the camera coordinate system O _c X _c Y _c Z _c is located in the center of the Kinect sensor, the Y _c axis is perpendicular to the bottom surface of the Kinect sensor and the direction is upward, and the Z _c axis is vertical On the Y _c -axis and in line with the front of the Kinect sensor.

Step S2, based on the color image Is, use the deep learning SSD (Single Shot _multiBox Detector) target detection method to detect the target object, obtain the bounding box of the target object, and then obtain the surrounding frame of the target object from the original three-dimensional point cloud data D _s . The three-dimensional data corresponding to the frame, these three-dimensional data constitute the first point cloud data D _{to of the target object; remove the first point cloud data D to of} the target object in the original three-dimensional point cloud data D _s , and the obtained three-dimensional point cloud data Denoted as the first environmental point cloud data _Drs .

Step S3, according to the coordinate transformation relationship from the camera coordinate system O _c X _c Y _c Z _c to the manipulator coordinate system _Or X _r Y _r Z _r of the service robot, the camera coordinate system O _c X _c Y _c Z _c The first environmental point cloud data _Drs is converted to the robotic arm coordinate system Or X _r Y _r Z _r , and the second environmental point cloud data _Dr is obtained _; similarly, the camera coordinate system O _c X _c Y _c Z _c The first point cloud data D _to of the target object is converted to the robot arm coordinate system _Or X _r Y _r Z _r , and the second point cloud data D _t of the target object is obtained; the robot arm coordinate system _Or X _r Y _r Z _r is a right-handed system, the origin _{Or of the robotic arm coordinate system Or X r Y r Z r is located} at the center of the base of the robotic arm of the service robot, the Z _r axis is perpendicular to the ground and the direction is upward, and the X _r axis is perpendicular to the Z _r axis And it is consistent with the front of the service robot, wherein the bottom surface of the base of the robotic arm of the service robot is parallel to the ground.

In this step, the coordinate transformation relationship from the camera coordinate system O _c X _c Y _c Z _c to the manipulator coordinate system _Or X _r Y _r Z _r is shown in formula (1):

Among them, (x _cp , y _cp , z _cp ) ^T and (x _rp , y _rp , z _rp ) ^T are the points in the original three-dimensional point cloud data D _s in the camera coordinate system O _c X _c Y _c Z _c and The coordinates in the robot arm coordinate system O _r X _r Y _r Z _r , T _m is the hand-eye relationship matrix, and the specific acquisition method of the hand-eye relationship matrix T _m is as follows:

和

t＝1,2,…,n_g，其中n_g为预设的常数，从而得到对应的齐次坐标分别为

和

t＝1,2,…,n_g；基于

和

t＝1,2,…,n_g，以及公式(1)，得到公式(2)：First, a red ball is fixed at the end of the robotic arm. Based on the color image provided by the Kinect sensor, the target recognition algorithm based on HSV (Hue, Saturation, Value) color features is used to identify the red ball, so as to obtain the center point of the red ball. The coordinates (x _c , y _c , z _c ) ^T in the camera coordinate system O _c X _c Y _c Z _c , use this center point to describe the end of the manipulator, and get the end of the manipulator in the camera coordinate system O _c X _c The three-dimensional coordinates in Y _c Z _c are (x _c , y _c , z _c ) ^T , and its homogeneous coordinates are p _c =(x _c , y _c , z _c , 1) ^T ; and use MoveIt! The function package obtains the three-dimensional coordinates (x _{r , y r , z r ) T of the end of the manipulator in the manipulator coordinate system Or X r Y r Z r through the forward} ^kinematics _{solution , and} its homogeneous coordinates are p _r =(x _r , y _r , z _r , 1) ^T ; then control the end of the manipulator to move to different positions to obtain the end of the manipulator in the camera coordinate system O _c X _c Y _c Z _c and the manipulator coordinate system _{Or X r Y} respectively n _g 3D coordinates in _r Z _r

and

t=1,2,...,n _g , where n _g is a preset constant, so the corresponding homogeneous coordinates are obtained as

and

t=1,2,...,n _g ; based on

and

t=1,2,...,n _g , and formula (1), resulting in formula (2):

The hand-eye relationship matrix T _m is calculated as follows:

表示求取矩阵

的伪逆。in,

Indicates to find a matrix

The pseudo-inverse of .

Step S4, in combination with the workspace S _rm of the robotic arm on the X _r Y _r plane, the second environmental point cloud data _Dr is processed in a reduced range, wherein the working space S _rm of the robotic arm on the X _r Y _r plane is a rectangle area, the coordinates of the lower left corner and upper right corner are (x _min , y _min ) and (x _max , y _max ) respectively, and x _min , y _min , x _max and y _max are all preset thresholds; for the second environment point The process of processing cloud data D _r is to remove the data whose values are outside the range of [y _min , y _max ] in the direction of the Y _r axis, and remove the data whose values are outside the range of [x _min , x _max ] in the direction of the X _r axis , obtain the third environmental point cloud data D _f .

Step S5 , based on the third environmental point cloud data D _f , a RANSAC (Random Sample Consensus) algorithm is used to perform plane fitting of the plane where the target object is located to obtain a plane equation of the plane where the target object is located. Specifically include S51-S54:

Step S51, select 3 data from the third environmental point cloud data D _f , and calculate a plane according to the corresponding points of these 3 data;

Step S52, obtain the distance from the corresponding point to the plane of other data in the third environmental point cloud data D _f (data other than the 3 data selected in step S51), and the distance is within the range of d _h . As an interior point, an interior point set corresponding to the plane is formed, where d _h is a preset distance threshold;

Step S53, repeat steps S51-S52, until the ratio of the number of inliers in the inlier set to the number of all data in the third environmental point cloud data D _f exceeds λ or reaches the preset number of iterations M _K ; wherein λ is The preset number ratio threshold;

In step S54, the plane equation corresponding to the maximum number of interior points is used as the plane equation of the plane where the target object is located, and the plane equation is expressed as A _op x+B _op in the coordinate system _Or X _r Y _r Z _r of the robot arm y+C _op ·z+D _op =0, wherein A _op , B _op , C _op , and D _op are the four coefficients of the plane equation.

Step S6, in combination with the plane equation of the plane where the target object is located, remove the data corresponding to the plane and the points below the plane in the third environmental point cloud data D _f to obtain the fourth environmental point cloud data D _o , and then adopt Euclidean aggregation. The clustering algorithm is used to cluster point cloud clusters, and the position and size information of obstacles corresponding to each point cloud cluster are obtained. Specifically, it includes steps S61-S62:

Step S61 , combining the plane equation of the plane where the target object is located, remove the data corresponding to the plane and the points below the plane from the third environmental point cloud data D _f to obtain fourth environmental point cloud data D _o .

Step S62 , based on the fourth environmental point cloud data D _o , use the Euclidean clustering algorithm to cluster point cloud clusters, and obtain the position and size information of obstacles corresponding to each point cloud cluster. Specifically, it includes steps S621-S626:

Step _S621 , for the fourth environmental point cloud data Do, set a cluster set C and a queue Q, initially both the cluster set C and the queue Q are empty sets, and _mark each data in Do as an unprocessed state;

Step S622, judge the point pi corresponding to the data in the fourth environmental point cloud data Do ( _i = ₀ , 1..., N _{d -1} , N _d is the total number of data in Do, where the point _pi The coordinates of the point are the data corresponding to the point in Do), if the data corresponding to the point p _i in the fourth environmental point cloud data _Do is in the processed state, continue to judge the next point of the point p _{i ,} When all the data in the fourth environment point cloud data D _o is in the processed state, jump to step S626; if the data corresponding to the point p _i in the fourth environment point cloud data D _o is in the unprocessed state, then execute the step S626 S623;

Step S623, the queue Q is set to an empty set, the point p _i is put into the queue Q, and the data corresponding to the point in the fourth environment point cloud data D _o is marked as a processed state;

Step S624, read a point from the queue Q, denoted as q, and obtain the distance between the point and the point q corresponding to all the data in the unprocessed state in the fourth environmental point cloud data D _o , when the distance does not exceed d. At _th , the corresponding point is added to the queue Q, and the data corresponding to the point in _{Do is marked as a processed state, where d th is} a preset threshold. Repeat this step until all points in the queue Q are traversed and no new points are added to the queue Q;

Step S625, if the number of points in the queue Q is greater than N _min , record all the points in the queue Q as a point cloud cluster, and put them into the cluster set C, where N _min is a preset threshold, and return to the step S622;

Step S626, obtain the number of point cloud clusters in the cluster set C, denoted as N _c , each point cloud cluster corresponds to an obstacle; the cluster set C is an empty set, that is, N _c =0, which means that no point is generated Cloud clusters; when the cluster set C is not an empty set, the position and size information of the obstacles corresponding to each point cloud cluster are obtained, and the average value of the coordinates of all the points of the corresponding point cloud clusters of the obstacle is recorded as the obstacle The position information of the obstacle; the size information of the obstacle is based on the value of all points in the corresponding point cloud cluster of the obstacle in the X _r axis, Y _r axis, Z _r axis direction, and the difference between the maximum value and the minimum value in the X _r axis direction is calculated. It is recorded as the length of the obstacle along the X _r axis direction, the difference between the maximum value and the minimum value in the Y _r axis direction is recorded as the length of the obstacle along the Y _r axis direction, and the maximum value in the Z _r axis direction and The difference of the minimum value is recorded as the height of the obstacle along the Z _r axis.

Step S7, combining the plane equation of the plane where the target object is located, remove the data corresponding to the plane and the point below the plane from the second point cloud data D _t of the target object, and obtain the third point cloud data D _tr of the target object, The average value of all data in D _tr is recorded as the position information of the target object _; the size information of the target object is based on the values of all data in D _tr in the directions of X _r axis, Y _r axis and Z _r axis. The difference between the maximum value and the minimum value of the target object is recorded as the length of the target object along the X _r -axis direction, the difference between the maximum value and the minimum value in the Y _r -axis direction is recorded as the length of the target object along the Y _r -axis direction, and the Z _r -axis direction is recorded. The difference between the maximum value and the minimum value in the direction is recorded as the height of the target object along the Z _r axis;

Step S8, based on the position and size information of the target object and the position and size information of each obstacle, first move the obstacles that prevent the target object from being directly grasped, and then complete the grasping of the target object. Specifically, it includes steps S81-S82:

Step S81, based on the position and size information of the target object and the position and size information of each obstacle, use MoveIt! The function package determines whether the robot arm of the service robot can directly grab the target object. If it can directly grab the target object, the robot arm will directly follow the MoveIt! The planning result of the function package implements the grasping of the target object; otherwise, step S82 is performed; wherein, the grasping direction of the end of the mechanical arm is vertically downward along the Z _r axis;

In step S82, the obstacles that prevent the target object from being directly grasped are moved, and then the grasping of the target object is completed. Specifically, it includes steps S821-S829:

Step S821, in the third point cloud data D _tr of the target object, select all the data whose value is within [z _max -z _h , z _max ] in the direction of the Z _r axis as the point cloud data of the grasping area of the target object D _gr , where z _max is the maximum value of each data in D _tr in the direction of the Z _r axis, and z _h is a preset threshold;

Step S822, for each obstacle, obtain the minimum value of the distance between each point in the point set corresponding to the point cloud data D _gr of the target object grasping area and each point in the corresponding point cloud cluster of the obstacle. The minimum value is recorded as the distance from the obstacle to the target object, and the distance from the obstacle to the target object and the ID number of the obstacle are combined to form the obstacle influence sequence;

Step S823, for the workspace S _rm of the robot arm on the X _r Y _r plane, divide the grid in units of c _x ×c _x to obtain a grid map of the workspace S _rm of the robot arm on the X _r Y _r plane, wherein c _x is the preset grid division unit, c _x can be divided into x _max -x _min , and c _x can be divided into y _max -y _min (mentioned in the previous step S4 (x _min , y _min ) and (x _max , y _max ) are the coordinates of the lower left corner and upper right corner of the workspace S _rm respectively); project all the points corresponding to the third point cloud data D _tr of the target object and the point cloud clusters corresponding to each obstacle to the coordinates of the robot arm On the X _r Y _r plane of _Or X _r Y _r Z _r , and obtain their corresponding circumscribed rectangles;

Step S824, according to the distance to the target object corresponding to each obstacle in the obstacle influence sequence, select the obstacle with the smallest distance to the target object as the obstacle O _{bs_mov} to be moved this time;

Step S825, assigning an assignment to each grid of the robotic arm in the workspace S _rm of the X _r Y _r plane to generate a grid map of the gravitational potential field corresponding to the obstacle to be moved _{Obs_mov} . The specific assignment method is: assign the values of all grids in the circumscribed rectangle obtained by the projection of the obstacle _{Obs_mov} on the X _r Y _r plane as the height of the obstacle along the Z _r axis; based on the obstacle _{Obs_mov} in the X _r The circumscribed rectangle projected on the Y _r plane expands outward in units of grids, each time a grid is expanded in the positive direction of the X _r axis, the positive direction of the Y _r axis, the negative direction of the X _r axis, and the negative direction of the Y _r axis. grid, thereby generating a series of extended rectangles, until the outermost extended rectangle contains all the grids of the robot arm in the workspace S _rm of the X _r Y _r plane, wherein each extended rectangle is generated with the time of generating the extended rectangle. The number of expansions is characterized, and the value F _at of the grid G _at outside the projected circumscribed rectangle and within S _rm is assigned according to formula (4):

F _at =n _at · _ka (4)

Wherein, _ka is the preset gravitational field coefficient, n _at is the expansion times corresponding to the expansion rectangle containing the minimum area of the grid G _at , n _at =1, 2, 3, . . .

In order to describe the method of generating the grid map of the gravitational potential field more clearly, we give an example here, assuming that the workspace S _rm of the manipulator in the X _r Y _r plane consists of 8 × 6 grids, and the obstacles to be moved are in the X r Y r plane. The circumscribed rectangle projected on the _r Y _r plane includes 3×2 grids. 2a-2e are schematic diagrams of a method for generating a grid map of a gravitational potential field corresponding to an obstacle to be moved. Among them, Figure 2a describes the workspace S _rm of the manipulator in the X _r Y _r plane, which consists of 8 × 6 dotted grids; in Figure 2b, a rectangle including 3 × 2 solid grids represents the obstacle O The enclosing rectangle projected by _{bs_mov} on the X _r Y _r plane, each grid in the enclosing rectangle is assigned the height of the obstacle along the Z _r axis, which is 0.1 m in this example; in Figure 2c, including 5× The rectangle of the four solid-line grids represents the expanded rectangle obtained after the first expansion, and the expanded rectangle is represented by the expansion times 1 of the expanded rectangle. The expanded grid is assigned a value of 1× _ka ; in Fig. In 2d, the rectangle including 7×6 solid-line grids represents the expanded rectangle obtained after the second expansion, and the expanded rectangle is represented by the expansion times 2 of the expanded rectangle. Some of the expanded grids have been expanded this time. Outside the workspace S _rm , but we don't care about this part of the grid, and only assign a value of 2 × _ka to the grid located within the workspace S _rm ; in Figure 2e, including 9 × 8 solid grids The rectangle of the grid represents the expanded rectangle obtained after the third expansion, and the expanded rectangle is represented by the expansion times 3 of the expanded _rectangle . Grid assignment, the assignment is 3× _ka . It can be seen that after 3 expansions, the outermost expansion rectangle has included all the grids in the workspace S _rm of the manipulator in the X _r Y _r plane, and all the grids with assigned values in Figure 2e are composed together The grid map of the gravitational potential field corresponding to the obstacle to be moved in this example is shown.

Step S826, for each other object (including other obstacles and target objects except the obstacle to be moved O _{bs_mov} ), reassign each grid of the robotic arm in the workspace S _rm of the X _r Y _r plane , generate a grid map of the repulsive potential field corresponding to the object. The specific assignment method is: assign the value of all grids in the circumscribed rectangle obtained by projecting the object on the X _r Y _r plane as the height of the object along the Z _r axis; based on the projection of the object on the X _r Y _r plane The obtained circumscribed rectangle expands outward in units of grids, and expands one grid at the same time in the positive direction of the X _r axis, the positive direction of the Y _r axis, the negative direction of the X _r axis, and the negative direction of the Y _r axis, thereby generating a grid. A series of expansion rectangles until the outermost expansion rectangle contains all the grids of the robot arm in the workspace S _rm of the X _r Y _r plane, wherein each expansion rectangle is characterized by the number of expansions when the expansion rectangle is generated , the value _Fre of the grid _Gre outside the projected circumscribed rectangle and within S _rm is assigned according to formula (5):

Among them, k _r is the preset repulsion field coefficient, Oh is the height of the object, _{n re} is the expansion times corresponding to the expansion rectangle containing the minimum area of the grid _{Gre, n re =} 1,2,3,..., μ is the mean of the given Gaussian distribution, σ is the standard deviation of the given Gaussian distribution;

Step S827, superimpose the gravitational potential field grid diagram corresponding to the obstacle O _{bs_mov} to be moved and the repulsive force potential field grid diagrams generated by other obstacles and the target object respectively (that is, add the values of the same grid in the above grid diagram. ) to generate the final potential field grid map, select the position corresponding to the grid with the smallest value in the potential field grid map as the position to be placed for the obstacle O _{bs_mov} , if there is more than one grid with the smallest value, select the The position corresponding to the nearest grid from the center point of the circumscribed rectangle obtained by the projection of the obstacle O _{bs_mov} on the X _r Y _r plane is taken as the to-be-placed position of the obstacle O _{bs_mov} ;

Step S828, based on the current position and size information of the obstacle to be moved _{Obs_mov} , as well as the position and size information of the obstacle to be moved, other obstacles and the position and size information of the target object, use MoveIt! The function package performs the motion planning of the manipulator and completes the movement of the obstacle O _{bs_mov} (so far, the "position to be placed" mentioned above has been renamed the "new position" of the obstacle, and the obstacle O _{bs_mov} is also changed from "the obstacle to be moved""object" was renamed "moved obstacle"); the obstacle O _{bs_mov} was moved out of the obstacle influence sequence, and the position of the center point of the circumscribed rectangle obtained by the projection of the obstacle on the X _r Y _r plane was moved by the obstacle. be updated with the new location;

Step S829, based on the position and size information of the target object, the new position and size information of the moved obstacle O _{bs_mov} , and the current position and size information of other obstacles, through MoveIt! The function package judges whether the target object can be directly grabbed, if the target object cannot be grabbed directly, return to step S824; otherwise, go through MoveIt! The function package performs the motion planning of the robotic arm and controls the robotic arm to complete the grasping of the target object.

In a specific embodiment, the Kinect sensor is installed obliquely downward at a tilt angle of 45°, ng = 15, _{x min} = 0.1 m, x _max = 0.5 m, y _min = -0.25 m, y _max = 0.25 m , dh = 0.01 m, λ = 0.5, M _K = 100, d _th = 0.02 m, N _min = 80, _zh = 0.05 m, c _{x =} 0.01 m, _ka = 0.015, k _r = 0.1, μ =0, σ=10.

By adopting the grasping method of the present invention, when there is an obstacle that prevents the target object from being grasped directly, the obstacle removal operation can be performed first, and then the grasping of the target object can be completed, which is useful for the service robot in environments such as home, office, and medical care. Provide technical support for the grabbing operation below, which can achieve better technical results.

In the above-mentioned embodiment, although each step is described according to the above-mentioned order, those skilled in the art can understand that, in order to realize the effect of this embodiment, different steps need not be performed in this order, and it can be performed simultaneously ( parallel) or in reverse order, simple variations of these are within the scope of the present invention.

Those skilled in the art should be aware that the method steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two, in order to clearly illustrate the possibilities of electronic hardware and software. Interchangeability, the above description has generally described the components and steps of each example in terms of functionality. Whether these functions are performed in electronic hardware or software depends on the specific application and design constraints of the technical solution. Skilled artisans may use different methods of implementing the described functionality for each particular application, but such implementations should not be considered beyond the scope of the present invention.

So far, the technical solutions of the present invention have been described with reference to the preferred embodiments shown in the accompanying drawings, however, those skilled in the art can easily understand that the protection scope of the present invention is obviously not limited to these specific embodiments. Without departing from the principle of the present invention, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will fall within the protection scope of the present invention.

Claims (10)

1. a method based on vision-based service robot grabbing target object, is characterized in that, comprises the following steps: Step S1, obtain the color image Is and the original three-dimensional point cloud data D _s under the camera coordinate system O _c X _c Y _c Z _c through the Kinect sensor installed on the service robot _; Step S2, based on the color image Is, utilizes the deep learning SSD target detection method _to detect the target object, and then obtains the first point cloud data _Dto of the target object from the original three-dimensional point cloud data _Ds ; removing the first point cloud data D _to of the target object from the original three-dimensional point cloud data D _s to obtain the first environment point cloud data D _rs ; Step S3, according to the coordinate transformation relationship from the camera coordinate system O _c X _c Y _c Z _c to the manipulator coordinate system _Or X _r Y _r Z _r of the service robot, transform the first environmental point cloud data _Drs Under the coordinate system Or X _r Y _r Z _r of the robot arm, the second environmental point cloud data D _r is obtained _; the first point cloud data D _to of the target object is converted to the coordinate system _Or of the robot arm Under X _r Y _r Z _r , obtain the second point cloud data D _t of the target object; Step S4, in combination with the working space S _rm of the robotic arm on the X _r Y _r plane, for the second environmental point cloud data D _r , remove the data whose values in the Y _r axis direction are outside the range of [y _min , y _max ] , and remove the data whose values in the X _r axis direction are outside the range of [x _min , x _max ] to obtain the third environmental point cloud data D _f ; wherein, the coordinates of the lower left corner and the upper right corner of the workspace S _rm are respectively (x _min , y _min ) and (x _max , y _max ), x _min , y _min , x _max and y _max are all preset thresholds; Step S5, based on the third environmental point cloud data D _f , using a random sampling consistency algorithm to fit the plane where the target object is located, to obtain a plane equation of the plane where the target object is located; Step S6, in combination with the plane equation of the plane where the target object is located, remove the data corresponding to the plane and the point below the plane in the third environmental point cloud data D _f to obtain the fourth environmental point cloud data D _o , Then perform point cloud cluster clustering, and obtain the position and size information of obstacles corresponding to each point cloud cluster; Step S7, in combination with the plane equation, remove the plane and the data corresponding to the point below the plane in the second point cloud data D _t of the target object to obtain the third point cloud data D _tr of the target object , and obtain the position and size information of the target object; Step S8, based on the position and size information of the target object and the position and size information of each obstacle, first move the obstacles that prevent the target object from being directly grasped, and then complete the grasping of the target object. Pick. 2. The method according to claim 1, wherein the coordinate transformation from the camera coordinate system O _c X _c Y _c Z _c to the robotic arm coordinate system _Or X _r Y _r Z _r of the service robot in step S3 The relationship is:

Wherein, (x _cp , y _cp , z _cp ) ^T and (x _rp , y _rp , z _rp ) ^T are respectively the points in the original 3D point cloud data D _s in the camera coordinate system O _c X _c Y _c Z _c and the coordinates in the robotic arm coordinate system _Or X _r Y _r Z _r , T _m is the hand-eye relationship matrix; The calculation method of the hand-eye relationship matrix includes: Control the end of the manipulator to move to different positions, and obtain the n _g of the end of the manipulator in the camera coordinate system O _c X _c Y _c Z _c and the manipulator coordinate system _Or X _r Y _r Z _r respectively three-dimensional coordinates

and

The corresponding homogeneous coordinates are

and

Among them, n _g is a preset constant, t=1,2,...,n _g ; The hand-eye relationship matrix is calculated according to the following formula:

in,

Indicates to find a matrix

The pseudo-inverse of . 3. The method according to claim 2, wherein the method for obtaining the coordinates of the end of the robotic arm in the camera coordinate system O _c X _c Y _c Z _c is: A red ball is fixed at the end of the robotic arm, and based on the color image provided by the Kinect sensor, the target recognition algorithm based on color features is used to identify the red ball, so as to obtain the center point of the red ball at the camera coordinates The coordinates (x _c , y _c , z _c ) ^T in the system O _c X _c Y _c Z _c are taken as the coordinates of the end of the robotic arm in the camera coordinate system O _c X _c Y _c Z _c . 4. The method according to claim 2, wherein the method for obtaining the coordinates of the end of the manipulator in the coordinate system of the manipulator _Or X _r Y _r Z _r is specifically: Take advantage of MoveIt! The function package obtains the three-dimensional coordinates (x _{r , y r , z r ) T of the end of the manipulator in the coordinate system Or X r Y r Z r of the manipulator} ^through _{forward kinematics solution} . 5. The method according to claim 1, characterized in that, in step S5 "based on the third environmental point cloud data D _f , a random sampling consistency algorithm is used to fit the plane where the target object is located, and the obtained result is obtained. Describe the plane equation of the plane where the target object is located", including: Step S51, select 3 data from the third environmental point cloud data D _f , and calculate a plane according to the corresponding points of the 3 data; Step S52, obtain the distance from the point corresponding to the data other than the 3 data in the third environmental point cloud data D _f to the plane, and use the point within the range of d _h as the inner point to form the plane corresponding to The interior point set of , where d _h is the preset distance threshold; Step S53, repeating steps S51-S52, until the ratio of the number of inliers in the inlier set to the number of all data in the third environmental point cloud data _Df exceeds λ or reaches a preset number of iterations _MK ; where λ is the threshold of the preset quantity ratio; Step S54, the plane equation corresponding to the maximum number of interior points is used as the plane equation of the plane where the target object is located, and the plane equation is expressed in the robotic arm coordinate system _Or X _r Y _r Z _r as: A _op x+B _op y+C _op z+D _op =0 Among them, A _op , B _op , C _op , and D _op are the four coefficients of the plane equation. 6. method according to claim 1, is characterized in that, in step S6 " in conjunction with the plane equation of the plane where described target object is located, remove this plane and the plane below this plane in described third environment point cloud data D _f . According to the data corresponding to the points, the fourth environmental point cloud data D _o is obtained, and then the point cloud clusters are clustered, and the position and size information of the obstacles corresponding to each point cloud cluster are obtained, including: Step S61, in conjunction with the plane equation of the plane where the target object is located, remove the data corresponding to the plane and the point below the plane in the third environmental point cloud data D _f to obtain the fourth environmental point cloud data D _o ; Step S62, based on the fourth environmental point cloud data D _o , use the Euclidean clustering algorithm to perform point cloud cluster clustering, and obtain the position and size information of obstacles corresponding to each point cloud cluster; Wherein, step S62 specifically includes: Step _S621 , for the fourth environmental point cloud data Do, set a cluster set C and a queue Q, initially the cluster set C and the queue Q are empty sets, and _mark each data in Do is unprocessed; Step S622, judging the point p _i corresponding to the data in the fourth environment point cloud data D _o , if the data corresponding to the point p _i in the fourth environment point cloud data D _o is in the processed state, then Continue to judge the next point of point p _i ; when all the data in the fourth environment point cloud data D _o are in the processed state, go to step S626; if point p _i is in the fourth environment point cloud data D o If the corresponding data in _o is in an unprocessed state, go to step S623; wherein, i= ₀ , 1..., N _d -1, and N _d is the total number of data in Do; Step S623, set the queue Q as an empty set, put the point p _i in the queue Q, and mark the data corresponding to the point in the fourth environment point cloud data D _o as a processed state; Step S624, read a point from the queue Q, denoted as q, and obtain the distance between the point and the point q corresponding to all the data in the unprocessed state in the fourth environment point cloud data D _o , when When the distance does not exceed _{d th} , the corresponding point is added to the queue Q, and the data corresponding to the point in Do is marked as a processed state; the execution is repeated until all points in the queue Q are traversed. Complete and no new points are added to the queue Q; wherein, d _th is a preset threshold; Step S625, if the number of points in the queue Q is greater than N _min , record all the points in the queue Q as a point cloud cluster and put it into the cluster set C; go to step S622; wherein, N _min is the preset threshold; Step S626, determine whether the cluster set C is an empty set, if not, calculate the position and size information of the obstacle corresponding to each point cloud cluster. 7. The method according to claim 1, characterized in that, in step S8, "based on the position and size information of the target object and the position and size information of each obstacle, the first step is to prevent the target object from being directly grasped. Move the picked obstacle, and then complete the grasp of the target object”, which includes: Step S81, based on the position and size information of the target object and the position and size information of each obstacle, use MoveIt! The function package determines whether the target object can be directly grabbed, and if so, according to MoveIt! The planning result of the function package implements the grasping of the target object; otherwise, step S82 is performed; wherein, the grasping direction of the end of the mechanical arm is vertically downward grasping along the Z _r axis; In step S82, the obstacles that prevent the target object from being directly grasped are moved, so as to complete the grasping of the target object. 8. The method according to claim 7, characterized in that, in step S82, "moving obstacles that prevent the target object from being directly grasped, so as to complete the grasping of the target object", specifically includes: Step S821, in the third point cloud data D _tr of the target object, select all the data whose value is within [z _max -z _h , z _max ] in the direction of the Z _r axis as the target object grasping area The point cloud data D _gr ; wherein, z _max is the maximum value of each data in D _tr in the direction of the Z _r axis, and z _h is a preset threshold; Step S822, for each obstacle, obtain the minimum value of the distance between each point in the point set corresponding to the point cloud data D _gr of the target object grasping area and each point in the corresponding point cloud cluster of the obstacle. , the minimum value is recorded as the distance from the obstacle to the target object, and the distance from the obstacle to the target object and the ID number of the obstacle are combined to form an obstacle influence sequence; Step S823, divide the working space S _rm of the robotic arm on the X _r Y _r plane into a grid to obtain a grid map of the working space S _rm ; respectively divide the third point cloud data D _tr of the target object corresponding to All points and point cloud clusters corresponding to obstacles are projected onto the X _r Y _{r plane of the robotic arm coordinate system Or X r Y r Z r , thereby} obtaining the corresponding circumscribed rectangle; Step S824, according to the distance to the target object corresponding to each obstacle in the obstacle influence sequence, select the obstacle with the smallest distance to the target object as the obstacle to be moved O _{bs_mov} ; Step S825, assigning a value to each grid in the workspace S _rm to generate a grid map of the gravitational potential field corresponding to the obstacle to be moved O _{bs_mov} ; Step S826, for each other object, reassign each grid in the workspace S _rm to generate a grid map of the repulsion potential field corresponding to the object; wherein, the other objects include: in addition to the to-be-moved Obstacles other than the obstacle O _{bs_mov} and the target object; Step S827, superimpose the gravitational potential field grid map corresponding to the obstacle to be moved O _{bs_mov} and the repulsive force potential field grid map of the other objects to generate the final potential field grid map; select the potential field grid map The position corresponding to the grid with the smallest numerical value in the figure is used as the position to be placed of the obstacle to be moved O _{bs_mov} ; if the number of grids with the smallest numerical value is greater than 1, then select the distance from the obstacle to be moved O _{bs_mov} at X _r Y The position corresponding to the grid with the smallest numerical value of the nearest center point of the circumscribed rectangle projected on the _r plane is used as the position to be placed of the obstacle to be moved O _{bs_mov} ; Step S828, based on the current position and size information of the obstacle to be moved _{Obs_mov} , the position to be placed of the obstacle to be moved _{Obs_mov} , other obstacles and the position and size information of the target object, use MoveIt! The function package performs the motion planning of the manipulator, and completes the transfer of the obstacle to be moved _{Obs_mov} ; the obstacle to be moved _{Obs_mov} is renamed the obstacle to be moved _{Obs_mov} , and the to-be-placed position of the obstacle Renamed as a new position; move the moved obstacle O _{bs_mov} out of the obstacle influence sequence, the position of the center point of the circumscribed rectangle obtained by the projection of the obstacle on the X _r Y _r plane is used after the obstacle has been moved. be updated with the new location; Step S829, based on the position and size information of the target object, the new position and size information of the moved obstacle O _{bs_mov} , and the current position and size information of other obstacles, through MoveIt! The function package judges whether the target object can be directly grabbed, and if so, use MoveIt! The function package performs motion planning of the robotic arm, and then grasps the target object; otherwise, go to step S824. 9. method according to claim 8, is characterized in that, in step S825 " assigns value to each grid in described workspace S _rm , generates the gravitational potential field grid corresponding to described obstacle to be moved O _{bs_mov} The grid diagram specifically includes: The numerical value of all grids in the circumscribed rectangle obtained by the projection of the obstacle to be moved O _{bs_mov} on the X _r Y _r plane is assigned as the height of the obstacle along the Z _r axis direction; Based on the circumscribed rectangle obtained by the projection of the obstacle to be moved O _{bs_mov} on the X _r Y _r plane, expand outward in the unit of grid, each time in the positive direction of the X _r axis, the positive direction of the Y _r axis, and the negative direction of the X _r axis A grid is simultaneously expanded in the negative direction of the Y _r -axis, thereby generating a series of expanded rectangles until the outermost expanded rectangle contains all the grids in the workspace S _rm , wherein each expanded rectangle uses The number of expansions when the expanded rectangle is generated is characterized, and is located outside the circumscribed rectangle obtained by the projection of the obstacle to be moved O _{bs_mov} on the X _r Y _r plane and within the working space S _rm The grid G _at Values are assigned according to the following formula: F _at =n _at · _ka Among them, F _at is the value of the grid Ga _at , _ka is the preset gravitational field coefficient, n _at is the expansion times corresponding to the expansion rectangle containing the minimum area of the grid Ga _at , n _at =1,2,3 , . . . 10. The method according to claim 8, characterized in that, in step S826, "for each other object, reassign each grid in the workspace S _rm to generate a repulsion potential field grid corresponding to the object. The grid diagram specifically includes: The value of all grids in the circumscribed rectangle obtained by projecting the object on the X _r Y _r plane is assigned as the height of the object along the Z _r axis; Based on the circumscribed rectangle projected by the object on the X _r Y _{r plane} , it _expands outward in units of _{grids .} A grid is simultaneously expanded in the direction, thereby generating a series of expanded rectangles, until the outermost expanded rectangle contains all the grids in the workspace S _rm , wherein each expanded rectangle uses the The number of expansions is used to characterize, and the value of the grid G _re located outside the circumscribed rectangle obtained by the projection of the object on the X _r Y _r plane and within the working space S _rm is assigned according to the following formula:

Among them, F _re is the value of the grid _Gre , k _r is the preset repulsion field coefficient, O _h is the height of the object, n _re is the expansion times corresponding to the expansion rectangle containing the minimum area of the grid _Gre , n _re = 1,2,3,..., μ is the mean of the given Gaussian distribution, σ is the standard deviation of the given Gaussian distribution.

Images