A servo control real-time position planning method and system

By dividing the motion trajectory of the servo motor into four segments and planning and calculation, the real-time response problem when the target position of the servo motor is changed is solved, and the universality and applicability of the control is improved, ensuring that the motion trajectory is effectively planned under different circumstances. .

CN118763953BActive Publication Date: 2024-12-03CHENGDU HAGONG DRIVE TECH CO LTD +1
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411241763.5
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2024-09-05
Publication Date
2024-12-03
Estimated Expiration
2044-09-05

AI Technical Summary

Technical Problem

When the target position of the servo motor is frequently changed, the latest position cannot be responded in time, and the speed planning does not consider whether the reverse or direct route is required, resulting in low control efficiency.

Method used

The servo motor's motion trajectory is divided into four segments: deceleration, acceleration, uniform speed and deceleration to the target position. It is planned and calculated based on the current position, speed and target position, generates planning data for control, and updates in real time. Current position and speed.

Benefits of technology

Real-time response of the servo motor to the target position changes is realized, the universality and applicability of control is improved, and the motion trajectory can be effectively planned under different circumstances.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118763953B_ABST
    Figure CN118763953B_ABST
Patent Text Reader

Abstract

The present invention discloses a servo control real-time position planning method and system, which is applied to the field of servo motor intelligent control technology, and the method includes: obtaining the current position, target position and current speed of the target servo motor; when the target position changes, dividing the motion trajectory of the target servo motor from the current position to the target position into four segments; performing planning calculations for the duration and speed of the four segments to generate planning data; controlling the movement of the target servo motor through the planning data, and updating the current position and current speed according to the duration of the target servo motor executing the planning data. Through the above technical solution, the present invention can provide a real-time response solution for the change of the target position of the servo motor, and at the same time, the calculation method based on the four-segment segmentation can take into account a variety of situations and set different calculation routes respectively, thereby effectively improving the versatility and applicability of the technical solution of the present application.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of servo motor intelligent control, and in particular to a servo control real-time position planning method and system. Background Art

[0002] Servo motors have many advantages, including high precision, high stability, high response speed, and high output torque. They are widely used in many fields, such as industrial automation, aerospace, etc. In the field of industrial automation, servo motors are often used in automation equipment such as robots, CNC machine tools, and printing equipment to achieve high-precision position and speed control. In the field of aerospace, servo motors are also widely used in the navigation and control systems of aircraft to maintain the stability of the aircraft and accurately control the attitude.

[0003] In the prior art, a Chinese patent with application number 202311217178.7 discloses a real-time speed planning method for a robot based on FPGA, including: receiving segmented interpolation data to obtain interpolation data, the interpolation data including the moving position increment of the motion axis; calculating the planning time of the acceleration segment, the uniform speed segment and the deceleration segment according to the interpolation data; discretizing the planning time of the uniform speed segment, the acceleration segment and the deceleration segment into three-segment feed amounts, judging the moving position increment in the interpolation data, and if the moving position increment is less than a set threshold, directly interpolating with the trapezoidal speed curve planned by the optimal time trajectory, and the pre-planning ends; if the moving position increment is greater than or equal to the set threshold, an S-shaped speed curve is obtained after secondary planning by calculating an auxiliary correction function, and the pre-planning ends; the speed curve after pre-planning is used to control the motor, and the S-shaped speed curve is converted into a pulse output. However, for some special application scenarios, the target position of the servo motor may be changed frequently. When the next target position is updated before the previous target position is reached, the servo controller cannot respond to the latest target position in time. At the same time, the existing technology is more about speed planning, and speed planning does not need to consider whether it needs to reverse or whether it can reach directly. Summary of the invention

[0004] In order to at least overcome the above-mentioned deficiencies in the prior art, an object of the present invention is to provide a servo control real-time position planning method and system.

[0005] In a first aspect, an embodiment of the present invention provides a servo control real-time position planning method, comprising:

[0006] Get the current position, target position and current speed of the target servo motor;

[0007] When the target position changes, the motion trajectory of the target servo motor from the current position to the target position is divided into four segments; the four segments include a first segment, a second segment, a third segment and a fourth segment which are sequentially arranged;

[0008] According to the current position, current speed and target position of the target servo motor, a planning calculation is performed for the duration and speed of the four segments to generate planning data;

[0009] The target servo motor is controlled to move according to the planning data, and the current position and the current speed are updated according to the duration of the target servo motor executing the planning data.

[0010] When the embodiment of the present invention is implemented, it is necessary to obtain the current position, target position and current speed of the target servo motor in real time. When the target position is not reached, if a new target position appears, the embodiment of the present invention divides the motion trajectory from the current position to the new target position into four segments to plan the trajectory of the servo motor; among the four segments, the first segment can be used to decelerate the servo motor, the second segment can be used to accelerate the servo motor, the third segment can be used to uniformly move the servo motor, and the fourth segment can be used to decelerate the servo motor. The fourth segment decelerating to 0 indicates that the new target point has just been reached. Among them, for the operation of the servo motor under different circumstances, each segment is operated separately. When planning, the segment selection can be made according to different situations, such as setting the duration of the first segment or the third segment to 0 to be suitable for different situations.

[0011] In an embodiment of the present invention, the four segments can be planned and calculated separately according to the current position, current speed and target position, and planning data for servo motor control can be generated. At the same time, when the servo motor is running according to the planning data, the new current position and current speed can be obtained according to the execution time, so as to prepare data for the next round of target position changes. Through the above technical solution, the embodiment of the present invention can provide a real-time response solution for the change of the target position of the servo motor. At the same time, the calculation method based on the four-segment segmentation can take into account a variety of situations and set different calculation routes respectively, thereby effectively improving the versatility and applicability of the technical solution of the present invention.

[0012] In a possible implementation, calculating and generating planning data includes:

[0013] The duration of the first segment is calculated according to the current position, current speed and target position of the target servo motor, and the total movement distance of the second segment, the third segment and the fourth segment is calculated as a reference movement distance;

[0014] The speed inflection point of the second segment, the third segment and the fourth segment as a whole is calculated based on the reference motion distance, and the duration and speed of each segment in the second segment, the third segment and the fourth segment, as well as the speed of the first segment are calculated based on the difference between the speed inflection point and the speed upper limit of the target servo motor.

[0015] In a possible implementation, the duration of the first segment is calculated according to the current position, current speed and target position of the target servo motor, and the total movement distance of the second segment, the third segment and the fourth segment is calculated as the reference movement distance, including:

[0016] When the direction from the current position to the target position is inconsistent with the direction of the current speed, it is calculated according to the following formula:

[0017]

[0018]

[0019]

[0020]

[0021] When the direction from the current position to the target position is consistent with the direction of the current speed, it is determined whether the target servo motor can directly reach the target position from the current position without reversing according to the following formula:

[0022]

[0023]

[0024] like It cannot be reached directly. You can reach it directly;

[0025] If it can be reached directly, it is calculated according to the following formula:

[0026]

[0027]

[0028]

[0029]

[0030] If it is not possible to reach directly, calculate according to the following formula:

[0031]

[0032]

[0033]

[0034]

[0035] In the formula, is the duration of the first segment, is the current speed, The deceleration preset for the target servo motor, is the starting speed of the second segment, is the total movement distance of the second, third and fourth segments, The distance required to decelerate the current speed to 0 according to the deceleration Dec. is the distance from the current position to the target position, is the movement direction of the second, third and fourth segments, is the operator for determining direction, is the absolute value operator, is the target location, is the current location.

[0036] In a possible implementation, calculating the velocity inflection point of the second segment, the third segment, and the fourth segment as a whole according to the reference motion distance includes:

[0037] The speed inflection point is calculated according to the following formula:

[0038]

[0039] In the formula, is the speed inflection point, The acceleration preset for the target servo motor.

[0040] In a possible implementation, calculating the duration and speed of each of the second segment, the third segment, and the fourth segment according to the difference between the speed inflection point and the speed upper limit of the target servo motor, and the speed of the first segment includes:

[0041] If the speed inflection point Exceeding speed limit , calculate the duration and speed of each segment, as well as the speed of the first segment, according to the following formula:

[0042]

[0043]

[0044]

[0045]

[0046]

[0047]

[0048]

[0049]

[0050] If the speed inflection point Speed ​​limit not exceeded , calculate the duration and speed of each segment, as well as the speed of the first segment, according to the following formula:

[0051]

[0052]

[0053]

[0054]

[0055]

[0056] In the formula, The acceleration preset for the target servo motor, is the duration of the second segment, is the duration of the third segment, is the duration of the fourth segment, is the speed at the end of the first segment, is the speed at the end of the second segment, is the speed at the end of the third segment, is the speed at the end of the fourth segment, is the movement distance of the second segment, The movement distance of the third segment is This is the fourth segment of the exercise journey.

[0057] In a possible implementation, updating the current position and the current speed according to the duration of the target servo motor executing the planning data includes:

[0058] The time length for the target servo motor to execute the planning data is accumulated by a timer as an accumulated time;

[0059] The speed at the corresponding moment is obtained from the planning data according to the accumulated timing as the current speed, and the current position is obtained by integrating the current speed.

[0060] In a possible implementation, acquiring the speed at a corresponding moment from the planning data as the current speed according to the accumulated timing includes:

[0061] According to the accumulated timing, the segment in which the target servo motor is located in the planning data in the current state is obtained;

[0062] The current speed is calculated by a preset acceleration or deceleration according to the position of the accumulated timing in the segment.

[0063] In a possible implementation, the first segment is a deceleration segment for decelerating to 0; the second segment is an acceleration segment; the third segment is a uniform speed segment; and the fourth segment is a deceleration segment for decelerating to 0 to reach a target position.

[0064] In a second aspect, an embodiment of the present invention further provides a servo control real-time position planning system, the system comprising:

[0065] an acquisition unit configured to acquire a current position, a target position, and a current speed of a target servo motor;

[0066] A dividing unit is configured to divide the motion trajectory of the target servo motor from the current position to the target position into four segments when the target position changes; the four segments include a first segment, a second segment, a third segment and a fourth segment which are sequentially arranged;

[0067] A calculation unit is configured to generate planning data by performing planning calculation according to the current position, current speed and target position of the target servo motor for the duration and speed of four segments;

[0068] A control unit is configured to control the movement of the target servo motor through the planning data, and update the current position and the current speed according to the duration of the target servo motor executing the planning data.

[0069] In a possible implementation, the computing unit is further configured to include:

[0070] The duration of the first segment is calculated according to the current position, current speed and target position of the target servo motor, and the total movement distance of the second segment, the third segment and the fourth segment is calculated as a reference movement distance;

[0071] The speed inflection point of the second segment, the third segment and the fourth segment as a whole is calculated based on the reference motion distance, and the duration and speed of each segment in the second segment, the third segment and the fourth segment, as well as the speed of the first segment are calculated based on the difference between the speed inflection point and the speed upper limit of the target servo motor.

[0072] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0073] The present invention provides a servo control real-time position planning method and system. Through the above technical scheme, it can provide a real-time response scheme for the change of the target position of the servo motor. At the same time, the calculation method based on the four-segment segmentation can take into account a variety of situations and set different calculation routes respectively, thereby effectively improving the versatility and applicability of the technical scheme of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] The drawings described herein are used to provide a further understanding of the embodiments of the present invention, constitute a part of the present invention, and do not constitute a limitation of the embodiments of the present invention. In the drawings:

[0075] Figure 1 This is a schematic diagram of the steps of the method according to an embodiment of the present invention;

[0076] Figure 2 This is a schematic diagram of the planned speed and position of the first case of an embodiment of the present invention;

[0077] Figure 3 This is a schematic diagram of the planned speed and position of the second case of the embodiment of the present invention;

[0078] Figure 4 This is a schematic diagram of the planned speed and position of the third case of an embodiment of the present invention. DETAILED DESCRIPTION

[0079] In order to make the purpose, technical solution and advantages of the embodiment of the present invention clearer, the technical solution in the embodiment of the present invention will be clearly and completely described in conjunction with the drawings in the embodiment of the present invention. It should be understood that the drawings in the present invention only serve the purpose of illustration and description and are not used to limit the scope of protection of the present invention. In addition, it should be understood that the schematic drawings are not drawn in real proportion. The flowchart used in the present invention shows the operations implemented according to some embodiments of the embodiment of the present invention. It should be understood that the operations of the flowchart can be implemented out of sequence, and the steps without logical context can be reversed in order or implemented simultaneously. In addition, under the guidance of the content of the present invention, those skilled in the art can add one or more other operations to the flowchart, or remove one or more operations from the flowchart.

[0080] In addition, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. The components of the embodiments of the present invention generally described and shown in the drawings herein can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed invention, but merely represents the selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making creative work belong to the scope of protection of the present invention.

[0081] Please refer to Figure 1 , which is a flow chart of a servo control real-time position planning method provided in an embodiment of the present invention. Furthermore, the servo control real-time position planning method may specifically include the contents described in the following steps S1 to S4.

[0082] S1: Get the current position, target position and current speed of the target servo motor;

[0083] S2: when the target position changes, dividing the motion trajectory of the target servo motor from the current position to the target position into four segments; the four segments include a first segment, a second segment, a third segment and a fourth segment which are sequentially arranged;

[0084] S3: generating planning data by performing planning calculation for the duration and speed of four segments according to the current position, current speed and target position of the target servo motor;

[0085] S4: Controlling the movement of the target servo motor through the planning data, and updating the current position and the current speed according to the duration of the target servo motor executing the planning data.

[0086] When the embodiment of the present invention is implemented, it is necessary to obtain the current position, target position and current speed of the target servo motor in real time. When the target position is not reached, if a new target position appears, the embodiment of the present invention divides the motion trajectory from the current position to the new target position into four segments to plan the trajectory of the servo motor; among the four segments, the first segment can be used to decelerate the servo motor, the second segment can be used to accelerate the servo motor, the third segment can be used to uniformly move the servo motor, and the fourth segment can be used to decelerate the servo motor. The fourth segment decelerating to 0 indicates that the new target point has just been reached. Among them, for the operation of the servo motor under different circumstances, each segment is operated separately. When planning, the segment selection can be made according to different situations, such as setting the duration of the first segment or the third segment to 0 to be suitable for different situations.

[0087] In an embodiment of the present invention, the four segments can be planned and calculated separately according to the current position, current speed and target position, and planning data for servo motor control can be generated. At the same time, when the servo motor is running according to the planning data, the new current position and current speed can be obtained according to the execution time, so as to prepare data for the next round of target position changes. Through the above technical solution, the embodiment of the present invention can provide a real-time response solution for the change of the target position of the servo motor. At the same time, the calculation method based on the four-segment segmentation can take into account a variety of situations and set different calculation routes respectively, thereby effectively improving the versatility and applicability of the technical solution of the present invention.

[0088] In a possible implementation, calculating and generating planning data includes:

[0089] The duration of the first segment is calculated according to the current position, current speed and target position of the target servo motor, and the total movement distance of the second segment, the third segment and the fourth segment is calculated as a reference movement distance;

[0090] The speed inflection point of the second segment, the third segment and the fourth segment as a whole is calculated based on the reference motion distance, and the duration and speed of each segment in the second segment, the third segment and the fourth segment, as well as the speed of the first segment are calculated based on the difference between the speed inflection point and the speed upper limit of the target servo motor.

[0091] When the embodiment of the present invention is implemented, the purpose of the first segment is to decelerate the target servo motor to 0 so as to facilitate the subsequent segment planning. At this time, calculating the duration of the first segment can facilitate the calculation and planning of subsequent data. In order to realize the calculation of the duration and speed of other segments, the embodiment of the present invention also needs to obtain a reference motion distance to facilitate classification calculation of different situations.

[0092] In a possible implementation, the duration of the first segment is calculated according to the current position, current speed and target position of the target servo motor, and the total movement distance of the second segment, the third segment and the fourth segment is calculated as the reference movement distance, including:

[0093] When the direction from the current position to the target position is inconsistent with the direction of the current speed, it is calculated according to the following formula:

[0094]

[0095]

[0096]

[0097]

[0098] When the direction from the current position to the target position is consistent with the direction of the current speed, it is determined whether the target servo motor can directly reach the target position from the current position without reversing according to the following formula:

[0099]

[0100]

[0101] like It cannot be reached directly. You can reach it directly;

[0102] If it can be reached directly, it is calculated according to the following formula:

[0103]

[0104]

[0105]

[0106]

[0107] If it is not possible to reach directly, calculate according to the following formula:

[0108]

[0109]

[0110]

[0111]

[0112] In the formula, is the duration of the first segment, is the current speed, The deceleration preset for the target servo motor, is the starting speed of the second segment, is the total movement distance of the second, third and fourth segments, The distance required to decelerate the current speed to 0 according to the deceleration Dec. is the distance from the current position to the target position, is the movement direction of the second, third and fourth segments, is the operator for determining direction, is the absolute value operator, is the target location, is the current location.

[0113] When implementing the embodiment of the present invention, in order to simplify the calculation process and improve the response efficiency of the device, it is necessary to preset the deceleration and acceleration of the target reduction motor, and each acceleration is accelerated at the preset acceleration, and each deceleration is decelerated at the preset deceleration, thereby avoiding the redundant computing power consumption caused by adjusting the acceleration and deceleration in the prior art. It is an operator for determining direction. When the value in the brackets is greater than 0, it is judged as 1; when it is less than 0, it is judged as -1; when it is equal to 0, it is judged as 0.

[0114] In the embodiment of the present invention, the processing process is divided into three cases according to the current position, current speed and target position of the target servo motor:

[0115] In the first case, see Figure 2 , shows a curve diagram of an actual execution situation. In the figure, the black broken line is the speed, the blue curve is the position, and the direction from the current position to the target position is inconsistent with the direction of the current speed. At this time, it is necessary to decelerate the target servo motor to 0 in the first segment and re-accelerate and return. At this time, the duration of the first segment can be obtained by dividing the distance from the current position to the target position by the preset deceleration. At the same time, the starting speed of the second segment is 0, and the total distance and direction required to be traveled in the second segment, the third segment and the fourth segment are calculated.

[0116] In the second case, see Figure 3 , which shows a curve diagram of an actual execution situation. In the figure, the black broken line is the speed, the blue curve is the position, the direction from the current position to the target position is consistent with the direction of the current speed, and the target position can be reached at 0 speed at the current speed. At this time, the first segment is not needed, so the duration of the first segment is set to 0, and the total distance and direction required for the second, third and fourth segments are calculated.

[0117] In the third case, see Figure 4 , which shows a curve diagram of an actual execution situation. In the figure, the black broken line is the speed, and the blue curve is the position. The direction from the current position to the target position is consistent with the direction of the current speed, but the target position cannot be reached at a speed of 0 at the current speed. At this time, it is necessary to decelerate in the first segment first, decelerate to 0, and then plan the speed through subsequent segments. The duration of the first segment can be obtained by dividing the distance from the current position to the target position by the preset deceleration. At the same time, the total distance and direction required to be traveled in the second segment, the third segment, and the fourth segment are calculated.

[0118] When judging the second and third situations, the judgment can be made based on the comparison between the distance from the current position to the target position and the distance required to move the current speed to 0 according to the deceleration Dec.

[0119] In a possible implementation, calculating the velocity inflection point of the second segment, the third segment, and the fourth segment as a whole according to the reference motion distance includes:

[0120] The speed inflection point is calculated according to the following formula:

[0121]

[0122] In the formula, is the speed inflection point, The acceleration preset for the target servo motor.

[0123] When the embodiment of the present invention is implemented, the main purpose of the speed inflection point is to determine whether the third segment is required for uniform speed operation. At this time, it is first assumed that the movement of the second segment, the third segment and the fourth segment has no uniform speed segment, that is, there is no third segment, and only acceleration and then deceleration to reach the target position, then is the speed turning point from acceleration to deceleration. Exceeding speed limit , it means a uniform speed segment is needed. Speed ​​limit not exceeded , it means there is no uniform speed segment.

[0124] In a possible implementation, calculating the duration and speed of each of the second segment, the third segment, and the fourth segment according to the difference between the speed inflection point and the speed upper limit of the target servo motor, and the speed of the first segment includes:

[0125] If the speed inflection point Exceeding speed limit , calculate the duration and speed of each segment, as well as the speed of the first segment, according to the following formula:

[0126]

[0127]

[0128]

[0129]

[0130]

[0131]

[0132]

[0133]

[0134] If the speed inflection point Speed ​​limit not exceeded , calculate the duration and speed of each segment, as well as the speed of the first segment, according to the following formula:

[0135]

[0136]

[0137]

[0138]

[0139]

[0140] In the formula, The acceleration preset for the target servo motor, is the duration of the second segment, is the duration of the third segment, is the duration of the fourth segment, is the speed at the end of the first segment, is the speed at the end of the second segment, is the speed at the end of the third segment, is the speed at the end of the fourth segment, is the movement distance of the second segment, The movement distance of the third segment is This is the fourth segment of the exercise journey.

[0141] When the embodiment of the present invention is implemented, the speed inflection point Exceeding speed limit , it means that a uniform speed segment is needed. At this time, the duration of the second segment is the time from the speed at the beginning of the second segment to the speed upper limit, that is, the ratio of the value of the speed upper limit minus the speed at the beginning of the second segment to the preset acceleration. The fourth segment is the process of decelerating from the speed upper limit to zero at a preset deceleration rate, and the third segment is a segment running at a uniform speed through the speed upper limit. Based on the above process, the duration of each segment and the speed at the end of the segment can be calculated.

