Position trajectory velocity preprocessing method for multi-machine collaborative industrial robots

Abstract

The present invention aims to provide a method for preprocessing the position trajectory speed of multiple industrial robots in a collaborative manner. A multi-machine control system including multiple robot sub-threads is set on a PC. Each robot sub-thread is equipped with: a control instruction storage area, a decoding module, a trajectory data buffer, a collaborative data buffer, a speed preprocessing module, a speed information storage module, a motion control module, a coarse interpolation data buffer, and a data processing module. Each robot sub-thread has a corresponding hardware-layer programmable I / O interface card for connecting to a robot. Multi-machine collaborative work is achieved through collaborative instructions between robot sub-threads and between sub-threads and their respective programmable I / O interface cards.

Description

Technical Field

[0001] The present invention belongs to the technical field of industrial robot motion control, and in particular relates to a method for preprocessing the position trajectory speed of multiple industrial robots in collaboration. Background Art

[0002] With the support of new technologies such as computer technology and network technology, robotics technology is developing rapidly. Based on the PC platform, the "PC+" control mode is used to develop related robot control systems with great advantages. In order to make rational use of computer resources, corresponding multi-robot control systems can be developed based on the "PC + programmable I / O interface" control mode. To ensure that multiple robots can work together, it is necessary to ensure that the underlying control algorithm can reasonably coordinate motion trajectory control instructions and collaborative instructions. Summary of the Invention

[0003] Based on speed planning, this paper proposes a method for preprocessing the position trajectory speed of multi-robot collaborative industrial robots based on a "PC + programmable I / O interface." This method enables each robot in the multi-robot system to complete continuous operation of multiple trajectory segments at the maximum allowable speed while taking into account both motion trajectory control instructions and multi-robot collaborative instructions.

[0004] Its main scheme includes a decoding module that extracts control instructions from the control instruction storage area and, depending on the instruction, stores the control instruction information in the trajectory data buffer or the collaborative data buffer. When velocity preprocessing begins, the velocity preprocessing module uses a reverse order method to extract the trajectory data of each trajectory segment from the trajectory data buffer and sets the velocity information corresponding to the trajectory segment. After setting, the velocity information of each trajectory segment is stored in the velocity information storage module. After completing the first velocity preprocessing using the reverse order method, the velocity preprocessing module extracts the velocity information of each trajectory segment from the velocity information storage module according to the speed information storage order, completes speed fine-tuning in sequence, and stores the velocity information in the velocity information storage module, thus achieving the second velocity information preprocessing. After preprocessing is completed, the motion control module extracts the velocity information of each trajectory segment from the velocity information storage module and, depending on the instruction type corresponding to each trajectory segment's velocity information, outputs the corresponding coarse interpolation data to the coarse interpolation data buffer. The data processing module sequentially extracts the coarse interpolation data of each trajectory segment from the coarse interpolation data buffer and combines it with the collaborative data of the collaborative instruction in the collaborative data buffer to complete multi-machine collaborative control.

[0005] It includes a decoding module that decodes motion instructions and collaborative instructions, and stores the instruction information in the form of a structure into the trajectory segment buffer, and initializes the speed information array vel_arr[N]. The speed preprocessing module reads instructions from the trajectory segment buffer in reverse, completes the first speed preprocessing, and stores the data into vel_arr[N]. Then, it reads the speed information of each trajectory segment in vel_arr[N] in the forward direction to complete speed fine-tuning, so as to achieve the continuous operation of multiple trajectory segments at the maximum allowable speed while ensuring that the acceleration and deceleration distance is sufficient and taking into account the collaborative instructions of multiple machines, adhering to the time optimization principle.

[0006] The technical solution specifically adopted by the present invention to solve the technical problem is:

[0007] A method for preprocessing the position trajectory speed of multiple industrial robots in collaboration, characterized by:

[0008] A multi-machine control system consisting of multiple robot sub-threads is set up on a PC. Each robot sub-thread is equipped with: a control instruction storage area, a decoding module, a trajectory data buffer, a collaborative data buffer, a velocity preprocessing module, a velocity information storage module, a motion control module, a coarse interpolation data buffer, and a data processing module. Each robot sub-thread has a corresponding hardware-layer programmable I / O interface card for connecting to a robot. Multi-machine collaborative work is achieved through collaborative instructions between robot sub-threads and between sub-threads and their respective programmable I / O interface cards:

[0009] When the robot sub-thread starts running, the decoding module extracts the control instruction from the control instruction storage area and stores the control instruction information into the trajectory data buffer or the collaborative data buffer according to the different instructions;

[0010] When starting speed preprocessing, in order to achieve continuous operation of multiple trajectory segments at the maximum allowable speed while taking into account the motion trajectory control instructions and the multi-machine coordination instructions, the speed preprocessing module uses a reverse order method, that is, the trajectory data of the trajectory segment is extracted from the trajectory data buffer in the reverse order of the trajectory data storage order, and pre-read forward in sequence. According to the different instruction types corresponding to each trajectory segment, the speed information corresponding to the trajectory segment is preliminarily set, and the instruction types corresponding to the trajectory segments before and after the trajectory segment are determined, and the speed information of the trajectory segment is set again according to the instruction type; after setting, the speed preprocessing module stores the speed information in the speed information storage module;

[0011] After the first speed preprocessing is completed using the reverse order method, the speed preprocessing module retrieves the speed information of each trajectory segment from the speed information storage module according to the speed information storage order, completes the speed fine-tuning in sequence, and stores the speed information into the speed information storage module to achieve the second speed information preprocessing;

[0012] After completing the preprocessing, the motion control module takes out the speed information of each trajectory segment from the speed information storage module according to the speed information storage order, and outputs the corresponding coarse interpolation data to the coarse interpolation data buffer according to the different instruction types corresponding to the speed information of each trajectory segment. The data processing module takes out the coarse interpolation data of each trajectory segment from the coarse interpolation data buffer in sequence according to the coarse interpolation data storage order, and completes the multi-machine collaborative control in combination with the collaborative data of the collaborative instruction in the collaborative data buffer.

[0013] In fact, under the condition of ensuring that the acceleration and deceleration distance is sufficient and taking into account the coordinated instructions of multiple machines, we adhere to the principle of time optimization and complete the continuous operation of multiple trajectory segments at the maximum allowable speed.

[0014] The multi-machine control system consists of multiple robot sub-threads at the PC software level. Each robot sub-thread includes the aforementioned modules and buffers. Each robot sub-thread also has a corresponding programmable I / O interface card at the hardware level. Each robot sub-thread can be considered a robot. Coordinated instructions are used between robot sub-threads and between sub-threads and their respective programmable I / O interface cards to achieve multi-machine collaborative operation.

[0015] Furthermore, after completing the preprocessing of the velocity information of each trajectory segment, the motion control module retrieves the velocity information of each trajectory segment from the velocity information storage module according to the order in which the velocity information is stored, and completes the motion planning in sequence. According to the different instruction types corresponding to the velocity information of each trajectory segment:

[0016] If it is a motion trajectory control instruction, motion planning is performed on it and the corresponding coarse interpolation data is output to the coarse interpolation data buffer. The coarse interpolation data includes: trajectory segment number, instruction type, and the number of pulses of each joint axis of the robot;

[0017] If it is a collaborative instruction, motion planning is not performed and the coarse interpolation data is directly output, including the trajectory segment number, instruction type, and the number of pulses of each joint axis of the robot, where the number of pulses of each joint axis is 0; the data processing module sequentially takes out the coarse interpolation data of each trajectory segment from the coarse interpolation data buffer in the order in which the coarse interpolation data is stored. If the instruction type corresponding to the coarse interpolation data is a motion trajectory control instruction, the number of pulses of each joint in the coarse interpolation data is sent to the programmable I / O interface card to control the movement of the robot. If the instruction type corresponding to the coarse interpolation data is a collaborative instruction, the collaborative data buffer is indexed and the collaborative instruction with the same collaborative instruction number as the trajectory segment number in the coarse interpolation data of the trajectory segment is taken out. According to the different instruction types:

[0018] If it is a SET instruction, the corresponding signal number is sent to each corresponding robot sub-thread.

[0019] If it is a WAIT instruction, wait for the arrival of the coordination signal of the corresponding robot sub-thread.

[0020] Furthermore, the control program of each robot in the multi-machine system consists of the start instruction NOP, motion trajectory control instructions, coordination instructions and end instruction END;

[0021] The motion trajectory control instructions include: MOVL and MOVC instructions; a single MOVL instruction represents a straight line segment, and each MOVL instruction contains the end point position vector data P of the straight line segment. E , Maximum speed allowed within the trajectory segment V m ; Each MOVC instruction contains the end point position vector data P of the arc trajectory E , Maximum speed allowed within the trajectory segment V m ,The two MOVC instructions represent a circular arc trajectory segment;

[0022] The collaborative instructions include SET and WAIT instructions; the SET instruction is used to send a collaborative signal, and each SET instruction contains the robot sub-thread number r_num that receives the collaborative signal and its corresponding signal number s_num; the WAIT instruction is used to wait for the collaborative signal, and each WAIT instruction contains the robot sub-thread number r_num that triggers the collaborative signal and its corresponding signal number s_num; when executing the SET and WAIT instructions, the robot is in a stopped state;

