Motion control method, motion controller, computer equipment and storage medium

Abstract

The invention relates to a motion control method, a motion controller, computer equipment and a storage medium. The method comprises the following steps: in response to a starting operation of a motor, determining a current displacement of the motor based on a target speed regulation rule every preset duration; and when the current displacement is increased by one unit compared with the previous displacement, sending a pulse signal to perform motion control of the motor. By adopting the method, sudden change of the motor speed can be avoided.

Description

Technical Field

[0001] The present application relates to the field of motor technology, and in particular to a motion control method, a motion controller, a computer device and a storage medium. Background Art

[0002] The motor controls the rotation of the motor through a pulse signal, and the frequency of the pulse signal can indicate the speed of the motor. When the frequency of the pulse signal is greater than the limit starting frequency of the motor, the motor cannot start normally. In practical applications, the operating frequency of the motor is often greater than the limit starting frequency of the motor. Therefore, the frequency of the pulse signal needs to be accelerated from low speed to high speed. During the acceleration or deceleration process, if the speed change is not smooth, the load driven by the motor will be subjected to uneven force changes during the movement process, and the load will be subjected to a large impact. At the same time, the inertia of the load will also cause the motor to overshoot and lose step. Therefore, the speed, acceleration, and deceleration of the motor during acceleration and deceleration need to be controllable.

[0003] During operation, the motor motion controller receives the parameters configured by the user through the host computer and sends pulse signals of a specific frequency and number according to the target speed and target displacement. The traditional method calculates the speed formula every certain period of time to obtain the frequency of the pulse signal at that moment, and passes this frequency during this period of time. However, the accuracy of this implementation method is affected by the time interval. When the time interval is too large, the speed may change suddenly. Summary of the invention

[0004] Based on this, it is necessary to provide a motion control method, motion controller, computer equipment and storage medium that can avoid sudden changes in motor speed in response to the above technical problems.

[0005] A motion control method, applied to a motion controller, comprising:

[0006] In response to a start-up operation of the motor, determining a current displacement of the motor based on a target speed regulation rule at every preset time interval;

[0007] When the current displacement increases by one unit compared to the previous displacement, a pulse signal is sent to perform motion control of the motor.

[0008] A motion controller is used to implement the steps of each motion control method embodiment.

[0009] A computer device comprises a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of various motion control method embodiments when executing the computer program.

[0010] A computer-readable storage medium stores a computer program, which implements the steps of various motion control method embodiments when executed by a processor.

[0011] The above-mentioned motion control method, motion controller, computer equipment and storage medium respond to the triggering operation of the motor and determine the current displacement of the motor based on the target speed regulation rule at preset time intervals. When the current displacement increases by one unit compared with the previous displacement, a pulse signal needs to be generated, that is, a pulse signal is generated every time the displacement meets the requirement to control the motor. Compared with the traditional method, sudden changes in the motor speed are avoided, and the motor control accuracy is not affected by the time interval. The motor speed change transition is good and the motor control is more stable. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] Figure 1 is an application environment diagram of a motion control method in an embodiment;

[0013] Figure 2 is a schematic flow chart of a motion control method in one embodiment;

[0014] Figure 3 A digital circuit diagram of a trapezoidal speed regulation rule in one embodiment;

[0015] Figure 4 A digital circuit diagram of an S-shaped speed regulation rule in one embodiment;

[0016] Figure 5 A digital circuit diagram of an S-shaped speed regulation rule in another embodiment;

[0017] Figure 6 A digital circuit diagram of a trapezoidal speed regulation rule in another embodiment;

[0018] Figure 7 A digital circuit diagram of an S-shaped speed regulation rule in another embodiment;

[0019] Figure 8 FIG. 4 is a diagram showing the internal structure of a computer device in one embodiment. DETAILED DESCRIPTION

[0020] It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0021] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0022] It should be noted that all directional indications in the embodiments of the present application (such as up, down, left, right, front, back, etc.) are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly. The connection can be a direct connection or an indirect connection.

[0023] In addition, in this application, descriptions such as "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of technical features indicated. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In addition, the technical solutions between the various embodiments can be combined with each other, but they must be based on the ability of ordinary technicians in this field to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by this application.

[0024] The terms "first", "second", etc. used in this application may be used herein to describe various elements, but these elements are not limited by these terms. These terms are only used to distinguish a first element from another element. For example, without departing from the scope of this application, a first accumulator may be referred to as a second accumulator, and similarly, a second accumulator may be referred to as a first accumulator. Both the first accumulator and the second accumulator are accumulators, but they are not the same accumulator.

[0025] It can be understood that the “connection” in the following embodiments should be understood as “electrical connection”, “communication connection”, etc. if the connected circuits, modules, units, etc. have electrical signals or data transmission between each other.

[0026] The motor control method provided in this application can be applied to Figure 1 application environment. Figure 1 1 is an application environment diagram of a motion control method in an embodiment. It includes a motion controller 110, a motor driver 120 and a motor 130. The motor 130 can specifically be a stepping motor, etc. The speed in the signal in each embodiment of the present application can refer to the frequency of the pulse signal, in Hertz. The displacement represents the number of signals, that is, when the displacement is 1, a pulse signal needs to be sent. The motion controller 110 outputs a pulse signal (displacement) and a direction signal to the motor driver 120, and the motor driver 120 outputs a current to the motor 130.

[0027] In one embodiment, Figure 2 FIG. 1 is a flow chart of a motion control method in an embodiment, which is applied to a motion controller as an example and includes the following steps:

[0028] Step 202 : In response to the start-up operation of the motor, the current displacement of the motor is determined based on the target speed regulation rule at every preset time period.

[0029] The preset time length is used to indicate how often the displacement of the motor is calculated. Preferably, the displacement within the preset time length is less than one unit of displacement.

[0030] The target speed regulation rule may include a displacement-related speed regulation rule, a speed-related speed regulation rule, or an acceleration-related speed regulation rule, depending on the specific situation.

