Service robot and control method

Abstract

The invention relates to the technical field of robot control, and provides a service robot and a control method, and the method comprises the following steps: obtaining joint initial radian data of each mechanical arm in actual operation, and calculating to obtain an initial actual position of the tail end of each mechanical arm based on the joint initial radian data; according to the initial expected position of the tail end of each mechanical arm and the initial actual position of the tail end of each mechanical arm, the error of the initial position of the tail end of each mechanical arm is calculated; based on the tail end initial position error of each mechanical arm, the actual joint angle of each mechanical arm is adjusted in real time, and the joint updating radian data of each mechanical arm after real-time adjustment is obtained; and according to the joint updating radian data of the mechanical arms and the tail end synchronous expected positions of the mechanical arms, the tail end relative position errors of the mechanical arms are adjusted through self-adaptive PID control. The invention provides a service robot and a control method. The flexibility and accuracy of the service robot are improved.

Description

Technical Field

[0001] The present application relates to the field of robot control technology, and in particular to a service robot and a control method thereof. Background Art

[0002] Service robots are robots that perform tasks in various non-industrial environments, which usually involve human service and assistance. With the rapid development of intelligence, the tasks that service robots need to perform are becoming more and more complex, and the requirements for the operational flexibility and precision of service robots are becoming higher and higher. Therefore, how to enable service robots to complete tasks more flexibly and accurately has become a major problem that needs to be solved. Summary of the invention

[0003] The present application provides a service robot and a control method to improve the flexibility and accuracy of the service robot.

[0004] In a first aspect, a control method for a service robot is provided, comprising:

[0005] Acquire the initial radian data of the joints of each robotic arm in actual operation, and calculate the initial actual position of the end of each robotic arm based on the initial radian data of the joints;

[0006] According to the initial expected position of the end of each robot arm and the initial actual position of the end of each robot arm, the initial position error of the end of each robot arm is calculated;

[0007] Based on the initial position error of the end of each robotic arm, the actual joint angle of each robotic arm is adjusted in real time, and the updated radian data of the joints of each robotic arm after the real-time adjustment is obtained;

[0008] According to the updated arc data of the joints of the robotic arms and the desired synchronous positions of the ends of the robotic arms, the relative position errors of the ends of the robotic arms are adjusted through adaptive PID control.

[0009] In one implementation, the calculating of the initial actual position of the end of each robotic arm based on the initial arc data of the joint includes:

[0010] According to the initial arc data of the joints of each robot arm in actual operation, the actual joint angle of each robot arm is calculated;

[0011] The actual transformation matrix of the end of each robotic arm is obtained according to the actual joint angle of each robotic arm, and the initial actual position of the end of each robotic arm is calculated based on the actual transformation matrix of the end of each robotic arm.

[0012] In one implementation, the actual joint angles of each robotic arm are calculated based on the initial arc data of the joints of each robotic arm in actual operation, including:

[0013] The joint initial radian data is read, and converted into the actual joint angle of each robotic arm through an angle calculation formula. The joint initial radian data is the hexadecimal representation data of the rotation radian of each joint.

[0014] In one implementation, the actual transformation matrix of the end of each robotic arm is obtained according to the actual joint angle of each robotic arm, and the initial actual position of the end of each robotic arm is calculated based on the actual transformation matrix of the end of each robotic arm, including:

[0015] The actual transformation matrix of the end of the manipulator is obtained by forward kinematics solution, and the initial actual position of the end of each manipulator is calculated. The formula is as follows:

[0016]

[0017] Among them, P actual is the initial actual position of the end of the robot arm, T actual is the actual transformation matrix of the end of the robot, T actual [1:3,4] represents the data of the 4th column and the 1st to the 3rd row of the actual transformation matrix of the end of the robotic arm.

[0018] In one implementation, the calculating of the initial position error of the end of each robotic arm according to the initial expected position of the end of each robotic arm and the initial actual position of the end of each robotic arm includes:

[0019] The initial desired position of the end of the robot arm is P desire ,

[0020] The initial position error of each robot end is e p :e p =P desire -P actual .

[0021] In one implementation, adjusting the actual joint angles of each robotic arm in real time based on the initial position errors of the ends of each robotic arm, and obtaining updated radian data of the joints of each robotic arm after real-time adjustment, includes:

[0022] Performing spatial angle mapping on the initial position error of the end of the robotic arm and associating it with the angle change of each joint of the robotic arm, and calculating the adjustment amount of each actual joint angle;

[0023] According to the adjustment amount of each actual joint angle, each actual joint angle of the robot arm is adjusted in real time.

[0024] In one implementation, the formula for calculating the adjustment amount of each actual joint angle includes:

[0025]

[0026] Δθ i =J + e p ;

[0027] J + =(J T J) -1 J T ;

[0028] Among them, Δθ i is the actual joint angle adjustment; e p The initial position error of each robot end; J is the Jacobian matrix; J + is the pseudo-inverse of the Jacobian matrix; α 1_actual …α 6_actual are the six joints of the robotic arm.

[0029] In one implementation, the updating of the arc data of the joints of the robotic arms and the desired synchronization position of the ends of the robotic arms to adjust the actual angles of the joints of the robotic arms includes:

[0030] According to the updated arc data of the joints of the robotic arms and the expected synchronous positions of the ends of the robotic arms, the relative position errors of the ends of the robotic arms are obtained, and the formula is as follows:

[0031] E sync =|(P desire_l -P actual_l )-(P desire_r -P actual_r )|;

[0032] Among them, E sync is the relative position error of the end of the robotic arm.

[0033] In one implementation, the updating of the arc data of the joints of the robotic arms and the desired synchronization positions of the ends of the robotic arms, and adjusting the relative position errors of the ends of the robotic arms by adaptive PID control, include:

[0034] The adaptive PID control formula is as follows:

[0035]

[0036] Among them, u(t) k is the control amount of the relative position error of the end of the robot arm; p (t), K i (t), K d (t) is the PID parameter.

[0037] In a second aspect, the present application further provides a service robot, the service robot being provided with a plurality of mechanical arms, each mechanical arm being provided with 6 joints, and further comprising:

[0038] The joint encoder is used to obtain the initial radian data of the joints of each robot arm in actual operation, and send the initial radian data of the joints to the central control unit;

[0039] The central control unit is used to transmit the initial arc data of the joint to the PID controller;

[0040] The PID controller is used to calculate the initial position error of the end of each robotic arm according to the initial expected position of the end of each robotic arm and the initial actual position of the end of each robotic arm;

[0041] The PID controller is further used to adjust the actual joint angle of each robotic arm in real time based on the initial position error of the end of each robotic arm, and obtain the joint update radian data of each robotic arm after real-time adjustment;

[0042] The PID controller is also used to update the arc data according to the joints of the robotic arms and synchronize the desired positions of the ends of the respective robotic arms, and to adjust the relative position errors of the ends of the respective robotic arms through adaptive PID control.

[0043] In the present application, error compensation can be performed on the robot's mechanical arm individually, and an adaptive PID control compensation strategy is designed for the robot in the synchronous operation scenario to eliminate the error of the robot's mechanical arm during the synchronization process. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] The preferred implementation scheme will be described below in a clear and understandable manner with reference to the accompanying drawings to further illustrate the above-mentioned characteristics, technical features, advantages and implementation methods of the present application.

[0045] Figure 1 is a flow chart of a control method of a service robot in the present application;

[0046] Figure 2 This is the flow chart of obtaining joint data by CAN bus in this application;

[0047] Figure 3 It is a flow chart of the dual-arm robot end position error compensation control system in this application;

[0048] Figure 4 It is the double-arm trajectory tracking diagram before double-arm optimization in this application;

[0049] Figure 5 It is the double-arm trajectory tracking diagram after double-arm optimization in this application;

[0050] Figure 6 It is a comparison diagram of the relative errors before and after the double-arm optimization in this application;

[0051] Figure 7 It is the structure diagram of the adaptive PID controller in this application;

[0052] Figure 8 is a single-arm error elimination flow chart in this application;

[0053] Fig. 9 It is a diagram of a single-arm two-dimensional model in this application;

[0054] Fig.10 It is the error analysis plane diagram in this application. DETAILED DESCRIPTION

[0055] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.

[0056] It should be understood that when used in this specification and the appended claims, the term "comprising" indicates the presence of the described features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or collections.

[0057] In order to simplify the drawings, only the parts related to the present invention are schematically shown in each figure, and they do not represent the actual structure of the product. In addition, in order to simplify the drawings and facilitate understanding, in some figures, only one of the parts with the same structure or function is schematically drawn or marked. In this article, "one" not only means "only one", but also means "more than one".

[0058] It should be further understood that the term “and / or” used in the specification and appended claims refers to any combination and all possible combinations of one or more of the associated listed items, and includes these combinations.

[0059] In addition, in the description of the present application, the terms "first", "second", etc. are only used to distinguish the description and cannot be understood as indicating or implying relative importance.

[0060] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the specific implementation methods of the present invention will be described below with reference to the accompanying drawings. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings and other implementation methods can be obtained based on these drawings without creative work.

[0061] Unless otherwise defined, all technical and scientific terms used in the embodiments of the present application have the same meanings as those commonly understood by those skilled in the art. The terms used in the embodiments of the present application are only for the purpose of describing the embodiments of the present application and are not intended to limit the present application.

[0062] Before further describing the embodiments of the present application in detail, the nouns and terms involved in the embodiments of the present application are explained, and the nouns and terms involved in the embodiments of the present application are subject to the following explanations. Service robots are a young member of the robot family and do not yet have a strict definition. Different countries have different understandings of service robots. They can be divided into professional service robots and personal / family service robots. Service robots have a wide range of applications and are mainly engaged in maintenance, repair, transportation, cleaning, security, rescue, guardianship and other tasks.

[0063] 1) Service robots are robots that perform tasks in various non-industrial environments, which usually involve human service and assistance.

[0064] 2) The robot arm refers to a complex system with high precision, multiple inputs and multiple outputs, high nonlinearity, and strong coupling. Due to its unique operational flexibility, it has been widely used in industrial assembly, safety and explosion protection, and other fields. The robot arm is a complex system with uncertainties such as parameter perturbations, external interference, and unmodeled dynamics. Therefore, the modeling model of the robot arm also has uncertainties. For different tasks, it is necessary to plan the motion trajectory of the robot arm joint space, so as to cascade and form the terminal posture.