[0023] The trajectory data buffer contains the trajectory data variables of each trajectory segment, including: trajectory segment number, instruction type, initial position point vector data P Start , the middle position point vector data P Middle , end position point vector data P End , the maximum speed allowed in the trajectory segment is V Max The collaborative data buffer contains the collaborative data variables of each collaborative instruction, including: collaborative instruction number, robot sub-thread number R_num, signal number S_num; the coarse interpolation data buffer contains the coarse interpolation data variables of each trajectory segment, including: trajectory segment number, instruction type, and number of pulses of each joint axis of the robot;

[0024] The speed information storage module stores the speed information of each track segment. The speed information contains the following variables: track segment number, instruction type, initial position point vector data P Start , the middle position point vector data P Middle , end position point vector data P End , control point vector data of the initial 2nd order Besizer transition curve and the corresponding node parameter variable ts , initial transition arc length variable L s , the initial transition total arc length variable L z , the middle segment arc length variable L r , control point vector data of the terminal 2nd order Besizer transition curve and the corresponding node parameter variable t e , end transition arc length L e , and the initial velocity variable V s , initial velocity upper and lower bound variables V min ,V max , final velocity variable V e , vel_arr[i] represents the velocity information of segment i of the trajectory.

[0025] Furthermore, when decoding starts, the decoding module extracts the control instruction from the control instruction storage area and stores the control instruction information into the trajectory data buffer or the collaborative data buffer according to the different instructions:

[0026] If the instruction is a NOP instruction, the first position point vector data P of the robot sub-thread control program contained in the instruction is S Assign to the global temporary position vector P temp ;

[0027] If it is a MOVL instruction, set the trajectory data of the trajectory segment, including: setting the trajectory segment number, setting the instruction type to MOVL, and setting P temp Assign the initial position point vector data P in the trajectory data Start , the intermediate position point vector data P in the trajectory data Middle Assign a value of 0 and set the P contained in the MOVL instruction to E Assign to the end position point vector data P in the trajectory data End and temporary position vector P temp , the MOVL instruction contains V m Assign the maximum speed V allowed within the trajectory segment in the trajectory data Max , store the trajectory data of the trajectory segment into the trajectory data buffer;

[0028] If it is a MOVC instruction, you need to extract another instruction. The next instruction is also MOVC, because two MOVC instructions with three points can uniquely determine a circular arc segment. Set the trajectory data of the trajectory segment, including: setting the trajectory segment number, setting the instruction type to MOVC, and setting P temp Assign the initial position point vector data P in the trajectory data Start , change the P in the first MOVC E Assign to the middle position point vector data P in the trajectory data Middle, change the P in the second MOVC E Assign to the end position point vector data P in the trajectory data End and temporary position vector P temp , the V contained in the second MOVC instruction m Assign the maximum speed V allowed within the trajectory segment in the trajectory data Max , store the trajectory data of the trajectory segment into the trajectory data buffer;

[0029] If it is a SET command, set the trajectory data of the trajectory segment, including: setting the trajectory segment number, setting the command type to SET, and other P Start 、P Middle 、P End 、V Max All are assigned to 0, and the collaborative data of the collaborative instruction is set, the trajectory segment number of the trajectory segment is assigned to the collaborative instruction number in the collaborative data, the r_num contained in SET is assigned to the robot sub-thread number R_num in the collaborative data, and the s_num contained in SET is assigned to the signal number S_num in the collaborative data. The trajectory data and collaborative data are sent to the trajectory data buffer and the collaborative data buffer respectively; according to the above decoding method, the decoding of all control instructions is completed.

[0030] Furthermore, during the first speed preprocessing, the speed preprocessing module uses a reverse order method, that is, extracting the trajectory data of the trajectory segment from the trajectory data buffer in the reverse order of the trajectory data storage order, and pre-reading forward in sequence and setting the speed information;

[0031] The speed information setting method is as follows: assuming that the current segment is trajectory segment i, it is necessary to determine the instruction type corresponding to the trajectory segment i. If the instruction type is a collaborative instruction, the trajectory data contained in the trajectory segment i is assigned one-to-one to the trajectory segment number and instruction type contained in the velocity information vel_arr[i] of the trajectory segment i. All other variables in vel_arr[i] are assigned to 0. After the assignment is completed, the velocity preprocessing module stores the velocity information vel_arr[i] of the trajectory segment into the velocity information storage module.

[0032] If it is a motion trajectory control instruction, the velocity information vel_arr[i] of the trajectory segment is preliminarily set, and the trajectory data contained in the trajectory segment i is assigned one by one to the trajectory segment number, instruction type, and initial position point vector data P contained in vel_arr[i]. Start , the middle position point vector data P Middle , end position point vector data P EndAfter the initial setting is completed, it is necessary to determine the instruction type corresponding to the subsequent and preceding trajectory segments of the trajectory segment, and set the terminal velocity information and initial velocity information in vel_arr[i] corresponding to the trajectory segment i according to the different instruction types corresponding to the subsequent trajectory segment i+1 and the preceding trajectory segment i-1;

[0033] Set the terminal velocity information in vel_arr[i] according to the instruction type corresponding to the subsequent trajectory segment i+1:

[0034] If the instruction type corresponding to the trajectory segment i+1 is a collaborative instruction or the trajectory segment i+1 does not exist, the method for setting the terminal velocity information in vel_arr[i] of the trajectory segment i is as follows: the control point vector data of the terminal quadratic Besizer transition curve contained in the velocity information vel_arr[i] of the trajectory segment i is converted to and the corresponding node parameter variable t e , end transition arc length L e and the final velocity variable V e All values ​​are assigned to 0;

[0035] If the instruction type corresponding to the trajectory segment i+1 is a motion trajectory control instruction, the velocity information vel_arr[i+1] corresponding to the trajectory segment i+1 is read, and the initial velocity information in vel_arr[i+1] is extracted as follows: The control point vector data of the initial 2nd Besizer transition curve and the corresponding node parameter t s , initial transition arc length L s , the total arc length of the initial transition is L z and initial velocity V s , the vector velocity of vel_arr[i+1] Node parameter t s , arc length difference L z -L s , initial velocity V s Assign one by one to the terminal velocity information contained in vel_arr[i];

[0036] Set the initial speed information in vel_arr[i] according to the instruction type corresponding to the previous trajectory segment i-1:

[0037] If the instruction type corresponding to the trajectory segment i-1 is a collaborative instruction or the trajectory segment i-1 does not exist, the method for setting the initial velocity information in vel_arr[i] of the trajectory segment i is as follows: the initial variables contained in the velocity information vel_arr[i] of the trajectory segment i: the control point vector data of the initial second order Besizer transition curve and the corresponding node parameter variable t s, initial transition arc length variable L s , the initial transition total arc length variable L z , initial velocity upper and lower bound variables V min ,V max and the initial velocity variable V s All values ​​are assigned to 0, and the motion trajectory control instruction type corresponding to the trajectory segment i is determined. If it is a MOVL instruction, the initial position point vector data P of the trajectory segment is used. Start and the end position point vector data P End Calculate the length of the straight line segment line at two points, and use line-L e -L s The value is assigned to the middle segment arc length variable L in vel_arr[i] r , where L e and L s is the end transition arc length L in vel_arr[i] e and the initial transition arc length variable L s If the motion trajectory control instruction type corresponding to the trajectory segment i is the MOVC instruction, the initial position point vector data P of the trajectory segment is obtained. Start , middle position point vector data P Middle and the end position point vector data P End Calculate the length of the arc segment arc from three points; and add arc-L e -L s The value is assigned to the middle segment arc length variable L in vel_arr[i] r , where L e and L s is the end transition arc length L in vel_arr[i] e and the initial transition arc length variable L s ;

[0038] If the instruction type corresponding to trajectory segment i-1 is a motion trajectory control instruction, a quadratic Besizer curve is used to fit the transition segment between trajectory segment i and trajectory segment i-1. According to the different instruction types corresponding to trajectory segment i and trajectory segment i-1, they are divided into two categories: straight-line-straight-line transition and straight-line-circular-arc transition.

[0039] Furthermore, the specific method of transition section fitting is as follows:

[0040] (1) Straight-line transition

[0041] Assume that the trajectory segment i and the trajectory segment i-1 are both straight line segments, where the starting and ending points of the trajectory segment i are P b and P c , the starting and ending points of trajectory segment i-1 are P a and Pb , P b It is the intersection of the two trajectory segments. First, calculate the lengths of the two trajectory segments. Take the smaller length of the two trajectory segments as the judgment basis. The product of the smaller value and the transition ratio ξ, ξ < 0.5 is used as the transition length L. According to the transition length L, the start and end control points P0 and P2 of the secondary Besizer transition segment are determined. Among them, P0 is on the trajectory segment i-1, P2 is on the trajectory segment i, and the intersection point P of the trajectory segment i and the trajectory segment i-1 is taken. b As the intermediate control point P1, where the vector and vector satisfy Construct a quadratic Besizer transition curve based on the three control points P0, P1, and P2; then set the initial velocity information in vel_arr[i] of trajectory segment i, including: the control point vector data of the initial quadratic Besizer transition curve and the corresponding node parameter variable t s , initial transition arc length variable L s , the initial transition total arc length variable L z , the middle segment arc length variable L r , and the initial velocity variable V s , initial velocity upper and lower bound variables V min ,V max ; The setting method is as follows:

