CN112207833A

CN112207833A - Method and system for planning movement path, host and storage medium

Info

Publication number: CN112207833A
Application number: CN202011114914.2A
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-12
Anticipated expiration: 2040-10-16
Also published as: CN112207833B

Abstract

The application relates to a planning method, a system, a host and a storage medium of a motion path, which comprises an input step and an execution step, wherein the input step comprises the following steps: selecting control points on the teaching sample and recording the control points as an ordered point set into a storage area; establishing an execution area and a temporary storage area with a plurality of storage bits in a cache area; recording the ordered point set into a temporary storage area; the execution steps comprise: s1, a group of control points are removed from the ordered point set in the temporary storage area and bits are stored; s2, performing B spline fitting on the control points in the execution area according to the writing sequence to update the fitting track; s3, interpolating the fitting track to update the interpolation track; s4, driving the execution element to move along the interpolation track, inquiring the execution area to detect the residual quantity, judging whether the residual quantity is smaller than a warning value, if so, clearing control points of the execution element in the execution area corresponding to nodes of the interpolation track, and returning to S1 until all the control points in the order point set in the temporary storage area are taken out; and S5, destroying the execution area. The method has the advantages of saving calculation power and memory.

Description

Method and system for planning movement path, 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 planning a motion path.

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 of fitting control points to a continuous curve using a uniform B-spline, thereby achieving smooth transition. However, the inventor believes that the fitting method has higher computational power requirement and memory requirement on a chip on a manipulator with the increase of the number of control points, so that the fitting method is difficult to be popularized and used in a low-configuration manipulator which is in large quantity in the market at present.

Disclosure of Invention

In order to save labor and memory, the application provides a method, a system, a host and a storage medium for planning a motion path.

In a first 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:

recording:

establishing an execution area and a temporary storage area in a cache area, wherein the execution area is provided with a fixed number of storage bits, and each storage bit at most contains one control point;

applying for an ordered point set from the storage area based on the adjustment strategy and recording the ordered point set into a temporary storage area;

the execution steps are as follows:

s1, a group of control points are intensively shifted out from the ordered points in the temporary storage area, and the group of control points are written into the storage bits of the execution area point by point;

s2, performing B spline fitting on each control point in the execution area according to a writing sequence to update a fitting track;

s3, interpolating the fitting track based on an interpolation strategy to update the interpolation track;

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; the warning value is a preset value smaller than the number of the storage bits;

and S5, destroying the execution area.

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 cache region establishes an execution region and a temporary storage region, the execution region has a fixed number of storage bits, the execution region can be a queue or other data structures, and only the ordered control points can be written into the execution region and then read sequentially. 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.

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 and interpolating the control points in the execution area according to the writing sequence, and then controlling an execution piece to move along an interpolation track, wherein the execution piece can be the tail end of a mechanical arm or other accurately movable devices. When the executive member moves on the interpolation track, the control points corresponding to the nodes which are not passed by the executive member on the interpolation track gradually decrease. And when the residual quantity of the control points is less than the warning value, clearing the control point data corresponding to the track which has been passed by the executive component in the executive area, and then carrying out next fitting.

And repeating the steps until all the control points in the ordered point set in the temporary storage area are taken out, namely, completing the movement of the executive component along the whole fitting track. And finally, destroying the execution area and deleting redundant intermediate data.

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.

Preferably, the entering step further comprises: recording a preset fitting order, and setting the order of B spline fitting as the preset fitting order; the storage bits are arranged in a sequence, and an alert node is arranged, wherein the alert node is the last N storage bit in the execution area, and N is an alert value;

the control points in the S1 are quantitatively moved out from the ordered point set in the temporary storage area and are sequentially written into the execution area;

the S4 includes the steps of:

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 yes, clearing storage bit data in the execution area before the warning node;

s43, transferring the remaining control points in the execution area to storage bits in the execution area, wherein the storage bits are arranged in the execution area in the front order and are arranged according to the original order;

and S44, returning to S1 until all the control points in the ordered point set in the temporary storage area are taken out.

