A compliant task control method based on planar constant force

Abstract

The present application relates to a kind of compliant task control method based on plane constant force, belong to robot control technical field, it includes the following steps: zero force control is carried out to robot, and the trajectory of robot terminal is collected to generate desired trajectory;Compliant task of robot terminal is decoupled in compliant coordinate system, is decomposed into position control in rigid plane and force control in the direction perpendicular to rigid plane, establishes the force-position hybrid control model based on position mode;According to the small offset amount that robot terminal is generated when being contacted with rigid plane due to the action of external force, establish trajectory adjustment controller model;The desired external force of robot terminal is set as constant force, robot terminal is controlled according to desired trajectory by using force-position hybrid control model, and the desired trajectory of robot terminal is fine-tuned by using trajectory adjustment controller model.This compliant task control method can make robot terminal contact with rigid plane movement, and not cause destructive damage to plane.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of robot control, and particularly relates to a compliant task control method based on a constant force of a plane. BACKGROUND

[0002] With the development of robots, the application of robots is not only satisfied with the control of a pose, and for some complex operation tasks (such as assembly, grinding, deburring, etc.), the pure position control cannot meet the operation requirements. The common feature of such complex tasks is that the robot end tool needs to keep in contact with the object (rigid body) to be operated, and they complete certain operation tasks through the interaction force. For such tasks, only the pose control is not enough, and a small pose error may make the tool and the environment out of contact or produce a large interaction force between them. Such a robot end and the environment have contact and energy exchange tasks, which are compliant tasks. To complete such operation tasks, compliant motion control of the robot is required, so that the robot end and the rigid plane are in contact and motion without causing destructive damage to the plane.

[0003] The traditional robot operation task only contains position control, and the difference between the compliant task and the traditional operation task lies in that the compliant task contains not only the constraint of position control but also the constraint of force control. If two sets of different trajectory tracking (position tracking and force tracking) are directly used to describe the entire compliant task in the robot base coordinate system, the two different control schemes will be completely coupled together, and the entire task description is extremely complex. Because the contact between rigid bodies is generally described, for any direction, it is impossible to simultaneously require position control and force control, and the two cannot coexist, and only one of them is selected.

[0004] Therefore, how to separate the position control and the force control in the compliant task, and control the force of the robot end to always maintain a constant force, so as to realize the compliant control of the robot end, is a technical problem that needs to be solved at present. SUMMARY

[0005] In view of the deficiencies in the related art, the present application provides a compliant task control method based on a constant force of a plane, which can make the robot end and the rigid plane in contact and motion without causing destructive damage to the plane.

[0006] The present application provides a compliant task control method based on a constant force of a plane, comprising the following steps:

[0007] Obtaining the expected trajectory: modeling dynamics of the robot and modeling friction, then compensating for gravity and friction during movement of the robot, establishing a zero force control model of the position control mode; using the zero force control model to control the robot in zero force control, dragging the robot end to move according to the expected trajectory, collecting the robot end trajectory to generate the expected trajectory;

[0008] Establishing a force-position hybrid control model based on the position mode, specifically including the following steps:

[0009] Establishing a compliant coordinate system based on the robot, taking a rigid plane as the xoy plane of the compliant coordinate system, and taking the direction perpendicular to the rigid plane outward as the z-axis direction of the compliant coordinate system; decomposing the compliant task of the robot end moving on the rigid plane into position control and force control of the robot end along the x-axis, the y-axis and the z-axis in the compliant coordinate system;

[0010] Then, the position control and force control of the robot end along the x-axis, the y-axis and the z-axis are converted into position control and force control of the robot joints along the x-axis, the y-axis and the z-axis;

[0011] According to the control mode of the servo motor at the robot joint, the position control and force control of the robot joint along the x-axis, the y-axis and the z-axis are used to establish a force-position hybrid control model based on the position mode;

