Robot manipulator control method, device and equipment and medium

Abstract

The invention provides a robot manipulator control method, device and equipment and a medium, and relates to the technical field of manipulator control, the method comprises the steps that according to obtained stress data of two hands of a robot, a trained neural network model is adopted, and the mass center position of a carried object is determined; on the basis of the double-hand force-moment balance condition, according to the mass center position, the grabbing point position of the carried object grabbed by the double hands of the robot is determined, and the double hands of the robot are controlled to grab the carried object; on the basis of a model prediction control technology and a control obstacle function technology, according to the obtained obstacle information, a collision-free translational motion track of the carried object and collision-free translational motion tracks of the two hands of the robot are generated; and according to the obtained translation acceleration when the two hands of the robot carry the carried object, the expected posture when the two hands of the robot carry the carried object is determined. According to the method, through logic connection among the steps, the defects of object grabbing, two-hand posture adjustment and obstacle avoidance links in the prior art are comprehensively overcome.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot control, in particular to a robot manipulator control method, device, equipment and medium. BACKGROUND

[0002] Robots are intelligent equipment that can autonomously or semi-autonomously perform various tasks through programming, integrating perception, decision-making, execution, and other core functions, and can replace humans to complete repetitive, complex, or dangerous operations in various fields such as industry, service, and scientific research. For example, as an important branch of robots, humanoid upper body robots are designed to simulate the structure and function of the human upper body, focusing on the coordinated operation of the torso, arms, and hands, and are designed for complex upper limb tasks such as grasping, carrying, and assembly. It usually carries a multi-degree-of-freedom manipulator, covering flexible joints such as shoulders, elbows, and wrists, and is equipped with a dexterous hand with an embedded force-torque sensor, and some also integrate a waist rotation joint to expand the operation space. In terms of function, it can obtain real-time environmental information such as force, position, and obstacles through sensors, autonomously plan grasping points and motion trajectories with the help of algorithms, and realize double-arm or waist-arm coordinated motion, which can handle complex scenarios such as centroid-unbalanced object carrying and dynamic obstacle avoidance, and balances operation stability, safety, and dexterity, effectively overcoming the limitations of traditional robots in single task execution, and is commonly used in industrial assembly, service assistance, and scientific research.

[0003] In related technologies, in terms of robot manipulator grasping control, existing methods focus on object stability or manipulator operation ability, without considering the influence of object gravity on the manipulator, which can easily lead to damage to the manipulator or dropping of the object; in terms of robot obstacle avoidance control, existing methods use reinforcement learning, imitation learning, etc., but still have the problems of poor universality, unstable motion, or the need to pause the carrying task; in terms of robot posture control, existing fixed posture control methods reduce the dexterity of the robot, and existing variable posture control methods lack autonomy or have insufficient degrees of freedom, making it difficult to adapt to complex carrying scenarios. SUMMARY

[0004] The present application aims to solve at least one of the above problems.

[0005] To solve the above problems, the present application provides a robot manipulator control method, device, equipment and medium.

[0006] In a first aspect, the present application provides a robot manipulator control method, comprising: determining the centroid position of the carried object using a trained neural network model according to the obtained force data of the robot hands; determine a grasp point position of the robot double hands grasping the carried object according to the center of mass position based on a double hands force-torque balance condition, and control the robot double hands to grasp the carried object; generate a collision-free translation motion trajectory of the carried object and a collision-free translation motion trajectory of the robot double hands according to the obtained obstacle information based on a model predictive control technique and a control barrier function technique; determine an expected pose of the robot double hands carrying the carried object according to an obtained translation acceleration of the robot double hands carrying the carried object.

[0007] Optionally, the determining the expected pose of the robot double hands carrying the carried object according to the obtained translation acceleration of the robot double hands carrying the carried object comprises: determining a synthetic acceleration according to the translation acceleration and a preset modified auxiliary acceleration, wherein the translation acceleration is determined according to the collision-free translation motion trajectory of the carried object; determining the expected pose of the robot double hands carrying the carried object according to an expected pose rotation matrix constructed according to the synthetic acceleration.

[0008] Optionally, the generating the collision-free translation motion trajectory of the carried object and the collision-free translation motion trajectory of the robot double hands according to the obtained obstacle information based on the model predictive control technique and the control barrier function technique comprises: adopting a five-level integrator and a second-order forward difference to construct a discrete point mass kinematics model of the carried object; determining constraint conditions according to the obstacle information and the discrete point mass kinematics model based on the model predictive control technique and the control barrier function technique, adding a relaxation variable and a regularization term, and constructing an MPC-CBF optimization problem; solving the MPC-CBF optimization problem to generate the collision-free translation motion trajectory of the carried object, and determining the collision-free translation motion trajectory of the robot double hands according to the collision-free translation motion trajectory of the carried object.

[0009] Optionally, the determining the grasp point position of the robot double hands grasping the carried object according to the center of mass position based on the double hands force-torque balance condition comprises: establishing a force-torque balance relationship of the robot double hands according to the center of mass position; constructing a grasp point optimization problem satisfying the double hands force-torque balance condition according to the force-torque balance relationship, solving the grasp point optimization problem, and obtaining the grasp point position.

[0010] Optionally, the controlling the robot dual hands to grasp the carried object comprises: According to the dual hands grasping point, based on a dual arm reachability condition, determining dual arm reachability data of the robot; When the dual arm reachability data is less than a reachability threshold, controlling the robot dual hands to grasp the carried object through waist-arm coordinated motion; When the dual arm reachability data is greater than or equal to the reachability threshold, controlling the robot dual hands to grasp the carried object through dual arm coordinated motion.

[0011] Optionally, after determining the expected pose of the robot dual hands when carrying the carried object, further comprising: According to the collision-free translational motion trajectory of the robot dual hands and the expected pose of the robot dual hands when carrying the carried object, generating joint velocity instructions.

