CN112276906A

CN112276906A - Method and system for controlling movement speed, host and storage medium

Info

Publication number: CN112276906A
Application number: CN202011114690.5A
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: 2021-01-29
Anticipated expiration: 2040-10-16
Also published as: CN114872035A; CN114872035B; CN112276906B

Abstract

The application relates to a control method, a system, a host and a storage medium of movement speed, comprising the following steps: establishing a plurality of adjacent threshold intervals, wherein each threshold interval corresponds to the running speed; obtaining a smooth fitting track and converting the fitting track into an interpolation track; dividing the fitting track into a plurality of sections of ordered local curves; calculating the average curvature radius of the current local curve and the next local curve of the executive component to obtain the corresponding threshold interval, and adjusting the running speed of the executive component in the current local curve and the next local curve to be the running speed corresponding to the corresponding threshold interval; and comparing the relative sizes of the two speeds, and driving the executing piece to perform smooth transition between the two speeds at the junction of the current local curve and the next local curve. The application ensures that the executing part is slowly accelerated or decelerated on a smooth track so as to reduce the risk that the executing part is suddenly stressed to aggravate abrasion.

Description

Method and system for controlling movement speed, host and storage medium

Technical Field

The present application relates to the field of automation control, and in particular, to a method, a system, a host and a storage medium for controlling a motion speed.

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 a method for fitting control points into a continuous curve at one time by using a B-spline, thereby realizing smooth transition. However, the inventors have considered that even if the B-spline fitting is performed, the curvature of the B-spline still changes, and if reasonable acceleration and deceleration is not performed, accelerated wear of the manipulator occurs, which affects the service life.

Disclosure of Invention

In order to enable the execution part to smoothly accelerate or decelerate on a smooth track so as to reduce the risk that the execution part is suddenly stressed to aggravate abrasion, the application provides a control method, a control system, a host and a storage medium of the motion speed.

In a first aspect, the present application provides a method for controlling a movement speed, which adopts the following technical scheme: a method of controlling the speed of movement 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;

obtaining a smooth fitting track, and converting the fitting track into an interpolation track for the movement of an executive component;

dividing the fitting track into a plurality of sections of ordered local curves;

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;

calculating the average curvature radius of a future track based on a statistical strategy, wherein the future track is a 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;

the relative magnitudes of the current and future speeds are determined, and an implement is driven to smoothly transition from the current speed to the future speed at the intersection of the current and future trajectories 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 the implement from having a sudden speed change 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 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.

Preferably, the step of obtaining a smooth fit trajectory comprises:

inputting a teaching track image;

dividing the teaching track into a plurality of sections;

selecting control points at equal intervals on each motion track;

and ordering the control points according to the selection sequence and fitting the control points into a B spline curve.

By adopting the technical scheme, the teaching track is obtained, dense control points can be selected at places with larger bending degree manually, and sparse control points are selected at places with smaller bending degree, so that the fitting curve obtained by the control points is more approximate to the teaching track. 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. When a certain control point is changed, only the local part of the curve is influenced, so that the curve is beneficial to modifying at the later stage and the calculation power is saved.

Preferably, the method further comprises the following steps:

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 zone at the detection points based on a local deceleration strategy;

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

The local deceleration strategy comprises the steps of:

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.

By adopting the technical scheme, the curvature radius of the future track is rechecked, and since the local curve is divided based on a statistical strategy to calculate the average curvature radius, if the speed fluctuation occurs locally, the curvature radius may not be sampled exactly. 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.

Preferably, the local deceleration strategy further comprises the steps of:

step four: moving a detection point positioned behind a 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 zone.

By adopting the technical scheme, whether a plurality of continuous detection points needing to be decelerated are simultaneously positioned in the deceleration area determined by the first point is judged. 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.

Preferably, the statistical strategy comprises the following steps:

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

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 input curve is sampled at multiple points to obtain the average value of the curvature radius of the input curve on all sampling points, so that the bending degree of the input curve is represented, and the planning of the moving speed of the mechanical arm is conveniently guided.

