Planning method for realizing quick response smooth following of mechanical arm in cooperation with sampler

Abstract

The invention relates to a planning method for achieving quick response smooth following of a mechanical arm in cooperation with a sampler, and the planning method comprises the steps that position data of the sampler are obtained, and the position data comprise position information at multiple moments and sampling intervals of the position information at adjacent moments; performing overtime denoising on the position data of the sampler according to the sampling interval to obtain denoised position data; storing the filtered position data into an FIFO (First In First Out) queue with a fixed length; determining the differential velocity of the median, the position information of the median and a velocity stable value; determining a target position and a target speed of each trajectory planning section based on the differential speed of the median, the position information of the median and the speed stable value; and planning each trajectory planning section according to the target position and the target speed to obtain a planning trajectory of each trajectory planning section. According to the method, the response speed of the mechanical arm is high, the response speed is smooth, and linkage control of rapid and smooth following can be achieved in cooperation with a sampler.

Description

Technical Field

[0001] This application relates to the field of robot motion control. Specifically, it relates to a planning method for realizing fast response and smooth following of a robotic arm in cooperation with a sampler. Background Art

[0002] In the fields of industrial automation, medical robots, and human-robot collaboration, the coordinated control of robotic arms and external samplers is increasingly widely used. Through the coordinated control of the sampler, the traditional teaching process can be greatly simplified in the field of industrial automation, and the distance limitation between doctors and patients can be broken through to save valuable treatment time in the medical field.

[0003] Traditional coordinated control planning methods generally adopt position control, perform speed planning for the displacement distance between the starting point and the ending point, and introduce speed limits when generating the planning curve. Common methods include trapezoidal planning and spline planning. Traditional planning methods face the following problems when dealing with continuous points:

[0004] When collecting continuous points through the sampler, it is easy to collect data whose speed and acceleration far exceed the hardware limitations of the robotic arm. When the user manipulates the sampler to collect position information, the acceleration at the moment from stationary to moving can reach dozens of times the maximum acceleration value of the robotic arm, and similarly, the deceleration from moving to stationary also far exceeds the hardware performance of the robotic arm. This results in that if the planning is strictly carried out according to the sampled point positions, the following effects are manifested as motion jitters accompanied by sudden stops and sudden starts, and response delays caused by insufficient acceleration and deceleration, which obviously do not meet the application requirements of robotic arm following.

[0005] In coordinated control, there are usually high requirements for the response delay of coordinated control and the speed smoothness of the robotic arm. For example, too high a response delay on an industrial assembly line will result in an inability to match the speed of the assembly line, and the jitter of the robotic arm in the medical field will affect the precision during surgery. In addition, the coordinated control method will also have a greater impact on the control effect. More common ones include time jitters caused by remote communication and invalid point collection caused by the sampler breaking through the mechanical hardware limitations. Summary of the Invention

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

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

[0008] Obtain the position data of the sampler, where the position data includes the position information at multiple moments and the sampling interval of the position information at adjacent moments;

[0009] Timeout denoising is performed on the position data of the sampler according to the sampling interval to obtain the denoised position data;

[0010] Determine the size of the noise window according to the sampling interval;

[0011] Determine the cut-off frequency according to the sampling interval, and perform low-pass filtering on the denoised position data based on the cut-off frequency to obtain the filtered position data;

[0012] Store 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;

[0013] Determine the differential velocity at each moment according to the position data stored in the FIFO queue, and determine the median differential velocity and median position information according to the differential velocity at each moment; Determine the acceleration at each moment according to the differential velocity at each moment, and the accelerations at all moments form an acceleration curve, and determine the velocity stability value according to the acceleration curve;

[0014] Determine the target position and target velocity of each trajectory planning segment based on the median differential velocity, median position information and velocity stability value; Plan each trajectory planning segment according to the target position and target velocity to obtain the planned trajectory of each trajectory planning segment.

[0015] In one embodiment, determine the target position and target velocity of each trajectory planning segment based on the median differential velocity, median position information and velocity stability value; Plan each trajectory planning segment according to the target position and target velocity to obtain the planned trajectory of each trajectory planning segment, including:

[0016] Step 71, for the first trajectory planning segment, initialize the planned starting point velocity of the first trajectory planning segment to 0;

[0017] Step 72, determine the target position and target velocity of the current trajectory planning segment according to the median differential velocity, median position information and velocity stability value obtained at the start time of the plan;

[0018] Step 73, calculate the differential acceleration according to the target velocity and the planning period;

[0019] Step 74, if the differential acceleration is greater than the maximum acceleration / deceleration range, use the maximum velocity that can be reached within the planning period as the planned end point velocity; Generate a velocity curve according to the planned starting point velocity, planned end point velocity and planning period; And use the planned end point velocity as the planned starting point velocity 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 planned end speed; a speed curve is generated according to the planned start speed, the planned end speed, and the planning period; and the planned end speed is taken as the planned start speed of the next trajectory planning segment.

[0021] Step 75: Obtain the current position of the sampler, and accumulate the speed curve to the current position to obtain the planned trajectory of the first trajectory planning segment.

[0022] Step 76: Return to Step 72 to plan the next trajectory planning segment.

[0023] In one embodiment, in Step 72, determining the target position and target speed of the current trajectory planning segment 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 includes:

[0024] Taking the position information of the median value as the target position of the current trajectory planning segment;

[0025] Calculating the distance between the position information of the median value and the adjacent hardware limit;

[0026] Determining the planned motion direction according to the position information of the median value, the adjacent hardware limit, and the differential speed of the median value;

[0027] Determining the initial target speed according to the size of the distance and the planned motion direction;

[0028] Calculating the position accuracy difference between the target position of the previous trajectory planning segment and the end point of the planned 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;

[0029] Determining the weight according to the speed stability value;

[0030] Determining the speed difference according to the position accuracy difference and the weight;

[0031] Adding the initial target speed and the speed difference to obtain the target speed of the current trajectory planning segment.

[0032] In one embodiment, determining the planned motion direction according to the position information of the median value, the adjacent hardware limit, and the differential speed of the median value, using the following formula:

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

[0034] where Direction is the planned motion direction, sign is the function to obtain the sign, pos_limit is the adjacent hardware limit, and pointrank The position information with the median value, v rank The differential speed with the median value.

[0035] In one embodiment, according to the size of the distance and the planned movement direction, the initial target speed is determined 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 movement direction, target_v0 is the initial target speed, Error_limit is the distance, and v rank is the differential speed with the median value.

[0041] In one embodiment, determining the weight according to the speed stability value includes:

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

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

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

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

[0046] In one embodiment, performing timeout denoising on the position data of the sampler according to the sampling interval to obtain the denoised position data includes:

[0047] Determining the median value of all sampling intervals;

[0048] Determining the sampling interval range according to the median value;

[0049] Removing the position information corresponding to the sampling intervals that do not belong to the sampling interval range to obtain the denoised position data.

[0050] In one embodiment, determining the speed stability value according to the acceleration curve includes:

[0051] If the acceleration curve intersects with the line where the acceleration value is 0, then the stable velocity value = 1; otherwise, the stable velocity value = 0.

[0052] In a second aspect, a planning system for achieving fast response and smooth following of a robotic arm is provided, including: a data acquisition module and a trajectory planning module;

[0053] The data acquisition module is used for:

[0054] Obtaining the position data of the sampler, where the position data includes the position information at multiple moments and the sampling interval of the position information at adjacent moments;

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

[0056] Determining the size of the noise window according to the sampling interval:

[0057] Determining the cut-off frequency according to the sampling interval, and performing low-pass filtering on the denoised position data based on the cut-off frequency to obtain the filtered position data;

[0058] Storing the filtered position data into a FIFO queue with a fixed length, where the length of the FIFO queue is the size of the noise window;

[0059] Determining the differential velocity at each moment according to the position data stored in the FIFO queue, and determining the median differential velocity and the median position information according to the differential velocity at each moment; determining the acceleration at each moment according to the differential velocity at each moment, and the accelerations at all moments form an acceleration curve, and determining the stable velocity value according to the acceleration curve;

