A planning method for realizing quick response and smooth following of a mechanical arm by cooperating with a sampler

Abstract

The application relates to a planning method for realizing quick response and smooth following of a mechanical arm in cooperation with a sampler, comprising the following steps: acquiring position data of the sampler, wherein the position data comprises position information at multiple time points and a sampling interval of position information at adjacent time points; performing timeout denoising on the position data of the sampler according to the sampling interval, to obtain denoised position data; storing the filtered position data into a FIFO queue with a fixed length; determining a differential speed of a median value, position information of the median value and a speed stability value; determining a target position and a target speed of each trajectory planning segment based on the differential speed of the median value, the position information of the median value and the speed stability value; and planning each trajectory planning segment according to the target position and the target speed, to obtain a planning trajectory of each trajectory planning segment. The method can make the response speed of the mechanical arm relatively fast and smooth, and can realize linkage control of quick and smooth following in cooperation with the sampler.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot motion control, in particular, to a planning method for realizing smooth following of fast response of a mechanical arm in cooperation with a sampler. BACKGROUND

[0002] In the fields of industrial automation, medical robots and human-machine cooperation, the linkage control of mechanical arms and external samplers is applied more and more widely. Through the linkage control of samplers, the traditional teaching process can be greatly simplified in the field of industrial automation, and the distance restriction between doctors and patients can be broken through to save valuable treatment time in the medical field.

[0003] The traditional linkage control planning method generally adopts position control, and the speed is planned according to the displacement distance between the starting point and the ending point. The velocity limit is introduced when the planning curve is generated. The common methods include trapezoidal planning and spline planning. The traditional planning method faces the following problems when dealing with continuous points:

[0004] When the sampler collects continuous points, it is easy to collect data whose speed and acceleration far exceed the hardware limit of the mechanical arm. When the user controls the sampler to collect position information, the acceleration at the moment when it is stationary to motion can reach dozens of times the maximum acceleration value of the mechanical arm. Similarly, the deceleration from motion to stationary also far exceeds the hardware performance of the mechanical arm. This results in that if the planning is strictly followed according to the sampling point position, the following effect is represented as sudden stop, sudden start accompanied by motion jitter, and response delay caused by insufficient acceleration and deceleration, which obviously does not meet the application requirements of the mechanical arm following.

[0005] In the linkage control, there are usually high requirements for the response delay of the linkage control and the smoothness of the speed of the mechanical arm. For example, in the industrial pipeline, too high response delay will result in the inability to match the speed of the pipeline. In the medical field, the jitter of the mechanical arm will affect the precision during the operation. In addition, the linkage control method will also have a great impact on the control effect. The common ones include time jitter caused by remote communication and invalid point collection caused by the mechanical hardware limit of the sampler. SUMMARY

[0006] In order to overcome at least one of the deficiencies in the prior art, the present application provides a planning method for realizing smooth following of fast response of a mechanical arm in cooperation with a sampler.

[0007] In a first aspect, a planning method for realizing smooth following of fast response of a mechanical arm is provided, comprising:

[0008] obtaining position data of the sampler, the position data including position information at multiple time points and sampling intervals of position information at adjacent time points;

[0009] The position data of the sampler is denoised according to a sampling interval, and denoised position data is obtained;

[0010] The size of the noise window is determined according to the sampling interval;

[0011] The cutoff frequency is determined according to the sampling interval, and the denoised position data is low-pass filtered based on the cutoff frequency, and filtered position data is obtained;

[0012] The filtered position data is stored in a FIFO queue with a fixed length, and the length of the FIFO queue is the size of the noise window;

[0013] The differential speed at each time is determined according to the position data stored in the FIFO queue, and the median differential speed and the median position information are determined according to the differential speed at each time; the acceleration at each time is determined according to the differential speed at each time, and the accelerations at all times constitute an acceleration curve, and the speed stable value is determined according to the acceleration curve;

[0014] The target position and target speed of each trajectory planning segment are determined based on the median differential speed, the median position information and the speed stable value; each trajectory planning segment is planned according to the target position and target speed, and the planning trajectory of each trajectory planning segment is obtained.

[0015] In one embodiment, the target position and target speed of each trajectory planning segment are determined based on the median differential speed, the median position information and the speed stable value; each trajectory planning segment is planned according to the target position and target speed, and the planning trajectory of each trajectory planning segment is obtained, including:

[0016] Step 71, for the first trajectory planning segment, the planning start point speed of the first trajectory planning segment is initialized to 0;

[0017] Step 72, the target position and target speed of the current trajectory planning segment are determined according to the median differential speed, the median position information and the speed stable value obtained at the planning start time;

[0018] Step 73, the differential acceleration is calculated according to the target speed and the planning period;

[0019] Step 74, if the differential acceleration is greater than the maximum acceleration / deceleration range, the maximum speed that can be reached within the planning period is taken as the planning end point speed; a speed curve is generated according to the planning start point speed, the planning end point speed and the planning period; and the planning end point speed is taken as the planning start point speed of the next trajectory planning segment;