[0065] 3) As a service robot, the dual-arm robot can significantly improve the flexibility and efficiency of operation by using two mechanical arms to complete tasks together. When the two arms work together to perform tasks, the two arms need to cooperate, which involves precise control of the two arms.

[0066] 4) PID control. As the most popular and widely used controller, PID controller is widely used for rigid robot arm control due to its simplicity, effectiveness and practicality. It is often used to improve the performance of PID controller by adjusting the controller gain to form a self-correcting PID controller or combining it with other control methods to form a composite control system.

[0067] In complex tasks, we should not only focus on the precise control of a single robot arm, but also on the synchronous control of both arms. The synchronous control of both arms aims to coordinate the movement of the two arms to achieve precise operation tasks. Among them, synchronous control not only involves the individual control of each robot arm, but also needs to ensure that the two arms can effectively cooperate when performing tasks, thereby improving the overall effect of the operation. However, in specific application scenarios, existing dual-arm control methods often have problems such as insufficient coordination effect and insufficient control accuracy, which makes it difficult to reach the expected position at the same time when the task is executed, and cannot meet the needs of complex tasks.

[0068] Based on the above analysis of the related technologies, it can be seen that the importance of synchronous control of the robot arm in the embodiment of the present application is obvious. In order to meet the design requirements of the humanoid robot arm, the mechanical structure is finely designed, and a lightweight design scheme is adopted (such as Fig. 9 The figure shows a single-arm model of a service robot. The human arm is mainly composed of shoulder joints, elbow joints, and wrist joints, and the joints are connected to each other through the muscles of the upper and lower arms. The shoulder joint enables the upper arm to swing forward, extend backward, abduct, adduct, and rotate internally and externally; the elbow joint mainly controls the forward movement and extension of the lower arm. The muscles of the lower arm can rotate internally and externally. Based on the analysis of these arm movements, the design considers adding redundant degrees of freedom, so the task is selected as a humanoid robot arm with six degrees of freedom.

[0069] In a first aspect, the present application provides a service robot, the service robot is provided with a plurality of mechanical arms, each mechanical arm is provided with 6 joints, and further includes:

[0070] The joint encoder is used to obtain the initial radian data of the joints of each robot arm during actual operation, and send the initial radian data of the joints to the central control unit.

[0071] The joint encoder includes but is not limited to a joint angle encoder, and each joint of each robotic arm is equipped with a joint encoder. The number of robotic arms in the present application includes two or more than two, and two robotic arms are used as an example for illustration.

[0072] In this application, reference Fig. 9As shown in the single-arm two-dimensional model diagram, the robot arm adopts a lightweight humanoid design. Each robot arm is equipped with 6 joint modules, with a total of 12 degrees of freedom. Among them, the shoulder joint 100 contains 2 degrees of freedom, the elbow joint 200 contains 2 degrees of freedom, and the wrist joint 300 contains 2 degrees of freedom. The connection between the upper arm and the lower arm of each robot arm adopts aluminum alloy machined parts. The advantages of aluminum alloy machined parts are beautiful and strong hardness, ensuring that the robot arm can flexibly perform complex tasks.

[0073] The central control unit receives the initial radian data of the joints of each mechanical arm in actual operation and transmits the initial radian data of the joints of each mechanical arm in actual operation to the PID controller, and the PID controller calculates the initial actual position of the end of each mechanical arm based on the initial radian data of the joints. According to the initial expected position of the end of each mechanical arm and the initial actual position of the end of each mechanical arm, the initial position error of the end of each mechanical arm is calculated.

[0074] Due to the differences in joint links and mechanical construction methods, the dynamic models of the robotic arms are also different. The robotic arms of this application set the shoulder joint as the base coordinate, establish a DH parameter table based on the shoulder joint improvement, construct the robotic arm dynamic model through the DH parameter table, and calculate the transformation matrix of the six joints of the two arms. Then, the actual transformation matrix of the end of each robotic arm is obtained by using the transformation matrix of this joint and the actual joint angle of each robotic arm, and based on the actual transformation matrix of the end of each robotic arm, the initial actual position of the end of each robotic arm is calculated.

[0075] In order to make the synchronous control of the robotic arms more flexible, the PID controller is also used to update the arc data according to the joints of the robotic arms and synchronize the desired positions of the ends of each robotic arm, and adjust the relative position errors of the ends of each robotic arm through adaptive PID control.

[0076] An adaptive PID control method is performed by a PID controller, which includes updating arc data according to the joints of the robotic arms and synchronizing the desired positions of the ends of the robotic arms, and adjusting the relative position errors of the ends of the robotic arms by adaptive PID control.

[0077] This application adopts an improved PID controller to perform error compensation measures. The PID controller structure is as follows: Figure 8 As shown in the figure, the PID controller controls the robot arm to reach the desired position by setting the desired end position, and then obtains the actual joint angle information through the joint angle encoder through CAN bus communication. p , K i ,K dThe three parameters are adaptively controlled to adjust the dual-arm synchronization error to achieve precise control. The present invention uses the gradient descent method to adjust the PID parameters online in real time, and achieves the control effect by continuously updating the gain value.

[0078] In this application, by adjusting the control parameters of the PID controller in real time, the proportional, integral and differential gains can be dynamically optimized according to the synchronous position error of the left and right manipulators to perform position compensation. The controller can be flexibly adapted under different working conditions, effectively improving the synchronous control accuracy of the service robot manipulator and reducing the synchronization error. The communication between the joint encoder and the central control unit, the communication between the joint encoder and the PID controller, and the communication between the PID controller and the central control unit can be achieved through wired or wireless communication methods. Wired communication methods include, but are not limited to, controller area network (CAN) bus, controller area network with flexible data rate (CAN with flexible data-rate, CAN FD) bus, local interconnect network (LIN) bus, Flex Ray bus, media oriented systems transport (MOST) bus, low voltage differential signaling (LVDS) bus, time triggered protocol / class C (TTP / C) bus, or Ethernet bus; wireless communication methods include, but are not limited to, wireless sensor networks (e.g., Bluetooth, Starflash, or ZigBee), wireless local area networks (e.g., WLAN or WiFi), short-range point-to-point communication, near field communication technology (NFC), etc. In addition, the communication between the joint encoder and the central control unit, the communication between the joint encoder and the PID controller, and the communication between the PID controller and the central control unit can be the same or different.

[0079] A control method for a robot is described below in conjunction with the accompanying drawings, which can perform single-arm precise control of the robot's mechanical arm and synchronous control of multiple arms, and uses an adaptive PID control method to achieve real-time adaptive synchronous control of the robot's mechanical arm.

[0080] Please refer to Figure 1 , which is an exemplary flow chart of a robot control method provided in an embodiment of the present application, specifically comprising:

[0081] S100 obtains the initial radian data of the joints of each robotic arm during actual operation, and calculates the initial actual position of the end of each robotic arm based on the initial radian data of the joints.

[0082] In the present application, in order to make the control process of the robotic arms more accurate and efficient, each robotic arm is first controlled independently to eliminate the initial position error of the end effector of each robotic arm, in preparation for the subsequent synchronous execution of tasks between the robotic arms.

[0083] This application first takes a robot with two mechanical arms as an example. First, the joint encoder obtains the actual angle data of the joints of the left and right arms during operation as the initial radian data of the joints, so that the initial radian data of the joints of the left arm and the initial radian data of the joints of the right arm can be obtained. Taking the left arm as an example, the initial radian data of the joints of the left arm is obtained through the joint encoder and transmitted to the central control unit. The initial actual position of the end of the left arm is calculated by the PID controller of the central control unit. The initial actual position of the end of the left arm refers to the initial actual position of the end effector of the left arm.

[0084] S200 calculates the initial position error of the end of each robotic arm according to the initial expected position of the end of each robotic arm and the initial actual position of the end of each robotic arm.

[0085] From the above, it can be seen that after the initial actual position of the end of each robot arm is obtained, the difference between the initial actual position of the end of the robot arm and the initial expected position of the end is calculated according to the set initial expected position of the end, which is used as the initial position error of the end.

[0086] It should be noted that at this time, each robotic arm has its own initial position error at the end, and it is necessary to independently perform error compensation on each robotic arm based on the initial position error at the end of each robotic arm.

[0087] S300 adjusts the actual joint angles of each robotic arm in real time based on the initial position errors of the ends of each robotic arm, and obtains updated radian data of the joints of each robotic arm after real-time adjustment.

[0088] S400 adjusts the relative position error of the end of each robotic arm through adaptive PID control according to the updated arc data of the joints of the robotic arms and the desired synchronous position of the end of each robotic arm.

[0089] The relative position error of the ends of each robotic arm refers to the synchronous position error of each robotic arm when performing a task synchronously. Taking a dual-arm robot as an example, it specifically refers to the synchronization deviation of the dual arms on the desired trajectory. The control goal of this application is to minimize the synchronous position error of each robotic arm.

[0090] This application uses an adaptive PID control method to compensate for the synchronization position error, controls the robot arm to reach the desired position by setting the desired end position, and then obtains the actual joint angle information through the joint angle encoder through CAN bus communication. By using an adaptive control strategy for PID parameters, the synchronization error of the two arms is adjusted to achieve precise control.

[0091] In order to more clearly describe the calculation method of the initial actual position of the end of the present application, a dual-arm robot is used for illustration. In one implementation, the process of calculating the initial actual position of the end of each robotic arm may include a process of calculating the actual joint angle and a process of calculating the initial actual position of the end effector, that is, the calculation based on the initial arc data of the joint to obtain the initial actual position of the end of each robotic arm includes:

[0092] According to the initial arc data of the joints of each robot arm in actual operation, the actual joint angle of each robot arm is calculated;

[0093] The actual transformation matrix of the end of each robotic arm is obtained according to the actual joint angle of each robotic arm, and the initial actual position of the end of each robotic arm is calculated based on the actual transformation matrix of the end of each robotic arm.

[0094] Due to the differences in joint links and mechanical construction methods, the dynamic models of the robotic arm are also different. The robotic arm of this application sets the shoulder joint as the base coordinate and establishes a DH parameter table based on the improvement of the shoulder joint. The DH parameter table of the left arm joint is shown in the following table:

[0095]

[0096]

[0097] The DH parameter table of the right arm joint of the robot arm in this application is as follows:

[0098]

[0099] The mechanical arm dynamics model is constructed through the DH parameter table, and the transformation matrix of the six joints of the two arms is calculated. Since the present application is a method for constructing a humanoid mechanical arm, the left and right arms are symmetrical, and the T matrix of each mechanical arm is the same. The following formula is the T matrix of the two arms of the present application:

[0100]

[0101] The transformation matrix T of all joints i Multiply to get the actual transformation matrix T of the end effector actual :

[0102] T actual=T1·T2·T3·T4·T5·T6;

[0103] Specifically, Figure 2 As shown, the joint encoder will use the CAN communication protocol to send the joint angle data of the current robot arm after actual movement to the central control unit through the host computer. The central control unit receives the joint angle data after actual movement transmitted from the joint encoder, that is, the joint initial radian data, and reads the feedback joint initial radian data. The joint initial radian data is the rotation radian of each joint expressed in hexadecimal, that is, the actual joint angle.

[0104] Among them, the CAN communication protocol is the communication protocol that comes with the robot's motor.

[0105] Specifically, according to the initial arc data of the joints of each robot arm in actual operation, the actual joint angles of each robot arm are calculated, including:

[0106] The initial joint radian data is read, and the initial joint radian data is converted into the actual joint angle of each robotic arm through the angle calculation formula. The initial joint radian data is the hexadecimal representation of the rotation radian of each joint. The calculation formula of the actual joint angle of each robotic arm is as follows:

[0107]

[0108] Among them, θ i_actual is the actual joint angle.

[0109] In the present application, the robot's motor obtains the conversion formula after the hexadecimal radian system, that is, the actual joint angle of each robotic arm. The robot's motor has its own conversion format, and this conversion formula is used to calculate the actual angle of each joint, that is, the actual joint angle described in this application.

[0110] In this application, the actual transformation matrix of the end of each robotic arm is obtained according to the actual joint angle of each robotic arm, and the initial actual position of the end of each robotic arm is calculated based on the actual transformation matrix of the end of each robotic arm, including:

[0111] The actual transformation matrix of the end of the manipulator is obtained by forward kinematics solution, and the initial actual position of the end of each manipulator is calculated. The formula is as follows:

[0112]

[0113] Among them, P actual is the initial actual position of the end of the robot arm, T actual is the actual transformation matrix of the end of the robot, T actual[1:3,4] represents the data of the 4th column and the 1st to the 3rd row of the actual transformation matrix of the end of the robot. 14 , T 24 , T 34 Specifically represents the actual position of x, y, z, that is, x actual ,y actual ,z actual

[0114] According to the initial expected position of the end of each robot arm and the initial actual position of the end of each robot arm, the initial position error of the end of each robot arm is calculated, including:

[0115] The initial desired position of the end of the robot arm is P desire :

[0116] The initial position error of each robot end is e p :e p =P desire -P actual .

[0117] Based on the initial position error of the end of each robotic arm, the actual joint angle of each robotic arm is adjusted in real time, and the joint update radian data of each robotic arm after real-time adjustment is obtained, including:

[0118] Performing spatial angle mapping on the initial position error of the end of the robotic arm and associating it with the angle change of each joint of the robotic arm, and calculating the adjustment amount of each actual joint angle;

[0119] According to the adjustment amount of each actual joint angle, each actual joint angle of the robot arm is adjusted in real time.

[0120] The formula for calculating the adjustment amount of each actual joint angle includes:

[0121]

[0122] J + =(J T J) -1 J T ;

[0123] Δθ i =J + e p ;

[0124] Among them, Δθ i is the actual joint angle adjustment; e p The initial position error of each robot end; J is the Jacobian matrix; J + is the pseudo-inverse of the Jacobian matrix; α1_actual …α 6_actual are the actual positions of the six joints of the robotic arm.

[0125] The expected end position is the expected input target position point, then the position error can be expressed as: e p =P desire -P actual , get the position error e p Then the pseudo-inverse Jacobian matrix is ​​calculated.

[0126] In this application, the main purpose of calculating the pseudo-inverse Jacobian matrix is ​​to convert the calculated end position error e p The spatial angle mapping is linked to the angle changes of each joint to achieve real-time adjustment of the joint angles.

[0127] refer to Figure 8 The single-arm error elimination flowchart shown in the figure first sets the desired position P desire , and use this as input to get the expected position data, obtain the current joint initial arc data frame through the CAN communication protocol, convert this joint initial arc data frame into arc data, and use the forward kinematics solution to calculate the actual initial position P of the end of the robot arm actual , and further calculate the error between the initial actual position and the expected position of the end, map the error into space, get the error value of each joint angle, add the joint angle error value to the actual joint angle, and finally use positive kinematics planning to make the robot get the expected position to eliminate the position error of the robot.

[0128] According to the updated arc data of the joints of the mechanical arms and the desired synchronization positions of the ends of the mechanical arms, the actual angles of the joints of the mechanical arms are adjusted, including:

[0129] According to the updated arc data of the joints of the robotic arms and the expected synchronous positions of the ends of the robotic arms, the relative position errors of the ends of the robotic arms are obtained, and the formula is as follows:

[0130] E sync =|(P desire_l -P actual_l )-(P desire_r -P actual_r )|;

[0131] Among them, E sync is the relative position error of the end of the robotic arm.

[0132] In order to more intuitively display the positional relationship between the two mechanical arms of the robot in space, the error relationship between the two is inferred. l , Y l, Z l ) and (X r , Y r , Z r ) are the actual positions of the left and right arms, (X desire , Y desire , Z desire ) is the expected position that the robot arm needs to reach. In order to eliminate the synchronous position error of both arms, the position error of the single arm must be eliminated first.

