Multi-axis motion control method and system

Abstract

The application discloses a kind of multi-axis motion control method and system, method includes: obtaining user motion instruction and according to user motion instruction, the planning position of motor is generated, and according to planning position, motor is driven to work;Several set positions are arranged in the path of planning position;The actual position of motor is obtained, and actual position is compared with set position;When the actual position of motor reaches first set position range, trigger output control signal, or, when the actual position of motor reaches second set position range, and the actual position of motor deviates towards the direction away from set position, trigger output control signal.The actual position of motor is detected in the application, and when detecting that the actual position of motor enters into set position range or enters into second set position range and moves away from set position, output control signal is triggered, so that even if motor cannot reach specified position, corresponding action control can be executed by outputting control signal.

Description

Technical Field

[0001] This invention relates to the field of industrial control technology, and in particular to a multi-axis motion control method and system. Background Technology

[0002] The high-speed, high-precision motion control system has a position comparison function. The position comparison function means that when the motor reaches the set position of the planned path, the controller can output a control signal to achieve the corresponding control. For example, it can realize some functions such as taking pictures, spraying glue, and laser marking.

[0003] However, due to the lag and mechanical vibration of the motor, in multi-axis (e.g., planar motion) movements, the actual motor may not reach the specified position. This will cause the position comparison function to fail, thus preventing the output of control signals to execute the corresponding motion control.

[0004] Therefore, existing technologies still need to be improved and developed. Summary of the Invention

[0005] In view of the shortcomings of the prior art, the purpose of this invention is to provide a multi-axis motion control method and system to solve the problem that the position comparison function cannot be realized in the existing high-speed and high-precision motion control system during multi-axis motion due to motor lag and jitter.

[0006] The technical solution of the present invention is as follows:

[0007] In a first aspect, the present invention provides a multi-axis motion control method, comprising:

[0008] The system acquires user motion commands and generates a planned position for the motor based on the user motion commands, and drives the motor to work according to the planned position; the path of the planned position is set with several preset positions;

[0009] Obtain the actual position of the motor and compare the actual position with the set position;

[0010] When the actual position of the motor reaches the first set position range, the output control signal is triggered; or, when the actual position of the motor reaches the second set position range and the actual position of the motor deviates in a direction away from the set position, the output control signal is triggered; wherein, the second set position range is greater than the first set position range.

[0011] The present invention further includes, in this embodiment, the multi-axis motion control method further comprising:

[0012] When the planned location is detected to have reached the set position, a timer is started at the coordinate point where the planned location reaches the set position.

[0013] When the timing duration is detected to have reached the preset duration and the actual position of the motor has not reached the second set position range, the output control signal is triggered.

[0014] The present invention further includes the step of triggering the output control signal when the actual position of the motor reaches the second preset position range and the actual position of the motor deviates in a direction away from the preset position:

[0015] When the actual position of the motor reaches the second set position range and the actual position of the motor moves toward the first set position range, wait for the trigger output control signal;

[0016] When the actual position of the motor moves from the second set position range into the first set position range, the output control signal is triggered.

[0017] When the actual position of the motor enters the second set position range and the actual position of the motor moves away from the first set position range, the output control signal is triggered.

[0018] The present invention further includes the step of triggering the output control signal when the actual position of the motor enters the second preset position range and the actual position of the motor moves away from the first preset position range:

[0019] When the actual position of the motor is detected to be moving away from the first set position range and the distance of the movement exceeds the preset distance, the output control signal is triggered.

[0020] The present invention further includes a step in which the motor filtering and anti-shake function is triggered when the actual position of the motor enters the second set position range and the actual position of the motor moves away from the first set position range.

[0021] In a further embodiment of the present invention, the preset duration is set according to the motor's following error and the planned speed.

[0022] The present invention further provides that the expression for calculating the preset duration is:

[0023] t^2=(a^2+b^2) / (v^2);

[0024] Where t represents the preset duration, v represents the current planned synthesis speed, a represents the x-axis following error, and b represents the y-axis following error.

[0025] Secondly, the present invention also provides a multi-axis motion control system for implementing the multi-axis motion control method described above, comprising: a motion controller, a motor, a mechanical structure, and a grating sensor;

[0026] The motion controller is connected to the motor, and the motion controller is used to drive the motor to work according to the planned position;

[0027] The mechanical structure is mounted on the motor;

[0028] The grating sensor is connected to the mechanical structure and the motion controller respectively. The grating is used to detect the actual position of the motor and feed it back to the motion controller.