[0031] The trapezoidal speed regulation rule is a motor speed regulation algorithm that accelerates and decelerates at a fixed acceleration. The S-shaped speed regulation rule is a motor speed regulation algorithm that accelerates and decelerates at an unfixed acceleration. The trapezoidal speed regulation rule and the S-shaped speed regulation rule are suitable for different application scenarios. For example, in some motor processing scenarios with less demanding requirements, the trapezoidal speed regulation rule can be used; in scenarios where the motor needs to run more stably, the S-shaped speed regulation rule can be used.

[0032] Specifically, in response to the start-up operation of the motor, the motor starts to run, and the current displacement of the motor is determined based on the target speed regulation rule every preset time. For example, the displacement of the motor is calculated based on the S-shaped speed regulation rule every 1 μs (microsecond).

[0033] Step 204 , when the current displacement increases by one unit compared to the previous displacement, a pulse signal is sent to control the motion of the motor.

[0034] Here, increasing the displacement by one unit refers to increasing it by a fixed value, for example, the displacement value may be increased by one, or the displacement value may be increased by another value.

[0035] Specifically, taking the preset duration of 1 μs as an example, after unifying the units and converting the unit of displacement s(t) into "units", 10^6 of the duration t is moved to the left side of the equation, and one unit is 10 to the sixth power. The number of pulse signals is used to represent the displacement of the motor.

[0036] Therefore, starting from t=0, time t begins to accumulate in increments of 1μs. Every time 1us is added, a displacement calculation is performed according to the motion parameter value and the target speed control rule. When 10^6*s(t)>=10^6, it indicates that the first pulse signal such as the STEP signal can be generated, and the value of 10^6*s(t0) at the current moment is recorded. The above process is guaranteed to be completed within 1μs. Time t continues to accumulate, and the value of 10^6*s(t) continues to be calculated. When 10^6*s(t)-10^6*s(t0)>=10^6 or 10^6*s(t)=2*10^6, a second pulse signal is generated, and the value of 10^6*s(t1) at the current moment is recorded. Calculations continue in this way to generate pulse signals with speed changes (i.e., frequency changes).

[0037] Optionally, in displacement mode, the target displacement is the key to determining whether the motor stops; therefore, in displacement mode, when the current displacement increases by one unit compared to the previous displacement, a pulse signal is sent until the total displacement of the motor reaches the target displacement, and the pulse signal is stopped. This can effectively combine the specific working mode to send signals, so that the motor can run stably.

[0038] In this embodiment, in response to the triggering operation of the motor, the current displacement of the motor is determined based on the target speed control rule at preset time intervals. When the current displacement increases by one unit compared to the previous displacement, a pulse signal needs to be generated. That is, a pulse signal is generated to control the motor every time the displacement reaches the requirement. Compared with the traditional method, sudden changes in the motor speed are avoided, and the motor control accuracy is not affected by the time interval. The motor speed change transition is good, and the motor control is more stable.

[0039] In one embodiment, the traditional method calculates the frequency of the pulse signal based on the displacement formula. This method requires an inverse operation on the displacement formula, solves the corresponding time according to the displacement, and then substitutes the solved time into the speed formula to finally find the frequency of the pulse signal. The displacement formula is generally a quadratic function, a quartic function, or a higher-order function of time t. When this implementation method is implemented in software, a function can be used to perform an inverse operation, but in the digital circuit implementation process, the digital circuit cannot perform an inverse operation on the displacement formula. By using the method in the present application, the target speed regulation rule can be implemented by a digital circuit.

[0040] Among them, the target speed regulation rules include trapezoidal speed regulation rules and S-shaped speed regulation rules. In the trapezoidal speed regulation rule, speed and time are proportional, and the proportional coefficient is acceleration. In the trapezoidal acceleration and deceleration, after the starting speed, target speed and acceleration and deceleration parameters (taking the acceleration and deceleration time as an example) are determined, the acceleration is a fixed value.

[0041]

[0042] v(t)=kt+v0

[0043]

[0044] Where a is the acceleration, k is the slope, v1 is the target speed, v0 is the starting speed, T is the acceleration or deceleration time, v(t) is the current speed, and s(t) is the current displacement.

[0045] Taking the preset duration of 1μs as an example, after converting the displacement unit into "pieces", the formula is:

[0046]

[0047] To improve the calculation accuracy, move 10^6 to the left side of the equation

[0048]

[0049] Then every time the value on the right side of the equation increases by 10 6 (a unit), then a pulse signal is sent. The above formula can be implemented by a digital circuit to adapt to the motor. For example, the multiplication term in the above formula is implemented by a multiplier, and the addition term is implemented by an adder.

[0050] The speed change of the S-shaped speed regulation rule is an S-shaped change, and its function can be a cubic function of time t; the acceleration changes in a parabola, and its function is a quadratic function of time t, and the displacement change is a quartic function of time t. The expression of the S-shaped speed regulation rule can be:

[0051]

[0052] Among them, a(t) is acceleration, v1 is target speed, v0 is starting speed, T is single acceleration or deceleration time, v(t) is current speed, s(t) is current displacement. And when the starting speed, target speed, acceleration and deceleration parameters are determined, the characteristics of the motion process are also determined.

[0053] In the implementation of the S-shaped speed regulation algorithm, t is the time change, and the time change accuracy is also 1μs. After unifying the units and converting the units of displacement to "pieces", the above displacement formula becomes

[0054]

[0055] To improve the calculation accuracy, move 10^6 to the left side of the equation:

[0056]

[0057] Similarly, the above formula can be implemented by digital circuits to adapt to the motor. For example, the multiplication term in the above formula is implemented by a multiplier, and the addition term is implemented by an adder.

[0058] In this embodiment, the method of determining the current displacement of the motor based on the target speed regulation rule at preset time intervals can be implemented through a digital circuit, so that the motion controller is adapted to the motor.