[0042] Assign the three control points P0, P1, and P2 to the control point vector data of the initial quadratic Besizer transition curve in vel_arr[i] in sequence

[0043] According to the quadratic Besizer transition curve parameter equation and the first-order and second-order curve functions, the minimum curvature radius r and the node parameter t corresponding to the minimum curvature radius are calculated using the calculation method of spatial curvature. s , use the arc trajectory to approximate the curve at the minimum curvature radius, according to the bow height error δ in the arc trajectory max The maximum speed V allowed at the minimum curvature radius r is calculated based on the geometric relationship with the curvature radius r. δ , the calculation method is as follows:

[0044] The parametric equation of the second-order Besizer transition curve and the first-order and second-order curve functions are shown below:

[0045]

[0046] For any t∈(0,1), we can substitute into formula (1) to get the curve at any P i The position vector p, velocity vector v and acceleration vector a at , the acceleration vector is a constant, where:

[0047]

[0048] The calculation method of space curvature is as follows:

[0049]

[0050] In t∈(0,1), multiple points are uniformly sampled and multiple times brought into the equation to obtain the minimum curvature radius and the node parameter t at the minimum curvature radius. s ;

[0051] According to the robot interpolation cycle T, the minimum curvature radius r, the system allows the maximum bow height error δ max Calculate the maximum allowable speed V at the minimum curvature radius δ ;

[0052]

[0053] Use the Besizer transition curve arc length calculation formula to calculate the total arc length L of the transition line segment z t∈(0,1) and the current segment transition arc length L s t∈(t s ,1); Assume that the interval of t in the transition curve parameter equation P(t) is 0≤a<t<b≤1, for interval 2 n After equal division, Simpson fitting is performed, and the fitting length is L(2 n ),but:

[0054] Arc length:

[0055] Where h = (ba) / 2 n ,c i =a i +h,b i =a i +2h,a i+1 =b i ,a1=0,b n =1i∈[1,n],n≥1, the fitting accuracy ε is introduced in the calculation, and the value of n is increased gradually until |L(2 n+1 )-L(2 n )|<ε,then L(2 n+1 ) as the arc length of P(t) in the interval [a,b];

[0056] Calculate the trajectory length of trajectory segment i and subtract L from it s and L e The value is assigned to the middle segment arc length variable L in vel_arr[i] r , where L e and Ls is the end transition arc length L in vel_arr[i] e and the initial transition arc length variable L s ;

[0057] Use the general polynomial acceleration and deceleration motion law in the acceleration phase; according to the actual path length S r With initial velocity v s , final velocity v e and the maximum acceleration a m relationship Estimated initial velocity lower bound variable V min ;if Indicates the current trajectory length, the initial speed is at least V min The final velocity can be V e requirements, otherwise V min =0;

[0058] Use the general polynomial acceleration and deceleration motion law in the deceleration stage; according to the actual path length S r With initial velocity v s , final velocity v e Maximum acceleration a m relationship Estimated initial velocity upper bound variable V max ; Indicates that the maximum initial velocity cannot exceed V based on the current trajectory segment length. max ; where V e is the final velocity variable in vel_arr[i], L s , L r and L e The initial transition arc length variable, the middle segment arc length variable, and the end transition arc length in vel_arr[i];

[0059] According to the maximum speed V allowed at the minimum curvature radius δ , the initial velocity lower bound variable V min , the upper bound variable of initial velocity V max , the trajectory data of trajectory segment i contains the maximum allowed speed V within the trajectory segment Max Set the initial velocity variable V in vel_arr[i] s ;

[0060] Take the maximum linear velocity V allowed for the track segment Max , the maximum speed V allowed at the minimum curvature radius δ , the maximum speed allowed by the initial velocity V max The minimum value of the three is assigned to V s ; If V s <V min, then the transition ratio ξ is too small, increase the transition ratio ξ=ξ(ξ+1), and recalculate V s ;

[0061] (2) Straight line-arc transition

[0062] Assume that the trajectory segment i is an arc trajectory and the trajectory segment i-1 is a straight line trajectory segment; the starting and ending points of the trajectory segment i are P b and P c , the starting and ending points of trajectory segment i-1 are P a and P b , P b It is the intersection of the two trajectory segments. First, calculate the lengths of the two trajectory segments. Take the smaller length of the two trajectory segments as the judgment basis. The product of the smaller value and the transition ratio ξ, ξ < 0.5 is used as the transition length L. According to the transition length L, the start and end control points P0 and P2 of the secondary Besizer transition segment are determined. Among them, P0 is on the trajectory segment i-1, P2 is on the trajectory segment i, and the intersection point P of the trajectory segment i and the trajectory segment i-1 is taken. b As the intermediate control point P1, during the transition, the straight line P3P1 is used to approximate the arc P3P1, and the straight line-arc transition is converted into a straight line-straight line transition. In the trajectory segment i-1, P0P1 is a straight line with a length of L, and in the trajectory segment i, P3P1 is an arc. The length of the straight line P0P1 is equal to the arc length of the arc P3P1. A quadratic Besizer transition curve is constructed based on the three control points P0, P1, and P2.

[0063] After setting all the variables in vel_arr[i] of trajectory segment i, the velocity preprocessing module stores them into the velocity information storage module and continues to read them forward.

[0064] Furthermore, after the first speed preprocessing is completed using the reverse order method, the speed preprocessing module takes out the speed information of each trajectory segment from the speed information storage module according to the speed information storage order, and completes the speed fine-tuning in sequence: the speed preprocessing module takes out the speed information of the i-th trajectory segment vel_arr[i] from the speed information storage module, and if the trajectory segment i is the first trajectory segment, that is, i=0, then the initial speed variable V in the current trajectory segment speed information vel_arr[i] is replaced by s Assign 0. If the trajectory segment i is not the first trajectory segment, that is, i>0, the initial velocity variable V in vel_arr[i] needs to be set according to the velocity information vel_arr[i-1] of the trajectory segment i-1. s , then the final velocity variable V in vel_arr[i-1] e The value is assigned to the initial velocity variable V in the velocity information vel_arr[i] of the current trajectory segment s ;

[0065] After the assignment is completed, the initial velocity variable V in vel_arr[i] s Make a judgment, if V s = 0, using the general polynomial acceleration and deceleration motion law in the acceleration phase, according to the actual path length S r With initial velocity v s , final velocity v e And the maximum acceleration a m relationship The estimated initial velocity is V s The final velocity V of trajectory segment i is e_t , L s , L r and L e is the initial transition arc length variable, the middle arc length variable, and the end transition arc length in vel_arr[i]; if V e_t Less than the final velocity V in the current trajectory segment vel_arr[i] e , which means that the length of the current trajectory segment cannot meet the speed from V s =0 Accelerate to V e , the final velocity V e_t Assign to V in vel_arr[i] e , if V e_t Greater than V in the current trajectory segment vel_arr[i] e , then maintain V e The value remains unchanged; if V s ≠0, judge V s Is the initial velocity upper and lower bounds [V min V max ], if V s In the interval [V min V max ], it means that the current distance can meet V s By accelerating and decelerating to reach V e , so keep V in vel_arr[i] e The value remains unchanged. If it is not in the range, the estimated final velocity V e_t Assign to V in vel_arr[i] e , for the initial velocity V in vel_arr[i] s and final velocity V e After fine-tuning is completed, the velocity preprocessing module stores the velocity information vel_arr[i] of the trajectory segment into the velocity information storage module and overwrites the velocity information vel_arr[i] of the principle; the pre-reading is continued until the second preprocessing of the velocity information of all trajectory segments is completed.

[0066] Compared with the existing technology, the present invention and its preferred solution can achieve the continuous operation of multiple track segments at the maximum allowable speed while adhering to the time optimization principle while ensuring sufficient acceleration and deceleration distances and taking into account multi-machine coordination instructions. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

[0068] Figure 1 Schematic diagram of a velocity preprocessing method according to an embodiment of the present invention.

[0069] Figure 2 This is a schematic diagram of the first speed preprocessing according to an embodiment of the present invention.

[0070] Figure 3 Schematic diagram of straight line-to-straight line transition according to an embodiment of the present invention.

[0071] Figure 4 This is a schematic diagram of a straight line-arc transition according to an embodiment of the present invention.

[0072] Figure 5 Schematic diagram of polynomial acceleration and deceleration rules according to an embodiment of the present invention.

[0073] Figure 6 Schematic diagram of the second speed preprocessing according to an embodiment of the present invention. DETAILED DESCRIPTION

[0074] To make the features and advantages of this patent more clearly understood, the following embodiments are specifically described in detail as follows:

[0075] It should be noted that the following detailed description is illustrative and is intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used in this specification have the same meaning as commonly understood by those skilled in the art to which this application belongs.

[0076] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or combinations thereof.

[0077] In the speed preprocessing solution provided in this embodiment, the control software based on the PC is equipped with: a control instruction storage area, a decoding module, a trajectory data buffer, a collaborative data buffer, a speed preprocessing module, a speed information storage module, a motion control module, a coarse interpolation data buffer, and a data processing module; and a programmable I / O interface card at the hardware layer, such as Figure 1 shown.