[0020] If the differential acceleration is less than or equal to the maximum acceleration / deceleration range, the target speed is taken as the planning end point speed; a speed curve is generated according to the planning start point speed, the planning end point speed and the planning period; and the planning end point speed is taken as the planning start point speed of the next trajectory planning segment;

[0021] In step 75, the current position of the sampler is obtained, and the speed curve is accumulated to the current position to obtain the planning trajectory of the first trajectory planning segment;

[0022] In step 76, step 72 is returned to perform planning of the next trajectory planning segment.

[0023] In one embodiment, in step 72, the target position and the target speed of the current trajectory planning segment are determined according to the differential speed of the median, the position information of the median and the speed stability value obtained at the planning start time, including:

[0024] The position information of the median is taken as the target position of the current trajectory planning segment;

[0025] The distance between the position information of the median and the adjacent hardware limit position is calculated;

[0026] The planning movement direction is determined according to the position information of the median, the adjacent hardware limit position and the differential speed of the median;

[0027] The initial target speed is determined according to the distance and the planning movement direction;

[0028] The position accuracy difference between the target position of the previous trajectory planning segment and the end point of the planning trajectory of the previous trajectory planning segment is calculated; if the current trajectory planning segment is the first trajectory planning segment, the position accuracy difference is 0;

[0029] The weight value is determined according to the speed stability value;

[0030] The speed difference value is determined according to the position accuracy difference and the weight value;

[0031] The initial target speed and the speed difference value are added to obtain the target speed of the current trajectory planning segment.

[0032] In one embodiment, the planning movement direction is determined according to the position information of the median, the adjacent hardware limit position and the differential speed of the median, and the following formula is used:

[0033] Direction=sign((pos_limit-point rank ) / v rank )

[0034] Wherein, Direction is the planning movement direction, sign is a function of symbol, pos_limit is the adjacent hardware limit position, and pointrank Position information of the median, v rank Differential speed of the median.

[0035] In one embodiment, the initial target speed is determined according to the size of the distance and the planned motion direction, using the following formula:

[0036] If Direction = 1, then:

[0037]

[0038] If Direction = -1, then:

[0039] target_v0 = v rank

[0040] where Direction is the planned motion direction, target_v0 is the initial target speed, Error_limit is the distance, v rank Differential speed of the median.

[0041] In one embodiment, the weight value is determined according to the speed stability value, including:

[0042] If the speed stability value Ts = 0, then the weight value K = 1.0; if the speed stability value Ts = 1, then the weight value K = 0.1.

[0043] In one embodiment, the speed difference value is determined according to the position accuracy difference value and the weight value, using the following formula:

[0044] target_v+ = K * (Error_pos / dt)

[0045] where target_v+ is the speed difference value, K is the weight value, Error_pos is the position accuracy difference value, and dt is the planning period.

[0046] In one embodiment, the position data of the sampler is timeout denoised according to the sampling interval, to obtain denoised position data, including:

[0047] Determine the median of all sampling intervals;

[0048] Determine the sampling interval range according to the median;

[0049] Remove the position information corresponding to the sampling interval that does not belong to the sampling interval range, to obtain the denoised position data.

[0050] In one embodiment, the speed stability value is determined according to the acceleration curve, including:

[0051] If the acceleration curve intersects with the line of 0 acceleration value, then the speed stable value is 1, otherwise, the speed stable value is 0.

[0052] In a second aspect, a planning system for realizing fast response and smooth following of a mechanical arm is provided, comprising a data acquisition module and a trajectory planning module.

[0053] The data acquisition module is configured to:

[0054] acquire position data of the sampler, the position data comprising position information at a plurality of time points and a sampling interval of position information between adjacent time points;

[0055] perform timeout denoising on the position data of the sampler according to the sampling interval to obtain denoised position data;

[0056] determine a size of a noise window according to the sampling interval;

[0057] determine a cutoff frequency according to the sampling interval, and perform low-pass filtering on the denoised position data based on the cutoff frequency to obtain filtered position data;

[0058] store the filtered position data in a FIFO queue with a fixed length, the length of the FIFO queue being the size of the noise window;

[0059] determine a differential speed at each time point based on the position data stored in the FIFO queue, and determine a median differential speed and median position information based on the differential speed at each time point; determine an acceleration at each time point based on the differential speed at each time point, the accelerations at all time points forming an acceleration curve, and determine a speed stable value based on the acceleration curve;

[0060] The trajectory planning module is configured to:

[0061] determine a target position and a target speed of each trajectory planning segment based on the median differential speed, the median position information and the speed stable value; and plan each trajectory planning segment based on the target position and the target speed to obtain a planning trajectory of each trajectory planning segment.

[0062] Compared with the prior art, the application has the following beneficial effects:

[0063] 1. Timeout denoising is performed based on the sampling interval, which solves the problem that the prior art directly filters the sampling data, which has poor effect, easily introduces large delay, and is difficult to handle noise caused by time jitter.

[0064] 2. A combination of high-frequency sampling and low-frequency control is adopted, a double-end FIFO queue mechanism with fast update is used to connect high-frequency sampling and low-frequency control, and a median filter based on speed differential is introduced to improve system stability, thereby ensuring the high-speed performance of the collector to the greatest extent.

[0065] 3. The maximum acceleration (deceleration) in trajectory planning is limited, the overall speed curve tends to be smooth and there is no risk of sudden change in speed. Compared with directly using a filter, this method is less likely to distort the original speed curve, and can directly process data through the maximum acceleration parameter of the hardware device without repeatedly adjusting the filter parameter.

[0066] 4. When the sampling device moves, if the sampling point position exceeds the limit, the mechanical arm will plan to decelerate in advance, and when the sampling point position returns to within the limit, it will quickly recover to the maximum planning speed. From the planning layer, the safety of the device is ensured, and the smoothness of the following is improved.

[0067] 5. On the basis of not affecting the speed smoothness, the position accuracy is dynamically adjusted, and the method of calculating the speed difference for compensation can dynamically switch position and speed control, greatly improving the robustness of the sampling point position. Even if the sampling device enters a large amount of non-continuous speed data, it will not cause mechanical arm control shock, and when the speed is continuous, the position accuracy can be quickly compensated. BRIEF DESCRIPTION OF DRAWINGS

[0068] The present application can be better understood by reference to the following description taken in conjunction with the accompanying drawings, which together with the detailed description below, form a part of the specification. In the drawings:

[0069] Figure 1 A flow chart of a planning method for realizing fast response and smooth following of a mechanical arm is shown;

[0070] Figure 2 A FIFO queue data storage diagram is shown;

[0071] Figure 3 A comparison diagram of median filtering is shown, where (a) is the position curve before median filtering, (b) is the position curve after median filtering, (c) is the speed curve before median filtering, and (d) is the speed curve after median filtering;

[0072] Figure 4 An acceleration curve diagram is shown;

[0073] Figure 5 An original sampling speed waveform is shown;

[0074] Figure 6 A constrained sampling speed waveform is shown;

[0075] Figure 7 Hardware limit sampling curve is shown, wherein (a) is a position curve without limit, (a) is a position curve after limit, (c) is a speed curve without limit, and (d) is a speed curve after limit;

[0076] Figure 8 Position curve after optimization of limit logic is shown;

[0077] Figure 9 K gain processing curve is shown;

[0078] Figure 10 Structure block diagram of a planning system for realizing fast response and smooth following of a mechanical arm is shown;

[0079] Figure 11 Principle diagram of a planning system for realizing fast response and smooth following of a mechanical arm is shown. DETAILED DESCRIPTION

[0080] In the following, exemplary embodiments of the present application will be described with reference to the accompanying drawings. In the description, not all features of a practical embodiment are described for the sake of clarity and conciseness. It should be appreciated, however, that many embodiment-specific decisions can be made in the process of developing any such practical embodiment in order to achieve the specific goals of the developer, and these decisions can vary from embodiment to embodiment.

[0081] It should also be noted here that, in order to avoid obscuring the present application due to unnecessary details, only the device structures closely related to the scheme according to the present application are shown in the accompanying drawings, and other details not closely related to the present application are omitted.

[0082] It should be understood that the present application is not limited to the described embodiments by virtue of the following description with reference to the drawings. In this context, embodiments can be combined with each other, features can be replaced or borrowed between different embodiments, and one or more features can be omitted in an embodiment, if possible.

[0083] The embodiment of the present application provides a planning method for realizing fast response and smooth following of a mechanical arm, Figure 1 A flow block diagram of the planning method for realizing fast response and smooth following of a mechanical arm is shown, referring to Figure 1 The method mainly includes the following steps:

[0084] In step S1, position data of a sampler is acquired, the position data including position information at multiple time points and a sampling interval of position information between adjacent time points.

[0085] The sampling frequencies of different samplers are different, and the connection mode of the sampler and the controller causes great fluctuation of the sampling period. For example, the serial communication frequency can be stably controlled at 100 Hz, while the network communication frequency fluctuates between 10-30 Hz. The traditional method directly filters the sampling data, and eliminates noise by continuously adjusting the filter parameters. Most filters only perform well when the sampling frequency is relatively stable, and it takes a lot of time to adjust the filter parameters, which also easily introduces a large delay. Unlike traditional data sampling, the embodiment does not directly filter the acquired data, but adds a processing link of the sampling interval dt of the position information at adjacent time points.

[0086] Here, the sampler can be a mechanical arm or a mouse, and the position information can be coordinates in mm or angles in degrees (°). The sampling interval of the position information at adjacent time points refers to the difference between adjacent sampling times.

