Humanoid robot control method and system based on hollow joint coordination

Abstract

A human-shaped robot control method based on hollow joint coordination, comprising the following steps: obtaining a robot upper limb rotation instruction, and analyzing the upper limb rotation instruction to obtain a first target angle of a robot shoulder joint and a second target angle of an elbow joint; determining whether the first target angle is greater than 90°, if less than 90°, directly driving the shoulder joint to rotate to the first target angle, and then driving the elbow joint to rotate to the second target angle; if greater than 90°, determining whether the weight of an object gripped by the robot hand is greater than a threshold weight, if not greater than the threshold weight, driving the shoulder joint to rotate to the first target angle, and then driving the elbow joint to rotate to the second target angle; effectively reducing the stress of the shoulder joint, avoiding joint damage caused by excessive torque, and prolonging the service life of the hollow joint.

Description

Technical Field

[0001] This invention relates to the field of humanoid robot hand control technology, and in particular to a humanoid robot control method and system based on hollow joint coordination. Background Technology

[0002] Humanoid robots are equipped with upper limb joints and leg joints, enabling them to mimic humans to perform simple tasks to a certain extent.

[0003] The upper limb joints of a humanoid robot include the shoulder, elbow, and hand joints. When the robot's hand is fully extended and holding a heavy object, rotating it to a horizontal or higher level results in a significant bending moment on the shoulder joint. However, due to the structure of the hollow joint, the output gear has a long gear shaft while the gear disc meshing with the input gear is short, resulting in a lower bending moment capacity. This low bending moment stiffness of the hollow output gear during operation leads to poor bending moment resistance of the robot joint, making the robot's shoulder joint susceptible to damage. Summary of the Invention

[0004] To address the aforementioned shortcomings, the present invention aims to propose a humanoid robot control method and system based on hollow joint coordination, in order to solve the problem that hollow joints of humanoid robots are prone to damage during rotation.

[0005] To achieve this objective, the present invention adopts the following technical solution: a humanoid robot control method based on hollow joint coordination, comprising the following steps; Obtain the upper limb rotation command of the robot, and parse the upper limb rotation command to obtain the first target angle of the robot's shoulder joint and the second target angle of the elbow joint; Determine whether the first target angle is greater than 90°. If it is less than 90°, directly drive the shoulder joint to rotate to the first target angle, and then drive the elbow joint to rotate to the second target angle. If the angle is greater than 90°, it is determined whether the weight of the object grasped by the robot hand is greater than the threshold weight. If it is not greater than the threshold weight, the shoulder joint is driven to rotate to the first target angle, and the elbow joint is driven to rotate to the second target angle. If the angle is greater than 90°, the elbow joint is driven to rotate so that the angle between the humanoid robot's forearm and upper arm is less than 90°. Then, the shoulder joint is driven to rotate to the first target angle, and the elbow joint is driven to rotate to the second target angle.

[0006] Preferably, the power input of the shoulder joint is connected to a permanent magnet motor.

[0007] Preferably, when the shoulder joint is between 0° and 90°, the permanent magnet motor performs the following control steps: Obtain the current angle of the shoulder joint as the third target angle; Determine if the angle of the first target is greater than 90°. If it is, obtain the angle difference between the angle of the third target and 90°. If it is less than 90°, obtain the angle difference between the angle of the first target and the angle of the third target. Determine whether the angle difference is greater than the angle threshold. If it is less than the threshold, use the MTPA current control method to generate the corresponding d-axis and q-axis currents. Input the d-axis and q-axis currents into the controller to generate the first control command. Drive the permanent magnet motor to rotate the shoulder joint to the first target angle by generating the first control command. If it is greater than that, the MTPA current control method is used to generate the corresponding d-axis and q-axis currents. The d-axis and q-axis are input to the controller to generate the second control command. The second control command drives the permanent magnet motor to rotate the shoulder joint to the third target angle plus the angle threshold. The d-axis current is obtained based on the motor's operating parameters, design parameters, and speed threshold. The q-axis current limit is obtained based on the maximum current and the d-axis current. The d-axis current and the q-axis current limit are input into the controller to generate a third control command. Based on the third control command, the permanent magnet motor is driven to rotate the shoulder joint to the first target angle or to 90°.