Preferably, the planning strategy 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;

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 at 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.

Preferably, the second speed schedule includes the steps of:

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 second aspect, the present application provides a control system for a movement speed, which adopts the following technical solutions: a control system for speed of movement comprising:

the comparison module comprises a plurality of adjacent threshold intervals, each threshold interval corresponds to the running speed, and all the threshold intervals are combined into a positive real number domain;

the fitting module is used for acquiring a smooth fitting track and dividing the fitting track into a plurality of sections of ordered local curves based on a first strategy;

the interpolation moving module is used for converting the fitting track into an interpolation track for the movement of the executive component;

the speed control module is used for calculating the average curvature radius of the current track, wherein the current track is a local curve corresponding to the current interpolation track of the executing piece; the system is used for 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 an executing piece on the interpolation track where the executing piece is located;

the speed presetting module is used for calculating the average curvature radius of a future track, wherein the future track is the next section of local curve connected with the current track; the future speed is the moving speed of the executive component on the interpolation track corresponding to the future track;

and the speed adjusting module is used for 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.

In a third aspect, the present application provides a host, which adopts the following technical solution:

a host comprising a memory and a processor, the memory having stored thereon a computer program that can be loaded by the processor and executed to perform the method as described above.

In a fourth aspect, the present application provides a host, which adopts the following technical solution:

a computer-readable storage medium storing a computer program which can be loaded by a processor and which executes the method according to any one of claims 1 to 7.

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 disclosure;

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 block diagram of a local slow down 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 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.

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, between the selection of control points on Line AThe interval is greater than Line B, and the selection interval of the control points on Line B is greater than 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_nWhere 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.

S2, B spline fitting is carried out on each point in the execution area according to the writing sequenceTo update the fitting trajectory and write the intermediate data resulting from the fitting calculation into the derived 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), node vector U-U₀,u₁,…,u_mConnecting 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

2≤k≤n+1，m＝n+k+1。

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 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;

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.

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;

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_iThen 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.

Step four: and updating the interpolation track based on the local interpolation track.

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+nMove 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+nWhen the interpolation track is corresponding to the control point { p } in the execution area is cleared_i,p_i+1,…,p_i+n-NAnd will { p }_i+n-N+1,p_i+n-N+2,…,p_i+nFill 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 fit 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 of the intersection of the deceleration zone and the part outside the deceleration zone based on the 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 dividing 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, the curvature of the whole curve is regarded as 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:

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 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 the point, the radius of curvature of the curve to be decelerated at the detection point is in the interval (1, 4), so that the corresponding maximum operating speed is 2m/s, and the corresponding operating 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 control system for motion speed, including:

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 is used for controlling the movement speed.

The embodiment also discloses a storage medium which stores a computer program capable of being loaded by a processor and executing the control method of the movement speed.

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 control method of motion speed is characterized by comprising the following steps:

2. The method for controlling a moving speed according to claim 1, wherein the step of obtaining a smooth fitting trajectory comprises:

inputting a teaching track image;

dividing the teaching track into a plurality of sections;

selecting control points at equal intervals on each motion track;

3. The method for controlling a moving speed according to claim 1, further comprising the steps of:

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

the local deceleration strategy comprises the steps of:

4. The method of controlling movement speed of claim 3, wherein the local deceleration strategy further comprises the steps of:

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

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

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

6. The method for controlling movement speed according to claim 1, 3 or 4, wherein the planning strategy comprises the steps of:

7. The method of controlling a moving speed according to claim 6, wherein the second speed profile includes the steps of:

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

8. A control system for speed of movement, comprising:

9. A host comprising a memory and a processor, the memory having stored thereon a computer program that can be loaded by the processor and that executes the method according to any of claims 1 to 7.

10. A computer-readable storage medium, in which a computer program is stored which can be loaded by a processor and which executes the method according to any one of claims 1 to 7.