CN112497208A - Mobile operation robot general control method based on full-state impedance controller - Google Patents
Mobile operation robot general control method based on full-state impedance controller Download PDFInfo
- Publication number
- CN112497208A CN112497208A CN202011141893.3A CN202011141893A CN112497208A CN 112497208 A CN112497208 A CN 112497208A CN 202011141893 A CN202011141893 A CN 202011141893A CN 112497208 A CN112497208 A CN 112497208A
- Authority
- CN
- China
- Prior art keywords
- chassis
- mobile
- mechanical arm
- robot
- matrix
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Abstract
The invention discloses a general control method of a mobile operation robot based on an all-state impedance controller, which comprises the steps of establishing the overall kinematics and dynamics equation of the mobile operation robot, and establishing the all-state impedance controller of a Cartesian space weighted matrix on the basis; according to the distance between the target object and the tail end of the robot, the behavior mode of the human is simulated, three motion modes of operation, positioning-operation and positioning are designed, and corresponding parameters under the three modes are set.
Description
Technical Field
The present invention relates to the field of robot automation, and in particular to the problem of cooperative control and compliant operation of mobile manipulator robots in both contact and non-contact tasks.
Background
The mobile operation robot integrates the light-weight cooperative mechanical arm on the multifunctional mobile chassis, has the advantages of dexterity of the cooperative mechanical arm and large working space of the mobile chassis, has various functions and wide application range, and is also more and more emphasized along with the transformation and upgrading of the manufacturing industry.
The mobile operation robot is complex and comprises a plurality of technical modules such as environment perception, path planning, motion control and the like, wherein the motion control is a key basic technology and plays an important role in the operation effect of the mobile operation robot. Depending on whether there is contact with the operating environment, there are two categories: the method aims at the coordination control problem (non-contact) of free motion and the coordination control problem when relative motion exists between a focusing moving chassis and a mechanical arm; the second is the contact operation control problem (contact) under the environment interaction, and the problem of cooperative control under the condition of the existence of interaction force is solved. And the specific control mode is divided into two modes of torque control and speed control. At present, the control of contact operation under environmental interaction is still a difficult problem, and most of the proposed methods only adapt to one of the two situations, while the proposed speed control-based method can adapt to two scenes, but has slow dynamic response and poor interaction performance, so that the application is limited.
Disclosure of Invention
Aiming at the problems in the prior art, the invention provides a general control method of a mobile operation robot based on a full-state impedance controller, which can simultaneously adapt to operation tasks under two scenes of contact and non-contact, realizes flexible operation, and is simple and efficient.
In order to solve the technical problems, the invention is realized by the following technical scheme:
the general control method of the mobile operation robot based on the full-state impedance controller is characterized by comprising the following steps:
Further, in step 1, the established kinematic model of the mobile robot is:
in the formula (I), the compound is shown in the specification,
a pose description matrix of the tail end coordinate system of the mechanical arm under a world coordinate system,
a pose description matrix of a chassis coordinate system under a world coordinate system,
is a pose description matrix of a mechanical arm base coordinate system under a chassis coordinate system,
a pose description matrix of the mechanical arm tail end coordinate system under a base coordinate system;
for the chassis coordinate system under the world coordinate systemThe matrix of the rotation is then rotated in a direction,
a rotation matrix of the mechanical arm end coordinate system under a base coordinate system; r ═ r (r)x,ry,rθ) Three elements represent the positions of the chassis in the x and y directions and the included angle between the chassis and the x axis respectively for moving 3 degrees of freedom of the chassis (1); (x)0,y0,z0) Sequentially setting the positions of the original point of the mechanical arm base coordinate system in the x, y and z directions of the chassis coordinate system; (x)q,yq,zq) Sequentially setting the positions of the tail end of the mechanical arm in the x, y and z directions of a mechanical arm base coordinate system; q ═ q1～q7) The robot arm has 7 degrees of freedom, and 7 joint positions of the robot arm are indicated.
Further, in step 2, the full-state impedance controller with the weight matrix is established in a Cartesian space based on a whole dynamic model of the mobile operation robot and an impedance control principle.
Further, in step 2, the established full-state impedance controller is:
in the formula (I), the compound is shown in the specification,
is a 10-dimensional vector which represents the input torque of each joint of the mobile operation robot; tau represents an expected moment vector of the joint of the mobile operation robot; q. q.swbThe term "r, q" is a 10-dimensional vector representing the positions of the joints of the mobile robot, and g is a gravity matrix of the mobile robot as a whole; and C is a matrix of the Coriolis force and the centrifugal force of the whole mobile operation robot.
Further, in step 2, τ ═ W-1M-1JTΛWΛ-1F+(I-W-1M-1JTΛWJM-1)τ0Wherein M and J respectively represent the inertia matrix and the Jacobian of the whole mobile robotA comparable matrix; Λ represents an inertia matrix of the mobile operating robot in a Cartesian space; lambdaWRepresenting a Cartesian space weighted inertia matrix; w represents a weight matrix; tau represents an expected moment vector of the joint of the mobile operation robot; f represents the generalized external force in the Cartesian space between the tail end of the mobile operation robot and the interactive environment; i is an identity matrix; tau is0Representing zero space moment.
Further, in step 3, three working modes of the mobile operation robot are provided, wherein the three working modes simulate the establishment of human behavior modes:
note dmax(x,y)The distance between the target object and the tail end of the mechanical arm in the x and y directions is a larger value; the radius of the projection of the mechanical arm working space on the xy plane is rws，
In the operating mode, only the mechanical arm moves; in the positioning-operating mode, the mechanical arm and the chassis move in coordination; in the positioning mode, only the chassis moves.
Further, in the step 3,
in the operating mode: the scaling matrix H is diag (1,1,1, 5, 5, 5, 5), the virtual inertia of the chassis MadmDesired stiffness value K in null space for diag (80,80,20)nIs 10;
in the operating mode: the scaling matrix H is diag diag (150, 150, 150,1,1,1, 1), the virtual inertia M of the chassisadmA desired stiffness value K in null space for diag (120, 40)nIs 5;
in the operating mode: the scaling matrix H is diag (1,1,1, 1,1,1), the virtual inertia of the chassis MadmFor diag (100, 30), the desired stiffness value K in the null spacenIs 2.
Further, in step 4, the calculation formula of the speed command is as follows:
in the formula (I), the compound is shown in the specification,
respectively representing the desired speed of the chassis at times t and t-1, at representing the time interval, MadmVirtual inertia of the chassis, DadmIs a virtual inertia and damping matrix of the chassis,
the virtual moment expected by the moving chassis corresponding to the time t is shown.
Compared with the prior art, the invention has at least the following beneficial effects:
compared with the traditional method for respectively planning the mobile chassis and the mechanical arm, the method provided by the invention has the advantages that the mobile operation robot is taken as a whole, the terminal expected track is planned only by taking the mobile operation robot as a whole, and the method is simple, convenient and efficient. The general control method of the mobile operation robot based on the full-state impedance controller establishes the overall kinematics and dynamics model of the mobile operation robot, establishes the dynamic interaction relation between the tail end of the robot and the environment based on the impedance control technology in the compliance control method, and provides the Cartesian space full-state impedance controller with the weight, so that the compliance interaction with the environment can be realized, and the interaction environment cannot be damaged; meanwhile, the method is based on torque control, high in control frequency and better in dynamic response characteristic compared with a method based on speed control, and is more suitable for scenes with large environmental rigidity parameter changes.
Furthermore, the full-state impedance controller with the weight matrix is established in a Cartesian space based on the whole dynamic model of the mobile operation robot and the impedance control principle, and the weight matrix is dynamically adjustable and corresponds to different control effects.
Furthermore, a simple motion execution strategy is provided by simulating the behavior mode of a human, three motion modes are designed, parameter settings under the general conditions of the three motion modes are provided, the problem of cooperative control of the chassis and the mechanical arm is solved, and the two task scenes of contact and non-contact can be adapted simultaneously.
In summary, the general control method of the mobile robot based on the full-state impedance controller solves the problem of flexible operation of the mobile robot in a contact (with continuous external force) scene by establishing the whole full-state impedance controller; three motion modes are designed by adding a weight matrix, so that the problem of cooperative control of the chassis and the mechanical arm in contact and non-contact scenes is solved.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings used in the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and it is obvious for those skilled in the art that other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a schematic diagram of a mobile robot system based on a general control method of a mobile robot based on a full-state impedance controller according to the present invention;
FIG. 2 is a mobile manipulator robot control framework incorporating the control method of the present invention;
FIG. 3 is a schematic diagram of a mobile operation scenario set for validating a non-contact operation task;
FIG. 4 is a graph of end effector and moving chassis motion recorded during a mobile operation experiment;
FIG. 5a is a schematic view of a change in position of an end effector;
FIG. 5b is a schematic view of a change in pose of the end effector;
FIG. 5c is a schematic view of a change in position of the mobile chassis;
FIG. 6a shows the external force recorded during the door opening task of the mobile robot;
fig. 6b shows the external moment recorded during the door opening task of the mobile robot.
In the figure: 1-moving the chassis; 2, a mechanical arm; 3-a target object; 4-notebook computer.
Detailed Description
To make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is apparent that the described embodiments are some, but not all embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
In order to make the aforementioned and other objects, features and advantages of the present invention comprehensible, preferred embodiments accompanied with figures are described in detail below.
In one embodiment of the present invention, with reference to fig. 1 and 2, a mobile robot system for operation based on a torque-controlled full-state impedance controller includes: the robot comprises a mobile chassis 1, a mechanical arm 2, a target object 3 and a notebook computer 4. The base of the mechanical arm 2 is fixedly connected with the surface of the movable chassis 1 to form a movable operation robot; the notebook computer 4 communicates with the mobile chassis 1 and the mechanical arm 2 through IP respectively and sends instructions. The weight matrix based full-state impedance controller in the left solid-line box in fig. 2 is the upper level controller, while the robot arm torque controller and the chassis speed controller in the right dashed-line box are the lower level controllers.
The joint torque sensor can acquire the execution torque of each joint for measurement, and can also observe the contact force/torque at the tail end of the mechanical arm.
In the embodiment, the mobile chassis 1 is a Trans-om4s mobile platform, is driven by 4 Mecanum, can realize in-plane omnidirectional movement, belongs to an incomplete constraint system, and has 3 degrees of freedom; based on ROS system development, a speed interface is provided, a bottom layer speed controller is arranged, and closed-loop control of the mobile chassis on a speed layer is achieved; the robot arm 2 is a FRANKA Panda light 7-degree-of-freedom cooperative robot arm. Each joint is provided with a joint torque sensor, so that the actual torque of each joint can be obtained, and the contact force/torque at the tail end of the mechanical arm can be observed; the radius of the projection of the working space in the xy horizontal plane is 0.855m, a joint torque interface is provided based on ROS system development, a bottom layer joint torque controller is arranged, and closed-loop control of the mechanical arm on a joint torque layer is achieved. The notebook computer 4 also works in the ROS environment, and is responsible for the calculation and operation of the weight matrix-based full-state impedance controller (upper controller), and communicates with the mobile chassis 1 and the robot arm 2 through IP, and issues speed and joint torque commands to the lower controller, respectively (better understood in conjunction with fig. 2).
A general control method of a mobile robot based on a full-state impedance controller, which is applied to the mobile robot (controlled object), comprises the following steps:
step one, establishing a kinematic and dynamic model of the whole 10 degrees of freedom of the mobile robot in the embodiment, which is the basis for establishing the weighted cartesian space full-state impedance controller in the step two. The 3 degrees of freedom of the mobile chassis (1) are denoted herein as r ═ (r)x,ry,rθ) The three elements respectively represent the positions of the chassis in the x and y directions and the included angle between the chassis and the x axis; the arm has 7 degrees of freedom q ═ q (q)1～q7) Respectively showing 7 joint positions of the mechanical arm; r, q can be obtained through feedback interfaces of the mobile chassis and the mechanical arm respectively.
As shown in fig. 1: sigmaWRepresenting the world coordinate system, ∑MRepresents the base coordinate system of the mobile chassis (1) ∑BBase system, sigma, representing a robot arm (2)EERepresenting a coordinate system fixedly connected with an end effector of the mechanical arm (2), and obtaining the integral positive kinematics model of the mobile operation robot by adopting a homogeneous coordinate transformation method:
in the formula
A pose description matrix of the tail end coordinate system of the mechanical arm under a world coordinate system, namely an integral positive kinematic equation of the mobile operation robot，
A pose description matrix of a chassis coordinate system under a world coordinate system,
is a pose description matrix of a mechanical arm base coordinate system under a chassis coordinate system,
a pose description matrix of the mechanical arm tail end coordinate system under a base coordinate system;
is a rotation matrix of the chassis coordinate system under the world coordinate system,
a rotation matrix of the mechanical arm end coordinate system under a base coordinate system; (x)0,y0,z0) The positions of the original point of the mechanical arm base coordinate system in the x, y and z directions of the chassis coordinate system can be obtained by measuring a three-dimensional model of the mobile operation robot; (x)q,yq,zq) The positions of the tail end of the mechanical arm in the x, y and z directions of the mechanical arm base coordinate system can be obtained through a feedback interface provided by the mechanical arm;
further, a jacobian matrix J of the whole mobile operation robot is obtained by deriving the positive kinematics equation, and the relationship between the joint velocity and the terminal velocity is established as follows:
in the formula
Respectively a linear velocity and an angular velocity of the tail end of the mobile operation robot under a world coordinate system,
for moving joint space velocity of the operating robot, wherein
Is the speed of the chassis part and,
the speed of the mechanical arm part is obtained through feedback interfaces of the mobile chassis and the mechanical arm respectively;
respectively, a moving chassis portion and a robotic arm portion of the overall jacobian matrix. The overall jacobian matrix J is used several times in the process of building the full-state impedance controller in step two.
The kinetic equation of the mechanical arm established by adopting the Lagrange method is written as follows:
in the formula MfRepresenting an inertia matrix of the mechanical arm, and defining symmetry; cfRepresenting the matrix of Coriolis forces and centrifugal forces, gfThe representative gravity matrix is related to the pose of the mechanical arm, and the three can be obtained through a feedback interface of the mechanical arm;
respectively represent the expected moment vector and the disturbance moment vector of the mechanical arm joint.
It is generally set empirically, and is set to 0 here.
The position, the speed and the acceleration of the joint of the mechanical arm are sequentially obtained through a feedback interface of the mechanical arm.
Further, since the mobile chassis is based on speed control, in order to establish an overall dynamic model of the mobile operation robot and simplify the original dynamic model of the chassis, an admittance controller is used for replacing the original dynamic equation of the mobile chassis, so as to convert the dynamic equation into a moment control interface, and the chassis dynamic equation is written as:
in the formula Madm,DadmRespectively represents the virtual inertia and damping matrix of the chassis, and the relation between the two matrixes is Dadm＝3.2MadmIs a set value;
respectively the desired speed and acceleration of the moving chassis,
respectively representing the desired virtual moment and disturbance moment of the moving chassis,
is obtained by calculation, and the calculation result is obtained,
is an empirically set value, and is set to 0 here.
The chassis mechanical equation and the mechanical arm dynamic equation are integrated to obtain the overall dynamic equation of the mobile operation robot, which is as follows:
further simplification yields:
in the formula, qwbIs a 10-dimensional vectorRepresenting the positions of the respective joints of the mobile robot,
the joint speed and the acceleration of the mobile operation robot are sequentially measured. M, C and g respectively represent an inertia matrix, a Coriolis force and centrifugal force matrix and a gravity matrix of the whole mobile operation robot; tau, tauextRespectively represent the joint expected moment vector and the disturbance moment vector of the whole mobile operation robot.
And step two, establishing a Cartesian space full-state impedance controller of the mobile operation robot with the weight.
By the kinematic and dynamic model, the Cartesian space full-state impedance controller of the mobile operation robot with the weight is as follows:
τ＝W-1M-1JTΛWΛ-1F+(I-W-1M-1JTΛWJM-1)τ0(7)
in the formula, M and J respectively represent an inertia matrix and a Jacobian matrix of the whole mobile operation robot, and are obtained by the first step; lambda represents an inertia matrix of the Cartesian space of the mobile operating robot, and the expression is lambda-J-TMJ-1；ΛWExpressing a Cartesian space inertia matrix with weights, and the expression is LambdaW＝J-TMWMJ-1(ii) a W represents a weight matrix expressed by W — HM-1H represents a proportional matrix, is a diagonal matrix and is positively symmetrical, and different weight matrixes are obtained by setting elements on the diagonals of different H matrixes to achieve different control purposes; tau represents an expected moment vector of the joint of the mobile operation robot; f represents generalized external force between the tail end of the mobile operation robot and the interactive environment in a Cartesian space (a task space of the mobile operation robot), and is obtained by an impedance control principle, wherein impedance control is realized by virtualizing a mass-spring-damping system between the tail end of the robot and the environment, and when interactive force exists between the tail end of the robot and the environment, the mass-spring-damping system can perform compliant response on the external force, so that compliant interaction and operation are realized; thus, the expression of F is
Λd,Dd,KdRespectively representing a mass matrix, a damping matrix and a rigidity matrix expected by a Cartesian space, namely a virtual second-order system, which are set; cartesian space pose error
Representing the actual Cartesian pose x of the mobile operating robot and the expected pose x of the mobile operating robotdThe difference value of (a) to (b),
respectively representing the velocity and the angular velocity of the pose error, and x when the interaction force exists with the environmentdUnequal, thus generating generalized external force F; and I is an identity matrix.
Since the mobile robot in this embodiment has 10 degrees of freedom and is a redundant system, it is necessary to add a null-space moment, where the null-space is to configure the joint space of the mobile robot without affecting the task space (the flexible response of the mobile robot to the generalized external force F in the cartesian space) for controlling the task. In this example τ0Represents the zero space moment and has the expression of
Dn,KnThe damping value and the rigidity value respectively representing the zero space expectation are set values; the two relations are
qwb,0Representing the initially corresponding joint space value, τ, of the mobile robot0The control aim of (1) is to keep the initial space configuration q as much as possible on the premise of not influencing the task execution of the mobile operation robotwb,0. Here, it should be noted that0There are other forms such as optimization of joint limitation and operability of a mobile robot by null space, and the likeBelonging to the protection scope of the invention. This form is adopted here because: initial spatial configuration q of mobile robotwb,0It is generally an artificial and relatively good bit pattern, and it is therefore desirable to maintain this bit pattern as much as possible during task execution.
The two terms on the right side of the formula (7) are respectively the joint response moment and the zero-space moment of the mobile operation robot to the Cartesian space generalized external force, which are control paradigms of the redundant robot based on moment control.
Further, considering the gravity vector and the coriolis force/centrifugal force vector of the whole mobile robot, the complete weight-based full-state impedance controller can be written as:
in the formula
Is a 10-dimensional vector which represents the input torque of each joint of the mobile operation robot; as shown in fig. 2, where the expected joint moments τ are corresponding to the last 7 elemental robotic armsfAnd directly sending the data to a torque controller at the bottom layer of the mechanical arm.
The moving chassis (1) is based on speed control, converting the moment into a speed command by:
in the formula
Respectively representing the desired speed of the chassis for time t and time t-1, as shown in fig. 2, will be
Sending to a chassis speed controller; at represents the time interval of the time interval,
representing the expected virtual moment of the moving chassis corresponding to the time t;
step three, simulating the behavior mode of the human when completing the mobile operation task, dividing the motion mode into three motion modes according to the distance between the position of the target object 3 and the tail end of the mobile operation robot, corresponding to different parameter settings, and calculating the virtual moment expected by the chassis corresponding to the time t according to a formula (8)
Expected joint moment tau corresponding to mechanical armf；
Note dmax(x,y)The larger value of the distance between the target position and the tail end of the robot in the x and y directions is obtained; the radius of the projection of the mechanical arm working space on the xy plane is rws0.855m, then
In the formula, when the target position is in the working space of the mechanical arm, an operation mode is adopted, only the mechanical arm moves, when the target position exceeds the working space of the mechanical arm, but in the working space 2 times that of the mechanical arm, a positioning-operation mode is adopted, the mechanical arm and the chassis move in a coordinated manner, and when the target position exceeds the working space 2 times that of the mechanical arm, a positioning mode is adopted, only the chassis moves. It should be noted that the starting points of the three modes are: the positioning accuracy of the mechanical arm is generally higher than that of the chassis, so that an operation mode only using the mechanical arm is required, namely the operation mode is suitable for a scene with higher requirement on the operation accuracy; meanwhile, when the target position exceeds the working space of the mechanical arm and is not far away (is larger than the working space of the mechanical arm but is within twice of the working space of the mechanical arm), the efficiency of the operation task can be improved by the cooperative motion of the vehicle arms, namely a positioning-operation mode; however, when the target position is very primitive (more than twice the space of the robot arm), the movement of the robot arm is not significant, and therefore only chassis movement is required, i.e., the positioning mode. The three modes can be organically combined and selected for different task scenes.
In general, the corresponding parameter settings in the three modes are shown in table 1.
TABLE 1 corresponding parameter settings in modes
Diag () in the table represents H and MadmA value set on a diagonal of the diagonal matrix;
it should be noted here that the elements on the diagonal of the H matrix are associated with qwbThe first three elements on the diagonal of the H matrix correspond to three degrees of freedom of the moving chassis in the x and y directions and rotating around the z axis in sequence; the last 7 elements on the diagonal line correspond to the degrees of freedom of the 7 joints of the mechanical arm in sequence. Only reference values are given in table 1, and specific parameters may be set according to specific scenarios. When a certain degree of freedom of the mobile chassis needs to be limited, only the corresponding value in the first 3 on the diagonal of the H matrix needs to be set to 150, and the other two values are kept to be 1; for the mechanical arm, only the corresponding value in the last 7 on the diagonal line of the H matrix needs to be set to be 5, and the other values are kept to be 1, so that the degree of freedom of the corresponding mechanical arm is limited, no motion is generated, and other joints normally move; for example, in the second door opening experiment, in order to smoothly complete the door opening task, the rotational degree of freedom of the movable chassis around the z-axis needs to be limited, and the other 9 degrees of freedom operate normally, and at this time, H is set to diag (1,1,150,1,1,1, 1).
Meanwhile, when the same task is completed, mode selection combinations are also diversified and are determined according to specific task scenes. For example, when the target position is much larger than the working space of the robot arm, only the positioning mode may be adopted, or two segments may be adopted (first moving operation experiment a-B-C): firstly, a positioning mode is adopted, then an operation mode is adopted, and the method can be divided into three sections: first in a positioning mode, then in a positioning-operating mode, and finally in an operating mode.
In order to verify the effectiveness of the universal control method of the mobile operation robot based on the torque control full-state impedance controller and be suitable for contact and non-contact scenes, a typical material transfer experiment (non-contact) and a door opening experiment (contact) of the mobile operation robot are designed for verification.
The overall control flow in two experiments is first explained:
terminal expected pose x of mobile operation robotdIs planned in advance, belongs to a known quantity according to xdTo obtain dmax(x,y)Combination (10), selected operating mode and corresponding H, Madm,KnA parameter; the position r of the mobile chassis and the joint position q of the mechanical arm can be acquired in real time through a feedback interface, r and q are taken into a positive kinematic equation of the (1) mobile operation robot to acquire an actual Cartesian pose x, and then the actual Cartesian pose x can be acquired
The differential can be obtained
So as to obtain the generalized external force F of the Cartesian space; and (3) obtaining an overall Jacobian matrix J of the mobile operation robot through the formula (2). Recording the initial space configuration q of the mobile operation robot before the whole movement startswb,0Then, the real-time space configuration q is obtained through a speed feedback interface of the movable chassis and the mechanical armwbTo thereby obtain a zero space moment τ0. Then through zero space moment tau0、H,Madm,KnParameters, a Jacobian matrix J, a generalized external force F and an equation (7) are used for obtaining a joint expected torque vector tau of the mobile operation robot, and input torques tau of all joints of the mobile operation robot are obtained through tau and an equation (8) full-state impedance controller (namely an upper controller)in(ii) a Input torque tau of each joint of mobile operation robotinLast 7 elements of (1)fDirect connectionSending the data to a mechanical arm moment controller (namely a bottom controller); and the input torque tau of each joint of the robot is operated by using the movementinThe first 3 elements in
The expected speed of the chassis at time t is obtained by equation (9)
To the chassis speed controller (i.e., the floor controller).
Cartesian space pose error
Representing an expected pose x with a mobile operating robotdThe difference value of (a) to (b),
respectively representing the velocity and the angular velocity of the pose error, and x when the interaction force exists with the environmentdNot equal and therefore a generalized external force F is generated.
Fig. 3 shows a scenario of a material transfer experiment, which is specifically as follows: the mobile operation robot firstly moves 1.7m from the point A to the point B along the positive direction of x in a positioning mode, then continues to move 0.4m in the positive direction of x to reach the point C in an operation mode to obtain the material, and finally moves 1.5m along the negative direction of x, moves 1.4m along the positive direction of y, moves 0.28m along the negative direction of z to reach the point D in a positioning-operation mode to place the material at a specified position on the table. The parameters of the whole process are set according to the table 1, fig. 4 records the motion conditions of the tail end and the chassis of the robot in the whole experimental process, the top graph records the position condition of the tail end of the robot, the middle graph records the posture change condition of the tail end of the robot based on quaternion, and the bottom graph records the position condition of the moving chassis, wherein yaw represents the rotational displacement around the z axis. It can be seen from the figure that the mobile operation robot smoothly transfers the material to the designated position according to the expected target, and in the whole process, the position of the end effector, the attitude of the end effector and the state of the mobile chassis in fig. 4 have no obvious mutation, and the motion is relatively smooth, which shows that the method can adapt to the non-contact operation task scene.
The experiment is a mobile operation experiment, and three designed motion modes successfully place materials at a specified position, which shows that the invention can effectively solve the problem of cooperative control of the chassis and the mechanical arm.
The door opening test is carried out, the door opening test means that the door handle is clamped by the mechanical arm 2, then the mechanical arm 2 and the mobile chassis 1 move cooperatively to open the door, and the impedance rigidity of the set Cartesian space is KdBiag (850, 30,30), zero spatial stiffness Kn(ii) 5; setting virtual mass of admittance controller of chassis to MadmDiag (80,80, 20); setting the proportion matrix as H ═ diag (1,1,150,1,1,1,1,1, 1), namely limiting the freedom of the chassis rotating around the Z axis, and completing the door opening task by the coordination of the remaining 9 degrees of freedom. Fig. 5a records the position of the end of the robot during the entire door opening process, fig. 5b shows the attitude change of the end of the robot based on quaternion, fig. 5c shows the position of the chassis, where yaw shows the rotational displacement around the z-axis, fig. 6a records the external force applied to the end of the robot arm 2 during the entire door opening process, and fig. 6b records the external moment applied to the end of the robot arm 2 during the entire door opening process. It can be seen that the whole door opening process is very smooth and compliant in track, and the absolute value of the contact force at the tail end of the mechanical arm 2 is about 20N at most; the absolute value of the contact torque does not exceed 2N m at most, which shows that the contact force/torque generated in the whole door opening process is small and flexible.
In the second door opening experiment, under the condition of continuous external force, the chassis-mechanical arm cooperative operation successfully opens the door, which shows that the invention effectively solves the dynamic operation problem under the contact condition. Therefore, the invention is suitable for the operation tasks of the mobile operation robot under two scenes of contact/non-contact.
Through the two experiments, the method provided by the invention can be fully suitable for door opening tasks under the contact and non-contact conditions, and the flexible operation effect is very good.
Finally, it should be noted that: although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art will understand that: any person skilled in the art can modify or easily conceive the technical solutions described in the foregoing embodiments or equivalent substitutes for some technical features within the technical scope of the present disclosure; such modifications, changes or substitutions do not depart from the spirit and scope of the embodiments of the present invention, and they should be construed as being included therein. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.
Claims (8)
1. The general control method of the mobile operation robot based on the full-state impedance controller is characterized by comprising the following steps:
step 1, establishing a kinematic model and a dynamic model of a mobile operation robot, wherein the mobile operation robot comprises a mobile chassis (1) and a mechanical arm (2);
step 2, establishing an all-state impedance controller with a weight matrix on the basis of the kinematic model and the dynamic model established in the step 1;
step 3, selecting a working mode of the mobile operation robot according to the distance between the target object (3) and the tail end of the mechanical arm (2), giving a value of a corresponding weight matrix in the selected working mode of the mobile operation robot, and substituting the value of the weight matrix into the full-state impedance controller with the weight matrix obtained in the step 2 for calculation to obtain the expected joint moment of the mechanical arm (2) and the virtual moment of the mobile chassis (1);
step 4, adding the disturbance torque to the virtual torque of the mobile chassis (1) obtained in the step 3, calculating to obtain a speed instruction of the mobile chassis (1), and sending the speed instruction to a bottom-layer speed controller; and (4) adding the disturbance torque to the expected joint torque of the mechanical arm (2) obtained in the step (3), and sending the expected joint torque to a bottom moment controller of the mechanical arm.
2. The general control method for mobile robots based on the full-state impedance controller according to claim 1, wherein in the step 1, the established kinematic model of the mobile robots is:
in the formula (I), the compound is shown in the specification,
a pose description matrix of the tail end coordinate system of the mechanical arm under a world coordinate system,
a pose description matrix of a chassis coordinate system under a world coordinate system,
is a pose description matrix of a mechanical arm base coordinate system under a chassis coordinate system,
a pose description matrix of the mechanical arm tail end coordinate system under a base coordinate system;
is a rotation matrix of the chassis coordinate system under the world coordinate system,
a rotation matrix of the mechanical arm end coordinate system under a base coordinate system; r ═ r (r)x,ry,rθ) Three elements represent the positions of the chassis in the x and y directions and the included angle between the chassis and the x axis respectively for moving 3 degrees of freedom of the chassis (1); (x)0,y0,z0) Sequentially setting the positions of the original point of the mechanical arm base coordinate system in the x, y and z directions of the chassis coordinate system; (x)q,yq,zq) Sequentially setting the positions of the tail end of the mechanical arm in the x, y and z directions of a mechanical arm base coordinate system; q ═ q1～q7) The robot arm has 7 degrees of freedom, and 7 joint positions of the robot arm are indicated.
3. The general control method for mobile robots based on full-state impedance controllers of claim 1 is characterized in that in step 2, the full-state impedance controllers with weight matrixes are established in a Cartesian space based on a dynamic model of the whole mobile robot in combination with an impedance control principle.
4. The general control method for mobile robot manipulator based on full-state impedance controller as claimed in claim 3, wherein in step 2, the full-state impedance controller is established as follows:
in the formula (I), the compound is shown in the specification,
is a 10-dimensional vector which represents the input torque of each joint of the mobile operation robot; tau represents an expected moment vector of the joint of the mobile operation robot; q. q.swbThe term "r, q" is a 10-dimensional vector representing the positions of the joints of the mobile robot, and g is a gravity matrix of the mobile robot as a whole; and C is a matrix of the Coriolis force and the centrifugal force of the whole mobile operation robot.
5. The general control method for mobile robot manipulator based on full-state impedance controller as claimed in claim 4, wherein in step 2, τ -W-1M-1JTΛWΛ-1F+(I-W-1M-1JTΛWJM-1)τ0Wherein M and J are moments of inertia of the entire mobile robotAn array and a jacobian matrix; Λ represents an inertia matrix of the mobile operating robot in a Cartesian space; lambdaWRepresenting a Cartesian space weighted inertia matrix; w represents a weight matrix; tau represents an expected moment vector of the joint of the mobile operation robot; f represents the generalized external force in the Cartesian space between the tail end of the mobile operation robot and the interactive environment; i is an identity matrix; tau is0Representing zero space moment.
6. The universal control method for mobile operation robots based on the full-state impedance controller as claimed in claim 1 is characterized in that in step 3, the mobile operation robot has three working modes, and the three working modes are established by simulating human behavior modes:
note dmax(x,y)The distance between the target object (3) and the tail end of the mechanical arm (2) in the x and y directions is a larger value; the radius of the work space of the mechanical arm (2) in the xy plane projection is rws，
In the operating mode, only the mechanical arm (2) moves; in the positioning-operating mode, the mechanical arm (2) and the chassis (1) move in a coordinated manner; in the positioning mode, only the chassis (1) moves.
7. The universal mobile robot control method based on the full-state impedance controller as claimed in claim 5, wherein in the step 3,
in the operating mode: the scaling matrix H is diag (1,1,1, 5, 5, 5, 5), the virtual inertia of the chassis MadmDesired stiffness value K in null space for diag (80,80,20)nIs 10;
in the operating mode: the scaling matrix H is diag diag (150, 150, 150,1,1,1, 1), the virtual inertia M of the chassisadmA desired stiffness value K in null space for diag (120, 40)nIs 5;
in the operating mode: the scaling matrix H isdiag (1,1,1, 1,1,1,1), virtual inertia of the chassis MadmFor diag (100, 30), the desired stiffness value K in the null spacenIs 2.
8. The general control method for mobile robot manipulator based on the full-state impedance controller as claimed in claim 7, wherein in step 4, the velocity command is calculated by the formula:
in the formula (I), the compound is shown in the specification,
respectively representing the desired speed of the chassis at times t and t-1, at representing the time interval, MadmVirtual inertia of the chassis, DadmIs a virtual inertia and damping matrix of the chassis,
the virtual moment expected by the moving chassis corresponding to the time t is shown.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011141893.3A CN112497208A (en) | 2020-10-22 | 2020-10-22 | Mobile operation robot general control method based on full-state impedance controller |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011141893.3A CN112497208A (en) | 2020-10-22 | 2020-10-22 | Mobile operation robot general control method based on full-state impedance controller |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112497208A true CN112497208A (en) | 2021-03-16 |
Family
ID=74954209
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011141893.3A Pending CN112497208A (en) | 2020-10-22 | 2020-10-22 | Mobile operation robot general control method based on full-state impedance controller |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112497208A (en) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113413214A (en) * | 2021-05-24 | 2021-09-21 | 上海交通大学 | Surgical robot manpower feedback method and device based on mixed reality guidance |
| CN113733052A (en) * | 2021-09-17 | 2021-12-03 | 西安交通大学 | Omnidirectional mobile robot and control method thereof |
| CN113843801A (en) * | 2021-10-18 | 2021-12-28 | 上海节卡机器人科技有限公司 | Control method, device and system of composite robot |
| CN114454180A (en) * | 2022-03-22 | 2022-05-10 | 深圳市优必选科技股份有限公司 | Motion control method and device of mobile robot and mobile robot |
| CN115946129A (en) * | 2023-03-10 | 2023-04-11 | 珞石（北京）科技有限公司 | Robot variable admittance control method for operating large-inertia object |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104476550A (en) * | 2014-12-05 | 2015-04-01 | 济南鲁智电子科技有限公司 | All-hydraulic autonomous moving manipulator and moving method thereof |
| CN108000477A (en) * | 2017-12-05 | 2018-05-08 | 哈尔滨工业大学 | A kind of complete main passive compliance robot of pose and screw valve method using the robot |
| US20180236659A1 (en) * | 2017-02-17 | 2018-08-23 | Massachusetts Institute Of Technology | Compositional impedance programming for robots |
| CN108453728A (en) * | 2017-02-10 | 2018-08-28 | 通用汽车环球科技运作有限责任公司 | Redundancy lack of driven robot with multi-mode control framework |
| CN109500821A (en) * | 2019-01-07 | 2019-03-22 | 深圳航天科技创新研究院 | Quadruped robot lands sufficient force distribution method, device, terminal and computer readable storage medium |
-
2020
- 2020-10-22 CN CN202011141893.3A patent/CN112497208A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104476550A (en) * | 2014-12-05 | 2015-04-01 | 济南鲁智电子科技有限公司 | All-hydraulic autonomous moving manipulator and moving method thereof |
| CN108453728A (en) * | 2017-02-10 | 2018-08-28 | 通用汽车环球科技运作有限责任公司 | Redundancy lack of driven robot with multi-mode control framework |
| US20180236659A1 (en) * | 2017-02-17 | 2018-08-23 | Massachusetts Institute Of Technology | Compositional impedance programming for robots |
| CN108000477A (en) * | 2017-12-05 | 2018-05-08 | 哈尔滨工业大学 | A kind of complete main passive compliance robot of pose and screw valve method using the robot |
| CN109500821A (en) * | 2019-01-07 | 2019-03-22 | 深圳航天科技创新研究院 | Quadruped robot lands sufficient force distribution method, device, terminal and computer readable storage medium |
Non-Patent Citations (4)
| Title |
|---|
| EDOARDO LAMON: "Towards_an_Intelligent_Collaborative_Robotic_System_for_Mixed_Case_Palletizing", 《2020 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA)》 * |
| PAOLO FERRARI: "Humanoid_whole-body_planning_for_loco-manipulation_tasks", 《2017 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA)》 * |
| PIETRO BALATTI: "A_Collaborative_Robotic_Approach_to_Autonomous_Pallet_Jack_Transportation_and_Positioning", 《IEEE ACCESS ( VOLUME: 8)》 * |
| WU YUQIANG: "A_Teleoperation_Interface_for_Loco-Manipulation_Control_of_Mobile_Collaborative_Robotic_Assistant", 《IEEE ROBOTICS AND AUTOMATION LETTERS》 * |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113413214A (en) * | 2021-05-24 | 2021-09-21 | 上海交通大学 | Surgical robot manpower feedback method and device based on mixed reality guidance |
| CN113413214B (en) * | 2021-05-24 | 2022-12-30 | 上海交通大学 | Surgical robot manpower feedback method and device based on mixed reality guidance |
| CN113733052A (en) * | 2021-09-17 | 2021-12-03 | 西安交通大学 | Omnidirectional mobile robot and control method thereof |
| CN113733052B (en) * | 2021-09-17 | 2023-07-25 | 西安交通大学 | Omnidirectional mobile robot and control method thereof |
| CN113843801A (en) * | 2021-10-18 | 2021-12-28 | 上海节卡机器人科技有限公司 | Control method, device and system of composite robot |
| CN113843801B (en) * | 2021-10-18 | 2022-11-29 | 上海节卡机器人科技有限公司 | Control method, device and system of composite robot |
| WO2023065781A1 (en) * | 2021-10-18 | 2023-04-27 | 节卡机器人股份有限公司 | Control method, device, and system for hybrid robot |
| CN114454180A (en) * | 2022-03-22 | 2022-05-10 | 深圳市优必选科技股份有限公司 | Motion control method and device of mobile robot and mobile robot |
| CN114454180B (en) * | 2022-03-22 | 2024-01-19 | 深圳市优必选科技股份有限公司 | Motion control method and device of mobile robot and mobile robot |
| CN115946129A (en) * | 2023-03-10 | 2023-04-11 | 珞石（北京）科技有限公司 | Robot variable admittance control method for operating large-inertia object |
| CN115946129B (en) * | 2023-03-10 | 2023-05-09 | 珞石（北京）科技有限公司 | Robot admittance-changing control method for operating large-inertia object |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112497208A (en) | Mobile operation robot general control method based on full-state impedance controller | |
| CN108883533B (en) | Robot control | |
| CN108241339B (en) | Motion solving and configuration control method of humanoid mechanical arm | |
| Žlajpah | Simulation in robotics | |
| CN103302667B (en) | Robot control method, robot controller and robot control system | |
| US9120227B2 (en) | Human motion tracking control with strict contact force constraints for floating-base humanoid robots | |
| JP7324932B2 (en) | dynamic planning controller | |
| Fang et al. | Skill learning for human-robot interaction using wearable device | |
| Patidar et al. | Survey of robotic arm and parameters | |
| CN115351780A (en) | Method for controlling a robotic device | |
| JP2018030210A (en) | Simulation device, control system, robot system, simulation method, program and recording medium | |
| CN115122325A (en) | Robust visual servo control method for anthropomorphic manipulator with view field constraint | |
| Bratchikov et al. | Development of digital twin for robotic arm | |
| JP3614824B2 (en) | Motion editing apparatus and motion editing method for legged mobile robot | |
| Žlajpah | Robot simulation for control design | |
| Khan et al. | ROS-based control for a robot manipulator with a demonstration of the ball-on-plate task | |
| Masone et al. | Mechanical design and control of the new 7-DOF CyberMotion simulator | |
| Zarafshan et al. | Which impedance strategy is the most effective for cooperative object manipulation? | |
| Mandić et al. | An application example of Webots in solving control tasks of robotic system | |
| Safeena et al. | Adaptive Super Twisting Control of Stewart Platform Based on Super Twisting Observer | |
| Koops | Trajectory tracking control of the philips experimental robot arm in the port-hamiltonian framework | |
| Fratu et al. | Robots collision avoidance using learning through imitation | |
| Kim et al. | A Study on Track Record and Trajectory Control of Articulated Robot Based on Monitoring Simulator for Smart Factory | |
| Suthar et al. | Contact Force-Velocity Control for a Planar Aerial Manipulator | |
| Fang et al. | Learning from wearable-based teleoperation demonstration |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20210316 |
|
| RJ01 | Rejection of invention patent application after publication |