[0078] When the robot sub-thread begins running, the decoding module extracts control instructions from the control instruction storage area and, depending on the instruction, stores the control instruction information in the trajectory data buffer or the collaborative data buffer. When velocity preprocessing begins, to achieve continuous operation of multiple trajectory segments at the maximum allowable speed while taking into account both motion trajectory control instructions and multi-machine collaborative instructions, the velocity preprocessing module uses a reverse order method. This method extracts trajectory data for each trajectory segment from the trajectory data buffer in the reverse order of the trajectory data storage, pre-reads forward in sequence, and preliminarily sets the velocity information for each trajectory segment based on the instruction type corresponding to each trajectory segment. It then determines the instruction type corresponding to the preceding and succeeding trajectory segments, and sets the velocity information for that trajectory segment again based on the instruction type. Once the speed information is set, the velocity preprocessing module stores the velocity information in the velocity information storage module. After completing the first velocity preprocessing using the reverse order method, the velocity preprocessing module retrieves the velocity information for each trajectory segment from the velocity information storage module in the order in which the velocity information was stored, performs speed fine-tuning in sequence, and stores the velocity information in the velocity information storage module, completing the second velocity preprocessing. After completing the preprocessing, the motion control module takes out the speed information of each trajectory segment from the speed information storage module according to the speed information storage order, and outputs the corresponding coarse interpolation data to the coarse interpolation data buffer according to the different instruction types corresponding to the speed information of each trajectory segment. The data processing module takes out the coarse interpolation data of each trajectory segment from the coarse interpolation data buffer in turn according to the coarse interpolation data storage order, and completes the multi-machine collaborative control in combination with the collaborative data of the collaborative instruction in the collaborative data buffer.

[0079] The multi-machine control system contains multiple robot sub-threads in the PC software layer. Each robot sub-thread contains the above-mentioned modules and buffers. Each robot sub-thread has a corresponding hardware layer programmable I / O interface card. Each robot sub-thread can be called a robot. The robot sub-threads and the sub-threads and the respective programmable I / O interface cards use collaborative instructions to achieve multi-machine collaborative work. The control program of each robot in the multi-machine system consists of the start instruction NOP, motion trajectory control instructions, collaborative instructions and end instructions END. The motion trajectory control instructions mainly include: MOVL and MOVC instructions. A single MOVL instruction can represent a straight line trajectory segment, and each MOVL instruction contains the end point position vector data P of the straight line trajectory. E , Maximum speed allowed within the trajectory segment V m Each MOVC instruction contains the end point position vector data P of the arc trajectory. E , Maximum speed allowed within the trajectory segment V mTwo MOVC instructions can represent a circular arc trajectory segment. Multi-machine coordination instructions primarily include the SET and WAIT instructions. The SET instruction is used to send coordination signals. Each SET instruction contains the robot sub-thread number r_num that receives the coordination signal and its corresponding signal number s_num. The WAIT instruction is used to wait for coordination signals. Each WAIT instruction contains the robot sub-thread number r_num that triggers the coordination signal and its corresponding signal number s_num. When executing SET and WAIT instructions, the robot is in a stopped state.

[0080] In the above buffer, the trajectory data buffer contains the trajectory data variables of each trajectory segment, including: trajectory segment number, instruction type, initial position point vector data P Start , the middle position point vector data P Middle , end position point vector data P End , the maximum speed allowed in the trajectory segment is V Max The collaborative data buffer contains the collaborative data variables for each collaborative instruction, including: collaborative instruction number, robot sub-thread number R_num, signal number S_num. The coarse interpolation data buffer contains the coarse interpolation data variables for each trajectory segment, including: trajectory segment number, instruction type, and pulse number of each joint axis of the robot.

[0081] In the above modules, the speed information storage module stores the speed information of each trajectory segment. The variables of the speed information are as follows: trajectory segment number, instruction type, initial position point vector data P Start , the middle position point vector data P Middle , end position point vector data P End , control point vector data of the initial 2nd order Besizer transition curve and the corresponding node parameter variable t s , initial transition arc length variable L s , the initial transition total arc length variable L z , the middle segment arc length variable L r , control point vector data of the terminal 2nd order Besizer transition curve and the corresponding node parameter variable t e , end transition arc length L e , and the initial velocity variable V s , initial velocity upper and lower bound variables V min ,V max , final velocity variable V e , vel_arr[i] is used below to represent the velocity information of segment i of trajectory.

[0082] When the robot sub-thread begins running, the decoding module extracts control instructions from the control instruction storage area and, depending on the instruction, stores the control instruction information in the trajectory data buffer or the collaborative data buffer. When velocity preprocessing begins, to achieve continuous operation of multiple trajectory segments at the maximum allowable speed while taking into account both motion trajectory control instructions and multi-machine collaborative instructions, the velocity preprocessing module uses a reverse order method. This method extracts trajectory data for each trajectory segment from the trajectory data buffer in the reverse order of the trajectory data storage, pre-reads forward in sequence, and preliminarily sets the velocity information for each trajectory segment based on the instruction type corresponding to each trajectory segment. It then determines the instruction type corresponding to the preceding and succeeding trajectory segments, and sets the velocity information for that trajectory segment again based on the instruction type. Once the speed information is set, the velocity preprocessing module stores the velocity information in the velocity information storage module. After completing the first velocity preprocessing using the reverse order method, the velocity preprocessing module retrieves the velocity information for each trajectory segment from the velocity information storage module in the order in which the velocity information was stored, performs speed fine-tuning in sequence, and stores the velocity information in the velocity information storage module, completing the second velocity preprocessing. After completing the preprocessing, the motion control module takes out the speed information of each trajectory segment from the speed information storage module according to the speed information storage order, and outputs the corresponding coarse interpolation data to the coarse interpolation data buffer according to the different instruction types corresponding to the speed information of each trajectory segment. The data processing module takes out the coarse interpolation data of each trajectory segment from the coarse interpolation data buffer in turn according to the coarse interpolation data storage order, and completes the multi-machine collaborative control in combination with the collaborative data of the collaborative instruction in the collaborative data buffer.

[0083] When decoding starts, the decoding module extracts the control instruction from the control instruction storage area and stores the control instruction information in the trajectory data buffer or the collaborative data buffer according to the different instructions. If the instruction is a NOP instruction, the first position point vector data P of the robot sub-thread control program contained in the instruction is stored in the trajectory data buffer or the collaborative data buffer. S Assign to the global temporary position vector P temp If it is a MOVL instruction, set the trajectory data of the trajectory segment, including: setting the trajectory segment number, setting the instruction type to MOVL, and setting P temp Assign the initial position point vector data P in the trajectory data Start , the intermediate position point vector data P in the trajectory data Middle Assign a value of 0 and set the P contained in the MOVL instruction to E Assign to the end position point vector data P in the trajectory data End and temporary position vector P temp , the MOVL instruction contains V m Assign the maximum speed V allowed within the trajectory segment in the trajectory data Max, store the trajectory data of the trajectory segment into the trajectory data buffer. If it is a MOVC instruction, you need to extract another instruction, and the next instruction is also MOVC, because two MOVC instructions with a total of three points can uniquely determine a circular arc trajectory segment. Setting the trajectory data of the trajectory segment includes: setting the trajectory segment number, setting the instruction type to MOVC, and setting P temp Assign the initial position point vector data P in the trajectory data Start , change the P in the first MOVC E Assign to the middle position point vector data P in the trajectory data Middle , change the P in the second MOVC E Assign to the end position point vector data P in the trajectory data End and temporary position vector P temp , the V contained in the second MOVC instruction m Assign the maximum speed V allowed within the trajectory segment in the trajectory data Max , save the trajectory data of the trajectory segment into the trajectory data buffer. If it is a SET instruction, set the trajectory data of the trajectory segment, including: setting the trajectory segment number, setting the instruction type to SET, and other P Start 、P Middle 、P End 、V Max All values ​​are assigned to 0, and the collaborative data for the collaborative instruction is set. The trajectory segment number of the trajectory segment is assigned to the collaborative instruction number in the collaborative data. The r_num contained in the SET is assigned to the robot subthread number R_num in the collaborative data. The s_num contained in the SET is assigned to the signal number S_num in the collaborative data. The trajectory data and collaborative data are sent to the trajectory data buffer and collaborative data buffer, respectively. If it is a WAIT instruction, the operation is similar to SET. According to the above decoding method, all control instructions are decoded.

[0084] During the first speed preprocessing, the speed preprocessing module uses the reverse order method, that is, the trajectory data of the trajectory segment is extracted from the trajectory data buffer in the order opposite to the storage order of the trajectory data, and the speed information is pre-read and set in sequence. The speed information setting method is as follows: Assuming that the trajectory segment i is currently taken out, it is necessary to determine the instruction type corresponding to the trajectory segment i. If the instruction type is a collaborative instruction, the trajectory data (trajectory segment number, instruction type) contained in the trajectory segment i is assigned one by one to the trajectory segment number and instruction type contained in the speed information vel_arr[i] of the trajectory segment i, and all other variables in vel_arr[i] are assigned to 0. After the assignment is completed, the speed preprocessing module stores the speed information vel_arr[i] of the trajectory segment into the speed information storage module. If it is a motion trajectory control instruction, the speed information vel_arr[i] of the trajectory segment is preliminarily set, and the trajectory data (trajectory segment number, instruction type, P Start , P Middle , P End ) are assigned to the trajectory segment number, instruction type, initial position point vector data P contained in vel_arr[i] one by one. Start , the middle position point vector data P Middle , end position point vector data P End After the initial setting is completed, it is necessary to determine the instruction type corresponding to the following and preceding trajectory segments of the trajectory segment. According to the instruction types corresponding to the following trajectory segment i+1 and the preceding trajectory segment i-1, the terminal velocity information and initial velocity information in vel_arr[i] corresponding to the trajectory segment i are set, as shown in the following example: Figure 2 shown.

