A path planning control method for a micro-step motor of a mechanical arm

Abstract

This invention discloses a path planning control method for the microstepping motors of a robotic arm, comprising the following steps: After receiving a path planning signal, the controller initializes the stepper motors on each joint of the robotic arm to the initial position of the path planning; the controller packages the block data of the path planning and sends it to the actuator, each block data including the number of microsteps S, the final speed V2, and the time t0. The actuator parses the data and determines whether to enter the planning mode; if it enters the planning mode, it sends the parsed data to the planner; the planner checks the received data; the planner calculates the acceleration and delay period of the block; and controls the speed change of the microstepping motor according to the delay period. This invention decomposes the stepping angle of the microstepping motor in the joint of the robotic arm into tiny differential angles, thereby reducing the vibration and shock of the robotic arm during movement, improving the stability and accuracy of the robotic arm, and achieving smoother movement, thus making it suitable for high-precision operations.

Description

Technical Field

[0001] This invention relates to the field of microstepping motor control methods, and more specifically, to a path planning control method for a microstepping motor of a robotic arm. Background Technology

[0002] In robotic arm control, precise position control and smooth motion are crucial. Stepper motors can control the position of robotic arm joints through precise step angles, thus achieving accurate operations. Stepper motors have relatively high resolution, providing precise position control, making them suitable for procedures requiring high-precision positioning, such as ophthalmic surgery. However, traditional stepper motor control methods can lead to problems such as resonance, vibration, and noise. These problems are mainly due to the abrupt transitions between each step angle, resulting in uneven motion.

[0003] Microstepping motors also offer advantages in high-speed motion and high-precision positioning. They can precisely control the position and movement of robotic arm joints, enabling the robotic arm to accurately position and perform various tasks. Microstepping motors also feature fast response speed and optimized power consumption, improving the working efficiency and productivity of robotic arms. By further decomposing the step angle into tiny differential step angles, microstepping motors achieve smoother motion and reduce vibration and noise. Therefore, microstepping motors have a wide range of applications, especially in robotic arms requiring high-precision control and smooth motion. However, current microstepping motor control methods lack fine and precise path planning, still exhibiting some vibration and instability, which cannot meet the demands of precision operations such as those required by surgical robot robotic arms. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of existing stepper motor control methods in terms of path planning, which leads to insufficient stability and accuracy of the robotic arm during movement, and to provide a stepper motor path planning control method.

[0005] The objective of this invention can be achieved using the following technical solutions:

[0006] A path planning and control method for a microstepping motor of a robotic arm includes the following steps:

[0007] S1: After receiving the path planning signal, the controller sends a signal to notify the actuator to enter the planning mode. The actuator sends a signal to notify the stepper motors on each joint of the robotic arm to rotate to the initial position of the path planning, thereby completing the initialization of each joint of the robotic arm.

[0008] S2: After completing the initialization of all joints, the controller packages the path planning block data and sends it to the actuator. Each block data includes the number of microsteps S that the stepper motor rotates in this block, the final speed V2 of the stepper motor in this block, and the duration t0 of this block.

[0009] S3: The planner checks the received data. If the block is invalid, the planner returns an exception message. If the block is valid, it first checks whether the speed and direction of the motor have changed. If they have not changed, the block is inserted into the circular buffer normally. If the direction has changed, the block is divided into two blocks with positive and negative speeds respectively at 0 speed and inserted into the circular buffer.

[0010] S4: When a block is inserted into the circular buffer, the planner reads the block data in the circular buffer and calculates the acceleration of the block, as well as the delay period generated by each microstep PWM pulse of the block;

[0011] S5: Based on the calculated delay period, a timer is used to generate PWM pulses for controlling the microstepping motor through periodic interrupts, thereby controlling the speed change of the microstepping motor.

[0012] The path planning signal is obtained from the robot arm trajectory planning algorithm running on the host computer, which generates the robot arm's subsequent path curve and path planning signal. This invention provides the path curve for each joint on the robot arm in the path planning signal. Through method planning, it controls the micro-stepping motors on the robot arm to reach a predetermined position, angle, and speed at a predetermined time, thereby achieving precise operation of the robot arm.

