CN114872035B

CN114872035B - Planning method for movement speed and path and method for deceleration control and data updating

Info

Publication number: CN114872035B
Application number: CN202111236791.4A
Authority: CN
Inventors: 梁旺; 石建军; 苗立晓
Original assignee: SHENZHEN HUACHENG INDUSTRIAL CONTROL CO LTD
Current assignee: SHENZHEN HUACHENG INDUSTRIAL CONTROL CO LTD
Priority date: 2020-10-16
Filing date: 2020-10-16
Publication date: 2023-08-22
Anticipated expiration: 2040-10-16
Also published as: CN112276906B; CN112276906A; CN114872035A

Abstract

The application relates to a local deceleration control method, a planning method of movement speed, a planning method of movement path and a data updating method, wherein the local deceleration control method comprises the following steps: establishing a plurality of adjacent threshold intervals corresponding to the running speeds respectively; acquiring a curve to be decelerated and a detection point on the curve; acquiring a curvature radius of a deceleration curve on a detection point, and judging a threshold interval in which the curvature radius is positioned so as to obtain a running speed corresponding to the detection point; obtaining a deceleration point from the detection points based on the relative magnitude of the detection points and the running speeds corresponding to the curve to be decelerated; a deceleration area corresponding to the running speed of the deceleration point is arranged before and after the deceleration point; establishing a deceleration point set corresponding to each deceleration zone and writing detection points in the deceleration zone; and obtaining the minimum value of the running speed corresponding to each detection point in the deceleration point set and taking the minimum value as the running speed of the deceleration area. The application has the advantage of enabling the actuator to decelerate at unpredicted steep track sections to reduce wear.

Description

Planning method for movement speed and path and method for deceleration control and data updating

The application relates to a divisional application, the basis of which is an application with the application number of 2020111146905, the application date of 2020.10.16 and the application name of a control method, a system, a host and a storage medium for movement speed.

Technical Field

The application relates to the field of automatic control, in particular to a planning method of movement speed and path and a method for deceleration control and data updating.

Background

With the development of industrial automation in recent years, automation control is becoming an emerging subject and has been developed rapidly. The manipulator is an important means for improving labor productivity, improving labor conditions, reducing labor intensity of workers and realizing industrial production automation, and is very important for application and development at home and abroad. The manipulator is used in the automobile manufacturing industry at the earliest and is commonly used for welding, paint spraying, loading and unloading and carrying. The manipulator extends and expands the hand and foot and brain functions of the person, and can replace the person to work in dangerous, harmful, toxic, low-temperature, high-temperature and other severe environments; the labor intensity and monotonous repeated labor are replaced by people, the labor productivity is improved, and the product quality is ensured.

Through action teaching, the manipulator can move according to a fixed program. The robot is controlled to complete the track motion by linear interpolation, circular interpolation or joint interpolation and other methods no matter the motion track is simple or complex. Obviously, the more points taught, the higher the accuracy of the motion trail of the manipulator. However, the more points means the more line segments, the more frequent acceleration and deceleration is required to complete the transition between the line segments, thereby reducing the overall efficiency. At present, the related art proposes to fit control points to a continuous curve at one time by using a B-spline pair, and then interpolate on the B-spline fit curve, thereby realizing smooth transition.

However, the inventor considers that curvature change still exists on the B-spline fitting curve, and although sampling points can be uniformly acquired on the fitting curve to calculate the curvature radius at the sampling points so as to guide acceleration and deceleration of the manipulator, for complex patterns, a plurality of wave crests and wave troughs are always stored in a short distance, if the wave crests and wave troughs are not exactly sampled, trowelling on data can occur, corresponding speed control cannot be performed, accelerated abrasion of the manipulator is easily caused, and the service life is influenced.

Disclosure of Invention

In order to reduce the speed of an executing piece in an unprecedented steep track section so as to reduce the risk of the sudden stress of the executing piece to increase the abrasion, the application provides a planning method of the movement speed and the path and a speed reduction control and data updating method.

In a first aspect, the present application provides a local deceleration control method for speed planning, which adopts the following technical scheme:

a local deceleration control method for speed planning, comprising the steps of:

establishing a plurality of adjacent threshold intervals, wherein each threshold interval corresponds to an operation speed, and all the threshold intervals are combined into a positive real number domain;

Acquiring a curve to be decelerated and a detection point positioned on the curve to be decelerated, wherein the point set of the detection point is not contained in the point set of the sampling point of the curve to be decelerated;

acquiring a curvature radius of a deceleration curve on a detection point, and judging a threshold interval in which the curvature radius is positioned so as to obtain a running speed corresponding to the detection point;

acquiring a deceleration point from the detection point based on the relative magnitude of the running speed corresponding to the detection point and the running speed corresponding to a threshold interval where the average curvature radius of the curve to be decelerated is located;

a deceleration area is arranged in front of and behind the deceleration point, wherein the running speed corresponding to the deceleration area is the running speed corresponding to the deceleration point;

establishing a deceleration point set corresponding to each deceleration zone, and writing detection points in the deceleration zones into the deceleration point set;

and acquiring the minimum value of the running speed corresponding to each detection point in the deceleration point set, and taking the minimum running speed as the running speed of the deceleration zone.

By adopting the technical scheme, a curve needing to be decelerated is selected, and a detection point is selected on the curve and used for calculating the curvature radius of a point which is not sampled on the curve. Since the curvature radius of the curve to be decelerated is obtained by averaging the curvature radii of the curves at all the sampling points, the point set of the detection points is not a subset of the sampling points, and the detection points can be made to avoid the sampling points.

In practical situations, the curve wave is often a section including a plurality of wave crests and wave troughs, and a single peak is rare and abrupt, so when a deceleration point is detected, in order to save calculation force, the scheme chooses to set the deceleration area directly in front of and behind the deceleration point, rather than setting the detection point in front of and behind the deceleration point with higher density. When a plurality of continuous detection points needing to be decelerated are simultaneously positioned in the deceleration zone, the range of the deceleration zone is smaller, and the mechanical arm is also worn by frequent acceleration and deceleration in the deceleration zone, so that the lowest running speed corresponding to each detection point is adopted as the uniform running speed of the deceleration zone, the running efficiency can be improved, the continuous setting and repeated setting of the deceleration are avoided, the calculation force is saved, and better protection effect is achieved on the mechanical arm.

Optionally, the selection step length of the detection point is smaller than the selection step length of the sampling point.

By adopting the technical scheme, as the local average curvature of the curve to be decelerated is calculated based on the sampling points, the detection points and the sampling points with different step sizes are selected, so that the detection points can avoid the sampling points to obtain new data, and the situation that the data overlap and fluctuation clamped between the two sampling points cannot be found is prevented.

Optionally, the curvature statistics method of the curve to be decelerated at the detection point comprises the following steps:

sampling points are uniformly or non-uniformly selected on a curve to be decelerated;