[0060] The trajectory planning module is used for:

[0061] Determining the target position and target velocity of each trajectory planning segment based on the median differential velocity, the median position information and the stable velocity value; planning each trajectory planning segment according to the target position and target velocity to obtain the planned trajectory of each trajectory planning segment.

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

[0063] 1. Performing timeout denoising based on the sampling interval, which solves the problems in the prior art that directly filtering the sampling data has poor effect, is easy to introduce large delay, and is difficult to handle the noise caused by time jitter.

[0064] 2. Adopt a combination method of high-frequency sampling + low-frequency control, use a fast-updating dual-ended FIFO queue mechanism to connect high-frequency sampling and low-frequency control, and introduce median filtering based on velocity differentiation to improve system stability, ensuring the high-speed performance of the collector to the greatest extent.

[0065] 3. Limit the maximum acceleration (deceleration) in trajectory planning, making the overall speed curve tend to be smooth and without the risk of speed mutation. Compared with directly using a filter, this method is not likely to cause distortion of the original speed curve, and can directly process data through the maximum acceleration parameter of the hardware device without repeatedly adjusting the filter parameters.

[0066] 4. When the robotic arm follows the sampler in motion, if the sampling point exceeds the limit, deceleration planning will be carried out in advance. When the sampling point returns within the limit, it will quickly resume tracking at the maximum planned speed, ensuring the safety of the device at the planning level and improving the smoothness of following.

[0067] 5. Dynamically adjust the position accuracy without affecting speed smoothness. By calculating the speed difference for compensation, the position and speed control can be dynamically switched, greatly improving the robustness to the sampling point. Even if the sampler inputs a large amount of data with discontinuous speeds, it will not cause control oscillation of the robotic arm, and when the speed is continuous, the position accuracy can be quickly compensated. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] This application can be better understood by referring to the description given below in conjunction with the accompanying drawings. The drawings, together with the following detailed description, are included in this specification and form a part of this specification. In the drawings:

[0069] Figure 1 Shows a flowchart of a planning method for realizing fast response and smooth following of a robotic arm;

[0070] Figure 2 Shows a schematic diagram of FIFO queue data storage;

[0071] Figure 3 Shows a comparison diagram 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;

[0072] Figure 4 Shows a schematic diagram of the acceleration curve;

[0073] Figure 5 Shows the original sampling speed waveform;

[0074] Figure 6 Shows the sampled speed waveform after constraint;

[0075] Figure 7 Shows the hardware limit sampling curve, where (a) is the position curve without limit, (b) is the position curve after limit, (c) is the speed curve without limit, and (d) is the speed curve after limit;

[0076] Figure 8 Shows the position curve after optimizing the limit logic;

[0077] Figure 9 Shows the K gain processing curve;

[0078] Figure 10 Shows the structural block diagram of the planning system for achieving fast response and smooth following of the robotic arm;

[0079] Figure 11 Shows the schematic diagram of the planning system for achieving fast response and smooth following of the robotic arm. Detailed implementation manners

[0080] In the following, exemplary embodiments of the present application will be described with reference to the accompanying drawings. For clarity and conciseness, not all features of the actual embodiments are described in the specification. However, it should be understood that many embodiment-specific decisions may be made during the development of any such actual embodiment to achieve the specific goals of the developer, and these decisions may vary with different embodiments.

[0081] Here, it should also be noted that in order to avoid obscuring the present application with unnecessary details, only the device structures closely related to the solution of the present application are shown in the drawings, while other details less related to the present application are omitted.

[0082] It should be understood that the present application is not limited to the described embodiments due to the following description with reference to the drawings. In this document, where feasible, embodiments may be combined with each other, features may be replaced or borrowed between different embodiments, and one or more features may be omitted in one embodiment.

[0083] An embodiment of the present application provides a planning method for achieving fast response and smooth following of a robotic arm, Figure 1 Shows the flow block diagram of the planning method for achieving fast response and smooth following of the robotic arm, see Figure 1 , and the method mainly includes the following steps:

[0084] Step S1, obtain the position data of the sampler, where the position data includes the position information at multiple moments and the sampling interval of the position information at adjacent moments.

[0085] There are differences in the sampling frequencies of different samplers, and the connection method between the sampler and the controller can lead to significant fluctuations in the sampling period. For example, the serial communication frequency can be stably controlled at 100 hz, while the network communication frequency fluctuates between 10 and 30 hz. Traditional methods directly filter the sampled data and eliminate noise by continuously adjusting the filter parameters. Most filters perform well only when the sampling frequency is relatively stable. It takes a long time to adjust the filter parameters and is also prone to introducing large delays. Different from traditional data sampling, in this embodiment, the obtained data is not directly filtered, but a processing step of the sampling interval dt of the position information at adjacent moments is added.

[0086] Here, the sampler can be a robotic arm or a mouse, etc. The position information can be coordinates with the unit of mm or angles with the unit of degrees (°). The sampling interval of the position information at adjacent moments refers to the difference between adjacent sampling times.

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

[0088] Specifically, determine the median of all sampling intervals; determine the sampling interval range according to the median; remove the position information corresponding to the sampling intervals that do not belong to the sampling interval range to obtain the denoised position data.

[0089] For example, the continuous sampling intervals dt are distributed as 11 ms, 12 ms, 15 ms, 18 ms… From this numerical change range, the range of the sampling interval dt can be specified. It can be determined that the median of the sampling interval is 15 mm. Using 15 mm ± 5 mm, the sampling interval range can be determined to be 10 to 20 ms. If a certain sampling interval dt exceeds 10 to 20 ms, the position information corresponding to the sampling interval dt is regarded as timeout noise and removed. Here, it is equivalent to a simplified band-pass filter. The frequency of the reasonable signal can be specified by analyzing the distribution range of dt. Compared with the traditional band-pass filter, this method hardly introduces delay and does not require too much parameter adjustment work. Taking the traditional Butterworth filter as an example, three parameters, namely the sampling frequency fs, the cut-off frequency fc, and the quality factor ksi, need to be set. And if the sampling frequency of the sampled signal fluctuates greatly, it is difficult to guarantee the filtering effect.

[0090] Step S3: Determine the size of the noise window according to the sampling interval.

[0091] Here, the number of sampling periods between any two adjacent sampling intervals that exceed the sampling interval range is used as the size of the noise window. For example, the size of the noise window is 2 sampling periods.

[0092] Step S4: Determine the cut-off frequency according to the sampling interval, and perform low-pass filtering on the denoised position data based on the cut-off frequency to obtain the 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 as 50 - 100 Hz according to the period of 10 - 20 ms, that is, the highest does not exceed 100 Hz. According to the Nyquist-Shannon sampling theorem, a low-pass filter with a cut-off frequency not higher than 50 Hz can be used for low-pass filtering at this time to process high-frequency noise.

[0094] Step S5: Store 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 is used as the length of the FIFO queue to ensure that even if communication fluctuations occur, it will not affect the valid data in the FIFO.

[0096] Step S6: Determine the differential speed at each moment according to the position data stored in the FIFO queue, and determine the median differential speed and the median position information according to the differential speed at each moment; determine the acceleration at each moment according to the differential speed at each moment, and the accelerations at all moments form an acceleration curve, and determine the speed stability value according to the acceleration curve.

[0097] In the traditional control framework, sampling and control correspond one by one. When using a sampler with a fast sampling frequency, in order to wait for the processing time of the control module, the sampling module needs to be downsampled. For example, when using industrial CAN bus communication, the sampler can achieve a sampling frequency of 1000 Hz, while the control frequency of the robotic arm is often lower than 100 Hz. Eventually, the slower one of the two will be used as the unified frequency. This approach cannot fully utilize the high-speed characteristics of the sampler, and at the same time, it is difficult to improve the anti-interference ability of the system. In this embodiment, it is stipulated that the data processing part is based on the maximum acquisition frequency of the collector, and the sampling sample capacity is increased as much as possible, so as to improve the data accuracy and the filtering effect.