[0013] The controller's main function is to send signals indicating whether to enter planning mode, key points of the path planning curve, and to receive feedback information from the motor. The actuator module's main function is to parse the data from the controller. Often, the controller runs on a high-performance host computer, so it needs to send data to the slave computer to complete instructions. After parsing, the actuator determines whether to enter planning mode. If so, it sends the block data received from the controller to the planner. One thread in the planner receives the block data sent by the actuator and checks the validity of the microsteps S, final speed V2, and block duration t0. Specifically, it checks that the block time t0 should be greater than 0 and V2 should be less than the motor's maximum speed. If valid, the block data is inserted into the circular buffer. Another thread in the planner continuously reads the block data from the circular buffer. Because parameter calculation is relatively slow, one thread in the planner handles block insertion, while the other thread reads blocks from the buffer for parameter calculation. This dual-thread approach is more efficient. The planner's parameter calculation function then uses this data to calculate the motor control parameters required for the block to reach speed V2 given S and t0. Based on the motor control parameters calculated by the planner, it calculates the PWM pulse delay required for the next microstep, thereby controlling the motor speed. Then, in the next timer interrupt, the PWM pulse generator is called to control the motor movement. The motor controller reads the block data from the circular buffer via hardware interrupts. If no block is found, the motor waits; if valid block data is read, the motor can be controlled to move based on the parameters calculated in the block.

[0014] Furthermore, in step S5, when calculating acceleration, if the number of steps S0 required for the stepper motor to change uniformly from speed V1 to a specified speed V2 is different from the given number of steps S, the motion of the block microstepper motor is planned:

[0015] When V1 < V2:

[0016] like Figure 2 As shown, when the given number of microsteps S is greater than the number of microsteps S0 for uniform acceleration throughout the entire process, the vehicle first undergoes uniform acceleration to a speed V2, and then undergoes uniform motion.

[0017] The time of uniform motion is: The acceleration of uniformly accelerated motion: ;

[0018] like Figure 3 As shown, when the given number of microsteps S is less than the number of microsteps S0 for uniform acceleration throughout the entire process, the vehicle first moves at a constant speed, and then moves at a constant speed until it reaches a velocity V2.

[0019] The time of uniform motion is: The acceleration of uniformly accelerated motion: ;

[0020] When V1 > V2:

[0021] like Figure 4 As shown, when the given number of microsteps S is less than the number of microsteps S0 for uniform acceleration throughout the entire process, the motion first undergoes uniform deceleration to a speed V2, and then undergoes uniform motion:

[0022] The time of uniform motion is: The acceleration of uniformly decelerated motion: .

[0023] Within a block, the stepper motor needs to rotate a number of microsteps S within a time t0. While completing this number of microsteps, the initial velocity V1 of the block also needs to change to V2. Therefore, the motion of each block needs to be planned. This invention adopts a segmented motion that combines uniform acceleration and uniform motion. Uniform acceleration motion occurs within time t1, with acceleration a, and uniform motion occurs within time t2, where t0 = t1 + t2.

[0024] Furthermore, in step S5, when calculating the delay period, each microstep i aims to achieve the corresponding speed V. i The delay period P required to generate PWM i It is determined by the timer frequency F and the motor speed V. i Jointly decided, that is Therefore, it is necessary to first calculate the speed V of the corresponding microstepping motor that each microstep i needs to reach. i Then calculate the delay period P for each microstep. i .

[0025] Furthermore, in step S5, the speed V that each microstep i needs to reach for the corresponding microstep motor is calculated. i The method is as follows:

[0026] .

[0027] In uniformly accelerated motion , ,in This represents the number of microsteps taken by the stepper motor. V represents the initial velocity of the microstep, and V is the number of microsteps taken. The target velocity after that. Combining the two equations, we get: Therefore, for each microstep i, Therefore, the speed after this microstep is completed .in, This refers to the initial velocity of the microstep or the velocity after the previous microstep was completed.

[0028] Furthermore, in step S5, the delay period P of each microstep is calculated. i The process is as follows:

[0029] .

[0030] Depend on and We can obtain:

[0031] .

[0032] In step S5, the delay period P of each microstep is calculated. i When using Taylor's formula ,when ,make ,

[0033] Then P i It can be simplified to:

[0034]

[0035] Among them, when in the acceleration phase When in the uniform speed phase When in the deceleration phase ;

[0036] Delay period during the initial stage: ;

[0037] The delay period during the termination phase: .

