A compliant control method for a mobile dual-arm robot

Abstract

The present invention belongs to the technical field of intelligent robots, and particularly relates to a compliant control method for a mobile dual-arm robot. The present invention configures a compliant control method on the mobile dual-arm robot to control the mobile body and / or the robotic arm to flexibly avoid the applied external force when the applied external force reaches a preset threshold, so that both the mobile body and the two robotic arms achieve compliant control, making the movement operation of the mobile dual-arm robot safer, with higher end execution accuracy, and improving the ability to cope with complex, harsh, and extreme environments.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent robots, and particularly relates to a compliant control method for a mobile dual-arm robot. Background Art

[0002] Mobile dual-arm robots have broader application scenarios and more flexible operation capabilities than single-arm robots, and are gradually being more and more widely used in many fields. Compliant control is a commonly used control technology for dual-arm robots, which is used to protect the safety of the robot, workpiece and staff during compliant contact or operation of objects such as grinding and assembly. In order to achieve compliant control, the robot must have the ability of force perception. When a large contact force is generated due to an obstacle during the movement process, it can allow a certain offset from the desired target trajectory, thereby achieving the effect of compliant contact and compliant operation. The compliant control effect is usually achieved by impedance control or admittance control methods.

[0003] In the prior art, compliant control has not been applied to wheeled mobile robots, that is, compliant control is achieved at the end of the manipulator of the mobile robot, but compliant control is not achieved on the mobile body, nor is there a compliant control method taking the entire mobile dual-arm robot as the control object. Summary of the Invention

[0004] In view of this, the present invention proposes a compliant control method for a mobile dual-arm robot, which is used to flexibly avoid an applied external force when the mobile body and / or the manipulator are subjected to an applied external force reaching a preset threshold, so that compliant control is achieved for both the mobile body and the two manipulators, making the movement operation of the mobile dual-arm robot safer, the end execution accuracy higher, and the ability to cope with complex, harsh and extreme environments improved.

[0005] In order to achieve the above technical objectives, the specific technical solutions adopted by the present invention are as follows:

[0006] A compliant control method for a mobile dual-arm robot, the robot including a mobile body and two manipulators mounted on the mobile body;

[0007] The robot is configured with the compliant control method; the compliant control method is used to control the mobile body and / or the manipulator of the robot to flexibly avoid the applied external force when the robot is subjected to an applied external force reaching a preset threshold;

[0008] At least one set of inertial measurement devices is provided on the mobile body, and the inertial measurement devices are used to measure the inclination angle and acceleration of the mobile body.

[0009] Further, when one of the two manipulators is subjected to an applied external force:

[0010] When the applied external force reaches a preset threshold, both of the two robotic arms and the mobile body respectively execute the compliant control method; otherwise, only the robotic arm that is subject to the applied external force executes the compliant control method.

[0011] Further, when both of the two robotic arms are subject to the applied external force:

[0012] When the applied external force on one of the two robotic arms reaches a preset threshold, both of the two robotic arms and the mobile body respectively execute the compliant control method; otherwise, the two robotic arms respectively execute the compliant control method;

[0013] Further, the amplitude of the applied external force for the mobile body to execute the compliant control method is equal to the square root of the sum of the squares of the amplitudes of the applied external forces on the two robotic arms.

[0014] Further, when the direction of the applied external force on the mobile body is perpendicular to the walking direction:

[0015] When the applied external force reaches a preset threshold, the mobile dual-arm robot generates a deceleration signal and / or an alarm signal.

[0016] Further, when the direction of the applied external force on the mobile body is not perpendicular to the walking direction, the mobile body executes the compliant control method based on the component of the applied external force in the walking direction of the mobile body.

[0017] Further, the applied external force borne by the mobile body is obtained based on the force direction, force amplitude, and acceleration measured by the inertial measurement device, as well as the driving wheel torque and driving wheel speed of the mobile body;

[0018] The applied external force borne by the robotic arm is obtained based on the torque, speed, and position measured by the joint servo driver and joint torque sensor of the robotic arm.

