CN111055281B - ROS-based autonomous mobile grabbing system and method - Google Patents

ROS-based autonomous mobile grabbing system and method Download PDF

Info

Publication number
CN111055281B
CN111055281B CN201911320327.6A CN201911320327A CN111055281B CN 111055281 B CN111055281 B CN 111055281B CN 201911320327 A CN201911320327 A CN 201911320327A CN 111055281 B CN111055281 B CN 111055281B
Authority
CN
China
Prior art keywords
pose
grabbing
mechanical arm
ros
delta
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201911320327.6A
Other languages
Chinese (zh)
Other versions
CN111055281A (en
Inventor
杨宇翔
孙卫军
高明煜
董哲康
林辉品
曾毓
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Dianzi University
Original Assignee
Hangzhou Dianzi University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hangzhou Dianzi University filed Critical Hangzhou Dianzi University
Priority to CN201911320327.6A priority Critical patent/CN111055281B/en
Publication of CN111055281A publication Critical patent/CN111055281A/en
Application granted granted Critical
Publication of CN111055281B publication Critical patent/CN111055281B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J5/00—Manipulators mounted on wheels or on carriages
    • B25J5/007—Manipulators mounted on wheels or on carriages mounted on wheels
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems

Abstract

The invention relates to an autonomous mobile grabbing system and method based on ROS. The invention can be divided into an image acquisition module, a visual algorithm processing module, a pose detection module and a capture control module. The method realizes the functions of environment perception, visual navigation and real-time object pose estimation and grabbing of the mobile robot in strange environment. The mobile robot is combined with the machine vision, so that the robot can complete more intelligent tasks, good service is provided, and unnecessary burden is reduced. The invention has the advantages of high efficiency, high detection precision, real-time detection, good adaptability and the like, can release the human input in daily work to a great extent, and has strong market potential. The multifunctional desk is greatly convenient for people to work and live, and is beneficial to promoting the improvement of living standard.

Description

ROS-based autonomous mobile grabbing system and method
Technical Field
The invention belongs to the field of machine vision, and particularly relates to an autonomous mobile grabbing system and method based on ROS.
Background
With the growing aging problem and the compelling society to transfer more manpower, the service robots can greatly release the manpower input of daily work in consideration of the life of the old people through material resources, but at present, most service robots are single-function and conversational robots, and mobile robots with anthropomorphic, autonomous environment perception and visual guidance tasks are still in the research stage, are not mature enough and are difficult to complete complex integrated tasks. In addition, the mobile robot is more and more widely applied to disaster rescue, military investigation and other places. For high-risk places, the use of the mobile robot can reduce casualties of people to a great extent, but how to integrate the visual sensing technology with methods such as automatic control, motion planning and visual navigation, and perfect self-gripping is a key technology in the field of mobile robots. Therefore, the autonomous mobile grabbing system capable of autonomously sensing the environment, detecting in real time, autonomously moving and executing related tasks is developed, the manual labor is reduced, the safety of high-risk operation is improved, and the system has important theoretical value and practical significance.
Disclosure of Invention
In view of the above-mentioned shortcomings of the background art, the present invention is directed to an autonomous mobile grabbing system based on ROS, which accomplishes the sensing of the environment and the object grabbing function through image acquisition, visual algorithm processing, visual navigation and visual grabbing. The visual sensing technology, the visual navigation, the motion planning, the automatic control technology and the like are well integrated. Unnecessary personnel operation is saved. Unnecessary human input is liberated. Has good engineering significance.
An autonomous mobile grabbing system based on ROS is divided into four parts: the system comprises an image acquisition module, a visual navigation module, a pose detection module and a capture control module; the image acquisition module consists of an RGB-D camera, an NVIDIA Jetson TX2 and a rotating tripod head module, the RGB-D camera and the NVIDIA Jetson TX2 are responsible for acquiring color images and depth images, the rotating tripod head control module consists of a motor and a Bluetooth control panel, the tripod head is controlled to control the camera to rotate to the horizontal direction when a visual navigation task is executed, and the tripod head is controlled to control the camera to rotate to the vertical direction when a pose detection task is executed; the visual navigation module consists of a mobile trolley, an infrared sensor and an NVIDIA GeForce GTX 1080Ti server; the method comprises the following steps that an RGB-D camera collects scene color images and depth images and transmits the scene color images and the depth images to a server, the server executes an SLAM algorithm to build images and uses a move _ base library to control a mobile trolley to navigate based on ROS, and an infrared sensor is used for avoiding obstacles in the navigation process; the pose detection module is completed by an NVIDIA GeForce GTX 1080Ti server, an RGB-D camera collects a target color image and a target depth image and transmits the target color image and the target depth image to the server, and the server executes the coordinates of a PVnet regression key point of a pixel voting neural network and calculates the 6D pose of an object based on a PnP algorithm; the grabbing control module comprises a mechanical arm with 5 degrees of freedom and a controller thereof, the controller receives a target grabbing pose, and a Moveit motion planning algorithm library of the ROS is used for planning a mechanical arm motion path and executing a grabbing task; the ROS is a Robot Operating System; the 6D pose includes a 3D position and a 3D orientation.
A grabbing method of an autonomous mobile grabbing system based on ROS comprises the following steps:
the method comprises the following steps: placing the trolley in a strange environment, and acquiring the color image and the depth image by the image acquisition module;
step two: the color image and the depth image acquired by the image acquisition module are transmitted to the visual navigation module through the ROS and execute the SLAM algorithm to construct a map; marking a target position area by using a two-dimensional code icon, determining the target area by detecting the pose of the two-dimensional code icon, and autonomously moving the mobile car to the vicinity of the target area according to the path planned by the move _ base motion navigation library of the ROS;
the map construction is based on an RGBD-SLAM-V2 framework, a front-end vision odometer extracts features from each frame of color image, the pose relationship between two frames of images is calculated by using an RANSAC + ICP algorithm by means of an ORB feature descriptor, then the rear-end and loop detection optimization based on a g2o optimization library is carried out, and a dense map is generated by combining the optimized pose with a color image and a depth image; then, the position and the pose of the target area are found through the two-dimensional code icon marks
Figure BDA0002326965470000021
The moving trolley receives
Figure BDA0002326965470000022
Navigating to the target area by using the move _ base motion navigation library of the ROS;
step three: after the target area is reached, sending an instruction to a Bluetooth control panel through a message queue telemetry transmission protocol to control the steering of a camera, then preprocessing a color image acquired by an image acquisition module, and sending the preprocessed color image to a position and posture detection module through an ROS (reactive oxygen species) module;
the preprocessing of the color image specifically comprises the following steps:
f (x, y, z) represents a frame image with the size of H multiplied by W, x, y represent horizontal pixel coordinates and vertical pixel coordinates of the image, z represents the number of channels, and the color map z is 3, and when the color map is used for pose detection, the average value and variance normalization processing of a data set is used for the color map:
f(x,y,z)＝(f(x,y,z)-mean[z])/std[z]z＝3 (1)
wherein mean ═ 0.485,0.456,0.406] std ═ 0.229,0.224,0.225 ];
step four: the pose detection module calculates the 6D pose of the object through a pose detection algorithm, converts the pose into a target grabbing pose and sends the target grabbing pose to the grabbing control module through an ROS (reactive oxygen species) system;
the pose detection module calculates a 6D pose of the object through a pose detection algorithm, and specifically comprises the following steps:
constructing an improved pixel-level voting neural network PVnet, which specifically comprises the following steps:
the input f (x, y, z) of PVnet is a color map with the size of H multiplied by W, a pre-trained Resnet-18 is used as a basic network, when the size of the feature map is H/8 multiplied by W/8, an associated spatial gradient posing module with three different void rates is used for extracting features, residual error connection is repeatedly used in a convolution layer and an upper sampling layer, and then up sampling is carried out until the size of the output reaches H multiplied by W; and outputting a pixel-level prediction direction vector and a semantic segmentation result by using a convolution layer of 1 multiplied by 1 in the feature map of the last layer. The specific ASPP module is that convolution kernels with different voidage rates are used for processing feature maps, then the feature maps are connected together to expand the number of channels, and finally an output feature map with the same size is obtained through a 1x 1 convolution layer.
(a) Sending the input picture f (x, y, z) into a pixel-level voting neural network PVnet regression to obtain a semantic segmentation result and a pixel-level prediction direction vector, wherein the specific direction vector is defined as:
for a pixel p, the network outputs an object label associated therewith and a representation of 2D keypoints from the pixel p to the object
Figure BDA0002326965470000031
A unit direction vector of (a); v. ofk(p) is defined as:
Figure BDA0002326965470000032
(b) voting positioning key point coordinate
Given semantic tags and unit vectors, keypoint hypotheses are generated using a RANSAC-based voting scheme, first using termsThe semantic label finds a pixel p belonging to the target object, then two pixels are randomly selected, and a prediction unit direction vector v corresponding to the two pixels is determinedk(p) intersection as a key point
Figure BDA0002326965470000033
Hypothesis h ofk,iRepeating N times to generate a series of predictions of keypoints { hk,i1,2,3, N, for the hypothesis hk,iScore w of votek,iIs defined as:
Figure BDA0002326965470000034
wherein theta is a threshold value, p belongs to O and represents that a pixel point p belongs to an object O, and the hypothesis with the highest score is selected as the predicted value of the 2D coordinate of the key point
Figure BDA0002326965470000035
Wherein the pose is to be estimated
Figure BDA0002326965470000036
Convert to grab pose pgraspThe method specifically comprises the following steps:
according to the obtained 2D key point coordinates
Figure BDA0002326965470000041
And three-dimensional point coordinates of corresponding key points on the known object model
Figure BDA0002326965470000042
Obtaining an estimated pose relative to a camera coordinate system by using a PnP algorithm according to the corresponding relation of 2D-3D
Figure BDA0002326965470000043
Hand-eye matrix obtained according to hand-eye calibration
Figure BDA0002326965470000044
Will be provided with
Figure BDA0002326965470000045
Transformed to pose relative to base coordinates of the mechanical arm
Figure BDA0002326965470000046
According to the position and posture
Figure BDA0002326965470000047
Obtaining a coordinate system C relative to the base of the mechanical armbaseEstimated object coordinate system of
Figure BDA0002326965470000048
A top-down grabbing mode is adopted, when the gripper at the tail end of the mechanical arm is vertically downward, the Z axis of a coordinate system at the tail end of the mechanical arm is also vertically downward, and the Z axis of a coordinate system at the base end of the mechanical arm is vertically upward; subject coordinate system CobjectAnd a robot base coordinate system CbaseThe original points are overlapped through displacement; the unit vector of the Z axis of the base coordinate system is
Figure BDA0002326965470000049
Object coordinate system CobjectThe unit vector of each axis is
Figure BDA00023269654700000410
Is calculated to obtain
Figure BDA00023269654700000411
And
Figure BDA00023269654700000412
the included angle between:
Figure BDA00023269654700000413
according to
Figure BDA00023269654700000414
Further obtaining acute angles theta between the vectorsx,θy,θz：
Figure BDA00023269654700000415
Then take the minimum thetaiThe corresponding axis is the final transformation axis, i is min (θ)i)
(b) Assuming an estimated object attitude coordinate system CobjectAnd a robot base coordinate system CbaseThe transformation relationship between the rotation matrix is R and the displacement is T
Figure BDA00023269654700000416
And obtaining Euler angles corresponding to all the axes according to the rotation matrix as follows:
Figure BDA00023269654700000417
when the rotation axis i is known from (a), the corresponding rotation angle θ is obtained from the equation (6)iRotating the base coordinate system of the mechanical arm to a Z axis vertically downwards, and then rotating the Z axis by thetaiAccording to
Figure BDA0002326965470000051
The final grabbing pose p can be obtained by making corresponding displacement on the displacement T in the middlegrasp；
Step four: after the mechanical arm controller receives the target grabbing pose, planning mechanical arm movement grabbing through a Moveit movement planning library of the ROS; if the mechanical arm cannot reach the target pose, the position of the trolley is adjusted correspondingly according to the feedback information, and the detection and the grabbing are carried out again until the grabbing is finished, or no object exists in the feedback;
the obtained grabbing pose pgraspThe information is sent to a mechanical arm controller of the grabbing control module in a topic form through the ROS; the mechanical arm receives the grabbing gesture pgraspAnd then, subtracting the target displacements delta x, delta y and delta z from the extreme displacement of the tail end of the mechanical arm, and judging pgraspWhether or not on the displacementExceeds the motion space of the mechanical arm; if the delta x, the delta y and the delta z are positive, the mechanical arm plans the motion of the mechanical arm through a Moveit motion planning library of the ROS, and a grabbing middle position p is set before the mechanical arm moves to a target positiongrasp_mid，pgrasp_midIs formed by pgraspIs translated upwards for a certain distance to obtain a placing position p after being grabbedplaceAlso past this intermediate position pgraspThen reaches the set placing position pplace(ii) a If the sizes of the delta x, the delta y and the delta z are negative numbers, and the sizes of the delta x, the delta y and the delta z are within the threshold values, corresponding direction movement can be carried out to enable the delta x, the delta y and the delta z to be positive, trial grabbing is carried out again, and the final visual grabbing task is completed; if one of the magnitudes of Δ x, Δ y, and Δ z is outside the threshold, no object is present in the feedback.
The invention has the beneficial effects that: the method realizes the functions of sensing unknown environment and moving and grabbing objects by adopting the modes of image acquisition, visual algorithm processing, visual navigation, visual grabbing and the like, integrates information fusion of a plurality of sensors, completes distributed design, has strong execution capacity and definite functional module division, can quickly identify targets in the environment and react, and can replace manual work to complete specific tasks. And manual labor is liberated to a certain extent. The invention has the advantages of autonomous perception, high detection precision, real-time detection of the pose of the target and the like, and has considerable theoretical value and practical value.
The specific implementation mode is as follows:
an autonomous mobile grabbing system based on ROS is divided into four parts: the system comprises an image acquisition module, a visual navigation module, a pose detection module and a capture control module; the image acquisition module consists of an RGB-D camera, an NVIDIA Jetson TX2 and a rotating tripod head module, the RGB-D camera and the NVIDIA Jetson TX2 are responsible for acquiring color images and depth images, the rotating tripod head control module consists of a motor and a Bluetooth control panel, the tripod head is controlled to control the camera to rotate to the horizontal direction when a visual navigation task is executed, and the tripod head is controlled to control the camera to rotate to the vertical direction when a pose detection task is executed; the visual navigation module consists of a mobile trolley, an infrared sensor and an NVIDIA GeForce GTX 1080Ti server; the method comprises the following steps that an RGB-D camera collects scene color images and depth images and transmits the scene color images and the depth images to a server, the server executes an SLAM algorithm to build images and uses a move _ base library to control a mobile trolley to navigate based on ROS, and an infrared sensor is used for avoiding obstacles in the navigation process; the pose detection module is completed by an NVIDIA GeForce GTX 1080Ti server, an RGB-D camera collects a target color image and a target depth image and transmits the target color image and the target depth image to the server, and the server executes the coordinates of a PVnet regression key point of a pixel voting neural network and calculates the 6D pose of an object based on a PnP algorithm; the grabbing control module comprises a mechanical arm with 5 degrees of freedom and a controller thereof, the controller receives a target grabbing pose, and a Moveit motion planning algorithm library of the ROS is used for planning a mechanical arm motion path and executing a grabbing task; the ROS is a Robot Operating System; the 6D pose includes a 3D position and a 3D orientation.
A grabbing method of an autonomous mobile grabbing system based on ROS comprises the following steps:
the method comprises the following steps: placing the trolley in a strange environment, and acquiring the color image and the depth image by the image acquisition module;
step two: the color image and the depth image acquired by the image acquisition module are transmitted to the visual navigation module through the ROS and execute the SLAM algorithm to construct a map; marking a target position area by using a two-dimensional code icon, determining the target area by detecting the pose of the two-dimensional code icon, and autonomously moving the mobile car to the vicinity of the target area according to the path planned by the move _ base motion navigation library of the ROS;
the map construction is based on an RGBD-SLAM-V2 framework, a front-end vision odometer extracts features from each frame of color image, the pose relationship between two frames of images is calculated by using an RANSAC + ICP algorithm by means of an ORB feature descriptor, then the rear-end and loop detection optimization based on a g2o optimization library is carried out, and a dense map is generated by combining the optimized pose with a color image and a depth image; then, the position and the pose of the target area are found through the two-dimensional code icon marks
Figure BDA0002326965470000061
The moving trolley receives
Figure BDA0002326965470000062
Navigating to the target area by using the move _ base motion navigation library of the ROS;
step three: after the target area is reached, sending an instruction to a Bluetooth control panel through a message queue telemetry transmission protocol to control the steering of a camera, then preprocessing a color image acquired by an image acquisition module, and sending the preprocessed color image to a position and posture detection module through an ROS (reactive oxygen species) module;
the preprocessing of the color image specifically comprises the following steps:
f (x, y, z) represents a frame image with the size of H multiplied by W, x, y represent horizontal pixel coordinates and vertical pixel coordinates of the image, z represents the number of channels, and the color map z is 3, and when the color map is used for pose detection, the average value and variance normalization processing of a data set is used for the color map:
f(x,y,z)＝(f(x,y,z)-mean[z])/std[z]z＝3 (1)
wherein mean ═ 0.485,0.456,0.406] std ═ 0.229,0.224,0.225 ];
step four: the pose detection module calculates the 6D pose of the object through a pose detection algorithm, converts the pose into a target grabbing pose and sends the target grabbing pose to the grabbing control module through an ROS (reactive oxygen species) system;
the pose detection module calculates a 6D pose of the object through a pose detection algorithm, and specifically comprises the following steps:
constructing a pixel level voting neural network PVnet, which specifically comprises the following steps:
the input f (x, y, z) of PVnet is a color map with the size of H multiplied by W, a pre-trained Resnet-18 is used as a basic network, when the size of the feature map is H/8 multiplied by W/8, an associated spatial gradient posing module with three different void rates is used for extracting features, residual error connection is repeatedly used in a convolution layer and an upper sampling layer, and then up sampling is carried out until the size of the output reaches H multiplied by W; and outputting a pixel-level prediction direction vector and a semantic segmentation result by using a convolution layer of 1 multiplied by 1 in the feature map of the last layer. The specific ASPP module is that convolution kernels with different voidage rates are used for processing feature maps, then the feature maps are connected together to expand the number of channels, and finally an output feature map with the same size is obtained through a 1x 1 convolution layer.
(a) Sending the input picture f (x, y, z) into a pixel-level voting neural network PVnet regression to obtain a semantic segmentation result and a pixel-level prediction direction vector, wherein the specific direction vector is defined as:
for a pixel p, the network outputs an object label associated therewith and a representation of 2D keypoints from the pixel p to the object
Figure BDA0002326965470000071
A unit direction vector of (a); v. ofk(p) is defined as:
Figure BDA0002326965470000072
(b) voting positioning key point coordinate
Giving semantic labels and unit vectors, generating a key point hypothesis by using a random sample consensus (RANSAC) -based voting scheme, firstly finding a pixel p belonging to a target object by using the semantic labels, then randomly selecting two pixels, and corresponding the two pixels to a prediction unit direction vector vk(p) intersection as a key point
Figure BDA0002326965470000073
Hypothesis h ofk,iRepeating N times to generate a series of predictions of keypoints { hk,i1,2,3, N, for the hypothesis hk,iScore w of votek,iIs defined as:
Figure BDA0002326965470000081
wherein theta is a threshold value, p belongs to O and represents that a pixel point p belongs to an object O, and the hypothesis with the highest score is selected as the predicted value of the 2D coordinate of the key point
Figure BDA0002326965470000082
Wherein the pose is to be estimated
Figure BDA0002326965470000083
Convert to grab pose pgraspThe method specifically comprises the following steps:
according to the obtained 2D key point coordinates
Figure BDA0002326965470000084
And three-dimensional point coordinates of corresponding key points on the known object model
Figure BDA0002326965470000085
Obtaining an estimated pose relative to a camera coordinate system by using a PnP algorithm according to the corresponding relation of 2D-3D
Figure BDA0002326965470000086
Hand-eye matrix obtained according to hand-eye calibration
Figure BDA0002326965470000087
Will be provided with
Figure BDA0002326965470000088
Transformed to pose relative to base coordinates of the mechanical arm
Figure BDA0002326965470000089
According to the position and posture
Figure BDA00023269654700000810
Obtaining a coordinate system C relative to the base of the mechanical armbaseEstimated object coordinate system of
Figure BDA00023269654700000811
A top-down grabbing mode is adopted, when the gripper at the tail end of the mechanical arm is vertically downward, the Z axis of a coordinate system at the tail end of the mechanical arm is also vertically downward, and the Z axis of a coordinate system at the base end of the mechanical arm is vertically upward; subject coordinate system CobjectAnd a robot base coordinate system CbaseThe original points are overlapped through displacement; the unit vector of the Z axis of the base coordinate system is
Figure BDA00023269654700000812
Object coordinate system CobjectThe unit vector of each axis is
Figure BDA00023269654700000813
Is calculated to obtain
Figure BDA00023269654700000814
And
Figure BDA00023269654700000815
the included angle between:
Figure BDA00023269654700000816
according to
Figure BDA00023269654700000817
Further obtaining acute angles theta between the vectorsx,θy,θz：
Figure BDA00023269654700000818
Then take the minimum thetaiThe corresponding axis is the final transformation axis, i is min (θ)i)
(b) Assuming an estimated object attitude coordinate system CobjectAnd a robot base coordinate system CbaseThe transformation relationship between the rotation matrix is R and the displacement is T
Figure BDA0002326965470000091
And obtaining Euler angles corresponding to all the axes according to the rotation matrix as follows:
Figure BDA0002326965470000092
when the rotation axis i is known from (a), the corresponding rotation angle θ is obtained from the equation (6)iRotating the base coordinate system of the mechanical arm to a Z axis vertically downwards, and then rotating the Z axis by thetaiAccording to
Figure BDA0002326965470000093
The final grabbing pose p can be obtained by making corresponding displacement on the displacement T in the middlegrasp；
Step four: after the mechanical arm controller receives the target grabbing pose, planning mechanical arm movement grabbing through a Moveit movement planning library of the ROS; if the mechanical arm cannot reach the target pose, the position of the trolley is adjusted correspondingly according to the feedback information, and the detection and the grabbing are carried out again until the grabbing is finished, or no object exists in the feedback;
the obtained grabbing pose pgraspThe information is sent to a mechanical arm controller of the grabbing control module in a topic form through the ROS; the mechanical arm receives the grabbing gesture pgraspAnd then, subtracting the target displacements delta x, delta y and delta z from the extreme displacement of the tail end of the mechanical arm, and judging pgraspWhether the movement space of the mechanical arm is exceeded in displacement; if the delta x, the delta y and the delta z are positive, the mechanical arm plans the motion of the mechanical arm through a Moveit motion planning library of the ROS, and a grabbing middle position p is set before the mechanical arm moves to a target positiongrasp_mid，pgrasp_midIs formed by pgraspIs translated upwards for a certain distance to obtain a placing position p after being grabbedplaceAlso past this intermediate position pgraspThen reaches the set placing position pplace(ii) a If the sizes of the delta x, the delta y and the delta z are negative numbers, and the sizes of the delta x, the delta y and the delta z are within the threshold values, corresponding direction movement can be carried out to enable the delta x, the delta y and the delta z to be positive, trial grabbing is carried out again, and the final visual grabbing task is completed; if one of the magnitudes of Δ x, Δ y, and Δ z is outside the threshold, no object is present in the feedback.