calculating the curvature radius of the input curve at each sampling point;

the average of the radius of curvature of the input curve at all the sampling points is calculated.

By adopting the technical scheme, the curve to be decelerated is subjected to multi-point sampling, and the average value of the curvature radius of the input curve at all sampling points is obtained so as to represent the bending degree of the input curve, so that the planning of the moving speed of the mechanical arm is conveniently guided.

In a second aspect, the present application provides a method for planning a movement speed, which adopts the following technical scheme:

a method of planning a movement speed, comprising the steps of:

dividing the smooth fitting track into a plurality of sections of ordered local curves at equal intervals or variable intervals or based on a dividing strategy;

calculating the average curvature radius of a future track based on a statistical strategy, wherein the current track is a local curve corresponding to the current position of the executing piece, and the future track is the next section of local curve connected with the current track;

Acquiring a threshold value interval in which the average curvature radius of the future track is positioned, and taking the running speed corresponding to the threshold value interval as the future speed, wherein the future speed is the moving speed of the executing piece on the interpolation track corresponding to the future track;

selecting a plurality of detection points on a future track at equal intervals or variable intervals, and sequentially writing the detection points into a detection point set;

sequentially reading detection points from the detection point set, and setting a deceleration zone at the detection point based on the local deceleration control method;

and planning the speed at the junction of the deceleration zone and the part outside the deceleration zone based on a planning strategy.

By adopting the technical scheme, the positive real number domain is divided into a plurality of sections and is used for classifying the average curvature radiuses of different local curves, so that the executing piece has different running speeds on the line segments corresponding to the different sections. The smooth fit trajectory may be a B-spline curve, or other smooth curve, or the like, to prevent abrupt changes in speed of the implement at a certain node on the curve. The fit trajectory is segmented into a plurality of shorter local curves so that the corresponding curvature radiuses of the points in the local curves are closer. And calculating the average curvature radius of the current track and the future track to judge the corresponding highest running speed under the average curvature radius so as to guide the speed planning of the executing piece on the track. According to the structural characteristics of the manipulator, when the manipulator moves on a track with smaller bending degree, the moving speed of the manipulator can be higher under the condition of relatively smaller guaranteed abrasion. Because of the existence of motion inertia, in order to avoid phenomena such as impact, step-out and oscillation caused by starting and stopping of a motion axis of an executing piece and track switching, acceleration and deceleration control must be performed on the connecting position of adjacent partial line segments of the executing piece, so that speed planning of the junction of a deceleration area and a part outside the deceleration area is realized, and reasonable speed planning not only can improve running precision, but also can effectively reduce running abrasion of the executing piece and prolong service life.

Optionally, the method further comprises the following steps:

calculating the average curvature radius of the current track based on a statistical strategy;

acquiring a threshold interval in which the average curvature radius of the current track is positioned, and taking the running speed corresponding to the threshold interval as the current speed, wherein the current speed is the moving speed of the executing piece on the interpolation track;

the actuator is driven to smoothly transition from the current speed to the future speed at the junction of the current trajectory and the future trajectory based on the planning strategy.

By adopting the technical scheme, because of the existence of motion inertia, in order to avoid phenomena of impact, step-out, oscillation and the like caused by starting and stopping of a motion axis of an executing piece and track switching, acceleration and deceleration control must be carried out on the connecting part of adjacent local line segments of the executing piece, so that the speed planning of the junction of the current track and the future track is embodied, the reasonable speed planning not only can improve the running precision, but also can effectively reduce the running abrasion of the executing piece, and the service life is prolonged.

Optionally, the partitioning strategy includes the following steps:

selecting a plurality of separation points on an input curve at equal intervals;

calculating the curvature radius of the input curve at each separation point;

Judging whether the ratio of the curvature radiuses of the input curves at all adjacent interval points is within a preset range or not; if not, segmenting the line segment between the two separation points again based on a partitioning strategy;

sequentially sequencing the line segments separated by the interval points of the input curve, and outputting.

By adopting the technical scheme, the fitting track is processed by using a partitioning strategy. The separation points are selected at equal intervals on the input curve, which is equivalent to multipoint sampling of the fitting track. And calculating the curvature radius of the fitting track on each adjacent separation point, if the curvature radius is within a preset range, indicating that the fluctuation of the fitting track is smaller, and outputting each line segment divided by the separation points as a local curve. If the fitting trajectory exceeds the preset range, the fitting trajectory is larger in fluctuation, and further division is needed. And further dividing the line segments between the two separation points based on a division strategy by using an iterative method until the line segments obtained by division meet the requirements, and sequentially sequencing the line segments formed by the separation points of the input curve and outputting the line segments. By means of the dividing strategy, the fitting track can be divided into a plurality of sections of local line segments with approximate curvature radiuses at all positions, so that subsequent processing is facilitated.

Optionally, the planning strategy includes the following steps:

acquiring two curves which are sequentially and smoothly connected, taking the curves as a first line segment and a second line segment, and setting the junction of the first line segment and the second line segment as a speed regulation area front and back;

acquiring the planned moving speed of the executing piece on the first line segment and the planned moving speed of the executing piece on the second line segment, and respectively serving as the initial speed and the final speed of the executing piece moving on the speed regulation area;

a first speed planning for uniformly changing the speed regulation area based on the initial speed and the final speed;

and (3) performing a second speed planning of symmetrical graph replacement of the equal integral area on the acceleration curve obtained by the first speed planning.

By adopting the technical scheme, the input of the planning strategy is two smoothly connected line segments, the junction is used as the speed regulation area, and when the planned moving speeds of the two line segments are equal, the speed at the speed regulation area is actually equal to the planned moving speed. When the planned moving speeds of the first line segment and the second line segment are different, the distance from one end of the speed regulating area to the other end is set to be a uniform speed changing stage, the uniform acceleration can be realized or the uniform deceleration can be realized, the acceleration at the stage is a constant value, and then the second speed planning is carried out, so that the phenomenon of acceleration mutation is eliminated in the process of changing the initial speed to the final speed. Abrupt changes in acceleration can be seen mechanically as sudden forces or reduced forces on the actuators, which tend to impact the actuators and cause wear and even misalignment.

Optionally, the second speed schedule includes the steps of:

acquiring a uniform speed change stage in a first speed plan;

adopting a symmetrical and smooth acceleration curve to replace an acceleration curve in a uniform speed change stage, wherein the integral area of the symmetrical and smooth acceleration curve with respect to time is equal to the integral area of the acceleration curve in the uniform speed change stage with respect to time;

and calculating a jerk curve, a speed curve and a displacement curve of the executing piece in the speed regulation area based on the replaced acceleration curve.

By adopting the technical scheme, after the symmetrical and smooth acceleration curve is replaced by the acceleration curve in the uniform speed change stage, the jerk of the new acceleration curve can be fixed or intermittent, so long as infinite or near infinite break points do not occur. Therefore, the acceleration curve is a continuous and conductive smooth curve, and the acceleration curve is embodied in that the force applied to and reduced from the executing piece is uniformly changed, so that the executing piece is protected.