[0019] Further, two inertial measurement devices are installed on the mobile body; both of the two inertial measurement devices are distributed on the center of gravity line of the mobile body;

[0020] Further, the force direction, force amplitude, and acceleration applied to the compliant control method measured by the inertial measurement device are obtained based on the inclination component and acceleration component;

[0021] The inclination component is calculated based on the following formula:

[0022] θx = θx1 - θx2; θy = θy1 - θy2; θz = θz1 - θz2;

[0023] The acceleration components are calculated based on the following formula:

[0024] λx = λx1 - λx2; λy = λy1 - λy2; λz = λz1 - λz2;

[0025] Wherein,

[0026] θx, θy and θz respectively represent the inclination angles of the moving body in the x, y, and z directions calculated by the inertial measurement device;

[0027] θx1 and θx2 respectively represent the inclination angles measured by the two inertial measurement devices in the x direction;

[0028] θy1 and θy2 respectively represent the inclination angles measured by the two inertial measurement devices in the y direction;

[0029] θz1 and θz2 respectively represent the inclination angles measured by the two inertial measurement devices in the z direction;

[0030] λx, λy and λz respectively represent the accelerations of the moving body in the x, y, and z directions calculated by the inertial measurement device;

[0031] λx1 and λx2 respectively represent the acceleration values measured by the two inertial measurement devices in the x direction;

[0032] λy1 and λy2 respectively represent the acceleration values measured by the two inertial measurement devices in the y direction;

[0033] λz1 and λz2 respectively represent the acceleration values measured by the two inertial measurement devices in the z direction.

[0034] Furthermore, the compliant control method is implemented based on a admittance controller;

[0035] The input values of the admittance controller include the initial desired trajectory instruction and the amplitude of the applied external force;

[0036] The admittance controller adopts the following control algorithm:

[0037]

[0038] Where: Ft is the amplitude of the applied external force; Md is the inertial characteristic value of the moving body or the robotic arm; Dd is the damping characteristic value of the moving body or the robotic arm; Kd is the stiffness characteristic value of the moving body or the robotic arm. Description of the Drawings

[0039] To more clearly illustrate the technical solutions of the embodiments of the present invention, the following will briefly introduce the drawings required for use in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0040] Figure 1 Schematic diagram of sensor data acquisition for the compliance control method in the specific embodiment of the present invention;

[0041] Figure 2 Logic block diagram of the compliance control method in the specific embodiment of the present invention;

[0042] Figure 3 Logic block diagram of the compliance control method when a robotic arm in the specific embodiment of the present invention is subjected to an applied external force;

[0043] Figure 4 Logic block diagram of the compliance control method when both robotic arms in the specific embodiment of the present invention are subjected to an applied external force;

[0044] Figure 5 Logic block diagram of the compliance control method when the mobile body in the specific embodiment of the present invention is subjected to an applied external force;

[0045] Figure 6 Schematic diagram of the structure of a mobile dual-arm robot with a compliance control method in the specific embodiment of the present invention;

[0046] Figure 7 Schematic diagram of the force calculation and control logic of the mobile body in the specific embodiment of the present invention;

[0047] Wherein: 1. Mobile body; 2. First robotic arm; 3. Second robotic arm; 11. First gyroscope; 12. Second gyroscope. Specific embodiment

[0048] The following will describe the embodiments of the present invention in detail with reference to the drawings.

[0049] The following describes the embodiments of the present invention through specific examples. Those skilled in the art can easily understand the other advantages and effects of the present invention from the content disclosed in this specification. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of them. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that, without conflict, the following embodiments and the features in the embodiments can be combined with each other. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts belong to the scope of protection of the present invention.

[0050] It should be noted that the following describes various aspects of the embodiments within the scope of the appended claims. It should be apparent that the aspects described herein can be embodied in a wide variety of forms, and any specific structure and / or function described herein is illustrative only. Based on the present invention, those skilled in the art should understand that one aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number of aspects described herein can be used to implement the device and / or practice the method. Additionally, this device and / or method can be implemented using other structures and / or functionality in addition to one or more of the aspects described herein.