Claims (3)

1. An autonomous mobile grabbing system based on ROS, its characterized in that: the system is divided into four parts: the system comprises an image acquisition module, a visual navigation module, a pose detection module and a capture control module; the image acquisition module consists of an RGB-D camera, an NVIDIA Jetson TX2 and a rotating tripod head control module, the RGB-D camera and the NVIDIA Jetson TX2 are responsible for acquiring color images and depth images, the rotating tripod head control module consists of a motor and a Bluetooth control panel, the tripod head is controlled to control the camera to rotate to the horizontal direction when a visual navigation task is executed, and the tripod head is controlled to control the camera to rotate to the vertical direction when a pose detection task is executed; the visual navigation module consists of a mobile trolley, an infrared sensor and an NVIDIA GeForce GTX 1080Ti server; the method comprises the following steps that an RGB-D camera collects scene color images and depth images and transmits the scene color images and the depth images to a server, the server executes an SLAM algorithm to build images and uses a move _ base library to control a mobile trolley to navigate based on ROS, and an infrared sensor is used for avoiding obstacles in the navigation process; the pose detection module is completed by an NVIDIA GeForce GTX 1080Ti server, an RGB-D camera collects a target color image and a target depth image and transmits the target color image and the target depth image to the server, and the server executes the coordinates of the PVnet regression key points of the pixel-level voting neural network and calculates the 6D pose of the object based on a PnP algorithm; the grabbing control module comprises a mechanical arm with 5 degrees of freedom and a controller thereof, the controller receives a target grabbing pose, and a Moveit motion planning algorithm library of the ROS is used for planning a mechanical arm motion path and executing a grabbing task; the ROS is a Robot Operating System; the 6D pose includes a 3D position and a 3D orientation.
2. The grabbing method of an ROS-based autonomous mobile grabbing system according to claim 1, characterized in that:
the method comprises the following steps: placing the trolley in a strange environment, and acquiring the color image and the depth image by the image acquisition module;
step two: the color image and the depth image acquired by the image acquisition module are transmitted to the visual navigation module through the ROS and execute the SLAM algorithm to construct a map; marking a target position area by using a two-dimensional code icon, determining the target area by detecting the pose of the two-dimensional code icon, and autonomously moving the mobile car to the vicinity of the target area according to the path planned by the move _ base motion navigation library of the ROS;
the map construction is based on an RGBD-SLAM-V2 framework, a front-end visual odometer extracts features from each frame of color image, and a RANSAC + ICP algorithm meter is used by means of an ORB feature descriptorCalculating the pose relationship between the two images, then carrying out rear end and loop detection optimization based on a g2o optimization library, and combining the optimized pose with a color image and a depth image to generate a dense map; then, the position and the pose of the target area are found through the two-dimensional code icon marks
Figure FDA0002885359910000011
The moving trolley receives
Figure FDA0002885359910000012
Navigating to the target area by using the move _ base motion navigation library of the ROS;
step three: after the target area is reached, sending an instruction to a Bluetooth control board through a message queue telemetry transmission protocol to control the RGB-D camera to turn, then preprocessing the color image acquired by the image acquisition module, and sending the preprocessed color image to the attitude and position detection module through an ROS (reactive oxygen species) module;
the preprocessing of the color image specifically comprises the following steps:
f (x, y, z) represents a frame image with the size of H multiplied by W, x, y represent horizontal pixel coordinates and vertical pixel coordinates of the image, z represents the number of channels, and the color map z is 3, and when the color map is used for pose detection, the average value and variance normalization processing of a data set is used for the color map:
f(x,y,z)＝(f(x,y,z)-mean[z])/std[z] z＝3 (1)
wherein mean ═ 0.485,0.456,0.406] std ═ 0.229,0.224,0.225 ];
step four: the pose detection module calculates the 6D pose of the object through a pose detection algorithm, converts the pose into a target grabbing pose and sends the target grabbing pose to the grabbing control module through an ROS (reactive oxygen species) system;
the pose detection module calculates a 6D pose of the object through a pose detection algorithm, and specifically comprises the following steps:
(a) sending the input picture f (x, y, z) into an improved pixel-level voting neural network PVnet regression to obtain a semantic segmentation result and a pixel-level prediction direction vector, wherein the specific direction vector is defined as:
for a pixel p, the network outputsObject tag associated therewith and representing 2D keypoints from pixel p to object
Figure FDA0002885359910000021
A unit direction vector of (a); v. ofk(p) is defined as:
Figure FDA0002885359910000022
(b) voting positioning key point coordinate
Giving semantic labels and unit vectors, generating a key point hypothesis by using a random sample consensus (RANSAC) -based voting scheme, firstly finding a pixel p belonging to a target object by using the semantic labels, then randomly selecting two pixels, and corresponding the two pixels to a prediction unit direction vector vk(p) intersection as a key point
Figure FDA0002885359910000023
Hypothesis h ofk,iRepeating N times to generate a series of predictions of keypoints { hk,i1,2,3, N, for the hypothesis hk,iScore w of votek,iIs defined as:
Figure FDA0002885359910000024
wherein theta is a threshold value, p belongs to O and represents that a pixel point p belongs to an object O, and the hypothesis with the highest score is selected as the predicted value of the 2D coordinate of the key point
Figure FDA0002885359910000031
Wherein the pose is to be estimated
Figure FDA0002885359910000032
Convert to grab pose pgraspThe method specifically comprises the following steps:
according to the obtained 2D key point coordinates
Figure FDA0002885359910000033
And three-dimensional point coordinates of corresponding key points on the known object model
Figure FDA0002885359910000034
Obtaining an estimated pose relative to a camera coordinate system by using a PnP algorithm according to the corresponding relation of 2D-3D
Figure FDA0002885359910000035
Hand-eye matrix obtained according to hand-eye calibration
Figure FDA0002885359910000036
Will be provided with
Figure FDA0002885359910000037
Transformed to pose relative to base coordinates of the mechanical arm
Figure FDA0002885359910000038
According to the position and posture
Figure FDA0002885359910000039
Obtaining a coordinate system C relative to the base of the mechanical armbaseEstimated object coordinate system of
Figure FDA00028853599100000310
A top-down grabbing mode is adopted, when the gripper at the tail end of the mechanical arm is vertically downward, the Z axis of a coordinate system at the tail end of the mechanical arm is also vertically downward, and the Z axis of a coordinate system at the base end of the mechanical arm is vertically upward; subject coordinate system CobjectAnd a robot base coordinate system CbaseThe original points are overlapped through displacement; the unit vector of the Z axis of the base coordinate system is
Figure FDA00028853599100000311
Object coordinate system CobjectThe unit vector of each axis is
Figure FDA00028853599100000312
Is calculated to obtain
Figure FDA00028853599100000313
And
Figure FDA00028853599100000314
the included angle between:
Figure FDA00028853599100000315
according to
Figure FDA00028853599100000316
Further obtaining acute angles theta between the vectorsx,θy,θz：
Figure FDA00028853599100000317
Then take the minimum thetaiThe corresponding axis is the final transformation axis, i is min (θ)i)
(b) Assuming an estimated object pose coordinate system CobjectAnd a robot base coordinate system CbaseThe transformation relationship between the rotation matrix is R and the displacement is T
Figure FDA0002885359910000041
And obtaining Euler angles corresponding to all the axes according to the rotation matrix as follows:
Figure FDA0002885359910000042
when the rotation axis i is known from (a), the corresponding rotation angle θ is obtained from the equation (6)iTo make the machineThe arm base coordinate system is rotated to the Z axis vertically downwards, and then the Z axis is rotated by thetaiAccording to
Figure FDA0002885359910000043
The final grabbing pose p can be obtained by making corresponding displacement on the displacement T in the middlegrasp；
Step four: after the mechanical arm controller receives the target grabbing pose, planning mechanical arm movement grabbing through a Moveit movement planning library of the ROS; if the mechanical arm cannot reach the target pose, the position of the trolley is adjusted correspondingly according to the feedback information, and the detection and the grabbing are carried out again until the grabbing is finished, or no object exists in the feedback;
the obtained grabbing pose pgraspThe information is sent to a mechanical arm controller of the grabbing control module in a topic form through the ROS; mechanical arm receiving and grabbing pose pgraspThen, according to the final limit displacement of the mechanical arm minus the target displacement delta x, delta y and delta z, judging pgraspWhether the movement space of the mechanical arm is exceeded in displacement; if the delta x, the delta y and the delta z are positive, the mechanical arm plans the motion of the mechanical arm through a Moveit motion planning library of the ROS, and a grabbing middle position p is set before the mechanical arm moves to a target positiongrasp_mid，pgrasp_midIs formed by pgraspIs translated upwards for a certain distance to obtain a placing position p after being grabbedplaceAlso past this intermediate position pgraspThen reaches the set placing position pplace(ii) a If the sizes of the delta x, the delta y and the delta 0z are negative numbers, and the sizes of the delta 1x, the delta y and the delta z are within the threshold values, the corresponding direction movement can be carried out to ensure that the delta x, the delta y and the delta z are positive, trial grabbing is carried out again, and the final visual grabbing task is completed; if one of the magnitudes of Δ x, Δ y, and Δ z is outside the threshold, no object is present in the feedback.
3. The grabbing method of an ROS-based autonomous mobile grabbing system according to claim 2, characterized in that: the improved pixel-level voting neural network PVnet is as follows:
the input f (x, y, z) of PVnet is a color map with the size of H multiplied by W, a pre-trained Resnet-18 is used as a basic network, when the size of the feature map is H/8 multiplied by W/8, an associated spatial gradient posing module with three different void rates is used for extracting features, residual error connection is repeatedly used in a convolution layer and an upper sampling layer, and then up sampling is carried out until the size of the output reaches H multiplied by W; outputting a pixel-level prediction direction vector and a semantic segmentation result by using a 1 multiplied by 1 convolutional layer in the feature map of the last layer; the specific ASPP module is that convolution kernels with different voidage rates are used for processing feature maps, then the feature maps are connected together to expand the number of channels, and finally an output feature map with the same size is obtained through a 1x 1 convolution layer.
CN201911320327.6A 2019-12-19 2019-12-19 ROS-based autonomous mobile grabbing system and method Active CN111055281B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911320327.6A CN111055281B (en) 2019-12-19 2019-12-19 ROS-based autonomous mobile grabbing system and method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911320327.6A CN111055281B (en) 2019-12-19 2019-12-19 ROS-based autonomous mobile grabbing system and method