[0098] In this embodiment, a combination method of high-frequency sampling + low-frequency control is adopted. A fast-updating dual-ended FIFO queue mechanism is used to connect high-frequency sampling and low-frequency control, and median filtering based on velocity differentiation is introduced to improve the system stability, which maximally guarantees the high-speed performance of the collector.

[0099] Figure 2 Shows a schematic diagram of FIFO queue data storage, see Figure 2, the filtered position data (including the position information point and the sampling interval dt) is stored in a FIFO queue with a fixed length, and the length of the FIFO queue is the size of the noise window. New data is pushed into the queue from the bottom, and old data is popped from the top synchronously. The frequency of this push & pop depends on the frequency of the sampling module. A high-frequency sampling module means lower control latency and more accurate filtering effect, which is also one of the key indicators to improve the speed stability in the trajectory planning module. Taking a sampling frequency of 1000 hz and a control frequency of 100 hz as an example, frequency division means that 1 set of valid data can be selected from 10 groups of sampling data for control, which can greatly reduce the impact of noise on control.

[0100] In this embodiment, it is stipulated that 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, this means that the queue always guarantees the existence of valid data. The FIFO queue is continuously 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 first the differential velocity v is obtained based on point and dt, and then v is filtered by the median filter to determine the output of the FIFO. Specifically, the following formula is used:

[0101]

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