[0085] Set the terminal velocity information in vel_arr[i] according to the instruction type corresponding to the subsequent trajectory segment i+1:

[0086] If the instruction type corresponding to the trajectory segment i+1 is a collaborative instruction or the trajectory segment i+1 does not exist, the method for setting the terminal velocity information in vel_arr[i] of the trajectory segment i is as follows: the control point vector data of the terminal quadratic Besizer transition curve contained in the velocity information vel_arr[i] of the trajectory segment i is converted to and the corresponding node parameter variable t e , end transition arc length L e and the final velocity variable V e All values ​​are assigned to 0.

[0087] If the instruction type corresponding to the trajectory segment i+1 is a motion trajectory control instruction, the velocity information vel_arr[i+1] corresponding to the trajectory segment i+1 is read, and the initial velocity information in vel_arr[i+1] is extracted as follows: The control point vector data of the initial 2nd Besizer transition curve and the corresponding node parameter t s , initial transition arc length L s , the total arc length of the initial transition is L z and initial velocity V s , the vector velocity of vel_arr[i+1] Node parameter t s , arc length difference L z -L s , initial velocity V s Assign one value to the terminal velocity information contained in vel_arr[i] (the control point vector data of the terminal 2nd Besizer transition curve) and the corresponding node parameter variable t e , end transition arc length L e , final velocity variable V e ).

[0088] Set the initial speed information in vel_arr[i] according to the instruction type corresponding to the previous trajectory segment i-1:

[0089] If the instruction type corresponding to the trajectory segment i-1 is a collaborative instruction or the trajectory segment i-1 does not exist, the method for setting the initial velocity information in vel_arr[i] of the trajectory segment i is as follows: the initial variables contained in the velocity information vel_arr[i] of the trajectory segment i: the control point vector data of the initial second order Besizer transition curve and the corresponding node parameter variable t s , initial transition arc length variable L s , the initial transition total arc length variable L z , initial velocity upper and lower bound variables V min ,V max and the initial velocity variable V s All values ​​are assigned to 0, and the motion trajectory control instruction type corresponding to the trajectory segment i is determined. If it is a MOVL instruction, the initial position point vector data P of the trajectory segment is used. Start and the end position point vector data P End Calculate the length of the straight line segment line at two points, and use line-L e -L s The value is assigned to the middle segment arc length variable L in vel_arr[i] r , where L e and L sis the end transition arc length L in vel_arr[i] e and the initial transition arc length variable L s If the motion trajectory control instruction type corresponding to the trajectory segment i is the MOVC instruction, the initial position point vector data P of the trajectory segment is Start , middle position point vector data P Middle and the end position point vector data P End Calculate the length of the arc segment arc from three points. And convert arc-L e -L s The value is assigned to the middle segment arc length variable L in vel_arr[i] r , where L e and L s is the end transition arc length L in vel_arr[i] e and the initial transition arc length variable L s .

[0090] If the instruction type corresponding to the trajectory segment i-1 is a motion trajectory control instruction, a quadratic Besizer curve is used to fit the transition segment between the trajectory segment i and the trajectory segment i-1. According to the different instruction types corresponding to the trajectory segment i and the trajectory segment i-1, they can be divided into two categories: straight line-straight line transition and straight line-circular arc transition. Other arc-circular arc transition methods are similar to straight line-circular arc. The transition methods are as follows:

[0091] (1) Straight-line transition

[0092] Assume that the trajectory segment i and the trajectory segment i-1 are both straight line trajectory segments, such as Figure 3 As shown. The starting and ending points of trajectory segment i are P b and P c , the starting and ending points of trajectory segment i-1 are P a and P b , P b It is the intersection of the two trajectory segments. First, calculate the lengths of the two trajectory segments. Take the smaller length of the two trajectory segments as the judgment basis. The product of the smaller value and the transition ratio ξ (ξ < 0.5) is used as the transition length L. According to the transition length L, the start and end control points P0 and P2 of the secondary Besizer transition segment are determined. Among them, P0 is on the trajectory segment i-1, P2 is on the trajectory segment i, and the intersection point P of the trajectory segment i and the trajectory segment i-1 is taken. b As the intermediate control point P1, where the vector and vector satisfy Construct a quadratic Besizer transition curve based on the three control points P0, P1, and P2. Next, you need to set the initial velocity information in vel_arr[i] of trajectory segment i, including: the control point vector data of the initial quadratic Besizer transition curve and the corresponding node parameter variable t s , initial transition arc length variable L s , the initial transition total arc length variable L z , the middle segment arc length variable L r , and the initial velocity variable V s , initial velocity upper and lower bound variables V min ,V max The setting method is as follows:

[0093] Assign the three control points P0, P1, and P2 to the control point vector data of the initial quadratic Besizer transition curve in vel_arr[i] in sequence

[0094] According to the quadratic Besizer transition curve parameter equation and the first-order and second-order curve functions, the minimum curvature radius r and the node parameter t corresponding to the minimum curvature radius are calculated using the calculation method of spatial curvature. s , use the arc trajectory to approximate the curve at the minimum curvature radius, according to the bow height error δ in the arc trajectory max The maximum speed V allowed at the minimum curvature radius r is calculated based on the geometric relationship with the curvature radius r. δ , the calculation method is as follows:

[0095] The parametric equation of the second-order Besizer transition curve and the first-order and second-order curve functions are shown below:

[0096]

[0097] For any t∈(0,1), by substituting the above formula (1), we can get the curve at any P i The position vector p, velocity vector v and acceleration vector a at , the acceleration vector is a constant, where:

[0098]

[0099] The calculation method of space curvature is as follows:

[0100]

[0101] In t∈(0,1), multiple points are uniformly sampled and multiple times brought into the equation to obtain the minimum curvature radius and the node parameter t at the minimum curvature radius. s .

[0102] According to the robot interpolation cycle T, the minimum curvature radius r, the system allows the maximum bow height error δ max Calculate the maximum allowable speed V at the minimum curvature radius δ .

[0103]

[0104] Use the Besizer transition curve arc length calculation formula to calculate the total arc length L of the transition line segment z t∈(0,1) and the current segment transition arc length L s t∈(t s ,1). Assume that the interval of t in the transition curve parameter equation P(t) is 0≤a<t<b≤1, for interval 2 n After equal division, Simpson fitting is performed, and the fitting length is L(2 n ),but:

[0105]

[0106] Where h = (ba) / 2 n ,c i =a i +h,b i =a i +2h,a i+1 =b i ,a1=0,b n =1i∈[1,n],n≥1, the fitting accuracy ε is introduced in the calculation, and the value of n is increased gradually until |L(2 n+1 )-L(2 n )|<ε,then L(2 n+1 ) as the arc length of P(t) in the interval [a,b].

[0107] Calculate the trajectory length of trajectory segment i and subtract L from it s and L e The value is assigned to the middle segment arc length variable L in vel_arr[i] r , where L e and L s is the end transition arc length L in vel_arr[i] e and the initial transition arc length variable L s .

[0108] Use the general polynomial acceleration and deceleration motion law in the acceleration phase, such as Figure 5 In the T1, T2, T4, and T5 stages. According to the actual path length S r With initial velocity v s , final velocity v e and the maximum acceleration a m relationship Estimated initial velocity lower bound variable V min .if (Indicates the current trajectory length, the initial speed is at least V min The final velocity can be Ve requirements), whereas V min = 0. Using the general polynomial acceleration and deceleration motion law in the deceleration stage, such as Figure 5 T7, T8, T 10 、T 11 stage. According to the actual path length S r With initial velocity v s , final velocity v e Maximum acceleration a m relationship Estimated initial velocity upper bound variable V max . (Indicates that the maximum initial velocity cannot exceed V based on the current trajectory segment length. max ). Where V e is the final velocity variable in vel_arr[i], L s 、L r and L e The initial transition arc length variable, the middle segment arc length variable, and the end transition arc length in vel_arr[i].

[0109] According to the maximum speed V allowed at the minimum curvature radius δ , the initial velocity lower bound variable V min , the upper bound variable of initial velocity V max , the trajectory data of trajectory segment i contains the maximum allowed speed V within the trajectory segment Max Set the initial velocity variable V in vel_arr[i] s .

[0110] Take the maximum linear velocity V allowed for the track segment Max , the maximum speed V allowed at the minimum curvature radius δ , the maximum speed allowed by the initial velocity V max The minimum value of the three is assigned to V s If V s <V min , then the transition ratio ξ is too small, increase the transition ratio ξ=ξ(ξ+1), and recalculate V s .

[0111] (2) Straight line-arc transition