[0038] Furthermore, in step S2, when the controller sends the block data to the actuator, it uses a CAN information frame to package and send the block data. The format of the CAN information frame includes a frame ID and block data, where the frame ID is 0xA + a joint number. The first 4 bytes of the block data together form a 32-bit int data, representing S; the 5th and 6th bits together form a 16-bit int data, representing V2; and the 7th and 8th bits together form a 16-bit uint data, representing t0.

[0039] A CAN information frame includes a frame ID and a data frame. When sending path planning signals, the data frame is block data. The frame ID is 0xA + one bit of the joint number. For example, if the first joint number is 1, the frame ID is 0xA1; if the second joint number is 2, the frame ID is 0xA2.

[0040] In step S1, the steps for determining whether to enter the planning mode are as follows:

[0041] The controller first sends data containing frame ID 0xAA to the actuator via a CAN message frame to notify it to enter path planning mode. The actuator parses the instruction and instructs the motor actuator to control the motor to rotate to the initial position. Once the motor has rotated to the initial position, the motor controller sends data containing frame ID 0xAB to the controller, notifying it that block data can be sent. Before each transmission of path data, a CAN message frame is first sent, where the frame ID of this CAN message frame is 0xAB. The data frame portion of the CAN message frame uses the i-th bit to represent the i-th joint. When the i-th bit is 0, it means that the i-th joint does not accept data; when the i-th bit is 1, it means that the i-th joint has entered path planning mode and is ready to receive the block data for that joint.

[0042] Furthermore, in step S2, the controller packages the path planning block data and sends it to the actuator in the following order: first, the first block of each joint on the robotic arm is sent sequentially; then, the second block of each joint on the robotic arm is sent sequentially; then, the next block of each joint on the robotic arm is sent sequentially, until the last block of each joint is sent.

[0043] This allows each joint to move along a given path. Furthermore, since the actuator can identify joints based on frame IDs, the transmission order of blocks with the same sequence number across joints can be changed. For example, the transmission order of the first block of the first joint, the first block of the second joint, the first block of the third joint, and so on, up to the first block of the last joint, can be changed. That is, the transmission order of the first block of the sixth joint can be before or after the first block of the first joint.

[0044] Furthermore, in step S5, when using a timer to control the speed of the microstepping motor, in planning mode, after the timer interrupt calls the microstepping motor control function, the block data in the circular buffer is read. If there is no block in the circular buffer, the system waits for the next interrupt cycle and continues to read the block data.

[0045] If a block exists in the circular buffer, then check the validity of the current block's data based on the data in the block, and whether the current block is in a constant-speed or variable-speed phase. If it is in a variable-speed phase, then calculate the delay period P for each microstep i calculated from the segment. i And assign it to the timer; if it is a non-variable speed phase, just keep the timer's default period and enter the next interrupt cycle.

[0046] When checking the validity of the current block data, the main checks are that the block time t0 should be greater than 0 and V2 should be less than the motor's maximum speed. If the data is invalid during each validity check, the block is discarded.

[0047] Furthermore, in step S5, 2 k The microstepping motor is driven by sine and cosine signals, where 5≤k≤7.

[0048] Using 2^k as the number of microsteps maintains relatively simple control logic. For example, when dividing a complete step angle into 2^k microsteps, controlling the motor only requires binary counting, which is relatively easy to implement. If the number of steps is too small, the motor's accuracy is too low, resulting in significant vibration and less smooth movement; if the number of steps is too large, the motor consumes more power and generates excessive heat. The optimal value for k is 6, meaning 64 microsteps are used to control the microstepping motor with sine and cosine signals.

[0049] Compared with the prior art, the beneficial effects of the present invention are:

[0050] (1) The present invention decomposes the stepping angle of the micro-stepping motor in the joint of the robotic arm into tiny differential angles, thereby reducing the vibration and shock of the robotic arm during movement, improving the stability and accuracy of the robotic arm, achieving smoother movement, and thus being suitable for high-precision operation.

[0051] (2) The PWM pulse control of the motor rotation is achieved by controlling the voltage of the stepper motor. The micro-stepping motor can achieve very precise position control, enabling the robotic arm to move accurately to the designated position. At the same time, the present invention calls the motor control function through hardware interrupt, which is more accurate than the multi-threaded method.

[0052] (3) Motion planning for the stepper motor in four cases is carried out to ensure that the stepper motor reaches the corresponding number of microsteps S within a given time t0. The microstepper motor achieves the predetermined position, angle and speed within a predetermined time.