[0012] Optionally, before the determining the center of mass position of the carried object according to the acquired force data of the robot dual hands, using a trained neural network model, further comprising: Controlling the robot dual hands to grasp the carried object at an initial position, and acquiring the force data using a six-dimensional force-torque sensor; Controlling the robot to put down the carried object.

[0013] In a second aspect, the present application provides a control device of a robot manipulator, comprising: A center of mass module for determining the center of mass position of the carried object according to the acquired force data of the robot dual hands, using a trained neural network model; A grasping module for determining the grasping point position of the robot dual hands to grasp the carried object according to the center of mass position based on a dual hands force-torque balance condition, and controlling the robot dual hands to grasp the carried object; An obstacle avoidance module for generating the collision-free translational motion trajectory of the carried object and the collision-free translational motion trajectory of the robot dual hands according to the acquired obstacle information based on model predictive control technology and control barrier function technology; A carrying module for determining the expected pose of the robot dual hands when carrying the carried object according to the acquired translational acceleration of the robot dual hands when carrying the carried object.

[0014] In a third aspect, the present application provides an electronic device comprising a memory and a processor; The memory is used to store a computer program; The processor is used to implement the control method of the robot manipulator as described in the first aspect when executing the computer program.

[0015] In a fourth aspect, the present application provides a computer readable storage medium, wherein the storage medium stores a computer program, and when the computer program is executed by a processor, the control method of the robot manipulator according to the first aspect is implemented.

[0016] The control method of the robot manipulator, the device, the equipment and the medium of the present application have the following beneficial effects: By obtaining the force data of the robot hands, using the trained neural network model capable of inversely deducing the center of mass distribution according to the force data, the center of mass position of the object being carried can be accurately determined, and then based on the double-hand force-torque balance condition for targetedly offsetting the additional force of the object gravity on the manipulator, the optimal double-hand grabbing point is calculated and the robot is controlled to re-grab, thereby solving the problem that the prior art does not consider the gravity influence and is prone to cause damage to the manipulator or object falling; in combination with the obstacle information, the model predictive control technology and the control barrier function technology are used to generate the collision-free translation motion trajectory of the robot hands, the multi-step prediction capability of the model predictive control technology can guarantee the continuity of the trajectory, the control barrier function technology can construct the obstacle avoidance constraint in real time, and the combination of the two can cope with the dynamic obstacle speed change without relying on specific task space integration and without pausing the carrying task, thereby effectively overcoming the defects of poor generality, unstable motion or task interruption of the existing obstacle avoidance methods; then the desired pose of the manipulator is determined according to the translation acceleration of the robot hands when carrying the object, since the translation acceleration directly reflects the motion state change of the object and the hands, and the adaptive adjustment of the hand pose based on this parameter can maximize the motion freedom of the object and the robot hands, thereby breaking through the limitations of the existing fixed robot hand pose, the variable robot hand pose lacks autonomy or freedom, and the robot is more suitable for complex carrying scenarios, and the overall logic connection between the steps can comprehensively make up for the deficiencies of the prior art in the three core links of object grabbing, hand pose adjustment and obstacle avoidance. BRIEF DESCRIPTION OF DRAWINGS

[0017] Figure 1 A flowchart of the control method of the robot manipulator provided by the embodiment of the present application is shown in the figure; Figure 2 A structural schematic diagram of a simulation model of the robot grabbing a tray and an object provided by the embodiment of the present application is shown in the figure; Figure 3 One of the simplified schematic diagrams of the robot hands grabbing the tray plane provided by the embodiment of the present application is shown in the figure; Figure 4 The other of the simplified schematic diagrams of the robot hands grabbing the tray plane provided by the embodiment of the present application is shown in the figure; Figure 5 A structural schematic diagram of the control device of the robot manipulator provided by the embodiment of the present application is shown in the figure; Figure 6 A structural schematic diagram of an electronic device provided by an embodiment of the present application is shown. DETAILED DESCRIPTION

[0018] In order to make the above objectives, features and advantages of the present application more apparent, specific embodiments of the present application will be described in detail below with reference to the accompanying drawings. Although some embodiments of the present application are shown in the drawings, it should be understood that the present application can be implemented in various forms and should not be construed as being limited to the embodiments set forth herein, but rather, these embodiments are provided so as to more completely and thoroughly understand the present application. It should be understood that the drawings and embodiments of the present application are only for illustrative purposes and are not intended to limit the scope of the present application.

[0019] It should be understood that each of the steps described in the method embodiments of the present application can be performed in different orders and / or in parallel. In addition, the method embodiments can include additional steps and / or omit the steps shown. The scope of the present application is not limited in this respect.

[0020] As used herein, the term "includes" and its variants are open-ended, meaning "includes but is not limited to"; the term "based on" is "based, at least in part, on"; the term "one embodiment" means "at least one embodiment"; the term "another embodiment" means "at least one additional embodiment"; the term "some embodiments" means "at least some embodiments"; the term "optionally" means "optional embodiments". Related definitions are given throughout the description. It should be noted that the concepts mentioned in the present application are merely used to distinguish different devices, modules or units, and are not intended to limit the functions performed by these devices, modules or units or the order or interdependence of these functions.

[0021] It should be noted that the modification of "one" or "multiple" mentioned in the present application is illustrative and not limiting, and those skilled in the art should understand that, unless otherwise explicitly indicated in the context, it should be understood as "one or more".

[0022] The names of the messages or information exchanged between the devices in the embodiments of the present application are only for illustrative purposes and are not intended to limit the scope of the messages or information.

[0023] As Figure 1 shown, the control method of the robot manipulator provided by the embodiment of the present application comprises: According to the force data of the robot double hands obtained, a trained neural network model is used to determine the center of mass position of the object being carried.

