Multi-degree-of-freedom connecting rod mechanism high-frequency control method and system and motion device

Abstract

The invention relates to the technical field of robot motion control, and provides a high-frequency control method and system for a multi-degree-of-freedom connecting rod mechanism and a motion device.The actual tail end position and the actual tail end speed of a tail end executor are positively mapped through an actual joint position set and an actual joint speed set; the target tail end speed of the tail end actuator is determined according to the actual and target tail end positions in cooperation with a position ring, and the target tail end equivalent force of the tail end actuator is determined according to the deviation value of the actual and target tail end speeds through a speed ring; according to a statics model of the connecting rod mechanism, the target tail end equivalent force is reversely mapped into target joint torque of all joints, target current values corresponding to all the motors are obtained, and a current loop controls the motors according to the target current values so as to achieve motion control over the tail end actuator. And equivalently, the direct control of the driver on the end effector is virtually equivalent. And the problems of control level limitation, communication rate bottleneck, insufficient control bandwidth under large load and the like are solved.

Description

Technical Field

[0001] This disclosure relates to the field of robot motion control technology, and in particular to high-frequency control methods, systems and motion devices for multi-degree-of-freedom linkage mechanisms. Background Technology

[0002] In the control systems of multi-degree-of-freedom linkage mechanisms (such as multi-link parallel or series mechanisms) in related technologies, multiple servo motors are typically used as power inputs, and precise tracking of the mechanism's motion trajectory is achieved through position loop control. Each servo motor is controlled by an independent servo driver.

[0003] However, the relevant technical solutions have the following technical defects:

[0004] First, there is a limitation in the control hierarchy. In the traditional architecture, in the three-loop proportional-integral (PI) control of a servo motor, the servo motor driver only runs the position loop control (PI control corresponding to the motor's rotation angle), while the speed loop (PI control corresponding to the motor's speed) and current loop (PI control corresponding to the motor's torque) control rely on the host computer to calculate and issue commands, resulting in low force control bandwidth and large latency.

[0005] Secondly, communication speed presents a significant bottleneck. Typically, the position and velocity of the end effector are indirectly calculated using corresponding parameters of the servo motor. Changes in the servo motor's position lead to changes in the calculation results, increasing computational complexity and error. If the position and velocity of the end effector were to be measured directly by sensors, current communication speeds or sensors would be insufficient to support the high dynamic response control requirements of multi-degree-of-freedom linkage mechanisms. Specifically, most industrial control systems currently use CAN bus or conventional industrial Ethernet (such as ModbusTCP, PROFINET, etc.), with communication cycles typically around 1ms (i.e., communication frequency of approximately 1kHz), which is insufficient to meet the high dynamic response control requirements of multi-degree-of-freedom linkage mechanisms. Therefore, although some high-performance buses (such as EtherCAT) can achieve communication cycles of hundreds of microseconds, the overall control cycle for complex multi-axis trajectory calculations by the host computer, especially those involving dynamic feedforward or impedance control algorithms, is often still around 1ms. This limits the system's response speed to external disturbances or command changes, making it difficult to achieve kHz-level high-frequency control.

[0006] Furthermore, traditional force control or compliant control typically requires a host computer to perform complex dynamic calculations, converting the desired end force / torque into joint space position or torque compensation, and then sending it down via the bus. This process involves a large amount of computation and is limited by the bus cycle time, resulting in a force control loop bandwidth that is usually below 300Hz, leading to insufficient dynamic performance and the problems of complex force control implementation and low bandwidth.

[0007] In addition, control becomes more difficult when the end effector is under high load. When the end effector of the linkage has a large load and high inertia, the slow update of position commands, lack of real-time force feedback adjustment mechanism, and poor coordination of multi-axis linkage pose a high challenge to the bandwidth of simple PI control, and overshoot, oscillation or even instability are likely to occur.

[0008] Therefore, there is an urgent need for a new driver and control scheme to break through the bottleneck of traditional bus communication and improve the high-frequency, precise and simple control capability of large inertial end units. Summary of the Invention

[0009] In view of the shortcomings of the prior art described above, the purpose of this disclosure is to provide a high-frequency control method, system and motion device for multi-degree-of-freedom linkage mechanisms to solve the problems in the related art.

[0010] The first aspect of this disclosure provides a high-frequency control system for a multi-degree-of-freedom linkage mechanism. The multi-degree-of-freedom linkage mechanism includes multiple joints and an end effector, with one group of joints driven by a group of motors. The system includes: a multi-channel motor drive circuit, each connected to and controlling one group of motors; a processor, connected to the motor drive circuit, including: a communication interface, a storage unit, a processing unit, a position loop control unit, and a speed loop control unit; the communication interface is used to receive operating parameters and target end positions of each motor; the storage unit is used to store a mapping relationship model; the mapping relationship model includes: a forward kinematic model of the linkage mechanism, containing a mapping relationship between the set of joint positions and joint velocities of each joint and the end position and end velocity of the end effector; a static model of the linkage mechanism, containing a mapping relationship between the set of target joint torques of each joint and the end force of the end effector; and the processing unit is used to obtain the actual set of joint positions and the actual set of joint velocities of each joint based on the operating parameters of each motor. The operating parameters include the actual rotation angle corresponding to the actual joint position and the actual rotational speed corresponding to the actual joint speed. Using the forward kinematics model of the linkage mechanism, the set of actual joint positions and the set of actual joint speeds are mapped to the actual end position and actual end speed of the end effector in the operating space, respectively. The position loop control unit is used to obtain the target end speed of the end effector in the operating space based on the deviation between the actual end position and the received target end position. The speed loop control unit is used to obtain the target end equivalent force of the end effector in the operating space based on the deviation between the actual end speed and the target end speed. The processing unit is used to inversely map the target end equivalent force to the target joint torque of each joint using the static model of the linkage mechanism. The target current value of each motor is obtained based on the target joint torque. The current loop control unit is used to control the actual output current of the motor to tend towards the target current value, thereby achieving motion control of the end effector.

[0011] In an embodiment of the first aspect, the processor is communicatively connected to a current sampling circuit configured to sample a set of actual drive currents of the motor drive circuit to obtain an actual current value; the current loop control unit is configured to update the control signal to the motor based on the deviation between the actual current value and the target current value.

[0012] In an embodiment of the first aspect, a virtual control unit is formed among the processing unit, the position loop control unit, and the speed loop control unit to periodically execute the process from the operating parameters of each motor to obtaining the target current value according to a first control cycle; the current loop control unit operates in a second control cycle; the second control cycle is less than or equal to the first control cycle.

[0013] In an embodiment of the first aspect, the frequency of the first control cycle is not less than 16 kHz, and the frequency of the second control cycle is not less than 48 kHz.

[0014] In an embodiment of the first aspect, the position loop control unit and / or the velocity loop control unit are implemented as a proportional-integral controller or a proportional-integral-derivative controller.

[0015] A second aspect of this disclosure provides a motion device, comprising: a multi-degree-of-freedom linkage mechanism including multiple joints and an end effector; at least two motors connected to and driving at least two of the joints; a high-frequency control system for the multi-degree-of-freedom linkage mechanism as described in any one aspect of the first aspect, communicatively connected to a group of the motors to obtain operating parameters, and connected to a host computer to send and receive target end positions.

[0016] In a second aspect embodiment, the motion device is implemented as an upper / lower limb rehabilitation training robot.

[0017] This disclosure provides a high-frequency control method for a multi-degree-of-freedom linkage mechanism, the multi-degree-of-freedom linkage mechanism including multiple joints and an end effector, wherein a group of joints is driven by a group of motors; the method is applied to a driver connected to the group of motors, including: obtaining a set of actual joint positions and a set of actual joint velocities for each joint based on the operating parameters of each motor; wherein the operating parameters include an actual rotation angle corresponding to the actual joint position and an actual rotational speed corresponding to the actual joint velocity; and mapping the set of actual joint positions and the set of actual joint velocities to the actual end position and actual end position of the end effector in the operating space through a forward kinematic model of the linkage mechanism. Speed; the position loop control unit obtains the target end-effector speed in the operating space based on the deviation between the actual end-effector position and the received target end-effector position; the speed loop control unit obtains the target end-effector equivalent force in the operating space based on the deviation between the actual end-effector speed and the target end-effector speed; the target end-effector equivalent force is inversely mapped to the target joint torque of each joint using the static model of the linkage mechanism; the target current value of each motor is obtained based on the target joint torque; each target current value is used as a set value and input to the current loop control unit to control the actual output current of the motor to tend towards the target current value, thereby realizing motion control of the end-effector.

[0018] This disclosure provides a fourth aspect of a processor, comprising: a stored computer program or instructions for executing a high-frequency control method for a multi-degree-of-freedom linkage mechanism as described in the second aspect.

[0019] The fifth aspect of this disclosure provides a computer-readable storage medium, characterized in that it stores a computer program or instructions, which are executed to perform a high-frequency control method for a multi-degree-of-freedom linkage mechanism as described in the second aspect.

[0020] As described above, this disclosure relates to the field of robot motion control technology, providing a high-frequency control method, system, and motion device for multi-degree-of-freedom linkage mechanisms. It forward maps the actual end effector position and velocity of the end effector using a set of actual joint positions and velocities. A position loop determines the target end effector velocity based on the actual and target end effectors, and a velocity loop determines the target end effector equivalent force based on the deviation between the actual and target end effectors. Based on the static model of the linkage mechanism, the target end effector equivalent force is inversely mapped to the target joint torque of each joint, and the target current value corresponding to each motor is obtained. The current loop then controls the motors based on the target current value to achieve motion control of the end effector. This is equivalent to a virtual equivalent actuator directly controlling the end effector. It overcomes problems such as control hierarchy limitations, communication rate bottlenecks, and insufficient control bandwidth under heavy loads.

[0021] Therefore, the control core is moved down from the host computer to the driver, breaking through the limitations of control hierarchy. The host computer only needs to provide low-frequency target position, no longer relying on host computer control. The host computer only needs to provide low-frequency position trajectory commands, without performing real-time dynamic calculations, reducing the computing power requirements and software complexity of the host computer, and improving system reliability.

[0022] Furthermore, since the entire control process occurs between the driver and the motor, the high-speed processor in the driver enables high-speed processing and communication, overcoming the bottleneck limitation of communication rate. By embedding the control core inside the driver, the virtual loop control cycle can reach tens to hundreds of microseconds (corresponding to several kHz to tens of kHz), completely independent of the relatively low bus communication cycle of the host computer (typically 1 ms), achieving extremely fast response to end-effector dynamics. Through static mapping, the microsecond-level response current loop is directly equivalent to the end-effector force controller, increasing the end-effector force control bandwidth to the current loop level (up to 16 kHz or more), greatly simplifying the control of large inertial loads and contact forces, effectively suppressing vibration, and achieving true high-frequency direct force control.

[0023] Throughout the control algorithm, the controlled variable can always be the decoupled coordinates (such as X, Y) of the end effector in the operating space, along with its first and second order quantities. This is equivalent to simplifying complex multi-link coordinated control into the control of several independent virtual linear axes, which has clear physical meaning, simple parameter tuning, and achieves control decoupling and intuitiveness.

[0024] Furthermore, the above scheme allows the current loop corresponding to each motor to be "virtually equivalent" to the force controller of the end effector. This significantly increases the bandwidth of the PI control for the end effector's force, simplifies the PI control objective, and suppresses vibration and overshoot under large inertial loads. Therefore, regardless of the mechanism's configuration, the controller senses and adjusts only the direct physical quantities of the end effector. This ensures that the dynamic characteristics of the linkage system regarding the closed-loop stiffness and damping of the end effector remain constant within the workspace, resulting in stable and predictable performance and globally consistent control performance. Attached Figure Description

[0025] Figure 1 A schematic diagram of the structure of a device in which a multi-degree-of-freedom linkage mechanism is applied, according to one embodiment of the present disclosure, is shown.

[0026] Figure 2 exhibit Figure 1 A top view of the multi-degree-of-freedom linkage mechanism of the equipment.

[0027] Figure 3 A schematic diagram of the control system of a multi-degree-of-freedom linkage mechanism according to one embodiment of the present disclosure is shown.

[0028] Figure 4 A schematic diagram of the functional units of a processor in one embodiment of the present disclosure is shown.

[0029] Figure 5 A flowchart illustrating a high-frequency control method for a multi-degree-of-freedom linkage mechanism according to an embodiment of the present disclosure is shown.

[0030] Figure 6 A schematic diagram of a control device for a multi-degree-of-freedom linkage mechanism is shown in one embodiment of this disclosure.

[0031] Figure 7 This illustration shows a schematic diagram of the principle of multi-degree-of-freedom linkage mechanism control in one embodiment of the present disclosure.

[0032] Figure 8 A schematic diagram of the structure of an embedded system according to an embodiment of the present disclosure is shown. Detailed Implementation

[0033] The following specific examples illustrate the implementation of this disclosure. Those skilled in the art can easily understand other advantages and effects of this disclosure from the information disclosed herein. This disclosure can also be implemented or applied through other different specific embodiments, and various details in this disclosure can be modified or changed according to different viewpoints and application modules without departing from the spirit of this disclosure. It should be noted that, unless otherwise specified, the embodiments and features in the embodiments of this disclosure can be combined with each other.

[0034] The embodiments of this disclosure will now be described in detail with reference to the accompanying drawings, so that those skilled in the art to which this disclosure pertains can readily implement it. This disclosure may be embodied in many different forms and is not limited to the embodiments described herein.

[0035] In this disclosure, the terms "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics represented in connection with that embodiment or example, which are included in at least two embodiments or examples of this disclosure. Furthermore, the specific features, structures, materials, or characteristics represented may be combined in any suitable manner in any one or a group of embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples represented in this disclosure, as well as the features of those different embodiments or examples.

[0036] Furthermore, the terms "first" and "second" are used for illustrative purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least two of that feature. In the representation of this disclosure, "a set" means two or more, unless otherwise explicitly specified.

[0037] For the purpose of clarity, devices unrelated to the description are omitted, and the same or similar components throughout the specification are given the same reference numerals.

[0038] Throughout this specification, when it is said that a device is "connected" to another device, this includes not only "direct connection" but also "indirect connection" by placing other components in between. Furthermore, when it is said that a device "comprises" a certain constituent element, unless otherwise stated otherwise, this does not exclude other constituent elements, but rather implies that other constituent elements may be included.

[0039] While the terms first, second, etc., are used in some examples herein to refer to various elements, these elements should not be limited by these terms. These terms are used only to distinguish one element from another. For example, first interface and second interface, etc., are used. Furthermore, as used herein, the singular forms “a,” “an,” and “the” are intended to also include the plural forms unless the context indicates otherwise. It should be further understood that the terms “comprising,” “including,” indicate the presence of the stated feature, step, operation, element, module, item, kind, and / or group, but do not exclude the presence, occurrence, or addition of one or more other features, steps, operations, elements, modules, items, kinds, and / or groups. The terms “or” and “and / or” as used herein are interpreted as inclusive, or mean any one or any combination thereof. Thus, “A, B, or C” or “A, B, and / or C” means “any one of: A; B; C; A and B; A and C; B and C; A, B, and C.” Exceptions to this definition will only occur if the combination of elements, functions, steps, or operations is inherently mutually exclusive in some way.

[0040] The technical terms used herein are for reference only to specific embodiments and are not intended to limit the scope of this disclosure. The singular form used herein includes the plural form unless the statement explicitly indicates otherwise. The word "comprising" as used in this specification means to specify a particular characteristic, region, integer, step, operation, element, and / or component, and does not exclude the presence or addition of other characteristics, regions, integers, steps, operations, elements, and / or components.

[0041] Although not explicitly defined, all terms, including technical and scientific terms used herein, shall have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure pertains. Terms defined in commonly used dictionaries shall be further interpreted as having a meaning consistent with the relevant technical literature and the message of the present disclosure, and shall not be over-interpreted as having an ideal or overly formulaic meaning unless otherwise defined.

[0042] In the control systems of multi-degree-of-freedom linkage mechanisms (such as multi-link parallel or series mechanisms) in related technologies, multiple servo motors are typically used as power inputs, and precise tracking of the mechanism's motion trajectory is achieved through position loop control. Each servo motor is controlled by an independent servo driver.

[0043] However, the relevant technical solutions have the following technical defects:

[0044] First, there is a limitation in the control hierarchy. In the traditional architecture, in the three-loop proportional-integral (PI) control of a servo motor, the servo motor driver only runs the position loop control (PI control corresponding to the motor's rotation angle), while the speed loop (PI control corresponding to the motor's speed) and current loop (PI control corresponding to the motor's torque) control rely on the host computer to calculate and issue commands, resulting in low force control bandwidth and large latency.

[0045] Secondly, communication speed presents a significant bottleneck. Typically, the position and velocity of the end effector are indirectly calculated using corresponding parameters of the servo motor. Changes in the servo motor's position cause changes in the calculation results, increasing the computational load and error. If the position and velocity of the end effector were to be measured directly by sensors, current communication speeds or sensors would be insufficient to support the high dynamic response control requirements of multi-degree-of-freedom linkage mechanisms. Specifically, most current industrial control systems use CAN bus or conventional industrial Ethernet (such as ModbusTCP, PROFINET, etc.), with communication cycles typically around 1ms (i.e., communication frequency of approximately 1kHz), which is insufficient to meet the high dynamic response control requirements of multi-degree-of-freedom linkage mechanisms.

[0046] Furthermore, control becomes more difficult when the end effector is under high load. When the end effector of the linkage has a large load and high inertia, the slow update of position commands, lack of real-time force feedback adjustment mechanism, and poor coordination of multi-axis linkage pose a significant challenge to the bandwidth of simple PI control, making it prone to overshoot, oscillation, or even instability.

[0047] Therefore, this disclosure provides a high-frequency control system and corresponding control method for a multi-degree-of-freedom linkage mechanism, thereby solving the problems in the related technology.

[0048] First, let's introduce multi-degree-of-freedom linkage mechanisms. A linkage mechanism, also known as a lower-pair mechanism, is a type of mechanical component. It refers to a mechanism composed of several (two or more) components with definite relative motion connected by lower pairs (revolute or prismatic pairs). Based on whether the relative motion between the components is planar or spatial, linkage mechanisms can be divided into planar linkage mechanisms and spatial linkage mechanisms. A planar linkage mechanism is a common type of transmission mechanism. It refers to a mechanism where all rigid components are connected by lower pairs, hence the name lower-pair mechanism. Planar linkage mechanisms are widely used in various machines, instruments, and control devices. For example, the main mechanisms of reciprocating engines, water pumps, air compressors, shapers, slotting machines, excavators, loaders, jaw crushers, oscillating conveyors, printing machinery, and textile machinery are all planar linkage mechanisms. In linkage mechanisms, if the components do not move in the same plane or in mutually parallel planes, it is called a spatial mechanism.

[0049] When a linkage mechanism has 1 degree of freedom, it is called a single-degree-of-freedom linkage mechanism. When the degree of freedom is greater than 1, it is called a multi-degree-of-freedom linkage mechanism.

[0050] Please see Figure 1 and Figure 2 An example of a multi-degree-of-freedom linkage mechanism 120 is provided. Figure 1 A schematic diagram of the structure of a device in which the multi-degree-of-freedom linkage 120 is applied, according to an embodiment of the present disclosure, is shown. Figure 2 exhibit Figure 1 A top view of the multi-degree-of-freedom linkage mechanism 120 of the device.

[0051] Figure 1 The device shown is, for example, a rehabilitation training device 100. Specifically, the rehabilitation training device 100 is, for example, an upper limb rehabilitation training device 100. The rehabilitation training device 100 includes a support table 110, which has a tabletop, and the tabletop is, for example, horizontally arranged. The multi-degree-of-freedom linkage mechanism 120 is arranged on the tabletop. Exemplarily, the multi-degree-of-freedom linkage is implemented as a two-degree-of-freedom linkage mechanism that can perform two-dimensional motion in the horizontal plane, with the horizontal plane as the motion plane.

[0052] The multi-degree-of-freedom linkage 120 includes multiple links, multiple rotatable joints (also referred to as "kinematic pairs") connected between the links, and an end effector 125. A set of the multiple joints can be driven by a motor. As an example, combined with... Figure 2 As can be seen, the multiple links form two sets of links connected in parallel. One end of each set of links is connected to the end effector 125, and the joints at the other ends can be defined as a first drive joint 121 and a second drive joint 122. The first drive joint 121 is connected to a first motor so that it can be driven to rotate by the power provided by the first motor, and the second drive joint 122 is connected to a second motor so that it can be driven to rotate by the power provided by the second motor. The joints between the multiple links in each set of links can be driven to rotate by the rotation of the first drive joint 121 and the second drive joint 122 via the connected links, and therefore can be defined as driven joints. For example, Figure 2 The left-hand group of links contains two links connected by a first driven joint 123, while the right-hand group of links contains two links connected by a second driven joint 124. Figure 1 Based on the four links and one link between the two drive joints, it can be known that the multi-degree-of-freedom linkage mechanism 120 is a two-degree-of-freedom five-bar linkage mechanism.

[0053] It is understandable that the rotation of the first motor and the second motor can drive the first drive joint 121 and the second drive joint 122 to rotate to different angle combinations, thereby driving the connected linkage and driven joint to move the end effector 125 to different positions in the motion plane. By establishing a Cartesian coordinate system of XY in the motion plane, each position reached by the end effector 125 can be represented by (x, y) coordinates.

[0054] It is worth mentioning that, in this embodiment, the end effector 125 belongs to the upper limb rehabilitation training device 100, and provides a gripping part that can contact the patient's hand and receive or apply force to the patient's hand. In actual training, the end effector 125 can move passively under the patient's force. During this process, the first motor and the second motor can provide a certain resistance to the end effector 125 according to the settings to achieve the training purpose for the patient. Alternatively, the first motor and the second motor can provide driving force to the end effector 125 so that the end effector 125 can drive the patient's hand to the target position.

[0055] Optionally, the rehabilitation training device 100 may further include a display screen 130 for displaying image content related to rehabilitation training. The image plane containing the image content may include virtual positions with a mapping relationship model between the virtual position and the physical position of the end effector 125 in the motion plane. The virtual positions may have indicative patterns that change accordingly with changes in the physical position, so that the image content presents an interactive result that dynamically changes in response to the patient's hand movements, thereby increasing the entertainment value of rehabilitation training. In some embodiments, the image content is presented in forms including but not limited to games or educational activities.

[0056] Figure 1 and Figure 2 This document merely illustrates a structural example of a two-degree-of-freedom five-bar parallel multi-degree-of-freedom linkage mechanism 120, where no translation occurs at the two drive joints, thus facilitating an intuitive understanding of the principles in the subsequent description. However, it should be noted that the structural type of the multi-degree-of-freedom linkage mechanism 120 is not limited to this.

[0057] like Figure 3 The diagram shown illustrates the structure of a control system for a multi-degree-of-freedom linkage mechanism according to an embodiment of this disclosure.

[0058] The control system includes a set of motors 310 and a driver 320. The set of motors 310 is used to drive the movement of a set of joints in a multi-degree-of-freedom linkage mechanism, such as... Figure 2 The two motors 310 drive the movement of the two joints.

[0059] The driver 320 includes a multi-channel motor drive circuit 321 and a processor 322.

[0060] A set of motor drive circuits 321 are respectively connected to and control a set of motors 310. Specifically, the motor drive circuits 321 and motors 310 are connected in a one-to-one correspondence. For example, this embodiment shows a first motor and a second motor, and there are two corresponding motor drive circuits 321, namely the first motor drive circuit and the second motor drive circuit.

[0061] In some embodiments, the motor drive circuit 321 can be classified into various types according to control requirements and topology, and different implementation methods can be selected based on the type of motor 310, control function, or circuit topology. For example, brushed DC motor (BDC) drives typically use H-bridge circuits to achieve bidirectional speed regulation. Brushless DC motors (BLDC) and AC servo motors use three-phase inverter bridge drives, switching the current direction through commutation logic.

[0062] The processor 322 is communicatively connected to a group of motor drive circuits 321. The processor 322 outputs control signals to the motor drive circuits 321 to control the motor drive circuits 321 to output corresponding motor drive signals to the motor 310, thereby controlling the motor 310 to rotate according to the parameters indicated by the control signals. In some embodiments, the control signal is a pulse width modulation (PWM) signal, providing signal parameters such as frequency and duty cycle for the motor 310 drive signal; in the figure, the control signal is shown as PWM(S). The motor 310 drive signal is a PWM signal with corresponding signal parameters, which is amplified to obtain energy; in the figure, the motor 310 drive signal is shown as PWM(E).

[0063] For example, the motor drive circuit 321 may also be provided with a current sampling circuit (not shown) to sample the actual drive current in the motor drive circuit 321 and obtain the current value through an analog-to-digital converter (AD). The current value of the current signal is related to the power of the motor 310, and thus to the torque of the motor 310.

[0064] In some embodiments, the current sampling circuit can be implemented based on one of the following: a Hall sensor, a current sampling resistor, a differential amplifier circuit, or a current detection chip. The Hall sensor obtains an induced signal related to the sampled current through electromagnetic induction to further calculate the current value, representing a non-contact measurement method. The current sampling resistor is connected in the loop, and the current value is calculated based on the voltage across the resistor and its resistance. The differential amplifier circuit works by simultaneously amplifying the voltage signal across the sampling resistor and calculating the difference to obtain an output signal proportional to the sampled current, thereby calculating a precise current value. The advantage of using a differential amplifier circuit is that it can eliminate errors caused by floating voltage across the sampling resistor, accurately sampling the current signal.

[0065] The processor 322 is connected to the current sampling circuit to update the signal parameters of the output control signal, such as duty cycle and frequency, based on the feedback current value.

[0066] The processor 322, motor drive circuit 321, and encoder (not shown) of motor 310 are connected via a communication line (identified by "code" in the diagram). The encoder of motor 310 can measure the operating parameters of motor 310, including actual rotation angle and actual speed. Both the encoder and processor 322 operate at high frequency, and their communication connection enables high-speed signal transmission. For example, the processor 322 and the encoder of servo motor 310 can transmit data in parallel or serial mode via a high-speed serial interface (such as SSI, BiSS, EnDat). This allows the processor 322 to obtain the actual rotation angle and actual speed of motor 310 at high speed through communication with the encoder.

[0067] In this embodiment, the processor 322 integrates a position loop controller and a speed loop controller corresponding to the PI control of each motor 310. This allows for real-time analysis of the actual position (i.e., actual rotation angle) and actual speed of each joint within the same processor 322, ensuring calculations are based on the motion state of the linkage mechanism at the same moment.

[0068] The processor 322 can calculate control signals locally based on the actual rotation angle and actual speed. Compared with the traditional solution where the host computer obtains the actual rotation angle and actual speed through CAN bus or conventional industrial Ethernet and then issues commands, it can significantly improve communication speed and break through communication bottlenecks.

[0069] Alternatively, the processor 322 may also provide terminals for connecting sensors, for communicative connection with sensors disposed in the multi-degree-of-freedom linkage mechanism. Further, the sensor may be connected to the processor 322 via a signal amplifier. For example, the end effector may be equipped with a force sensor, and the processor 322 may be communicatively connected to the force sensor. Of course, since the processor 322 and the force sensor may need to be connected through an existing communication network, high-frequency, high-speed communication is not possible; therefore, the force sensor can be used for low-frequency force feedback. For another example, the motor 310 may be equipped with a temperature sensor, and the processor 322 may be connected to the temperature sensor to adjust the power of the motor 310 based on the operating temperature of the motor 310 obtained from the temperature sensor. Additionally, the processor 322 may also determine whether the actual current value obtained from the current sampling circuit indicates that the torque of the motor 310 is too high; if so, it may stop the continued rotation of the motor 310, etc.

[0070] like Figure 4 The diagram shows a functional unit schematic of processor 322 in one embodiment of the present disclosure.

[0071] The processor 322 includes a communication interface 3220, a storage unit 3221, a processing unit 3222, a position ring control unit 3223, and a speed ring control unit 3224.

[0072] The storage unit 3221 is used to store the mapping relationship model 3226. The mapping relationship model 3226 includes the forward kinematics model 32261 and the static model 32262 of the linkage mechanism. The forward kinematics model 32261 contains the mapping relationship between the set of joint positions and joint velocities of each joint and the end position and end velocity of the end effector. The static model 32262 contains the mapping relationship between the target joint torque set of each joint and the end force of the end effector.

[0073] In some embodiments, the mapping model 3226 is obtained based on the physical model of the multi-degree-of-freedom linkage mechanism. It is understood that, due to the physical constraints of the multi-degree-of-freedom linkage mechanism, the force, speed, and position of the end effector can be uniquely determined based on the torque, speed, and angle of a set of motors 310. Therefore, by modeling a physical model that accurately describes the aforementioned physical constraints of the multi-degree-of-freedom linkage mechanism, the mapping model 3226 can be obtained.

[0074] In some embodiments, the mapping model 3226 may include the forward and reverse relationships between the parameters of the motor 310 and the end effector. For example, at least a set of joint torques of the motor 310 can be obtained by reverse mapping the force of the end effector in the mapping model 3226, and a set of current values ​​of the drive current of the motor 310 can be obtained from the set of joint torques. Alternatively, the force of the end effector can be obtained by forward mapping the set of joint torques.

[0075] The processing unit 3222 is used to obtain the actual joint position set and the actual joint speed set of each of the motors based on the operating parameters of each motor. The operating parameters include the actual rotation angle corresponding to the actual joint position and the actual rotation speed corresponding to the actual joint speed, which can be obtained from the encoder of the motor 310. The processing unit 3222 maps the actual joint position set and the actual joint speed set to the actual end effector position and actual end effector speed in the operating space using the forward kinematic model 32261 of the linkage mechanism. In some embodiments, the actual end effector position can be represented as the actual position coordinates of the end effector in the operating space (e.g., a Cartesian coordinate system).

[0076] The position loop control unit 3223 manages the "position loop" corresponding to each of the motors 310. A position loop refers to the feedback loop in a closed-loop feedback system used to control the position of the motor 310. The position loop control unit 3223 calculates the target end-effector speed in the operating space based on the deviation between the actual end-effector position and the received target end-effector position. Here, the actual end-effector position is the current value, and the target position is the set target value. The target speed is calculated based on the deviation between the two to ensure the end-effector moves at the target speed to eliminate the deviation of the current position. In some embodiments, the position loop has a position loop PI function for calculating the speed based on the position deviation.

[0077] The speed loop control unit 3224 manages the "speed loop" corresponding to each of the motors 310. A speed loop refers to the feedback loop in a closed-loop feedback system used to control the speed of the motor 310. The speed loop control unit 3224 determines the target end effector force in the operating space based on the deviation between the actual end speed and the target end speed. Here, the actual speed is the current value, and the target speed is the set target value. The target end effector force is calculated based on the deviation between the two, aiming to adjust the end effector to apply the target end effector force to eliminate the deviation in the current speed. In some embodiments, the speed loop has a speed loop PI function for calculating the force based on the speed deviation.

[0078] The processing unit 3222 is further configured to inversely map the target end effector into target joint torques for each joint using the static model 32262 of the linkage mechanism, and obtain the target current value for each motor based on the target joint torques. In some embodiments, the motors driving each joint may have torque constants, which describe the conversion relationship between the driving current value and torque of the motor. Thus, using the torque constants, the target joint torques can be converted into target current values.

[0079] Additionally, the driver may be configured with a current loop control unit 3225 for managing the "current loop" corresponding to each of the motors 310. A current loop refers to the feedback loop in a closed-loop feedback system used to control the drive current of the motor 310 (i.e., the torque of the motor 310). Exemplarily, the current loop control unit 3225 is integrated into the processor 322. Alternatively, in other embodiments, it may be located outside the processor 322. The current loop control unit 3225 can be input with the target current value as a setpoint, controlling the actual output current of the motor to tend towards the target current value to achieve motion control of the end effector. Here, the actual current value is the current value, and the target current value is the set target value; an updated control signal is output based on the deviation between the two. The purpose is to adjust the torque of the motor 310 to eliminate torque deviation, i.e., to eliminate the deviation of the actual force acting on the end effector relative to the target end effector. Each motor drive circuit 321 outputs a corresponding motor drive signal based on the updated control signal to drive its connected motor 310 to perform corresponding actions, enabling the end effector to be driven by the motor 310 via a multi-degree-of-freedom linkage mechanism to eliminate the positional deviation. Exemplarily, the control and drive signals are implemented as PWM signals, such as PWM(S) and PWM(E) shown in the figures. In some embodiments, the current loop has a current loop PI function for generating control signals based on the current value deviation.

[0080] Unlike solutions in related technologies, in this embodiment, the target current value is obtained by inverse mapping of the target end effector through the static model. Specifically, the target current value is obtained through the process of "target position → position deviation → target velocity → velocity deviation → target end effector → target torque → target current value". The target position is a virtual equivalent ideal value. Under the action of the precise mapping relationship model 3226, the target end effector is also a virtual equivalent ideal value, and the target current value mapped from the target end effector is also a virtual equivalent ideal value. This achieves "direct" control of the end effector through virtual equivalence, without considering the position change of the motor 310.

[0081] Therefore, the host computer only needs to provide an input of the target position. The above calculation process is completed by the high-speed processor 322, which directly controls the motor 310 to make the end effector move into position. This breaks through the communication rate limit and control level limit in related technologies, meets the high dynamic response control requirements of multi-degree-of-freedom linkage mechanisms, reduces the burden on the host computer, and improves the robustness of the system.

[0082] In some alternative embodiments, the target position and / or target velocity are represented as multiple components along multiple degrees of freedom to decouple control between multiple degrees of freedom. For example, in two degrees of freedom, the target position is represented as (Px, Py), and the target velocity is represented as (Vx, Vy). Before forming the control signal, the deviation can be calculated separately for each degree of freedom to obtain the control quantity (i.e., the output of the corresponding loop), and finally the target current value is formed and input into the current loop, thereby simplifying the calculation process.

[0083] In some embodiments, the processor 322 is implemented as an embedded processor 322, including a microprocessor 322 (MCU), a digital signal processing unit 3222 (DSP), or a system-on-a-chip (SoC). Optionally, the processor 322 and the motor drive circuit 321 are integrated in the same driver 320, located on the same or different circuit boards, achieving a high degree of integration and modularization, and simplifying the structure of the control system of the motor 310.

[0084] In some embodiments, the position loop control unit 3223 and the velocity loop control unit 3224 may be implemented as hardware circuit units integrated into the processor 322, or they may be implemented as virtual units implemented by the processor 322 running program instructions. As an example, the position loop control unit and / or the velocity loop control unit may be implemented as a proportional-integral controller or a proportional-integral-derivative controller. The current loop control unit 3225 may be implemented as a hardware circuit unit integrated into the processor 322, or as a virtual unit implemented by the processor 322 running program instructions. In other embodiments, the current loop control unit 3225 may also be located outside the processor 322.

[0085] In some embodiments, a virtual control unit is formed among the processing unit 3222, the position loop control unit 3223, and the speed loop control unit 3224, for periodically executing the process from the operating parameters of each of the motors 310 to obtaining the target current value according to a first control cycle. In some embodiments, the current loop control unit operates with a second control cycle. The second control cycle is less than or equal to the first control cycle. For example, the frequency of the first control cycle is not less than 16kHz, and the frequency of the second control cycle is not less than 48kHz. This allows for the control of the force applied to a high-speed end effector via low-speed commands. The high-frequency operation of the current loop control unit enables rapid adjustment of the PWM duty cycle, forcing the actual motor current to accurately and quickly track the target current, thereby achieving precise execution of the desired force applied to the end effector.

[0086] It is worth mentioning that, in the embodiments disclosed herein, the current loop control unit realizes torque control of the motor 310 based on the input target current value, that is, it realizes control of the force of the end effector. In effect, the current loop is "virtually equivalent" to a "force controller" that directly controls the end effector, thereby breaking through the bandwidth limitation of the original PI control.

[0087] This disclosure also provides a motion device, which includes a multi-degree-of-freedom linkage mechanism, including multiple joints and an end effector; at least two motors connected to and driving at least two of the joints; and a high-frequency control system for the multi-degree-of-freedom linkage mechanism described in the previous embodiments, which is communicatively connected to a group of the motors.

[0088] In some alternative embodiments, the motion device can be implemented as a rehabilitation training robot, such as an upper / lower limb rehabilitation training robot, or more specifically... Figure 1 and Figure 2 The present invention relates to rehabilitation training robots. Further, the actuator and its control system for the multi-degree-of-freedom linkage mechanism are also applicable to planar or multi-dimensional linkage mechanisms such as five-bar and four-bar linkages with redundant drive or dual-input characteristics, particularly suitable for industrial robots with heavy-duty end-effectors, precision assembly equipment, or rehabilitation exoskeleton systems. Of course, it is understood that the motion device can also be other types of equipment using multi-degree-of-freedom linkage mechanisms, such as industrial equipment and engineering machinery, and is not limited to the examples described above.

[0089] like Figure 5The diagram illustrates a flowchart of a high-frequency control method for a multi-degree-of-freedom linkage mechanism according to an embodiment of this disclosure. The multi-degree-of-freedom linkage mechanism includes multiple joints and an end effector, with one group of joints driven by a set of motors. This high-frequency control method for the multi-degree-of-freedom linkage mechanism can be applied to the high-frequency control system for multi-degree-of-freedom linkage mechanisms in previous embodiments. Specifically, it can be executed by a processor in the driver. As an example, the processor can execute the control method by running program instructions, or it can execute the control method in conjunction with other connected / integrated hardware circuits.

[0090] exist Figure 5 In this context, the control method may include the following flow:

[0091] Step S501: Based on the operating parameters of each motor, obtain the actual joint position set and the actual joint speed set of each joint; wherein, the operating parameters include the actual rotation angle corresponding to the actual joint position and the actual rotation speed corresponding to the actual joint speed.

[0092] Step S502: The processing unit maps the actual joint position set and the actual joint velocity set to the actual end position and actual end velocity of the end effector in the operating space through the forward kinematic model of the linkage mechanism.

[0093] Step S503: The position loop control unit obtains the target end speed of the end effector in the operating space based on the deviation between the actual end position and the received target end position.

[0094] Step S504: The speed loop control unit obtains the target end effector in the operating space based on the deviation between the actual end speed and the target end speed.

[0095] Step S505: The processing unit reverse maps the equivalent force of the target end to the target joint torque of each joint through the static model of the linkage mechanism.

[0096] Step S506: Obtain the target current value of each motor based on the target joint torque.

[0097] Step S507: The target current values ​​are used as set values ​​and input to the current loop control unit to control the actual output current of the motor to tend towards the target current value, thereby realizing motion control of the end effector.

[0098] In some embodiments, a virtual control unit is formed among the processing unit, the position loop control unit, and the speed loop control unit to periodically execute the process from the operating parameters of each motor to obtaining the target current value according to a first control cycle; the current loop control unit operates in a second control cycle; the second control cycle is less than or equal to the first control cycle.

[0099] In some embodiments, the frequency of the first control cycle is not less than 16kHz, and the frequency of the second control cycle is not less than 48kHz. The current loop control unit operates at high frequency to achieve rapid adjustment of the PWM duty cycle, forcing the actual motor current to accurately and quickly track the target current, thereby achieving precise execution of the desired force on the end effector.

[0100] In some embodiments, the position loop control unit and / or velocity loop control unit are implemented as a proportional-integral controller or a proportional-integral-derivative controller.

[0101] It is understandable that, due to the physical constraints of the multi-degree-of-freedom linkage mechanism, the force, speed, and position of the end effector can be uniquely determined based on the torque, speed, and angle of a set of motors. Therefore, by modeling a physical model that accurately describes the aforementioned physical constraints of the multi-degree-of-freedom linkage mechanism, the mapping relationship model can be obtained. The mapping relationship model includes the forward kinematics model and the static model of the linkage mechanism.

[0102] In some embodiments, the mapping model may include the forward and reverse relationships between the parameters of the motor and the end effector. For example, at least a set of joint torques of the motor can be obtained by inversely mapping the force of the end effector into the mapping model, and a set of current values ​​of the drive current of the motor can be obtained from the set of joint torques. Alternatively, the force of the end effector can be obtained from the set of joint torques in a forward manner.

[0103] based on Figure 3 As can be seen from the architecture, the driver can obtain the actual rotation angle and actual speed from the encoders of each motor, thereby quickly and accurately obtaining the actual position and actual speed of the end effector through the mapping relationship model. In some embodiments, the driver can communicate with a host computer through a communication network to receive instructions from the host computer and extract the target position from it.

[0104] In some embodiments, the position loop control unit manages a "position loop" corresponding to each motor. A position loop refers to a feedback loop in a closed-loop feedback system used to control the motor position. The position loop control unit calculates the target end-effector speed in the operating space based on the deviation between the actual end-effector position and the received target end-effector position. Here, the actual end-effector position is the current value, and the target position is the set target value. The target speed is calculated based on the deviation between the two to ensure the end-effector moves at the target speed to eliminate the deviation in the current position. In some embodiments, the position loop has a position loop PI function for calculating the speed based on the position deviation.

[0105] In some embodiments, the speed loop control unit manages a "speed loop" corresponding to each motor. A speed loop refers to the feedback loop in a closed-loop feedback system used to control the motor speed. The speed loop control unit determines the target end effector force of the end effector in the operating space based on the deviation between the actual end speed and the target end speed. Here, the actual speed is the current value, and the target speed is the set target value. The target end effector force is calculated based on the deviation between the two, aiming to adjust the end effector to apply the target end effector force to eliminate the deviation in the current speed. In some embodiments, the speed loop has a speed loop PI function for calculating the force based on the speed deviation.

[0106] Unlike solutions in related technologies, in this embodiment, the target current value is obtained by inverse mapping of the target end effector through the mapping relationship model. Specifically, the processing unit is used to inversely map the target end effector into the target joint torque of each joint using the static model of the linkage mechanism, and further obtain the target current value of each motor based on the target joint torque.

[0107] In some embodiments, the current loop control unit can be input with the target current value as a setpoint, controlling the actual output current of the motor to tend towards the target current value to achieve motion control of the end effector. Here, the actual current value is the current value, and the target current value is the set target value. An updated control signal is output based on the deviation between the two. The purpose is to adjust the motor torque to eliminate torque deviation, that is, to eliminate the deviation of the actual force acting on the end effector relative to the target end effector. Each motor drive circuit outputs a corresponding control signal to drive its connected motor to perform corresponding actions based on the updated control signal, enabling the end effector to be driven by the motor via a multi-degree-of-freedom linkage mechanism to eliminate the positional deviation. Exemplarily, the control signal and drive signal are implemented as PWM signals, such as PWM(S) and PWM(E) shown in the figure.

[0108] Unlike solutions in related technologies, in this embodiment, the target current value is obtained by inverse mapping of the target end effector through the static model. Specifically, the target current value is obtained through the process of "target position → position deviation → target velocity → velocity deviation → target end effector → target torque → target current value". The target position is a virtual equivalent ideal value. Under the action of the precise mapping relationship model 3226, the target end effector is also a virtual equivalent ideal value, and the target current value mapped from the target end effector is also a virtual equivalent ideal value. This achieves "direct" control of the end effector through virtual equivalence, without considering the position change of the motor 310.

[0109] Therefore, the host computer only needs to provide an input of the target position. All the above calculations are completed by a high-speed processor, which directly controls the motor to move the end effector into position. This breaks through the communication rate and control level limitations in related technologies, meets the high dynamic response control requirements of multi-degree-of-freedom linkage mechanisms, reduces the burden on the host computer, and improves the robustness of the system.

[0110] Therefore, this embodiment aims to overcome the aforementioned deficiencies of the prior art and provide a high-frequency control method for multi-degree-of-freedom linkage mechanisms. This method "virtualizes" the control effect of a high-bandwidth current loop and directly maps it to the mechanical end effector, constructing a fast control channel from the equivalent physical quantity at the end effector to the motor current, thereby achieving direct, decoupled, high-frequency control of the end effector.

[0111] like Figure 6 The diagram illustrates a module schematic of a control device for a multi-degree-of-freedom linkage mechanism according to an embodiment of this disclosure. It should be noted that the principle and technical implementation of the control device can be referenced from previous embodiments of the control method and the principles in the driver embodiment; therefore, they will not be repeated in this embodiment. The multi-degree-of-freedom linkage mechanism includes multiple joints and an end effector, with one group of joints driven by a group of motors. The high-frequency control method for the multi-degree-of-freedom linkage mechanism can be applied to the high-frequency control system for the multi-degree-of-freedom linkage mechanism in previous embodiments. Specifically, it can be executed by the processor in the driver. As an example, the control device for the multi-degree-of-freedom linkage mechanism can be implemented in the processor of the driver.

[0112] The control device 600 for the multi-degree-of-freedom linkage mechanism includes:

[0113] The acquisition module 601 is used to obtain the actual joint position set and the actual joint speed set of each of the motors based on the operating parameters of each motor. The operating parameters include the actual rotation angle corresponding to the actual joint position and the actual rotational speed corresponding to the actual joint speed.

[0114] The mapping processing module 602 is used to map the actual joint position set and the actual joint velocity set to the actual end position and actual end velocity of the end effector in the operating space through the positive kinematic model of the linkage mechanism.

[0115] The position loop control module 603 is used to obtain the target end speed of the end effector in the operating space based on the deviation between the actual end position and the received target end position through the position loop control unit.

[0116] The speed loop control module 604 is used to obtain the target end effect of the end effector in the operating space based on the deviation between the actual end speed and the target end speed through the speed loop control unit.

[0117] The mapping processing module 602 is used to reverse map the target end effect into the target joint torque of each joint through the static model of the linkage mechanism.

[0118] The torque and current calculation module 605 is used to obtain the target current value of each motor based on the target joint torque.

[0119] The current loop control module 606 is used to input the target current values ​​as set values ​​to the current loop control unit to control the actual output current of the motor to tend towards the target current value, thereby realizing motion control of the end effector.

[0120] In some embodiments, the mapping processing module 602, the position loop control unit, and the speed loop control unit form a virtual control unit for periodically executing the process from the operating parameters of each motor to obtaining the target current value according to a first control cycle; the current loop control unit operates in a second control cycle; the second control cycle is less than or equal to the first control cycle.

[0121] In some embodiments, the frequency of the first control cycle is not less than 16 kHz, and the frequency of the second control cycle is not less than 48 kHz.

[0122] In some embodiments, the position loop control unit and / or velocity loop control unit are implemented as a proportional-integral controller or a proportional-integral-derivative controller.

[0123] It is understandable that, due to the physical constraints of the multi-degree-of-freedom linkage mechanism, the force, speed, and position of the end effector can be uniquely determined based on the torque, speed, and angle of a set of motors. Therefore, by modeling a physical model that accurately describes the aforementioned physical constraints of the multi-degree-of-freedom linkage mechanism, the mapping relationship model can be obtained. The mapping relationship model includes the forward kinematics model and the static model of the linkage mechanism.

[0124] In some embodiments, the mapping model may include the forward and reverse relationships between the parameters of the motor and the end effector. For example, at least a set of joint torques of the motor can be obtained by inversely mapping the force of the end effector into the mapping model, and a set of current values ​​of the drive current of the motor can be obtained from the set of joint torques. Alternatively, the force of the end effector can be obtained from the set of joint torques in a forward manner.

[0125] based on Figure 3 As can be seen from the architecture, the driver can obtain the actual rotation angle and actual speed from the encoders of each motor, thereby quickly and accurately obtaining the actual position and actual speed of the end effector through the mapping relationship model. In some embodiments, the driver can communicate with a host computer through a communication network to receive instructions from the host computer and extract the target position from it.

[0126] In some embodiments, the position loop control unit manages a "position loop" corresponding to each motor. A position loop refers to a feedback loop in a closed-loop feedback system used to control the motor position. The position loop control unit calculates the target end-effector speed in the operating space based on the deviation between the actual end-effector position and the received target end-effector position. Here, the actual end-effector position is the current value, and the target position is the set target value. The target speed is calculated based on the deviation between the two to ensure the end-effector moves at the target speed to eliminate the deviation in the current position. In some embodiments, the position loop has a position loop PI function for calculating the speed based on the position deviation.

[0127] In some embodiments, the speed loop control unit manages a "speed loop" corresponding to each motor. A speed loop refers to the feedback loop in a closed-loop feedback system used to control the motor speed. The speed loop control unit determines the target end effector force of the end effector in the operating space based on the deviation between the actual end speed and the target end speed. Here, the actual speed is the current value, and the target speed is the set target value. The target end effector force is calculated based on the deviation between the two, aiming to adjust the end effector to apply the target end effector force to eliminate the deviation in the current speed. In some embodiments, the speed loop has a speed loop PI function for calculating the force based on the speed deviation.

[0128] Unlike solutions in related technologies, in this embodiment, the target current value is obtained by inverse mapping of the target end effector through the mapping relationship model. Specifically, the processing unit is used to inversely map the target end effector into the target joint torque of each joint using the static model of the linkage mechanism, and further obtain the target current value of each motor based on the target joint torque.

[0129] In some embodiments, the current loop control unit can be input with the target current value as a setpoint, controlling the actual output current of the motor to tend towards the target current value to achieve motion control of the end effector. Here, the actual current value is the current value, and the target current value is the set target value. An updated control signal is output based on the deviation between the two. The purpose is to adjust the motor torque to eliminate torque deviation, that is, to eliminate the deviation of the actual force acting on the end effector relative to the target end effector. Each motor drive circuit outputs a corresponding control signal to drive its connected motor to perform corresponding actions based on the updated control signal, enabling the end effector to be driven by the motor via a multi-degree-of-freedom linkage mechanism to eliminate the positional deviation. Exemplarily, the control signal and drive signal are implemented as PWM signals, such as PWM(S) and PWM(E) shown in the figure.

[0130] Unlike solutions in related technologies, in this embodiment, the target current value is obtained by inverse mapping of the target end effector through the static model. Specifically, the target current value is obtained through the process of "target position → position deviation → target velocity → velocity deviation → target end effector → target torque → target current value". The target position is a virtual equivalent ideal value. Under the action of the precise mapping relationship model, the target end effector is also a virtual equivalent ideal value, and the target current value mapped from the target end effector is also a virtual equivalent ideal value. This achieves "direct" control of the end effector through virtual equivalence, without considering changes in the motor's position.

[0131] Therefore, the host computer only needs to provide an input of the target position. All the above calculations are completed by a high-speed processor, which directly controls the motor to move the end effector into position. This breaks through the communication rate and control level limitations in related technologies, meets the high dynamic response control requirements of multi-degree-of-freedom linkage mechanisms, reduces the burden on the host computer, and improves the robustness of the system.

[0132] Therefore, this embodiment aims to overcome the aforementioned deficiencies of the prior art and provide a high-frequency control method for multi-degree-of-freedom linkage mechanisms. This method "virtualizes" the control effect of a high-bandwidth current loop and directly maps it to the mechanical end effector, constructing a fast control channel from the equivalent physical quantity at the end effector to the motor current, thereby achieving direct, decoupled, high-frequency control of the end effector.

[0133] It should be noted that, in Figure 6 The various functional modules in the embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any combination thereof. When implemented in software, they can be implemented, in whole or in part, in the form of a computer program or instruction product. A computer program or instruction product includes one or more computer programs or instructions. When a computer program or instruction is loaded and executed on a computer, it produces, in whole or in part, the flow or function according to this disclosure. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer program or instructions can be stored in a computer-readable storage medium or transferred from one computer-readable storage medium to another.

[0134] and, Figure 6 The apparatus disclosed in the embodiments can be implemented through other modular division methods. The apparatus embodiments shown above are merely illustrative. For example, the module division is only a logical functional division, and in actual implementation, there may be other division methods. For example, a group of modules or modules may be combined or dynamically integrated into another system, or some features may be ignored or not executed. Furthermore, the shown or discussed mutual coupling, direct coupling, or communication connection may be through some interfaces, and the indirect coupling or communication connection between devices or modules may be electrical or other forms.

[0135] in addition, Figure 6The functional modules and sub-modules in the embodiments can be dynamically integrated within a single processing unit, or each module can exist physically independently, or two or more modules can be dynamically integrated within a single unit. These dynamic units can be implemented in hardware or as software functional modules. If these dynamic units are implemented as software functional modules and sold or used as independent products, they can also be stored in a computer-readable storage medium. This storage medium can be a read-only memory, a hard disk, or an optical disk, etc.

[0136] It should be specifically noted that the flowchart representations of the embodiments described above in this disclosure can be understood as representing modules, segments, or portions of code comprising one or more sets of executable instructions configured to implement specific logical functions or processes. Furthermore, the scope of the preferred embodiments of this disclosure includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved.

[0137] For example, Figure 5 The order of the steps in the method embodiment may vary in specific scenarios and is not limited to the above representation.

[0138] To more intuitively illustrate the principle of the solution in the embodiments of this disclosure, please refer to... Figure 7 The diagram shown illustrates the principle of multi-degree-of-freedom linkage mechanism control in one embodiment of this disclosure.

[0139] exist Figure 7 In this process, based on the obtained operating parameters of the motors of each joint (two in this example), the actual rotation angle set {θ1, θ2} and the actual speed set {w1, w2} of each motor are obtained.

[0140] The virtual actual end position (px, py) is obtained by mapping the actual rotation angle set {θ1, θ2}. This virtual end position, along with the user-input target end position (ptx, pty), is input to the position loop control unit. Based on their deviation, the virtual target end velocity (Vtx, Vty) is output. Similarly, the virtual actual end velocity (Vx, Vy) is obtained by mapping the actual rotation speed set {w1, w2}. The target end velocity (Vtx, Vty) and the actual end velocity (Vx, Vy) are input to the speed loop control unit. Based on their deviation, the virtual target end equivalent velocity (Ftx, Fty) is output.

[0141] Based on the torque constant, the target end-effector equivalent force (Ftx, Fty) is converted into the target current value set {It1, It2} for each motor. Furthermore, the actual current value set {I1, I2} and the target current value set {It1, It2} of each joint are input into the current loop control unit, which outputs a control signal PWM(S) for a group of motors based on the deviation between the actual current value and the target current value of each motor.

[0142] like Figure 8 The diagram shows a schematic representation of an embedded system according to an embodiment of the present disclosure.

[0143] In this embodiment, the embedded system can be applied to the driver in the previous embodiments of this disclosure, which includes the processor.

[0144] The embedded system 800 includes a bus 801, a processor 322, and a memory 803. The processor 322 and the memory 803 can communicate via the bus 801. The memory 803 can store computer programs or instructions. The processor 322 implements the method flow or function described in the previous embodiments by executing the program instructions stored in the memory 803, for example... Figure 5 The method and process in the process.

[0145] Bus 801 can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. Buses can be categorized as address buses, data buses, control buses, etc. For ease of representation, although only one thick line is used in the diagram, this does not indicate that there is only one bus or one type of bus.

[0146] In some embodiments, processor 322 may be implemented as a microprocessor unit (MCU), system on chip, or field-programmable array (FPGA). Memory 803 may include volatile memory for temporary data storage during program execution, such as random access memory (RAM).

[0147] The memory 803 may also include non-volatile memory for data storage, such as read-only memory (ROM), flash memory, etc.

[0148] In some embodiments, the embedded system 800 may further include a communicator 804. The communicator 804 is used for communication with external devices. In specific examples, the communicator 804 may include one or more wired and / or wireless communication circuit modules. For example, the communicator 804 may include one or more of, such as a wired network card, a USB module, a serial interface module, etc. The wireless communication protocols followed by the wireless communication module include, for example, Nearfield Communication (NFC) technology, Infrared (IR) technology, Global System for Mobile Communications (GSM), General Packet Radio Service (GPRS), Code Division Multiple Access (CDMA), Wideband Code Division Multiple Access (WCDMA), Time-Division Code Division Multiple Access (TD-SCDMA), Long Term Evolution (LTE), Bluetooth (BT), Global Navigation Satellite System (GNSS), etc., one or more of these.

[0149] This disclosure also provides a computer-readable storage medium storing a computer program or instructions, which, when run, implement the method flow or function of any of the previous embodiments.

[0150] That is, the method steps in the above embodiments are implemented as software or computer code that can be stored in a recording medium (such as CD ROM, RAM, floppy disk, hard disk or magneto-optical disk), or implemented as computer code that is originally stored in a remote recording medium or a non-transitory machine-readable medium and will be stored in a local recording medium after being downloaded via a network, so that the method represented herein can be stored in such software processing on a recording medium using a general-purpose computer, a special processor or programmable or special hardware (such as ASIC or FPGA).

[0151] This disclosure may also provide a computer program product, comprising one or more computer programs or instructions, which, when run, perform all or part of the processes or functions described in this disclosure. The computer program product includes one or more computer programs or instructions.

[0152] Computer programs or instructions can be stored in a readable storage medium or transferred from one readable storage medium to another. For example, the computer program or instructions can be transferred from one website, computer, server, or data center to another website, computer, server, or data center via wired or wireless means. The readable storage medium can be any available medium capable of access, or a data storage device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium, such as a floppy disk, hard disk, or magnetic tape; an optical medium, such as a digital video optical disc; or a semiconductor medium, such as a solid-state drive. The computer-readable storage medium can be a volatile or non-volatile storage medium, or it can include both volatile and non-volatile types of storage media.

[0153] In summary, this disclosure relates to the field of robot motion control technology, providing a high-frequency control method, system, and motion device for multi-degree-of-freedom linkage mechanisms. It forward maps the actual end effector position and velocity of the end effector using a set of actual joint positions and velocities. A position loop determines the target end effector velocity based on the actual and target end effector positions, and a velocity loop determines the target end effector equivalent force based on the deviation between the actual and target end effectors. Based on the static model of the linkage mechanism, the target end effector equivalent force is inversely mapped to the target joint torque of each joint, and the target current value corresponding to each motor is obtained. The current loop then controls the motors based on the target current value to achieve motion control of the end effector. This is equivalent to a virtual equivalent actuator directly controlling the end effector. It overcomes problems such as control hierarchy limitations, communication rate bottlenecks, and insufficient control bandwidth under heavy loads.

[0154] Therefore, the control core is moved down from the host computer to the driver, breaking through the limitations of control hierarchy. The host computer only needs to provide low-frequency target position, no longer relying on host computer control. The host computer only needs to provide low-frequency position trajectory commands, without performing real-time dynamic calculations, reducing the computing power requirements and software complexity of the host computer, and improving system reliability.

[0155] Furthermore, since the entire control process occurs between the driver and the motor, the high-speed processor in the driver enables high-speed processing and communication, overcoming the bottleneck limitation of communication rate. By embedding the control core inside the driver, the virtual loop control cycle can reach tens to hundreds of microseconds (corresponding to several kHz to tens of kHz), completely independent of the relatively low bus communication cycle of the host computer (typically 1 ms), achieving extremely fast response to end-effector dynamics. Through static mapping, the microsecond-level response current loop is directly equivalent to the end-effector force controller, increasing the end-effector force control bandwidth to the current loop level (up to 16 kHz or more), greatly simplifying the control of large inertial loads and contact forces, effectively suppressing vibration, and achieving true high-frequency direct force control.

[0156] Throughout the control algorithm, the controlled variable can always be the decoupled coordinates of the end effector in the operational space (see reference). Figure 7 This is similar to the control of multiple linkages (e.g., X and Y in a two-dimensional plane) and their first and second order quantities. This simplifies complex multi-link coordination control into the control of several independent virtual linear axes, resulting in clear physical meaning, simple parameter tuning, and achieving control decoupling and intuitiveness.

[0157] Furthermore, the above scheme allows the current loop corresponding to each motor to be "virtually equivalent" to the force controller of the end effector. This significantly increases the bandwidth of the PI control for the end effector's force, simplifies the PI control objective, and suppresses vibration and overshoot under large inertial loads. Therefore, regardless of the mechanism's configuration, the controller senses and adjusts only the direct physical quantities of the end effector. This ensures that the dynamic characteristics of the linkage system regarding the closed-loop stiffness and damping of the end effector remain constant within the workspace, resulting in stable and predictable performance and globally consistent control performance.

[0158] The above embodiments are merely illustrative of the principles and effects of this disclosure and are not intended to limit this disclosure. Any person skilled in the art can modify or alter the above embodiments without departing from the spirit and scope of this disclosure. Therefore, all equivalent modifications or alterations made by those skilled in the art without departing from the spirit and technical concept disclosed in this disclosure should still be covered by the protection scope of this disclosure.

Claims

1. A high-frequency control system for a multi-degree-of-freedom linkage mechanism, characterized in that, The multi-degree-of-freedom linkage mechanism includes multiple joints and an end effector, and a group of joints among the multiple joints is driven by a group of motors respectively. The system includes: A multi-channel motor drive circuit, which connects to and controls one group of the motors respectively; The processor is connected to a set of motor drive circuits and includes: a communication interface, a storage unit, a processing unit, a position loop control unit, and a speed loop control unit; The communication interface is used to receive the operating parameters of each motor and the target end position; The storage unit is used to store the mapping relationship model; the mapping relationship model includes: the forward kinematics model of the linkage mechanism, which includes the mapping relationship between the joint position set and joint velocity of each joint and the end position and end velocity of the end effector; the static model of the linkage mechanism, which includes the mapping relationship between the target joint torque set of each joint and the end force of the end effector. The processing unit is used to obtain the actual joint position set and the actual joint velocity set of each joint based on the operating parameters of each motor; wherein, the operating parameters include the actual rotation angle corresponding to the actual joint position and the actual rotation speed corresponding to the actual joint velocity; and through the forward kinematic model of the linkage mechanism, the actual joint position set and the actual joint velocity set are respectively mapped to the actual end position and actual end velocity of the end effector in the operating space. The position loop control unit is used to obtain the target end speed of the end effector in the operating space based on the deviation between the actual end position and the received target end position; The speed loop control unit is used to obtain the target end effector in the operating space based on the deviation between the actual end speed and the target end speed; The processing unit is used to reverse map the target end effector into the target joint torque of each joint using the static model of the linkage mechanism; and to obtain the target current value of each motor based on the target joint torque. The current loop control unit is used to control the actual output current of the motor to tend towards the target current value, thereby achieving motion control of the end effector.

2. The high-frequency control system for a multi-degree-of-freedom linkage mechanism according to claim 1, characterized in that, The processor is communicatively connected to a current sampling circuit, which is configured to sample a set of actual drive currents of the motor drive circuit to obtain actual current values. The current loop control unit is used to update the control signal to the motor based on the deviation between the actual current value and the target current value.

3. The high-frequency control system for a multi-degree-of-freedom linkage mechanism according to claim 1, characterized in that, The processing unit, the position loop control unit, and the speed loop control unit form a virtual control unit, which is used to periodically execute the process from the operating parameters of each motor to obtaining the target current value according to the first control cycle. The current loop control unit operates in a second control cycle; the second control cycle is less than or equal to the first control cycle.

4. The high-frequency control system for a multi-degree-of-freedom linkage mechanism according to claim 3, characterized in that, The frequency of the first control cycle is not less than 16kHz, and the frequency of the second control cycle is not less than 48kHz.

5. The high-frequency control system for a multi-degree-of-freedom linkage mechanism according to claim 1, characterized in that, The position loop control unit and / or velocity loop control unit are implemented as proportional-integral controllers or proportional-integral-derivative controllers.

6. A motion device, characterized in that, include: A multi-degree-of-freedom linkage mechanism, including multiple joints and an end effector; At least two motors are connected to and drive at least two of the joints; The high-frequency control system for a multi-degree-of-freedom linkage mechanism as described in any one of claims 1 to 5 includes a communication connection to a group of motors to obtain operating parameters, and a connection to a host computer to send and receive target end positions.

7. The motion device according to claim 6, characterized in that, The motion device is implemented as an upper / lower limb rehabilitation training robot.

8. A high-frequency control method for a multi-degree-of-freedom linkage mechanism, characterized in that, The multi-degree-of-freedom linkage mechanism includes multiple joints and an end effector, and a group of joints among the multiple joints is driven by a group of motors respectively. The method is applied to a driver that connects a group of the motors; the method includes: Based on the operating parameters of each motor, the actual joint position set and the actual joint speed set of each joint are obtained; wherein, the operating parameters include the actual rotation angle corresponding to the actual joint position and the actual rotation speed corresponding to the actual joint speed; Using the forward kinematics model of the linkage mechanism, the actual joint position set and the actual joint velocity set are respectively mapped to the actual end position and actual end velocity of the end effector in the operating space. The position loop control unit obtains the target end velocity of the end effector in the operating space based on the deviation between the actual end position and the received target end position; The speed loop control unit obtains the target end effector's equivalent force in the operating space based on the deviation between the actual end speed and the target end speed. The static model of the linkage mechanism is used to inversely map the equivalent force of the target end to the target joint torque of each joint. The target current value of each motor is obtained based on the target joint torque; The target current values ​​are used as setpoints and input to the current loop control unit to control the actual output current of the motor to tend toward the target current value, thereby realizing motion control of the end effector.

9. A processor, characterized in that, include: Used to run stored computer programs or instructions to perform the high-frequency control method for the multi-degree-of-freedom linkage mechanism as described in claim 8.

10. A computer-readable storage medium, characterized in that, The system stores a computer program or instructions that are executed to perform the high-frequency control method for the multi-degree-of-freedom linkage mechanism as described in claim 8.

Images