[0012] Establishing a trajectory adjustment controller model: according to the small offset of the robot end when in contact with the rigid plane due to external force, establishing a trajectory adjustment controller model to correct the expected trajectory of the robot end in the z-axis direction;

[0013] Controlling the movement of the robot end: setting the expected external force of the robot end in the z-axis direction as a constant force, using the force-position hybrid control model based on the position mode to control the servo motor at the robot joint to make the robot end move according to the expected trajectory, and using the trajectory adjustment controller model to fine-tune the expected trajectory of the robot end in the z-axis direction during control.

[0014] In some embodiments, in the step of establishing a force-position hybrid control model based on the position mode, the compliant task of the robot end moving on the rigid plane is decomposed into position control and force control of the robot end in the xoy plane and the z-axis direction in the compliant coordinate system.

[0015] In some embodiments, when decomposing the compliant task of the robot end, the position control and force control of the robot end meet natural constraint conditions and artificial constraint conditions;

[0016] The natural constraint condition is:

[0017]

[0018] Human constraints are:

[0019]

[0020] where f x is the force of the robot end-effector along the x-axis, f y is the force of the robot end-effector along the y-axis, f z is the force of the robot end-effector along the z-axis, f x des is the desired force of the robot end-effector along the x-axis, f y des is the desired force of the robot end-effector along the y-axis, f z des is the desired force of the robot end-effector along the z-axis, g x is the torque of the robot end-effector along the x-axis, g y is the torque of the robot end-effector along the y-axis, g z is the torque of the robot end-effector along the z-axis, v x is the average velocity of the robot end-effector along the x-axis, v y is the average velocity of the robot end-effector along the y-axis, v z is the average velocity of the robot end-effector along the z-axis, v x des is the desired velocity of the robot end-effector along the x-axis, v y des is the desired velocity of the robot end-effector along the y-axis, v z des is the desired velocity of the robot end-effector along the z-axis, w x is the rotational velocity of the robot end-effector along the x-axis, w y is the rotational velocity of the robot end-effector along the y-axis, w z is the rotational velocity of the robot end-effector along the z-axis. x y z dz x y z x y z dx dy x y z

[0021] In some embodiments, in the position mode based force / position hybrid control model, the expected displacement amount and the expected torque of the robot joint along the x-axis, the y-axis and the z-axis are set as the set values, the displacement values obtained by the position sensor at the robot joint are taken as the displacement feedback values, the torque values obtained by the force sensor at the robot joint are taken as the torque feedback values, and the differences between the expected displacement amount and the displacement feedback values and the differences between the expected torque and the torque feedback values are taken as the input values of the joint space position controller for control.

[0022] In some embodiments, in the position mode based force / position hybrid control model, the displacement values obtained by the position sensor at the robot joint need to be converted into the compliant coordinate system, and the converted displacement values are taken as the displacement feedback values.

[0023] In some embodiments, in the step of establishing the trajectory adjustment controller model, the robot end-effector is equivalent to a mass-damping-spring system, and the inertia, damping and stiffness are used to describe the dynamic properties of the robot end-effector, respectively, and the small displacement of the robot end-effector caused by external force when in contact with a rigid plane is calculated.

[0024] ​​​​​​​​​​​​​​​In some embodiments, the trajectory adjustment controller model is as follows:

[0025]

[0026] wherein z d is the desired position of the robot end in the z-axis direction; z r is the reference position, taking the position when the robot end contacts the rigid plane as the reference position; F dz is the desired external force of the robot end in the z-axis direction; F ez is the equivalent external force of the robot end in the z-axis direction, calculated based on the joint torque sensor and the dynamics model; M d is the desired inertia of the robot; B d is the desired inertia of the robot; K d is the desired inertia of the robot; s is a complex variable obtained by Laplace transform.

[0027] Compared with the prior art, the advantages and positive effects of the present application are that:

[0028] The compliant task control method based on the plane constant force provided by the present application decouples the compliant task of the robot end moving in the rigid plane in the compliant coordinate system, thereby decomposing the complex compliant task into the position control in the rigid plane and the force control in the direction perpendicular to the rigid plane, separating the position control and the force control in the compliant task, and further establishing the force-position hybrid control model based on the position mode and the trajectory adjustment controller model. By setting the desired external force of the robot end in the z-axis direction in the compliant coordinate system as a constant force, the force-position hybrid control model based on the position mode is used for force-position hybrid control, and the trajectory adjustment controller model is used for correcting the desired trajectory of the robot end in the z-axis direction, so that the robot end moves in the rigid plane and maintains the contact force with the plane unchanged, the plane constant force control is realized, and the control requirements of the compliant task of the robot end can be met. BRIEF DESCRIPTION OF DRAWINGS

[0029] The accompanying drawings, which are included to provide a further understanding of the present application and constitute a part of this application, illustrate embodiments of the present application and together with the description serve to explain the present application. In the drawings:

[0030] Figure 1 FIG. 1 is a control block diagram of the force-position hybrid control model based on the position mode established in an embodiment of the compliant task control method based on the plane constant force of the present application;

[0031] Figure 2A comparison chart of the external force tracking results in the z-axis direction when the robot end is controlled by the compliant task control method based on the planar constant force provided by the embodiment of the present application and the conventional position control method respectively controls the robot end to contact a rigid plane. DETAILED DESCRIPTION

[0032] The technical solutions in the embodiments will be clearly and completely described with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments of the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0033] The embodiment of the present application provides a compliant task control method based on a planar constant force, comprising the following steps:

[0034] S1: obtaining a desired trajectory: performing dynamic modeling and friction modeling on the robot, then compensating the gravity and friction in the movement of the robot, establishing a zero force control model of the position control mode; performing zero force control on the robot by using the zero force control model, dragging the robot end to run according to the expected trajectory, collecting the robot end trajectory to generate the desired trajectory.

[0035] In this step, it should be noted that the definition of the robot "zero force" control is that the robot complies with the movement of the external force as if it is in an environment without gravity and friction control, that is, the gravity and friction of the robot are compensated by using the servo control of the motor, so that the external force is completely applied to the movement of the robot. In this step, the zero force control model of the position control mode is established based on the Cartesian space zero force control system. The core idea of the Cartesian space zero force control is that when the operator drags the robot end, the external force on each joint is obtained according to the dynamic model and the feedback data of the torque sensor of each joint at present, then the actual external force on the end is calculated according to the external torque of the joint, the acceleration of the end movement is obtained according to the external force, the expected pose of the end is obtained by twice integration, and finally the expected joint angle is obtained by inverse kinematics calculation, and the joint command is sent to the motor, that is, the zero force control in the Cartesian space can be realized. The specific steps of establishing the zero force control model of the position control mode based on the Cartesian space zero force control system are well known to those skilled in the art, and are not described here.

[0036] S2: establishing a force-position hybrid control model based on the position mode, which specifically comprises the following steps:

[0037] S201, a compliant coordinate system is established based on the robot, a rigid plane is taken as an xoy plane of the compliant coordinate system, and a direction perpendicular to the rigid plane and outward is taken as a z-axis direction of the compliant coordinate system; a compliant task of the robot end in the rigid plane motion is decomposed into position control and force control of the robot end along the x-axis, the y-axis and the z-axis in the compliant coordinate system.

[0038] It needs to be explained that, when the robot end moves to the rigid plane and has not contacted the rigid plane, all directions of the robot end are not contacted with the environment, and are position control; when the robot end contacts the rigid plane, motion along the z-axis direction is hindered by the rigid plane, and position control cannot be performed on the z-axis direction, only force control can be performed, and other directions are still position control, and force control is not performed. Therefore, in this step, when the compliant task is decomposed, the compliant task of the robot end in the rigid plane motion is decomposed into position control of the robot end in the xoy plane and force control of the robot end in the z-axis direction in the compliant coordinate system.