[0008] Preferably, when the shoulder joint is at 91°~180°, the permanent magnet motor performs the following control steps: The average voltage of the permanent magnet motor is changed by controlling the pulse width of the power supply to the permanent magnet motor through the PWM speed regulation method, thereby controlling the speed of the permanent magnet motor to maintain the first speed and generating a fourth control command. According to the fourth control command, the permanent magnet motor is driven to rotate the shoulder joint to the first target angle.

[0009] Preferably, the specific steps for obtaining the d-axis current based on the motor's operating parameters, design parameters, and speed threshold are as follows: Real-time acquisition of the given current along the q-axis , give current The input is fed into a low-pass filter to obtain the first parameter. ; Real-time acquisition of q-axis feedback current , give current The input is fed into a low-pass filter to obtain the second parameter. ; Get the first parameter With the second parameter The difference E is used as the input of the PI controller to obtain the current feedback setpoint D1; According to the rated current and rated speed Calculate the amplification factor Kc of the feedforward term of the field weakening current; Obtain the current rotational speed increment of the field weakening, and adjust the rotational speed increment through the amplification factor Kc to obtain the current feedback setpoint D2; The d-axis current is obtained by adding the feedback reference D1 and the feedforward reference D2. .

[0010] Preferably, the formula for obtaining the limiting value of the q-axis current is as follows: ; in For maximum output current, The current is along the d-axis.

[0011] Preferably, the formula for obtaining the current feedback given term D1 is as follows: ; Where t is time, and These are the proportional gain coefficient and integral gain coefficient in the PI controller, respectively. The formula for obtaining the magnification factor Kc is as follows: , in For rated current, Rated speed; The specific formula for obtaining the feedforward given term D2 is as follows: ;in For the target speed, The turning speed is the rotational speed.

[0012] A humanoid robot control system based on hollow joint coordination, using the aforementioned humanoid robot control method based on hollow joint coordination, includes a receiving module, a parsing module, a power module, and an analysis module; The receiving module is used to receive the upper limb rotation command issued by the user and send the upper limb rotation command to the parsing module; The parsing module is used to parse the upper limb rotation command, obtain the first target angle of the robot's shoulder joint and the second target angle of the elbow joint, and send the first target angle and the second target angle to the analysis module; The analysis module is used to determine whether the first target angle is greater than 90°. If it is less than 90°, the first power output command is directly sent to the power module. If the angle is greater than 90°, it is determined whether the weight of the object grasped by the robot hand is greater than the threshold weight. If it is not greater than the threshold weight, the first power output command is sent to the power module. If the weight exceeds the threshold, a second power output command is sent to the power module. The power module is used to receive a first power output command or a second power output command. When the first power output command is received, the shoulder joint is driven to rotate to a first target angle, and the elbow joint is driven to rotate to a second target angle. When the second power output command is received, the elbow joint is driven to rotate so that the angle between the humanoid robot's forearm and upper arm is less than 90°. Then, the shoulder joint is driven to rotate to the first target angle, and the elbow joint is driven to rotate to the second target angle.