[0059] In one embodiment, the division term in the target speed regulation rule is represented by a multiplication term capable of performing a shift process.

[0060] The multiplication items that can be shifted refer to the calculation items that can be simply calculated in binary form, such as approximating 0.3 to 0.25, because 0.25 is 2 to the power of -2, and the digital circuit can calculate it quickly.

[0061] Specifically, the expression of the trapezoidal speed regulation algorithm is:

[0062]

[0063] After converting division into multiplication and shift processing:

[0064]

[0065] Understandably, After the shift processing, we get

[0066] In addition, the expression of the S-shaped speed regulation algorithm is

[0067]

[0068] To improve computational efficiency, approximate processing is performed and the division is converted into multiplication and shift processing:

[0069]

[0070] In this embodiment, the divider in the digital circuit is difficult to implement and consumes a lot of resources. By representing the division term as a multiplication term that can be shifted, the calculation efficiency can be improved, thereby improving the motor response speed and control effect.

[0071] In one embodiment, the current displacement in the target speed regulation rule is obtained by accumulating and integrating a first-order parameter related to the speed.

[0072] Specifically, when the target speed regulation rule is a trapezoidal speed regulation rule, the first-order parameter related to the speed may be the speed. Since the acceleration in the trapezoidal speed regulation rule is a constant value and the speed is a first-order function, the displacement can be obtained by accumulating and integrating the speed.

[0073] v(t)=kt+v0

[0074] s(t)=v1+v2+v3+…+vt

[0075] Among them, v1 represents the speed obtained by the first interval, v t Represents the speed obtained at the tth interval duration.

[0076] In one embodiment, when the target speed regulation rule is an S-type speed regulation rule, since the acceleration has begun to change, the displacement is obtained by performing cumulative integration processing on at least the acceleration, as follows:

[0077]

[0078] So

[0079] v(t)=a1+a2+a3+…+a t

[0080] s(t)=v1+v2+v3+…+v t

[0081] Therefore, each term in the displacement is a multiplication, and is the product of the change t and the constant. In digital circuits, the product of the constant and the change can be realized by a shift register instead of a multiplier, which can ensure that the resource usage is minimized under the premise of a certain calculation accuracy.

[0082] In one embodiment, the addition term in the target speed regulation rule is implemented by an adder, and the multiplication term is implemented by a shift register.

[0083] Specifically, the adder can be an accumulator for accumulating values. A single multiplication of time and constant terms is not a true multiplication in a digital circuit, but is implemented through addition and shifting. For example, x*6 is decomposed into x*4+x*2, which is a shift. Since the power is uncertain, it cannot be calculated through shifting and accumulation. If a real multiplier is used, a large amount of resources will be occupied, resulting in a slow response speed.

[0084] Then, since the current displacement in the target speed control rule is obtained by cumulative integration of a first-order parameter related to the speed, and cumulative integration is a method of calculating the integral value by accumulation, the addition term in the target speed control rule can be implemented by an adder, and the multiplication term can be implemented by a shift register, thereby minimizing the resource usage of the motion controller.

[0085] In one embodiment, the target speed regulation rule includes a trapezoidal speed regulation rule; the target speed regulation rule is implemented by a shift register for determining the speed and an accumulator for accumulating the speed, and the output end of the shift register is connected to the input end of the accumulator.

[0086] Specifically, the multiplication of a variable and a constant can be realized by a shift register. When the starting speed is 0, the speed can be determined only by the shift register; when the starting speed is not 0, the speed can be determined by combining the shift register and the adder. It can be understood that the output end of the shift register used to determine the speed outputs the speed, which is used to input the accumulator for speed accumulation, and the output of the accumulator is the displacement. The formula is as follows:

[0087] Shift register and adder: v(t)=at+v0

[0088] Accumulator: s(t) = v t +s t-1

[0089] like Figure 3 As shown in FIG. 1 , a digital circuit diagram of a trapezoidal speed regulation rule in an embodiment is shown. The “×” in the figure represents a shift register, and the “+” represents an adder. It can be understood that the accumulator is a special adder. Figure 3 It includes a shift register 302, an adder 304 and an accumulator 306 for determining the speed, and the three are connected in sequence. Taking the calculation of displacement every 1 microsecond as an example, the first time t is 1μs, the trigger calculation obtains v1=a*1+v0, and the displacement s1=v1; the second time t is 2μs, the trigger calculation obtains v2=a*2+v0, and the displacement s2=s1+v2... and so on to obtain the current displacement of the motor. And when the current displacement increases by one unit compared to the previous displacement, a pulse signal is sent. And when some parameters in the target speed regulation rule change, it can also respond based on the digital circuit.

[0090] It can be understood that in displacement mode, when s(t) reaches the target displacement, the motor stops rotating. In speed mode, speed-related parameters change, which can also be controlled by the digital circuit.

[0091] In this embodiment, the target speed regulation rule is implemented by a shift register for determining the speed and an accumulator for accumulating the speed, which can be adapted to the motor, and since no multiplier is required, the resources consumed in calculation are greatly reduced.

[0092] In one embodiment, the target speed regulation rule includes an S-shaped speed regulation rule. The target speed regulation rule is implemented by a shift register for determining a first-order parameter related to speed, a first accumulator for accumulating acceleration, and a second accumulator for accumulating speed, wherein the shift register, the first accumulator, and the second accumulator are connected in sequence.

[0093] Specifically, in the S-shaped speed regulation rule, the acceleration is variable, and may be a linear change related to time t, or a quadratic or cubic change related to time t, etc. Therefore, the shift register is used to determine the linear parameters related to the speed, and the subsequent calculations involving quadratic and cubic calculations are implemented through the accumulator.