[0039] Specifically, the position control and the force control of the robot end need to meet natural constraint conditions (that is, constraint relationships determined according to the geometric structure of the task) and artificial constraint conditions (that is, expected motion set according to the requirements of the task). Assuming that the friction between the robot end and the rigid plane in the xoy plane is 0 under the condition of ignoring friction, and there is no resisting torque for rotation of the robot end around the x-axis, the y-axis and the z-axis, the natural constraint condition can be expressed as:

[0040]

[0041] Where, f x is force of the robot end along the x-axis, f y is force of the robot end along the y-axis, v z is average speed of the robot end along the z-axis, g x is torque of the robot end along the x-axis, g y is torque of the robot end along the y-axis, and g z is torque of the robot end along the z-axis.

[0042] According to the requirement of hard contact, the expected trajectory given artificially is artificial constraint, and the artificial constraint condition can be expressed as:

[0043]

[0044] Where, v x is average speed of the robot end along the x-axis, v y is average speed of the robot end along the y-axis, and v dxis the desired velocity of the robot end along the x-axis, v dy is the desired velocity of the robot end along the y-axis, f z is the force at the end of the robot along the z-axis, f dz is the desired force along the z-axis at the end of the robot, w x is the rotation speed of the robot end along the x-axis, w y is the rotation speed of the robot end along the y-axis, w z is the rotation speed of the robot end along the z-axis.

[0045] Therefore, this step establishes a compliant coordinate system, and decomposes the compliant task of the robot end moving in the rigid plane into position control of the robot end in the xoy plane and force control in the z-axis direction under the compliant coordinate system, thereby separating the position control and force control, making it easier to achieve subsequent compliant control.

[0046] S202 , converting the position control and force control of the robot end along the x-axis, y-axis, and z-axis into the position control and force control of the robot joints along the x-axis, y-axis, and z-axis.

[0047] In this step, it should be noted that since the smallest driving unit of the robot is the joint, the position control and force control of the robot end in the compliant coordinate system need to be changed to the joint space.

[0048] S203. According to the control method of the servo motor at the robot joint, and according to the position control and force control of the robot joint along the x-axis, y-axis and z-axis, a force-position hybrid control model based on the position mode is established.

[0049] Specifically, the control block diagram of the force-position hybrid control model based on the position mode used in this step is as follows: Figure 1 As shown, Figure 1 In, x d is the expected displacement, x is the displacement feedback value, f d is the desired torque, f is the torque feedback value, S is the selection matrix, I is the identity matrix, J is the robot Jacobian matrix, q is the joint angle, is the joint angular velocity, For the estimated environmental stiffness, the FCL is a force controller, the PCLJ is a joint space position controller, and X = f(q) is a displacement coordinate transformation relationship based on joint angles, which is used to convert the measured values fed back by the position sensor into the compliant coordinate system. In the force-position hybrid control model based on the position mode, the expected displacement amount and the expected torque of the robot joint along the x-axis, the y-axis, and the z-axis are set values, the displacement value obtained by the position sensor at the robot joint is a displacement feedback value, the torque value obtained by the force sensor at the robot joint is a torque feedback value, and the difference between the expected displacement amount and the displacement feedback value and the difference between the expected torque and the torque feedback value are input values of the joint space position controller for control. The displacement value obtained by the position sensor at the robot joint needs to be converted into the compliant coordinate system, and the converted displacement value is used as the displacement feedback value.

[0050] S3, a trajectory adjustment controller model is established: according to the small displacement of the robot end when in contact with the rigid plane due to the action of external force, a trajectory adjustment controller model is established to correct the expected trajectory of the robot end in the z-axis direction.