In a third aspect, the present application provides a method for planning a motion path, which adopts the following technical scheme:

a method of planning a motion path, comprising:

selecting a teaching sample, orderly selecting a plurality of control points on the teaching sample based on a sampling strategy, and recording the control points as an ordered point set into a storage area;

Establishing an execution area and a temporary storage area in the cache area; wherein, the read-write speed of the execution area is greater than the read-write speed of the temporary storage area and greater than the read-write speed of the storage area;

establishing ordered storage bits in an execution region;

acquiring an ordered point set in a storage area and recording a temporary storage area based on an adjustment strategy;

grouping control points in the temporary storage area according to a point sequence and moving the control points into the execution area group by group, wherein the number of the control points of each group of control points is less than or equal to the number of storage bits;

b spline fitting is carried out on each control point in the execution area according to the point sequence so as to generate a fitting track;

and interpolating the fitting track based on the interpolation strategy and the planning method of the motion speed so as to generate an interpolation track.

By adopting the technical scheme, people select teaching samples and select a series of ordered control points through the teaching samples to serve as data generated for teaching tracks. These control points are stored in order in a memory area, which is usually located in an external memory such as a hard disk, for storing inactive data. When the control points are all recorded in, the buffer area establishes an execution area and a temporary storage area, and the temporary storage area is used for storing the ordered point set so as to adjust the ordered point set when the ordered point set does not meet the requirements. The execution area has a fixed number of memory bits for storing control point data, the quantitative setting of which determines the upper limit of the control points used for a single fit. The buffer area is usually located in an internal memory such as a memory, and is used for reading and writing data at a high speed, and the ratio of the price to the unit memory is far higher than that of an external memory.

Prior to the first fitting, an ordered set of points is applied from the memory area and entered into the scratch pad. At the beginning of the first fitting, a set of control points is applied quantitatively or quantitatively from the head of the ordered set of points in the scratch pad and written into the execution area, while the copied control points in the ordered set of points in the scratch pad are deleted. Then fitting the control points in the execution area according to the writing sequence. And in the fitting process, planning the speed based on the future curve pair, interpolating the future curve based on the planned speed, and finally controlling the execution piece to move along the interpolation track.

For storage considerations, the memory of the system is limited. For single trajectory fitting, if the trajectory to be fitted is long and the shape of the trajectory is variable, such as an embroidery operation, more control points, such as thousands, are required to ensure approximation of the fitted shape. If one fitting is performed, all control points are necessarily recorded into the memory of the system, and a large amount of intermediate data is generated during fitting calculation and interpolation calculation, so that high requirements on memory capacity are certainly set. According to the scheme, a distributed computing mode is adopted, the B-spline is divided into a plurality of smaller paragraphs for computing, and only a smaller number of control points are required to be recorded at a time, so that less intermediate data are generated, and the requirement on the memory capacity is reduced.

The computational effort of the system is limited for computational load considerations. In addition to fitting interpolation calculations, the system also needs to distribute limited computing power to various aspects. The method adopts a distributed computing mode, less control points are subjected to fitting interpolation at a time, the position of the next point is calculated through the points, and then the motion quantity of each joint of the mechanical arm is obtained through the position of the next point and a structural algorithm (bottom logic). The generated data are called in real time, and the data are used and deleted, so that the calculation is dispersed into the beats of each movement, the phenomenon that the work of other devices is influenced due to the fact that excessive calculation force is occupied at a certain moment is avoided, the phenomenon that the mechanical arm is suddenly stopped and started is avoided, and the abrasion caused by the movement of the mechanical arm is reduced.

Optionally, the method further comprises the following steps:

establishing a derivative data area in the cache area;

writing the intermediate data generated by fitting calculation into a derivative data area;

intermediate data generated by interpolation calculation is written into the derivative data area.

By adopting the technical scheme, when the control point data is used for fitting, the control point data can be derived into a lot of intermediate data according to an algorithm built in the system, the number of the intermediate data is about ten times of that of the input data, and a large amount of related data can be generated by data interpolation. And more temporary variables are generated in the process of operation. Therefore, the temporary data are uniformly placed in the derivative data area, so that the temporary data are convenient to call, and the memory pressure can be effectively relieved when the temporary data are used and deleted.

Optionally, the sampling strategy includes the following steps:

selecting control points uniformly or non-uniformly on the teaching track;

and sequencing the control points according to the selection order and taking the control points as an ordered point set.

By adopting the technical scheme, as the required track has a gentle and twisted difference, when the curvature change of the track is small, for example, a line segment, the control points can be uniformly selected on the line segment. When the curvature change is severe, denser control points are selected at the parts with large curvature change and large curvature change, and sparser control points are selected at the parts with small curvature change. Too many control points will affect the fitting efficiency, and too few control points will make the deviation of the fitting curve from the original trajectory more obvious.

Optionally, the step of selecting a control point on the teaching track includes:

dividing the teaching track into a plurality of sections of local teaching tracks, and calculating the average curvature radius of each section of local teaching track;

and selecting control points on each section of local teaching track at equal intervals, wherein the intervals of the control points on each section of local teaching track are positively correlated with the average curvature radius of each section of local teaching track.

By adopting the technical scheme, the teaching track is uniformly or non-uniformly divided into a plurality of sections, and the dividing step length can be manually set according to the overall shape of the pattern so as to avoid small and abrupt bending. Calculating the average curvature radius of each section of local teaching track, wherein the indication of high average curvature radius is that the whole body is gentle, and fewer control points are needed; a low average radius of curvature indicates that the whole is more curved and requires more control points. For small and abrupt bends somewhere, control points can be added manually to achieve the purpose of fitting a shape more similar to the teaching track.

Optionally, the adjustment strategy includes the following steps:

copying the head of the ordered point set in the temporary storage area to be at least k coincident points and merging the head of the ordered point set in the temporary storage area;

copying the tail bits of the ordered point set in the temporary storage area to at least k coincident points and merging the tail parts of the ordered point set in the temporary storage area;

where k is the order of the B-spline fit.

By adopting the technical scheme, the first point and the last point of the ordered point set in the temporary storage area respectively correspond to the two end points of the fitting track. The two points are respectively copied into at least k coincident points, and the formed fitting group ensures that the corresponding fitting line segment is degenerated into one point, so that the whole fitting track coincides with the head end and the tail end of the input track.

Optionally, the interpolation strategy includes the following steps:

step one: acquiring a head end point and a tail end point of the fitting track, and substituting the head end point into a guiding node, wherein the guiding node is used for guiding the motion of an executing piece;

step two: carrying out first-order Taylor expansion on the fitting track at the guide node, and obtaining a first derivative of the fitting track at the guide node;

