Joint limiting control method and device, control equipment and storage medium
By generating limit torque commands and combining torque control commands, the robot joints are controlled to move within the limit in the torque control mode of the servo system, solving the problem of hardware limit failure and improving the safety and efficiency of robot motion.
Patent Information
- Application Number
- CN202311725001.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-12-14
- Publication Date
- 2025-06-17
AI Technical Summary
In the prior art, robot joint limit control mainly relies on hardware limits, which can easily lead to failure of limit protection when joint control errors and cause safety accidents.
By obtaining the current position and speed of the joint, as well as the preset limit, a limit torque command is generated, and combined with the torque control command, the joint is controlled to move within the limit in the torque control mode of the servo system.
It realizes that the joint moves within the limit in the torque control mode of the servo system, avoids the problem of hardware limit failure, and improves the safety and efficiency of robot movement.
Smart Images
Figure CN120155918A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of device control, and more specifically, to a control method, device, control equipment and storage medium for joint limit. Background Art
[0002] In devices such as medical robots, surgical robots, and collaborative robots, the torque control in the servo system is usually used to complete the dragging function of the joint, that is, the human hand drags the joint to move. In order to ensure that the robot does not exceed its reliable operation range during the movement, it is necessary to implement limit control on the joint, and the effective implementation of the robot joint limit control is crucial for improving the working efficiency, safety and system life of the robot.
[0003] In the related art, the control of the robot joint limit mostly relies on hardware limit. When a mistake occurs in the joint control, the control protection of the joint limit is likely to fail, resulting in serious safety accidents. Therefore, there is an urgent need to provide a control method for joint limit to enable the joint to move within the joint limit in the torque control mode of the servo system. Summary of the Invention
[0004] The embodiments of the present application provide a control method, device, control equipment and storage medium for joint limit, which can enable the joint to move within the joint limit in the torque control mode of the servo system.
[0005] In a first aspect, a control method for joint limit is provided; the control method includes:
[0006] Obtain the current position, current speed of the joint and the torque control instruction for controlling the movement of the joint;
[0007] Generate a limit torque instruction according to the current position, current speed of the joint and the preset limit;
[0008] Combine the torque control instruction and the limit torque instruction to control the joint to move within the limit.
[0009] The control method for joint limit provided by the present application first obtains the current position and current speed of the joint and the torque control instruction for the joint movement. Obtaining the current position and current speed of the joint is used to determine whether the joint exceeds the limit. Secondly, a limit torque instruction is generated according to the current position and current speed of the joint and the preset limit. Finally, the torque control instruction and the limit torque instruction are combined to control the joint to move within the limit, realizing that the joint moves within the joint limit in the torque control mode of the servo system.
[0010] In a possible implementation manner of the first aspect, generating a limit torque instruction according to the current position, current speed of the joint and the preset limit includes:
[0011] Determine the position where the joint acts on the limit according to the current position of the joint and the preset limit;
[0012] Compare the current position with the position where the joint acts on the limit to obtain the distance difference between the current position and the position of the acting limit;
[0013] Generate a limit torque command according to the current speed and the distance difference.
[0014] In a possible implementation manner of the first aspect, the preset limit includes a preset left limit and a preset right limit, and the joint acting limit includes a joint acting right limit and a joint acting left limit. When the current position is the initial position for entering the torque mode; determining the position where the joint acts on the limit according to the current position of the joint and the preset limit includes:
[0015] When the initial position exceeds the preset right limit, determine the position of the joint acting right limit as the initial position;
[0016] When the initial position is within the preset right limit, determine the position of the joint acting right limit as the position of the preset right limit;
[0017] When the initial position exceeds the preset left limit, determine the position of the joint acting left limit as the position of the preset left limit;
[0018] When the initial position is within the preset left limit, determine the position of the joint acting left limit as the initial position.
[0019] In a possible implementation manner of the first aspect, when the current position is the real-time position after entering the torque mode; determining the position where the joint acts on the limit according to the current position of the joint and the preset limit includes:
[0020] When the position of the joint acting right limit at the previous moment exceeds the preset right limit, determine the minimum value between the position of the joint acting right limit at the previous moment and the current position as the joint acting right limit at the current moment;
[0021] When the position of the joint acting right limit at the previous moment is within the preset right limit, determine the preset right limit as the joint acting right limit at the current moment;
[0022] When the position of the joint acting left limit at the previous moment is within the preset left limit, determine the maximum value between the position of the joint acting left limit at the previous moment and the current position as the joint acting left limit at the current moment;
[0023] When the position of the joint acting left limit at the previous moment exceeds the preset left limit, determine the preset left limit as the joint acting left limit at the current moment.
[0024] In a possible implementation of the first aspect, when the current position exceeds the joint action limit; according to the current speed and the distance difference, a limit torque command is generated, including:
[0025] Determine the damping of the speed outside the boundary according to the current speed and the damping coefficient of the speed outside the preset boundary;
[0026] Determine the boundary elastic force according to the distance difference and the preset boundary torque coefficient;
[0027] Generate a limit torque command according to the damping of the speed outside the boundary and the boundary elastic force.
[0028] In a possible implementation of the first aspect, combining the torque control command and the limit torque command to control the joint to move within the limit, including:
[0029] Perform an addition operation on the torque control command and the limit torque command to obtain the target torque;
[0030] Determine the torque deviation between the target torque and the current feedback torque;
[0031] Input the torque deviation into the torque controller;
[0032] The torque controller outputs a drive signal to control the joint to move within the limit according to the drive signal.
[0033] In a possible implementation of the first aspect, obtaining the current position and current speed of the joint, including:
[0034] Obtain the current position and current speed of the joint through an encoder connected to the power device of the joint.
[0035] In a possible implementation of the first aspect, a speed reducer is installed between the power device of the joint and the load; obtaining the current position and current speed of the joint further includes:
[0036] Obtain the current speed of the joint through an encoder connected to the power device of the joint;
[0037] Obtain the current position of the joint through an encoder connected to the speed reducer.
[0038] In a second aspect, an embodiment of the present application provides a control device for joint limit, and the control device includes:
[0039] An acquisition module, configured to acquire the current position, current speed of the joint, and a torque control command for controlling the movement of the joint;
[0040] A generation module, configured to generate a limit torque command according to the current position, current speed of the joint, and a preset limit;
[0041] A control module, configured to combine a torque control instruction and a limit torque instruction to control the movement of the joint within the limit.
[0042] In a third aspect, a control device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the method according to the first aspect above is implemented.
[0043] In a fourth aspect, a computer-readable storage medium is provided, storing a computer program, wherein when the computer program is executed by a processor, the method according to the first aspect above is implemented.
[0044] In a fifth aspect, a computer program product is provided, which when running on a control device causes the control device to execute the method according to the first aspect above.
[0045] It can be understood that the second aspect, the third aspect, the fourth aspect, the fifth aspect, and the sixth aspect provided above are all used to execute the corresponding methods provided above. Therefore, the beneficial effects that can be achieved can refer to the beneficial effects in the corresponding methods provided above, and will not be elaborated here. Description of the Drawings
[0046] Figure 1 is a schematic flowchart of a method for controlling joint limit provided in Embodiment 1 of the present application;
[0047] Figure 2 is a motion control drive block diagram based on a target torque;
[0048] Figure 3 is a schematic flowchart of another method for controlling joint limit provided in Embodiment 2 of the present application;
[0049] Figure 4 is a motion control drive block diagram corresponding to another method for controlling joint limit;
[0050] Figure 5 is a schematic structural diagram of a device for controlling joint limit provided in an embodiment of the present application;
[0051] Figure 6 is a schematic structural diagram of a control device provided in an embodiment of the present application. Detailed Embodiments
[0052] In the following description, specific details such as specific system architectures, technologies, etc. are presented for purposes of illustration and not limitation, so as to provide a thorough understanding of the embodiments of the present application. However, those skilled in the art should understand that the present application can 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 avoid unnecessary details from obscuring the description of the present application.
[0053] It should be understood that when used in the specification and claims of the present application, the term "comprising" indicates the presence of the described features, wholes, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, wholes, steps, operations, elements, components, and / or their combinations.
[0054] It should also be understood that the term "and / or" as used in the specification and claims of the present application refers to any combination and all possible combinations of one or more of the associated listed items, and includes these combinations.
[0055] As used in the specification and claims of the present application, the term "if" can be interpreted as "when" or "once" or "in response to determining" or "in response to detecting" depending on the context. Similarly, the phrase "if determined" or "if [the described condition or event] is detected" can be interpreted as meaning "once determined" or "in response to determining" or "once [the described condition or event] is detected" or "in response to detecting [the described condition or event]" depending on the context.
[0056] In addition, in the description of the specification and claims of the present application, the terms "first", "second", "third", etc. are only used for distinguishing descriptions and cannot be understood as indicating or implying relative importance.
[0057] The reference to "one embodiment" or "some embodiments" or the like described in the specification of the present application means that a specific feature, structure, or characteristic described in connection with that embodiment is included in one or more embodiments of the present application. Thus, statements such as "in one embodiment", "in some embodiments", "in other some embodiments", "in still other embodiments", etc. that appear in different places in this specification do not necessarily all refer to the same embodiment, but mean "one or more but not all embodiments", unless otherwise specifically emphasized in another way. The terms "comprising", "including", "having", and their variants all mean "including but not limited to", unless otherwise specifically emphasized in another way.
[0058] In devices such as medical robots, surgical robots, and collaborative robots, the torque control in the servo system is usually used to complete the dragging function of the joints, that is, the human hand drags the joint to move. In order to ensure that the robot does not exceed its reliable operation range during the movement, it is necessary to implement limit control on the joints, and the effective implementation of the robot joint limit control is crucial for improving the working efficiency, safety, and system life of the robot. In the related art, the control of the robot joint limit mostly relies on hardware limit. When a mistake occurs in the joint control, the control protection of the joint limit is likely to fail, resulting in serious safety accidents.
[0059] In view of the above problems, the present application provides a control method for joint limit. First, obtain the current position and current speed of the joint and the torque control instruction for the joint movement. The current position and current speed of the joint are obtained to determine whether the joint exceeds the limit. Secondly, generate a limit torque instruction according to the current position and current speed of the joint and the preset limit. Finally, combine the torque control instruction and the limit torque instruction to control the joint to move within the limit, realizing the movement of the joint within the joint limit in the torque control mode of the servo system.
[0060] It should be understood that the magnitudes of the sequence numbers of the steps in this embodiment do not mean the order of execution. The order of execution of each process should be determined by its function and internal logic, and should not constitute any limitation to the implementation process of the embodiments of the present application. To illustrate the technical solution of the present application, the following will be described by specific embodiments.
[0061] Refer to Figure 1 , which shows a schematic flow chart of a control method for joint limit provided in Embodiment 1 of the present application. The method includes the following steps:
[0062] Step 101, obtain the current position, current speed of the joint, and the torque control instruction for controlling the joint movement.
[0063] It should be noted that the control method for joint limit in the embodiments of the present application can be executed by the control device for joint limit in the embodiments of the present application. The control device for joint limit in the embodiments of the present application can be configured in any control device to execute the control method for joint limit in the embodiments of the present application. For example, the control device for joint limit in the embodiments of the present application can be configured in a robot to control the movement of the robot joint within the joint limit.
[0064] In the embodiment of the present application, the control method of joint limit can be implemented through the torque control loop in the servo system. It should be noted that in the servo system, the torque control loop is a closed-loop system for controlling the output torque of the servo motor. Its inputs are the target torque and the feedback torque. The target torque is the desired output torque specified by the upper control system or the user, and the feedback torque is the actual output torque measured by the sensor. The goal of this torque control loop is to make the actual output torque follow the target torque, so as to achieve the required mechanical movement.
[0065] Without considering joint limit, the torque control command for controlling joint movement can be directly used as the target torque of the torque control loop. However, since the servo system needs to prevent the joint from moving outside the workspace to avoid collisions in the mechanical system, the target torque input to the torque control loop needs to consider the joint limit problem.
[0066] In the embodiment of the present application, the sensor can be used to read the current position and current speed of the joint. The sensor is usually attached to the joint to measure the position and speed of the joint. In torque control, the control system usually adjusts the movement of the joint by calculating the required torque. Therefore, the control system can generate a torque control command according to the target movement of the joint.
[0067] In a possible implementation, obtaining the current position and current speed of the joint may include:
[0068] Obtaining the current position and current speed of the joint through an encoder connected to the power device of the joint.
[0069] In the embodiment of the present application, the joint is usually driven by a power device. For example, the joint is driven by a motor. Connecting the encoder to the motor can obtain the current position and current speed of the joint. Specifically, an absolute encoder or an incremental encoder can be used to obtain the current position and current speed of the joint. The absolute encoder can directly provide the absolute position of the joint, and the incremental encoder can provide the position change relative to the initial position. Whether using an absolute encoder or an incremental encoder, the control system can calculate the rotational speed of the joint by detecting the change in the number of pulses output by the encoder over time, and then calculate the angular change of the joint during this period by recording the difference in pulse counts between two time points, so as to obtain the movement speed. Therefore, the type of encoder used in the present application is not limited.
[0070] Step 102, generating a limit torque command according to the current position, current speed of the joint and the preset limit;
[0071] In the embodiments of the present application, it is possible to determine whether the joint currently exceeds the corresponding limit according to the current position, current speed, and preset limit of the joint, and then generate a limit torque command according to the determination result.
[0072] In a possible implementation manner, generating a limit torque command according to the current position, current speed, and preset limit of the joint includes:
[0073] Determine the position where the joint acts on the limit according to the current position of the joint and the preset limit;
[0074] Compare the current position with the position where the joint acts on the limit to obtain the distance difference between the current position and the position of the acting limit;
[0075] Generate a limit torque command according to the current speed and the distance difference.
[0076] In the embodiments of the present application, the position where the joint acts on the limit refers to the joint position where the limit takes effect. In different situations, the position where the joint acts on the limit is different. For example, when the current position is within the preset limit, the joint acting limit should be the preset limit at this time. If the current position exceeds the preset limit, the joint movement needs to be stopped immediately at this time. Therefore, the joint acting limit should be the current position at this time.
[0077] When there is a distance difference between the current position and the joint acting limit, the limit torque command can be calculated according to the current speed and the distance difference to avoid unexpected movement in the torque mode and the resulting movement injuries.
[0078] In a possible implementation manner, the preset limit includes a preset left limit and a preset right limit, and the joint acting limit includes a joint acting right limit and a joint acting left limit. When the current position is the initial position to enter the torque mode; determining the position where the joint acts on the limit according to the current position of the joint and the preset limit includes:
[0079] When the initial position exceeds the preset right limit, determine that the position of the joint acting right limit is the initial position;
[0080] When the initial position is within the preset right limit, determine that the position of the joint acting right limit is the position of the preset right limit;
[0081] When the initial position exceeds the preset left limit, determine that the position of the joint acting left limit is the position of the preset left limit;
[0082] When the initial position is within the preset left limit, determine that the position of the joint acting left limit is the initial position.
[0083] In the embodiments of the present application, the preset limit positions include the forward rotation limit of the motor, i.e., the preset right limit position, and the reverse rotation limit of the motor, i.e., the preset left limit position. Correspondingly, the joint acting limit positions also include the joint acting right limit position and the joint acting left limit position. Among them, the positions of the preset limit positions are all set inside the joint mechanical limit positions.
[0084] Among them, the current position can be the initial position at the moment of entering the torque mode, or the real-time position after entering the torque mode. Since there is no initial value for the joint acting limit positions at the moment of entering the torque mode, it is necessary to calculate according to the initial position at the moment of entering the torque mode and the preset limit positions. After entering the torque mode, the value of the joint acting limit positions at the current moment needs to be updated according to the value at the previous moment. Therefore, it is necessary to calculate the positions of the joint acting limit positions in different cases.
[0085] In the embodiments of the present application, assuming that the current position is the initial position at the moment of entering the torque mode, for example, the joint acting limit positions include the joint acting right limit Limit workR and the joint acting left limit Limit workL . At the moment of entering the torque mode, if the current position > the preset right limit Limit setR , then Limit workR = the current position; otherwise, Limit workR = Limit setR .
[0086] At the moment of entering the torque mode, if the current position < the preset left limit Limit setL , then Limit workL = the current position; otherwise, Limit workL = Limit setL .
[0087] In a possible implementation manner, when the current position is the real-time position after entering the torque mode, determining the positions of the joint acting limit positions according to the current position of the joint and the preset limit positions includes:
[0088] When the position of the joint acting right limit at the previous moment exceeds the preset right limit, determining the minimum value between the position of the joint acting right limit at the previous moment and the current position as the joint acting right limit at the current moment;
[0089] When the position of the joint acting right limit at the previous moment is within the preset right limit, determining the preset right limit as the joint acting right limit at the current moment;
[0090] When the position of the joint acting left limit at the previous moment is within the preset left limit, determining the maximum value between the position of the joint acting left limit at the previous moment and the current position as the joint acting left limit at the current moment;
[0091] When the position of the left limit of the joint action at the previous moment exceeds the preset left limit, the preset left limit is determined as the left limit of the joint action at the current moment.
[0092] In the embodiment of the present application, the current position refers to the real-time position after entering the torque mode, and the value of its joint action limit at the current moment needs to be updated according to the value at the previous moment.
[0093] Exemplarily, if Limit at the previous moment workR >Limit setR , then let Limit at the current moment workR =min(Limit workR , current position), otherwise let Limit at the current moment workR =Limit setR .
[0094] If Limit at the previous moment workL <Limit setL , then let Limit at the current moment workL =max(Limit workL , current position), otherwise let Limit at the current moment workL =Limit setL .
[0095] In a possible implementation manner, when the current position exceeds the position of the action limit; according to the current speed and the distance difference, a limit torque command is generated, including:
[0096] Determine the damping of the speed outside the boundary according to the current speed and the damping coefficient of the speed outside the preset boundary;
[0097] Determine the boundary elastic force according to the distance difference and the preset boundary torque coefficient;
[0098] Generate a limit torque command according to the damping of the speed outside the boundary and the boundary elastic force.
[0099] In the embodiment of the present application, if the current position exceeds the position of the joint action limit, a limit torque command is generated to avoid unexpected movement in the torque mode and the resulting movement damage. If the current position is within the joint action limit, the generated limit torque command is 0, that is, no adjustment is required. The control system can be made to perform torque control according to the torque control command.
[0100] Exemplarily, if the current position exceeds the joint action limit, the distance exceeding the joint action limit is calculated in real time as Δx. That is:
[0101] When the position is within the positive and negative action limits, Δx = 0;
[0102] If the current position > LimitworkR Then, Δx = Current Position - Limit workR ;
[0103] If the current position < Limit workL Then, Δx = Current Position - Limit workL ;
[0104] In the embodiments of the present application, the limiting torque command can be denoted as T limit , which consists of two parts, namely T limit = T sp + T damp , T sp is the boundary elastic force, and T damp is the velocity damping outside the boundary. Among them, when the current position is inside and outside the positive and negative acting limits, the obtained limiting torque commands are different. That is:
[0105] When the position is inside the positive and negative acting limits, T limit = 0, T sp = 0, T damp = 0, and the limiting torque command is 0.
[0106] When the position is outside the positive and negative acting limits, T sp = K sp *Δx, where K sp is the boundary torque coefficient, the coefficient is negative, and the direction of the boundary elastic force points to the inside of the acting limit.
[0107] When the position is outside the positive and negative acting limits, T damp = K damp *Velocity, where K damp is the velocity damping coefficient outside the boundary, the coefficient is negative, the direction of the velocity damping outside the boundary points to the opposite direction of the motion direction, and Velocity is the current velocity.
[0108] In the embodiments of the present application, the velocity damping term T damp can effectively suppress the oscillation of the servo system caused by the boundary elastic force.
[0109] Step 103: Combine the torque control command and the limiting torque command to control the joint to move within the limit.
[0110] In the embodiments of the present application, by combining the torque control command and the limiting torque command, the joint can be controlled to move within the joint acting limit. That is, if the joint reaches the joint acting limit, the joint can be directly controlled to stop to avoid unexpected motion in the torque mode and the resulting motion injuries.
[0111] In the embodiments of the present application, the torque control command and the limit torque command can be combined as the target torque input in the torque control loop, and then the torque control of the joint can be performed in combination with the feedback torque.
[0112] In a possible implementation, by combining the torque control command and the limit torque command, controlling the joint to move within the limit includes:
[0113] Performing an addition operation on the torque control command and the limit torque command to obtain the target torque;
[0114] Determining the torque deviation between the target torque and the current feedback torque;
[0115] Inputting the torque deviation into the torque controller;
[0116] The torque controller outputs a drive signal to control the joint to move within the limit according to the drive signal.
[0117] In the embodiments of the present application, the torque controller can adopt a PI controller or a PID controller, and the present application does not make a limitation on this.
[0118] It should be noted that the PI controller consists of two control parts: proportional and integral. In the current loop, the proportional part is used to respond to the current error, and the integral part is used to process the accumulated error to ensure the stability and accuracy of the system. The PID controller includes three control parts: proportional, integral, and derivative. In the current loop, the derivative part helps to reduce overshoot and improve the response speed of the system, but it may also increase the noise sensitivity of the system. The choice between a PI controller and a PID controller depends on the characteristics and performance requirements of the system.
[0119] Exemplarily, as Figure 2 shown is the motion control drive block diagram based on the target torque. It can be obtained from the figure that the target torque can be obtained by adding the torque control command and the limit torque command, and the limit torque command is obtained by the boundary limit calculation module. The boundary limit calculation module calculates the boundary limit torque, that is, the limit torque command, according to the distance deviation of the current position exceeding the joint action limit and the current speed. After obtaining the target torque, the difference between the target torque and the feedback torque is input into the PID control, and a drive signal is output to the drive circuit to drive the motor to drive the load to work. The current position and current speed of the joint can be obtained through an encoder connected to the motor.
[0120] It should be understood that the motor current can be measured through a current sensor and converted into a feedback torque through a modeling and calibration process. The torque control loop can adjust the control current or torque in real time to achieve the desired target torque.
[0121] In the embodiment of the present application, first, the current position and current speed of the joint and the torque control instruction for the joint movement are obtained. The current position and current speed of the joint are obtained to determine whether the joint exceeds the limit. Secondly, according to the current position and current speed of the joint and the preset limit, a limit torque instruction is generated. Finally, by combining the torque control instruction and the limit torque instruction, the joint is controlled to move within the limit, realizing that the joint moves within the joint limit in the torque control mode of the servo system.
[0122] See Figure 3 , which shows a schematic flowchart of another control method for joint limit provided in the second embodiment of the present application; the method includes the following steps:
[0123] Step 301, obtain the current speed of the joint through an encoder connected to the power device of the joint.
[0124] In the embodiment of the present application, the joint is usually driven by a power device. For example, the joint is driven by a motor. By connecting the encoder to the motor, the current speed of the joint can be obtained. Specifically, an absolute encoder or an incremental encoder can be used to obtain the current speed of the joint. Whether an absolute encoder or an incremental encoder is used, the control system can calculate the rotational speed of the joint by detecting the change in the number of pulses output by the encoder over time, and then calculate the angular change of the joint during this period by recording the difference in pulse counts between two time points, so as to obtain the movement speed. Therefore, the type of encoder used in the present application is not limited.
[0125] Step 302, obtain the current position of the joint through an encoder connected to the reducer.
[0126] In the embodiment of the present application, if there is a reducer between the motor and the load, the position feedback needs to use the result collected by the encoder at the output end of the reducer as the position feedback. The reducer can decelerate when the joint moves to the joint limit, so as to better realize that the joint can move within the joint limit in the torque control mode of the servo system.
[0127] Exemplarily, as Figure 4 shown in the motion control drive block diagram corresponding to another control method for joint limit. In this block diagram, a reducer is installed between the motor and the load to decelerate the motor in time. After the reducer is installed, the result collected from the encoder at the output end of the reducer should be used as the current position.
[0128] As an example rather than a limitation, the reducer can be a gear reduction, a planetary reduction, a harmonic reduction, etc.
[0129] Step 303, obtain the torque control instruction for the joint movement.
[0130] Step 304: Generate a limit torque command based on the current position, current speed of the joint, and the preset limit.
[0131] Step 305: Combine the torque control command and the limit torque command to control the joint to move within the limit.
[0132] Steps 303 to 305 in this embodiment are the same as steps 101 to 103 in the foregoing embodiment and can be referred to each other. This embodiment will not be elaborated herein.
[0133] Compared with the first embodiment, in this embodiment, by installing a reducer, it can better achieve the movement of the joint within the joint limit in the torque control mode of the servo system, and also avoid the damage of the joint caused by the sudden stop when reaching the limit.
[0134] See Figure 5 , which shows a schematic structural diagram of a control device for joint limit provided by an embodiment of the present application; for the convenience of description, only the parts related to the embodiment of the present application are shown.
[0135] The control device for joint limit may specifically include the following modules:
[0136] An acquisition module 501, configured to acquire the current position, current speed of the joint, and a torque control command for controlling the movement of the joint;
[0137] A generation module 502, configured to generate a limit torque command based on the current position, current speed of the joint, and the preset limit;
[0138] A control module 503, configured to combine the torque control command and the limit torque command to control the joint to move within the limit.
[0139] In the embodiment of the present application, the generation module 502 may specifically include the following units:
[0140] A position determination unit, configured to determine the position where the joint acts on the limit according to the current position of the joint and the preset limit;
[0141] A position comparison unit, configured to compare the current position with the position where the joint acts on the limit to obtain the distance difference between the current position and the position of the acting limit;
[0142] A command generation unit, configured to generate a limit torque command according to the current speed and the distance difference.
[0143] In the embodiment of the present application, the preset limit includes a preset left limit and a preset right limit, and the joint acting limit includes a joint acting right limit and a joint acting left limit. When the current position is the initial position for entering the torque mode, the position determination unit may specifically be used for:
[0144] When the initial position exceeds the preset right limit position, determine the position of the right limit of the joint action as the initial position;
[0145] When the initial position is within the preset right limit position, determine the position of the right limit of the joint action as the preset right limit position;
[0146] When the initial position exceeds the preset left limit position, determine the position of the left limit of the joint action as the preset left limit position;
[0147] When the initial position is within the preset left limit position, determine the position of the left limit of the joint action as the initial position.
[0148] In the embodiment of the present application, when the current position is the real-time position after entering the torque mode, the position determination unit may specifically further be used for:
[0149] When the position of the right limit of the joint action at the previous moment exceeds the preset right limit position, determine the minimum value of the position of the right limit of the joint action at the previous moment and the current position as the right limit of the joint action at the current moment;
[0150] When the position of the right limit of the joint action at the previous moment is within the preset right limit position, determine the preset right limit as the right limit of the joint action at the current moment;
[0151] When the position of the left limit of the joint action at the previous moment is within the preset left limit position, determine the maximum value of the position of the left limit of the joint action at the previous moment and the current position as the left limit of the joint action at the current moment;
[0152] When the position of the left limit of the joint action at the previous moment exceeds the preset left limit position, determine the preset left limit as the left limit of the joint action at the current moment.
[0153] In the embodiment of the present application, when the current position exceeds the position of the joint action limit; the instruction generation unit may specifically be used for:
[0154] Determine the damping of the speed outside the boundary according to the current speed and the damping coefficient of the speed outside the preset boundary;
[0155] Determine the boundary elastic force according to the distance difference and the preset boundary torque coefficient;
[0156] Generate a limit torque instruction according to the damping of the speed outside the boundary and the boundary elastic force.
[0157] In the embodiment of the present application, the control module 503 may specifically include the following units:
[0158] The target determination unit is used to perform an addition operation on the torque control instruction and the limit torque instruction to obtain the target torque;
[0159] A deviation determination unit for determining the torque deviation between the target torque and the current feedback torque;
[0160] A deviation input unit for inputting the torque deviation into the torque controller;
[0161] A drive output unit for outputting a drive signal by the torque controller to control the movement of the joint within the limit according to the drive signal.
[0162] In the embodiment of the present application, the acquisition module 501 may specifically include the following units:
[0163] A first acquisition unit for acquiring the current position and current speed of the joint through an encoder connected to the power device of the joint.
[0164] In the embodiment of the present application, when a speed reducer is installed between the power device of the joint and the load, the acquisition module 501 may specifically further include the following units:
[0165] A second acquisition unit for acquiring the current speed of the joint through an encoder connected to the power device of the joint;
[0166] A third acquisition unit for acquiring the current position of the joint through an encoder connected to the speed reducer.
[0167] The control device for joint limit provided in the embodiment of the present application can be applied in the foregoing method embodiments. For details, please refer to the description of the foregoing method embodiments and will not be elaborated here.
[0168] Figure 6 is a structural block diagram of a control device provided in the embodiment of the present application. As Figure 6 shown, the control device 600 of this embodiment includes: a processor 610, a memory 620, and a computer program 630 stored in the memory 620 and executable on the processor 610, such as a control program for a force balance device. When the processor 610 executes the computer program 630, the steps in the embodiments of the above-mentioned control method for each joint limit are implemented, such as Figure 1 shown in 101 to 103, or when the processor 610 executes the computer program 630, the functions of each module in the corresponding embodiments above are implemented. For example, Figure 5 the functions of the modules 501 to 503 shown in Figure 5 For details, please refer to the relevant descriptions in the corresponding embodiments. Figure 5
[0169] Exemplarily, the computer program 630 can be divided into one or more modules. One or more modules are stored in the memory 620 and executed by the processor 610 to complete the present application. One or more modules can be a series of computer program instruction segments capable of completing specific functions, and these instruction segments are used to describe the execution process of the computer program 630 in the control device 600. For example, the computer program 630 can be divided into various unit modules, and the specific functions of each module are as described above.
[0170] The control device 600 may include, but is not limited to, the processor 610 and the memory 620. Those skilled in the art can understand that Figure 6 merely examples of the control device 600, which do not constitute a limitation on the control device 600. It may include more or fewer components than shown in the figure, or combine certain components, or different components. For example, the control device may also include input / output devices, network access devices, buses, etc.
[0171] The so-called processor 610 may be a central processing unit, or may also be other general-purpose processors, digital signal processors, application-specific integrated circuits, off-the-shelf programmable gate arrays, or other programmable logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc.
[0172] The memory 620 may be an internal storage unit of the control device 600, such as the hard disk or memory of the control device 600. The memory 620 may also be an external storage device of the control device 600, such as a plug-in hard disk, a smart memory card, a flash memory card, etc. equipped on the control device 600. Further, the memory 620 may also include both the internal storage unit and the external storage device of the control device 600.
[0173] Those skilled in the art can clearly understand that, for the convenience and simplicity of description, only the above division of each functional unit and module is used as an example. In actual applications, the above functions can be allocated to different functional units and modules according to needs, that is, the internal structure of the device is divided into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above integrated unit can be implemented in the form of hardware or in the form of a software functional unit. In addition, the specific names of each functional unit and module are only for the convenience of mutual distinction and do not limit the protection scope of the present application. The specific working processes of the units and modules in the above system can refer to the corresponding processes in the foregoing method embodiments and will not be elaborated here.
[0174] In the above embodiments, the descriptions of the various embodiments each have their own emphasis. For parts not described in detail or recorded in a certain embodiment, reference may be made to the relevant descriptions of other embodiments.
[0175] Those of ordinary skill in the art will realize that the units and algorithm steps of the examples described in conjunction with the embodiments disclosed herein can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are executed in hardware or software depends on the specific application and design constraints of the technical solution. A professional technician can use different methods for each specific application to implement the described functions, but such implementation should not be considered to exceed the scope of this application.
[0176] In the embodiments provided in this application, it should be understood that the disclosed devices / electronic devices and methods can be implemented in other ways. For example, the device / electronic device embodiments described above are merely illustrative. For example, the division of the modules or units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed couplings or direct couplings or communication connections to each other can be through some interfaces. The indirect couplings or communication connections of the devices or units can be in electrical, mechanical or other forms.
[0177] The units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they can be located in one place, or they can be distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.
[0178] In addition, the functional units in the various embodiments of this application can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above integrated units can be implemented in the form of hardware or in the form of software functional units.
[0179] When the integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, to implement all or part of the processes in the above-described embodiment methods of this application, it can also be completed by a computer program instructing relevant hardware. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by a processor, the steps of the above-described various method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in the form of source code, object code, executable file, or some intermediate form, etc. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disc, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal, and software distribution medium, etc. It should be noted that the content included in the computer-readable medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, the computer-readable medium does not include electrical carrier signals and telecommunication signals.
[0180] To implement all or part of the processes in the above-described embodiment methods of this application, it can also be completed by a computer program product. When the computer program product runs on an electronic device, the electronic device can be made to execute the steps in the above-described various method embodiments when executed.
[0181] The above-described embodiments are only used to illustrate the technical solutions of this application, rather than to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the various embodiments of this application, and should all be included within the protection scope of this application.
Claims
1. A control method for joint limit, characterized in that, The control method includes: Obtaining the current position, current speed of the joint, and a torque control command for controlling the movement of the joint; Generating a limit torque command based on the current position of the joint, the current speed, and a preset limit; Combining the torque control command and the limit torque command to control the joint to move within the limit.
2. The control method according to claim 1, characterized in that, The generating of the limit torque command based on the current position of the joint, the current speed, and the preset limit includes: Determining the position where the joint acts on the limit according to the current position of the joint and the preset limit; Comparing the current position with the position where the joint acts on the limit to obtain a distance difference between the current position and the position of the acting limit; Generating the limit torque command based on the current speed and the distance difference.
3. The control method according to claim 2, characterized in that, The preset limit includes a preset left limit and a preset right limit, and the joint acting limit includes a joint acting right limit and a joint acting left limit. When the current position is the initial position for entering the torque mode; The determining of the position where the joint acts on the limit according to the current position of the joint and the preset limit includes: When the initial position exceeds the preset right limit, determining the position of the joint acting right limit as the initial position; When the initial position is within the preset right limit, determining the position of the joint acting right limit as the position of the preset right limit; When the initial position exceeds the preset left limit, determining the position of the joint acting left limit as the position of the preset left limit; When the initial position is within the preset left limit, determining the position of the joint acting left limit as the initial position.
4. The control method according to claim 3, characterized in that, When the current position is the real-time position after entering the torque mode; the determining of the position where the joint acts on the limit according to the current position of the joint and the preset limit includes: When the position of the joint acting right limit at the previous moment exceeds the preset right limit, determining the minimum value between the position of the joint acting right limit at the previous moment and the current position as the joint acting right limit at the current moment; When the position of the joint acting right limit at the previous moment is within the preset right limit, determining the preset right limit as the joint acting right limit at the current moment; When the position of the joint acting left limit at the previous moment is within the preset left limit, determining the maximum value between the position of the joint acting left limit at the previous moment and the current position as the joint acting left limit at the current moment; When the position of the joint acting left limit at the previous moment exceeds the preset left limit, determining the preset left limit as the joint acting left limit at the current moment.
5. The control method according to claim 2, characterized in that, When the current position exceeds the position of the joint acting limit; the generating of the limit torque command based on the current speed and the distance difference includes: Determining an out-of-boundary speed damping according to the current speed and a damping coefficient of the speed outside the preset boundary; Determining a boundary elastic force according to the distance difference and a boundary torque coefficient. Generate the limit torque command according to the velocity damping outside the boundary and the boundary elastic force.
6. The control method according to claim 1, characterized in that, Combining the torque control command and the limit torque command to control the movement of the joint within the limit includes: Performing an addition operation on the torque control command and the limit torque command to obtain a target torque; Determining the torque deviation between the target torque and the current feedback torque; Inputting the torque deviation into a torque controller; Outputting a drive signal by the torque controller to control the movement of the joint within the limit according to the drive signal.
7. The control method according to claim 1, characterized in that, Obtaining the current position and current velocity of the joint includes: Obtaining the current position and the current velocity of the joint through an encoder connected to the power device of the joint.
8. The control method according to claim 1, characterized in that, A speed reducer is installed between the power device of the joint and the load; obtaining the current position and current velocity of the joint further includes: Obtaining the current velocity of the joint through an encoder connected to the power device of the joint; Obtaining the current position of the joint through an encoder connected to the speed reducer.
9. A control device for joint limit, characterized in that, The control device includes: An acquisition module for acquiring the current position and current velocity of the joint and the torque control command for controlling the movement of the joint; A generation module for generating a limit torque command according to the current position of the joint, the current velocity, and a preset limit; A control module for combining the torque control command and the limit torque command to control the movement of the joint within the limit.
10. A control device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, the method according to any one of claims 1 to 8 is implemented.
11. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, the method according to any one of claims 1 to 8 is implemented.