TWI851310B - Robot and method for autonomously moving and grabbing objects - Google Patents

Abstract

A robot and method for autonomously moving and grabbing objects, the robot comprising: a mechanical arm, and a mobile platform carrying the robotic arm, wherein the mobile platform is used to navigate the robot to where the target object to be grabbed is located, and has: Semantic navigation system, used to navigate the mobile platform to the location of the target; the first camera, used to take pictures of the external environment during navigation; automatic docking coordination controller, used to optimize the moving path and docking position of robot's automatic grasping; the mobile platform controller, used to control the movement of the mobile platform; and the drive system, used to drive the mobile platform forward. The robotic arm is used to grab the target object and has: the second camera, used to allow the robotic arm to understand the environment; the object recognition and pose estimation system, used for semantic recognition, segmentation of object and pose estimation of the grasping target; the grasping index calculation module, used to determine the most appropriate grasping pose, and the mobile grasping controller, used to control the robot movement.

Description

Autonomous mobile robot and method for grasping objects

This invention is a robotic arm that can autonomously move and grasp objects, and can be applied to industries such as robotic automation and intelligent manufacturing systems.

The US patent application US9785911B2 proposes a method and system including a central server and a robot arm for sorting or shelving in a logistics facility, wherein the central server is configured to communicate with a robot to send and receive picking data; the robot can autonomously navigate and locate itself in the logistics facility by identifying landmarks through at least one of a plurality of sensors, and the sensor also provides signals related to the detection, identification and location of the workpiece to be picked up or stored. However, the technology disclosed in the patent is all for positioning of known landmarks, lacking application flexibility.

The US patent application US11033338B2 proposes an adaptive robotic arm grasping planning technology for chaotic stack grasping, analyzing the shape of the workpiece to identify multiple stable grasping options, each grasping option has a position and direction; and further analyzing the shape of the workpiece to determine multiple stable intermediate postures; evaluating each individual workpiece in the box to identify a set of feasible grasps. However, the technology disclosed in the patent only describes a single workpiece, and does not propose a solution for different workpieces.

The US patent US20180161986A1 proposes a semantic synchronous tracking, object recognition and three-dimensional (3D) mapping system that can maintain a world map composed of static and dynamic objects instead of a 3D point cloud, and can learn the semantic properties of objects in real time. Robots can use this semantic information to improve their navigation and positioning capabilities. However, the patent only uses objects for navigation and does not discuss the grasping of objects.

The literature "K.T.Song and L.Kang,"Object-Oriented Navigation with a Multi-layer Semantic Map,"in Proc.of the 13th Asian Control Conference (ASCC 2022),Jeju Island,Korea,2022,pp.1386-1391." proposes an unmanned vehicle navigation system based on semantic map information. The system can perform semantic simultaneous localization and mapping (SLAM) for the workspace where the robot needs to navigate, and generate an environmental semantic SLAM map for semantic navigation tasks. However, the semantic information is only used for unmanned vehicle navigation, and no grasping planning is performed on objects.

The paper "E.Brzozowska,O.Lima and R.Ventura, "A Generic Optimization Based Cartesian Controller for Robotic Mobile Manipulation," in Proc.of 2019 International Conference on Robotics and Automation (ICRA), Montreal, QC, Canada, 2019, pp.2054-2060." considers the optimal joint velocity of the robot arm mounted on the base of the mobile robot under the given three-dimensional target posture. The paper focuses on the real-time closed-loop system architecture between the depth camera sensing and the robot arm and the mobile platform, but does not consider the need for the robot arm to dock.

The literature "S. Zhu, X. Zheng, M. Xu, Z. Zeng and H. Zhang, "A Robotic Semantic Grasping Method For Pick-and-place Tasks," in Proc. of 2019 Chinese Automation Congress (CAC), Hangzhou, China, 2019, pp. 4130-4136" proposes a robot semantic grasping method to estimate the robot's six-degree-of-freedom (6-DOF) grasping posture. It combines the object detection method of pixel-level semantic segmentation with the point cloud processing method to calculate the grasping configuration and realize the grasping perpendicular to the object surface. It only grasps the center of mass position and cannot meet the grasping requirements of all objects.

The main purpose of this invention is to enable the robot arm to autonomously complete the grasping task in the environment of daily life. Therefore, we hope to combine semantic information to enable the robot to understand the environment, and through object posture estimation and grasping planning design, to improve the grasping success rate, and combine the mobile grasping controller to make the robot arm posture and mobile platform reach the grasping position at the same time, so as to complete the grasping efficiently.

A second purpose of the present invention is to optimize the movement of the robot arm and the mobile platform for robot arm mobile grasping, while taking into account the hardware limitations of the robot arm and the mobile platform docking position.