[0051] Furthermore, in the following description, specific details are provided to facilitate a thorough understanding of the examples. However, those skilled in the art will understand that the aspects can be practiced without these specific details.

[0052] In an embodiment of the present invention, a compliant control method for a mobile dual-arm robot is proposed. As Figure 6 shown, the robot includes a movable mobile body 1 and two robotic arms mounted on the mobile body 1;

[0053] A compliant control method is configured on the robot; as Figures 3 - 5 shown, the compliant control method is used to control the mobile body 1 and / or the robotic arms of the robot to perform flexible avoidance of an applied external force when the applied external force reaches a preset threshold;

[0054] At least one set of inertial measurement devices is provided on the mobile body 1, and the inertial measurement devices are used to measure the inclination angle and acceleration of the mobile body 1.

[0055] The mobile body 1 of this embodiment has the ability to move and is provided with rolling wheels driven by servo motors; the two robotic arms are driven by joint motors and are both provided with end effectors to perform humanoid operation functions that coordinate with each other; the current, voltage, and motor encoder data of each servo motor, or the torque sensor data, are all fed back to the robot control system. The robot control system compares these data with the desired trajectory instructions, analyzes and obtains whether the movement of the mobile body 1 and the robotic arms is affected by an applied external force and data such as the magnitude and direction of the applied external force.

[0056] The compliance control method of this embodiment is specifically as follows: When the robot is in motion and suddenly subjected to an external force, the robot and the robotic arms will move along the direction of this external force or the direction of the component force of this external force. After the external force is withdrawn, it will return to the previous motion state. The compliance control method forms a feedback quantity input admittance controller by separately establishing the forward dynamics model and inverse dynamics model of the mobile body 1 and the robotic arms, relying on the data of the robotic arm servo driver (such as torque magnitude, position angle) and the data of the mobile body 1 drive servo driver (such as driving torque magnitude, rotational speed). At the same time, the trajectory data generated by the upper robot controller is decomposed into the walking trajectory of the mobile body 1 and the action trajectory of the robotic arm and input into the admittance controller. The output data of the admittance controller goes to the inverse dynamics model, and finally forms a control quantity to each servo driver. At this time, the mobile body 1 and the robotic arms have the avoidance function controlled by the above compliance control method.

[0057] During the movement of the robot in this embodiment, when the mobile body 1 is suddenly impacted by an external force, the compliance control of the mobile body 1 will take effect instantly. The mobile body 1 will deviate a small distance in the direction of the impact force, which is equivalent to "dodging" by itself after being hit, thereby actively dissipating the impact force, playing a buffering role, and naturally also playing a protective role, so the safety is improved.

[0058] It should be noted that admittance control is only one of the embodiments of the present invention. There are other compliance control methods, such as impedance control methods. When a similar implementation method is used for the compliance control of a wheeled mobile robot to achieve a similar effect, it also belongs to the protection scope of the present invention.

[0059] The compliance control method of this embodiment is implemented based on an admittance controller; the input values of the admittance controller include the initial desired trajectory instruction and the amplitude of the applied external force.

[0060] The admittance controller adopts the following control algorithm:

[0061]

[0062] Where: Ft is the amplitude of the applied external force; Md is the inertia characteristic value of the mobile body 1 or the robotic arm; Dd is the damping characteristic value of the mobile body 1 or the robotic arm; Kd is the stiffness characteristic value of the mobile body 1 or the robotic arm.

[0063] As Figure 2 shown, the control of the robotic arm or the mobile body 1 includes a trajectory generator, an admittance controller, a robotic arm controller, and a mobile body controller.

[0064] In the figure,

[0065] Xr1, Xr2: are respectively the initial desired trajectory commands of the mobile body 1 and the robotic arm;

[0066] Xd1, Xd2: are the outputs of the admittance controller;

[0067] F1, F2: are respectively the forces output by the mobile body controller and the robotic arm controller;

[0068] Ft1, Ft2: are respectively the external forces received by the mobile body 1 and the robotic arm;

[0069] X1, X2: are respectively the feedback quantities generated by the mobile body 1 and the feedback quantities generated by the robotic arm;