[0053] (4) The motor control parameters are calculated in advance by the planner and the parameters are approximated by Taylor sequence, which can greatly reduce the amount of calculation of the motor controller and thus ensure the real-time performance of the motor. Attached Figure Description

[0054] Figure 1 This is a schematic diagram of the path planning signal of the present invention;

[0055] Figure 2 This is a schematic diagram illustrating the invention of first performing uniformly accelerated motion and then uniform motion.

[0056] Figure 3This is a schematic diagram illustrating the invention of first performing uniform motion and then uniformly accelerated motion.

[0057] Figure 4 This is a schematic diagram illustrating the invention of first performing uniformly decelerated motion and then uniformly accelerated motion.

[0058] Figure 5 The illustration shows the present invention, which first undergoes uniform motion and then undergoes uniform deceleration motion. Detailed Implementation

[0059] The present invention will be further described below with reference to specific embodiments. The accompanying drawings are for illustrative purposes only, representing schematic diagrams rather than actual physical objects, and should not be construed as limiting the scope of this patent. To better illustrate the embodiments of the present invention, some components in the drawings may be omitted, enlarged, or reduced, and do not represent the actual dimensions of the product. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0060] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present patent. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0061] Example 1

[0062] A path planning and control method for a microstepping motor of a robotic arm includes the following steps:

[0063] S1: After receiving the path planning signal, the controller sends a signal to notify the actuator to enter the planning mode. The actuator sends a signal to notify the stepper motors on each joint of the robotic arm to rotate to the initial position of the path planning, thereby completing the initialization of each joint of the robotic arm.

[0064] S2: After completing the initialization of all joints, the controller packages the path planning block data and sends it to the actuator. Each block data includes the number of microsteps S that the stepper motor rotates in this block, the final speed V2 of the stepper motor in this block, and the duration t0 of this block.

[0065] S3: The planner checks the received data. If the block is invalid, the planner returns an exception message. If the block is valid, it first checks whether the speed and direction of the motor have changed. If they have not changed, the block is inserted into the circular buffer normally. If the direction has changed, the block is divided into two blocks with positive and negative speeds respectively at 0 speed and inserted into the circular buffer.

[0066] S4: When a block is inserted into the circular buffer, the planner reads the block data in the circular buffer and calculates the acceleration of the block, as well as the delay period generated by each microstep PWM pulse of the block;

[0067] S5: Based on the calculated delay period, a timer is used to generate PWM pulses for controlling the microstepping motor through periodic interrupts, thereby controlling the speed change of the microstepping motor.

[0068] like Figure 1 As shown, the total reduction ratio from the stepper motor to the output shaft is known, denoted as R. Therefore, for every degree the joint rotates, the corresponding number of microsteps S = R * 8.888888889. Figure 1 In this context, for each joint, the area between two adjacent key points is called a block, which is the portion between two straight lines.

[0069] The path planning signal is obtained from the robot arm trajectory planning algorithm running on the host computer, which generates the robot arm's subsequent path curve and path planning signal. This invention provides the path curve for each joint on the robot arm in the path planning signal. Through method planning, it controls the micro-stepping motors on the robot arm to reach a predetermined position, angle, and speed at a predetermined time, thereby achieving precise operation of the robot arm.

[0070] The controller's main function is to send signals indicating whether to enter planning mode, key points of the path planning curve, and to receive feedback information from the stepper motor. The actuator module's main function is to parse the data from the controller. Often, the controller runs on a high-performance host computer, so it needs to send data to the slave computer to complete instructions. After parsing, the actuator determines whether to enter planning mode. If so, it sends the block data received from the controller to the planner. One thread of the planner receives the block data sent by the actuator and checks the validity of the microsteps S, final speed V2, and block duration t0. Specifically, it checks that the block time t0 should be greater than 0 and V2 should be less than the motor's maximum speed. If valid, the block data is inserted into the circular buffer. Another thread of the planner continuously reads the block data from the circular buffer. Because parameter calculation is relatively slow, one thread of the planner handles block insertion, while the other thread reads blocks from the buffer for parameter calculation. This dual-thread approach is more efficient. The planner's parameter calculation function then uses this data to calculate the motor control parameters required for the block to reach speed V2 given S and t0. Based on the motor control parameters calculated by the planner, it calculates the PWM pulse delay required for the next microstep, thereby controlling the motor speed. Then, in the next timer interrupt, the PWM pulse generator is called to control the motor movement. The motor controller reads the block data from the circular buffer via hardware interrupts. If no block is found, the motor waits; if valid block data is read, the motor can be controlled to move based on the parameters calculated in the block.