[0087] Step S2, timeout denoising the position data of the sampler according to the sampling interval, to obtain denoised position data.

[0088] Specifically, the median of all sampling intervals is determined; the sampling interval range is determined according to the median; the position information corresponding to the sampling interval outside the sampling interval range is removed to obtain the denoised position data.

[0089] For example, the continuous sampling interval dt is distributed as 11 ms, 12 ms, 15 ms, 18 ms, …, and the range of the sampling interval dt can be determined from the value range. The median of the sampling interval can be determined as 15 mm, and the sampling interval range can be determined as 10-20 ms using 15 mm±5 mm. If a sampling interval dt exceeds 10-20 ms, the position information corresponding to the sampling interval dt is considered as timeout noise and removed. This is equivalent to a simplified band-pass filter, which can determine the frequency of the reasonable signal by analyzing the distribution range of dt. Compared with the traditional band-pass filter, this method almost does not introduce delay, and does not need to do much parameter adjustment. For example, the traditional Butterworth filter needs to set three parameters: sampling frequency fs, cutoff frequency fc, and quality factor ksi, and if the sampling frequency of the sampling signal fluctuates greatly, the filtering effect is difficult to guarantee.

[0090] Step S3, determining the size of the noise window according to the sampling interval.

[0091] Here, the number of sampling periods between any two adjacent sampling intervals outside the sampling interval range is taken as the size of the noise window, for example, the size of the noise window is 2 sampling periods.

[0092] Step S4, determining the cut-off frequency according to the sampling interval, low-pass filtering the de-noised position data based on the cut-off frequency to obtain filtered position data.

[0093] Since the sampling interval range is determined, for example, 10-20 ms, the sampling frequency of the signal can be calculated according to the period of 10-20 ms, which is 50-100 Hz, i.e. not higher than 100 Hz, according to the Nyquist-Shannon sampling theorem, at this time a low-pass filter with a cut-off frequency not higher than 50 Hz can be used for low-pass filtering to process high-frequency noise.

[0094] Step S5, storing the filtered position data into a FIFO queue with a fixed length, and the length of the FIFO queue is the size of the noise window.

[0095] The size of the noise window as the length of the FIFO queue ensures that even if communication fluctuations occur, the effective data in the FIFO will not be affected.

[0096] Step S6, determining the differential speed at each time according to the position data stored in the FIFO queue, and determining the median differential speed and the median position information according to the differential speed at each time; determining the acceleration at each time according to the differential speed at each time, and the accelerations at all times constitute an acceleration curve, and determining the speed stability value according to the acceleration curve.

[0097] In the traditional control framework, sampling and control correspond one by one, when a faster sampler is used, the sampling module needs to be down-sampled in order to wait for the processing time of the control module, for example, when an industrial CAN bus is used, the sampler can achieve a sampling frequency of 1000 Hz, while the control frequency of the robot arm is often lower than 100 Hz, and finally the slower one of the two is used as the unified frequency. This approach cannot fully utilize the high-speed characteristics of the sampler, and the anti-interference performance of the system is also difficult to improve. In this embodiment, the data processing part is specified to follow the maximum sampling frequency of the collector, and the sampling sample capacity is increased as much as possible, so as to improve the data precision and filtering effect.

[0098] In this embodiment, a combination of high-frequency sampling and low-frequency control is used, a double-ended FIFO queue mechanism with fast updating is used to connect high-frequency sampling and low-frequency control, and a median filter based on speed differential is introduced to improve system stability, and the high-speed performance of the collector is maximized.

[0099] Figure 2 The FIFO queue data storage diagram is shown, referring to Figure 2, the filtered position data (including position information point and sampling interval dt) is stored in a fixed-length FIFO queue, the length of the FIFO queue is the size of the noise window. New data is pushed into the queue from the bottom of the queue, and old data is popped from the top at the same time. The frequency of push & pop depends on the frequency of the sampling module. High frequency of the sampling module means lower control delay and more accurate filtering effect, which is one of the key indicators to improve the stability of the speed in the trajectory planning module. Taking a sampling frequency of 1000hz and a control frequency of 100hz as an example, frequency division means that 1 set of effective data can be selected from 10 sets of sampling data for control, which can greatly reduce the influence of noise on control.

[0100] In this embodiment, the length of the FIFO is determined by the noise window size k of dt. Since the noise trigger frequency is obtained during the dt communication timeout analysis, it means that the queue always guarantees the existence of valid data. The FIFO queue is updated according to the sampling frequency, but the actual output frequency is the control frequency. Therefore, the information output to the trajectory planning module is not the pop value at the top of the queue, but the differential speed v is first calculated according to point and dt, and then v is filtered through a median filter to determine the output of the FIFO. The following formula is used:

[0101]

[0102] rank=median(v1,v2,…v k )

[0103] output=[v rank ,point rank ]