[0013] Preferably, the power module includes a first angle control submodule and a second angle control submodule; The first angle control submodule is used to control the shoulder joint when it is between 0° and 90°, and the control method is as follows: Obtain the current angle of the shoulder joint as the third target angle; Determine if the angle of the first target is greater than 90°. If it is, obtain the angle difference between the angle of the third target and 90°. If it is less than 90°, obtain the angle difference between the angle of the first target and the angle of the third target. Determine whether the angle difference is greater than the angle threshold. If it is less than the threshold, use the MTPA current control method to generate the corresponding d-axis and q-axis currents. Input the d-axis and q-axis currents into the controller to generate the first control command. Drive the permanent magnet motor to rotate the shoulder joint to the first target angle by generating the first control command. If it is greater than that, the MTPA current control method is used to generate the corresponding d-axis and q-axis currents. The d-axis and q-axis are input to the controller to generate the second control command. The second control command drives the permanent magnet motor to rotate the shoulder joint to the third target angle plus the angle threshold. The d-axis current is obtained based on the motor's operating parameters, design parameters, and speed threshold. The q-axis current limit value is obtained based on the maximum current and the d-axis current. The d-axis current and the q-axis current limit value are input into the controller to generate a third control command. Based on the third control command, the permanent magnet motor is driven to rotate the shoulder joint to the first target angle or to rotate the shoulder joint to 90°. The second angle control submodule is used to control the shoulder joint when it is between 91° and 180°, and the control method is as follows: The average voltage of the permanent magnet motor is changed by controlling the pulse width of the power supply to the permanent magnet motor through the PWM speed regulation method, thereby controlling the speed of the permanent magnet motor to maintain the first speed and generating a fourth control command. According to the fourth control command, the permanent magnet motor is driven to rotate the shoulder joint to the first target angle.

[0014] One of the above technical solutions has the following advantages or beneficial effects: This invention specifically considers the situation where the shoulder joint rotation angle is greater than 90° and introduces a judgment of the weight of the grasped object. When a large-angle rotation is required and the grasped weight is large, the elbow joint angle is first adjusted so that the angle between the forearm and upper arm is less than 90°. This can effectively reduce the force on the shoulder joint, avoid joint damage caused by excessive torque, and thus extend the service life of the hollow joint. Attached Figure Description

[0015] Figure 1 This is a flowchart of one embodiment of the method of the present invention.

[0016] Figure 2 This is a schematic diagram of the structure of one embodiment of the system of the present invention.

[0017] Figure 3 This is a schematic diagram of the structure of a humanoid robot after the upper limb completes the upper limb rotation command in one embodiment of the present invention. Detailed Implementation

[0018] Embodiments of the present invention are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0019] In the description of embodiments of the present invention, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the stated features. In the description of embodiments of the present invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0020] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this invention, unless otherwise stated, "a plurality of" means two or more. Those skilled in the art will understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0021] like Figures 1-3 As shown, a humanoid robot control method based on hollow joint coordination includes the following steps; Obtain the upper limb rotation command of the robot, and parse the upper limb rotation command to obtain the first target angle of the robot's shoulder joint and the second target angle of the elbow joint; Determine whether the first target angle is greater than 90°. If it is less than 90°, directly drive the shoulder joint to rotate to the first target angle, and then drive the elbow joint to rotate to the second target angle. If the angle is greater than 90°, it is determined whether the weight of the object grasped by the robot hand is greater than the threshold weight. If it is not greater than the threshold weight, the shoulder joint is driven to rotate to the first target angle, and the elbow joint is driven to rotate to the second target angle. If the weight exceeds the threshold, the elbow joint is driven to rotate so that the angle between the humanoid robot's forearm and upper arm is less than 90°. Then, the shoulder joint is driven to rotate to the first target angle, and the elbow joint is driven to rotate to the second target angle.