[0029] The motion controller is also used to compare the actual position of the motor with the planned position to determine whether the actual position of the motor is within a first set position range or a second set position range, and to trigger an output control signal when the actual position of the motor reaches the first set position range, and to trigger an output control signal when the actual position of the motor reaches the second set position range and the actual position of the motor deviates in a direction away from the set position.

[0030] The invention further provides that the motion controller includes: a position comparison point first-in-first-out queue, an X-axis grating position counter, a Y-axis grating position counter, a position comparison unit, and a motion planning unit;

[0031] The first-in-first-out queue of the position comparison points is connected to the position comparison unit and is used to control the triggering order of each set position in the planned path of the motor.

[0032] The X-axis grating position counter is connected to the position comparison unit and is used to count the position coordinates of the motor on the X-axis.

[0033] The Y-axis grating position counter is connected to the position comparison unit and is used to count the position coordinates of the motor on the Y-axis.

[0034] The position comparison unit is used to compare the actual position of the motor with the set position, and triggers the output control signal when the actual position of the motor reaches the first set position range, or when the actual position of the motor reaches the second set position range and the actual position of the motor deviates in a direction away from the set position, or when the timing is started when the set position is detected to have been reached in the planned position, and the timing duration is detected to have reached the preset duration, and the actual position of the motor has not reached the second set position range, then the output control signal is triggered.

[0035] The motion planning unit is connected to the position comparison unit, and the motion planning unit is used to generate the planned position of the motor according to the user's motion command.

[0036] The present invention further comprises, wherein the position comparison unit includes: a first subtractor, a second subtractor, a third subtractor, a fourth subtractor, a first differential velocity calculation unit, a second differential velocity calculation unit, a first judgment unit, a second judgment unit, a third judgment unit, a fourth judgment unit, a timeout duration calculation unit, a fifth judgment unit, and AND and OR gate circuits; wherein,

[0037] The first subtractor is used to subtract the X-axis grating position from the X-axis comparison position point;

[0038] The second subtractor is used to subtract the Y-axis grating position from the Y-axis comparison position point;

[0039] The third subtractor is used to subtract the X-axis comparison position from the X-axis planned position;

[0040] The fourth subtractor is used to subtract the Y-axis comparison position from the Y-axis planned position;

[0041] The first judgment unit is connected to the first subtractor and the second subtractor. The first judgment unit is used to determine whether the actual position of the motor enters the first set position range.

[0042] The second judgment unit is connected to the first subtractor and the second subtractor respectively, and the second judgment unit is used to determine whether the actual position of the motor enters the second set position range;

[0043] The third judgment unit is connected to the first subtractor and the second subtractor respectively, and the third judgment unit is used to determine whether the actual position of the motor deviates from the set position.

[0044] The fourth judgment unit is connected to the third subtractor and the fourth subtractor respectively, and the fourth judgment unit is used to determine whether the actual position of the motor has reached the planned position;

[0045] The first differential speed calculation unit is used to calculate the speed of the motor at the X-axis grating position;

[0046] The second differential speed calculation unit is used to calculate the speed of the motor at the position of the Y-axis grating;

[0047] The timeout duration calculation unit is connected to the first differential speed calculation unit and the second differential speed calculation unit respectively. The timeout duration calculation unit is used to calculate the waiting time based on the movement speed of the motor at the X-axis grating position and the movement speed at the Y-axis grating position.

[0048] The fifth judgment unit is connected to the fourth judgment unit and the timeout duration calculation unit respectively, and the second judgment unit is used to determine whether the waiting time has reached the preset duration.

[0049] The AND gate circuit is connected to the second judgment unit and the third judgment unit respectively;

[0050] The OR gate circuit is connected to the first judgment unit, the AND gate circuit, and the fifth judgment unit respectively. The OR gate circuit is used to determine whether to output a control signal based on the output signals of the first judgment unit, the AND gate circuit, and the fifth judgment unit.

[0051] This invention provides a multi-axis motion control method and system. The method includes: acquiring user motion commands and generating a planned position for a motor based on the user motion commands, and driving the motor to work according to the planned position; the planned position has several preset positions along its path; acquiring the actual position of the motor and comparing the actual position with the preset positions; triggering an output control signal when the actual position of the motor reaches a first preset position range, or triggering an output control signal when the actual position of the motor reaches a second preset position range and the actual position of the motor deviates from the preset position; wherein the second preset position range is greater than the first preset position range. This invention detects the actual position of the motor and compares it with the planned position. When the actual position of the motor enters the first preset position range, or enters the second preset position range and the motor moves away from the preset position, an output control signal is triggered. That is, the actual position of the motor can have a certain error range, so even if the motor cannot reach the specified position, the motion controller can still output a control signal to execute corresponding motion control. Attached Figure Description