[0104] Where v t is the differential speed at time t, point t is the position information at time t, point t is the position information at time t-1, dt is the sampling interval between point t and point t-1 , k is the noise window size, median is the function of finding the median, rank is the index number of the median, v rank is the differential speed of the median, point rank is the position information of the median, and output is the output of the median filter.

[0105] Figure 3 The figure shows the comparison of median filtering, where (a) is the position curve before median filtering, (b) is the position curve after median filtering, (c) is the speed curve before median filtering, and (d) is the speed curve after median filtering.

[0106] Specifically, the acceleration at each time is determined according to the differential speed at the time, and the following formula is used:

[0107]

[0108] Wherein, a t is the acceleration at time t;

[0109] The accelerations at all times in the FIFO queue constitute an acceleration curve, Figure 4 The acceleration curve is shown in the schematic diagram, according to Figure 4 Whenever the acceleration curve passes through the zero point, that is, the acceleration curve and the line with an acceleration value of 0 have an intersection point, it means that the acceleration and deceleration switching occurs at this time, and the more times the zero point is passed, the more unstable the speed of the trajectory is. When the acceleration curve has a part passing through the zero point, the speed stability value T s = 1, indicating that the position accuracy compensation at this time should not be too much. When the acceleration curve does not have a part passing through the zero point, it indicates that the speed of the trajectory is relatively stable, and the speed stability value T s = 0, indicating that sufficient position accuracy compensation is performed.

[0110] Step S7, determining the target position and target speed of each trajectory planning segment based on the differential speed of the median, the position information of the median, and the speed stability value; planning each trajectory planning segment according to the target position and target speed to obtain the planning trajectory of each trajectory planning segment.

[0111] In this embodiment, the position data obtained includes position information and sampling interval, and the timeout denoising is performed based on the sampling interval, solving the problem that the existing technology directly filters the sampling data, which has poor effect, easily introduces large delay, and is difficult to process noise caused by time jitter. In addition, a combination of high-frequency sampling and low-frequency control is adopted, a double-end FIFO queue mechanism with fast update is used to connect high-frequency sampling and low-frequency control, and a median filter based on speed differential is introduced to improve system stability and maximize the high-speed performance of the collector. In this embodiment, the speed stability value is obtained based on the data stored in the FIFO queue, which is used to determine the target speed of each trajectory planning segment in the trajectory planning process to eliminate position accuracy error.

[0112] In one embodiment, in step 7, the target position and target speed of each trajectory planning segment are determined based on the differential speed of the median, the position information of the median, and the speed stability value; each trajectory planning segment is planned according to the target position and target speed to obtain the planning trajectory of each trajectory planning segment, including:

[0113] Step 71, initialize the planning start point velocity Ve of the first trajectory planning segment as 0 for the first trajectory planning segment;

[0114] Step 72, according to the median differential velocity v rank , the position information point rank and the velocity stability value T s , determine the target position target_pos and the target velocity target_v of the current trajectory planning segment;

[0115] Step 73, according to the target velocity target_v and the planning period T, here the planning period T is a known value, calculate the differential acceleration Diff_v; use the following formula:

[0116] Diff_v=(target_v–Ve) / T

[0117] Step 74, if the differential acceleration Diff_v is greater than the maximum acceleration / deceleration range, here the maximum acceleration / deceleration range is a known value, then the maximum velocity Vm that can be reached within the planning period T is taken as the planning end point velocity; according to the planning start point velocity Ve, the planning end point velocity and the planning period T, generate a velocity curve V, the method of generating the velocity curve is prior art, for example, the cubic polynomial and quintic polynomial trajectory planning method can be used; and the planning end point velocity is taken as the planning start point velocity of the next trajectory planning segment;

[0118] If the differential acceleration Diff_v is less than or equal to the maximum acceleration / deceleration range, then the target velocity target_v is taken as the planning end point velocity; according to the planning start point velocity Ve, the planning end point velocity and the planning period T, generate a velocity curve V; and the planning end point velocity is taken as the planning start point velocity of the next trajectory planning segment;

[0119] Step 75, obtain the current position current_pos of the sampler, and accumulate the velocity curve V to the current position current_pos to obtain the planning trajectory of the first trajectory planning segment;

[0120] Here, since the velocity curve V is a differential velocity curve, it can be directly accumulated to the current position current_pos.

[0121] For example, the velocity values in the velocity curve V are: [V1=0, V2=0.1, V3=0.2, …, Ve=1.0], the current_pos is a constant, such as 10, and the trajectory is represented as:

[0122]

[0123] wherein V i is the i-th speed value in V, e is the number of speed values in V, traj k is the k-th trajectory point in traj.

[0124] Step 76, return to step 72 to plan the next trajectory planning segment.

[0125] Finally, the planning trajectory of each trajectory planning segment is obtained.