[0022] To protect the shoulder joint of the humanoid robot, this invention analyzes the upper limb rotation command to obtain a first target angle and a second target angle for the elbow joint, wherein, for example... Figure 3 The first target angle shown is the angle between the robot arm and the vertical line, that is... Figure 3 In the context of angle A, the second target angle is the angle between the robot's upper arm and forearm, i.e. Figure 3 The included angle B in the middle can be used to control the position of the robot's hand, thereby enabling the robot to grasp and place objects. When the first target angle is greater than 90 degrees, it indicates that the shoulder joint needs to rotate beyond the horizontal. At this point, the bending moment on the shoulder joint is very large. If the robot hand is holding a heavy object, the stress on the shoulder joint may increase. Since hollow joints are typically used as the connecting joints in the shoulder joint, the output gear shaft of the hollow joint can withstand a lower bending moment. When the shoulder joint rotates above 90 degrees, the stress at this point may exceed the material strength of the output gear shaft, potentially causing it to break. Therefore, when the first target angle is greater than 90 degrees, it is necessary to determine whether the robot hand is holding an object. If it is, it is necessary to determine whether the weight of the object exceeds a weight threshold. If it does not exceed the weight threshold, the material strength of the output gear shaft is still sufficient to meet the rotation requirements. Therefore, the shoulder joint can be directly driven to rotate to the first target angle, and then the elbow joint can be driven to rotate to the second target angle, completing the rotation of the robot's upper limb. When the weight of the object grasped by the robot hand exceeds a threshold weight, the stress on the output gear shaft needs to be considered. Therefore, the elbow joint needs to be driven first to reduce the angle between the forearm and upper arm to less than 90°. When this angle is less than 90°, the torque on the shoulder joint decreases, and the bending moment it experiences also decreases, preventing the output gear shaft from breaking due to excessive bending moment. Then, the shoulder joint is driven to rotate to the first target angle, and the elbow joint is driven to rotate to the second target angle. It's worth noting that after the elbow joint rotates to the second target angle, the angle between the forearm and upper arm may exceed 90°, increasing the bending moment on the shoulder joint. Therefore, someone needs to quickly remove the object from the robot hand.

[0023] This invention specifically considers situations where the shoulder joint rotation angle is greater than 90° and incorporates a judgment of the weight of the object being grasped. When a large-angle rotation is required and a heavy object is being grasped, the elbow joint angle is first adjusted so that the angle between the forearm and upper arm is less than 90°. This effectively reduces the stress on the shoulder joint, avoids joint damage caused by excessive torque, and thus extends the service life of the hollow joint.

[0024] Preferably, the power input of the shoulder joint is connected to a permanent magnet motor.

[0025] Preferably, when the shoulder joint is between 0° and 90°, the permanent magnet motor performs the following control steps: Obtain the current angle of the shoulder joint as the third target angle; Determine if the angle of the first target is greater than 90°. If it is, obtain the angle difference between the angle of the third target and 90°. If it is less than 90°, obtain the angle difference between the angle of the first target and the angle of the third target. Determine whether the angle difference is greater than the angle threshold. If it is less than the threshold, use the MTPA current control method to generate the corresponding d-axis and q-axis currents. Input the d-axis and q-axis currents into the controller to generate the first control command. Drive the permanent magnet motor to rotate the shoulder joint to the first target angle by generating the first control command. When the shoulder joint rotates between 0° and 90°, the bending moment is relatively small. Therefore, the permanent magnet motor can be driven to rotate rapidly, allowing the shoulder joint to quickly reach the first target angle. During this process, the angle difference between the third target angle (the current shoulder joint angle) and the first target angle is first determined. If the angle difference is less than a threshold, the permanent magnet motor is driven to rotate at maximum speed. When the shoulder joint reaches the first target angle, the permanent magnet motor's speed has not yet entered the field weakening control stage. Therefore, when the angle difference is less than the threshold, only the MTPA current control method is needed to generate the corresponding d-axis and q-axis currents to produce a stable current that drives the permanent magnet motor to rotate the shoulder joint. At this time, the permanent magnet motor can provide a stable speed, allowing the shoulder joint to rotate stably.