Another purpose of the present invention is to be applicable to industries of robot automation and intelligent manufacturing systems, to enhance the flexibility of robots, to meet the needs of flexible manufacturing systems, to adapt to changing environments to perform tasks, and to increase the working space of robot arms.

Another purpose of the present invention is to apply it to collaborative robots, industrial robots and companion robots. Collaborative robots can share a working environment with workers to perform tasks and are easy to reprogram to respond to changes in the manufacturing process. In this way, the robot arm can move around these manufacturing units to perform tasks such as transporting finished products, semi-finished products and automatic assembly.

In order to achieve the above-mentioned purposes and other purposes, the present invention provides a robot and method for autonomously moving and grasping objects, wherein the robot comprises: a robot arm, and a mobile platform carrying the robot arm, wherein the mobile platform is used to navigate the robot to the location of the target object to be grasped, and has: a semantic navigation system, used to navigate the mobile platform to the location of the target object; a first camera, used to photograph the external environment during navigation; an automatic docking coordination controller, used to optimize the robot's automatic docking The moving path and grasping posture of the station; the mobile platform controller is used to control the movement of the mobile platform; and the driving system is used to drive the mobile platform forward; The robot arm is used to grasp the target object, and has: a second camera for the robot arm to understand the environment, an object recognition and posture estimation system for semantic recognition and segmentation of the grasping target object and posture estimation, a grasping index calculation module for determining the most appropriate grasping posture, and a mobile grasping controller for controlling the movement of the robot.

1:Robot

7: Mobile robot

10: First Camera

11: Second camera

20:Semantic Navigation System

30: Automatic docking coordination controller

40: Mechanical arm gripper

100:Robotic arm

200: Mobile platform

102: Object recognition and posture estimation system

104: Mobile grabbing controller

202: Mobile platform controller

1021: Image preprocessing module

1022: Semantic segmentation module

1023: Posture estimation module

S1~S8: Steps

Figure 1 is a module block diagram of the robot; Figure 2 is a system architecture diagram of the robot arm; Figure 3 is an architecture diagram of the object recognition and posture estimation system; Figure 4A is a grasping planning architecture diagram of the robot arm; Figure 4B is a 3D stereogram of the grasping target object; Figure 4C is a plane diagram of the grasping target object; Figure 4D is a position marking diagram for the grasping target object; Figure 5 is a flowchart of the grasping of the robot arm; Figure 6 is a flowchart of the motion planning of the robot arm; Figure 7 is a flowchart of the robot's docking position planning; Figure 8 is a diagram of the mobile grasping motion control system; and Figure 9 is a flowchart of a method for autonomous mobile grasping of objects according to an embodiment of the present invention.

Please refer to FIG. 1 . The present invention is a robot and method for autonomously moving and grasping objects. The robot 1 includes: a robot arm 100 and a mobile platform 200 carrying the robot arm 100. The mobile platform 200 is used to move the robot 1 to the location of the target object grasped by the robot arm 100. The mobile platform 200 has: a semantic navigation system 20 electrically connected to the mobile platform 200, which is used to navigate the mobile platform 200 to the location of the target object; a first camera 10 electrically connected to the semantic navigation system 20, which is used to shoot the external environment during navigation, and can provide high-quality synchronized video color and depth, which is an image depth camera (RGB-D camera); an automatic docking coordination controller 30 electrically connected to the robot arm 100 and the mobile platform 200, used to obtain the best mobile grasping path and posture of the robot arm 100 and the mobile platform 200; and a mobile platform controller 202 electrically connected to the mobile platform 200, used to control the movement of the robot 1; the robot arm 100 is used to grasp the target object, and has: an object recognition and posture estimation system 102, connected to the robot The robot arm 100 is electrically connected to perform semantic recognition, segmentation, and posture estimation of the grasping target object; a mobile grasping controller 104 is electrically connected to the robot arm 100 to control the movement of the robot arm so that the robot arm 100 can stop at a better position for grasping; and a second camera 11 is electrically connected to the object recognition and posture estimation system 102 to enable the robot arm 100 to understand the environment, and is a hand-eye visual image depth camera (Eye in hand RGB-D camera).

Please continue to refer to Figure 1. The object recognition and posture estimation system 102 includes: an image pre-processing module 1021, which is used to process the image captured by the second camera 11 and is a pre-processing module for RGB-D images; a semantic segmentation module 1022, which is electrically connected to the image pre-processing module 1021 and performs image semantic segmentation on the RGB color image after image pre-processing; and a posture estimation module 1023, which is electrically connected to the semantic segmentation module 1022 and the image pre-processing module 1021 and is used to estimate posture and calculate the six-degree of freedom (6-DOF) posture estimation result of the object after image pre-processing and semantic segmentation.