step three: generating a local interpolation line segment based on the guide node and a first derivative corresponding to the guide node, and substituting a coordinate value of one end of the local interpolation line segment far away from the guide node into the guide node;

step four: updating the interpolation track based on the local interpolation track;

step five: and returning to the second step until the guiding node is the tail end point of the fitting track.

By adopting the technical scheme, the fitting track is continuously unfolded at the guide node to obtain the local interpolation line segment so as to update the interpolation track and acquire a new guide node. Through multiple iterations, a local interpolation line segment corresponding to the fitting track is continuously obtained, and after the robot walks to the current position, the robot can obtain the coordinates of the position which should be reached at the next moment through an interpolation algorithm.

In a fourth aspect, the present application provides a method for updating data based on real-time motion state of an executing member, which adopts the following technical scheme:

A data updating method based on real-time motion state of an executing piece is used for the planning method of the motion path, and comprises the following steps:

driving the executing piece to move along the existing interpolation track;

the position of the execution part on the interpolation track is monitored in real time to calculate a residual point value, wherein the quantity of control points corresponding to the part of the execution part passing through the interpolation track is taken as a passing point value, and the residual point value is the difference between the quantity of the control points in the execution area and the passing point value;

based on the relative sizes of the residual point value and the warning value, removing the control point positioned in front of the warning node in the execution area and transferring the control point behind the warning node to the front of the warning node according to the original sequence; the warning node is a storage bit in the middle of the storage bit sequence, and the warning value is the storage bit sequence of the warning node in the execution area;

the control points in the temporary storage area are sequentially fetched and sequentially written into the storage bits after the original control points in the execution area.

By adopting the technical scheme, along with the movement of the execution piece on the interpolation track, the fitting track gradually reduces relative to the control point corresponding to the part of the execution piece which is not passed through. When the residual point value is smaller than the guard value, the control point data before the guard node in the execution area is cleared, the head part of the ordered point set in the temporary storage area is sequentially moved to the part before the guard node in the execution area, the control point data moved out of the temporary storage area is cleared, and then the next fitting is started based on the control point data in the execution area.

The data updating method is adopted in each fitting process until all control points in the ordered point set in the temporary storage area are taken out, namely, the movement of the executing piece along the whole fitting track is completed, and finally, the executing area is destroyed and redundant intermediate data are deleted.

Optionally, the alert node is a storage bit with a sequence greater than the B-spline order in the execution area, and the alert value is a value smaller than the number of the storage bits and greater than or equal to the B-spline order.

Drawings

FIG. 1 is a flow diagram of the logging steps in an embodiment of the application;

FIG. 2 is a flow diagram of a sampling strategy in an embodiment of the application;

FIG. 3 is a flow diagram of an offline programming mode in an embodiment of the present application;

FIG. 4 is a flow chart of the steps performed in an embodiment of the application;

FIG. 5 is a flow diagram of an interpolation strategy in an embodiment of the application;

FIG. 6 is a flow chart of the S4 substep in an embodiment of the application;

FIG. 7 is a flow chart of the sub-step S3 in an embodiment of the application;

FIG. 8 is a flow diagram of a partitioning strategy in an embodiment of the present application;

FIG. 9 is a flow diagram of a statistical strategy in an embodiment of the present application;

FIG. 10 is a flow diagram of a planning strategy in an embodiment of the application;

FIG. 11 is a flow chart of a second speed plan in an embodiment of the application;

FIG. 12 is a flow diagram of a local deceleration strategy in an embodiment of the application.

Detailed Description

The present application will be described in further detail with reference to fig. 1-12.

Through action teaching, the manipulator can move according to a fixed program. The robot is controlled to complete the track motion by linear interpolation, circular interpolation or joint interpolation and other methods no matter the motion track is simple or complex. When the mechanical arm is driven to move along a complex track, if a control point is simply recorded according to the shape of the track, and an executive component is controlled to move along a small line segment connected with the control point, acceleration and deceleration are required when two adjacent straight lines or circular arcs are in transition, and tiny pauses exist, the frequent acceleration and deceleration can prolong the working period, the click feeling in the whole process is enhanced along with the improvement of the running speed, the speed is difficult to be improved to a faster speed, and the working efficiency of the robot is limited. The more points taught, the higher the accuracy of the motion trajectory of the manipulator. However, the more points means the more line segments, the more frequent acceleration and deceleration, and thus the overall efficiency is reduced.

At present, common fitting modes include polynomial fitting and least square fitting. The polynomial fitting is carried out by presetting corresponding powers, and substituting a certain control point, thus obtaining a smooth curve for connecting the control points. However, as the power of polynomial fitting increases, the computational effort of the single-pass fitting operation will also increase significantly. Meanwhile, the Dragon phenomenon is more prominent in the high-power polynomial, and the slope change of the curve between adjacent control points becomes very large, which is unfavorable for acceleration and deceleration control. In addition, when one control point is added or reduced, the overall shape of the higher-order curve is greatly changed, and the fitting of the original track is not facilitated.

When the least square method is used for fitting, a basic function of a fitting curve is required to be preset, and then the sum of the distance variances from each control point to the fitting curve is minimized, so that each parameter value of the function is determined, and the closest fitting curve is obtained. The fitted curve obtained by the method approximates the shape of the teaching track very much, but the method has a large limit because the shape of the teaching track is usually changeable and a proper basic function is difficult to find.

The embodiment of the application discloses a planning method of a motion path. The planning method is a B-spline fitting-based method and comprises an input step for inputting control points and an execution step for generating a B-spline fitting track based on the control points and controlling an executing piece to move along the fitting track. The actuator is a working device with multiple degrees of freedom, and may be a mechanical arm end, or may be a truss type control device, and in this embodiment, a mechanical arm is selected.

Referring to fig. 1, the entering step includes at least two steps, respectively:

step one: selecting a teaching sample, orderly selecting a plurality of control points on the teaching sample based on a sampling strategy, and recording the control points as an ordered point set into a storage area; and simultaneously, recording a preset fitting order k, and setting the preset fitting order k as the order of B spline fitting.

Wherein, referring to fig. 2, the sampling strategy comprises:

strategy step one: selecting control points uniformly or non-uniformly on input;

the teaching sample can be an object with a plane surface or an object with a three-dimensional curved surface. The manipulator typically has multiple degrees of freedom that enable the position and orientation of the manipulator tip to be adjusted in three dimensions to conform around the surface of the teaching sample. The teaching sample can be blank, and an operator controls the manipulator to select a control point on the teaching sample according to the requirement. Wherein the control point refers to a special point for controlling and adjusting the shape of the curve.

The teaching sample can also be provided with a marking line, such as a colored curve, for guiding a person to control the mechanical arm to conduct teaching actions. In this embodiment, the teaching sample is selected as a flat plate-like object, and the surface thereof is provided with black mark lines.