[0026] If it is greater than that, the MTPA current control method is used to generate the corresponding d-axis and q-axis currents. The d-axis and q-axis are input to the controller to generate the second control command. The second control command drives the permanent magnet motor to rotate the shoulder joint to the third target angle plus the angle threshold. The d-axis current is obtained based on the motor's operating parameters, design parameters, and speed threshold. The q-axis current limit is obtained based on the maximum current and the d-axis current. The d-axis current and the q-axis current limit are input into the controller to generate a third control command. Based on the third control command, the permanent magnet motor is driven to rotate the shoulder joint to the first target angle or to 90°.

[0027] When the angle difference exceeds the angle threshold, the permanent magnet motor will reach a turning speed after accelerating for a period of time, thus initiating a field weakening control phase. During this phase, the magnitude of the q-axis current needs to be limited to prevent excessive q-axis current from causing the permanent magnet motor to rotate too fast, which would affect the smooth rotation of the shoulder joint. Effective control of the d-axis and q-axis currents can stably increase the speed of the permanent magnet motor, allowing the shoulder joint to quickly and smoothly reach the first target angle or up to 90°.

[0028] Preferably, when the shoulder joint is at 91°~180°, the permanent magnet motor performs the following control steps: The average voltage of the permanent magnet motor is changed by controlling the pulse width of the power supply to the permanent magnet motor through the PWM speed regulation method, thereby controlling the speed of the permanent magnet motor to maintain the first speed and generating a fourth control command. According to the fourth control command, the permanent magnet motor is driven to rotate the shoulder joint to the first target angle.

[0029] When the shoulder joint rotates between 91° and 180°, it experiences a significant bending moment. If a variable speed control method is used, the force on the output gear shaft of the shoulder joint will change with the rotational speed. Changes in rotational speed may lead to changes in the force transmitted by the gears, thus affecting the bending moment distribution on the shaft and potentially causing the output gear shaft to break. Therefore, when the shoulder joint is between 91° and 180°, the pulse width of the permanent magnet motor's power supply is controlled using a PWM speed regulation method to change the average voltage of the permanent magnet motor. By outputting a fixed voltage, the permanent magnet motor is kept rotating at a constant initial speed, ensuring the force on the output gear shaft during rotation.

[0030] Preferably, the specific steps for obtaining the d-axis current based on the motor's operating parameters, design parameters, and speed threshold are as follows: Real-time acquisition of the given current along the q-axis , give current The input is fed into a low-pass filter to obtain the first parameter. ; Real-time acquisition of q-axis feedback current , give current The input is fed into a low-pass filter to obtain the second parameter. ; By acquiring the given and feedback currents of the q-axis in real time and processing them using a low-pass filter, more stable and accurate current parameters can be obtained. This method helps to precisely control the motor's operation, improving stability and reliability.

[0031] Get the first parameter With the second parameter The difference E is used as the input of the PI controller to obtain the current feedback setpoint D1; By calculating the difference E between the given current and the feedback current and using it as the input to the PI controller, a feedback setpoint D1 for adjusting the current can be obtained. The PI controller can effectively reduce errors and improve the accuracy of current control.

[0032] According to the rated current and rated speed The amplification factor Kc of the feedforward term of the field weakening current is calculated. The amplification factor Kc is determined based on the rated current and rated speed of the motor. By adjusting the speed increment through the amplification factor Kc, the motor can be ensured to operate in a high-efficiency and stable state.

[0033] Obtain the current rotational speed increment of the field weakening, and adjust the rotational speed increment through the amplification factor Kc to obtain the current feedback setpoint D2; The d-axis current is obtained by adding the feedback reference D1 and the feedforward reference D2. .

[0034] In this invention, feedback control and feedforward control are combined. The d-axis current is obtained through simple addition, without relying on complex algorithms, and the d-axis current can be obtained accurately. The feedforward reference term D1 of the field weakening current can improve the speed response of the motor during rapid speed switching, while the feedback reference term of the field weakening current ensures the speed stability of the motor under heavy load. This field weakening control method has low computational load, simple structure, and high operational stability.