Publications (2)

Publication Number Publication Date
CN111055281A CN111055281A (en) 2020-04-24
CN111055281B true CN111055281B (en) 2021-05-07

Family

ID=70302469

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911320327.6A Active CN111055281B (en) 2019-12-19 2019-12-19 ROS-based autonomous mobile grabbing system and method

Country Status (1)

Country Link
CN (1) CN111055281B (en)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111531407B (en) * 2020-05-08 2021-08-17 太原理工大学 Workpiece attitude rapid measurement method based on image processing
CN111482967B (en) * 2020-06-08 2023-05-16 河北工业大学 Intelligent detection and grabbing method based on ROS platform
CN111906785A (en) * 2020-07-23 2020-11-10 谈斯聪 Multi-mode comprehensive information identification mobile double-arm robot device system and method
CN111890366B (en) * 2020-08-05 2023-06-13 北京航空航天大学 Mechanical arm object grabbing planning principle and ROS-based implementation method
CN112372641B (en) * 2020-08-06 2023-06-02 北京航空航天大学 Household service robot character grabbing method based on visual feedforward and visual feedback
CN113084808B (en) * 2021-04-02 2023-09-22 上海智能制造功能平台有限公司 Monocular vision-based 2D plane grabbing method for mobile mechanical arm
CN113110513A (en) * 2021-05-19 2021-07-13 哈尔滨理工大学 ROS-based household arrangement mobile robot
CN115222809B (en) * 2021-06-30 2023-04-25 达闼科技（北京）有限公司 Target pose estimation method, device, computing equipment and storage medium
CN113758415A (en) * 2021-06-30 2021-12-07 广东食品药品职业学院 Machine vision positioning support, system and positioning method based on deep learning
CN113566830B (en) * 2021-07-20 2023-09-26 常州大学 Outdoor high-precision autonomous navigation device and method for wheel-foot composite robot
CN113791620A (en) * 2021-09-14 2021-12-14 上海景吾智能科技有限公司 Dynamic self-adaptive positioning method, positioning system, robot and storage medium
CN113878578B (en) * 2021-09-30 2024-01-16 上海景吾智能科技有限公司 Dynamic self-adaptive positioning method and system suitable for composite robot
CN114102662A (en) * 2021-11-26 2022-03-01 江西省智能产业技术创新研究院 Composite robot
CN114770461B (en) * 2022-04-14 2023-12-01 深圳技术大学 Mobile robot based on monocular vision and automatic grabbing method thereof
CN114912287B (en) * 2022-05-26 2023-07-25 四川大学 Robot autonomous grabbing simulation system and method based on target 6D pose estimation
CN115018876B (en) * 2022-06-08 2023-09-26 哈尔滨理工大学 ROS-based non-cooperative target grabbing control method

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101091723B1 (en) * 2011-04-07 2011-12-08 국방과학연구소 System for integrated operating of unmanned robot and method for operating of the system
CN106272415A (en) * 2016-08-30 2017-01-04 上海大学 Omni-mobile transport robot
CN106846468B (en) * 2017-01-25 2021-05-07 南京阿凡达机器人科技有限公司 Method for realizing mechanical arm modeling and motion planning based on ROS system
CN207309951U (en) * 2017-06-30 2018-05-04 南京信息工程大学 A kind of intelligent ball collecting robot
CN110216698A (en) * 2019-03-11 2019-09-10 浙江工业大学 A kind of mechanical arm remote control system based on ROS
CN109927012B (en) * 2019-04-08 2021-07-30 清华大学 Mobile grabbing robot and automatic goods taking method