[0024] Specifically, during transport, the object to be transported is placed in a tray. The tray includes a tray plane and a handle. The handle can be cylindrical or other shapes that facilitate gripping by the robot's manipulator. The robot grasps the tray handle to transport the object. Before transport, the object is pre-grabbed to obtain force data, and then the object is placed down. In this embodiment, the object to be transported refers to the combination of the object and the tray, i.e., the object-tray whole. The center of mass position refers to the projection of the actual center of mass of the object-tray whole onto the tray plane. A manipulator is connected to the robot's robotic arm, which can grasp the tray handle. Sensors can be used to obtain force data from the robot's hands, for example, two six-dimensional force-torque sensors mounted on the robot's arms. The force data includes the force and torque acting on the robot. Then, a trained neural network model, such as a backpropagation neural network, a multilayer perceptron (MLP), or a radial basis function network (RBFNetwork), is used to analyze the force data to obtain the center of mass position of the object to be transported.

[0025] For example, the training process of a neural network model includes: using a physics simulation engine, such as Vrep, to build a... Figure 2 The robot grasping simulation model shown here refers to the tray handle, the force-torque sensor, the tray flat platform, and the tray coordinate system, denoted as ∑. T Object refers to the physical object, and Force-torque sensor coordinate refers to the force-torque sensor coordinate system, denoted as ∑. F Dexteroushand is the robotic arm of a robot. W Using the world coordinate system, l t,r The straight line on the right edge of the tray plane. l t,l The straight line on the left edge of the tray plane. l h,r The right handle of the tray is in a straight line. l h,l For the straight line of the left handle of the tray, p com Let H and L be the location of the center of mass. α t For the dimensions of the pallet, p1 * and p2 * p represents the virtual gripping points of the robot's two robotic arms on the tray plane. h,1 * and p h,2 *The actual gripping points of the robot's two manipulators are then used. Objects of different masses are randomly placed at different positions on the tray to obtain corresponding force and torque data, i.e., force data, and a training set is constructed. Using the training set, an algorithm for determining the parameters of a neural network is designed using a set of empirical formulas and a centroid prediction error algorithm. The pre-trained neural network model is then trained to determine the number of hidden layers and the number of neurons in each layer. The set of empirical formulas includes: , , , , ; Where, n j (j∈[1,h]) represents the number of hidden layer nodes, h represents the number of hidden layers, k represents the iteration parameter, and N represents the number of hidden layer nodes. in N represents the number of nodes in the input layer of the neural network. out α represents the number of nodes in the output layer of the neural network, and α, β, and γ are all node count decay coefficients.

[0026] The specific algorithm for determining neural network parameters is as follows: Input parameters: Number of neurons in the input layer N in Number of output layer neurons N out Maximum number of hidden layers N max Filtering parameter N avr Training dataset A, validation dataset B, and validation data size N val The training dataset is A, the validation dataset is B, and the amount of validation data is N. val The simulation model is obtained by the robot grasping it; the number of hidden layers is traversed: let the number of hidden layers be... l Cycle from 1 to N max ; Generate combinations of hidden layer node numbers: for each l Let k cycle from 1 to 10, and generate the number of nodes in each layer according to the empirical formula; train the neural network: train the neural network model with the current combination of node numbers using the training dataset A; calculate the validation error: initialize the error and E. sum =0, let k cycle from 1 to N avr Each time, N is selected from the validation dataset B. val Data points are used to obtain predicted values ​​through a trained network, and the mean squared error V between the predicted and actual values ​​is calculated. mse And accumulate to E sum Record parameters and errors: Record the current combination of hidden layer node counts and the corresponding total error E. sum ; Filtering for optimal parameters: E for all combinations sumSort and select the node combination with the minimum total error; output the result: determine the optimal number of hidden layers and the corresponding number of nodes for each layer.

[0027] Exemplarily, the final number of neural network hidden layers is 3, and the number of neurons in each layer is 11, 9, and 7, respectively.

[0028] Based on the double-hand force-torque balance condition, the centroid position is determined to determine the grasping point position of the robot double hands grasping the object to be carried, and the robot double hands are controlled to grasp the object to be carried.

[0029] Specifically, the double-hand force-torque balance condition means that the force on the robot double hands is balanced, and the undesired overturning torque at the double-hand grasping point is 0, i.e. the double-hand torque balance, so that the double-hand grasping point of the robot double hands grasping the object to be carried is determined according to the centroid position to balance the force and torque of the robot double hands, so that the robot can more stably grasp the object to be carried for carrying.

[0030] Based on the model predictive control technology and the control barrier function technology, the obtained obstacle information is used to generate the collision-free translation motion trajectory of the object to be carried and the collision-free translation motion trajectory of the robot double hands.

[0031] Specifically, model predictive control (MPC) is an advanced control strategy based on rolling optimization, and the core is to predict the state evolution in the future period of time by using the system dynamic model, and to obtain the optimal control sequence by solving the optimization problem online. The control barrier function technology (CBF) is a constraint design method for ensuring system safety, which converts safety requirements such as "no collision" into inequality constraints that can be included in the optimization problem by constructing a differentiable function about the system state and obstacles. The multi-step prediction capability of model predictive control technology can guarantee the continuity of the trajectory, and the control barrier function technology can construct obstacle avoidance constraints in real time, and the combination of the two can obtain the collision-free translation motion trajectory of the object to be carried and the collision-free translation motion trajectory of the robot double hands, without relying on specific task space integration, and can cope with dynamic obstacle speed changes, and does not need to pause the carrying task, effectively overcoming the defects of poor generality, unstable motion or the need to interrupt the task of existing obstacle avoidance methods.