[0035] Preferably, the formula for obtaining the limiting value of the q-axis current is as follows: ; in For maximum output current, The current is along the d-axis.

[0036] Since the q-axis current is directly related to the electromagnetic torque, precise control of the q-axis current ensures the required torque output from the motor. However, excessively high q-axis current may cause the motor to overheat or be damaged. Therefore, by limiting the maximum value of the q-axis current command, the motor can be ensured to operate within a safe range while optimizing torque output. Excessively high q-axis current may cause large torque fluctuations during dynamic response, affecting the stable operation of the motor. Therefore, in this invention, the q-axis current is limited by the maximum output current and the d-axis current. The purpose is to weaken the motor's magnetic field, reduce back EMF when the power input voltage is limited, and thus increase the motor speed. Therefore, the switching of field weakening control is determined based on the motor's operating speed.

[0037] Preferably, the formula for obtaining the current feedback given term D1 is as follows: ; Where t is time, and These are the proportional gain coefficient and integral gain coefficient in the PI controller, respectively. The formula for obtaining the magnification factor Kc is as follows: , in For rated current, Rated speed; The specific formula for obtaining the feedforward given term D2 is as follows: ;in For the target speed, The turning speed is the rotational speed.

[0038] A humanoid robot control system based on hollow joint coordination, using the aforementioned humanoid robot control method based on hollow joint coordination, includes a receiving module, a parsing module, a power module, and an analysis module; The receiving module is used to receive the upper limb rotation command issued by the user and send the upper limb rotation command to the parsing module; The parsing module is used to parse the upper limb rotation command, obtain the first target angle of the robot's shoulder joint and the second target angle of the elbow joint, and send the first target angle and the second target angle to the analysis module; The analysis module is used to determine whether the first target angle is greater than 90°. If it is less than 90°, the first power output command is directly sent to the power module. If the angle is greater than 90°, it is determined whether the weight of the object grasped by the robot hand is greater than the threshold weight. If it is not greater than the threshold weight, the first power output command is sent to the power module. If the weight exceeds the threshold, a second power output command is sent to the power module. The power module is used to receive a first power output command or a second power output command. When the first power output command is received, the shoulder joint is driven to rotate to a first target angle, and the elbow joint is driven to rotate to a second target angle. When the second power output command is received, the elbow joint is driven to rotate so that the angle between the humanoid robot's forearm and upper arm is less than 90°. Then, the shoulder joint is driven to rotate to the first target angle, and the elbow joint is driven to rotate to the second target angle.

[0039] Preferably, the power module includes a first angle control submodule and a second angle control submodule; The first angle control submodule is used to control the shoulder joint when it is between 0° and 90°, and the control method is as follows: Obtain the current angle of the shoulder joint as the third target angle; Determine if the angle of the first target is greater than 90°. If it is, obtain the angle difference between the angle of the third target and 90°. If it is less than 90°, obtain the angle difference between the angle of the first target and the angle of the third target. Determine whether the angle difference is greater than the angle threshold. If it is less than the threshold, use the MTPA current control method to generate the corresponding d-axis and q-axis currents. Input the d-axis and q-axis currents into the controller to generate the first control command. Drive the permanent magnet motor to rotate the shoulder joint to the first target angle by generating the first control command. If it is greater than that, the MTPA current control method is used to generate the corresponding d-axis and q-axis currents. The d-axis and q-axis are input to the controller to generate the second control command. The second control command drives the permanent magnet motor to rotate the shoulder joint to the third target angle plus the angle threshold. The d-axis current is obtained based on the motor's operating parameters, design parameters, and speed threshold. The q-axis current limit value is obtained based on the maximum current and the d-axis current. The d-axis current and the q-axis current limit value are input into the controller to generate a third control command. Based on the third control command, the permanent magnet motor is driven to rotate the shoulder joint to the first target angle or to rotate the shoulder joint to 90°. The second angle control submodule is used to control the shoulder joint when it is between 91° and 180°, and the control method is as follows: The average voltage of the permanent magnet motor is changed by controlling the pulse width of the power supply to the permanent magnet motor through the PWM speed regulation method, thereby controlling the speed of the permanent magnet motor to maintain the first speed and generating a fourth control command. According to the fourth control command, the permanent magnet motor is driven to rotate the shoulder joint to the first target angle.