[0051] In this step, it needs to be explained that when the robot end is in contact with the rigid plane, the robot end will produce a small displacement due to the action of external force, therefore, by establishing a trajectory adjustment controller model, impedance control can be performed on the robot end in the z-axis direction, so as to correct the expected trajectory of the robot end in the z-axis direction, so that the robot end motion meets the control requirements of the compliant task.

[0052] Specifically, in the step of establishing the trajectory adjustment controller model, the robot end is equivalent to a mass-damping-spring system, and the inertia, damping, and stiffness are used to describe the dynamic properties of the robot end, respectively, then the corresponding restoring force F generated when the robot end is subjected to external force and produces a small displacement can be represented as:

[0053]

[0054] In formula (1), F dz is the expected external force of the robot end in the z-axis direction; F ez is the equivalent external force of the robot end in the z-axis direction, which is calculated based on the joint torque sensor and the dynamics model; M d is the expected inertia of the robot; B d is the expected inertia of the robot; K d is the expected inertia of the robot; e is the small displacement of the robot end when in contact with the rigid plane due to the action of external force.

[0055] The Laplace transform is performed on formula (1), and then the small displacement e of the robot end when the robot end is in contact with the rigid plane and is subjected to an external force can be expressed as:

[0056]

[0057] In formula (2), z d is the expected position of the robot end in the z-axis direction; z r is a reference position, and the position of the robot end when the robot end is in contact with the rigid plane is taken as the reference position; and s is a complex variable obtained by the Laplace transform.

[0058] According to formula (2), the expression of the expected position of the robot end in the z-axis direction (i.e., the established trajectory adjustment controller model) is as follows:

[0059]

[0060] The S4 controls the movement of the robot end: the expected external force of the robot end in the z-axis direction is set as a constant force, the force-position hybrid control model based on the position mode is used to control the servo motor at the robot joint so that the robot end moves according to the expected trajectory, and the trajectory adjustment controller model is used to fine-tune the expected trajectory of the robot end in the z-axis direction in the control process.

[0061] In this step, by setting the expected external force of the robot end in the z-axis direction in the compliant coordinate system as a constant force, the established force-position hybrid control model based on the position mode and the trajectory adjustment controller model can be used to control the movement of the robot end on the rigid plane and keep the contact force with the plane unchanged, the plane constant force control is realized, and the control needs of the compliant task of the robot end can be met.

[0062] The force of 4N is taken as the expected external force of the robot end in the z-axis direction, the compliant task control method based on the plane constant force is used to control the movement of the robot end, and the conventional position control method is used to control the movement of the robot end as a control. The external force tracking result of the robot end in the z-axis direction when the robot end is in contact with the rigid plane is as shown in Figure 2 It can be seen from Figure 2 that due to the physical interference such as deformation or inclination of the rigid contact surface, when the conventional position control method is used, there is a large extrusion force between the robot end and the rigid contact surface, and when the compliant task control method based on the plane constant force provided by the application is used, the extrusion force between the robot end and the rigid contact surface is significantly reduced, and the force on the robot end can be basically stabilized on the expected external force.

[0063] Finally, it should be noted that the various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referenced to each other.

[0064] The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the same. Although the present invention has been described in detail with reference to preferred embodiments, persons skilled in the art should understand that the specific implementation methods of the present invention may still be modified or some technical features may be replaced by equivalents without departing from the spirit of the technical solutions of the present invention, and all of these should fall within the scope of the technical solutions claimed for protection by the present invention.

Claims