[0032] According to the obtained translation acceleration of the robot double hands carrying the object to be carried, the desired pose of the robot double hands carrying the object to be carried is determined.

[0033] Specifically, during the movement of the robot's double arms carrying the object, it is assumed that the double hands rigidly grasp the tray, and the relative pose between the hands and the tray remains unchanged, then the change of the pose of the tray is consistent with the change of the pose of the double hands. In order to prevent the generation of an unexpected internal force between the double hands and the tray, the relative pose between the double hands remains unchanged. At the same time, if the absolute pose of the robot's double hands in the global coordinate system also remains unchanged, that is, only the translational motion of the double hands is allowed during the carrying process, and the rotational motion is prohibited, which will greatly reduce the dexterity of the robot's double arms. In order to solve this problem, the embodiment determines the expected pose of the robot's double hands carrying the carried object according to the obtained translational acceleration of the object when the robot carries the carried object, that is, the pose of the tray is adaptively changed according to the acceleration of the translational motion of the tray, thereby increasing the dexterity of the robot's double arms.

[0034] In the embodiment, by obtaining the force data of the robot's double hands, using the trained neural network model capable of inversely deducing the center of mass distribution according to the force data, the center of mass position of the carried object can be accurately determined, and then based on the double hand force-torque balance condition for targetedly offsetting the additional force of the object gravity on the manipulator, the optimal double hand grasping point is calculated and the robot is controlled to re-grasp, thereby solving the problem that the prior art does not consider the influence of the gravity and is prone to damage the manipulator or cause the object to fall; combined with the obstacle information, the model predictive control technology and the control barrier function technology are used to generate the collision-free translational motion trajectory of the robot's double hands, the multi-step prediction capability of the model predictive control technology can guarantee the continuity of the trajectory, and the control barrier function technology can construct the obstacle avoidance constraint in real time, the combination of the two can cope with dynamic obstacle speed changes without relying on specific task space integration, and the carrying task does not need to be paused, thereby effectively overcoming the defects of the prior art, such as poor universality, unstable motion, or the need to interrupt the task; then the expected pose of the manipulator is determined according to the translational acceleration of the robot's double hands carrying the object, since the translational acceleration directly reflects the change of the motion state of the object and the double hands, adaptively adjusting the pose of the double hands based on this parameter can maximize the motion freedom of the object and the robot's double hands, thereby breaking through the limitations of the prior art, such as the fixed pose of the robot's double hands limiting the dexterity of the robot's double arms, the variable pose of the robot's double hands lacking autonomy or insufficient freedom, and the like, and making the robot more suitable for complex carrying scenarios. Through the logical connection between the steps, the deficiencies of the prior art in the three core links of object grasping, double hand pose adjustment, and obstacle avoidance are comprehensively made up.

[0035] Optionally, the determining the expected pose of the robot's double hands carrying the carried object according to the obtained translational acceleration of the robot's double hands carrying the carried object comprises: determining a synthetic acceleration according to the translational acceleration and a preset modified auxiliary acceleration, wherein the translational acceleration is determined according to the collision-free translational motion trajectory of the carried object; According to the desired pose rotation matrix constructed based on the synthetic acceleration, a desired pose of the robot when carrying the carried object is determined.

[0036] Specifically, the translational acceleration and the preset modified auxiliary acceleration are vector-added to obtain the synthetic acceleration, i.e., , is the synthetic acceleration of the tray control point, is the translational acceleration of the carried object when the robot carries the carried object, i.e., the translational acceleration of the tray control point, as shown in Figure 2 , the tray control point is located at the origin of the tray center coordinate system ∑ T , and can also be expressed as an acceleration trajectory, i.e., , the acceleration trajectory of the tray is expressed as is the acceleration of the motion trajectory of the origin of the tray center coordinate system, i.e., the acceleration of the collision-free translational motion trajectory of the carried object, the collision-free translational motion trajectory includes position, velocity and acceleration, and t is time, is the preset modified auxiliary acceleration, and = , used to reduce an undesired tilt angle of the tray θ s , which is caused by excessive acceleration of the tray, wherein, is a design parameter, is a three-dimensional real vector space. During the motion of the tray, is set as a constant, and its value depends on the maximum acceleration of the tray motion and the allowed maximum tilt angle of the tray, , , , is the maximum value of the synthetic acceleration, is the maximum value of the synthetic acceleration in the horizontal direction, is the maximum value of the synthetic acceleration in the vertical direction. Then, according to the desired pose rotation matrix constructed based on the synthetic acceleration, a desired pose of the robot when carrying the carried object is determined, and the specific steps are as follows: align the third column component of the rotation matrix with the synthetic acceleration acting at the tray control point: ; Next, a two-dimensional direction vector is defined: ; wherein, is a two-dimensional direction vector, and σ is an angle parameter, and the value of σ satisfies that is aligned with the x-axis of the tray center coordinate system ∑ T .

[0037] Then, the first column component of the rotation matrix and the second column component can be calculated: ; ; Finally, we can store the above three components to obtain the desired pose of the tray , the desired pose of the tray can be obtained through coordinate transformation when the robot double hands carry the carried object, wherein the desired pose of the tray is: .

[0038] Optionally, the model predictive control technology and the control barrier function technology generate the collision-free translational motion trajectory of the carried object and the collision-free translational motion trajectory of the robot double hands according to the obtained obstacle information, including: A five-level integrator and a second-order forward difference are used to construct a discrete point mass kinematics model of the carried object; Based on the model predictive control technology and the control barrier function technology, the constraint conditions are determined according to the obstacle information and the discrete point mass kinematics model, and a relaxation variable and a regularization term are added to construct an MPC-CBF optimization problem; Solving the MPC-CBF optimization problem generates the collision-free translational motion trajectory of the carried object, and the collision-free translational motion trajectory of the robot double hands is determined according to the collision-free translational motion trajectory of the carried object.