[0040] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "illustrative embodiment," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0041] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A humanoid robot control method based on hollow joint coordination, characterized in that, Includes the following steps; Obtain the upper limb rotation command of the robot and parse the upper limb rotation command to obtain the first target angle of the robot's shoulder joint and the second target angle of the elbow joint. The first target angle is the angle between the robot's upper arm and the vertical line, and the second target angle is the angle between the robot's upper arm and forearm. Determine whether the first target angle is greater than 90°. If it is less than 90°, directly drive the shoulder joint to rotate to the first target angle, and then drive the elbow joint to rotate to the second target angle. If the angle is greater than 90°, it is determined whether the weight of the object grasped by the robot hand is greater than the threshold weight. If it is not greater than the threshold weight, the shoulder joint is driven to rotate to the first target angle, and the elbow joint is driven to rotate to the second target angle. If the weight exceeds the threshold, the elbow joint is driven to rotate so that the angle between the humanoid robot's forearm and upper arm is less than 90°. Then the shoulder joint is driven to rotate to the first target angle, and the elbow joint is driven to rotate to the second target angle. The power input of the shoulder joint is connected to the permanent magnet motor for transmission. When the shoulder joint is between 0° and 90°, the permanent magnet motor performs the following control steps: Obtain the current angle of the shoulder joint as the third target angle; Determine if the angle of the first target is greater than 90°. If it is, obtain the angle difference between the angle of the third target and 90°. If it is less than 90°, obtain the angle difference between the angle of the first target and the angle of the third target. Determine whether the angle difference is greater than the angle threshold. If it is less than the threshold, use the MTPA current control method to generate the corresponding d-axis and q-axis currents. Input the d-axis and q-axis currents into the controller to generate the first control command. Drive the permanent magnet motor to rotate the shoulder joint to the first target angle by generating the first control command. If the current is greater than the target value, the MTPA current control method is used to generate the corresponding d-axis and q-axis currents. The d-axis and q-axis currents are input into the controller to generate a second control command. The second control command drives the permanent magnet motor to rotate the shoulder joint to the third target angle plus the angle threshold. The d-axis current is obtained based on the motor's operating parameters, design parameters, and speed threshold. The q-axis current limit value is obtained based on the maximum current and the d-axis current. The d-axis current and q-axis current limit values ​​are input into the controller to generate a third control command. The third control command drives the permanent magnet motor to rotate the shoulder joint to the first target angle. When the shoulder joint is between 91° and 180°, the permanent magnet motor performs the following control steps: The average voltage of the permanent magnet motor is changed by controlling the pulse width of the power supply to the permanent magnet motor through the PWM speed regulation method, thereby controlling the speed of the permanent magnet motor to maintain the first speed and generating a fourth control command. According to the fourth control command, the permanent magnet motor is driven to rotate the shoulder joint to the first target angle.

2. The humanoid robot control method based on hollow joint coordination according to claim 1, characterized in that, The specific steps to obtain the d-axis current based on the motor's operating parameters, design parameters, and speed threshold are as follows: Real-time acquisition of the given current along the q-axis , give current The input is fed into a low-pass filter to obtain the first parameter. ; Real-time acquisition of q-axis feedback current , feedback current The input is fed into a low-pass filter to obtain the second parameter. ; Get the first parameter With the second parameter The difference E is used as the input of the PI controller to obtain the current feedback setpoint D1; According to the rated current and rated speed Calculate the amplification factor Kc of the feedforward term of the field weakening current; Obtain the current rotational speed increment of the field weakening, and adjust the rotational speed increment through the amplification factor Kc to obtain the feedforward setpoint D2; The d-axis current is obtained by adding the feedback reference D1 and the feedforward reference D2. .