[0126] In this embodiment, since the maximum acceleration (deceleration) in trajectory planning is limited, the overall speed curve tends to be smooth and there is no risk of sudden speed change. Compared with directly using a filter, this method is less likely to cause distortion of the original speed curve, and can directly process data through the maximum acceleration parameter of the hardware device without the need to repeatedly adjust the filter parameter. Figure 5 The original sampling speed waveform is shown in FIG. 6, Figure 6 The sampling speed waveform after constraint is shown in FIG. 7.

[0127] In one embodiment, step 72, according to the differential speed of the median value, the position information of the median value and the speed stability value obtained at the planning start time, the target position and the target speed of the current trajectory planning segment are determined, including:

[0128] Step 721, the position information point rank of the median value is taken as the target position target_pos of the current trajectory planning segment.

[0129] Step 722, the distance Error_limit between the position information point rank of the median value and the adjacent hardware limit pos_limit is calculated, using the following formula:

[0130] Error_limit = abs(pos_limit-target_pos)

[0131] wherein abs is an absolute value function.

[0132] Here, when the position information is an angle, the adjacent hardware limit is also an angle, the units of the adjacent hardware limit and the position information are degrees (°), and the unit of the distance is also degrees (°).

[0133] When the position information is a coordinate, the adjacent hardware limit is also a coordinate, the units of the adjacent hardware limit and the position information are mm, and the unit of the distance is also mm.

[0134] Step 723, according to the position information point rank of the median value, the adjacent hardware limit pos_limit and the differential speed v rank, determine the planned motion direction; the following formula is used:

[0135] Direction=sign((pos_limit-point rank ) / v rank )

[0136] where Direction is the planned motion direction, sign is the function of the sign, pos_limit is the adjacent hardware limit, point rank is the position information of the median, v rank is the differential speed of the median. If Direction is 1, it indicates that the planning point is close to the limit at this time, and if Direction is -1, it indicates that the planning point is away from the limit.

[0137] Step 724, determine the initial target speed according to the size of the distance and the planned motion direction; the following formula is used:

[0138] If Direction=1, then:

[0139]

[0140] If Direction=-1, then:

[0141] target_v0=v rank

[0142] where Direction is the planned motion direction, target_v0 is the initial target speed, Error_limit is the distance, v rank is the differential speed of the median.

[0143] Specifically, the deceleration section is in the range of (2, 5], the low-speed section is in the range of (0.5, 2], and the static section is in the range of [0, 0.5].

[0144] According to the above planning steps, if the sampling point position exceeds the limit, the robot arm will decelerate in advance when following the sampler, and when the sampling point position returns to within the limit, it will quickly recover tracking at the maximum planning speed. From the planning layer, the safety of the equipment is guaranteed, and the smoothness of the following is improved. Figure 7 The hardware limit sampling curve is shown, where (a) is the position curve without limit, (a) is the position curve after limit, (c) is the speed curve without limit, and (d) is the speed curve after limit. Figure 8 The position curve after optimizing the limit logic is shown.

[0145] Step 725, calculate the position accuracy difference error_pos between the target position target_pos of the previous trajectory planning segment and the end point plan_pos of the planning trajectory of the previous trajectory planning segment; if the current trajectory planning segment is the first trajectory planning segment, the position accuracy difference is 0;

[0146] Step 726, determine the weight value according to the speed stability value; specifically, if the speed stability value Ts=0, indicating that the speed is stable in this segment, then the weight value K=1.0; if the speed stability value Ts=1, indicating that the speed fluctuates greatly in this segment, then the weight value K=0.1.

[0147] Step 727, determine the speed difference value according to the position accuracy difference and the weight value; the following formula is used:

[0148] target_v+=K*(Error_pos / T)

[0149] Where target_v+ is the speed difference value, K is the weight value, Error_pos is the position accuracy difference, and T is the planning period.

[0150] Step 728, add the initial target speed target_v0 and the speed difference value target_v+ to obtain the target speed target_v of the current trajectory planning segment.

[0151] The integral of the speed (i.e., the position) has a cumulative error, and the above planning steps can dynamically adjust the position accuracy without affecting the speed smoothing. The method of calculating the speed difference value for compensation can dynamically switch between position and speed control, greatly improving the robustness of the sampling point position. Even if the sampler enters a large amount of speed discontinuous data, it will not cause the robot arm control to oscillate, and when the speed is continuous, the position accuracy can be quickly compensated. Figure 9 The K gain processing curve is shown, according to Figure 9 , the red line in the figure is the original curve, and the blue line is the curve after switching the K gain. It can be seen that at time 0-100, the compensation gain is reduced due to speed jitter, and at time 150-300, the compensation gain is increased due to stable speed. The position accuracy after compensation is improved by more than 50% compared to no K dynamic gain.

[0152] The embodiment of the application also provides a planning system for realizing fast response and smooth following of a robot arm, Figure 10 The structure block diagram of the planning system for realizing fast response and smooth following of a robot arm is shown, Figure 11 The principle diagram of the planning system for realizing fast response and smooth following of a robot arm is shown, which includes a data acquisition module and a trajectory planning module.