[0070] After the trajectory generator module in the robot controller generates a trajectory command, it is input to the trajectory decomposition unit. The function of the trajectory decomposition unit is to decompose the overall trajectory command into the motion trajectory Xr1 of the mobile body 1 and the motion trajectory Xr2 of the robotic arm, and input them into the subsequent links respectively.

[0071] Xr1 and Xr2 are input to the admittance controller. The admittance controller outputs Xd1. The mobile body controller and the mobile body 1 form a closed-loop speed / torque control system, and the robotic arm controller and the robotic arm form a closed-loop speed / torque control system. When the external force Ft1 acts on the mobile body 1, the external force Ft1 will be superimposed on the input of the admittance controller, which is equivalent to adding an offset to the input of the admittance controller. The output Xd1 of the admittance controller will follow (Xr1 + Xt1). When the external force Ft1 is withdrawn, the output Xd1 of the admittance controller will still follow the input Xr1; the robotic arm in this embodiment is also analyzed by the same method.

[0072] In one embodiment, the compliant control methods of the two robotic arms and the mobile body 1 of the present invention are combined with each other. As Figure 3 shown, when an external force is applied to one of the two robotic arms: when the applied external force reaches a preset threshold, both robotic arms and the mobile body 1 respectively execute the compliant control method; otherwise, only the robotic arm that receives the applied external force executes the compliant control method.

[0073] AsFigure 3 As shown, when an external force Ft is applied to one of the first robotic arm 2 and the second robotic arm 3 in a dual-arm robot, if the amplitude of the external force is within a certain set threshold range, such as F threshold within the range, then only the compliant control algorithm based on the external force Ft is executed for the first robotic arm 2 and the second robotic arm 3 simultaneously until Ft disappears. This compliant control method helps, when an external force is applied to one robotic arm, the other unloaded robotic arm to move in the same direction as the loaded robotic arm, to avoid the possibility of collision between the two robotic arms, thereby improving safety. When the external force Ft is higher than F threshold , it indicates that the amplitude of the external force has exceeded a certain amplitude. For example, if a robotic arm suddenly collides with an external object, not only the two robotic arms need to execute compliant control, but also the mobile body 1 executes compliant control based on Ft or based on Ftx to generate a compliant control effect from the overall robot, further reducing risks and enhancing safety; here, Ftx is the component of Ft in the x direction (the moving direction or the opposite direction of the mobile body 1). Specifically, whether to use Ft or Ftx as the basis for the compliant control of the mobile body 1 can be set in a set manner or according to the specific application of the robot.

[0074] In one embodiment, when external forces are applied to both robotic arms: as Figure 4 shown, when the external force applied to one of the two robotic arms reaches a preset threshold, the two robotic arms and the mobile body 1 all execute the compliant control method respectively; otherwise, the two robotic arms execute the compliant control method respectively;

[0075] wherein: the amplitude of the external force for the mobile body 1 to execute the compliant control method is equal to the square root of the sum of the squares of the amplitudes of the external forces received by the two robotic arms.

[0076] In this embodiment, when external forces are applied to both robotic arms simultaneously, the two robotic arms execute flexible control simultaneously; when the force applied to one of the two robotic arms is higher than the set threshold value, then the mobile body 1 also executes compliant control. The control flow block diagram is as Figure 4 shown, in the figure: Fta and Ftb respectively represent the external forces applied to the first robotic arm 2 and the second robotic arm 3;

[0077] When one of the values of Fta or Ftb is higher than the set threshold value F threshold , then the mobile body 1 also executes compliant control and performs compliant control based on the value.

[0078] This embodiment combines the admittance control of the mobile body 1 with the admittance control of the robotic arms, enabling the robotic arms and the mobile body 1 of the robot to form an integrated whole, making the overall robot have better compliance when encountering external forces applied.

[0079] In one embodiment, the mobile body 1 is driven by two pairs of drive wheels. If the mobile body 1 steers and avoids the applied external force, the time delay will be too long and the compliance control method will fail. Therefore, when the direction of the applied external force on the mobile body 1 is perpendicular to the walking direction:

[0080] When the applied external force reaches a preset threshold, the mobile dual-arm robot generates a deceleration signal and / or an alarm signal.

[0081] When the direction of the applied external force on the mobile body 1 is not perpendicular to the walking direction, the compliance control method is executed based on the component force of the applied external force in the walking direction of the mobile body 1.

[0082] As Figure 5 shown, when the mobile body 1 is subjected to an external force Ft, the system judges the relationship between the direction of the external force and the current walking direction. If the external force Fty perpendicular to the current walking direction has an amplitude greater than the set value Ftysh, it indicates that the robot is laterally impacted by a large force. The robot automatically decelerates and alarms until it stops for inspection. Conversely, if the system judges that there is an external force in the walking axis direction (including the forward or backward direction) of the robot, the component external force Ftx of the external force in this direction is calculated accordingly, and the compliance control of the mobile body 1 is executed based on this until the external force disappears and the previous movement is continued. This control strategy helps when the mobile body 1 is impacted by an external force in the same or opposite direction to the walking direction. The robot can move to a certain extent with the external force through the compliance control method, thus playing a buffering role and reducing the possible damage to the mobile body 1 or the external force applicator.

[0083] In one embodiment, the applied external force borne by the mobile body 1 is obtained based on the force direction, force amplitude, and acceleration measured by the inertial measurement device, as well as the drive wheel torque and drive wheel speed of the mobile body 1;

[0084] The applied external force borne by the robotic arm is obtained based on the torque, speed, and position measured by the joint servo driver and joint torque sensor of the robotic arm.

[0085] In one embodiment, as Figure 6 shown, two inertial measurement devices are installed on the mobile body 1; both of the two inertial measurement devices are distributed on the center of gravity line of the mobile body 1; the inertial measurement device in this embodiment is a gyroscope sensor. The first gyroscope 11 is installed on the upper part of the mobile body 1, near the head and neck position; the second gyroscope 12 is installed at the position of the robot chassis.

[0086] In this embodiment, an xyz coordinate system is established based on the installation center position of the gyroscope sensor 2. The horizontal plane is set as the xy plane, the positive front direction of the robot's movement is the x-axis, the direction perpendicular to the movement direction is the y-axis, and the z-axis is perpendicular to the xy plane and points upward.

[0087] When an external force acts on the moving body 1, both of the two gyroscope sensors will output data, such as inclination angle, acceleration and other data. Based on this, the robot control system calculates the acting force on the moving body 1, which is divided into components in three directions, such as Ftx in the x direction, Fty in the y direction, and Ftz in the z direction. The specific processing principle is as Figure 6 shown.

[0088] In this embodiment, the force direction, force amplitude, and acceleration applied to the compliant control method measured by the inertial measurement device are obtained based on the inclination angle components and acceleration components;

[0089] The inclination angle components are calculated based on the following formula:

[0090] θx = θx1 - θx2; θy = θy1 - θy2; θz = θz1 - θz2;

[0091] The acceleration components are calculated based on the following formula:

[0092] λx = λx1 - λx2; λy = λy1 - λy2; λz = λz1 - λz2;

[0093] Where,

[0094] θx, θy, and θz respectively represent the inclination angles of the moving body in the x, y, and z directions calculated by the inertial measurement device;

[0095] θx1 and θx2 respectively represent the inclination angles measured by the two inertial measurement devices in the x direction;

[0096] θy1 and θy2 respectively represent the inclination angles measured by the two inertial measurement devices in the y direction;

[0097] θz1 and θz2 respectively represent the inclination angles measured by the two inertial measurement devices in the z direction;

[0098] λx, λy, and λz respectively represent the accelerations of the moving body in the x, y, and z directions calculated by the inertial measurement device;

[0099] λx1 and λx2 respectively represent the acceleration values measured by the two inertial measurement devices in the x direction;

[0100] λy1 and λy2 respectively represent the acceleration values measured by the two inertial measurement devices in the y direction;

[0101] λz1 and λz2 respectively represent the acceleration values measured in the z direction by two inertial measurement devices.

[0102] As described above, it is only the specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims

1. A compliant control method for a mobile dual-arm robot, characterized in that, The robot includes a mobile body and two robotic arms mounted on the mobile body; The compliant control method is configured on the robot; the compliant control method is used to control the mobile body and / or the robotic arms of the robot to flexibly avoid the applied external force when the applied external force reaches a preset threshold; At least one set of inertial measurement devices is provided on the mobile body, and the inertial measurement devices are used to obtain the feedback quantity of the external force, including the inclination angle and acceleration of the mobile body; The force direction, force amplitude, and acceleration applied to the compliant control method measured by the inertial measurement device are obtained based on the inclination angle component and acceleration component; wherein, there are two inertial measurement devices; The inclination angle component is calculated based on the following formula: θx = θx1 - θx2; θy = θy1 - θy2; θz = θz1 - θz2; The acceleration component is calculated based on the following formula: λx = λx1 - λx2; λy = λy1 - λy2; λz = λz1 - λz2; Wherein, θx, θy, and θz respectively represent the inclination angles of the mobile body in the x, y, and z directions calculated by the inertial measurement device; θx1 and θx2 respectively represent the inclination angles measured by the two inertial measurement devices in the x direction; θy1 and θy2 respectively represent the inclination angles measured by the two inertial measurement devices in the y direction; θz1 and θz2 respectively represent the inclination angles measured by the two inertial measurement devices in the z direction; λx, λy, and λz respectively represent the accelerations of the mobile body in the x, y, and z directions calculated by the inertial measurement device; λx1 and λx2 respectively represent the acceleration values measured by the two inertial measurement devices in the x direction; λy1 and λy2 respectively represent the acceleration values measured by the two inertial measurement devices in the y direction; λz1 and λz2 respectively represent the acceleration values measured by the two inertial measurement devices in the z direction.

2. The compliant control method according to claim 1, wherein When one of the two robotic arms is subjected to an applied external force: When the applied external force reaches a preset threshold, both of the two robotic arms and the mobile body respectively execute the compliant control method; otherwise, only the robotic arm subjected to the applied external force executes the compliant control method.

3. The compliant control method according to claim 1, characterized in that When both of the two robotic arms are subjected to an applied external force: When the applied external force received by one of the two robotic arms reaches a preset threshold, both of the two robotic arms and the mobile body respectively execute the compliant control method; otherwise, the two robotic arms respectively execute the compliant control method.

4. The compliant control method according to claim 3, wherein The amplitude of the applied external force for the mobile body to execute the compliant control method is equal to the square root of the sum of the squares of the amplitudes of the applied external forces received by the two robotic arms.

5. The compliant control method according to claim 1, characterized in that When the direction of the applied external force received by the mobile body is perpendicular to the walking direction: When the applied external force reaches a preset threshold, the mobile dual-arm robot generates a deceleration signal and / or an alarm signal.

6. The compliant control method according to claim 1, characterized in that, When the direction of the applied external force received by the mobile body is not perpendicular to the walking direction, the mobile body executes the compliant control method based on the component force of the applied external force in the walking direction of the mobile body.

7. The compliant control method according to claim 1, characterized in that, The applied external force borne by the mobile body is obtained based on the force direction, force amplitude, and acceleration measured by the inertial measurement device, as well as the driving wheel torque and driving wheel speed of the mobile body; The applied external force borne by the robotic arm is obtained based on the torque, speed, and position measured by the joint servo driver and joint torque sensor of the robotic arm.

8. The compliant control method according to claim 1, wherein Both of the two inertial measurement devices are distributed on the center of gravity line of the mobile body.

9. The compliant control method according to claim 1, wherein The compliant control method is implemented based on an admittance controller; The input value of the admittance controller includes an initial desired trajectory command and the amplitude of the applied external force; The admittance controller adopts the following control algorithm: Where: Ft is the amplitude of the applied external force; M d is the inertia characteristic value of the mobile body or the robotic arm; D d is the damping characteristic value of the mobile body or the robotic arm; K d is the stiffness characteristic value of the mobile body or the robotic arm; x d is the output of the admittance controller; x r is the initial desired trajectory command.

Images