[0112] Assume that the trajectory segment i is a circular arc trajectory and the trajectory segment i-1 is a straight line trajectory segment, such as Figure 4 As shown. The starting and ending points of trajectory segment i are P b and P c , the starting and ending points of trajectory segment i-1 are P a and P b , P bIt is the intersection of the two trajectory segments. First, calculate the lengths of the two trajectory segments. Take the smaller length of the two trajectory segments as the judgment basis. The product of the smaller value and the transition ratio ξ (ξ < 0.5) is used as the transition length L. According to the transition length L, the start and end control points P0 and P2 of the secondary Besizer transition segment are determined. Among them, P0 is on the trajectory segment i-1, P2 is on the trajectory segment i, and the intersection point P of the trajectory segment i and the trajectory segment i-1 is taken. b As the intermediate control point P1, during the transition, the straight line P3P1 is used to approximate the arc P3P1, converting the straight-to-arc transition into a straight-to-straight transition. In trajectory segment i-1, P0P1 is a straight line with a length of L, while in trajectory segment i, P3P1 is an arc with the same length as the arc P3P1. A quadratic Besizer transition curve is constructed based on the three control points P0, P1, and P2. The initial velocity information in vel_arr[i] for trajectory segment i is set in the same way as for the straight-to-straight transition.

[0113] After setting all the variables in vel_arr[i] of trajectory segment i, the velocity preprocessing module stores them into the velocity information storage module and continues to read them forward.

[0114] After the first speed preprocessing is completed using the reverse order method, the speed preprocessing module takes out the speed information of each trajectory segment from the speed information storage module according to the speed information storage order, and completes the speed fine-tuning in sequence, such as Figure 6 The velocity preprocessing module takes out the velocity information of the i-th track segment vel_arr[i] from the velocity information storage module. If the track segment i is the first track segment, that is, i=0, the initial velocity variable V in the current track segment velocity information vel_arr[i] is converted to s Assign 0. If the trajectory segment i is not the first trajectory segment, that is, i>0, the initial velocity variable V in vel_arr[i] needs to be set according to the velocity information vel_arr[i-1] of the trajectory segment i-1. s , then the final velocity variable V in vel_arr[i-1] e The value is assigned to the initial velocity variable V in the velocity information vel_arr[i] of the current trajectory segment s .

[0115] After the assignment is completed, the initial velocity variable V in vel_arr[i] s Make a judgment, if V s =0, using the general polynomial acceleration and deceleration motion law in the acceleration phase, such as Figure 5 In the T1, T2, T4, and T5 stages, according to the actual path length S r With initial velocity v s , final velocity v e And the maximum acceleration am relationship The estimated initial velocity is V s The final velocity V of trajectory segment i is e_t , L s 、L r and L e The initial transition arc length variable, the middle arc length variable, and the end transition arc length in vel_arr[i]. If V e_t Less than the final velocity V in the current trajectory segment vel_arr[i] e , which means that the length of the current trajectory segment cannot meet the speed from V s =0 Accelerate to V e , the final velocity V e_t Assign to V in vel_arr[i] e , if V e_t Greater than V in the current trajectory segment vel_arr[i] e , then maintain V e The value of remains unchanged. If V s ≠0, judge V s Is the initial velocity upper and lower bounds [V min V max ], if V s In the interval [V min V max ], it proves that the current distance can satisfy V s By accelerating and decelerating to reach V e , so keep V in vel_arr[i] e The value remains unchanged. If it is not in the range, the estimated final velocity V e_t Assign to V in vel_arr[i] e , for the initial velocity V in vel_arr[i] s and final velocity V e After fine-tuning is completed, the velocity preprocessing module stores the velocity information vel_arr[i] of the trajectory segment into the velocity information storage module and overwrites the original velocity information vel_arr[i]. Continue pre-reading until the second preprocessing of the velocity information of all trajectory segments is completed.

[0116] After preprocessing the velocity information for each trajectory segment, the motion control module retrieves the velocity information for each trajectory segment from the velocity information storage module in the order in which the velocity information was stored. Motion planning is then performed sequentially. Depending on the instruction type corresponding to each trajectory segment's velocity information, if it is a motion trajectory control instruction, motion planning is performed and the corresponding coarse interpolation data is output to the coarse interpolation data buffer. This coarse interpolation data includes the trajectory segment number, instruction type, and the number of pulses for each joint axis of the robot. If it is a collaborative instruction, motion planning is not performed and the coarse interpolation data is directly output, including the trajectory segment number, instruction type, and the number of pulses for each joint axis of the robot, where the number of pulses for each joint axis is 0. The data processing module sequentially retrieves the coarse interpolation data for each trajectory segment from the coarse interpolation data buffer in the order in which the coarse interpolation data is stored. If the instruction type corresponding to the coarse interpolation data is a motion trajectory control instruction, the pulse counts for each joint in the coarse interpolation data are sent to the programmable I / O interface card to control the robot's motion. If the instruction type corresponding to the coarse interpolation data is a collaboration instruction, the collaboration data buffer is indexed and the collaboration instruction with the same collaboration instruction number as the trajectory segment number in the coarse interpolation data for that trajectory segment is retrieved. Depending on the instruction type, if it is a SET instruction, the corresponding signal number is sent to the corresponding robot sub-thread; if it is a WAIT instruction, the module waits for the arrival of the collaboration signal from the corresponding robot sub-thread. Based on the above method, each robot in the multi-machine system completes the continuous operation of multiple trajectory segments at the maximum allowable speed while taking into account both motion trajectory control instructions and multi-machine collaboration instructions.

[0117] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0118] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the steps in the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0119] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0120] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0121] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other manner. Any person skilled in the art may utilize the above-disclosed technical content to modify or modify the present invention into equivalent embodiments. However, any simple modifications, equivalent variations, and modifications to the above embodiments that do not depart from the technical content of the present invention and are based on the technical essence of the present invention remain within the scope of protection of the present invention.

[0122] This patent is not limited to the above-mentioned optimal implementation method. Anyone can derive various other forms of industrial robot multi-machine collaborative position trajectory speed preprocessing methods based on the inspiration of this patent. All equal changes and modifications made within the scope of the patent application of this invention should be covered by this patent.

Claims

1. A method for preprocessing the position trajectory and velocity of multiple industrial robots in a coordinated manner, characterized by: A multi-machine control system consisting of multiple robot sub-threads is set up on a PC. Each robot sub-thread is equipped with: a control instruction storage area, a decoding module, a trajectory data buffer, a collaborative data buffer, a velocity preprocessing module, a velocity information storage module, a motion control module, a coarse interpolation data buffer, and a data processing module. Each robot sub-thread has a corresponding hardware-layer programmable I / O interface card for connecting to a robot. Multi-machine collaborative work is achieved through collaborative instructions between robot sub-threads and between sub-threads and their respective programmable I / O interface cards: When the robot sub-thread starts running, the decoding module extracts the control instruction from the control instruction storage area and stores the control instruction information into the trajectory data buffer or the collaborative data buffer according to the different instructions; When starting speed preprocessing, in order to achieve continuous operation of multiple trajectory segments at the maximum allowable speed while taking into account the motion trajectory control instructions and multi-machine coordination instructions, the speed preprocessing module uses a reverse order method, that is, the trajectory data of the trajectory segment is extracted from the trajectory data buffer in the reverse order of the trajectory data storage order, and pre-read forward in sequence. According to the different instruction types corresponding to each trajectory segment, the speed information corresponding to the trajectory segment is preliminarily set, and the instruction types corresponding to the trajectory segments before and after the trajectory segment are determined, and the speed information of the trajectory segment is set again according to the instruction type; After the setting is completed, the speed preprocessing module stores the speed information into the speed information storage module; After the first speed preprocessing is completed using the reverse order method, the speed preprocessing module retrieves the speed information of each trajectory segment from the speed information storage module according to the speed information storage order, completes the speed fine-tuning in sequence, and stores the speed information into the speed information storage module to achieve the second speed information preprocessing; After completing the preprocessing, the motion control module takes out the speed information of each trajectory segment from the speed information storage module according to the speed information storage order, and outputs the corresponding coarse interpolation data to the coarse interpolation data buffer according to the different instruction types corresponding to the speed information of each trajectory segment. The data processing module takes out the coarse interpolation data of each trajectory segment from the coarse interpolation data buffer in sequence according to the coarse interpolation data storage order, and completes the multi-machine collaborative control in combination with the collaborative data of the collaborative instruction in the collaborative data buffer.

2. The method for preprocessing the position trajectory and velocity of multiple industrial robots in collaboration according to claim 1, characterized in that: After completing the preprocessing of the velocity information of each trajectory segment, the motion control module retrieves the velocity information of each trajectory segment from the velocity information storage module according to the order in which the velocity information is stored, and completes the motion planning in sequence. According to the different instruction types corresponding to the velocity information of each trajectory segment: If it is a motion trajectory control instruction, motion planning is performed on it and the corresponding coarse interpolation data is output to the coarse interpolation data buffer. The coarse interpolation data includes: trajectory segment number, instruction type, and the number of pulses of each joint axis of the robot; If it is a collaborative instruction, motion planning is not performed and the coarse interpolation data is directly output, including the trajectory segment number, instruction type, and the number of pulses of each joint axis of the robot, where the number of pulses of each joint axis is 0; the data processing module sequentially takes out the coarse interpolation data of each trajectory segment from the coarse interpolation data buffer in the order in which the coarse interpolation data is stored. If the instruction type corresponding to the coarse interpolation data is a motion trajectory control instruction, the number of pulses of each joint in the coarse interpolation data is sent to the programmable I / O interface card to control the movement of the robot. If the instruction type corresponding to the coarse interpolation data is a collaborative instruction, the collaborative data buffer is indexed and the collaborative instruction with the same collaborative instruction number as the trajectory segment number in the coarse interpolation data of the trajectory segment is taken out. According to the different instruction types: If it is a SET instruction, the corresponding signal number is sent to each corresponding robot sub-thread. If it is a WAIT instruction, wait for the arrival of the coordination signal of the corresponding robot sub-thread.