[0039] Specifically, in the robot carrying scene, C4 continuity is the fourth-order derivative continuity of the trajectory or function, which means that the position, velocity, acceleration, jerk (third-order derivative), and jounce (fourth-order derivative) of the motion trajectory are all continuous, and it is a high-order smooth continuous motion characteristic. Because the double-hand pose is dynamically bound to the tray acceleration, the joint angular velocity and angular acceleration are related to the jounce and jounce of the tray, respectively. Only when the collision-free translational motion trajectory satisfies C4 continuity, can the joint motion be smooth and the force / torque change be stable, which not only protects the mechanical arm structure, but also improves the carrying precision and safety. Therefore, on the one hand, in order to meet the C4 continuity, a five-level integrator and a second-order forward difference are used to construct the discrete point mass kinematics model of the carried object: ; wherein, is the state vector at t+1, is the state vector, and , , , , , , , , , , , , ,

[0040] , , , , , , , , , , , , , , , , , , , , , , ; ; ; ; where subscript t : t + N -1| t denotes the time period between t and t+N-1 starting from time t, is the system control quantity in the time period between t and t+N-1, subscript t + N | t denotes the time t+N after the N time points starting from time t, is the system state at time t+N, subscript t + k | t denotes the time t+k after the k time points starting from time t, is the system state at time t+k, is the system control quantity at time t+k, n o is the number of obstacles, n c is the number of robot collision detection points, subscript t | t denotes the time t after the t time points starting from time t, is the system state at time t, is the system control quantity at time k, is the system state at time k, is the initial condition.

[0041] ; ; where the terminal cost is: ; where is the expected value of the system state, is the actual value of the system state.

[0042] The stage cost is: ; is a regularization term, is a discrete point mass kinematics model, denotes the initial condition, i.e., the actual state of the system at the current time step , , and Feasible sets of system input, system state and terminal state, P, Q, R and S are the corresponding positive definite weight matrices, The reaction of the distance information between the obstacle and the collision detection point on the robot system, it can be constructed as follows the form of the differentiable quadratic obstacle function: ; Where, And The center of the i-th obstacle and the center of the j-th collision detection point are represented, respectively. The position of the robot collision detection point can be obtained by the coordinate transformation of the position of the tray control point p c(t) , r o,i The radius of the obstacle is r c,j The radius of the collision detection point is s The safety threshold of collision detection is δ. Finally, the above MPC-CBF optimization problem is solved to generate the collision-free translation motion trajectory of the carried object, and the collision-free translation motion trajectory of the robot double hands is obtained through coordinate transformation according to the collision-free translation motion trajectory of the carried object.

[0043] Optionally, the double-hand force-torque balance condition is established according to the center of mass position, and the grabbing point position of the robot double hands grabbing the carried object is determined, comprising: According to the center of mass position, the force-torque balance relationship of the robot double hands is established; According to the force-torque balance relationship, a grabbing point optimization problem satisfying the double-hand force-torque balance condition is constructed, and the grabbing point optimization problem is solved to obtain the grabbing point position.

[0044] Specifically, the schematic diagram of the robot double hands grabbing the tray plane is as shown in Figure 3 The grabbing point of one of the robot manipulators is p 1, and the grabbing point of the other manipulator is p 2, p 1and the center of mass position p com The line connecting d 1, P 2and the center of mass position p com is defined as the virtual connecting rod d 2, then d 1= p com - p 1, d 2= p 2- p com, the relative position vector between the two grasp points can be defined as p r = p 2- p 1, the force at each grasp point can be expressed as: ; where, f 1is the force of the left robot arm, is the projection amplitude of the virtual link d 1on the relative position vector p r , f com is the force acting on the center of mass, and f com = f 1+ f 2, f 2is the force of the right robot arm, is the projection amplitude of the virtual link d 2on the relative position vector p r , where the projection amplitude is calculated as: ; where, is the projection amplitude of the virtual link d i corresponding to the transpose of the vector.

[0045] The moment of force at the first grasp point and the moment of force at the second grasp point are expressed as: ; ; Considering the effect of two-handed grasping on the moment of force at each grasp point, assuming that the robot two-handed grasps the tray as a rigid grasp, in the tray coordinate system ∑ T , the moments of force at the two grasp points are equal, and related to the distance p com between the center of mass position p r and the relative position vector , can be defined as: ; Therefore, the moment of force at the two-handed grasp point can be rewritten as , that is, the force-moment balance relationship. Since f comThe component along the normal direction of the tray plane will generate an undesirable overturning torque about p r which can make the tray fall from the hands or damage the manipulator, minimizing which can reduce the above risks, the double-hand force-torque balance condition can be obtained, i.e., the forces on the robot hands are balanced, and the undesirable overturning torque at the double-hand gripping points is 0, i.e., the double-hand torque balance: ; ; wherein, f e is the double-hand force error, is the double-hand torque error, is the vector related to the centroid position and the gripping point position, which determines the key parameters of torque balance.