[0153] The data acquisition module is used to:

[0154] acquire position data of the sampler, the position data comprising position information at a plurality of time instants and a sampling interval of position information at adjacent time instants;

[0155] perform timeout denoising on the position data of the sampler according to the sampling interval, to obtain denoised position data;

[0156] determine a size of a noise window according to the sampling interval;

[0157] determine a cutoff frequency according to the sampling interval, and perform low-pass filtering on the denoised position data based on the cutoff frequency, to obtain filtered position data;

[0158] store the filtered position data into a FIFO queue with a fixed length, the length of the FIFO queue being the size of the noise window;

[0159] determine a differential speed at each time instant according to the position data stored in the FIFO queue, and determine a median differential speed and a median position information according to the differential speed at each time instant; determine an acceleration at each time instant according to the differential speed at each time instant, the accelerations at all time instants constituting an acceleration curve, and determine a speed stable value according to the acceleration curve; specifically, a median filter can be used to achieve this;

[0160] the trajectory planning module is configured to:

[0161] determine a target position and a target speed of each trajectory planning segment based on the median differential speed, the median position information and the speed stable value; and plan each trajectory planning segment according to the target position and the target speed, to obtain a planned trajectory of each trajectory planning segment.

[0162] The planning system for enabling the mechanical arm to quickly respond and smoothly follow of the embodiment achieves the same inventive concept as the planning method for enabling the mechanical arm to quickly respond and smoothly follow described above, and thus the specific implementation of the device can be seen in the embodiment part of the planning method for enabling the mechanical arm to quickly respond and smoothly follow described above, and the technical effects thereof correspond to those of the method, which will not be repeated here.

[0163] The above merely describes various embodiments of the present application, but the protection scope of the present application is not limited thereto, and any person skilled in the art can easily think of changes or replacements within the technical scope disclosed in the present application, which should be encompassed in the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A planning method for achieving rapid response and smooth following of a robotic arm, characterized in that, include: Acquire the location data of the sampler, the location data including location information at multiple times and the sampling interval of the location information at adjacent times; The position data of the sampler is timed out and denoised according to the sampling interval to obtain the denoised position data. The size of the noise window is determined based on the sampling interval; The cutoff frequency is determined according to the sampling interval, and the denoised position data is low-pass filtered based on the cutoff frequency to obtain filtered position data. The filtered position data is stored in a fixed-length FIFO queue, the length of which is the size of the noise window; The differential velocity at each moment is determined based on the position data stored in the FIFO queue, and the median differential velocity and median position information are determined based on the differential velocity at each moment; the acceleration at each moment is determined based on the differential velocity at each moment, and the acceleration at all moments constitutes an acceleration curve, and the velocity stability value is determined based on the acceleration curve; The target position and target velocity of each trajectory planning segment are determined based on the differential velocity of the median, the position information of the median, and the velocity stability value. Based on the target position and the target velocity, each trajectory planning segment is planned to obtain the planned trajectory for each trajectory planning segment, including: Step 71: For the first trajectory planning segment, initialize the planning start point velocity of the first trajectory planning segment to 0; Step 72: Based on the median differential velocity, median position information, and velocity stability value obtained at the start of the planning, determine the target position and target velocity of the current trajectory planning segment; Step 73: Calculate the differential acceleration based on the target velocity and the planning period; Step 74: If the differential acceleration is greater than the maximum acceleration / deceleration range, then the maximum speed that can be reached within the planning period is taken as the planning endpoint speed; a speed curve is generated based on the planning starting point speed, the planning endpoint speed, and the planning period; and the planning endpoint speed is taken as the planning starting point speed of the next trajectory planning segment. If the differential acceleration is less than or equal to the maximum acceleration / deceleration range, the target velocity is taken as the planning endpoint velocity; a velocity curve is generated based on the planning starting point velocity, the planning endpoint velocity, and the planning period; and the planning endpoint velocity is taken as the planning starting point velocity for the next trajectory planning segment. Step 75: Obtain the current position of the sampler and add the velocity curve to the current position to obtain the planned trajectory of the first trajectory planning segment; Step 76, return to step 72, and proceed with the planning of the next trajectory segment; Step 72, based on the median differential velocity, median position information, and velocity stability value obtained at the start of planning, determines the target position and target velocity of the current trajectory planning segment, including: Use the location information of the median as the target location of the current trajectory planning segment; Calculate the location information of the median and the distance between adjacent hardware limits; The planned motion direction is determined based on the position information of the median, the adjacent hardware limits, and the differential velocity of the median. Based on the distance and the planned direction of motion, determine the initial target speed; Calculate the positional accuracy difference between the target position of the previous trajectory planning segment and the endpoint of the planned trajectory of the previous trajectory planning segment; if the current trajectory planning segment is the first trajectory planning segment, the positional accuracy difference is 0. The weights are determined based on the velocity stability values. The velocity difference is determined based on the position accuracy difference and the weight. The initial target speed is added to the speed difference to obtain the target speed of the current trajectory planning segment.