[0103] output = [v rank , point rank

[0104] Among them, v t is the differential velocity 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 to find the median, rank is the index number corresponding to the median, v rank is the differential velocity of the median, point rank is the position information of the median, and output is the output of the median filter.

[0105] Figure 3 shows the comparison diagram of median filtering. Among them, (a) is the position curve before median filtering, (b) is the position curve after median filtering, (c) is the velocity curve before median filtering, and (d) is the velocity curve after median filtering.​

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

[0107]

[0108] where a t is the acceleration at time t;

[0109] The accelerations at all moments in the FIFO queue form an acceleration curve. Figure 4 Figure shows a schematic diagram of the acceleration curve. According to Figure 4 , whenever the acceleration curve passes through zero, that is, there is an intersection between the acceleration curve and the line with an acceleration value of 0, it means that the acceleration and deceleration are switched at this time. The more times it passes through zero, the more unstable the speed of this section of the trajectory is. When there is a part of the acceleration curve passing through zero, use the speed stability value T s = 1, indicating that too much position accuracy compensation is not appropriate at this time. When there is no part of the acceleration curve passing through zero, it indicates that the speed of this section of the trajectory is relatively stable, and use the speed stability value T s = 0, indicating that sufficient position accuracy compensation is performed.

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

[0111] In this embodiment, the obtained position data includes position information and sampling interval. Time-out denoising is performed based on the sampling interval, which solves the problems in the prior art of directly filtering the sampling data, resulting in poor effects, easy introduction of large delays, and difficulty in processing noise caused by time jitter. In addition, a combination method of high-frequency sampling + low-frequency control is adopted. The fast-updating double-ended FIFO queue mechanism is used to connect high-frequency sampling and low-frequency control, and median filtering based on velocity differentiation is introduced to improve system stability, ensuring the high-speed performance of the collector to the greatest extent. This embodiment also obtains the speed stability value based on the data stored in the FIFO queue, which is used to determine the target speed of each trajectory planning segment during the trajectory planning process to eliminate position accuracy errors.

[0112] In one embodiment, in step 7, determining the target position and target speed of each trajectory planning segment based on the differential velocity 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 planned trajectory of each trajectory planning segment includes:

[0113] Step 71, for the first trajectory planning segment, initialize the planning starting point speed Ve of the first trajectory planning segment to 0;

[0114] Step 72, based on the differential speed v of the median value obtained at the start of the planning rank , the position information point of the median value rank and the speed stability value T s , determine the target position target_pos and the target speed target_v of the current trajectory planning segment;

[0115] Step 73, according to the target speed target_v and the planning period T, where 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, where the maximum acceleration / deceleration range is a known value, then take the maximum speed Vm that can be reached within the planning period T as the planning end point speed; based on the planning starting point speed Ve, the planning end point speed, and the planning period T, generate a speed curve V, and the method for generating the speed curve is a prior art, for example, the cubic polynomial and quintic polynomial trajectory planning methods can be used; and take the planning end point speed as the planning starting point speed 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 take the target speed target_v as the planning end point speed; based on the planning starting point speed Ve, the planning end point speed, and the planning period T, generate a speed curve V; and take the planning end point speed as the planning starting point speed of the next trajectory planning segment;

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

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

[0121] For example, the speed values in the speed curve V are: [V1 = 0, V2 = 0.1, V3 = 0.2,..., Ve = 1.0], current_pos is a constant, such as 10, and traje represents a set of trajectories, which can be expressed as:

[0122]

[0123] Among them, V i is the i-th speed value in V, e is the number of speed values in V, and traje k is the k-th trajectory point in traje.

[0124] Step 76, return to Step 72 to perform the planning of the next trajectory planning segment.

[0125] Finally, the planned trajectories of each trajectory planning segment are obtained.

[0126] In this embodiment, since the maximum acceleration (deceleration) in the trajectory planning is restricted, the overall speed curve tends to be smooth and there is no risk of speed mutation. Compared with directly using a filter, this method is not likely to cause distortion of the original speed curve, and can directly perform data processing through the maximum acceleration parameter of the hardware device without the need to repeatedly adjust the filter parameters. Figure 5 shows the original sampled speed waveform, Figure 6 shows the sampled speed waveform after constraint.

[0127] In one embodiment, in 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, determine the target position and target speed of the current trajectory planning segment, including:

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

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

[0130] Error_limit = abs(pos_limit - target_pos)

[0131] where abs is the function of taking the absolute value.

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

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

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

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

[0136] where Direction is the planned motion direction, sign is the function to obtain the sign, pos_limit is the adjacent hardware limit, point rank is the position information of the median value, and v rank is the differential speed of the median value. If Direction is 1, it means the planned point is approaching the limit at this time; if Direction is -1, it means the planned point is moving away from the limit.

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

[0138] If Direction = 1, then:

[0139]

[0140] If Direction = -1, then:

[0141] target_v0 = v rank

[0142] [[ID=�3]]where Direction is the planned motion direction, target_v0 is the initial target speed, Error_limit is the distance, and v rank is the differential speed of the median value.

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

[0144] According to the above planning steps, when the robotic arm follows the sampler, if the sampling point exceeds the limit, deceleration planning will be carried out in advance, and when the sampling point returns within the limit, it will quickly resume tracking at the maximum planned speed, ensuring the safety of the equipment from the planning layer and improving the smoothness of following. Figure 7 Shows the hardware limit sampling curve, where (a) is the position curve without limit, (b) is the position curve after limit, (c) is the speed curve without limit, and (d) is the speed curve after limit. Figure 8 Shows the position curve after optimizing the limit logic.

[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 planned 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 according to the speed stability value; specifically, if the speed stability value Ts = 0, indicating that the speed is stable in this section, the weight K = 1.0; if the speed stability value Ts = 1, indicating that the speed fluctuates greatly in this section, the weight K = 0.1.

[0147] Step 727: Determine the speed difference according to the position accuracy difference and the weight; use the following formula:

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

[0149] where target_v+ is the speed difference, K is the weight, 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 target_v+ to obtain the target speed target_v of the current trajectory planning segment.

[0151] There is an accumulated error in the integration of speed (i.e., position). Through the above planning steps, the position accuracy can be dynamically adjusted without affecting the speed smoothness. By calculating the speed difference for compensation, the position and speed control can be dynamically switched, greatly improving the robustness to the sampling points. Even if the sampler enters a large amount of data with discontinuous speeds, it will not cause mechanical arm control oscillation, and when the speed is continuous, the position accuracy can be quickly compensated. Figure 9 Shows the K gain processing curve. According to Figure 9 , the red line in the figure is the original curve, and the blue line is the curve for switching the K gain. It can be seen that the compensation gain is reduced due to speed jitter at the time 0 - 100, and the compensation gain is increased due to stable speed at the time 150 - 300. The position accuracy after compensation is improved by more than 50% compared to without K dynamic gain.

[0152] The embodiment of the present application also provides a planning system for realizing the fast response and smooth following of the robotic arm. Figure 10 Shows the structural block diagram of the planning system for realizing the fast response and smooth following of the robotic arm. Figure 11 Shows the schematic diagram of the planning system for realizing the fast response and smooth following of the robotic arm. The system includes: a data acquisition module and a trajectory planning module.

[0153] The data acquisition module is used for:

[0154] Obtain the position data of the sampler, where the position data includes the position information at multiple moments and the sampling interval of the position information at adjacent moments;

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

[0156] Determine the size of the noise window according to the sampling interval:

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

[0158] Store 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;

[0159] Determine the differential velocity at each moment according to the position data stored in the FIFO queue, and determine the median differential velocity and the median position information according to the differential velocity at each moment; determine the acceleration at each moment according to the differential velocity at each moment, and the accelerations at all moments form an acceleration curve, and determine the velocity stability value according to the acceleration curve; specifically, it can be implemented by a median filter;

[0160] The trajectory planning module is used for:

[0161] Determine the target position and target velocity of each trajectory planning segment based on the median differential velocity, the median position information and the velocity stability value; plan each trajectory planning segment according to the target position and target velocity to obtain the planned trajectory of each trajectory planning segment.

[0162] The planning system for realizing the fast response and smooth following of the robotic arm in this embodiment has the same inventive concept as the above-mentioned planning method for realizing the fast response and smooth following of the robotic arm. Therefore, the specific implementation manner of this device can be seen in the embodiment part of the above-mentioned planning method for realizing the fast response and smooth following of the robotic arm, and its technical effect corresponds to that of the above method, which will not be elaborated here.

[0163] The above is only various implementation manners of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present application, and all of them should be covered within 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 fast response and smooth following of a robotic arm, characterized in that Including: Obtain the position data of the sampler, where the position data includes the position information at multiple moments and the sampling interval of the position information at adjacent moments; Perform timeout denoising on the position data of the sampler according to the sampling interval to obtain the denoised position data; Determine the size of the noise window according to the sampling interval; Determine the cut-off frequency according to the sampling interval, and perform low-pass filtering on the denoised position data based on the cut-off frequency to obtain the filtered position data; Store 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; Determine the differential velocity at each moment according to the position data stored in the FIFO queue, and determine the median differential velocity and the median position information according to the differential velocity at each moment; determine the acceleration at each moment according to the differential velocity at each moment, and the accelerations at all moments form an acceleration curve, and determine the velocity stability value according to the acceleration curve; Determine the target position and target velocity of each trajectory planning segment based on the median differential velocity, the median position information, and the velocity stability value; Perform planning on each trajectory planning segment according to the target position and the target velocity to obtain the planned trajectory of each trajectory planning segment.

2. The method according to claim 1, wherein Wherein, Determine the target position and target velocity of each trajectory planning segment based on the median differential velocity, the median position information, and the velocity stability value; Perform planning on each trajectory planning segment according to the target position and the target velocity to obtain the planned trajectory of each trajectory planning segment, including: Step 71, for the first trajectory planning segment, initialize the planned starting point velocity of the first trajectory planning segment to 0; Step 72, determine the target position and target velocity of the current trajectory planning segment according to the median differential velocity, the median position information, and the velocity stability value obtained at the planning start moment; Step 73, calculate the differential acceleration according to the target velocity and the planning period; Step 74, if the differential acceleration is greater than the maximum acceleration / deceleration range, use the maximum velocity that can be reached within the planning period as the planned end point velocity; generate a velocity curve according to the planned starting point velocity, the planned end point velocity, and the planning period; and use the planned end point velocity as the planned starting point velocity of the next trajectory planning segment; If the differential acceleration is less than or equal to the maximum acceleration / deceleration range, use the target velocity as the planned end point velocity; generate a velocity curve according to the planned starting point velocity, the planned end point velocity, and the planning period; and use the planned end point velocity as the planned starting point velocity of the next trajectory planning segment; Step 75, obtain the current position of the sampler, and accumulate the velocity curve to the current position to obtain the planned trajectory of the first trajectory planning segment; Step 76, return to step 72 to perform the planning of the next trajectory planning segment.

3. The method according to claim 1, wherein Step 72, determine the target position and target velocity of the current trajectory planning segment according to the median differential velocity, the median position information, and the velocity stability value obtained at the planning start moment, including: Use the position information of the median value as the target position of the current trajectory planning segment; Calculate the distance between the position information of the median value and the adjacent hardware limit; Determine the planned movement direction based on the position information of the median value, the adjacent hardware limit, and the differential speed of the median value; Determine the initial target speed according to the magnitude of the distance and the planned movement direction; Calculate the position accuracy difference between the target position of the previous trajectory planning segment and the end point of the planned 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; Determine the weight according to the speed stability value; Determine the speed difference according to the position accuracy difference and the weight; Add the initial target speed and the speed difference to obtain the target speed of the current trajectory planning segment.

4. The method according to claim 3, characterized in that, Wherein, To determine the planned movement direction based on the position information of the median value, the adjacent hardware limit, and the differential speed of the median value, the following formula is used: Direction=sign((pos_limit-point rank ) / v rank ) Among them, Direction is the planned motion direction, sign is the function for obtaining the sign, pos_limit is the adjacent hardware limit, and point rank is the position information of the median value, and v rank is the differential speed of the median value.

5. The method according to claim 3, characterized in that Wherein, To determine the initial target speed according to the magnitude of the distance and the planned movement direction, the following formula is used: If Direction = 1, then: If Direction = -1, then: target_v0 = v rank Among them, Direction is the planned motion direction, target_v0 is the initial target speed, Error_limit is the distance, and v rank is the differential speed with the median value.

6. The method according to claim 3, wherein Wherein, Determining the weight according to the speed stability value includes: If the speed stability value Ts = 0, the weight K = 1.0; if the speed stability value Ts = 1, the weight K = 0.

1.

7. The method according to claim 3, characterized in that, Wherein, To determine the speed difference according to the position accuracy difference and the weight, the following formula is used: target_v+ = K * (Error_pos / dt) Wherein, target_v+ is the speed difference, K is the weight, Error_pos is the position accuracy difference, and dt is the planning period.

8. The method according to claim 1, wherein Wherein, Performing timeout denoising on the position data of the sampler according to the sampling interval to obtain denoised position data, including: Determine the median value of all sampling intervals; Determine the sampling interval range according to the median value; Remove the position information corresponding to the sampling intervals that do not belong to the sampling interval range to obtain the denoised position data.

9. The method according to claim 1, characterized in that, Wherein, Determining the speed stability value according to the acceleration curve includes: If the acceleration curve has an intersection with the line where the acceleration value is 0, the speed stability value = 1, otherwise, the speed stability value = 0.

10. A planning system for achieving fast response and smooth following of a robotic arm, characterized in that, Including: A data acquisition module and a trajectory planning module; The data acquisition module is used for: Obtain the position data of the sampler, where the position data includes the position information at multiple moments and the sampling interval of the position information at adjacent moments; Perform timeout denoising on the position data of the sampler according to the sampling interval to obtain denoised position data; Determine the size of the noise window according to the sampling interval: Determine the cut-off frequency according to the sampling interval, and perform low-pass filtering on the denoised position data based on the cut-off frequency to obtain filtered position data; Store 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; Determine the differential velocity at each moment according to the position data stored in the FIFO queue, and determine the median differential velocity and the median position information according to the differential velocities at each moment; determine the acceleration at each moment according to the differential velocities at each moment, and the accelerations at all moments form an acceleration curve, and determine the velocity stability value according to the acceleration curve; The trajectory planning module is used for: Determine the target position and target velocity of each trajectory planning segment based on the median differential velocity, the median position information and the velocity stability value; plan each trajectory planning segment according to the target position and the target velocity to obtain the planned trajectory of each trajectory planning segment.

Images