As shown in FIG. 2, the system architecture of the present invention is shown. The second camera 11 transmits the captured color image and depth image to the object recognition and posture estimation module 102 to perform object recognition semantic segmentation in block 211. The object recognition and posture estimation module 102 uses the object recognition and posture estimation algorithm and the lightweight deep learning model ESPNetv2 to perform real-time semantic segmentation. The segmented color image and the corresponding point cloud information are then placed in another A lightweight DenseFusion model is used for posture estimation; after the robot arm 100 reaches the grasping position of the target object, the object recognition and posture estimation module 102 performs a secondary posture estimation of the target object through the second camera 11 above, so that the posture estimation at a closer distance can improve the accuracy of object recognition; the following processing is performed in blocks 212-214: after obtaining the posture and outline of the target object, the proposed Grasp Indicator (Grasp The grasping planning algorithm of the automatic docking coordination controller 30 generates the grasping planning of block 212, which can plan a better grasping posture of the target object through the relationship between the grasping position and the distance of the center of mass and the width of the grasping segment, and then control the robot arm 100 to improve the grasping success rate. At the same time, the target posture of the target object is generated through block 213 for object positioning and block 214 for docking planning processing to input the automatic docking coordination controller 30. The automatic docking coordination controller 30 transmits the processing results to the robot arm 100 and the mobile platform controller 202 at the same time, so that the robot moves the robot arm 100 to grasp the target object in block 218, which can improve the working efficiency and stability of mobile grasping.

Please continue to refer to FIG. 2 . The first camera 10 transmits the color image and the depth image to the semantic navigation system 20. The semantic navigation system 20 uses the Real-Time Appearance-Based Mapping (RTAB-Map) algorithm combined with object detection to create a map through blocks 221, 222, 231 and 232. The semantic map information of block 221 is pre-established, and the semantic SLAM (Simultaneous Localization and The purpose of the positioning processing of block 222 is to improve the robot arm 100's understanding of the environment and increase the flexibility of the task; the path planning processing of block 222 is to enable the mobile platform 200 to plan the path after receiving the semantic information and transmit the result to the navigation controller 232; the laser SLAM positioning processing of block 231 is to input the signal from the laser scanner 230 and cooperate with the navigation controller 232 to autonomously navigate and dock to the target point; and the semantic navigation system 2 0 will transmit the object posture and recognition results to the mobile platform controller 202 and the robot arm 100. Using the movement planning algorithm and the grasping planning algorithm, the robot arm 100 and the mobile platform 200 can move simultaneously to improve the grasping efficiency of the robot arm 100. The mobile platform 100 can smoothly dock in front of the workstation to facilitate the grasping task, and the robot arm 100 reaches the top of the object to prepare for grasping. In block 218, the robot moves the robot arm 100 to grasp the target object.

FIG3 is a schematic diagram of the object recognition and posture estimation system 102, wherein the image preprocessing module 1021 preprocesses the block 301 with the RGB image color map input by the second camera 11 to obtain the object depth point cloud information. The preprocessing of the block 301 can reduce the light and shadow changes in the environment and improve the stability of the object recognition and posture estimation system 102. The preprocessed RGB image is transmitted to the semantic segmentation processing of the block 302, wherein the semantic segmentation processing is performed by the semantic segmentation module 1021. The semantic segmentation module 1022 uses the ESPNetV2 model in block 305 to perform semantic segmentation on the RGB image after image preprocessing, so that the pixel-wise position of the object can be segmented; then, the segmented image is transmitted to block 303 for object pose estimation processing, wherein the pose estimation module 1023 uses the DenseFusion model in block 306 for processing, combining the above-mentioned color image segmentation image with the object depth point cloud information obtained by the RGB-D camera, and performing object pose estimation to calculate the 6-DOF pose estimation result of the target object in block 304.

The mobile grasping controller 104 uses a grasping planning algorithm to design the grasping posture of the robot arm 100 based on the known object appearance contour. The main core concept is to select a better grasping range of the target object based on the condition after obtaining the posture and contour of the target object to be grasped. "Better grasping" also means that the grasping success rate can be increased through this method, that is, the stability of grasping the known target object.

As shown in FIG4A, it is the grasping planning framework of the robot arm 100, wherein the grasping planning algorithm of the robot arm 100 is divided into three parts: obtaining the centroid and contour fragments, designing the grasping index, and transforming the grasping posture. The first part of the centroid and contour fragment acquisition is to transmit the object contour and 6D posture 400 to the block 401 to obtain the image object centroid and object slice processing. The centroid and contour fragment acquisition can first understand the object centroid position through the design, and based on this position, select the contour fragment of the target object to evaluate the grasping position. In the block 402, the grasping position processing is determined by the grasping index. By grasping the index, the grasping position is determined. The design of the marker is to evaluate the grasping positions of the target object at various locations, so as to select the best position to grasp the target object, and finally perform the grasping posture coordinate conversion processing of block 403. Through the coordinate conversion relationship of the grasping posture of the robot arm 100, since the grasping position obtained is based on the outline of the target object on the image plane, the grasping posture conversion can convert the target object grasping information from the plane to the three-dimensional space, thereby achieving the robot control of block 404.

Continuing with FIG. 4A , the algorithm for grasping planning of the robot arm 100 is described. In the first part, the center of mass position of the object in the image plane can be understood by obtaining the center of mass and contour segments. Based on this position, the orientation of the object can be understood through the 3D model of the object and the DenseFusion model of the pose estimation algorithm. The long axis of the object is defined by the orientation of the object. The gripper grasps the object in a manner perpendicular to the long axis. The contour segments of the object are also segmented in the direction of the long axis to evaluate the grasping position. In the object contour color image (Contour Image), the center of mass of the object can be calculated by marking each pixel on the contour color image. The formula is as shown in Equations 3.1~3.4: M ₀₀ =Σ _X Σ _y v ( x,y ) (3.1)

M ₁₀ = _ΣX ． Σ _y v ( x,y ) x (3.2)

M ₀₁ = Σ _X． Σ _y v ( x,y ) y (3.3)

M00 is the area of the object, M10 and M01 are the rotational inertia _of the object on the image plane about the x _- axis and y _- axis respectively. The center of mass mx _and my of the object on the image plane can be obtained by _calculation .

The design of the object contour segment is performed by segment cutting in the direction of the long axis mentioned above to evaluate the grasping position. The practical method is to obtain the positive direction perpendicular to the grasping surface of the object through the 3D model of the object, as shown in Figure 4B. We define the Z axis of the robot gripper 40 to be perpendicular to the Z axis of the object, that is, the long axis direction of the object, with R _{Z_tcp} ⊥ R _{Z_obj} indicates that this vertical relationship not only makes it easier for the gripper to grasp the object, but also can convert the long axis direction into the axis direction of the object in the contour color image, and then obtain the segment width perpendicular to the positive direction of the object contour color image. The practical method is to start from the center of mass to both sides of the positive axis, and calculate the segment width value every 5% displacement. The obtained contour image and width l are shown in Figure 4C.

The second part, the design of the Grasp Index is used to determine the most appropriate grasping posture. By obtaining the center of mass and contour segments, the center of mass can be used as a reference to increase the understanding of the object's position. The long axis of the object obtained by the 3D model of the object is the positive direction. The width of each segment cut out from the contour image is used to increase the success rate of grasping the object of interest through the design of the Grasp Index. The calculation method of the Grasp Index is discussed based on two parameters. The first is the distance between the grasping position and the center of mass. For object grasping, the closer the grasping position is to the center of mass, the better. However, the center of mass position of the object is not necessarily a good grasping position. Some objects are The body center of mass may be a bend or some objects may not be evenly distributed, so the second parameter is the width of the grasping position. The smaller the grasping width, the more stable the gripper will be. In the calculation of the grasping index, the two parameters are considered together, and the grasping position that is close to the center of mass and has a smaller width of the object is selected as the priority. The object width and the distance from the center of mass constitute the grasping index (GI) algorithm as shown in formula 3.5. Then, the grasping position with the largest GI (optimal) is calculated by the grasping index as the selected grasping position.

GI=(1- r _i )( l _max - l _i ) (3.5)

In terms of the grasping index related parameters, ri _is the ratio of the distance between the candidate grasping position and the object's centroid. The ratio is a percentage unit and is used to measure the distance between the grasping position and the centroid. lmax _is designed to be the longest width (unit: pixels) among the contour segments of the color image. This length will be compared with other candidate lengths. li _is the width of the contour segment of the color image at the grasping candidate position (unit: pixels). Through the calculation of the algorithm, the grasping index GI value taken on both sides of the centroid is taken as the grasping position.

In the third part, for the robot arm 100, the grasping planning is based on three-dimensional space, so it is necessary to transform the grasping posture so that the optimal grasping position originally planned by the grasping index on the two-dimensional image plane can be converted into three-dimensional space coordinates for the robot arm to carry out grasping planning through inverse kinematics, which are the displacement (Translation) and rotation (Rotation) of the object in the three-dimensional space. The conversion of the displacement in the three-dimensional space can be calculated by the pinhole model, and the point of the object on the two-dimensional plane can be converted into the space coordinate by projection, and the displacement ( X _tcp , Y _tcp , Z _tcp ) of the grasping position of the object is obtained as shown in Formula 3.6:

X _obj , Y _obj , Z _obj are the positions of the object in three-dimensional coordinates after posture estimation, u _O , v _O are the center positions of the object on the two-dimensional image plane, u _p , v _p are the centers of the grasping positions calculated by the grasping indexes according to formula (3.5), as shown in FIG4D , and c _x and c _y are the internal references of the first camera 10, representing the x-coordinate and y-coordinate of the image center, respectively.

，而R _{y_tcp} 便能藉由另外兩旋轉量之外積取得，所以經由計算之後，終端工具在空間座標中之旋轉量(R _{x_tcp} ,R _{y_tcp} ,R _{z_tcp} )如公式3.7：

After obtaining the displacement of the object in three-dimensional space, the rotation needs to be calculated. Since the 3D model of the object can be used to obtain the terminal tool posture perpendicular to the positive direction of the object grasping surface, that is, R _{Z_tcp} ⊥ R _{Z_obj} . The perpendicular relationship can be used for more stable grasping. In addition, on the grasping plane, the grasping position of the gripper should be parallel to the calculated color image outline segment width l , that is, R _{x_tcp} ∥

, and R _{y_tcp} can be obtained by the product of the other two rotation quantities. Therefore, after calculation, the rotation quantity of the terminal tool in the spatial coordinate ( R _{x_tcp} , R _{y_tcp} , R _{z_tcp} ) is as shown in formula 3.7:

After calculating the designed grasping posture transformation, the displacement ( X _tcp , Y _tcp , Z tcp) of the final terminal tool grasping position in three-dimensional space and the rotation ( R _{x_tcp} , R _{y_tcp} , R _{z_tcp} ) of the terminal tool can be calculated from the grasping position of the object in the two-dimensional image plane _combined with the 3D model of the grasped object.

The first camera 10 recognizes the object above the robot arm 100, and then obtains the semantic segmentation image and three-dimensional posture of the object to be grasped after estimating the object posture. The optimal grasping position of the object to be grasped in the two-dimensional space plane can be calculated by the grasping index through the known 3D model. Then, the grasping posture conversion method is used to convert the grasping position of the two-dimensional plane into the position of the terminal tool in the three-dimensional space for grasping the object.

As shown in FIG5 , it is a flowchart of autonomous grasping of the robot arm 100. First, in step 500, the posture and contour of the object are processed. The posture and contour of the grasped object obtained by the object recognition and posture estimation system 102 are used through the grasping planning algorithm to obtain the grasping position of the terminal tool in space for the object. Then, in step 501, grasping planning processing is performed. The grasping coordinates of the terminal tool of the grasping planning are calculated through inverse kinematics in step 502. In step 503, the speed control command of the six axes of the robot arm is issued to the robot arm controller, and then the robot arm 100 and the gripper complete the object grasping in step 504.

As shown in FIG. 6 , it is a flow chart of the motion planning of the mobile robot arm 100. First, in step 600, it is determined whether the robot 1 has reached the target. If not, the process goes to step N and returns to step 600, and continues to navigate through the semantic information of the landmarks established on the semantic map, so that the robot can navigate to the target semantic space. When the robot reaches the target space, the process goes to step Y and proceeds. Step 602, the target object detection is performed through the second camera 11 on the robot arm 100 for object posture recognition, and then step 603 is performed to perform secondary motion planning of the robot arm 100 based on the result of object posture recognition to achieve target posture estimation, and finally step 30 is performed to automatically dock the coordination controller to make the robot arm 100 correctly dock in front of the grasped object for grasping.

Since the robot 1 is a redundant system, there are infinite path planning that can enable the robot 1 to complete the grasping of objects, but some paths may cause collision danger to the robot 1, or make it difficult to complete the grasping task. Therefore, the automatic docking coordination controller 30 is used to optimize the speed control based on the position vision servo closed loop. The mobile robot can be controlled by speed optimization control. First, the posture error between the current and target terminal tool postures is calculated, and the mobile grasping controller 104 generates a feasible joint speed to minimize this error. The minimization problem is formulated as a constrained optimization problem, where the constraint is the joint speed limit.

The mobile platform controller 202 uses the mobile platform docking motion planning algorithm to avoid the robot arm 100 from colliding with the docking station, and docks at a suitable grasping position to grasp the objects in a disorderly stack. Considering that the robot 1 is long in appearance, in order to successfully complete the grasping and avoid collision with the docking station, the safety and stability of the grasping are improved, as shown in FIG. 7 , which is a flowchart of the docking position planning of the robot 1. First, in step 700, the second camera 11 above the robot arm 100 is used to perform object recognition through the object recognition and posture estimation module 102; then in step 701, the posture estimation module 1023 estimates the three-dimensional posture of the object, and then through the conversion relationship of the robot arm 100, the object can be located in the two-dimensional map to complete the object positioning. The expression for converting the object posture into the two-dimensional map is as shown in formula (4.1):

為抓取物基於相機的坐標系，透過該姿態估測模組1023能得到此轉換關係，已知的相機座標與基座之間的座標轉換為

，並且透過該機械手臂100之語義導航系統20定位所得之該機械手臂100在地圖上的位置關係

，由矩陣相乘運算能得到

，代表物體與地圖之間的相應位置。 in

The coordinate system of the camera is used to grasp the object. The conversion relationship between the known camera coordinates and the base coordinates is obtained through the posture estimation module 1023.

, and the position relationship of the robot arm 100 on the map obtained by positioning the robot arm 100 through the semantic navigation system 20

, which can be obtained by matrix multiplication operation

, representing the corresponding position between the object and the map.

Please continue to refer to FIG. 7 and proceed to step 702. After identifying the position of the object in the map, the coordinates of the object can be described through the map coordinate system, so that the mobile platform 200 is parallel to the x-axis of the object on the map coordinate system and has the same x-coordinate as the object, and the y-coordinate is docked at 80 cm next to the object. The docking position of the mobile platform 200 is determined as shown in formula (4.2):

Where Xb , Yb , _θb are _{the docking} positions of the mobile platform 200 in map coordinates, and Xo _, Yo are the positions of the _objects on the docking station located on the map by the above formula (4.1). In this way, the docking position of the mobile platform 200 can be determined.

Please continue to refer to FIG. 7 and finally proceed to step 703. Through the semantic segmentation and recognition algorithm of the semantic segmentation module 1022, the object posture estimation results T _obj and R _obj represent the displacement and rotation of the object. The object position conversion relationship at the docking station is obtained as shown in formula (4.3):

系抓取物基於相機之座標關係可由T _obj 及R _obj 得知，

為已知的相機座標與基座之座標轉換，

係該移動平台200位置與靠站位置之關係，因此

係靠站位置與目標物體之間的關係。結合該移動平台200停靠位置及該機械手臂100終點時的姿態，得到目標姿態P _target ,R _target ,P _target 代表該移動平台200之位置(X _b ,Y _b ,θ _b )及該機械手臂1最終之目標姿態位移量(T _tcp )，R _target 係該機械手臂100最終抵達目標點時之旋轉角度(R _tcp )。 in

The coordinate relationship of the captured object based on the camera can be obtained from T _obj and R _obj .

The coordinate conversion between the known camera coordinates and the base.

is the relationship between the position of the mobile platform 200 and the docking position, so

The relationship between the docking position and the target object. Combining the docking position of the mobile platform 200 and the posture of the robot arm 100 at the end point, the target posture P _target , R _target is obtained . P _target represents the position ( X _b , Y _b , θ _b ) of the mobile platform 200 and the final target posture displacement ( T _tcp ) of the robot arm 1, and R _target is the rotation angle ( R _tcp ) of the robot arm 100 when it finally reaches the target point.

As shown in FIG8 , it is a diagram illustrating the architecture of the mobile grasping motion control system. In step 800, the docking position planning is performed. Through the design of the object recognition and posture estimation system 102 and the grasping planning algorithm, the final result is in step 801. Through the directional kinematics, the robot arm 100 can obtain the terminal tool grasping posture of the object to be grasped. After obtaining the 6-DOF posture of the terminal gripper, the posture error between it and the target terminal tool posture is calculated, and the automatic docking coordination controller 30 is used to move to a posture suitable for grasping to grasp. Due to the joint limitation of the mobile robot arm 8, the robot arm 100 and the mobile platform 200 can be controlled in steps 802 and 803 respectively, and the automatic docking coordination controller 30 can also be used to eliminate this error and move to the appropriate position for autonomous grasping.

The automatic docking coordination controller 30 is controlled based on the terminal tool speed error in the Cartesian coordinates. First, the terminal tool speed V is described. The current terminal tool speed calculation method is as shown in formula (4.4):

、

、

係該移動平台200之速度，J(θ)為雅可比矩陣(Jacobian matrix)，

為機械臂各軸之軸速度。 Where v _x , v _y , v _z are the linear velocities of the end tool, ω _x , ω _y , ω _z are the angular velocities of the end tool, x , y represent the projection of the end tool on the XY plane,

,

,

is the velocity of the mobile platform 200, J ( θ ) is the Jacobian matrix,

is the axis speed of each axis of the robot arm.

The station position of the mobile platform 200 and the target posture of the robot arm 100 are integrated with the calculation method of the ideal speed Vr _of the robot arm as shown in formula (4.5):

)進行速度控制，當位置差距及該機械手臂100之角度差距漸漸縮小時，接近目標位置，速度會漸漸縮小，且順利停靠至目標位置。 Wherein P _current is the position of the mobile platform 200 and the robot arm 100, R _current is the rotation angle of the robot arm 100, P _target and R _target are the docking position of the mobile platform 100 and the position and rotation angle of the target grasping point of the robot arm 100, respectively, Δt is the sampling time of the optimized automatic docking coordination controller 30, and the ideal speed is designed to reach the target position in a single sampling time, through the position difference P _target - P _current between the target and the robot arm 100, and the angle difference log(

) is used for speed control. When the position difference and the angle difference of the robot arm 100 gradually decrease, the speed will gradually decrease as it approaches the target position, and it will smoothly stop at the target position.

In view of the requirement of the docking design of the mobile platform 200, the optimization algorithm in the optimized automatic docking coordination controller 30 is used to minimize the problem so that the mobile platform 200 can successfully complete the docking and grasping during the motion control process of the robot arm 100. The purpose of the optimization method is to minimize the value obtained by the penalty function, which is the best solution, as shown in formula (4.6):

)係對數的屏障函數，W(

,

,

,

)係一階正規化的函數，其中，