[0071] In step S5, when calculating acceleration, if the number of steps S0 required for the stepper motor to change uniformly from speed V1 to a specified speed V2 is different from the given number of steps S, the motion of the block microstepper motor is planned:

[0072] When V1 < V2:

[0073] like Figure 2 As shown, when the given number of microsteps S is greater than the number of microsteps S0 for uniform acceleration throughout the entire process, the vehicle first undergoes uniform acceleration to a speed V2, and then undergoes uniform motion.

[0074] The time of uniform motion is: The acceleration of uniformly accelerated motion: ;

[0075] like Figure 3 As shown, when the given number of microsteps S is less than the number of microsteps S0 for uniform acceleration throughout the entire process, the vehicle first moves at a constant speed, and then moves at a constant speed until it reaches a velocity V2.

[0076] The time of uniform motion is: The acceleration of uniformly accelerated motion: ;

[0077] When V1 > V2:

[0078] like Figure 4 As shown, when the given number of microsteps S is less than the number of microsteps S0 for uniform acceleration throughout the entire process, the motion first undergoes uniform deceleration to a speed V2, and then undergoes uniform motion:

[0079] The time of uniform motion is: The acceleration of uniformly decelerated motion: .

[0080] like Figure 5 As shown, when the given number of microsteps S is greater than the number of microsteps S0 for uniform acceleration throughout the entire process, the vehicle first moves at a constant speed, and then moves at a constant speed until it reaches a velocity V2.

[0081] The time of uniform motion is: The acceleration of uniformly decelerated motion: .

[0082] Within a block, the stepper motor needs to rotate a number of microsteps S within a time t0. While completing this number of microsteps, the initial velocity V1 of the block also needs to change to V2. Therefore, the motion of each block needs to be planned. This invention adopts a segmented motion that combines uniform acceleration and uniform motion. Uniform acceleration motion occurs within time t1, with acceleration a, and uniform motion occurs within time t2, where t0 = t1 + t2.

[0083] In step S5, when calculating the delay period, each microstep i needs to achieve the corresponding speed V. i The delay period P required to generate PWM i It is determined by the timer frequency F and the motor speed V. i Jointly decided, that is Therefore, it is necessary to first calculate the speed V of the corresponding microstepping motor that each microstep i needs to reach. i Then calculate the delay period P for each microstep. i .

[0084] In step S5, the speed V that each microstep i needs to reach for the corresponding microstep motor is calculated. i The method is as follows:

[0085] .

[0086] In uniformly accelerated motion , ,in This represents the number of microsteps taken by the stepper motor. V represents the initial velocity of the microstep, and V is the number of microsteps taken. The target velocity after that. Combining the two equations, we get: Therefore, for each microstep i, Therefore, the speed after this microstep is completed .in, This refers to the initial velocity of the microstep or the velocity after the previous microstep was completed.

[0087] In step S5, the delay period P of each microstep is calculated. i The process is as follows:

[0088] .

[0089] Depend on and We can obtain:

[0090] .

[0091] In step S5, the delay period P of each microstep is calculated. i When using Taylor's formula ,when ,make ,

[0092] Then P i It can be simplified to:

[0093]

[0094] Among them, when in the acceleration phase When in the uniform velocity phase, m=0; when in the deceleration phase... ;

[0095] Delay period during the initial stage: ;

[0096] The delay period during the termination phase: .

[0097] Example 2

[0098] This embodiment is similar to Embodiment 1, except that it further includes:

[0099] In step S2, when the controller sends the block data to the actuator, it uses a CAN information frame to package and send the block data. The format of the CAN information frame includes a frame ID and block data. The frame ID is 0xA + a joint number. The first 4 bytes of the block data form a 32-bit int data, representing S; the 5th and 6th bits form a 16-bit int data, representing V2; and the 7th and 8th bits form a 16-bit uint data, representing t0.

[0100] A CAN information frame includes a frame ID and a data frame. When sending path planning signals, the data frame is block data. The frame ID is 0xA + one bit of the joint number. For example, if the first joint number is 1, the frame ID is 0xA1; if the second joint number is 2, the frame ID is 0xA2.

[0101] In step S1, the steps for determining whether to enter the planning mode are as follows:

[0102] The controller first sends data containing frame ID 0xAA to the actuator via a CAN message frame to notify it to enter path planning mode. The actuator parses the instruction and instructs the motor actuator to control the motor to rotate to the initial position. Once the motor has rotated to the initial position, the motor controller sends data containing frame ID 0xAB to the controller, notifying it that block data can be sent. Before each transmission of path data, a CAN message frame is first sent, where the frame ID of this CAN message frame is 0xAB. The data frame portion of the CAN message frame uses the i-th bit to represent the i-th joint. When the i-th bit is 0, it means that the i-th joint does not accept data; when the i-th bit is 1, it means that the i-th joint has entered path planning mode and is ready to receive the block data for that joint.

[0103] In step S2, the controller packages the path planning block data and sends it to the actuator. The sending order is as follows: first, the first block of each joint on the robotic arm is sent sequentially; then, the second block of each joint on the robotic arm is sent sequentially; then, the next block of each joint on the robotic arm is sent sequentially, until the last block of each joint is sent.

[0104] This allows each joint to move along a given path. Furthermore, since the actuator can identify joints based on frame IDs, the transmission order of blocks with the same sequence number across joints can be changed. For example, the transmission order of the first block of the first joint, the first block of the second joint, the first block of the third joint, and so on, up to the first block of the last joint, can be changed. That is, the transmission order of the first block of the sixth joint can be before or after the first block of the first joint.

[0105] In this embodiment, the block data transmission order is as follows: first joint first block → second joint first block → ... last joint first block → first joint second block → second joint second block → ... last joint second block ... → first joint last block → second joint last block → ... last joint last block.

[0106] Example 3

[0107] In step S5, when using a timer to control the speed of the microstepping motor, in planning mode, after the timer interrupt calls the microstepping motor control function, the block data in the circular buffer is read. If there is no block in the circular buffer, wait for the next interrupt cycle and continue to read the block data.

[0108] If a block exists in the circular buffer, then check the validity of the current block's data based on the data in the block, and whether the current block is in a constant-speed or variable-speed phase. If it is in a variable-speed phase, then calculate the delay period P for each microstep i calculated from the segment. i And assign it to the timer; if it is a non-variable speed phase, just keep the timer's default period and enter the next interrupt cycle.

[0109] When checking the validity of the current block data, the main checks are that the block time t0 should be greater than 0 and V2 should be less than the motor's maximum speed. If the data is invalid during each validity check, the block is discarded.

[0110] In step S5, 2 k The microstepping motor is driven by sine and cosine signals, where 5≤k≤7.

[0111] Using 2^k as the number of microsteps maintains relatively simple control logic. For example, when dividing a complete step angle into 2^k microsteps, controlling the motor only requires binary counting, which is relatively easy to implement. If the number of steps is too small, the motor's accuracy is too low, resulting in significant vibration and less smooth movement; if the number of steps is too large, the motor consumes more power and generates excessive heat. The optimal value for k is 6, meaning 64 microsteps are used to control the microstepping motor with sine and cosine signals.

[0112] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the implementation of the present invention. Those skilled in the art can make other variations or modifications based on the above description. It is neither necessary nor possible to exhaustively describe all embodiments here. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the claims of the present invention.

Claims

1. A path planning and control method for a microstepping motor of a robotic arm, characterized in that, Includes the following steps: S1: After receiving the path planning signal, the controller sends a signal to notify the actuator to enter the planning mode. The actuator sends a signal to notify the stepper motors on each joint of the robotic arm to rotate to the initial position of the path planning, thereby completing the initialization of each joint of the robotic arm. S2: After completing the initialization of all joints, the controller packages the path planning block data and sends it to the actuator. Each block data includes the number of microsteps S that the stepper motor rotates in this block, the final speed V2 of the stepper motor in this block, and the duration t0 of this block. S3: The planner checks the received data. If the block is invalid, the planner returns an exception message. If the block is valid, it first checks whether the speed and direction of the motor have changed. If they have not changed, the block is inserted into the circular buffer normally. If the direction has changed, the block is divided into two blocks with positive and negative speeds respectively at 0 speed and inserted into the circular buffer. S4: When a block is inserted into the circular buffer, the planner reads the block data in the circular buffer and calculates the acceleration of the block, as well as the delay period generated by each microstep PWM pulse of the block; S5: Based on the calculated delay period, a timer is used to generate PWM pulses for controlling the microstepping motor through periodic interrupts, thereby controlling the speed change of the microstepping motor.