The selection mode of the control point includes a manual mode or an off-line programming mode. The manual mode is to use a control handle to control the tail end of the robot to move along the track on the teaching sample, and continuously adjust the gesture of each joint of the robot in the moving process, so that the tail end of the robot is attached to the surface of the teaching sample or keeps a certain distance from the surface of the teaching sample. In the moving process, the control points are sequentially recorded according to the requirements of operators.

In the off-line programming mode, instead of using a handle operation, the selection of control points is made based on digital images. Referring to fig. 3, the offline programming mode includes the sub-steps of:

the method comprises the following substeps: and recording a teaching track image.

The teaching track image can be a 3D track image formed by converting a 3D modeling graph through offline programming software, or can be a 2D plane image with a required teaching track.

Sub-step two: dividing the teaching track into a plurality of sections of local teaching tracks according to the rule. The teaching track can be automatically segmented, i.e. divided according to fixed step sizes, the step sizes being dependent on the specific image. The teaching track can also be segmented according to manual work, and the manual segmentation has the advantages that the gentle part can be segmented less and the steep part can be segmented more according to a visual inspection method. In actual operation, the segmentation can also be performed by using a method combining automatic segmentation and manual segmentation.

After segmentation, the average curvature radius of each segment is calculated, and the average curvature radius can approximately represent the gentle condition of the local track of the segment.

And a sub-step three: and selecting control points on each section of local teaching track at equal intervals, wherein the intervals of the control points on each section of local teaching track are positively correlated with the average curvature radius of each section of local teaching track.

The indication of high average curvature radius is that the whole body is gentle, and fewer control points are needed; the lower average radius of curvature indicates that the whole body is more curved, and more control points are needed to better represent the shape characteristics of the teaching track. For example, the teaching trajectory has a smoothness of Line A greater than Line B and a smoothness of Line B greater than Line C. Therefore, the selection interval of the control points on Line a is larger than Line B, and the selection interval of the control points on Line B is larger than Line C. For small and abrupt bending at a certain place, a control point can be manually added to achieve the purpose that the fitting shape is more similar to the teaching track, and the control point is ignored according to the importance degree of the control point, so that the flatness of the later-stage fitting track is kept.

Strategy step two: the control points are ordered according to the selection sequence and used as an ordered point set;

After the teaching tracks are orderly divided, the control points on each section of local teaching track are orderly selected, so that all the control points are orderly arranged on the teaching tracks. Each control point is orderly arranged and orderly written into the ordered point set so as to be sequentially taken out from the ordered point set at a later stage, thereby enabling the fitting track to be approximate to the teaching track.

Step two: establishing an execution area, a temporary storage area and a derivative data area for accommodating intermediate data in the cache area; the execution area is provided with a fixed number of storage bits, the storage bits are arranged in sequence, and each storage bit contains at most one control point; setting an alarm value N, wherein the alarm value N is a preset value which is smaller than the number of storage bits and is greater than or equal to the B spline order, and setting the N storage bits at the last in the execution area as alarm nodes.

The temporary storage area is used for storing the ordered point set so as to adjust the ordered point set when the ordered point set does not meet the requirement. The storage bits are used to store control point data whose quantitative settings determine the upper limit of the control points used for a single fit. The buffer area is usually located in an internal memory such as a memory, and is used for reading and writing data at a high speed, and the ratio of the price to the unit memory is far higher than that of an external memory.

When fitting, the fitting group will generate a large amount of intermediate data to obtain the final fitting formula, where the intermediate data includes each basis function calculated by the debulk recurrence formula, temporary parameters generated when calculating the basis functions, and the like. These data are generated and then stored in the derivative data area to be distinguished from the execution area in which the control point is stored, so that the control point is not affected when the intermediate data are operated. The execution area is provided with fixed memory bits, which means that the memory area can only store a limited number of control point data.

Step three: applying for an ordered point set from the storage area based on the adjustment strategy and entering a temporary storage area.

Wherein the adjustment strategy comprises the following steps:

copying the first bit of the ordered point set in the temporary storage area into at least k coincident points and merging the at least k coincident points into the head of the ordered point set in the temporary storage area, wherein k is a preset fitting order;

the last bit of the ordered set of points in the scratch pad is copied to at least k coincident points and incorporated into the tail of the ordered set of points in the scratch pad.

The first point and the last point of the ordered point set in the temporary storage area respectively correspond to two end points of the fitting track. The two points are respectively copied into at least k coincident points, and the formed fitting group ensures that the corresponding fitting line segment is degenerated into one point, so that the whole fitting track coincides with the head end and the tail end of the input track.

Referring to fig. 4, the execution steps include at least the following five steps S1 to S5, respectively:

s1, a group of control points are moved out of an ordered point set in a temporary storage area, and the group of control points are written into storage bits of an execution area point by point.

In this embodiment, S1 quantitatively or quantitatively shifts out a set of control points from the head of the ordered point set in the temporary storage area, and writes the set of control points into the storage bit in the execution area that is the next earlier in sequence.

For example, the ordered set of points in the register p= { P1, P2, P3, …, pn }, where n is a finite value, in this embodiment n=100 is taken as an example. The system can quantitatively fetch points from the ordered point set in the temporary storage area in multiple times, and if the control point quantity of the ordered point set cannot be divided by the single fetch point quantity, the last fetch point quantity is less than the single fetch point quantity. For example, if the number of dots is 11 at a time, ten times are taken, and the last time is 1 dot. Of course, the system may not define a single dotting amount, for example, in the course of several consecutive dotting processes, the dotting is performed in the number of 10, 9, 8, respectively.

S2, performing B spline fitting on each point in the execution area according to the writing sequence to update a fitting track, and writing intermediate data generated by fitting calculation into the derivative data area. The B-spline curve surface has many excellent properties such as geometric invariance, convex hull property, convex keeping property, degradation reducibility, local propping property and the like. For example, n+1 control points Pi (i=0, 1,2, n), node vector u= { U0, U1, …, um }, the control points are connected in turn to form a characteristic polygon, and the expression of the k+1-order (k times) B spline curve can be defined as

Where Ni, k (u) is a k-th order B-spline basis function, also called a harmonic function, or a k-th order canonical B-spline basis function, the definition of its recursive formula (Cox-de Boor formula) is as follows:

here is stipulated that

Obviously, the basis function is defined by U, where the basis function satisfies the differential equation:

and when fitting is performed, the computer reads all control points in the execution area to be used as variables of fitting operation, so that intermediate data and a fitting curve are obtained, and the obtained intermediate data are stored in a derivative data area and are used in a follow-up interpolation process.

After the fitting is finished, a whole smooth curve is obtained, and the locality of the B-spline curve can be used for knowing that when one control point is modified, the shape of the control point is only changed locally.

In addition, the fitting track refers to a fitting track obtained by fitting all control points in the execution area at this time, and is not a fitting track obtained by fitting all control points in the whole ordered point set. As will be apparent from the following, the fitting tracks obtained in the two adjacent fitting processes have overlapping portions.

S3, interpolating the fitting track based on the interpolation strategy to update the interpolation track.

Wherein, referring to fig. 5, the interpolation strategy comprises the following steps:

the fitting track corresponds to the control point set in the current execution area, and when the control points in the execution area are updated, the fitting track is changed, so that the head end points of the substituted guide nodes are different.

first, node u (t) can be seen as a function of time t, and coordinate p (u) on the B-spline curve, i.e., the coordinates of the steering node, can be seen as a function of node u. Since p (u) and u (t) are not linearly related, it is necessary to estimate the coordinates of the next time from the coordinates and the speed of the current time. One or more taylor formulas may be selected for expansion, and in this embodiment, the node u (t) function is expanded by the taylor formula, and the node p (u) function is expanded by the taylor formula.

Now, there is a relation of p (u+Δ) =p (u) +p' (u) ×Δl, and it is now necessary to determine a node value corresponding to the movement distance (speed) Δl at the next moment. Only if the corresponding node value is found, the actual length of |p (u+Δ) -p (u) | can be close to the value of our desired travel distance (speed) Δl, otherwise the machine running speed is not controlled.

We now develop node u (t) at time t using a first order taylor formula, which can yield the following relationship,

the derivative of B-spline curve with respect to time t is known:

the finishing is carried out so as to obtain the finished products,

for a B-spline curve, the differential relationship is Δl=vi (t) ·Δt.

Let Δt=1, then Δl=vi (t), that is, when knowing the moving distance Δl, and the current node ui, the node value ui+1 at the next time can be obtained. Knowing the node value, the coordinate value on the B-spline curve corresponding to the node can be obtained.

The local interpolation line segments are connected end to end in sequence, and are combined to form a continuous interpolation track for guiding the movement of the executing piece. Through interpolation, when the robot walks to the current position, the robot can obtain the coordinates of the position that should be reached at the next moment through an interpolation algorithm.

Step five: returning to the second step until the guiding node is the end point of the fitting track;

The parameter i in the guiding node pi (u) is iterated continuously to continuously acquire the coordinate value of the guiding node pi+1 (u) along the fitting track until the end of the fitting track is reached.

S4, driving the execution element to move along the interpolation track, detecting the relative position relation between the execution element and interpolation track nodes, and inquiring the execution area to detect the residual quantity, wherein the residual quantity is the quantity of control points, corresponding to the nodes through which the execution element passes most recently, arranged in the following control point according to the writing sequence; judging whether the residual quantity is smaller than the warning value, if so, clearing the control points corresponding to the execution parts passing through the interpolation track nodes in the execution area, and returning to S1 until all the control points in the ordered point set in the temporary storage area are taken out; the warning value is a preset value smaller than the number of storage bits.

The execution area may be a queue or other data structure, and only the ordered control points need to be written into and read from the execution area after being written into the execution area in sequence. For example, the first time a fit is made, the system quantitatively copies a set of control points from the head of the ordered set of points in the scratch pad and deletes the copied control points in the ordered set of points in the scratch pad. And starting to sort and input control points by the first bit of the storage bits, fitting, interpolating the control points in the execution area, and controlling the execution piece to move along the interpolation track. When the control points are close to the tail end of the interpolation track, the control point data corresponding to the track which is already carried out by the execution part in the execution area are cleared, and then the rest control points are migrated to the storage bits which are in front of the sequence in the execution area according to the original sequence, and are usually the first bit and the later storage bits of the storage bits. Then the next fitting is performed.

Specifically, referring to fig. 6, S4 includes the following sub-steps:

s41, driving the executing piece to move along the interpolation track;

s42, detecting the relative position relation between the executing piece and the interpolation track node, inquiring the executing area to detect the residual quantity, judging whether the residual quantity is smaller than the warning value, and returning to S41 if not; if yes, clearing the storage bit data before the guard node in the execution area, and clearing the intermediate data corresponding to the control point stored by the data bit before the guard node in the derivative data area.

In the second step of the recording step, an alarm value N is set, wherein the alarm value N is a preset value which is smaller than the number of storage bits and is greater than or equal to the B spline order, and the last N storage bits in the execution area are set as alarm nodes. Because the calculated amount of fitting calculation is relatively large, when an executing piece moves to the tail end of the current fitting track, the executing piece is calculated again, the corresponding interpolation track cannot be formed in time, and the motion is stopped, so that the speed is reduced. The warning nodes are arranged, so that the system starts to update the fitting track when the executing piece is close to the fitting track end point but not reached, enough time can be reserved for subsequent calculation, the whole action flow of the mechanical arm is operated, and the calculated amount is reasonably dispersed in each beat.

S43, migrating the rest control points in the execution area to the storage bit with the front sequence in the execution area, and arranging according to the original sequence.

For example, the end of the mechanical arm is located on the interpolation track corresponding to the control point set { pi, pi+1, …, pi+n }, when the end of the mechanical arm moves to the interpolation track corresponding to the { pi+n-n+1, pi+n-n+2, …, pi+n }, the control points [ pi, pi+1, …, pi+n-N }, in the execution area are cleared, and the { pi+n-n+1, pi+n-n+2, …, pi+n } are sequentially filled from the first storage position in the execution area.

The intermediate nodes generated during the fitting and interpolation of the control points can be cleared immediately, and can be cleared uniformly when the executing piece reaches the warning node.

S44, judging whether all control points in the ordered point set in the temporary storage area are taken out, if not, returning to the step S1, and if so, entering the next step.

Following the example of S43, i+n-N control points are proposed from the ordered point set in the temporary storage area, and are sequentially filled into the remaining storage bits in the execution area until all the control points in the ordered point set in the temporary storage area are taken out, at this time, the movement of the execution element along the fitting track corresponding to the entire ordered point set is completed. The number of control points extracted may be less than i+n-N when the control points are extracted the last time.

From a storage perspective, the memory of the system is limited. For single track fitting, if the track to be fitted is longer and the track shape is variable, such as an embroidery operation, more control points are needed to ensure approximation of the fitted shape. If the fitting operation is only carried out once, all control points are necessarily recorded into the memory of the system, a large amount of intermediate data is generated during fitting calculation and interpolation calculation, and high requirements are certainly set for the memory capacity. According to the scheme, a mode of instant calculation is adopted, the B-spline is divided into a plurality of smaller paragraphs for calculation, and only a small number of control points are required to be recorded at a time, so that less intermediate data are generated, and the requirement on the memory capacity is reduced.

From a computational standpoint, the computational effort of the system is limited. In addition to fitting interpolation calculations, the system also needs to distribute limited computing power to various aspects. The scheme adopts a calculation and instant action mode, less control points are subjected to fitting interpolation at a time, the position of the next point is calculated through the points, and then the motion quantity of each joint of the mechanical arm is obtained through the position of the next point and a structural algorithm (bottom logic). The generated data are called in real time, and the data are used and deleted, so that the calculation is dispersed into the beats of each movement, the phenomenon that the work of other devices is influenced due to the fact that excessive calculation force is occupied at a certain moment is avoided, the phenomenon that the mechanical arm is suddenly stopped and started is avoided, and the abrasion caused by the movement of the mechanical arm is reduced.

S5, destroying the execution area.

After the executing piece completes the movement, destroying the executing area to release the space of the buffer area.

The embodiment of the application also discloses a control method of the motion speed based on the motion path planning method, which is used for controlling the movement of the executing piece on the interpolation track corresponding to the fitting track.

In the recording step, the system establishes a plurality of adjacent threshold intervals, each threshold interval corresponds to an operation speed, and all the threshold intervals are combined into a positive real number domain;

for example, the positive real number field is divided into (0, 1), (1, 4), (4, 25), (25, + -infinity) for classifying the average radius of curvature of different segments, the four intervals may correspond here to an operating speed of 1m/s,2m/s,4m/s,10m/s, respectively.

Referring to fig. 7, S3 described above may be divided into the following sub-steps:

s31, converting the fitting track into an interpolation track based on an interpolation strategy, and writing intermediate data generated by interpolation calculation into a derivative data area; wherein the intermediate data generated by the interpolation computation corresponds to the control point.

It should be noted that the interpolation does not perform interpolation on the whole fitting track at one time, but performs interpolation on a local curve where the executing piece is located. In the subsequent process, the speed planning is performed according to the local line segment of the fitting track which the executing piece will pass through next, and the interpolation is continued according to the result of the speed planning.

S32, equally or variably spacing the fitting track or dividing the fitting track into a plurality of sections of ordered local curves based on a dividing strategy.

The current fit trajectory is generated based on control points in the execution region, and the trajectory is smooth and relatively gentle since the selection interval of the control points is related to the curvature of the teaching trajectory. The fit trajectory is further divided here so that the radii of curvature corresponding to the points inside the local curve are closer. The method can adopt an equipartition mode, so that excessive intermediate data are not required to be generated during division, and more calculation force is not required.

Alternatively, a partitioning strategy may be employed to process the fitted curve to partition into multiple ordered segments of local curves.

Referring to fig. 8, the partitioning strategy includes the steps of:

step one: selecting a plurality of separation points on an input curve at equal intervals;

step two: calculating the curvature radius of the input curve at each separation point;

step three: judging whether the ratio of the curvature radiuses of the input curves at all adjacent interval points is within a preset range or not; if not, segmenting the line segment between the two separation points again based on a partitioning strategy;

step four: sequentially sequencing the line segments separated by the interval points of the input curve, and outputting.

For example, the fit trajectory is processed using a partitioning strategy. In the first step of the partitioning strategy, partition points are selected at equal intervals on the input curve, which is equivalent to multipoint sampling on the fitting track. In the second and third steps, the curvature radius of the fit track on each adjacent separation point is calculated, if the curvature radius is smaller than a preset range, for example, the preset range is (0.9,1.1), the fluctuation of the fit track is smaller, and each line segment divided by the separation points can be output as a local curve. If the ratio of the curvature radius of the fitting track on two adjacent separation points is 2 and is larger than the upper limit of the preset range, the fitting track is larger in fluctuation and needs to be further divided. And thirdly, further dividing the line segments between the two separation points based on a division strategy by using an iterative method until the line segments obtained by division meet the requirements, and entering a fourth step, and sequentially sequencing the line segments separated by the separation points and outputting the line segments.

By means of the dividing strategy, the fitting track can be divided into a plurality of sections of local line segments with approximate curvature radiuses at all positions, so that subsequent processing is facilitated.

S33, calculating the average curvature radius of the current track based on a statistical strategy, wherein the current track is a local curve corresponding to the interpolation track where the executing piece is currently located; judging a threshold value interval in which the average curvature radius of the current track is positioned, and adjusting the current speed to be the running speed corresponding to the threshold value interval, wherein the current speed is the moving speed of the executing piece on the interpolation track; and calculating the average curvature radius of the local curve where the current local curve is located so as to judge the corresponding highest running speed under the average curvature radius. The structural characteristics of the manipulator can show that the moving speed of the manipulator can be higher under the condition of ensuring relatively smaller impact when moving on a track with smaller bending degree.

For example, if the local curve has an average radius of curvature of 10cm, and is located in a section (4, 25) in units of centimeters, the local curve may have a maximum operating speed of 4m/s.

Referring to fig. 9, the statistical policy includes the steps of:

sampling points are uniformly or non-uniformly selected on an input curve;

calculating the curvature radius of the input curve at each sampling point;

And sampling the input curve through a statistical strategy to obtain the average value of the curvature radius of the input curve at all sampling points so as to represent the bending degree of the input curve, and the planning of the movement speed of the mechanical arm is conveniently guided. For example, 5 sampling points are uniformly obtained on a local curve, and the curvature radius of the local curve at the 5 sampling points is respectively 9.5cm,10.5cm,10.2cm,9.8cm and 10.0cm, so that the average value of the average curvature radius corresponding to the local curve is 10.0cm, and the corresponding highest running speed is 4m/s. Here, for example, sampling points with slightly different average curvature radii are used in the same division interval, and if the average curvature radii are slightly different, a smaller running speed is used as the corresponding curvature radius of the corresponding interval.

S34, calculating the average curvature radius of a future track based on a statistical strategy, wherein the future track is the next section of local curve connected with the current track; judging a threshold value interval in which the average curvature radius of the future track is positioned, and setting the future speed as the running speed corresponding to the threshold value interval, wherein the future speed is the moving speed of the executing piece on the interpolation track corresponding to the future track;

similarly, the average radius of curvature corresponding to the future trajectory is calculated and used to guide the speed planning of the implement on the future trajectory.

S35, judging the relative sizes of the current speed and the future speed, and driving the execution piece to smoothly transition from the current speed to the future speed at the junction of the current track and the future track based on a planning strategy.

Because of the existence of motion inertia, in order to avoid phenomena such as impact, step-out and oscillation caused by starting and stopping of a motion axis of an executing piece and track switching, acceleration and deceleration control must be performed on the connecting position of adjacent local line segments of the executing piece, so that speed planning at the junction of the current track and the future track is embodied, and reasonable speed planning not only can improve running precision, but also can effectively reduce running abrasion of the executing piece and prolong service life.

Referring to fig. 10, the planning strategy includes the steps of:

step one: acquiring two curves which are sequentially and smoothly connected, taking the curves as a first line segment and a second line segment, and setting the vicinity of the junction of the first line segment and the second line segment as a speed regulation area;

step two: acquiring the planned moving speed of the executing piece on the first line segment and the planned moving speed of the executing piece on the second line segment, and respectively serving as the initial speed and the final speed of the executing piece moving on the speed regulation area;

step three: a first speed planning for uniformly changing the speed regulation area based on the initial speed and the final speed;

step four: and (3) performing a second speed planning of symmetrical graph replacement of the equal integral area on the acceleration curve obtained by the first speed planning.

The input of the planning strategy is two smoothly connected line segments, and the junction is taken as a speed regulation area, when the planned moving speeds of the two line segments are equal, the speed at the speed regulation area is also equal to the planned moving speed in practice. When the planned moving speeds of the first line segment and the second line segment are different, the distance from one end of the speed regulating area to the other end is set to be a uniform speed changing stage, the first speed planning can be uniform acceleration or uniform deceleration, the acceleration at the first segment is a constant value, and then the second speed planning is carried out, so that the phenomenon of acceleration mutation is eliminated in the process of changing the initial speed to the final speed. Abrupt changes in acceleration can be seen mechanically as sudden forces or reduced forces on the actuators, which tend to impact the actuators and cause wear and even misalignment.

Referring to fig. 11, the second speed profile includes the steps of:

step one: acquiring a uniform speed change stage in a first speed plan;

step two: adopting a symmetrical and smooth acceleration curve to replace an acceleration curve in a uniform speed change stage, wherein the integral area of the symmetrical and smooth acceleration curve with respect to time is equal to the integral area of the acceleration curve in the uniform speed change stage with respect to time;

step three: and calculating a jerk curve, a speed curve and a displacement curve of the executing piece in the speed regulation area based on the replaced acceleration curve.

After the symmetrical and smooth acceleration curve is replaced by the acceleration curve in the uniform speed change stage, the jerk of the new acceleration curve can be fixed or intermittent, so long as infinite or near infinite break points do not occur. Therefore, the acceleration curve is a continuous and conductive smooth curve, and the acceleration curve is embodied in that the force applied to and reduced from the executing piece is uniformly changed, so that the executing piece is protected.

S36, selecting a plurality of detection points on a future track at equal intervals or variable intervals, and sequentially writing the detection points into a detection point set; sequentially reading detection points from the detection point set, and setting a deceleration zone at the detection points based on a local deceleration strategy; and planning the speed at the junction of the deceleration zone and the part outside the deceleration zone based on a planning strategy.

In this step, the radius of curvature of the future track may be rechecked, or the step may be omitted, and the recheck may not be performed. Since the division of the local curve is based on a statistical strategy to calculate the average radius of curvature, if the local part has speed fluctuation and is not sampled, for example, the local curve is a sinusoidal curve, and the sampling points are located at the wave crests, the curvature of the whole curve is regarded as unchanged, that is, the local part is sampled to a straight line, which is obviously unreasonable. The local curve will be continually substituted into the future track by oversampling the future track and determining whether to perform speed regulation based on the sampling result.

Referring to fig. 12, the local deceleration strategy includes the steps of:

step one: inputting a detection point and a curve to be decelerated;

step two: calculating the curvature radius of a curve to be decelerated on a detection point, and judging a threshold interval where the curvature radius is positioned so as to obtain the corresponding running speed of the detection point;

step three: judging whether the running speed corresponding to the detection point is smaller than the running speed corresponding to the threshold interval where the average curvature radius of the curve to be decelerated is located, if so, the point is a deceleration point, and deceleration areas are arranged in front of and behind the deceleration point, wherein the running speed corresponding to the deceleration area is the running speed corresponding to the deceleration point.

The detection point and the curve to be decelerated are input and the radius of curvature of the deceleration curve at the detection point is calculated, and it should be noted that the selection step of the detection point in S36 should be smaller than the selection step of the points in the previous step, so as to find out whether the future track ignores the area requiring deceleration due to the previous statistical error. For example, the average value of the average radius of curvature corresponding to the curve to be decelerated is 10.0cm, and the corresponding maximum running speed is 4m/s. However, the radius of curvature of a detection point in the curve to be decelerated is 3.5cm, which is lower than the lower limit value of the interval (4, 25), so that deceleration is required at that point.

Step four: the detection point positioned behind the deceleration point in the deceleration zone moves to a deceleration point set;

step five: judging whether the operation speed corresponding to the detection point in the deceleration point set is smaller than the operation speed corresponding to the deceleration point, if so, taking the corresponding operation speed of the detection point corresponding to the smaller operation speed as the operation speed of the deceleration area, and if not, ending.

And step four and step five are used for judging whether a plurality of continuous detection points needing to be decelerated are simultaneously positioned in the deceleration area determined by the first point. When found, the corresponding minimum operating speed of the several points is taken as the operating speed of the deceleration zone to avoid continuous or repeated setting of the deceleration.

S37, interpolating the fitting track based on the speed planning to generate a corresponding interpolation track, and driving the executing piece to move along the interpolation track on the base material according to the sequence direction of the control points.

After the speed planning is finished, the executing piece can be driven to move along the interpolation track on the substrate material according to the sequence direction of the control points. The speed planning method is performed on the basis of logic of fitting, speed planning, interpolation, movement, continuous speed planning and continuous interpolation, and instant interpolation instant movement is planned in instant, and the speed planning is prospective, so that the movement of an executing piece on a current track is not influenced. The embodiment also discloses a host computer, which comprises a memory and a processor, wherein the memory stores a computer program which can be loaded by the processor and is used for the motion path planning method.

The present embodiment also discloses a storage medium storing a computer program capable of being loaded by a processor and executing the above-described motion path planning method.

The above embodiments are not intended to limit the scope of the present application, so: all equivalent changes in structure, shape and principle of the application should be covered in the scope of protection of the application.

Claims

1. A local deceleration control method for speed planning, comprising the steps of:

acquiring a curve to be decelerated and a detection point positioned on the curve to be decelerated;

2. The local deceleration control method for speed planning according to claim 1, characterized in that the curvature statistics method of the curve to be decelerated at the detection point comprises the steps of:

calculating the curvature radius of the input curve at each sampling point;

3. A method of planning a movement speed, comprising the steps of:

sequentially reading detection points from a detection point set, and setting a deceleration zone at the detection point based on the local deceleration control method according to any one of claims 1-2;

4. A method of planning a speed of movement according to claim 3, further comprising the steps of:

5. A method of planning a speed of movement according to claim 3, wherein the partitioning strategy comprises the steps of: selecting a plurality of separation points on an input curve at equal intervals;

calculating the curvature radius of the input curve at each separation point;

6. The method of claim 4, wherein the planning strategy comprises the steps of: acquiring two curves which are sequentially and smoothly connected, taking the curves as a first line segment and a second line segment, and setting the junction of the first line segment and the second line segment as a speed regulation area front and back;

performing a second speed planning of symmetrical graph replacement with equal integral area on the acceleration curve obtained by the first speed planning;

the second speed schedule includes the steps of:

acquiring a uniform speed change stage in a first speed plan;