3. The humanoid robot control method based on hollow joint coordination according to claim 1, characterized in that, The formula for obtaining the q-axis current limit value is as follows: ； in For maximum output current, The current is along the d-axis.

4. The humanoid robot control method based on hollow joint coordination according to claim 2, characterized in that, The formula for obtaining the current feedback reference term D1 is as follows: ； Where t is time, and These are the proportional gain coefficient and integral gain coefficient in the PI controller, respectively. The formula for obtaining the magnification factor Kc is as follows: ， in For rated current, Rated speed; The specific formula for obtaining the feedforward given term D2 is as follows: ;in For the target speed, The turning speed is the rotational speed.

5. A humanoid robot control system based on hollow joint coordination, using the humanoid robot control method based on hollow joint coordination as described in any one of claims 1 to 4, characterized in that, It includes a receiving module, a parsing module, a power module, and an analysis module; The receiving module is used to receive the upper limb rotation command issued by the user and send the upper limb rotation command to the parsing module; The parsing module is used to parse the upper limb rotation command, obtain the first target angle of the robot's shoulder joint and the second target angle of the elbow joint, and send the first target angle and the second target angle to the analysis module; The analysis module is used to determine whether the first target angle is greater than 90°. If it is less than 90°, the first power output command is directly sent to the power module. If the angle is greater than 90°, it is determined whether the weight of the object grasped by the robot hand is greater than the threshold weight. If it is not greater than the threshold weight, the first power output command is sent to the power module. If the weight exceeds the threshold, a second power output command is sent to the power module. The power module is used to receive a first power output command or a second power output command. When the first power output command is received, the shoulder joint is driven to rotate to a first target angle, and the elbow joint is driven to rotate to a second target angle. When the second power output command is received, the elbow joint is driven to rotate so that the angle between the humanoid robot's forearm and upper arm is less than 90°. Then the shoulder joint is driven to rotate to the first target angle, and the elbow joint is driven to rotate to the second target angle. The power module includes a first angle control submodule and a second angle control submodule; The first angle control submodule is used to control the shoulder joint when it is between 0° and 90°, and the control method is as follows: Obtain the current angle of the shoulder joint as the third target angle; Determine if the angle of the first target is greater than 90°. If it is, obtain the angle difference between the angle of the third target and 90°. If it is less than 90°, obtain the angle difference between the angle of the first target and the angle of the third target. Determine whether the angle difference is greater than the angle threshold. If it is less than the threshold, use the MTPA current control method to generate the corresponding d-axis and q-axis currents. Input the d-axis and q-axis currents into the controller to generate the first control command. Drive the permanent magnet motor to rotate the shoulder joint to the first target angle by generating the first control command. If the current is greater than the target value, the MTPA current control method is used to generate the corresponding d-axis and q-axis currents. The d-axis and q-axis currents are input into the controller to generate the second control command. The second control command drives the permanent magnet motor to rotate the shoulder joint to the third target angle plus the angle threshold. The d-axis current is obtained based on the motor's operating parameters, design parameters, and speed threshold. The q-axis current limit is obtained based on the maximum current and the d-axis current. The d-axis current and the q-axis current limit are input into the controller to generate a third control command. The permanent magnet motor is driven to rotate the shoulder joint to the first target angle based on the third control command. The second angle control submodule is used to control the shoulder joint when it is between 91° and 180°, and the control method is as follows: The average voltage of the permanent magnet motor is changed by controlling the pulse width of the power supply to the permanent magnet motor through the PWM speed regulation method, thereby controlling the speed of the permanent magnet motor to maintain the first speed and generating a fourth control command. According to the fourth control command, the permanent magnet motor is driven to rotate the shoulder joint to the first target angle.

Images