係該機械手臂100各軸角度，受限在

及

之間，

,

,

係該移動平台200之線速度及角速度，受限在

及

之間。其中第一項Minimize∥V _r -V∥²，係為了縮小該機械手臂100之速度V和理想速度V _r 兩者之間的速度誤差，該最小化誤差的方式係為了將理想速度V _r 在單個取樣時間中到達目標終端工具姿態，並且在速度和關節角度極限的限制下調整終端工具的速度，讓移動手臂車上的手臂及該移動平台200接近理想速度，選用此兩者的平方誤差(Quadratic error)來當作代價函數(Cost function)，且能加快誤差的收斂，但是機器人1上有許多硬體限制例如：機械手臂100上每個軸運動角度的上限以及移動平台200的硬體限制等，所以必須增加一些懲罰函數(Penalty function)。P(

)是一個對數的屏障函數(Log barrier function)，來避免軸上的角度接近極限，W(

,

,

,

)是一個一階正規化的函 數(Normalization to first normal form)，透過給每個軸及該移動平台200不同的權重，來平衡各軸的運動。 Wherein, V _r and V represent the ideal speed and joint speed of the robot arm 100, P (

) is the logarithmic barrier function, W (

,

,

,

) is a first-order regularized function, where

The robot arm 100 has an angle of each axis, which is limited by