1. A compliant task control method based on planar constant force, characterized in that: The following steps are involved: Obtaining the desired trajectory: Performing dynamic modeling and friction modeling on the robot, then compensating for gravity and friction during the robot's motion to establish a zero-force control model for the position control mode; using the zero-force control model to perform zero-force control on the robot, dragging the robot end to run along the desired trajectory, and collecting the robot end trajectory to generate the desired trajectory; Establishing a force-position hybrid control model based on position mode includes the following steps: A compliant coordinate system is established based on the robot, with the rigid plane as the xoy plane of the compliant coordinate system and the direction perpendicular to the rigid plane as the z axis of the compliant coordinate system; the compliant task of the robot end moving on the rigid plane is decomposed into position control and force control of the robot end along the x-axis, y-axis and z-axis in the compliant coordinate system; Then, the position control and force control of the robot end along the x-axis, y-axis and z-axis are converted into the position control and force control of the robot joint along the x-axis, y-axis and z-axis; According to the control mode of the servo motor at the robot joint, the position control and force control of the robot joint along the x-axis, y-axis and z-axis, a force-position hybrid control model based on the position mode is established; Establish a trajectory adjustment controller model: Based on the small offset caused by external forces when the robot end contacts a rigid surface, a trajectory adjustment controller model is established to correct the desired trajectory of the robot end in the z-axis direction. The robot end is equivalent to a mass-damper-spring system, and the dynamic properties of the robot end are described by inertia, damping, and stiffness. The small offset caused by external forces when the robot end contacts a rigid surface is calculated. The trajectory adjustment controller model is as follows: Among them, z d is the desired position of the robot end in the z-axis direction; r The reference position is the position where the robot end contacts the rigid plane; F dz is the expected external force on the robot end in the z-axis direction; F ez is the equivalent external force on the robot end in the z-axis direction, which is calculated based on the joint torque sensor and dynamic model; M d is the inertia that the robot is expected to exhibit; B d is the damping that the robot is expected to exhibit; K d is the stiffness expected to be exhibited by the robot; s is a complex parameter variable obtained by Laplace transform; Control the movement of the robot end: set the desired external force of the robot end in the z-axis direction to a constant force, use the position-based force-position hybrid control model to control the servo motor at the robot joint so that the robot end moves along the desired trajectory, and use the trajectory adjustment controller model to fine-tune the desired trajectory of the robot end in the z-axis direction during the control process.

2. The compliant task control method based on planar constant force according to claim 1, characterized in that: In the step of establishing a force-position hybrid control model based on the position mode, the compliant task of the robot end moving in the rigid plane is decomposed into the position control of the robot end in the xoy plane and the force control in the z-axis direction in the compliant coordinate system.

3. The compliant task control method based on planar constant force according to claim 2, characterized in that: When decomposing the compliance task of the robot end, the position control and force control of the robot end meet the natural constraints and artificial constraints; The natural constraints are: The artificial constraints are: Among them, f x is the force at the end of the robot along the x-axis, f y is the force on the robot end along the y-axis, f z is the force at the end of the robot along the z-axis, f dz is the desired force at the end of the robot along the z-axis, g x is the torque of the robot end along the x-axis, g y is the torque of the robot end along the y-axis, g z is the torque of the robot end along the z-axis, v x is the average velocity of the robot end along the x-axis, v y is the average velocity of the robot end along the y-axis, v z is the average velocity of the robot end along the z-axis, v dx is the desired velocity of the robot end along the x-axis, v dy is the desired velocity of the robot end along the y-axis, w x is the rotation speed of the robot end along the x-axis, w y is the rotation speed of the robot end along the y-axis, w z is the rotation speed of the robot end along the z-axis.

4. The compliant task control method based on planar constant force according to claim 1, characterized in that: In the force-position hybrid control model based on the position mode, the expected displacement and expected torque of the robot joint along the x-axis, y-axis and z-axis are used as set values, the displacement value obtained by the position sensor at the robot joint is used as the displacement feedback value, the torque value obtained by the force sensor at the robot joint is used as the torque feedback value, and the difference between the expected displacement and the displacement feedback value, as well as the difference between the expected torque and the torque feedback value, are used as input values ​​of the joint space position controller for control.

5. The compliant task control method based on planar constant force according to claim 4, characterized in that: In the force-position hybrid control model based on the position mode, the displacement value obtained by the position sensor at the robot joint needs to be converted into a compliant coordinate system, and the converted displacement value is used as the displacement feedback value.

Images