[0046] Although the double-hand force-torque balance condition can limit the robot to stable gripping, in actual operation, the gripping position of the robot hands can only change within a certain range due to actual conditions, such as being limited by the size of the tray, so it is difficult to satisfy the double-hand force-torque balance condition. Therefore, in order to satisfy the double-hand force-torque balance condition as much as possible, i.e., to minimize the double-hand force error and the double-hand torque error, a gripping point optimization problem that satisfies the double-hand force-torque balance condition can be constructed: ; ; ; wherein, α, β, λ are weight coefficients, and are inequality constraint functions, which limit the feasible gripping points of the robot hands to the straight lines l 1and l 2along the edges of the tray, is an additional cost, i.e., an objective function, which can make the distance between the double-hand gripping points as small as possible to select an optimal set of gripping point positions from multiple sets of gripping points that satisfy the double-hand force-torque balance condition, is represented as: ; wherein, p t，c is the tray center position, d th is a preset distance threshold for judging whether the centroid deviates from the tray center, d minThis represents the minimum allowable distance between the gripping points of both hands, used to ensure that the distance between the gripping points is minimized, thereby improving the dexterity of the robot's operation. Then, based on solving the gripping point optimization problem, we obtain the following... Figure 4 The virtual two-hand gripping point shown and Since the virtual two-handed gripping point is located on a simplified diagram, it needs to be converted into the actual gripping point on the tray handle, i.e., the two-handed gripping point, using a conversion formula. The conversion formula is as follows: ; in, For one of the virtual crawl points, For another virtual crawl point, As one of the actual crawling points, For another actual crawl point, GraspTransform is the transformation function. H, L and α t These are all pallet size parameters, such as Figure 2 As shown, H The distance from the center point of the central axis of the cylindrical handle of the pallet to the pallet plane. L The vertical projection of the center point of the central axis of the pallet cylindrical handle onto the pallet plane is a straight line from the edge of the pallet. Length, α t Let be the angle between the central axis of the cylindrical handle of the pallet and the plane of the pallet. Furthermore, in the pallet coordinate system ∑ T The following represents the gripping points of both hands, ∑ i The pose can be represented as: ; in, Let the i-th grab point be in the tray coordinate system ∑ T The homogeneous transformation matrix (4x4 matrix) contains position and orientation information. R i Let be the attitude matrix of the reference coordinate system at the i-th grasping point. This refers to the location of the capture point.

[0047] Optionally, controlling the robot to grasp the object being transported with both hands includes: Based on the gripping points of both hands, and considering the reachability conditions of both arms, the reachability data of the robot's two arms is determined; When the reachability data of the two arms is less than the reachability threshold, the robot's hands are controlled to grasp the object being transported through coordinated waist and arm movements. When the reachability data of the two arms is greater than or equal to the reachability threshold, the robot's two arms are controlled to grasp the object being transported through coordinated movement of the two arms.

[0048] Specifically, after obtaining the double-hand grasping point position, the robot double hands re-grasp the tray handle at the double-hand grasping point position. However, since there is a difference between the double-hand grasping point and the robot double hands position, and the difference between different double-hand grasping points and robot double hands positions is different in each time of carrying, the robot double hands need to be moved to the double-hand grasping point in combination with different motion strategies. Therefore, the double-arm reachability data of the robot is determined through the double-arm reachability condition, and the final motion strategy, such as waist-arm cooperative motion or double-arm cooperative motion, is determined through the double-arm reachability data. The double-arm reachability data can be obtained by querying the reachability map online, and the formula is as follows: ; Among them, , S i is the double-arm reachability score, i.e., the double-arm reachability data, is the coordinate transformation of the coordinate system of the i th robot arm relative to the global coordinate system, i is the pose (homogeneous transformation matrix containing position and attitude information) of the i th real grasping point relative to the global coordinate system, is the pose transformation matrix of the tray coordinate system relative to the global coordinate system (known preset parameters), which is responsible for converting the pose in the local coordinate system of the tray to the global coordinate system, is the pose (homogeneous transformation matrix) of the i th real grasping point in the tray coordinate system, and ReachabilityMap is the double-arm reachability mapping function. When the double-arm reachability data is less than the reachability threshold, it means that the reachability of the robot double arms is very low at the current waist joint angle, which may be because the tray is far away from the position of the robot and the desired grasping point is on the side of the tray edge far away from the robot, so the waist-arm cooperative motion is needed to control the robot to move to the double-hand grasping point, because the motion of the waist joint can expand the workspace of the double arms; when the double-arm reachability data is greater than or equal to the reachability threshold, only the double-arm cooperative motion is needed to control the robot double hands to move to the grasping point to grasp the carried object, and the waist joint angle can remain at the current value. Through the double-arm reachability condition, the robot can preferentially consider adopting the double-arm cooperative motion, and only in the case of very poor double-arm reachability, the robot will simultaneously utilize the motion of the waist and the double arms. This scheme improves the stability of double-hand operation and maximally reduces the energy consumption of the robot. Optionally, after determining the desired pose of the robot double hands when carrying the carried object, the method further comprises:

[0049] generating joint speed instructions according to the collision-free translational motion trajectory of the robot double hands and the desired pose of the robot double hands when carrying the carried object.

[0050] ​Specifically, the specific solving process of the collision-free translational motion trajectory of the robot hands and the desired pose of the robot hands when carrying the carried object is as follows: The position and pose of the tray control point in the global coordinate system represented by the homogeneous transformation matrix are as follows: ; wherein, is the collision-free translational motion trajectory of the carried object, which can obtain the collision-free translational motion trajectory of the robot hands after coordinate conversion, is the desired pose of the tray, that is, the desired pose of the carried object when the robot hands carry the carried object, which can obtain the desired pose of the robot hands when carrying the carried object after coordinate conversion.

[0051] Further, the dual-hand pose represented by the homogeneous transformation matrix relative to the dual-arm coordinate system can be represented as: ; wherein, is the transformation matrix of the dual-arm coordinate system relative to the global coordinate system, i=1 corresponds to the left arm, and i=2 corresponds to the right arm. c is the pose of the tray control point represented by the homogeneous transformation matrix, that is, , is the pose matrix of the i-th real grasping point in the tray coordinate system, i=1 corresponds to the left-hand grasping point, and i=2 corresponds to the right-hand grasping point. includes the collision-free translational motion trajectory of the robot hands and the desired pose of the robot hands when carrying the carried object.