and

In between,

,

,

is the linear velocity and angular velocity of the mobile platform 200, which is limited by

and

The first item Minimize ∥ V _r - V ∥ ² is to reduce the speed error between the speed V of the robot arm 100 and the ideal speed V _r . The method of minimizing the error is to make the ideal speed V _r reach the target terminal tool posture in a single sampling time, and adjust the speed of the terminal tool under the speed and joint angle limits, so that the arm on the mobile arm vehicle and the mobile platform 200 are close to the ideal speed. The quadratic error of the two is used as the cost function, and the convergence of the error can be accelerated. However, there are many hardware limitations on the robot 1, such as the upper limit of the motion angle of each axis on the robot arm 100 and the hardware limitations of the mobile platform 200, so some penalty functions must be added. P (

) is a logarithmic barrier function to prevent the angle on the axis from approaching the limit, W (

,

,

,

) is a first-order normalization function (Normalization to first normal form), which balances the motion of each axis by giving different weights to each axis and the mobile platform 200.

As shown in FIG. 9 , a method of autonomously moving and grasping an object of the present invention is shown. First, in step S1, the first camera 10 takes a picture of the external environment and transmits it to the semantic navigation system 20; in step S2, the semantic navigation system 20 is used to cooperate with the first camera 10 to autonomously navigate the robot 1 to the position where the target object is located through the mobile platform controller 202; in step S3, the automatic docking coordination controller 30 is used to optimize the redundant degree of freedom and speed control according to the difference between the current position of the robot arm 100 and the mobile platform 200 and the target position by using an optimization algorithm, so that the mobile platform 200 can automatically coordinate and control the mobile base and the robot arm, and simultaneously move the robot arm 100 to the grasping position above the target object. The mobile platform 200 is positioned to dock at the best position; step S4, the mobile platform 200 uses the second camera 11 to shoot the stacked objects on the target workbench from top to bottom; step S5, the robot arm 100 receives the object image captured by the second camera 11; then, step S6, the semantic segmentation module 1022 and the posture estimation module 1023 respectively identify the object posture, and the respective models are ESPNetV2 model and DenseFusion model; step S7, the grasping planning algorithm obtained by the mobile grasping controller 104 calculates the better grasping position of the object to improve the grasping success rate; step S8, the mobile grasping controller 104 controls the robot arm 100 to grasp the object according to the algorithm.