Also Published As

Publication number Publication date
CN111055281A (en) 2020-04-24

Similar Documents

Publication Publication Date Title
CN111055281B (en) ROS-based autonomous mobile grabbing system and method
CN111496770B (en) Intelligent carrying mechanical arm system based on 3D vision and deep learning and use method
CN110480634B (en) Arm guide motion control method for mechanical arm motion control
JP3994950B2 (en) Environment recognition apparatus and method, path planning apparatus and method, and robot apparatus
CN114728417B (en) Method and apparatus for autonomous object learning by remote operator triggered robots
CN109164829B (en) Flying mechanical arm system based on force feedback device and VR sensing and control method
CN112634318B (en) Teleoperation system and method for underwater maintenance robot
US20230247015A1 (en) Pixelwise Filterable Depth Maps for Robots
JP2013184257A (en) Robot apparatus, method for controlling robot apparatus, and computer program
CN111015649B (en) Driving and controlling integrated control system
JP2022542241A (en) Systems and methods for augmenting visual output from robotic devices
CN111462154A (en) Target positioning method and device based on depth vision sensor and automatic grabbing robot
CN107450556A (en) A kind of independent navigation intelligent wheel chair based on ROS
CN113103230A (en) Human-computer interaction system and method based on remote operation of treatment robot
US11769269B2 (en) Fusing multiple depth sensing modalities
EP4095486A1 (en) Systems and methods for navigating a robot using semantic mapping
CN109079777B (en) Manipulator hand-eye coordination operation system
Kragic et al. Model based techniques for robotic servoing and grasping
CN116175582A (en) Intelligent mechanical arm control system and control method based on machine vision
Schnaubelt et al. Autonomous assistance for versatile grasping with rescue robots
CN114888768A (en) Mobile duplex robot cooperative grabbing system and method based on multi-sensor fusion
KR102452315B1 (en) Apparatus and method of robot control through vision recognition using deep learning and marker
EP3842888A1 (en) Pixelwise filterable depth maps for robots
CN116867611A (en) Fusion static large-view-field high-fidelity movable sensor for robot platform
US20220268939A1 (en) Label transfer between data from multiple sensors

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant