CN114872035A

CN114872035A - Local deceleration control method, movement speed planning method, movement path planning method and data updating method

Info

Publication number: CN114872035A
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: 2022-08-09
Anticipated expiration: 2040-10-16
Also published as: CN112276906A; CN114872035B; CN112276906B

Abstract

The application relates to a local deceleration control method, a movement speed planning method, a movement path planning method and a data updating method, wherein the local deceleration control method comprises the following steps of: establishing a plurality of adjacent threshold intervals respectively corresponding to the running speeds; acquiring a curve to be decelerated and detection points on the curve; acquiring the curvature radius of the deceleration curve on a detection point, and judging a threshold interval where the curvature radius is located to obtain the running speed corresponding to the detection point; obtaining a deceleration point from the detection point based on the relative size of the operation speed of the detection point and the curve to be decelerated; setting deceleration zones corresponding to the running speed of the deceleration point before and after the deceleration point; establishing a deceleration point set corresponding to each deceleration zone and writing the deceleration point set into detection points in the deceleration zones; and acquiring 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 zone. The application has the advantage that the executing part is decelerated in an unpredicted steep track section to reduce abrasion.

Description

Local deceleration control method, movement speed planning method, movement path planning method and data updating method

The invention is a divisional application, which is based on the application number of 2020111146905, the application date of 2020.10.16 and the name of 'a method, a system, a host and a storage medium for controlling the movement speed'.

Technical Field

The present invention relates to the field of automation control, and in particular, to a local deceleration control method, a movement speed planning method, a movement path planning method, and a data updating method.

Background

With the development of industrial automation in recent years, automation control is becoming an emerging subject and is developing rapidly. The mechanical arm is an important means for improving labor productivity, improving labor conditions, reducing labor intensity of workers and realizing industrial production automation, and the application and development of the mechanical arm are very important at home and abroad. Manipulators were first used in the automotive industry, and were commonly used for welding, painting, loading, unloading and handling. The manipulator extends and expands the functions of hands, feet and brain of a person, and can replace the person to work in dangerous, harmful, toxic, low-temperature, high-temperature and other severe environments; the labor intensity is increased, the labor productivity is improved, and the product quality is ensured.

The robot can be moved according to a fixed program by motion teaching. No matter the motion track is simple or complex, the robot needs to be controlled to complete the track motion by methods such as linear interpolation, circular interpolation or joint interpolation. Obviously, the more point positions are taught, the higher the precision of the motion trajectory of the manipulator. However, more point locations means more line segments, and frequent acceleration and deceleration is required to complete the transition between line segments, thereby reducing the overall efficiency. At present, the related art proposes to fit control points into a continuous curve at one time by using a B-spline, and then perform interpolation on the B-spline fitting curve, thereby realizing smooth transition.

However, the inventor believes that there will still be curvature change on the B-spline fitting curve, and although it is possible to uniformly obtain sampling points on the fitting curve to calculate the curvature radius at the sampling points to guide acceleration and deceleration of the manipulator, for complex patterns, there will often exist a plurality of peaks and valleys in a short distance, if they are not exactly sampled, there will be "floating" on the data, and corresponding speed control will not be performed, which will easily cause accelerated wear of the manipulator, and will affect the service life.

Disclosure of Invention

In order to decelerate the executing part in an unpredicted steep track section so as to reduce the risk that the executing part is suddenly stressed to aggravate abrasion, the application provides a local deceleration control method, a motion speed planning method, a motion path planning method and a 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 detection points positioned on the curve to be decelerated, wherein a point set of the detection points does not contain a point set of sampling points of the curve to be decelerated;

acquiring the curvature radius of the deceleration curve on a detection point, and judging a threshold interval where the curvature radius is located to obtain the running speed corresponding to the detection point;

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

setting a deceleration zone before and after the deceleration point, wherein the running speed corresponding to the deceleration zone 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, the curve needing to be decelerated is selected, and the detection point is selected from the curve and used for calculating the curvature radius of the point position which is not sampled on the curve. The curvature radius of the curve to be decelerated is obtained by averaging the curvature radii of the curves at all the sampling points, so that the point set of the detection points is not a subset of the sampling points, and the detection points can be enabled to avoid the sampling points.

In actual conditions, the fluctuation of the curve is often a section comprising a plurality of wave crests and wave troughs, and a single abrupt peak is rare, so when a deceleration point is detected, in order to save calculation power, the scheme selects to directly set deceleration zones before and after the deceleration point instead of setting detection points with higher density before and after the deceleration point. When a plurality of continuous detection points needing to be decelerated are simultaneously positioned in the deceleration zone, because the range of the deceleration zone is smaller, the mechanical arm is abraded when frequent acceleration and deceleration are carried out 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 the repeated setting of the deceleration are avoided, the calculation force is saved, and a 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, the local average curvature of the curve to be decelerated is calculated based on the sampling points, so that the detection points and the sampling points with different step lengths are selected, the detection points can avoid the sampling points to obtain new data, and the situation that the data are overlapped and the fluctuation between the two sampling points cannot be found is prevented.

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

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

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

the average of the radii of curvature of the input curve over all sample points is calculated.

By adopting the technical scheme, the multi-point sampling is carried out on the curve to be decelerated, and the average value of the curvature radius of the input curve on all sampling points is obtained so as to represent the bending degree of the input curve and facilitate the guidance of the planning of the moving speed of the mechanical arm.

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

a method for planning movement speed comprises the following steps:

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 executive component, and the future track is a next section of local curve connected with the current track;

acquiring a threshold interval where the average curvature radius of the future track is located, and taking the running speed corresponding to the threshold interval as the future speed, wherein the future speed is the moving speed of the executive component on the interpolation track corresponding to the future track;

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

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

and planning the speed at the intersection 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 intervals for classifying the average curvature radius of different local curves, so that the executing piece has different running speeds on the line segments corresponding to different intervals. The smooth fit trajectory may be a B-spline curve, or other smooth curve, etc., to prevent abrupt velocity changes of the actuator at a certain node on the curve. The fitted track is sliced into several shorter local curves to make the corresponding curvature radius of each point in the local curve 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 part on the track. As can be seen from the structural characteristics of the manipulator, when the manipulator moves on a trajectory with a smaller degree of curvature, the moving speed of the manipulator can be higher while ensuring relatively small wear. Due to the existence of motion inertia, in order to avoid the phenomena of impact, step loss, oscillation and the like caused by starting and stopping of a motion shaft of an executing piece and track switching, acceleration and deceleration control must be carried out on the executing piece at the joint of adjacent local line segments, and the speed planning is embodied as the speed planning of the junction of a deceleration zone and a part outside the deceleration zone.

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 where the average curvature radius of the current track is located, 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 element on the interpolation track where the executing element is located;

an implement is driven to smoothly transition from a current speed to a future speed at an intersection of the current trajectory and the future trajectory based on a planning strategy.

By adopting the technical scheme, due to the existence of motion inertia, in order to avoid the phenomena of impact, step loss, oscillation and the like caused by starting and stopping of a motion shaft of the executing piece and track switching, acceleration and deceleration control must be carried out on the executing piece at the joint of adjacent local line segments, so that the speed planning of the current track and the future track junction is realized, the running precision can be improved by reasonable speed planning, the running abrasion of the executing piece can be effectively reduced, and the service life is prolonged.

Optionally, the partitioning policy includes the following steps:

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

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

judging whether the ratio of the curvature radii of the input curve at all adjacent spaced points is within a preset range or not; if not, segmenting the line segment between the two separation points based on the division strategy again;

and 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. And (4) equally spacing the input curve to select the separation points, which is equivalent to carrying out multi-point sampling on the fitting track. And then calculating the curvature radius of the fitting track on each adjacent separation point, wherein if the curvature radius is within a preset range, the fluctuation of the fitting track is small, and each line segment divided by the separation points can be output as a local curve. If the deviation exceeds the preset range, the fluctuation of the fitting track is large, and further division is needed. And further dividing the line segment between the two separation points by using an iteration method based on a division strategy, and sequencing the line segments of the input curve separated by the separation points in sequence and outputting the line segments until the divided line segments meet the requirements. By the dividing strategy, the fitting track can be divided into a plurality of local line segments with approximate curvature radiuses at all positions, so that the 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 in front and at the back;

acquiring the planned moving speed of the executive component on the first line segment and the planned moving speed of the executive component on the second line segment, and respectively taking the planned moving speeds as the initial speed and the final speed of the executive component moving on the speed regulation area;

performing a first speed planning of uniform speed change on the speed regulation area based on the initial speed and the final speed;

and performing a second speed planning of replacing the symmetrical graph with 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 a speed regulation area, and when the planned moving speeds of the two line segments are equal, the speed of the speed regulation area is also equal to the planned moving speed actually. When the planned moving speeds of the first line segment and the second line segment are different, the path from one end of the speed regulating area to the other end is roughly set as a uniform speed changing stage, uniform acceleration or uniform deceleration can be performed, the acceleration of the section is a fixed value, and then the second speed is planned, 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 viewed as mechanically applying or reducing force to the actuator, easily causing impact to the actuator and resulting in wear and even motion misalignment.

Optionally, the second speed schedule includes the following steps:

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

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

and calculating a jerk curve, a speed curve and a displacement curve of the actuating member in the speed regulating region based on the replaced acceleration curve.

By adopting the technical scheme, after the acceleration curve in the uniform speed change stage is replaced by the symmetrical and smooth acceleration curve, the jerk of the new acceleration curve can be fixed or discontinuous, and the jerk is only required to be free from infinite or nearly infinite discontinuity points. Therefore, the curve of the acceleration is a smooth curve which can be continuously guided, and the stress application and the stress reduction of the executing part are uniformly changed, so that the executing part is protected.

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

a planning method of a motion path comprises the following steps:

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

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

establishing ordered memory bits in the execution region;

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

grouping the control points in the temporary storage area according to a point sequence and moving the control points into an 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 the storage bits;

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

and interpolating the fitted track based on the interpolation strategy and the planning method of the motion speed to generate an interpolated track.

By adopting the technical scheme, people select the teaching sample, and select a series of ordered control points through the teaching sample to serve as data generated for a teaching track. These control points are stored in an ordered manner in a memory area, which is usually located outside a hard disk or the like, for storing inactive data. When all the control points are recorded, 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 requirement. The execution region has a fixed number of storage bits for storing control point data, the quantitative setting of which determines the upper limit of control points used for a single fit. The cache region 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 price to unit storage is far higher than that of an external memory.

Before the first fit, an ordered set of points is applied from the storage area and entered into the scratch pad. At the beginning of the first fit, a set of control points is applied from the head of the ordered set of points in the scratch pad, either quantitatively or non-quantitatively, and written into the execution area, while the copied control points in the ordered set of points in the scratch pad are deleted. And then fitting the control points in the execution area according to the writing sequence. And in the fitting process, speed planning is carried out based on the future curve pair, the future curve is interpolated based on the planned speed, and finally the executive component is controlled to move along the interpolation track.

For storage considerations, the system has limited memory. For a single trace fit, if the trace to be fitted is long and the trace shape is variable, such as an embroidery operation, more control points, such as thousands, are required to ensure an approximation of the fitted shape. If one-time fitting is carried out, all control points are bound to be recorded into a memory of a system, a large amount of intermediate data is generated when fitting calculation and interpolation calculation are carried out, and undoubtedly, a high requirement is put on the memory capacity. The scheme adopts a distributed calculation mode, the B spline is divided into a plurality of small paragraphs for calculation, and a small amount of control points are only needed to be recorded at a time, so that less intermediate data are generated, and the requirement on the memory capacity is reduced.

The computational power of the system is limited with respect to the computational load. The system needs to distribute finite computational power to various aspects in addition to performing the fitting interpolation calculation. The scheme adopts a distributed calculation mode, fitting interpolation is carried out on fewer control points at a time, the position of the next point is calculated through the points, and then the motion amount of each joint of the mechanical arm is obtained through the position and the structural algorithm (bottom layer logic) of the next point. The generated data are called in real time and deleted after being used, so that the calculation is dispersed to the beats of each movement, the phenomenon that the work of other devices is influenced due to the fact that a certain moment occupies too much calculation force is avoided, the phenomenon that the mechanical arm is suddenly stopped and suddenly 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 derived data area in a cache area;

writing intermediate data generated by fitting calculation into a derived data area;

and writing the intermediate data generated by the interpolation calculation into the derived data area.

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

Optionally, the sampling strategy includes the following steps:

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

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

By adopting the technical scheme, the required track has the difference of smoothness and distortion, and when the curvature change of each position of the track is small, for example, a line segment, the control points can be uniformly selected on the line segment. When the curvature changes more severely, denser control points are selected at the part with larger curvature change and sparser control points are selected at the part with smaller curvature. Too much control point selection will affect the fitting efficiency, and too little control point selection 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 local teaching tracks, and calculating the average curvature radius of each local teaching track;

and respectively selecting control points at equal intervals on each section of local teaching track, wherein the interval of the control points on each section of local teaching track is 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 that small and abrupt bends are avoided. Calculating the average curvature radius of each section of local teaching track, wherein the high average curvature radius indicates that the whole teaching track is relatively smooth and needs fewer control points; a low average radius of curvature means that the whole is more curved and requires more control points. For small and abrupt bends at a certain position, control points can be added manually so as to achieve the purpose that the fitting shape is more similar to the teaching track.

Optionally, the adjusting policy includes the following steps:

copying the head of the ordered point set in the temporary storage area into the head of the ordered point set in the temporary storage area, wherein the head of the ordered point set in the temporary storage area is at least k coincident points;

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

wherein k is the order of the B spline fitting.

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 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 integral fitting track is coincident with the head end and the tail end of the input track.

Optionally, the interpolation strategy includes the following steps:

the method comprises the following steps: acquiring a head end point and a tail end point of the fitting track, and substituting the head end point into a guide node, wherein the guide node is used for guiding the movement of the executive component;

step two: performing first-order Taylor expansion on the fitting track at the guide node, and obtaining a first-order 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 coordinate values of one end, far away from the guide node, of the local interpolation line segment into the guide node;

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

step five: and returning to the step two until the guide node is the end point of the fitting track.

By adopting the technical scheme, the fitting track is continuously expanded at the guide node to obtain a local interpolation line segment so as to update the interpolation track and obtain a new guide node. And continuously obtaining a local interpolation line segment corresponding to the fitting track through multiple iterations, and obtaining the coordinates of the position to be reached at the next moment through an interpolation algorithm after the robot moves to the current position.

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

a data updating method based on the 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 executive component to move along the existing interpolation track;

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

based on the relative size of the redundant value and the warning value, clearing a control point in the execution area before the warning node 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;

and taking out the control points in the temporary storage area in sequence, and writing the control points into storage bits behind the original control points in the execution area in sequence.

By adopting the technical scheme, along with the movement of the executive component on the interpolation track, the control points corresponding to the part of the fitting track, which is not passed by the executive component, are gradually reduced. When the remaining point value is smaller than the warning value, clearing the control point data before the warning node in the execution area, sequentially moving the head part of the ordered point set in the temporary storage area to the part, located in front of the warning node, in the execution area, clearing the moved control point data in the temporary storage area, and then starting next fitting based on the control point data in the execution area.

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

Optionally, the guard node is a storage bit with an order greater than the order of the B-spline in the execution region, and the guard value is a value which is smaller than the number of the storage bits and greater than or equal to the order of the B-spline.

Drawings

FIG. 1 is a block diagram of a procedure for entering steps in an embodiment of the present application;

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

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

FIG. 4 is a block diagram of a process for performing steps in an embodiment of the present application;

FIG. 5 is a block diagram of an interpolation strategy according to an embodiment of the present application;

FIG. 6 is a block diagram of the S4 sub-step in an embodiment of the present application;

FIG. 7 is a block diagram of the S3 sub-step in an embodiment of the present application;

FIG. 8 is a block diagram of a partitioning policy in an embodiment of the present application;

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

FIG. 10 is a block diagram of a process for planning a strategy in an embodiment of the present application;

FIG. 11 is a block diagram of a second speed plan in an embodiment of the present application;

fig. 12 is a flowchart of a local deceleration strategy in an embodiment of the present application.

Detailed Description

The present application is described in further detail below with reference to figures 1-12.

The robot can be moved according to a fixed program by motion teaching. No matter the motion track is simple or complex, the robot needs to be controlled to complete the track motion by methods such as linear interpolation, circular interpolation or joint interpolation. When the mechanical arm moves along a complex track, if control points are simply input according to the shape of the track and an executive part is controlled to move along a small line segment formed by connecting the control points, when two adjacent straight lines or arcs are transited, acceleration and deceleration are needed and small pause is generated, the frequent acceleration and deceleration can prolong the working period, the pause feeling of the clamp is enhanced along with the increase of the running speed in the whole process, the speed is difficult to increase to the faster speed, and the working efficiency of the robot is limited. When the taught points are more points, the motion trail precision of the manipulator is higher. However, more point locations means more line segments, and the acceleration and deceleration are more frequent, thereby reducing the overall efficiency.

Currently, polynomial fitting and least square fitting are commonly used as fitting methods. The polynomial fitting is carried out by presetting corresponding powers and substituting certain control points to obtain a smooth curve connecting the control points. However, as the power of the polynomial fit increases, the computational load of the single fitting operation will also increase significantly. Meanwhile, the dragon lattice phenomenon is more prominent in high-power polynomials, and the change of the slope of the curve between adjacent control points becomes very large, which is not beneficial to acceleration and deceleration control. In addition, when one control point is added or subtracted, the overall shape of the high-order curve is greatly changed, which is not favorable for fitting the original track.

When least square fitting is used, a basic function of a fitting curve needs to be preset, and then the sum of the distance variances between each control point and 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 is very similar to the shape of the teaching track, but the method has large limitation 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 method for planning a motion path. The planning method is a B-spline fitting-based method and comprises an entering step for entering control points and an executing 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 executing component is a working component with multiple degrees of freedom, and can be a tail end of a mechanical arm, or a truss type control device, and is selected as the mechanical arm in the embodiment.

With reference to fig. 1, the logging step comprises at least two steps, respectively:

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

Referring to fig. 2, the sampling strategy includes:

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

the teaching sample can be an object with a plane surface or an object with a three-dimensional curved surface on the surface. The robot generally has a plurality of degrees of freedom, and the position and orientation of the robot tip can be adjusted in a three-dimensional space so as to be attached to various positions on 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 as required. Wherein a control point refers to a special point used to control and adjust the shape of the curve.

The teaching sample can also be provided with marking lines, such as colored curves, for guiding the human to operate the mechanical arm to perform teaching actions. In this embodiment, the teaching sample is selected as a flat plate-like object, and the surface of the teaching sample is provided with black marking lines.

The selection mode of the control point comprises a manual mode or an off-line programming mode. The manual mode is that the control handle is used for controlling the tail end of the robot hand to move along a track on the teaching sample, and the postures of all joints of the robot hand are continuously adjusted in the moving process, so that the tail end of the robot hand is attached to the surface of the teaching sample or keeps a certain distance from the surface of the teaching sample to move. And in the moving process, sequentially recording the control points according to the requirements of an operator.

In the off-line programming mode, the control points are selected based on the digital image instead of using the handle. Referring to fig. 3, the offline programming mode includes the following sub-steps:

the first substep: 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, and can also be a 2D plane image with a required teaching track.

And a second substep: and dividing the teaching track into a plurality of local teaching tracks. The teaching trajectory can be automatically segmented, i.e. divided according to fixed step sizes, and the step sizes are determined according to specific images. The teaching track can also be segmented manually, 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 measurement method. In actual operation, the segmentation can be performed by a method combining automatic segmentation and manual segmentation.

And after segmentation, calculating the average curvature radius of each segment, wherein the average curvature radius can roughly represent the smoothness of the local track of the segment.

And a third substep: and respectively selecting control points at equal intervals on each section of the local teaching track, wherein the interval of the control points on each section of the local teaching track is positively correlated with the average curvature radius of each section of the local teaching track.

The high average curvature radius shows that the whole is relatively gentle, and fewer control points are needed; the low average curvature radius indicates that the whole body is relatively curved, and more control points are needed to better represent the shape characteristics of the teaching track. For example, teaching trajectories

Wherein

That is, Line A is more gradual than Line B, and Line B is more gradual than Line C. Therefore, the selection interval of the control points on Line A is greater than that of Line B, and the selection interval of the control points on Line B is greater than that of Line C. For small and abrupt bending at a certain position, control points can be added manually to achieve the purpose that the fitting shape is more similar to the teaching track, and the control points are ignored according to the importance degree of the teaching track so as to keep the flatness of the fitting track at the later stage.

And a strategy step two: the control points are sorted according to the selection sequence and are used as an ordered point set;

after the teaching tracks are divided in order, the control points on each section of local teaching track are also selected in order, so that all the control points are arranged in order on the teaching tracks. And orderly arranging each control point, writing the control points into the ordered point set in order, so that the control points can be taken out from the ordered point set in order at a later stage, and the fitting track is approximate to the teaching track.

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

The scratch pad is used to store the ordered set of points so that the ordered set of points can be adjusted when the ordered set of points does not meet the requirements. The storage bits are used to store control point data whose quantitative settings determine the upper limit of control points used for a single fit. The cache region 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 price to unit storage is far higher than that of an external memory.

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

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

The adjustment strategy comprises the following steps:

copying the head of the ordered point set in the temporary storage area into at least k coincident points and merging the 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 merged into the tail of the ordered set of points in the scratch pad.

The first and last points of the ordered set of points in the scratch pad correspond to two endpoints of the fit trajectory, respectively. 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 integral fitting track is coincident with the head end and the tail end of the input track.

Referring to FIG. 4, the executing steps include at least the following five steps S1-S5:

s1, a group of control points are removed from the ordered point set in the temporary storage area, and the group of control points are written into the storage bit of the execution area point by point.

In this embodiment, S1 quantitatively or non-quantitatively removes a set of control points from the head of the ordered set of points in the scratch pad, and sequentially writes the set of control points to the sequentially preceding storage bits in the execution area.

For example, the ordered set of points in the scratch pad, P ═ P ₁ ，P ₂ ，P ₃ ，…，P _n Where n is a finite value, in this embodiment, n is 100 as an example. The system can quantitatively fetch points from the ordered point set in the temporary storage area for a plurality of times, and if the control point quantity of the ordered point set cannot be completely removed by the single point fetching quantity, the last point fetching time is less than the single point fetching quantity. For example, if the number of dots taken in a single time is 11, ten times are taken, and the last time is 1 dot. Of course, the system may not specify a single point-taking amount, for example, in the process of taking points continuously for several times, the point-taking is performed in the number of 10, 9, or 8, respectively.

And S2, performing B spline fitting on each point in the execution area according to the writing sequence to update the 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, convexity protection property, deterioration reduction property, local support property and the like. For example, n +1 control points P are known _i (i-0, 1, 2.., n), and a node vector U-U ₀ ，u ₁ ，…，u _m Connecting the control points in sequence can form a characteristic polygon, and an expression of a B spline curve of k +1 order (k times) can be defined as

Wherein N is _i，k (u) is a k-th order B-spline basis function, also called harmonic function, or a k-th order canonical B-spline basis function, whose recursive formula (Cox-de Boor formula) is defined below:

is defined herein

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

during each fitting, the computer reads all the control points in the execution area as variables of fitting operation, so that intermediate data and a fitting curve are obtained, and the obtained intermediate data are stored in the derived data area for use in the subsequent interpolation process.

After the fitting is finished, a whole smooth curve is obtained, and the shape change only occurs locally when a certain control point is modified according to the locality of the B-spline curve.

In addition, the fitting track here refers to only the fitting track obtained by fitting all the control points in the execution region at this time, but not the fitting track obtained by fitting all the control points in the whole ordered point set. It can be subsequently known from the following that the fitting trajectories obtained in the two adjacent fitting processes have overlapping parts.

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

Referring to fig. 5, the interpolation strategy includes the following steps:

the fitting track is corresponding 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 substituted into the guide nodes are different.

first, the node u (t) can be considered as a function of time t, and the coordinates p (u) on the B-spline curve, i.e., the coordinates of the index node, can be considered as a function of node u. Since p (u) and u (t) are not in a linear relationship, it is necessary to estimate the coordinates of the next time from the coordinates and speed of the current time. The first order or multi-order Taylor formula can be selected for expansion, in this embodiment, the first order Taylor formula is used to expand the function of the node u (t), and the first order Taylor formula is used to expand the function of the node p (u).

There is a relational expression of p (u + Δ) ═ p (u) + p' (u) × Δ l, and it is now necessary to determine a node value corresponding to a moving distance (speed) Δ l at the next time. Only if the corresponding node value is found, the actual length of | p (u + Δ) -p (u) | can be similar to the value of the movement distance (speed) Δ l that we expect, otherwise the machine running speed is not controlled.

Now we expand the node u (t) at time t with the first order taylor formula, we can get the following relationship,

knowing the derivative of the B-spline curve over time t:

the finishing agent can be obtained by finishing,

for B-spline curves, the differential relationship is Δ l ═ v _i (t)·Δt。

Let Δ t equal to 1, then Δ l equal to v _i (t), that is, when the distance of movement Δ l is known, and the current node u _i Then the node value u at the next moment can be obtained _i+1 . 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 sequentially connected end to end, and a continuous interpolation track is formed after combination and is used for guiding the movement of the executive component. By the interpolation action, after the robot moves to the current position, the robot can obtain the coordinates of the position to be reached at the next moment by the interpolation algorithm.

Step five: returning to the step two until the guide node is the terminal point of the fitting track;

to guide node p _i (u) the parameter i is iterated continuously to obtain the guidance node p continuously along the fitted trajectory _i+1 (u) until the end of the fitted trajectory is reached.

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

The execution area may be a queue or other data structure, as long as the ordered control points are enabled to write sequential reads after sequential writes to the execution area. For example, at 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 sequencing and recording the control points from the first bit of the storage bit, fitting and interpolating the control points in the execution area, and controlling the execution piece to move along the interpolation track. When the interpolation track is close to the tail end of the interpolation track, control point data corresponding to the track already passed by the execution element in the execution area is cleared, and then the rest control points are transferred to the storage bits which are arranged in the execution area in the former sequence, usually the first storage bit and the later storage bit of the storage bits. The next fit is then made.

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

s41, driving the executive component to move along the interpolation track;

s42, detecting the relative position relation between the executive component and the interpolation track node, inquiring the executive area to detect the residual quantity, judging whether the residual quantity is less than a warning value, and if not, returning to S41; if so, clearing the storage bit data positioned in the execution area before the guard node, and clearing the intermediate data corresponding to the control point stored by the data bit before the guard node in the derived data area.

And setting an alarm value N in the second step of the logging step, wherein the alarm value N is a preset value which is smaller than the number of the storage bits and is larger than or equal to the B spline order, and the last Nth storage bit in the execution area is set as an alarm node. Because the calculated amount of the fitting calculation is relatively large, the executing part calculates when moving to the tail end of the current fitting track, so that a corresponding interpolation track cannot be formed in time, the motion is stopped, and the speed is reduced. Due to the arrangement of the warning nodes, the system starts to update the fitting track when the executive component is close to the fitting track end point but does not reach the fitting track end point, sufficient time can be reserved for subsequent calculation, the whole action process of the mechanical arm runs, and the calculated amount is favorably reasonably dispersed in each beat.

S43, transferring the remaining control points in the execution area to the storage positions in the execution area, which are in the front of the sequence, and arranging the control points according to the original sequence.

For example, the end of the robot arm is located at the set of control points { p } _i ，p _i+1 ，…，p _i+n Move on the corresponding interpolation track, when the end of the mechanical arm moves to { p } _i+n-N+1 ，p _i+n-N+2 ，…，p _i+n When the interpolation track is corresponding to the control point [ p ] in the execution area is cleared _i ，p _i+1 ，…，p _i+n-N And will { p } _i+n-N+1 ，p _i+n-N+2 ，…，p _i+n Fill in from the first storage bit of the execution area in turn.

The intermediate nodes generated during control point fitting and interpolation can be cleared immediately after being used, and can also be cleared uniformly when the executive component reaches the warning node.

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

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

The memory of the system is limited in terms of storage. For a single trajectory fit, if the trajectory to be fit is long and the shape of the trajectory is variable, such as an embroidery operation, more control points are required to ensure an approximation of the fit shape. If only one fitting operation is performed, all control points are necessarily recorded in the memory of the system, and a large amount of intermediate data is generated during fitting calculation and interpolation calculation, which undoubtedly puts high requirements on the memory capacity. The scheme adopts an instant calculation mode, the B spline is divided into a plurality of small paragraphs for calculation, and a small amount of control points are only needed to be recorded at a time, so that less intermediate data are generated, and the requirement on the memory capacity is reduced.

The computational power of the system is limited in terms of computational power. The system needs to distribute finite computational power to various aspects in addition to performing the fitting interpolation calculation. The scheme adopts a calculation-instant mode, fitting interpolation is carried out on fewer control points once, the position of the next point is calculated through the points, and then the motion amount of each joint of the mechanical arm is obtained through the position and the structural algorithm (bottom layer logic) of the next point. The generated data are called in real time and deleted after being used, so that the calculation is dispersed to the beats of each movement, the phenomenon that the work of other devices is influenced due to the fact that a certain moment occupies too much calculation force is avoided, the phenomenon that the mechanical arm is suddenly stopped and suddenly started is avoided, and the abrasion caused by the movement of the mechanical arm is reduced.

And S5, destroying the execution area.

After the execution piece finishes moving, the execution area is destroyed to release the space of the buffer area.

The embodiment of the application is also based on the motion path planning method, and discloses a motion speed control method for controlling the movement of an executive component on an interpolation track corresponding to a fitting track.

In the recording step, the system establishes a plurality of adjacent threshold intervals, each threshold interval is respectively corresponding to the running speed, wherein all the threshold intervals are combined into a positive real number domain;

for example, the real number domain is divided into (0, 1], (1, 4], (4, 25], (25, + ∞) and is used for classifying the average curvature radius of different line segments, so that the executing element has different running speeds on the line segments corresponding to different intervals, wherein the four intervals can respectively correspond to the running speeds of 1m/s, 2m/s, 4m/s and 10 m/s.

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

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

It should be noted that the interpolation is not performed on the whole fitted track at one time, but performed on the local curve where the actuator is located. In the subsequent process, speed planning is carried out according to the local line segment of the fitting track which is going to be passed by the executive component, and interpolation is carried out continuously according to the result of the speed planning.