In summary, the present invention can achieve the goal of enabling the robot arm to autonomously complete the grasping task in the environment of daily life. The present invention combines semantic information to enable the robot to understand the environment, and through object posture estimation and grasping planning design, to improve the grasping success rate, and combines the mobile grasping controller to make the robot arm posture and mobile platform reach the grasping position at the same time, so as to efficiently complete the grasping.

1:Robot

10: First Camera

11: Second camera

20:Semantic Navigation System

30: Automatic docking coordination controller

100:Robotic arm

200: Mobile platform

102: Object recognition and posture estimation system

104: Mobile grabbing controller

202: Mobile platform controller

203: Drive system

1021: Image preprocessing module

1022: Semantic segmentation module

1023: Posture estimation module

Claims (10)

A robot for autonomously moving and grasping an object comprises: a mechanical arm for grasping a target object; a mobile platform, carrying the mechanical arm and moving the mechanical arm to a position where the target object is grasped; a semantic navigation system, electrically connected to the mobile platform, for navigating the mobile platform to the position where the target object is located; a first camera, electrically connected to the semantic navigation system, for The navigation system takes photos of the external environment during navigation; a second camera is electrically connected to the robot arm and is used to obtain the relative image of the robot arm to the environment; an object recognition and posture estimation system is electrically connected to the robot arm and controls the robot arm to grasp the target object through semantic recognition segmentation and posture estimation by the second camera, wherein the object recognition and posture estimation system obtains the center of mass and wheel of the target object The object recognition and posture estimation system is used to obtain the center of mass position of the target object in the image plane, and a grasping indicator is generated based on the center of mass position. The DenseFusion model is used to perform posture estimation, and the most appropriate grasping position is selected according to the grasping indicator. An automatic docking coordination controller is electrically connected to the robot arm and the mobile platform, and obtains the robot arm and the mobile platform through the object recognition and posture estimation system. The system can determine the optimal mobile grasping path and posture of the robot, and control the robot arm and the mobile platform so that they can move synchronously for grasping; a mobile grasping controller is electrically connected to the robot arm, and controls the movement of the robot arm through the object recognition and posture estimation system, so that the robot arm grasps according to the grasping index; and a mobile platform controller is electrically connected to the mobile platform and is used to control the movement of the robot. The robot capable of autonomously moving and grasping objects as described in claim 1, wherein the object recognition and posture estimation system further comprises: an image pre-processing module, which is used to process the images captured by the first camera and the second camera and serve as pre-processing of the image depth image; a semantic segmentation module, which is electrically connected to the pre-processing module, and performs image semantic segmentation processing on the RGB color image of the pre-processed image; and, a posture estimation module, which is electrically connected to the semantic segmentation module and the posture estimation module, and is used to estimate the posture and calculate the six-degree-of-freedom posture estimation result of the object after image pre-processing and semantic segmentation. A robot capable of autonomously moving and grasping objects as described in claim 1, wherein the first camera is an image depth camera capable of providing high-quality synchronized video with color and depth. A robot capable of autonomously moving and grasping objects as described in claim 1, wherein the second camera is a hand-eye visual image depth camera. A robot capable of autonomously moving and grasping objects as described in claim 1, wherein the semantic navigation system can pre-establish a semantic map so that the robot arm can reach the location of the target object through the semantic map. A robot capable of autonomously moving and grasping objects as described in claim 1, wherein the mobile grasping controller uses a robotic arm grasping planning algorithm, and the robotic arm grasping planning algorithm is divided into three parts: center of mass and contour segment acquisition, grasping indicator design, and grasping posture transformation. A robot capable of autonomously moving and grasping objects as described in claim 6, wherein the grasping posture transformation is performed by calculating a pinhole model to transform the points of the object on a two-dimensional plane into spatial coordinates by projection. A method for autonomously moving and grasping objects includes the following steps: A first camera takes pictures of the external environment and transmits them to a semantic navigation system; the semantic navigation system is used to cooperate with the first camera to autonomously navigate a robot to the location of a target object through a mobile platform; an automatic docking coordination controller is provided, and an algorithm is used to allow the mobile platform to dock at the best position; a second camera moves the target object from top to bottom The stacked objects on a workbench are photographed; a robot arm receives the image of the target object captured by the second camera; a semantic segmentation module and a posture estimation module respectively calculate and identify the posture of the target object; a mobile grasping controller derives a grasping planning design algorithm and calculates the optimal grasping position of the target object; the mobile grasping controller controls the robot arm to grasp the object according to the algorithm. The method for autonomously moving and grasping an object as described in claim 8, wherein the automatic docking coordination controller performs redundant degree of freedom optimization speed control based on the difference between the current position of the robot arm and the mobile platform and the target position, so that the mobile platform can automatically coordinate and control the simultaneous movement of the mobile base and the robot arm, so that the robot arm moves to the grasping position above the target object. The method for autonomously moving and grasping an object as described in claim 8, wherein the models in the semantic segmentation module and the posture estimation module are an ESPNetV2 model and a DenseFusion model respectively.