[0052] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0053] Figure 1 This is a flowchart illustrating the multi-axis motion control method of this invention.

[0054] Figure 2 It is a schematic diagram of the curves of the ideal motion path and the actual motion path in multi-axis motion.

[0055] Figure 3 This is a block diagram illustrating the principle of the multi-axis motion control system in this invention.

[0056] Figure 4 This is a schematic diagram of the working principle of the motion controller in one embodiment of the present invention.

[0057] Figure 5 This is a schematic diagram of the working principle of the position comparison unit in one embodiment of the present invention.

[0058] The labels in the attached diagram are as follows: 100, Motion Controller; 110, Position Comparison Point FIFO Queue; 120, X-axis Grating Position Counter; 130, Y-axis Grating Position Counter; 140, Position Comparison Unit; 160, Motion Planning Unit; 141, First Subtractor; 142, Second Subtractor; 143, Third Subtractor; 144, Fourth Subtractor; 145, First Differential Velocity Calculation Unit; 146, Second Differential Velocity Calculation Unit; 147, First Judgment Unit; 148, Second Judgment Unit; 149, Third Judgment Unit; 150, Fourth Judgment Unit; 151, Timeout Duration Calculation Unit; 152, Fifth Judgment Unit; 153, AND Gate Circuit; 154, OR Gate Circuit; 200, Motor; 300, Mechanical Structure; 400, Grating Sensor. Detailed Implementation

[0059] This invention provides a multi-axis motion control method and system. To make the objectives, technical solutions, and effects of this invention clearer and more explicit, the invention will be further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0060] In the implementation methods and claims, unless otherwise specified in the text, the terms "a," "an," "the," and "the" may also include plural forms. If the embodiments of the present invention involve descriptions of "first," "second," etc., such descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of indicated technical features. Therefore, a feature defined with "first" or "second" may explicitly or implicitly include at least one of those features.

[0061] It should be further understood that the term "comprising" as used in this specification means the presence of the stated features, integers, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof. It should be understood that when an element is referred to as "connected" or "coupled" to another element, it can be directly connected or coupled to the other element, or there may be intermediate elements present. Furthermore, "connected" or "coupled" as used herein can include wireless connections or wireless coupling. The term "and / or" as used herein includes all or any of the units and all combinations thereof of one or more associatedly listed items.

[0062] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the same meaning as in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless specifically defined as herein.

[0063] Furthermore, the technical solutions of the various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such combination of technical solutions does not exist and is not within the scope of protection claimed by this invention.

[0064] Please also refer to Figure 1 The present invention provides a preferred embodiment of a multi-axis motion control method and system.

[0065] In some embodiments, the present invention provides a multi-axis motion control method, such as... Figure 1 As shown, it includes the following steps:

[0066] S100: Obtain user motion commands and generate a planned position for the motor based on the user motion commands, and drive the motor to work according to the planned position; the path of the planned position is set with several preset positions;

[0067] Specifically, multi-axis motion refers to motion on the plane formed by the X and Y axes, and motion in three-dimensional space formed by the X, Y, and Z axes. The planned position refers to the position of the control motor's travel path, i.e., the planned path. The motor's planned position is a real-time planned position generated through user motion commands. Several preset positions, i.e., position comparison points, are set within the planned position. Theoretically, when the motor's actual position reaches one of the preset positions, a control signal is triggered to achieve the position comparison function. This is applicable to scenarios requiring position comparison, such as taking photos, laser marking, and glue spraying. Taking mobile phone glue dispensing as an example, when the motor's actual position reaches the glue dispensing position on the phone, the motion controller outputs a control signal to trigger the glue dispensing machine to spray glue.

[0068] S200: Obtain the actual position of the motor and compare the actual position with the set position;

[0069] Specifically, due to issues such as motor lag and mechanical vibration, the motor may fail to reach the set position, for example... Figure 2 As shown, Figure 2 The dashed line represents the ideal path, while the solid line represents the actual movement curve, which does not pass through point A. To determine the motor's actual position, it is necessary to monitor it in real time. By comparing the motor's actual position with the set position, the distance between the motor and the set position can be determined.