[0142] Similarly, the speed inflection point Speed ​​limit not exceeded , then the duration of the third segment is zero, and the duration of each segment and the speed at the end of the segment can be calculated. For the embodiment of the present invention, the servo controller only needs to execute the duration of each segment, because the speed change during the acceleration, deceleration or uniform speed of each segment is fixed, so the computing power required for control can be effectively reduced.

[0143] In a possible implementation, updating the current position and the current speed according to the duration of the target servo motor executing the planning data includes:

[0144] The time length for the target servo motor to execute the planning data is accumulated by a timer as an accumulated time;

[0145] The speed at the corresponding moment is obtained from the planning data according to the accumulated timing as the current speed, and the current position is obtained by integrating the current speed.

[0146] When the embodiment of the present invention is implemented, when the current position and current speed are updated, since the deceleration and acceleration of different segments are fixed, the current speed can be calculated based on the accumulated timing time, and the current position can be obtained by integration to facilitate the calculation of the subsequent new round of target position changes.

[0147] In a possible implementation, acquiring the speed at a corresponding moment from the planning data as the current speed according to the accumulated timing includes:

[0148] According to the accumulated timing, the segment in which the target servo motor is located in the planning data in the current state is obtained;

[0149] The current speed is calculated by a preset acceleration or deceleration according to the position of the accumulated timing in the segment.

[0150] In a possible implementation, the first segment is a deceleration segment for decelerating to 0; the second segment is an acceleration segment; the third segment is a uniform speed segment; and the fourth segment is a deceleration segment for decelerating to 0 to reach a target position.

[0151] In a second aspect, an embodiment of the present invention further provides a servo control real-time position planning system, the system comprising:

[0152] an acquisition unit configured to acquire a current position, a target position, and a current speed of a target servo motor;

[0153] A dividing unit is configured to divide the motion trajectory of the target servo motor from the current position to the target position into four segments when the target position changes; the four segments include a first segment, a second segment, a third segment and a fourth segment which are sequentially arranged;

[0154] A calculation unit is configured to generate planning data by performing planning calculation according to the current position, current speed and target position of the target servo motor for the duration and speed of four segments;

[0155] A control unit is configured to control the movement of the target servo motor through the planning data, and update the current position and the current speed according to the duration of the target servo motor executing the planning data.

[0156] In a possible implementation, the computing unit is further configured to include:

[0157] The duration of the first segment is calculated according to the current position, current speed and target position of the target servo motor, and the total movement distance of the second segment, the third segment and the fourth segment is calculated as a reference movement distance;

[0158] The speed inflection point of the second segment, the third segment and the fourth segment as a whole is calculated based on the reference motion distance, and the duration and speed of each segment in the second segment, the third segment and the fourth segment, as well as the speed of the first segment are calculated based on the difference between the speed inflection point and the speed upper limit of the target servo motor.

[0159] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described in terms of function in the above description. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0160] In the several embodiments provided by the present invention, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or units, or it can be an electrical, mechanical or other form of connection.