By adopting the technical scheme, the control points in the execution area are fitted and interpolated to obtain an interpolated track, and the control points stored in the warning nodes have corresponding nodes on the interpolated track. And monitoring the world coordinate of the execution piece, when the world coordinate of the execution piece reaches the point, clearing the storage bit data before the warning point, and transferring the rest data to the storage bit in the execution area in the front of the sequence, usually starting from the first position and arranging according to the original sequence.

If the system empties the original execution area and takes out a new execution point from the ordered point set in the temporary storage area to substitute into the execution area after the execution point moves to the end point completely along an interpolation track, and fits and interpolates a track for guiding the execution piece to move again, the execution piece inevitably stops moving and stops suddenly in the calculation process. Even if speed planning is performed, the overall operating time will be increased. Therefore, the technical scheme adopts a distributed calculation mode, forward planning can be effectively carried out on the path, the calculation process is dispersed in each exercise beat, and the use ratio of the calculation force in the calculation process of single calculation is reduced.

Preferably, the entering step further comprises: marking out a derived data area for accommodating intermediate data in the buffer area;

the S22 further includes: writing intermediate data generated by fitting calculation into a derived data area;

the S3 further includes: writing intermediate data generated by interpolation calculation into the derived data area, wherein the intermediate data generated by interpolation calculation corresponds to the control point;

the S42 further includes: and clearing the intermediate data corresponding to the control point stored by the data bit before the guard node in the derivative 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.

Preferably, the sampling strategy comprises the following steps:

selecting control points on the input uniformly or non-uniformly;

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.

Preferably, 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; copying the last bit of the ordered point set in the temporary storage area into at least k coincident points and merging the coincident points into the tail of the ordered point set in the temporary storage area, wherein k is a preset fitting order number-1.

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.

Preferably, the sampling strategy comprises the following steps:

inputting a teaching track image;

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 method for recording the teaching sample can be adopted under the condition that the teaching point is manually selected without adopting a control controller. The teaching trajectory image corresponds to a teaching sample, and may be a CAD image. The teaching track is uniformly or non-uniformly divided into a plurality of sections, and the dividing step length can be manually set according to the overall shape of the pattern so as to avoid missing small and abrupt bends. 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.

Preferably, 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 second aspect, the present application provides a mobile system based on path planning, which adopts the following technical solutions:

a mobile system based on path planning, comprising:

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.

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

a host computer comprising a memory and a processor, the memory having stored thereon a computer program that can be loaded by the processor and that implements the motion path planning method described above.

In a fourth aspect, the present application provides a computer-readable storage medium, which adopts the following technical solutions: a computer-readable storage medium storing a computer program that can be loaded by a processor and executes the method as described above.

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, 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_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.

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), 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

u_i∈[0,1]。

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

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

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:

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 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 system for planning a movement path, 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 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 for planning a motion path, comprising:

recording:

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;

the execution steps are as follows:

and S5, destroying the execution area.

2. The method for planning a movement path according to claim 1, wherein the entering step further comprises: recording a preset fitting order, and setting the order of B spline fitting as the preset fitting order; the storage bits are arranged in a sequence, and an alert node is arranged, wherein the alert node is the last N storage bit in the execution area, and N is an alert value;

the S4 includes the steps of:

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

3. The method for planning a movement path according to claim 2,

the entering step further comprises: marking out a derived data area for accommodating intermediate data in the buffer area;

the S2 further includes: writing intermediate data generated by fitting calculation into a derived data area;

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

selecting control points on the input uniformly or non-uniformly;

5. The method for planning a movement path according to claim 2, wherein 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; copying the last bit of the ordered point set in the temporary storage area into at least k coincident points and merging the coincident points into the tail of the ordered point set in the temporary storage area, wherein k = a preset fitting order-1.

6. The method for planning a movement path according to claim 1 or 5, wherein the sampling strategy comprises the following steps:

inputting a teaching track image;

7. A method for planning a movement path according to claim 1, wherein the interpolation strategy comprises the following steps:

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

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

and the circulating judgment module is used for detecting the relative position relationship between the executive component and the interpolation track node, inquiring the executive area to detect the residual quantity, clearing the control points of the executive component in the executive area corresponding to the interpolation track node when the residual quantity is less than the warning value, and sequentially driving the fitting module, the interpolation module and the moving module until all the destruction control points concentrated by the ordered points are taken out, and executing the area.

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.