[0133] Left arm error: e pl =P desire_l -P actual_l ;

[0134] Right arm error: e pr =P desire_r -P actual_r ;

[0135] To solve the relationship between position error and joint adjustment, the pseudo-inverse of the Jacobian matrix J is selected. + To solve the linear equations and update the actual joint angles.

[0136] In the present application, solving the joint angles through the pseudo-inverse of the Jacobian matrix can avoid the problem of being unable to calculate or having low calculation efficiency due to singular joint angles.

[0137] Through the minimum norm solution, the minimum angle change is selected as the optimal value, and then the adjustment amount Δθ of each joint angle is calculated. i The specific formula is as follows:

[0138] J + =(J T J) -1 J T ;

[0139] Δθ i =J + e p ;

[0140] Among them, e p It can refer to the error between the left arm and the right arm, and the calculated adjustment amount Δθ i As the joint compensation angle, the angle deviation of the joint can be updated: θ i_new =θ i_actual +Δθ i After error compensation, θ i_new After conversion to radians, the updated joint angle values ​​are sent for target planning through angle mapping to eliminate the errors of both arms. In this application, the formula for angle mapping is:

[0141] After the single-arm error is calculated, when the two robotic arms perform tasks that require the synchronous movement of both arms, such as handling tasks, the control of both arms may be out of sync due to control delay. In addition, the movements of the two robotic arms may affect or interfere with each other. Therefore, it is necessary to perform synchronous control after reaching their respective expected positions to ensure a safe distance between the two arms. If the synchronization error is not controlled, it may lead to inaccurate task execution or even damage to the robotic arms or the working environment, for example:

[0142] 1. Carrying heavy objects: When you need to use both arms to carry heavy objects, such as transporting large boxes or equipment, the two arms need to operate synchronously to balance the center of gravity of the object and prevent it from tipping or falling.

[0143] 2. Clamping objects: When clamping objects, both arms need to move synchronously to ensure that the clamping force is evenly distributed to avoid damage to the object.

[0144] In order to solve the problems of asynchrony and mutual influence, the concept of synchronous position error is introduced in this application, and this application proposes to compensate for the synchronous position error of the two robotic arms.

[0145] The following is an example of a handling task: The above scenarios are all scenarios of dual-arm collaborative control. Fig.10 The error analysis plane diagram of the dual-arm coordinated control is shown. In order to solve the above problem, the present application plans two coordinate points for the left arm and the right arm respectively when performing the handling task. These two coordinate points are the positions that the dual arms are expected to reach, that is, the expected positions. After the dual arms are self-adjusted, two actual position points are planned as P actual_l and P actual_r At this time, the scene requires simultaneous operation of both arms, and the expected position point is P desire_l and P desire_r , and then the synchronous position error should be dynamically monitored.

[0146] in, is the absolute error of the left arm, P desire_r -R sctual_r is the absolute error of the right arm, then the relative position error of the two arms is E sync for:

[0147] E sync =|(P desire_l -P actual_l )-(P desire_r -P actual_r )|;

[0148] The synchronization position error is the synchronization deviation of the two arms on the desired trajectory, and the control goal of the present application is to minimize the synchronization position error. According to the synchronization position error, the control system of the robot arm can be compensated to make the two robot arms more synchronized.

[0149] In order to make the robot arm synchronous control more flexible, refer to Figure 3 The dual-arm synchronization control flow chart shown in the present application also proposes an adaptive PID control method, wherein, according to the joint update arc data of the robotic arm and the desired synchronization position of the end of each robotic arm, the relative position error of the end of each robotic arm is adjusted by adaptive PID control, including:

[0150] The adaptive PID control formula is as follows:

[0151]

[0152] Among them, u (t) k is the control amount of the relative position error of the end of the robot arm; p (t), K i (t), K d (t) is the PID parameter.

[0153] In this application, u (t) is the double-arm error value control quantity, through which the radian is compensated. In the synchronous control of the double arms, the output u (t) The compensation amount is usually the control input acting on each joint. This means that the compensation result of the synchronization error needs to be distributed to each joint of the left arm and the right arm. τ is the integral independent variable, which represents the error accumulation from time 0 to t.

[0154] Define the cost function J c Used to optimize PID parameters, to avoid large fluctuations and improve the efficiency of gradient descent. In this application, the gradient descent method is used to adjust the PID control parameters.

[0155] Among them, the cost function is defined as the quadratic cost function. The weighted sum of the square of the error is:

[0156]

[0157] Where N is the number of iterations, E sync,i is the synchronization position error of the i-th sample, is the defined synchronization position error function.

[0158] Cost function for K p The partial derivative of is:

[0159]

[0160] Cost function for K i The partial derivative of is:

[0161]

[0162] Cost function for K d The partial derivative of is:

[0163]

[0164] In this application, PID parameters are optimized through cost function, which is a method for measuring system performance and guiding parameter optimization, to guide adaptive parameter optimization.

[0165] After obtaining the cost function, p , K i ,K d The parameters are adaptively iterated until the optimal value is found:

[0166] Update proportional gain K p :

[0167] Update proportional gain K i :

[0168] Update proportional gain K d :

[0169] Where α is the learning rate, which controls the update step size. are the partial derivatives of the cost function with respect to the proportional gain, integral gain, and differential gain, respectively.

[0170] Through continuous iteration, we can accurately (t) The compensation amount is input into the joint. (t) Distribute to each joint. When distributing, set different weights according to the influence of the joint on the position of the end effector. The specific formula is as follows:

[0171] Δθ i =ω i u (t) ;

[0172] Among them, ω i is the joint weight.

[0173] Update the angles of each joint, and the updated joint angles are: θ l_new =Δθ i +θ l_actual θ r_new =Δθ i +θ r_actual .

[0174] After the joint angles are updated, the forward kinematics of the two arms are solved to obtain the end position of the robot arm. The error value is continuously iterated to make it reach the desired end synchronization position and realize synchronous control.

[0175] The present invention designs a group of comparative experiments for the above method, which are respectively the comparison before and after adding the adaptive PID control.

[0176] The arc path is selected as the experimental path, and the time parameter t is given as a linear interval from 0 to π, N = 100, during which a total of 100 sampling points are recorded.

[0177] Initialize PID parameters to K p =0.4, K i =0.05, K d =0.15.

[0178] The random error introduced in the uncontrolled situation, with the standard deviation set to 0.02, simulates the synchronization error of both arms under fixed parameters.

[0179] In the present application, the above PID parameters are updated using the gradient descent method, and the learning rate (step size) is set to α=0.0002.

[0180] refer to Figure 4 The figure shows the trajectory tracking diagram of both arms before optimization (Path Tracking Without Control), where the black dotted line is the planned path (Desired Path), the red line is the trajectory of the left arm before optimization (Left Arm(NoControl)), and the blue dotted line is the trajectory of the right arm before optimization (Right Arm(No Control)). It can be seen that there are errors in the trajectory tracking and planned path of both arms before optimization.

[0181] In order to more intuitively describe the error compensation effect of the present application on the dual-arm synchronous control, the present application gives Figure 5 The optimized double-arm trajectory tracking diagram is shown, and Figure 6 The relative error comparison chart before and after the dual-arm optimization is shown.

[0182] refer to Figure 5 , where the black dotted line is the planned path (Desired Path), the red line is the trajectory of the left arm after optimization Left Arm (With adapt PID Control), and the blue dotted line is the trajectory of the right arm after optimization Right Arm (With adapt PID Control). Figure 5 It can be seen that the trajectory tracking method based on adaptive PID control of the present application can make the error between the dual-arm trajectory tracking and the planned path extremely small.

[0183] refer to Figure 6 The relative error comparison diagram before and after the dual-arm optimization (Synchronization Position Error Comparison) is shown in the figure. The horizontal axis is time, and the vertical axis is synchronization error (Synchronization Error). The red solid line represents the dual-arm synchronization position error without PID control (Without Control), and the blue solid line represents the dual-arm synchronization position error under adaptive PID control. Figure 6 It can be seen that the adaptive PID control method of the present application can significantly reduce the synchronous position error of the two arms.

[0184] pass Figure 4 , Figure 5 , Figure 6 It can be seen from the simulation results shown that the control accuracy of the dual arms is improved after the position control method of the present application is added, and the path tracking effect of the robotic arm is more obvious.

[0185] This application controls the robot arm to reach the desired position by setting the desired end position, and then obtains the actual joint angle information through the joint angle encoder through CAN bus communication. p , K i ,K d The three parameters are adaptively controlled to adjust the dual-arm synchronization error to achieve precise control. The present invention uses the gradient descent method to adjust the PID parameters online in real time, and achieves the control effect by continuously updating the gain value.

[0186] The adaptive PID control method of the present application simplifies the poor adaptability of manual parameter adjustment by automatically adjusting the gain parameters, reduces the dependence on the professional skills of the operator, and makes the deployment and application of the system more convenient.

[0187] Those skilled in the art can clearly understand that, for the convenience and simplicity of description, only the division of the above-mentioned program modules is used as an example for illustration. In actual applications, the above-mentioned functions can be assigned to different program modules as needed, that is, the internal structure of the device can be divided into different program units or modules to complete all or part of the functions described above. The program modules in the embodiment can be integrated into a processing unit, or each unit can exist physically separately, or two or more units can be integrated into a processing unit, and the above-mentioned integrated unit can be implemented in the form of hardware or in the form of software program units. In addition, the specific names of the program modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application.

[0188] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0189] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented with electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed with hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0190] It should be noted that the above embodiments can be freely combined as needed. The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered as the protection scope of the present invention.

Claims