3. The method for preprocessing the position trajectory and velocity of multiple industrial robots in collaboration according to claim 1, characterized in that: The control program of each robot in a multi-machine system consists of the start instruction NOP, motion trajectory control instructions, coordination instructions and end instruction END; The motion trajectory control instructions include: MOVL and MOVC instructions; a single MOVL instruction represents a straight line segment, and each MOVL instruction contains the end point position vector data P of the straight line segment. E , Maximum speed allowed within the trajectory segment V m ; Each MOVC instruction contains the end point position vector data P of the arc trajectory E , Maximum speed allowed within the trajectory segment V m ,The two MOVC instructions represent a circular arc trajectory segment; The collaborative instructions include SET and WAIT instructions; the SET instruction is used to send a collaborative signal, and each SET instruction contains the robot sub-thread number r_num that receives the collaborative signal and its corresponding signal number s_num; the WAIT instruction is used to wait for the collaborative signal, and each WAIT instruction contains the robot sub-thread number r_num that triggers the collaborative signal and its corresponding signal number s_num; when executing the SET and WAIT instructions, the robot is in a stopped state; The trajectory data buffer contains the trajectory data variables of each trajectory segment, including: trajectory segment number, instruction type, initial position point vector data P Start , the middle position point vector data P Middle , end position point vector data P End , the maximum speed allowed in the trajectory segment is V Max The collaborative data buffer contains the collaborative data variables of each collaborative instruction, including: collaborative instruction number, robot sub-thread number R_num, signal number S_num; the coarse interpolation data buffer contains the coarse interpolation data variables of each trajectory segment, including: trajectory segment number, instruction type, and number of pulses of each joint axis of the robot; The speed information storage module stores the speed information of each track segment. The speed information contains the following variables: track segment number, instruction type, initial position point vector data P Start , the middle position point vector data P Middle , end position point vector data P End , initial 2nd order Besizer transition curve control point vector data and the corresponding node parameter variable t s , initial transition arc length variable L s , the initial transition total arc length variable L z , the middle segment arc length variable L r , control point vector data of the terminal 2nd order Besizer transition curve and the corresponding node parameter variable t e , end transition arc length L e , and the initial velocity variable V s , initial velocity upper and lower bound variables V min ,V max , final velocity variable V e , vel_arr[i] represents the velocity information of segment i of the trajectory.

4. The method for preprocessing the position trajectory and velocity of multiple industrial robots in collaboration according to claim 3, characterized in that: When decoding starts, the decoding module extracts the control instruction from the control instruction storage area and stores the control instruction information in the trajectory data buffer or the collaborative data buffer according to the different instructions: If the instruction is a NOP instruction, the first position point vector data P of the robot sub-thread control program contained in the instruction is S Assign to the global temporary position vector P temp ; If it is a MOVL instruction, set the trajectory data of the trajectory segment, including: setting the trajectory segment number, setting the instruction type to MOVL, and setting P temp Assign the initial position point vector data P in the trajectory data Start , the intermediate position point vector data P in the trajectory data Middle Assign a value of 0 and set the P contained in the MOVL instruction to E Assign to the end position point vector data P in the trajectory data End and temporary position vector P temp , the MOVL instruction contains V m Assign the maximum speed V allowed within the trajectory segment in the trajectory data Max , store the trajectory data of the trajectory segment into the trajectory data buffer; If it is a MOVC instruction, you need to extract another instruction. The next instruction is also MOVC, because two MOVC instructions with three points can uniquely determine a circular arc segment. Set the trajectory data of the trajectory segment, including: setting the trajectory segment number, setting the instruction type to MOVC, and setting P temp Assign the initial position point vector data P in the trajectory data Start , change the P in the first MOVC E Assign to the middle position point vector data P in the trajectory data Middle , change the P in the second MOVC E Assign to the end position point vector data P in the trajectory data End and temporary position vector P temp , the V contained in the second MOVC instruction m Assign the maximum speed V allowed within the trajectory segment in the trajectory data Max , store the trajectory data of the trajectory segment into the trajectory data buffer; If it is a SET command, set the trajectory data of the trajectory segment, including: setting the trajectory segment number, setting the command type to SET, and other P Start 、P Middle 、P End 、V Max All are assigned to 0, and the collaborative data of the collaborative instruction is set, the trajectory segment number of the trajectory segment is assigned to the collaborative instruction number in the collaborative data, the r_num contained in SET is assigned to the robot sub-thread number R_num in the collaborative data, and the s_num contained in SET is assigned to the signal number S_num in the collaborative data. The trajectory data and collaborative data are sent to the trajectory data buffer and the collaborative data buffer respectively; according to the above decoding method, the decoding of all control instructions is completed.

5. The method for preprocessing the position trajectory and velocity of multiple industrial robots in collaboration according to claim 4, characterized in that: During the first speed preprocessing, the speed preprocessing module uses the reverse order method, that is, the trajectory data of the trajectory segment is extracted from the trajectory data buffer in the reverse order of the trajectory data storage order, and the speed information is pre-read and set in sequence. The speed information setting method is as follows: assuming that the current segment is trajectory segment i, it is necessary to determine the instruction type corresponding to the trajectory segment i. If the instruction type is a collaborative instruction, the trajectory data contained in the trajectory segment i is assigned one-to-one to the trajectory segment number and instruction type contained in the velocity information vel_arr[i] of the trajectory segment i. All other variables in vel_arr[i] are assigned to 0. After the assignment is completed, the velocity preprocessing module stores the velocity information vel_arr[i] of the trajectory segment into the velocity information storage module. If it is a motion trajectory control instruction, the velocity information vel_arr[i] of the trajectory segment is preliminarily set, and the trajectory data contained in the trajectory segment i is assigned one by one to the trajectory segment number, instruction type, and initial position point vector data P contained in vel_arr[i]. Start , the middle position point vector data P Middle , end position point vector data P End After the initial setting is completed, it is necessary to determine the instruction type corresponding to the subsequent and preceding trajectory segments of the trajectory segment, and set the terminal velocity information and initial velocity information in vel_arr[i] corresponding to the trajectory segment i according to the different instruction types corresponding to the subsequent trajectory segment i+1 and the preceding trajectory segment i-1; Set the terminal velocity information in vel_arr[i] according to the instruction type corresponding to the subsequent trajectory segment i+1: If the instruction type corresponding to the trajectory segment i+1 is a collaborative instruction or the trajectory segment i+1 does not exist, the method for setting the terminal velocity information in vel_arr[i] of the trajectory segment i is as follows: the control point vector data of the terminal quadratic Besizer transition curve contained in the velocity information vel_arr[i] of the trajectory segment i is converted to and the corresponding node parameter variable t e , end transition arc length L e and the final velocity variable V e All values ​​are assigned to 0; If the instruction type corresponding to the trajectory segment i+1 is a motion trajectory control instruction, the velocity information vel_arr[i+1] corresponding to the trajectory segment i+1 is read, and the initial velocity information in vel_arr[i+1] is extracted as follows: The control point vector data of the initial 2nd Besizer transition curve and the corresponding node parameter t s , initial transition arc length L s , the total arc length of the initial transition is L z and initial velocity V s , the vector data of vel_arr[i+1] Node parameter t s , arc length difference L z -L s , initial velocity V s Assign one by one to the terminal velocity information contained in vel_arr[i]; Set the initial speed information in vel_arr[i] according to the instruction type corresponding to the previous trajectory segment i-1: If the instruction type corresponding to the trajectory segment i-1 is a collaborative instruction or the trajectory segment i-1 does not exist, the method for setting the initial velocity information in vel_arr[i] of the trajectory segment i is as follows: the initial variables contained in the velocity information vel_arr[i] of the trajectory segment i: the control point vector data of the initial second order Besizer transition curve and the corresponding node parameter variable t s , initial transition arc length variable L s , the initial transition total arc length variable L z , initial velocity upper and lower bound variables V min ,V max and the initial velocity variable V s All values ​​are assigned to 0, and the motion trajectory control instruction type corresponding to the trajectory segment i is determined. If it is a MOVL instruction, the initial position point vector data P of the trajectory segment is used. Start and the end position point vector data P End Calculate the length of the straight line segment line at two points, and use line-L e -L s The value is assigned to the middle segment arc length variable L in vel_arr[i] r , where L e and L s is the end transition arc length L in vel_arr[i] e and the initial transition arc length variable L s If the motion trajectory control instruction type corresponding to the trajectory segment i is the MOVC instruction, the initial position point vector data P of the trajectory segment is obtained. Start , middle position point vector data P Middle and the end position point vector data P End Calculate the length of the arc segment arc from three points; and add arc-L e -L s The value is assigned to the middle segment arc length variable L in vel_arr[i] r , where L e and L s is the end transition arc length L in vel_arr[i] e and the initial transition arc length variable L s ; If the instruction type corresponding to trajectory segment i-1 is a motion trajectory control instruction, a quadratic Besizer curve is used to fit the transition segment between trajectory segment i and trajectory segment i-1. According to the different instruction types corresponding to trajectory segment i and trajectory segment i-1, they are divided into two categories: straight-line-straight-line transition and straight-line-circular-arc transition.