[0070] S300. When the actual position of the motor reaches the first set position range, an output control signal is triggered; or, when the actual position of the motor reaches the second set position range and the actual position of the motor deviates in a direction away from the set position, an output control signal is triggered; wherein, the second set position range is greater than the first set position range.

[0071] Specifically, the first set position range and the second set position range are the error ranges of the position comparison points. In other words, the actual position of the motor is an ideal position within both the first and second set position ranges, meaning that the position comparison function can be implemented within both ranges. Both the first and second set position ranges are circular areas with a certain coverage area, formed by circling the set positions.

[0072] In this embodiment, the second set position range is larger than the first set position range, meaning the error of the first set position range is smaller than the second set error position range. In this embodiment, as long as the actual position of the motor enters the first set position range, it indicates that the position accuracy is relatively high, and an output control signal can be triggered. When the actual position of the motor is within the second set position range, and the actual position of the motor gradually deviates from the set position, it indicates that the motor position is the optimal position, and an output control signal can be triggered.

[0073] In the above technical solution, the present invention detects the actual position of the motor and compares the actual position of the motor with the planned position. When the actual position of the motor is detected to enter the first set position range or the second set position range and the motor moves away from the set position, the output control signal can be triggered. That is, the actual position of the motor can have a certain error range. In this way, even if the motor cannot reach the specified position, the motion controller can output the control signal to execute the corresponding action control.

[0074] In some embodiments, the multi-axis motion control method further includes the step of:

[0075] S400: When it is detected that the planned position has reached the set position, start timing when the planned position reaches the coordinate point of the set position;

[0076] S500: When the timing duration is detected to have reached the preset duration and the actual position of the motor has not reached the second set position range, the output control signal is triggered.

[0077] In this embodiment, if the motor reaches the set position according to the planned path, but the actual position of the motor has not reached the set position and is not within the error range (i.e., the actual position of the motor is not within the range of the second set position), the output control signal will not be triggered. Thus, in practical applications, the position comparison function cannot be implemented at this set position, and because the position comparison function is not triggered, the motor will remain stationary, unable to perform position comparison at the next set position. Therefore, to ensure that the output signal is triggered at each position comparison point, in this embodiment, when it is detected that the set position has been reached in the planned position, but the actual position of the motor is not within the error range, a timer will start. When the timer reaches a preset duration and the actual position of the motor has not reached the range of the second set position, the output control signal is triggered, thereby ensuring that a control signal is output at each position comparison point.

[0078] In some embodiments, the preset duration is set based on the motor's following error and the planned speed.

[0079] In this embodiment, timing begins when the motor is detected to have reached the set position in the planned position. If the motor has not entered the second set position range within a preset time period, it indicates that it cannot enter the error range, and an output control signal needs to be triggered immediately. The preset time period is a timeout point predicted based on the motor's following error and the planned speed, and timing begins when the planned position reaches the set position (i.e., the position comparison point).

[0080] The expression for calculating the preset duration is as follows:

[0081] t^2=(a^2+b^2) / (v^2);

[0082] Where t represents the timeout duration, v represents the current planned synthesis speed, a represents the x-axis following error, and b represents the y-axis following error.

[0083] In some embodiments, the step of triggering the output control signal when the actual position of the motor reaches a second preset position range and the actual position of the motor deviates in a direction away from the preset position includes:

[0084] S310. When the actual position of the motor reaches the second set position range and the actual position of the motor moves toward the first set position range, wait for the trigger output control signal.

[0085] S320: When the actual position of the motor moves from the second set position range into the first set position range, an output control signal is triggered.

[0086] S330. When the actual position of the motor enters the second set position range and the actual position of the motor moves away from the first set position range, the output control signal is triggered.

[0087] In this embodiment, when the actual position of the motor is detected to have entered the second preset position range, it is necessary to further detect whether the current actual position of the motor is moving closer to the preset position or gradually moving away from the preset position. If it is moving closer to the preset position, it indicates that it may enter the first preset position range, which can improve the accuracy of the position comparison function. Therefore, it is possible to wait a certain period of time before triggering the output control signal. If the actual position of the motor is detected to be gradually moving away from the coordinates of the preset position, it indicates that the current position is the optimal coordinate position for implementing the position comparison function, and the output control signal should be triggered as soon as possible.