1. A control method for a service robot, characterized in that: include: Acquire the initial radian data of the joints of each robotic arm in actual operation, and calculate the initial actual position of the end of each robotic arm based on the initial radian data of the joints; According to the initial expected position of the end of each robot arm and the initial actual position of the end of each robot arm, the initial position error of the end of each robot arm is calculated; Based on the initial position error of the end of each robotic arm, the actual joint angle of each robotic arm is adjusted in real time, and the updated radian data of the joints of each robotic arm after the real-time adjustment is obtained; According to the updated arc data of the joints of the robotic arms and the desired synchronous positions of the ends of the robotic arms, the relative position errors of the ends of the robotic arms are adjusted through adaptive PID control.

2. A control method for a service robot according to claim 1, characterized in that: The step of calculating the actual initial position of the end of each robotic arm based on the initial arc data of the joint includes: According to the initial arc data of the joints of each robot arm in actual operation, the actual joint angle of each robot arm is calculated; The actual transformation matrix of the end of each robotic arm is obtained according to the actual joint angle of each robotic arm, and the initial actual position of the end of each robotic arm is calculated based on the actual transformation matrix of the end of each robotic arm.

3. A control method for a service robot according to claim 2, characterized in that: The actual joint angles of each robotic arm are calculated based on the initial arc data of the joints of each robotic arm in actual operation, including: The joint initial radian data is read, and converted into the actual joint angle of each robotic arm through an angle calculation formula. The joint initial radian data is the hexadecimal representation data of the rotation radian of each joint.

4. A control method for a service robot according to claim 3, characterized in that: The actual transformation matrix of the end of each robotic arm is obtained according to the actual joint angle of each robotic arm, and the initial actual position of the end of each robotic arm is calculated based on the actual transformation matrix of the end of each robotic arm, including: The actual transformation matrix of the end of the manipulator is obtained by forward kinematics solution, and the initial actual position of the end of each manipulator is calculated. The formula is as follows: Among them, P actual is the initial actual position of the end of the robot arm, T actual is the actual transformation matrix of the end of the robot, T actual [1:3,4] represents the data of the 4th column and the 1st to the 3rd row of the actual transformation matrix of the end of the robotic arm.

5. The control method of a service robot according to claim 3, characterized in that: The step of calculating the initial position error of the end of each robotic arm according to the initial expected position of the end of each robotic arm and the initial actual position of the end of each robotic arm comprises: The initial desired position of the end of the robot arm is P desire , The initial position error of each robot end is e p :e p =P desire -P actual .

6. A control method for a service robot according to claim 5, characterized in that: The actual joint angles of each robotic arm are adjusted in real time based on the initial position errors of the ends of each robotic arm, and the updated radian data of the joints of each robotic arm after the real-time adjustment are obtained, including: Performing spatial angle mapping on the initial position error of the end of the robotic arm and associating it with the angle change of each joint of the robotic arm, and calculating the adjustment amount of each actual joint angle; According to the adjustment amount of each actual joint angle, each actual joint angle of the robot arm is adjusted in real time.

7. A control method for a service robot according to claim 6, characterized in that: The formula for calculating the adjustment amount of each actual joint angle includes: Dth i =J + e p ; I + =(J T J) -1 I T ; Among them, Δθ i is the actual joint angle adjustment; e p The initial position error of each robot end; J is the Jacobian matrix; J + is the pseudo-inverse of the Jacobian matrix; α 1_actual …α 6_actual are the six joints of the robotic arm.

8. A control method for a service robot according to claim 7, characterized in that: The method of updating the arc data of the joints of the mechanical arms and synchronizing the desired positions of the ends of the mechanical arms and adjusting the actual angles of the joints of the mechanical arms comprises: According to the updated arc data of the joints of the robotic arms and the expected synchronous positions of the ends of the robotic arms, the relative position errors of the ends of the robotic arms are obtained, and the formula is as follows: E sync =|(P desire_l -P actual_l )-(P desire_r -P actual_r )|; Among them, E sync is the relative position error of the end of the robotic arm.

9. A control method for a service robot according to claim 8, characterized in that: The method of updating the arc data of the joints of the robotic arms and synchronizing the desired positions of the ends of the robotic arms and adjusting the relative position errors of the ends of the robotic arms by adaptive PID control comprises: The adaptive PID control formula is as follows: Among them, u (t) k is the control amount of the relative position error of the end of the robot arm; p (t), K i (t), K d (t) is the PID parameter.

10. A service robot, characterized in that: The service robot is provided with a plurality of mechanical arms, each of which is provided with 6 joints, and further comprises: The joint encoder is used to obtain the initial radian data of the joints of each robot arm in actual operation, and send the initial radian data of the joints to the central control unit; The central control unit is used to transmit the initial arc data of the joint to the PID controller; The PID controller is used to calculate the initial position error of the end of each robotic arm according to the initial expected position of the end of each robotic arm and the initial actual position of the end of each robotic arm; The PID controller is further used to adjust the actual joint angle of each robotic arm in real time based on the initial position error of the end of each robotic arm, and obtain the joint update radian data of each robotic arm after real-time adjustment; The PID controller is also used to update the arc data according to the joints of the robotic arms and synchronize the desired positions of the ends of the respective robotic arms, and to adjust the relative position errors of the ends of the respective robotic arms through adaptive PID control.

Images