[0094] Optionally, when the acceleration calculation method is a first power, the first power parameter related to the speed is the acceleration, which can be realized by a shift register. Taking a(t)=2t as an example, Figure 4 FIG. 1 is a schematic diagram of a digital circuit for an S-shaped speed regulation rule in one embodiment. Figure 4 The shift register 402 for determining acceleration, the first accumulator 404 for accumulating acceleration, and the second accumulator 406 for accumulating velocity are connected in sequence. It can be understood that the output of the shift register 402 for determining acceleration is acceleration a(t), the output of the first accumulator for accumulating acceleration is velocity v(t), and the output of the second accumulator for accumulating velocity is current displacement s(t). Then the representation on the accumulator is

[0095] a(t)=2*t

[0096] v(t)=a t +v t-1

[0097] s(t)=v t +s t-1

[0098] Optionally, when the acceleration calculation method is quadratic, the first-order parameter related to the velocity is the jerk, which can be realized by a shift register. For example, the calculation method is as follows:

[0099]

[0100] The only velocity-related parameter that contains a first-order term is the acceleration a ` (t). The cumulative expression is as follows:

[0101] a t =a′1+a′2+a′3+…+a′ t

[0102] v t =a1+a2+a3+…+a t +v0

[0103] s t =v1+v2+v3+…+v t

[0104] Specifically, it is represented on the accumulator as

[0105] a(t)=a′ t +a t-1

[0106] v(t)=a t +v t-1

[0107] s(t)=v t +s t-1

[0108] It is understandable that the initial speed may be 0 or not. Taking the above calculation formula as an example, Figure 5 FIG. 2 is a schematic diagram of a digital circuit for an S-shaped speed regulation rule in another embodiment. Figure 5 The coefficient in is Figure 5 The shift register 502 and 506 for determining the jerk, and the adder 504, the third accumulator 508 for jerk accumulation, the first accumulator 510 for acceleration accumulation, and the second accumulator 512 for velocity accumulation are included, and the shift register 502, the adder 504, the shift register 506, the third accumulator 508, the first accumulator 510, and the second accumulator 512 are connected in sequence. It can be understood that, Figure 5 The output of the middle shift register 506 is the jerk a′(t), the output of the third accumulator 508 for jerk is acceleration a(t), the output of the first accumulator for acceleration accumulation is velocity v(t), and the output of the second accumulator for velocity accumulation is the current displacement s(t).

[0109] In this embodiment, in the S-shaped speed regulation rule, acceleration is a time-related change. Therefore, the acceleration can be calculated first through a shift register, and then the current displacement is determined by a first accumulator for acceleration accumulation and a second accumulator for velocity accumulation. In this way, when the speed regulation rule is implemented by a digital circuit, high-order calculations are converted into first-order accumulation calculations, thereby ensuring that resource usage is minimized under the premise of a certain calculation accuracy.

[0110] In one embodiment, the target speed regulation rule is a trapezoidal speed regulation rule, and is implemented by a digital circuit as an example. In a trapezoidal slope, the speed is proportional to the time. In the trapezoidal acceleration and deceleration, after the starting speed v0 (hz), the target speed v1 (hz) and the acceleration and deceleration time T (ms) are determined, the acceleration is a constant value.

[0111]

[0112] v(t)=kt+v0

[0113]

[0114] Where a is the acceleration, k is the slope, v1 is the target speed, v0 is the starting speed, T is the acceleration or deceleration time, v(t) is the current speed, and s(t) is the current displacement.

[0115] Taking the preset duration of 1 μs as an example, the unit of t is μs, and the unit of displacement is converted into "pieces", the formula is

[0116]

[0117] To improve the calculation accuracy, move 10^6 to the left side of the equation

[0118]

[0119] After converting division into multiplication and shift processing:

[0120]

[0121] Then every time the value on the right side of the equation increases by 10 6 , a pulse signal is sent. In order to reduce resource consumption, the displacement formula calculation is also implemented by cumulative integration, starting with the velocity formula.

[0122] Let D1p0 = t, D2p0 = v0, and calculate the two items separately. Then the integral term of t is D1p1 = D1p1 + D1p0, which is equal to the integral term of t last time plus the value of t this time. The integral term of v0 is D2p1 = D2p1 + D2p0, which is equal to the integral term of v0 last time plus the value of v0 this time.

[0123] The cumulative calculation formula is:

[0124]

[0125] like Figure 6 FIG. 1 is a schematic diagram of a digital circuit of a trapezoidal speed regulation rule in another embodiment. Figure 6 It represents the process of the above formula. Vk1 is the sk1 is the Then t is accumulated, multiplied by sk1 to get the intermediate value s_1, and then multiplied by a for unit conversion to get the intermediate value Sa; in another item, the starting speed v0 is accumulated in the D2p1 item, and then added to the intermediate value Sa to get the current displacement s(t). t and k are calculated through the shift register to get v_1, and then multiplied by a for unit conversion to get va=kt, and the sum of it and the starting speed v0 is the current speed v(t) of the motor.

[0126] In this embodiment, by converting the division of the trapezoidal speed regulation rule into multiplication and shift, and realizing the calculation of the motor speed and displacement through a shift register and an adder, resource occupation can be greatly reduced and the motor response speed can be improved.

[0127] In one embodiment, the target speed regulation rule is an S-shaped speed regulation rule, the acceleration is a quadratic function of time t, and is implemented by a digital circuit as an example. Then, the speed changes in an S-shaped manner, and its function is a cubic function of time t, the acceleration changes in a parabolic manner, and its function is a quadratic function of time t, and the displacement changes in a quartic function of time t.

[0128]

[0129] Among them, a(t) is acceleration, v1 is target speed, v0 is starting speed, T is single acceleration or deceleration time, v(t) is current speed, s(t) is current displacement. And when the starting speed, target speed, acceleration and deceleration parameters are determined, the characteristics of the motion process are also determined.

[0130] In the implementation of the S-shaped speed regulation algorithm, t is the time change, and the time change accuracy is also 1μs. After unifying the units and converting the units of displacement to "pieces", the above displacement formula becomes

[0131]

[0132] To improve the calculation accuracy, move 10^6 to the left side of the equation:

[0133]

[0134] To improve computational efficiency, approximate processing is performed and the division is converted into multiplication and shift processing:

[0135]

[0136] The displacement formula includes the fourth power of time. The longer the time, the more registers are required for multiplication, which consumes resources. Therefore, in the algorithm implementation, the displacement formula is solved by accumulating integrals. Starting from the derivative jerk of acceleration, accumulation is performed every 1μs.

[0137] The function of acceleration (deceleration) is as follows:

[0138]

[0139] The formula is as follows

[0140] Jerk a′(t) D1p0=2*t D2p0=T D3p0=0 Acceleration a(t) D1p1=D1p1+D1p0 D2p1=D2p1+D2p0 D3p1=0 Speed ​​v(t) D1p2=D1p2+D1p1 D2p2=D2p2+D2p1 <![CDATA[D3p2=v0]]> Displacement s(t) D1p3=D1p3+D1p2 D2p3=D2p3+D2p2 D3p3=D3p3+D3p2

[0141] The calculation of the jerk is divided into two items, D1p0 and D2p0, where D1p0=2*t, D1p0=2*t. Since the starting velocity does not need to be added at this time, D3p0=0.

[0142] Then the acceleration is calculated by accumulating the jerk, that is, the previous acceleration value (accumulated jerk value) plus the current jerk value, which is also divided into two items for accumulation calculation: D1p1=D1p1+D1p0; D2p1=D2p1+D2p0.

[0143] The calculation of velocity is realized by accumulating acceleration, that is, the last velocity value (accumulated acceleration value) plus the current acceleration value, where two items are accumulated: D1p2 = D1p2 + D1p1; D2p2 = D2p2 + D2p1. Since the initial velocity may not be 0, there is a third item D3p2 = v0 added to the calculation.

[0144] The displacement is calculated by accumulating the velocity, that is, the last displacement value (accumulated velocity value) plus the current velocity value. Due to the existence of the initial velocity, it is divided into three items for cumulative calculation: D1p3=D1p3+D1p2; D2p3=D2p3+D2p2; D3p3=D3p3+D3p2.

[0145] Then, by adding up the above formula, we can get

[0146]

[0147] like Figure 7 FIG. 2 is a schematic diagram of a digital circuit for an S-shaped speed regulation rule in another embodiment. Figure 7 K is a fixed value ak1 is the vk1 is the value in v(t) vk2 is the value in v(t) sk1 is the sk2 is the number of The outputs in the figure include acceleration a(t), velocity v(t) and displacement s(t). For the first term D1p0=2*t, the acceleration without coefficient is output after being accumulated with D1p1, and then multiplied with ak1 to obtain the intermediate value a_1. For the second term T, the value obtained by accumulating D2p0 and the intermediate value a_1 are added to obtain the intermediate value a_del, which is multiplied by the unit conversion coefficient K to obtain the current acceleration a(t) of the motor.

[0148] D1p2 is added to D1p1 and multiplied by vk1 to obtain v_1, a part of the speed expression; D2p1 is added to D2p2 and multiplied by vk2 to obtain v_2, a part of the speed expression, v_1 ​​plus v_2 is obtained to obtain the intermediate value v_del, the intermediate value v_del is multiplied by K to obtain the intermediate value vK, and the sum of the intermediate value vK and the starting speed v0 is the current speed v(t).

[0149] Furthermore, D1p2 and D1p3 are accumulated and multiplied by sk1 to obtain a part of the displacement expression s_1; D2p2 and D2p3 are accumulated and multiplied by sk2 to obtain a part of the displacement expression s_2. After s_1 plus s_2, the intermediate value s_del is obtained, which is then multiplied by K to obtain the intermediate value sK, and the sum of the accumulated value of D3p3 is obtained to obtain the current displacement s(t).

[0150] In this embodiment, by converting the division of the S-shaped speed regulation rule into multiplication and shifting, and realizing the calculation of the motor speed and displacement through a shift register and an adder, resource occupation can be greatly reduced and the motor response speed can be improved.

[0151] In one embodiment, a motion control method includes: in response to the start-up operation of the motor, at preset time intervals, determining the current displacement of the motor based on a target speed control rule; the division term in the target speed control rule is represented by a multiplication term that can be shifted; and the current displacement in the target speed control rule is obtained by accumulating and integrating a first-order parameter related to the speed; when the target speed control rule is a trapezoidal speed control rule, the trapezoidal speed control rule is implemented by a shift register for determining a first-order speed and an accumulator for speed accumulation, and the output end of the shift register is connected to the input end of the accumulator; when the target speed control rule is an S-shaped speed control rule, the S-shaped speed control rule is implemented by a shift register for determining a first-order parameter related to the speed, a first accumulator for acceleration accumulation, and a second accumulator for speed accumulation, and the shift register, the first accumulator, and the second accumulator are connected in sequence; when the current displacement increases by one unit compared to the previous displacement, a pulse signal is sent to perform motion control of the motor.

[0152] In this embodiment, in response to the triggering operation of the motor, the current displacement of the motor is determined based on the target speed control rule at preset time intervals. When the current displacement increases by one unit compared to the previous displacement, a pulse signal needs to be generated, that is, a pulse signal is generated every time the displacement meets the requirement to control the motor. Compared with the traditional method, the accuracy is not affected by the time interval, the motor speed change transition is good, and the motor control is more stable; and the target speed control rule is implemented through the shift register and the accumulator, which can greatly improve the motor response speed.

[0153] In one embodiment, in the implementation process of the traditional speed regulation algorithm, when the frequency of the pulse signal is calculated according to the speed formula, the implementation method is usually to select a fixed time interval t, calculate the speed formula once every t time, and obtain the frequency f of the pulse signal at that moment. Within the time interval t, the frequency of the pulse signal is f. In this implementation method, the accuracy is affected by the time interval t. When the time interval t is too large, the speed changes too quickly.

[0154] In addition, the frequency of the pulse signal can also be calculated based on the displacement formula. This method requires an inverse operation on the displacement formula. The time corresponding to the displacement is solved, and the solved time is substituted into the speed formula to finally find the frequency of the pulse signal. The displacement formula is generally a quadratic function, a quartic function, or a higher-order function of time t. When this implementation method is implemented in software, a function can be used to perform an inverse operation. However, in the process of digital circuit implementation, the digital circuit cannot perform an inverse operation on the displacement formula. Therefore, the motion control method in this implementation is proposed.

[0155] 1. Configuration parameters

[0156] Taking the pulse signal as STEP signal as an example, before the motion controller works, the user needs to set the following parameters. Based on the following parameters, the digital internal will calculate the acceleration, deceleration, displacement of each process and other parameters.

[0157] (1)v targ : Target speed (frequency of STEP signal), unit: hz, signed number (complement code), the sign bit indicates the direction of the motor (only in speed mode), and the absolute value indicates the target speed. |v targ When | is 0, it means the motor stops, and its maximum value should be less than 1MHZ. And ensure v targ >v start , v targ >v stop .

[0158] (2)v start : Starting speed (frequency of STEP signal), the initial speed at which the motion controller accelerates to the target speed, in hz, with a default value of 350hz.

[0159] (3)v stop : Stop speed (frequency of STEP signal), the final speed of the motion controller during deceleration after the stop signal is triggered, in hz, with a default value of 350hz.

[0160] (4)S targ : Target displacement, a signed number (complement code), the sign bit indicates the direction (only in displacement mode), and the absolute value indicates the number of STEP signals under the corresponding subdivision number.

[0161] (5) T_incre: acceleration time, the time of a single acceleration process, in ms, with a default value of 200 ms.

[0162] (6) T_dec: Deceleration time, the time of a single deceleration process, in ms, with a default value of 200 ms.

[0163] (7) Amax_lim: Maximum acceleration limit, the maximum absolute value of acceleration allowed by the user. If the absolute value of acceleration is greater than this value, an alarm signal is generated and the motion controller is prohibited from working. The unit is hz / ms.

[0164] The above 7 parameters are the parameters required for calculation. After the calculation parameter configuration is completed, the user also needs to configure the enabling parameters.

[0165] Motion_mode: Motion control mode selection, including speed mode and displacement mode.

[0166] Ramp_mode: acceleration / deceleration ramp type, including polynomial S-shaped speed regulation rule and trapezoidal speed regulation rule.

[0167] En_motion: Motion controller is always enabled.

[0168] Buff_en: START_TRIG trigger signal buffer enable.

[0169] Stop_mode: In motion controller mode, the stop mode triggered by pin FORCE_STOP (DRV_ENn).

[0170] Force_stop: motion controller stop control signal, only hard stop.

[0171] START_TRIG: Triggers the motion controller to work or parameters to take effect. Hard stop and pin-triggered soft stop do not require this trigger signal.

[0172] 2. Working Mode

[0173] Includes velocity mode and displacement mode.

[0174] Speed ​​mode: The motion controller starts with a starting speed v start Accelerate to target speed v targ Then, at the target speed v targ Perform uniform motion; when the target speed changes (increases or decreases), accelerate or decelerate to the new target speed. The entire process does not pay attention to the change requirements of displacement, but only changes the state according to the speed requirements.

[0175] Displacement mode: starting speed v start Accelerate to target speed v targThe acceleration process, the speed is the target speed v targ uniform speed process (if any), by the target speed v targ To stop speed v stop The total displacement of these three processes is equal to the target displacement S targ In displacement mode, the state changes according to the displacement requirements.

[0176] 1. Speed ​​mode

[0177] v targ It is a signed number. The user configures it through registers. Its sign bit indicates the direction of the motor. Its absolute value indicates the target speed. During the acceleration and deceleration calculation process, only its absolute value is calculated. Vnow is the current actual speed.

[0178] In the initial state or uniform speed state (velocity_reach=1), it will respond to v targ During the change, acceleration or deceleration process, the register v will not respond immediately targ If the machine is changed, it still runs according to the original settings until it reaches the initial state or uniform speed state.

[0179] During the motor rotation, when v targ After the sign bit of changes (i.e., the direction changes), wait until the state is uniform or initial. In the uniform state, when the direction change is detected and START_TRIG is triggered, after the last STEP signal ends, it enters the pre-stop state. This state follows the original direction and the set stop speed v stop (can be 0) decelerates to the initial waiting state, waits for the DIRCH_WAIT time, and then accelerates to the target speed according to the newly set direction; in the initial state, a steering change is detected and the steering is accelerated to the target speed according to the newly set direction.

[0180] In speed mode, the integral value of acceleration is detected to be greater than or equal to v targ To decide whether to end the acceleration process, the same applies to the deceleration process.

[0181] 2. Displacement mode

[0182] In displacement mode, the direction is determined only by S targ The target displacement S will be responded only in the initial state. targ The size, direction, speed and other parameters of the change. After completing a movement and returning to the initial state, if the target displacement S targ It still has the original size and direction, and will not move next time unless the register START_TRIG is triggered (triggering the motion controller to work or parameters to take effect); if the direction is reversed and the size remains unchanged, the reverse movement is performed according to the displacement (after START_TRIG is triggered).

[0183] In the deceleration state, because there is an error in the S-curve calculation (trapezoidal curve calculation), the acceleration changes faster than the ideal acceleration, resulting in the speed decreasing to the stop speed and then increasing in the reverse direction during the deceleration time, and the speed after the deceleration time is over will be too high. Therefore, a correction state is added. When it is detected that the current speed is less than the stop speed, the deceleration is stopped and the machine runs at a constant speed at the stop speed until the displacement requirement is met.

[0184] In displacement mode, state transfer is performed by detecting whether the displacement meets the requirements under acceleration, uniform speed and deceleration states. The following formula is the displacement size of each state.

[0185] Acceleration stage displacement step incre :

[0186]

[0187] Deceleration stage displacement step dec :

[0188]

[0189] Then the uniform speed stage displacement step con :

[0190] step con =S targ -step incre -step dec

[0191] In displacement mode, according to the set v start 、v stop , acceleration time T_incre, deceleration time T_dec and v targ The size of the acceleration and deceleration phase displacement is calculated according to the above formula. If the target displacement S is set targ If it is greater than or equal to the sum of the acceleration and deceleration displacements, it works normally. Otherwise, a displacement error alarm is issued, indicating that the parameter setting is abnormal (the speed setting is too large or the acceleration and deceleration time is too long), and the displacement mode does not work at this time.

[0192] 3. Startup Confirmation

[0193] After the motion controller is enabled, the motion control parameters (speed, displacement, time), ramp type and working mode are configured, the motion controller will start working only after writing 1 to the register START_TRIG.

[0194] In speed mode, the register is updated during acceleration or deceleration and the register START_TRIG is triggered. If the register buff_en=1 at this time, the parameter change will not be responded immediately in this scenario, and the trigger will be automatically responded after reaching the target speed or initial state; if the register buff_en=0 at this time, the parameter change and START_TRIG trigger during acceleration or deceleration are ignored, and only the trigger after reaching the target speed or initial state is responded.

[0195] In displacement mode, if the register is updated during the movement and the register START_TRIG is triggered, if the register buff_en=1 at this time, the parameter change will not be responded immediately in this scenario, and it will automatically respond to the trigger in the initial state after reaching the target displacement; if the register buff_en=0 at this time, the parameter change and START_TRIG trigger during the movement are ignored, and only the trigger of the initial state is responded.

[0196] In displacement mode, when the target displacement is reached, after receiving the START_TRIG signal (or the START_TRIG maintained during the movement in buff_en=1 mode), the register SNOW is automatically cleared, and then the movement is repeated again according to the target displacement.

[0197] Hard stop and pin-triggered soft stop do not require register START_TRIG trigger.

[0198] 4. Stop method

[0199] 1. Soft stop

[0200] In velocity mode (no soft stop in displacement mode), the register v targ = 0 to stop the motion controller. When the value in the register takes effect, the motion controller decelerates to stop, that is, soft stop.

[0201] In speed mode, soft stop can also be achieved by using the pin FORCE_STOP (DRV_ENn) and configuring the register stop_mode = 1.

[0202] There is no soft stop in displacement mode.

[0203] In this mode, the motor still has a holding current after it stops.

[0204] 2. Hard stop

[0205] In speed or displacement mode, you can achieve a hard stop by configuring the register stop_mode = 0 through the pin FORCE_STOP (DRV_ENn); or by configuring the register force_stop = 0 to turn off the motion controller function to achieve a hard stop. Once the above hard stop conditions are triggered, the motion controller will stop immediately regardless of which stage it is in. In this mode, the motor still has a holding current after stopping.

[0206] 5. Fault alarm

[0207] (1) Displacement error

[0208] The target displacement STARG is smaller than the sum of the acceleration and deceleration displacements, and a displacement error alarm is issued, indicating that the parameter setting is abnormal and the displacement mode does not work at this time.

[0209] (2) Acceleration error

[0210] According to the setting v start 、v stop , T_incre, and T_dec can calculate the maximum value of acceleration a_max. Users can limit the acceleration during motion control by configuring the AMAX register. When a_max is greater than the value set in the AMAX register, an acceleration error warning is issued, indicating that the set parameters are unreasonable (the difference between the initial velocity and the final velocity is large or the acceleration and deceleration time is too small), and the acceleration exceeds the limit. At this time, both the velocity mode and the displacement mode do not work.

[0211] 6. Speed ​​Regulation Rules Implementation

[0212] The speed regulation rules include S-shaped speed regulation rules and trapezoidal speed regulation rules, which means that the speed changes in an S shape or trapezoidal shape.

[0213] 1. Polynomial S-shaped speed regulation rule

[0214] The speed change of the S-shaped speed regulation rule is an S-shaped change, and its function can be a cubic function of time t; the acceleration changes in a parabola, and its function is a quadratic function of time t, and the displacement change is a quartic function of time t. The expression of the S-shaped speed regulation rule can be:

[0215]

[0216] Among them, a(t) is acceleration, v1 is target speed, v0 is starting speed, T is single acceleration or deceleration time, v(t) is current speed, s(t) is current displacement. And when the starting speed, target speed, acceleration and deceleration parameters are determined, the characteristics of the motion process are also determined.

[0217] In the implementation of the S-shaped speed regulation algorithm, t is the time change, and the time change accuracy is also 1μs. After unifying the units and converting the units of displacement to "pieces", the above displacement formula becomes

[0218]

[0219] To improve the calculation accuracy, move 10^6 to the left side of the equation:

[0220]

[0221] Starting from t=0, time t starts to accumulate in increments of 1μs. Every time 1us is added, a displacement calculation is performed according to the motion parameter value and the target speed control rule. When 10^6*s(t)>=10^6, it indicates that the first STEP signal can be generated, and the value of 10^6*s(t0) at the current moment is recorded. The above process is guaranteed to be completed within 1μs. Time t continues to accumulate, and the value of 10^6*s(t) continues to be calculated. When 10^6*s(t)-10^6*s(t0)>=10^6 or 10^6*s(t)=2*10^6, a second pulse signal is generated, and the value of 10^6*s(t1) at the current moment is recorded. The calculation continues in this way to generate a pulse signal of speed change (i.e. frequency change). In displacement mode, until the total displacement reaches the target displacement, it indicates that the motion process is over.

[0222] The displacement formula contains the fourth power of time. The longer the time, the more registers are required for multiplication, which consumes resources. Therefore, in the algorithm implementation, the displacement formula is solved by accumulating integrals. Starting from the derivative of acceleration, the jerk, the accumulation is performed every 1us.

[0223] The function of acceleration (deceleration) is as follows

[0224]

[0225] Figure 7 This is a digital circuit diagram for solving the acceleration, velocity and displacement changes by using the S-shaped speed regulation rule. By accumulating the above formula, we can finally get

[0226]

[0227] 2. Implementation of trapezoidal speed regulation rules

[0228] In a trapezoidal ramp, the speed is proportional to the time. In trapezoidal acceleration and deceleration, after the starting speed v0 (hz), target speed v1 (hz) and acceleration and deceleration time T (ms) are determined, the acceleration is a constant.

[0229]

[0230] v(t)=kt+v0

[0231]

[0232] Where a is the acceleration, k is the slope, v1 is the target speed, v0 is the starting speed, T is the acceleration or deceleration time, v(t) is the current speed, and s(t) is the current displacement.

[0233] Taking the preset duration of 1 μs as an example, the unit of t is μs, and the unit of displacement is converted into "pieces", the formula is

[0234]

[0235] To improve the calculation accuracy, move 10^6 to the left side of the equation

[0236]

[0237] After converting division into multiplication and shift processing:

[0238]

[0239] The method of determining whether to send a pulse signal is the same as the above S-shaped method. In order to reduce resource consumption, the displacement formula calculation is also implemented by cumulative integration, starting with the velocity formula.

[0240] Figure 6 It is a digital circuit schematic diagram for solving the changes of acceleration, speed and displacement through trapezoidal speed regulation rules. Figure 6 The expression in is as follows:

[0241]

[0242] In this embodiment, on the premise that the digital circuit realizes the speed regulation algorithm, a trapezoidal speed curve algorithm and a polynomial S-type speed curve algorithm are provided, and the resource occupation is minimized under the premise of ensuring a certain calculation accuracy.

[0243] In addition, a stepper motor motion controller with simple configuration parameters, controllable startup mode, and optional speed control algorithm is also provided. The motion controller can select trapezoidal speed curve algorithm and polynomial S-type speed curve algorithm according to different application scenarios, and the required motion parameters are consistent. During the operation of the controller, it can also alarm for acceleration errors and displacement errors, and prohibit the motion controller from working, to prevent the motor from rotating incorrectly and causing unnecessary losses.

[0244] It should be understood that although the above Figure 2The steps in the flowchart are shown in sequence as indicated by arrows, but these steps are not necessarily executed in the order indicated by arrows or numbers. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. Moreover, Figure 2 At least part of the steps may include multiple steps or multiple stages. These steps or stages are not necessarily performed at the same time, but can be performed at different times. The execution order of these steps or stages is not necessarily sequential, but can be performed in turn or alternately with other steps or at least part of the steps or stages in other steps.

[0245] In one embodiment, a computer device is provided. The computer device may be a terminal device, and its internal structure diagram may be as follows: Figure 8 As shown. The computer device includes a processor, a memory, a communication interface, a display screen and an input device connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The communication interface of the computer device is used to communicate with an external terminal in a wired or wireless manner, and the wireless manner can be achieved through WIFI, an operator network, NFC (near field communication) or other technologies. When the computer program is executed by the processor, a motion control method is implemented. The display screen of the computer device can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device can be a touch layer covered on the display screen, or a button, a trackball or a touch pad set on the computer device housing, or an external keyboard, touch pad or mouse, etc.

[0246] Those skilled in the art will understand that Figure 8 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0247] In one embodiment, a motion controller is provided, wherein the motion controller is used to implement the steps of each motion control method embodiment.

[0248] In one embodiment, a computer device is provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor implements the steps of the above-mentioned method embodiments when executing the computer program.

[0249] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned method embodiments are implemented.

[0250] In one embodiment, a computer program product or computer program is provided, the computer program product or computer program includes computer instructions, the computer instructions are stored in a computer-readable storage medium. A processor of a computer device reads the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, so that the computer device performs the steps in the above-mentioned method embodiments.

[0251] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes in the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory may include read-only memory (ROM), magnetic tape, floppy disk, flash memory or optical memory, etc. Volatile memory may include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0252] The above description is only a preferred embodiment of the present application, and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A motion control method, characterized in that: Applied to a motion controller, the method comprises: In response to a start-up operation of the motor, determining a current displacement of the motor based on a target speed regulation rule at every preset time interval; When the current displacement increases by one unit compared to the previous displacement, a pulse signal is sent to perform motion control of the motor.

2. The method according to claim 1, characterized in that: The target speed regulation rule is implemented through a digital circuit.

3. The method according to claim 2, characterized in that The division term in the target speed regulation rule is represented by a multiplication term capable of performing a shift process.

4. The method according to claim 2, wherein the current displacement in the target speed regulation rule is obtained by accumulating and integrating a first-order parameter related to the speed.

5. The method according to claim 4, wherein the addition term in the target speed regulation rule is implemented by an adder, and the single multiplication term in the target speed regulation rule is implemented by a shift register.

6. The method according to claim 5, characterized in that The target speed regulation rule includes a trapezoidal speed regulation rule; The trapezoidal speed regulation rule is implemented by a shift register for determining a first-order speed and an accumulator for speed accumulation, and the output end of the shift register is connected to the input end of the accumulator.

7. The method according to claim 5, characterized in that The target speed regulation rule includes an S-shaped speed regulation rule; The S-shaped speed regulation rule is implemented by a shift register for determining a first-order parameter related to speed, a first accumulator for accumulating acceleration, and a second accumulator for accumulating speed. The shift register, the first accumulator, and the second accumulator are connected in sequence.

8. A motion controller, characterized in that: The motion controller is used to implement the steps of the method according to any one of claims 1 to 7.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images