[0161] The units described as separate components may or may not be physically separated. As units, it is obvious that a person of ordinary skill in the art can realize that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described in terms of function in the above description. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0162] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0163] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a server, or a grid device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0164] The specific implementation methods described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation method of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A servo control real-time position planning method, It is characterized in that include: Get the current position, target position and current speed of the target servo motor; When the target position changes, the motion trajectory of the target servo motor from the current position to the target position is divided into four segments; the four segments include a first segment, a second segment, a third segment and a fourth segment which are sequentially arranged; According to the current position, current speed and target position of the target servo motor, a planning calculation is performed for the duration and speed of the four segments to generate planning data; Controlling the movement of the target servo motor through the planning data, and updating the current position and the current speed according to the duration of the target servo motor executing the planning data; The calculation and generation of planning data includes: The duration of the first segment is calculated according to the current position, current speed and target position of the target servo motor, and the total movement distance of the second segment, the third segment and the fourth segment is calculated as a reference movement distance; Calculating the speed inflection point of the second segment, the third segment and the fourth segment as a whole according to the reference motion distance, and calculating the duration and speed of each segment in the second segment, the third segment and the fourth segment, as well as the speed of the first segment according to the difference between the speed inflection point and the speed upper limit of the target servo motor; The duration of the first segment is calculated according to the current position, current speed and target position of the target servo motor, and the total movement distance of the second segment, the third segment and the fourth segment is calculated as the reference movement distance, including: When the direction from the current position to the target position is inconsistent with the direction of the current speed, it is calculated according to the following formula: When the direction from the current position to the target position is consistent with the direction of the current speed, it is determined whether the target servo motor can directly reach the target position from the current position without reversing according to the following formula: like It cannot be reached directly. You can reach it directly; If it can be reached directly, calculate according to the following formula: If it is not possible to reach directly, calculate according to the following formula: In the formula, is the duration of the first segment, is the current speed, The deceleration preset for the target servo motor, is the starting speed of the second segment, is the total movement distance of the second, third and fourth segments, The distance required to decelerate the current speed to 0 according to the deceleration Dec. is the distance from the current position to the target position, is the movement direction of the second, third and fourth segments, is the operator for determining direction, is the absolute value operator, is the target location, is the current location; Calculating the velocity inflection points of the second segment, the third segment, and the fourth segment as a whole according to the reference motion distance includes: The speed inflection point is calculated according to the following formula: In the formula, is the speed inflection point, a preset acceleration for the target servo motor; The difference between the speed inflection point and the speed upper limit of the target servo motor is used to calculate the duration and speed of each of the second segment, the third segment, and the fourth segment, and the speed of the first segment, including: If the speed inflection point Exceeding speed limit , calculate the duration and speed of each segment, as well as the speed of the first segment, according to the following formula: If the speed inflection point Speed ​​limit not exceeded , calculate the duration and speed of each segment, as well as the speed of the first segment, according to the following formula: In the formula, The acceleration preset for the target servo motor, is the duration of the second segment, is the duration of the third segment, is the duration of the fourth segment, is the speed at the end of the first segment, is the speed at the end of the second segment, is the speed at the end of the third segment, is the speed at the end of the fourth segment, is the movement distance of the second segment, The movement distance of the third segment is This is the movement distance of the fourth segment; each acceleration is performed at a preset acceleration, and each deceleration is performed at a preset deceleration.

2. A servo control real-time position planning method according to claim 1, It is characterized in that Updating the current position and the current speed according to the duration of the target servo motor executing the planning data comprises: The time length for the target servo motor to execute the planning data is accumulated by a timer as an accumulated time; The speed at the corresponding moment is obtained from the planning data according to the accumulated timing as the current speed, and the current position is obtained by integrating the current speed.

3. A servo control real-time position planning method according to claim 2, It is characterized in that Acquiring the speed at the corresponding time from the planning data as the current speed according to the accumulated timing includes: According to the accumulated timing, the segment in which the target servo motor is located in the planning data in the current state is obtained; The current speed is calculated by a preset acceleration or deceleration according to the position of the accumulated timing in the segment.

4. A servo control real-time position planning method according to claim 1, It is characterized in that The first segment is a deceleration segment for decelerating to 0; the second segment is an acceleration segment; the third segment is a uniform speed segment; and the fourth segment is a deceleration segment for decelerating to 0 to reach the target position.

5. A servo-controlled real-time position planning system, It is characterized in that Applying the method described in any one of claims 1 to 4, the system comprises: an acquisition unit configured to acquire a current position, a target position, and a current speed of a target servo motor; A dividing unit is configured to divide the motion trajectory of the target servo motor from the current position to the target position into four segments when the target position changes; the four segments include a first segment, a second segment, a third segment and a fourth segment which are sequentially arranged; A calculation unit is configured to generate planning data by performing planning calculation according to the current position, current speed and target position of the target servo motor for the duration and speed of four segments; A control unit is configured to control the movement of the target servo motor through the planning data, and update the current position and the current speed according to the duration of the target servo motor executing the planning data.

6. A servo control real-time position planning system according to claim 5, It is characterized in that The computing unit is further configured to include: The duration of the first segment is calculated according to the current position, current speed and target position of the target servo motor, and the total movement distance of the second segment, the third segment and the fourth segment is calculated as a reference movement distance; The speed inflection point of the second segment, the third segment and the fourth segment as a whole is calculated based on the reference motion distance, and the duration and speed of each segment in the second segment, the third segment and the fourth segment, as well as the speed of the first segment are calculated based on the difference between the speed inflection point and the speed upper limit of the target servo motor.