[0088] In some embodiments, the step of triggering the output control signal when the actual position of the motor enters the second preset position range and the actual position of the motor moves away from the first preset position range includes:

[0089] S331. When it is detected that the actual position of the motor is moving away from the first set position range and the distance away exceeds the preset distance, the output control signal is triggered.

[0090] In this embodiment, to avoid the deviation caused by the vibration of the motor itself during low-speed movement being mistakenly detected as the motor's actual position moving away from the set position, the current position is only confirmed as the optimal position and the output control signal is triggered when the motor's actual position moves a certain range away from the set position. In some embodiments, the preset distance is 10-20 micrometers, for example, it can be 10 micrometers, 15 micrometers, or 20 micrometers.

[0091] In some embodiments, in the step of when the actual position of the motor enters the second set position range and the actual position of the motor moves away from the first set position range, the motor filtering and anti-shake function is triggered. This can prevent false detection from causing the output control signal to be falsely triggered before the actual position of the motor has reached the optimal position.

[0092] In some embodiments, such as Figure 3 As shown, the present invention also provides a multi-axis motion control system for implementing the multi-axis motion control method described above, comprising: a motion controller 100, a motor 200, a mechanical structure 300, and a grating sensor 400. The motion controller 100 is connected to the motor 200 and is used to drive the motor 200 to work according to a planned position. The mechanical structure 300 is mounted on the motor 200. The grating sensor 400 is connected to both the mechanical structure 300 and the motion controller 100, and is used to detect the actual position of the motor 200 and feed it back to the motion controller 100. The motion controller 100 is also used to compare the actual position of the motor 200 with the planned position to determine whether the actual position of the motor 200 is within a first set position range or a second set position range. It is also used to trigger an output control signal when the actual position of the motor 200 reaches the first set position range, and to trigger an output control signal when the actual position of the motor 200 reaches the second set position range and the actual position of the motor 200 deviates in a direction away from the set position.

[0093] In this embodiment, the motion controller 100 is implemented using a Field-Programmable Gate Array (FPGA), enabling nanosecond-level judgment and output. Theoretically, the motor 200 will operate according to the planned position, performing a position comparison function at each set position and outputting corresponding control signals. The grating sensor 400 can detect the actual position of the motor 200 and feed it back to the motion controller 100, thus allowing the motion controller 100 to know the current position of the motor 200.

[0094] The motion controller 100 sets an error range for the motor 200. By comparing the actual position of the motor 200 with the planned position, it determines whether the actual position of the motor 200 is within a first or second set position range. It triggers an output control signal when the actual position of the motor 200 reaches the first set position range, and also triggers an output control signal when the actual position of the motor 200 reaches the second set position range and deviates from the set position. Furthermore, it starts timing when it detects that the planned position has been reached, and if the timing duration reaches a preset duration but the actual position of the motor 200 has not reached the second set position range, it triggers an output control signal. Thus, even if the motor 200 cannot reach the designated position, the motion controller 100 can still output a control signal at the ideal position to perform corresponding motion control.

[0095] In some embodiments, such as Figure 4As shown, the motion controller 100 includes: a position comparison point first-in-first-out queue 110, an X-axis grating position counter 120, a Y-axis grating position counter 130, a position comparison unit 140, and a motion planning unit 160; the position comparison point first-in-first-out queue 110 is connected to the position comparison unit 140 and is used to control the triggering order of each set position in the planned path of the motor 200; the X-axis grating position counter 120 is connected to the position comparison unit 140 and is used to count the position coordinates of the motor 200 on the X-axis; the Y-axis grating position counter 130 is connected to the position comparison unit 140 and is used to count the position coordinates of the motor 200 on the Y-axis; the position comparison unit 140 is used to... The actual position of motor 200 is compared with the set position, and an output control signal is triggered when the actual position of motor 200 reaches the first set position range, or when the actual position of motor 200 reaches the second set position range and the actual position of motor 200 deviates from the set position, or when a timer is started when the set position is detected in the planned position, and the timer duration is detected to reach the preset duration, and the actual position of motor 200 has not reached the second set position range, then an output control signal is triggered; the motion planning unit 1660 is connected to the position comparison unit 140, and the motion planning unit 160 is used to generate the planned position of motor 200 according to the user's motion command.

[0096] In this embodiment, the motion planning unit 160 can generate a real-time planned position of the motor 200 according to the user's motion command to drive the motor 200 to work, and simultaneously send the planned position to the position comparison unit 140. The X-axis grating position counter and the Y-axis grating position counter respectively count the position coordinates of the motor 200 on the X-axis and the position coordinates of the motor 200 on the Y-axis to obtain the actual position coordinates of the motor 200. The position comparison unit 140 compares the actual position of the motor 200 with the planned position, and can output a control signal when the actual position of the motor 200 enters the error range, and output a control signal when the actual position of the motor 200 reaches a preset time outside the error range.

[0097] In some embodiments, such as Figure 5As shown, the position comparison unit 140 includes: a first subtractor 141, a second subtractor 142, a third subtractor 143, a fourth subtractor 144, a first differential velocity calculation unit 145, a second differential velocity calculation unit 146, a first judgment unit 147, a second judgment unit 148, a third judgment unit 149, a fourth judgment unit 150, a timeout duration calculation unit 151, a fifth judgment unit 152, an AND gate circuit 153, and an OR gate circuit 154. The first subtractor 141 is used to subtract the X-axis grating position from the X-axis comparison position point; the second subtractor 142 is used to subtract the Y-axis grating position from the Y-axis comparison position point; the third subtractor 143 is used to subtract the X-axis comparison position point from the X-axis planned position; the fourth subtractor 144 is used to subtract the Y-axis comparison position point from the Y-axis planned position; the first judgment unit 147 is connected to the first subtractor 141 and the second subtractor 142, and the first judgment unit 147 is used to determine whether the actual position of the motor 200 has entered the first set position range; the third subtractor 143 is used to subtract the X-axis comparison position point from the planned X-axis position; the fourth subtractor 144 is used to subtract the Y-axis comparison position point from the planned Y-axis position; the fourth subtractor 145 is used to determine whether the actual position of the motor 200 has entered the first set position range; the fifth subtractor 146 is used to subtract the X-axis comparison position point from the planned X-axis position point; the sixth subtractor 147 is used to subtract the X-axis comparison position point from the planned X-axis position point; the seventh subtractor 142 is used to subtract the X-axis comparison position point from the planned X-axis position point; the eighth subtractor 143 is used to subtract the X-axis comparison position point from the planned X-axis position point; the ninth subtractor 144 is used to subtract the X-axis comparison position point from the planned X-axis position point; the tenth subtractor 144 is used to subtract the X-axis comparison position point from the planned X-axis position point; the eleventh subtractor 145 is used to subtract the X-axis comparison position point from the planned X-axis position point; the eleventh subtractor 144 is used to subtract the X-axis comparison position point The second judgment unit 148 is connected to the first subtractor 141 and the second subtractor 142 respectively. The second judgment unit 148 is used to determine whether the actual position of the motor 200 has entered the second set position range. The third judgment unit 149 is connected to the first subtractor 141 and the second subtractor 142 respectively. The third judgment unit 149 is used to determine whether the actual position of the motor 200 has deviated from the set position. The fourth judgment unit 150 is connected to the third subtractor 143 and the fourth subtractor 144 respectively. The fourth judgment unit 150 is used to determine whether the actual position of the motor 200 has deviated from the set position. Whether the actual position of the motor 200 has reached the planned position; the first differential speed calculation unit 145 is used to calculate the movement speed of the motor 200 at the X-axis grating position; the second differential speed calculation unit 146 is used to calculate the movement speed of the motor 200 at the Y-axis grating position; the timeout duration calculation unit 151 is connected to the first differential speed calculation unit 145 and the second differential speed calculation unit 146 respectively, and the timeout duration calculation unit 151 is used to calculate the waiting time based on the movement speed of the motor 200 at the X-axis grating position and the movement speed at the Y-axis grating position; the fifth judgment unit 15 2. The fourth judgment unit 150 and the timeout duration calculation unit 151 are connected respectively. The second judgment unit 148 is used to judge whether the waiting time has reached the preset duration. The AND gate circuit 153 is connected to the second judgment unit 148 and the third judgment unit 149 respectively. The OR gate circuit 154 is connected to the first judgment unit 147, the AND gate circuit 153 and the fifth judgment unit 152 respectively. The OR gate circuit 154 is used to judge whether to output a control signal based on the output signals of the first judgment unit 147, the AND gate circuit 153 and the fifth judgment unit 152.

[0098] In this embodiment, the first subtractor 141, the second subtractor 142, and the first judgment unit 147 determine whether the actual position of the current motor 200 is within the first set range by measuring the X-axis grating position, the Y-axis grating position, the X-axis comparison position point, and the Y-axis comparison position point. If so, the OR gate circuit 154 outputs a high-level signal, and at this time, the output control signal is triggered.

[0099] The first subtractor 141, the second subtractor 142, and the second judgment unit 148 determine whether the actual position of the motor 200 is within the second set range by checking the X-axis grating position, the Y-axis grating position, and the X-axis comparison position point and the Y-axis comparison position point. If so, the first subtractor 141, the second subtractor 142, and the third judgment unit 149 further determine whether the actual position of the motor 200 is gradually approaching or gradually moving away from the position comparison point by checking the X-axis grating position, the Y-axis grating position, and the X-axis comparison position point and the Y-axis comparison position point. If it is gradually moving away from the position comparison point, then the AND gate circuit 153 outputs a high-level signal, or the gate circuit 154 outputs a high-level signal, at which time a control signal is output.

[0100] The third subtractor 143, the fourth subtractor 144, and the fourth judgment unit 150 determine whether the motor 200 has reached the planned position comparison point (theoretical position) by comparing the X-axis position comparison point, the Y-axis comparison position point, the planned X-axis position, and the planned Y-axis position. Furthermore, the first differential speed calculation unit 145, the second differential speed calculation unit 146, and the timeout calculation unit 151 calculate the time to reach the position comparison point at the planned position. Then, the fifth judgment unit 152 determines whether the timing duration has reached the preset duration. If so, a high-level signal is output to the OR gate circuit 154, which outputs a high-level signal, triggering the output control signal.

[0101] In summary, the multi-axis motion control method and system provided by this invention have the following advantages:

[0102] Beneficial effects:

[0103] By detecting the actual position of the motor and comparing it with the planned position, when the actual position of the motor enters the first set position range or the second set position range and the motor moves away from the set position, the output control signal can be triggered. That is, the actual position of the motor can have a certain error range. In this way, even if the motor cannot reach the specified position, the motion controller can output the control signal to execute the corresponding action control.

[0104] When the planned position has been reached and the actual position of the motor is not within the error range, timing will begin. When the timing duration reaches the preset duration and the actual position of the motor has not reached the second set position range, an output control signal will be triggered, thereby ensuring that a control signal is output at each position comparison point.

[0105] It should be understood that the application of the present invention is not limited to the examples above. Those skilled in the art can make improvements or modifications based on the above description, and all such improvements and modifications should fall within the protection scope of the appended claims.

Claims

1. A multi-axis motion control method, characterized in that, include: The system acquires user motion commands and generates a planned position for the motor based on the user motion commands, and drives the motor to work based on the planned position. The planned location has several designated locations along its path; Obtain the actual position of the motor and compare the actual position with the set position; When the actual position of the motor reaches the first set position range, an output control signal is triggered; or, when the actual position of the motor reaches the second set position range and the actual position of the motor deviates in a direction away from the set position, an output control signal is triggered; wherein, the second set position range is greater than the first set position range. The step of triggering the output control signal when the actual position of the motor reaches the second set position range and the actual position of the motor deviates in a direction away from the set position includes: When the actual position of the motor reaches the second set position range and the actual position of the motor moves toward the first set position range, wait for the trigger output control signal; When the actual position of the motor moves from the second set position range into the first set position range, the output control signal is triggered. When the actual position of the motor enters the second set position range and the actual position of the motor moves away from the first set position range, the output control signal is triggered.

2. The multi-axis motion control method according to claim 1, characterized in that, Also includes: When the planned location is detected to have reached the set position, a timer is started at the coordinate point where the planned location reaches the set position. When the timing duration is detected to have reached the preset duration and the actual position of the motor has not reached the second set position range, the output control signal is triggered.

3. The multi-axis motion control method according to claim 1, characterized in that, The step of triggering the output control signal when the actual position of the motor enters the second preset position range and the actual position of the motor moves away from the first preset position range includes: When the actual position of the motor is detected to be moving away from the first set position range and the distance of the movement exceeds the preset distance, the output control signal is triggered.

4. The multi-axis motion control method according to claim 3, characterized in that, In the step where the actual position of the motor enters the second set position range and the actual position of the motor moves away from the first set position range, the motor filtering and anti-shake function is triggered.

5. The multi-axis motion control method according to claim 2, characterized in that, The preset duration is set based on the motor's following error and the planned speed.