And S32, dividing the fitting track into a plurality of sections of ordered local curves at equal intervals or variable intervals or based on a dividing strategy.

The current fitted trajectory is generated based on the control points in the execution area, and since the selection intervals of the control points are related to the curvature of the taught trajectory, the trajectory is smooth and relatively gentle. The fit trajectory is further divided so that the corresponding radii of curvature for each point within the local curve are closer together. The division can be carried out in an equal division mode, so that excessive intermediate data do not need to be generated during division, and more calculation power does not need to be generated.

Optionally, a partitioning strategy may be adopted to process the fitted curve to partition the fitted curve into a plurality of segments of ordered local curves.

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

the method comprises the following steps: selecting a plurality of separation points at equal intervals on an input curve;

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

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

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

For example, the fitted trajectory is processed using a partitioning strategy. In the first step of the partitioning strategy, partition points are selected at equal intervals from the input curve, which is equivalent to performing multi-point sampling on the fitting track. In the second step and the third step, the curvature radius of the fitting 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), it is indicated that the fluctuation of the fitting track is small, and each line segment divided by the separation points can be output as a local curve. If the ratio of the curvature radii of the fitting track on two adjacent separation points is larger than the upper limit of the preset range, the fluctuation of the fitting track is large, and further division is needed. And in the third step, the line segment between the two separation points is further divided by using an iterative method based on a division strategy until the divided line segments meet the requirements, and then the fourth step is carried out, and the line segments of the input curve separated by the separation points are sequentially sorted and output.

By the dividing strategy, the fitting track can be divided into a plurality of local line segments with approximate curvature radiuses at all positions, so that the 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 current interpolation track of the executive component; judging a threshold interval where the average curvature radius of the current track is located, and adjusting the current speed to be the running speed corresponding to the threshold interval, wherein the current speed is the moving speed of the executing piece on the interpolation track where the executing piece is located; and calculating the average curvature radius of the current local curve to judge the corresponding highest running speed under the average curvature radius. As can be seen from the structural characteristics of the robot, when the robot moves on a trajectory having a smaller degree of curvature, the moving speed of the robot can be higher while ensuring that the impact is relatively small.

For example, if the local curve has an average radius of curvature of 10cm and is located in the interval (4, 25), and the unit of the value in the interval is cm, the maximum running speed corresponding to the local curve is 4 m/s.

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

uniformly or non-uniformly selecting sampling points on an input curve;

calculating the curvature radius of the input curve on 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 on all sampling points so as to represent the bending degree of the input curve and facilitate guidance of planning of the moving speed of the mechanical arm. For example, if 5 sampling points are uniformly obtained on the local curve, 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, the average value of the average curvature radius corresponding to the local curve is 10.0cm, and the corresponding maximum running speed is 4 m/s. The sampling points with the average curvature radii which are not different are exemplified, the average is used in the same division area, and if the average curvature radii are different greatly, the smaller running speed is adopted as the corresponding curvature radii of the corresponding intervals.

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 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 interval, wherein the future speed is the moving speed of the executive component on the interpolation track corresponding to the future track;

similarly, the corresponding average radius of curvature for the future trajectory is calculated and used to guide the velocity planning of the implement over the future trajectory.

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

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

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

the method comprises 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 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 executive component on the first line segment and the planned moving speed of the executive component on the second line segment, and respectively taking the planned moving speeds as the initial speed and the final speed of the executive component moving on the speed regulation area;

step three: performing a first speed planning of uniform speed change on the speed regulation area based on the initial speed and the final speed;

step four: and performing a second speed planning of replacing the symmetrical graph with 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 used 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 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 path from one end of the speed regulating area to the other end is roughly set as a uniform speed changing stage, the first speed is planned to be uniform acceleration or uniform deceleration, the acceleration of the section is a fixed value, and then the second speed is planned to eliminate the phenomenon of acceleration sudden change in the process of changing the initial speed to the final speed. Abrupt changes in acceleration can be viewed as mechanically applying or reducing force to the actuator, easily causing impact to the actuator and resulting in wear and even motion misalignment.

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

the method comprises the following steps: acquiring a uniform speed change stage in a first speed plan;

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

step three: and calculating a jerk curve, a speed curve and a displacement curve of the actuating member in the speed regulating region based on the replaced acceleration curve.

After the acceleration curve of the uniform speed change stage is replaced by the symmetrical and smooth acceleration curve, the jerk of the new acceleration curve can be fixed or discontinuous as long as infinite or nearly infinite discontinuity points do not occur. Therefore, the curve of the acceleration is a smooth curve which can be continuously guided, and the stress application and the stress reduction of the executing part are uniformly changed, so that the executing part 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 intersection of the deceleration zone and the part outside the deceleration zone based on a planning strategy.

In this step, the curvature radius of the future trajectory is rechecked, or this step may be omitted and the recheck is not performed. Since the division of the local curve is based on the calculation of the average curvature radius by the statistical strategy, if the speed fluctuation occurs locally and is not sampled, for example, the local curve is a sine curve, and the sampling points are located at the peaks, it is considered that the curvature of the whole curve is unchanged, that is, a straight line is sampled, which is obviously unreasonable. The local curves will therefore be continuously substituted into future trajectories, by oversampling the future trajectories and deciding whether to perform speed regulation or not based on the sampling result.