2. The method as described in claim 1, characterized in that, in, Based on the location information of the median, the adjacent hardware limits, and the differential velocity of the median, the planned motion direction is determined using the following formula: in, For the planned direction of movement, To find the sign of the function, Limiting adjacent hardware, The location information is the median. The difference velocity is the median.

3. The method as described in claim 1, characterized in that, in, Based on the distance and the planned direction of motion, the initial target velocity is determined using the following formula: ,but: ,but: in, For the planned direction of movement, For the initial target velocity, For distance, The difference velocity is the median.

4. The method as described in claim 1, characterized in that, in, Determining the weights based on the velocity stability value includes: If the speed is stable Then the weight If the speed is stable Then the weight .

5. The method as described in claim 1, characterized in that, in, The velocity difference is determined based on the position accuracy difference and the weight, using the following formula: in, The speed difference For weights, This is the difference in positional accuracy. For the planning cycle.

6. The method as described in claim 1, characterized in that, in, The position data of the sampler is timed out and denoised according to the sampling interval to obtain denoised position data, including: Determine the median of all sampling intervals; The sampling interval range is determined based on the median value; Remove the location information corresponding to the sampling intervals that do not belong to the sampling interval range to obtain the denoised location data.

7. The method as described in claim 1, characterized in that, in, Determining the stable velocity value based on the acceleration curve includes: If the acceleration curve intersects with the line where the acceleration value is 0, then the stable velocity value is 1; otherwise, the stable velocity value is 0.

8. A planning system for achieving rapid response and smooth following of a robotic arm, characterized in that, include: Data acquisition module and trajectory planning module; The data acquisition module is used for: Acquire the location data of the sampler, the location data including location information at multiple times and the sampling interval of the location information at adjacent times; The position data of the sampler is timed out and denoised according to the sampling interval to obtain the denoised position data. The size of the noise window is determined based on the sampling interval: The cutoff frequency is determined according to the sampling interval, and the denoised position data is low-pass filtered based on the cutoff frequency to obtain filtered position data. The filtered position data is stored in a fixed-length FIFO queue, the length of which is the size of the noise window; The differential velocity at each moment is determined based on the position data stored in the FIFO queue, and the median differential velocity and median position information are determined based on the differential velocity at each moment; the acceleration at each moment is determined based on the differential velocity at each moment, and the acceleration at all moments constitutes an acceleration curve, and the velocity stability value is determined based on the acceleration curve; The trajectory planning module is used for: Based on the differential velocity of the median, the position information of the median, and the velocity stability value, the target position and target velocity of each trajectory planning segment are determined; each trajectory planning segment is planned according to the target position and the target velocity to obtain the planned trajectory of each trajectory planning segment; The trajectory planning module is also used for: Step 71: For the first trajectory planning segment, initialize the planning start point velocity of the first trajectory planning segment to 0; Step 72: Based on the median differential velocity, median position information, and velocity stability value obtained at the start of the planning, determine the target position and target velocity of the current trajectory planning segment; Step 73: Calculate the differential acceleration based on the target velocity and the planning period; Step 74: If the differential acceleration is greater than the maximum acceleration / deceleration range, then the maximum speed that can be reached within the planning period is taken as the planning endpoint speed. A speed curve is generated based on the planned starting point speed, the planned ending point speed, and the planning period; The planned endpoint speed is then used as the planned starting point speed for the next trajectory planning segment; If the differential acceleration is less than or equal to the maximum acceleration / deceleration range, then the target velocity is taken as the planned endpoint velocity; A speed curve is generated based on the planned starting point speed, the planned ending point speed, and the planning period; The planned endpoint speed is then used as the planned starting point speed for the next trajectory planning segment; Step 75: Obtain the current position of the sampler and add the velocity curve to the current position to obtain the planned trajectory of the first trajectory planning segment; Step 76, return to step 72, and proceed with the planning of the next trajectory segment; Step 72, based on the median differential velocity, median position information, and velocity stability value obtained at the start of planning, determines the target position and target velocity of the current trajectory planning segment, including: Use the location information of the median as the target location of the current trajectory planning segment; Calculate the location information of the median and the distance between adjacent hardware limits; The planned motion direction is determined based on the position information of the median, the adjacent hardware limits, and the differential velocity of the median. Based on the distance and the planned direction of motion, determine the initial target speed; Calculate the positional accuracy difference between the target position of the previous trajectory planning segment and the endpoint of the planned trajectory of the previous trajectory planning segment; if the current trajectory planning segment is the first trajectory planning segment, the positional accuracy difference is 0. The weights are determined based on the velocity stability values. The velocity difference is determined based on the position accuracy difference and the weight. The initial target speed is added to the speed difference to obtain the target speed of the current trajectory planning segment.

Images