6. The method for preprocessing the position trajectory and velocity of multiple industrial robots in collaboration according to claim 5, characterized in that: The specific method of transition section fitting is as follows: (1) Straight-line transition Assume that the trajectory segment i and the trajectory segment i-1 are both straight line segments, where the starting and ending points of the trajectory segment i are P b and P c , the starting and ending points of trajectory segment i-1 are P a and P b , P b It is the intersection of the two trajectory segments. First, calculate the lengths of the two trajectory segments. Take the smaller length of the two trajectory segments as the judgment basis. The product of the smaller value and the transition ratio ξ, ξ < 0.5 is used as the transition length L. According to the transition length L, the start and end control points P0 and P2 of the secondary Besizer transition segment are determined. Among them, P0 is on the trajectory segment i-1, P2 is on the trajectory segment i, and the intersection point P of the trajectory segment i and the trajectory segment i-1 is taken. b As the intermediate control point P1, where the vector and vector satisfy Construct a quadratic Besizer transition curve based on the three control points P0, P1, and P2; then set the initial velocity information in vel_arr[i] of trajectory segment i, including: the control point vector data of the initial quadratic Besizer transition curve and the corresponding node parameter variable t s , initial transition arc length variable L s , the initial transition total arc length variable L z , the middle segment arc length variable L r , and the initial velocity variable V s , initial velocity upper and lower bound variables V min ,V max ; The setting method is as follows: Assign the three control points P0, P1, and P2 to the control point vector data of the initial quadratic Besizer transition curve in vel_arr[i] in sequence According to the quadratic Besizer transition curve parameter equation and the first-order and second-order curve functions, the minimum curvature radius r and the node parameter t corresponding to the minimum curvature radius are calculated using the calculation method of spatial curvature. s , use the arc trajectory to approximate the curve at the minimum curvature radius, according to the bow height error δ in the arc trajectory max The maximum speed V allowed at the minimum curvature radius r is calculated based on the geometric relationship with the curvature radius r. δ , the calculation method is as follows: The parametric equation of the second-order Besizer transition curve and the first-order and second-order curve functions are shown below: For any t∈(0,1), we can substitute into formula (1) to get the curve at any P i The position vector p, velocity vector v and acceleration vector a at , the acceleration vector is a constant, where: The calculation method of space curvature is as follows: In t∈(0,1), multiple points are uniformly sampled and multiple times brought into the equation to obtain the minimum curvature radius and the node parameter t at the minimum curvature radius. s ; According to the robot interpolation cycle T, the minimum curvature radius r, the system allows the maximum bow height error δ max Calculate the maximum allowable speed V at the minimum curvature radius δ ; Use the Besizer transition curve arc length calculation formula to calculate the total arc length L of the transition line segment z t∈(0,1) and the current segment transition arc length L s t∈(t s ,1); Assume that the interval of t in the transition curve parameter equation P(t) is 0≤a<t<b≤1, for interval 2 n After equal division, Simpson fitting is performed, and the fitting length is L(2 n ),but: Arc length: Where h = (ba) / 2 n ,c i =a i +h,b i =a i +2h,a i+1 =b i ,a1=0,b n =1i∈[1,n],n≥1, the fitting accuracy ε is introduced in the calculation, and the value of n is increased gradually until |L(2 n+1 )-L(2 n )|<ε,then L(2 n+1 ) as the arc length of P(t) in the interval [a,b]; Calculate the trajectory length of trajectory segment i and subtract L from it s and L e The value is assigned to the middle segment arc length variable L in vel_arr[i] r , where L e and L s is the end transition arc length L in vel_arr[i] e and the initial transition arc length variable L s ; Use the general polynomial acceleration and deceleration motion law in the acceleration phase; according to the actual path length S r With initial velocity V s , final velocity V e and the maximum acceleration a m relationship Estimated initial velocity lower bound variable V min ;if Indicates the current trajectory length, the initial speed is at least V min The final velocity can be V e requirements, otherwise V min =0; Use the general polynomial acceleration and deceleration motion law in the deceleration stage; according to the actual path length S r With initial velocity V s , final velocity V e Maximum acceleration a m relationship Estimated initial velocity upper bound variable V max ; Indicates that the maximum initial velocity cannot exceed V based on the current trajectory segment length. max ; where V e is the final velocity variable in vel_arr[i], L s , L r and L e The initial transition arc length variable, the middle segment arc length variable, and the end transition arc length in vel_arr[i]; According to the maximum speed V allowed at the minimum curvature radius δ , initial velocity lower bound variable V min , the upper bound variable of initial velocity V max The maximum speed VMAX allowed in the trajectory segment contained in the trajectory data of trajectory segment i sets the initial speed variable V in vel_arr[i] s ; Take the maximum speed VMAX allowed by the trajectory segment and the maximum speed V allowed at the minimum curvature radius δ , the upper bound variable of initial velocity V max The minimum value of the three is assigned to V s ; If V s <V min , then the transition ratio ξ is too small, increase the transition ratio ξ=ξ(ξ+1), and recalculate V s ; (2) Straight line-arc transition Assume that the trajectory segment i is an arc trajectory and the trajectory segment i-1 is a straight line trajectory segment; the starting and ending points of the trajectory segment i are P b and P c , the starting and ending points of trajectory segment i-1 are P a and P b , P b It is the intersection of the two trajectory segments. First, calculate the lengths of the two trajectory segments. Take the smaller length of the two trajectory segments as the judgment basis. The product of the smaller value and the transition ratio ξ, ξ < 0.5 is used as the transition length L. According to the transition length L, the start and end control points P0 and P2 of the secondary Besizer transition segment are determined. Among them, P0 is on the trajectory segment i-1, P2 is on the trajectory segment i, and the intersection point P of the trajectory segment i and the trajectory segment i-1 is taken. b As the intermediate control point P1, during the transition, the straight line P3P1 is used to approximate the arc P3P1, and the straight line-arc transition is converted into a straight line-straight line transition. In the trajectory segment i-1, P0P1 is a straight line with a length of L, and in the trajectory segment i, P3P1 is an arc. The length of the straight line P0P1 is equal to the arc length of the arc P3P1. A quadratic Besizer transition curve is constructed based on the three control points P0, P1, and P2. After setting all the variables in vel_arr[i] of trajectory segment i, the velocity preprocessing module stores them into the velocity information storage module and continues to read them forward.

7. The method for preprocessing the position trajectory and velocity of multiple industrial robots in collaboration according to claim 6, characterized in that: After the first speed preprocessing is completed using the reverse order method, the speed preprocessing module takes out the speed information of each trajectory segment from the speed information storage module according to the speed information storage order, and completes the speed fine-tuning in sequence: the speed preprocessing module takes out the speed information of the i-th trajectory segment vel_arr[i] from the speed information storage module. If the trajectory segment i is the first trajectory segment, that is, i=0, the initial speed variable V in the current trajectory segment speed information vel_arr[i] is set to s Assign 0. If the trajectory segment i is not the first trajectory segment, that is, i>0, the initial velocity variable V in vel_arr[i] needs to be set according to the velocity information vel_arr[i-1] of the trajectory segment i-1. s , then the final velocity variable V in vel_arr[i-1] e The value is assigned to the initial velocity variable V in the velocity information vel_arr[i] of the current trajectory segment s ; After the assignment is completed, the initial velocity variable V in vel_arr[i] s Make a judgment, if V s = 0, using the general polynomial acceleration and deceleration motion law in the acceleration phase, according to the actual path length S r With initial velocity V s , final velocity V e And the maximum acceleration a m relationship The estimated initial velocity is V s The final velocity V of trajectory segment i is e_t , L s , L r and L e is the initial transition arc length variable, the middle arc length variable, and the end transition arc length in vel_arr[i]; if V e_t Less than the final velocity V in the current trajectory segment vel_arr[i] e , which means that the length of the current trajectory segment cannot meet the speed from V s =0 Accelerate to V e , the final velocity V e_t Assign to V in vel_arr[i] e , if V e_t Greater than V in the current trajectory segment vel_arr[i] e , then maintain V e The value remains unchanged; if V s ≠0, judge V s Is the initial velocity upper and lower bounds [V min V max ], if V s In the interval [V min V max ], it means that the current distance can meet V s By accelerating and decelerating to reach V e , so keep V in vel_arr[i] e The value remains unchanged. If it is not in the range, the estimated final velocity V e_t Assign to V in vel_arr[i] e , for the initial velocity V in vel_arr[i] s and final velocity V e After fine-tuning is completed, the velocity preprocessing module stores the velocity information vel_arr[i] of the trajectory segment into the velocity information storage module and overwrites the velocity information vel_arr[i] of the principle; the pre-reading is continued until the second preprocessing of the velocity information of all trajectory segments is completed.

Images