2. The path planning and control method for the microstepping motor of the robotic arm according to claim 1, characterized in that, In step S5, when calculating acceleration, if the number of steps S0 required for the stepper motor to change uniformly from speed V1 to a specified speed V2 is different from the given number of steps S, the motion of the block microstepper motor is planned: When V1 < V2: When the given number of microsteps S is greater than the number of microsteps S0 for uniform acceleration throughout the entire process, the motion first undergoes uniform acceleration to a speed V2, and then undergoes uniform motion: The time of uniform motion is: The acceleration of uniformly accelerated motion: ; When the given number of microsteps S is less than the number of microsteps S0 for uniform acceleration throughout the entire process, the motion first proceeds at a constant speed, and then proceeds at a constant acceleration to a speed V2. The time of uniform motion is: The acceleration of uniformly accelerated motion: ; When V1 > V2: When the given number of microsteps S is less than the number of microsteps S0 for uniform acceleration throughout the entire process, the motion first undergoes uniform deceleration to a speed V2, and then undergoes uniform motion: The time of uniform motion is: The acceleration of uniformly decelerated motion: ; When the given number of microsteps S is greater than the number of microsteps S0 for uniform acceleration throughout the entire process, the vehicle first moves at a constant speed, and then moves at a constant speed until it reaches a velocity V2. The time of uniform motion is: The acceleration of uniformly decelerated motion: .

3. The path planning and control method for the microstepping motor of the robotic arm according to claim 2, characterized in that, In step S5, when calculating the delay period, each microstep i needs to achieve the corresponding speed V. i The delay period P required to generate PWM i It is determined by the timer frequency F and the motor speed V. i Jointly decided, that is Therefore, it is necessary to first calculate the speed V of the corresponding microstepping motor that each microstep i needs to reach. i Then calculate the delay period P for each microstep. i .

4. The path planning and control method for the microstepping motor of the robotic arm according to claim 3, characterized in that, In step S5, the speed V that each microstep i needs to reach for the corresponding microstep motor is calculated. i The method is as follows: 。 5. The path planning and control method for the microstepping motor of the robotic arm according to claim 4, characterized in that, In step S5, the delay period P of each microstep is calculated. i The process is as follows: 。 6. The path planning and control method for the microstepping motor of the robotic arm according to claim 5, characterized in that, In step S5, the delay period P of each microstep is calculated. i When using Taylor's formula ,when ,make , Then P i It can be simplified to: Among them, when in the acceleration phase When in the uniform speed phase When in the deceleration phase ; Delay period during the initial stage: ; The delay period during the termination phase: .

7. The path planning and control method for the microstepping motor of the robotic arm according to any one of claims 1 to 6, characterized in that, In step S2, when the controller sends the block data to the actuator, it uses a CAN information frame to package and send the block data. The format of the CAN information frame includes a frame ID and block data. The frame ID is 0xA + a joint number. The first 4 bytes of the block data form a 32-bit int data, representing S; the 5th and 6th bits form a 16-bit int data, representing V2; and the 7th and 8th bits form a 16-bit uint data, representing t0.

8. The path planning and control method for the microstepping motor of the robotic arm according to claim 7, characterized in that, In step S2, the controller packages the path planning block data and sends it to the actuator. The sending order is as follows: first, the first block of each joint on the robotic arm is sent sequentially; then, the second block of each joint on the robotic arm is sent sequentially; then, the next block of each joint on the robotic arm is sent sequentially, until the last block of each joint is sent.

9. The path planning and control method for the microstepping motor of the robotic arm according to any one of claims 1 to 6, characterized in that, In step S5, when using a timer to control the speed of the microstepping motor, in planning mode, after the timer interrupt calls the microstepping motor control function, the block data in the circular buffer is read. If there is no block in the circular buffer, wait for the next interrupt cycle and continue to read the block data. If a block exists in the circular buffer, then check the validity of the current block's data based on the data in the block, and whether the current block is in a constant-speed or variable-speed phase. If it is in a variable-speed phase, then calculate the delay period P for each microstep i calculated from the segment. i And assign it to the timer; if it is a non-variable speed phase, just keep the timer's default period and enter the next interrupt cycle.

10. The path planning and control method for the microstepping motor of the robotic arm according to claim 9, characterized in that, In step S5, 2 k The microstepping motor is driven by sine and cosine signals, where 5≤k≤7.

Images