6. The multi-axis motion control method according to claim 5, characterized in that, The expression for calculating the preset duration is as follows: t^2 = (a^2 + b^2) / (v^2); Where t represents the preset duration, v represents the current planned synthesis speed, a represents the x-axis following error, and b represents the y-axis following error.

7. A multi-axis motion control system for implementing the multi-axis motion control method as described in any one of claims 1-6, characterized in that, include: Motion controllers, motors, mechanical structures, and optical grating sensors; The motion controller is connected to the motor, and the motion controller is used to drive the motor to work according to the planned position; The mechanical structure is mounted on the motor; The grating sensor is connected to the mechanical structure and the motion controller respectively. The grating is used to detect the actual position of the motor and feed it back to the motion controller. The motion controller is also used to compare the actual position of the motor with the planned position to determine whether the actual position of the motor is within a first set position range or a second set position range, and to trigger an output control signal when the actual position of the motor reaches the first set position range, and to trigger an output control signal when the actual position of the motor reaches the second set position range and the actual position of the motor deviates in a direction away from the set position.

8. The multi-axis motion control system according to claim 7, characterized in that, The motion controller includes: a first-in-first-out queue for position comparison points, an X-axis grating position counter, a Y-axis grating position counter, a position comparison unit, and a motion planning unit; The first-in-first-out queue of the position comparison points is connected to the position comparison unit and is used to control the triggering order of each set position in the planned path of the motor. The X-axis grating position counter is connected to the position comparison unit and is used to count the position coordinates of the motor on the X-axis. The Y-axis grating position counter is connected to the position comparison unit and is used to count the position coordinates of the motor on the Y-axis. The position comparison unit is used to compare the actual position of the motor with the set position, and triggers the output control signal when the actual position of the motor reaches the first set position range, or when the actual position of the motor reaches the second set position range and the actual position of the motor deviates in a direction away from the set position, or when the timing is started when the set position is detected to have been reached in the planned position, and the timing duration is detected to have reached the preset duration, and the actual position of the motor has not reached the second set position range, then the output control signal is triggered. The motion planning unit is connected to the position comparison unit, and the motion planning unit is used to generate the planned position of the motor according to the user's motion command.

9. The multi-axis motion control system according to claim 8, characterized in that, The position comparison unit includes: a first subtractor, a second subtractor, a third subtractor, a fourth subtractor, a first differential velocity calculation unit, a second differential velocity calculation unit, a first judgment unit, a second judgment unit, a third judgment unit, a fourth judgment unit, a timeout duration calculation unit, a fifth judgment unit, and AND and OR gate circuits; wherein, The first subtractor is used to subtract the X-axis grating position from the X-axis comparison position point; The second subtractor is used to subtract the Y-axis grating position from the Y-axis comparison position point; The third subtractor is used to subtract the X-axis comparison position from the X-axis planned position; The fourth subtractor is used to subtract the Y-axis comparison position from the Y-axis planned position; The first judgment unit is connected to the first subtractor and the second subtractor. The first judgment unit is used to determine whether the actual position of the motor enters the first set position range. The second judgment unit is connected to the first subtractor and the second subtractor respectively, and the second judgment unit is used to determine whether the actual position of the motor enters the second set position range; The third judgment unit is connected to the first subtractor and the second subtractor respectively, and the third judgment unit is used to determine whether the actual position of the motor deviates from the set position. The fourth judgment unit is connected to the third subtractor and the fourth subtractor respectively, and the fourth judgment unit is used to determine whether the actual position of the motor has reached the planned position; The first differential speed calculation unit is used to calculate the speed of the motor at the X-axis grating position; The second differential speed calculation unit is used to calculate the speed of the motor at the position of the Y-axis grating; The timeout duration calculation unit is connected to the first differential speed calculation unit and the second differential speed calculation unit respectively. The timeout duration calculation unit is used to calculate the waiting time based on the movement speed of the motor at the X-axis grating position and the movement speed at the Y-axis grating position. The fifth judgment unit is connected to the fourth judgment unit and the timeout duration calculation unit respectively, and the second judgment unit is used to determine whether the waiting time has reached the preset duration. The AND gate circuit is connected to the second judgment unit and the third judgment unit respectively; The OR gate circuit is connected to the first judgment unit, the AND gate circuit, and the fifth judgment unit respectively. The OR gate circuit is used to determine whether to output a control signal based on the output signals of the first judgment unit, the AND gate circuit, and the fifth judgment unit.

Images