[0052] The dual-hand pose represented by the homogeneous transformation matrix can also be converted into a compact form: ; is represented as , the collision-free translational motion trajectory of the robot hands is represented as , and the desired pose of the robot hands when carrying the carried object is represented by Euler angles , and , is a six-dimensional real vector space, is a three-dimensional real vector space.

[0053] According to the desired pose of the robot hands and the collision-free translational motion trajectory of the robot hands, a joint velocity command formula is used to generate a joint velocity, and a joint velocity command is obtained, and the joint velocity command formula is: ; wherein, is the joint velocity, and , is a fourteen-dimensional real vector space, is a twelve-dimensional real vector space, , is the translational velocity, and is obtained by The first derivative with respect to time is obtained, is the angular velocity, and The angular velocity can be obtained by Euler angles The derivative with respect to time is obtained by matrix transformation, that is, , is the transformation matrix. J1 and J2 represent the Jacobian matrices of one of the arms and the other arm, respectively, and J m is the combined Jacobian matrix of the dual arms.

[0054] Optionally, before the mass center position of the object to be carried is determined according to the acquired force data of the robot dual hands by using the trained neural network model, the method further comprises the following steps of: controlling the robot dual hands to grasp the object to be carried at an initial position, and acquiring the force data by using a six-dimensional force-torque sensor; controlling the robot to put down the object to be carried.

[0055] Specifically, when the force data of the robot dual hands is acquired, the object to be carried is pre-grasped, that is, the robot dual hands first grasp the tray at the initial position, that is, the center position of the tray handle, and carry the object to be carried vertically upward for a distance, so that the object to be carried is separated from the support plane. At this time, the robot dual hands are subjected to the force and torque caused by the gravity of the object to be carried, and the force and torque, that is, the force data, are acquired by the six-dimensional force-torque sensor, and then the object to be carried is put down.

[0056] Exemplarily, part of the parameters are set according to actual conditions, for example, the preset correction auxiliary acceleration is set as [0, 0, 10] T , and the parameters of model predictive control (MPC) are: N=19, Q=diag(10 5 I 5, 10 5 I 5, 10 5 I 5), P=diag(10 5 I 5, 10 5 I 5, 10 5 I 5), and R=10 5I 3, S = 10 4 I 3, = 10, the parameter g of the control barrier function technique (CBF) is set to 0.4, the radius of the collision detection point enclosing sphere is set to 10mm, the obstacle radius is set to 50mm, and the safety distance is set to 10mm. r c,j r o,i δ s

[0057] As shown in Figure 5 , the control device of the robot manipulator provided by the embodiment of the application comprises: a center of mass module configured to determine the center of mass position of the object to be carried by using a trained neural network model according to acquired force data of robot hands; a grasping module configured to determine a grasping point position of the object to be carried by the robot hands according to the center of mass position based on a force-torque balance condition of the hands, and control the robot hands to grasp the object to be carried; an obstacle avoidance module configured to generate a collision-free translational motion trajectory of the object to be carried and a collision-free translational motion trajectory of the robot hands according to acquired obstacle information based on a model predictive control technique and a control barrier function technique; a carrying module configured to determine an expected pose of the robot hands when carrying the object to be carried according to acquired translational acceleration of the robot hands when carrying the object to be carried.

[0058] As shown in Figure 6 , the electronic device 600 provided by the embodiment of the application comprises a memory 610 and a processor 620; the memory 610 is configured to store a computer program; and the processor 620 is configured to implement the control method of the robot manipulator as described above when executing the computer program.

[0059] Alternatively, the electronic device 600 comprises a memory 610 and a processor 620 coupled to the memory 610; the memory 610 is configured to store a computer program; and the processor 620 is configured to perform the following operations when executing the computer program: determine the center of mass position of the object to be carried by using a trained neural network model according to acquired force data of robot hands; determine a grasping point position of the object to be carried by the robot hands according to the center of mass position based on a force-torque balance condition of the hands, and control the robot hands to grasp the object to be carried; ​​​generate a collision-free translational motion trajectory of the object to be carried and a collision-free translational motion trajectory of the robot hands based on the obtained obstacle information according to the model predictive control technology and the control barrier function technology; determine an expected pose of the robot hands when carrying the object to be carried according to the obtained translational acceleration of the robot hands when carrying the object to be carried.

[0060] The embodiment of the present application provides a computer readable storage medium, and the storage medium stores a computer program. When the computer program is executed by a processor, the control method of the robot manipulator is realized.

[0061] Alternatively, a non-volatile computer readable storage medium stores a computer program. When the computer program is executed by a processor, the processor executes the following operations: determine the center of mass position of the object to be carried according to the obtained force data of the robot hands by using the trained neural network model; determine the grasping point position of the object to be carried by the robot hands according to the center of mass position based on the force-torque balance condition of the two hands, and control the robot hands to grasp the object to be carried; generate a collision-free translational motion trajectory of the object to be carried and a collision-free translational motion trajectory of the robot hands based on the obtained obstacle information according to the model predictive control technology and the control barrier function technology; determine an expected pose of the robot hands when carrying the object to be carried according to the obtained translational acceleration of the robot hands when carrying the object to be carried.

[0062] An electronic device 600, which can be a server or a client of the present application, will now be described, which is an example of a hardware device that can be applied to various aspects of the present application. The electronic device 600 is intended to represent various forms of digital electronic computer devices, such as laptops, desktops, tablets, personal digital assistants, servers, blade servers, mainframes, and other appropriate computers. The electronic device 600 can also represent various forms of mobile devices, such as personal digital processors, cellular telephones, smart phones, wearable devices, and other like computing devices. The components shown here, their connections, and their functions, are meant to be examples only, and are not intended to limit the implementations of the present application described and / or claimed in this document.