Referring to fig. 12, the local deceleration strategy comprises the following steps:

the method comprises the following steps: inputting a detection point and a curve to be decelerated;

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

step three: and judging whether the running speed corresponding to the detection point is less than the running speed corresponding to the threshold interval where the average curvature radius of the curve to be decelerated is located, if so, determining that the point is a deceleration point, and setting deceleration areas 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.

Inputting the detected point and the curve to be decelerated and calculating the radius of curvature of the deceleration curve at the detected point, it should be noted that the selected step size of the detected point in S36 should be smaller than that of the point of the previous step, so as to find out whether the future trajectory ignores the region requiring deceleration because of the previous statistical error. For example, the average radius of curvature of the curve to be decelerated corresponds to an average value of 10.0cm, and the maximum operating speed corresponds to 4 m/s. However, the curvature radius of a detection point in the curve to be decelerated is 3.5cm and is lower than the lower limit value of the interval (4, 25), so that deceleration is needed at the point, the curvature radius of the curve to be decelerated at the detection point is positioned in the interval (1, 4), so that the corresponding highest running speed is 2m/s, and the corresponding running speed of the deceleration zone is 2 m/s.

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

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

And step four and step five are used for judging whether a plurality of continuous detection points needing deceleration 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.

And S37, interpolating the fitted track based on the speed plan to generate a corresponding interpolated track, and driving the executing piece to move along the interpolated 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 on the base material along the interpolation track according to the sequence direction of the control points. The speed planning method is wholly carried out based on the logic of fitting, speed planning, interpolation, motion, continuous speed planning and continuous interpolation, and the real-time motion of the real-time interpolation is planned in real time.

The embodiment of the present application further discloses a system for planning a movement path, including:

the input module is used for sequentially selecting a plurality of control points on the teaching sample and inputting the control points into the storage area as an ordered point set;

the cache module comprises an execution area and a temporary storage area, wherein the execution area is positioned in the cache area, the temporary storage area is used for storing the ordered point set copied from the storage area, the execution area is provided with a fixed number of storage bits, and each storage bit at most contains one control point;

the writing module is used for shifting out a group of control points from the ordered point set in the temporary storage area and writing the group of control points into the storage bits of the execution area point by point;

the fitting module is used for performing B spline fitting on each control point in the execution area according to the writing sequence so as to update the fitting track;

the interpolation module is used for interpolating the fitting track to update the interpolation track;

the moving module is used for driving the executive component to move along the interpolation track;

and the circulating judgment module is used for detecting the relative position relationship between the execution piece and the interpolation track node, inquiring the execution area to detect the residual quantity, clearing the control points of the execution piece in the execution area corresponding to the interpolation track node when the residual quantity is less than the warning value, sequentially driving the fitting module, the interpolation module and the moving module until all the control points in the ordered point set in the temporary storage area are taken out, and destroying the execution area.

The embodiment also discloses a host computer, which comprises a memory and a processor, wherein the memory is stored with a computer program which can be loaded by the processor and used for the motion path planning method.

The embodiment also discloses a storage medium which stores a computer program capable of being loaded by a processor and executing the motion path planning method.

The above embodiments are preferred embodiments of the present application, and the protection scope of the present application is not limited by the above embodiments, so: all equivalent changes made according to the structure, shape and principle of the present application shall be covered by the protection scope of the present application.

Claims

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

2. The local deceleration control method of motion speed according to claim 1, wherein the selection step of the detection point is smaller than the selection step of the sampling point.

3. The local deceleration control method of motion speed according to claim 1, characterized in that said statistical method of curvature of curve to be decelerated on a detection point comprises the following steps:

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

4. A method for planning a movement speed is characterized by comprising the following steps:

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

and planning the speed of the intersection of the deceleration zone and the part outside the deceleration zone based on the planning strategy.

5. The method for planning a movement speed of claim 4, further comprising the steps of:

6. Method for planning a movement velocity according to claim 4, characterized in that the partitioning strategy comprises the following steps:

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

judging whether the ratio of the curvature radii of the input curve at all adjacent spaced points is within a preset range or not; if not, segmenting the line segment between the two separation points again based on the division strategy;

7. Method for the planning of movement speed according to claim 5, characterized in that the planning strategy comprises the following steps:

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

the second speed schedule comprises the steps of:

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

8. A method for planning a motion path, comprising:

establishing ordered memory bits in the execution region;

interpolating the fitted trajectory to generate an interpolated trajectory based on an interpolation strategy and a method of planning a motion speed according to any of claims 4 to 7.

9. The method for planning a movement path according to claim 8, wherein the sampling strategy comprises the steps of:

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

the control points are sorted according to the selection sequence and are used as an ordered point set;

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;

selecting control points at equal intervals on each section of local teaching track, 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;

optionally, the adjusting policy includes the following steps:

wherein k is the order of B spline fitting;

optionally, the interpolation strategy includes the following steps:

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

10. A data updating method based on real-time motion state of an executing part, which is used for the planning method of the motion path according to claim 8 or 9, and comprises the following steps:

driving the executive component to move along the existing interpolation track;