[0063] The electronic device 600 includes a computing unit that can perform various appropriate actions and processes in accordance with a computer program stored in a read only memory (ROM) or a computer program loaded from a storage unit into a random access memory (RAM). In the RAM, various programs and data required for device operation can also be stored. The computing unit, the ROM, and the RAM are connected to each other through a bus. An input / output (I / O) interface is also connected to the bus.

[0064] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing relevant hardware through a computer program, and the program can be stored in a computer readable storage medium. When the program is executed, it can include the processes of the above-mentioned embodiment methods. The storage medium can be a magnetic disc, an optical disc, a read-only memory (ROM), a random access memory (RAM), or the like. In this application, the units described as separate components can or can not be physically separated, and the components shown as units can or can not be physical units, that is, they can be located in one place or distributed on multiple network units. Part or all of the units can be selected according to actual needs to achieve the purpose of the embodiment of the present application. In addition, the functional units in each embodiment of the present application can be integrated in one processing unit, or each unit can exist physically, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of a software functional unit.

[0065] Although the present application is disclosed as above, the protection scope of the present application is not limited to this. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present application, and these changes and modifications will fall within the protection scope of the present application.

Claims

1. A control method of a robot manipulator, characterized by, The method comprises the following steps: According to the acquired force data of the robot's two hands, a trained neural network model is used to determine the center of mass position of the object being carried; Based on the force-torque balance condition of the two hands, the center of mass position is used to determine the grasping point position of the object being carried by the robot's two hands, and the robot's two hands are controlled to grasp the object being carried; Based on the model predictive control technology and the control barrier function technology, the acquired obstacle information is used to generate the collision-free translation motion trajectory of the object being carried and the collision-free translation motion trajectory of the robot's two hands; According to the acquired translation acceleration of the robot's two hands when carrying the object being carried, the expected pose of the robot's two hands when carrying the object being carried is determined.

2. The control method of the robot manipulator according to claim 1, characterized by, The method of determining the expected pose of the robot's two hands when carrying the object being carried according to the acquired translation acceleration of the robot's two hands when carrying the object being carried comprises: According to the translation acceleration and the preset modified auxiliary acceleration, a synthetic acceleration is determined, wherein the translation acceleration is determined according to the collision-free translation motion trajectory of the object being carried; According to the expected pose rotation matrix constructed based on the synthetic acceleration, the expected pose of the robot's two hands when carrying the object being carried is determined.

3. The control method of the robot manipulator according to claim 1, characterized by, The method of generating the collision-free translation motion trajectory of the object being carried and the collision-free translation motion trajectory of the robot's two hands based on the model predictive control technology and the control barrier function technology comprises: A five-level integrator and a second-order forward difference are used to construct a discrete point mass kinematics model of the object being carried; Based on the model predictive control technology and the control barrier function technology, the obstacle information and the discrete point mass kinematics model are used to determine the constraint condition, add a relaxation variable and a regularization term, and construct an MPC-CBF optimization problem; The MPC-CBF optimization problem is solved to generate the collision-free translation motion trajectory of the object being carried, and the collision-free translation motion trajectory of the robot's two hands is determined according to the collision-free translation motion trajectory of the object being carried.

4. The control method of the robot manipulator according to claim 1, characterized by, The method of determining the grasping point position of the object being carried by the robot's two hands based on the force-torque balance condition of the two hands comprises: According to the center of mass position, a force-torque balance relationship of the robot's two hands is established; According to the force-torque balance relationship, a grasping point optimization problem satisfying the force-torque balance condition of the two hands is constructed, and the grasping point optimization problem is solved to obtain the grasping point position.

5. The control method of the robot manipulator according to claim 1, characterized by, The method of controlling the robot's two hands to grasp the object being carried comprises: According to the grasping point of the two hands, the dual-arm reachability data of the robot is determined based on the dual-arm reachability condition; When the dual-arm reachability data is less than the reachability threshold, the robot's two hands are controlled to grasp the object being carried through waist-arm cooperative motion; When the dual-arm reachability data is greater than or equal to the reachability threshold, the robot's two hands are controlled to grasp the object being carried through dual-arm cooperative motion.

6. The control method of the robot manipulator according to claim 1, characterized by, After determining the expected pose of the robot's two hands when carrying the object being carried, the method further comprises: According to the collision-free translational motion trajectory of the robot double hands and the expected posture of the robot double hands when carrying the carried object, a joint speed instruction is generated.

7. The control method of the robot manipulator according to claim 1, characterized by, Before the step of determining the center of mass position of the carried object according to the acquired force data of the robot double hands and using the trained neural network model, the method further includes: controlling the robot double hands to grasp the carried object at an initial position, and acquiring the force data by using a six-dimensional force-torque sensor; controlling the robot to put down the carried object.

8. A control device of a robot manipulator, characterized by comprising: The method includes: a center of mass module configured to determine the center of mass position of the carried object according to the acquired force data of the robot double hands and using the trained neural network model; a grasping module configured to determine the grasping point position of the robot double hands for grasping the carried object according to the center of mass position based on a double-hand force-torque balance condition, and control the robot double hands to grasp the carried object; an obstacle avoidance module configured to generate the collision-free translational motion trajectory of the carried object and the collision-free translational motion trajectory of the robot double hands according to the acquired obstacle information based on a model predictive control technology and a control barrier function technology; a carrying module configured to determine the expected posture of the robot double hands when carrying the carried object according to the acquired translational acceleration of the robot double hands when carrying the carried object.

9. An electronic device, comprising: include a memory and a processor; the memory is configured to store a computer program; the processor is configured to implement the control method of the robot manipulator as claimed in any one of claims 1 to 7 when executing the computer program.

10. A computer-readable storage medium, characterized in that, The storage medium has a computer program stored thereon, and when the computer program is executed by a processor, the control method of the robot manipulator as claimed in any one of claims 1 to 7 is implemented.

Images