Target following trajectory planning method and device, electronic device, storage medium

Abstract

The target following trajectory planning method and device, electronic device, and storage medium provided by the embodiments of the present disclosure belong to the technical field of trajectory planning. The target following trajectory planning method includes: estimating the real-time speed of the target to be followed, obtaining the first position and the first speed of the target to be followed in the reference coordinate system, calculating position following data and speed following data according to the first position and the first speed, determining the estimated completion status of the following task according to the preset following parameters, the position following data, and the speed following data, and superimposing the position following movement and the speed following movement according to the position following data and the speed following data to complete the target following task. In the technical solution provided by this embodiment, the accuracy of the following trajectory can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of trajectory planning, and in particular, to a trajectory planning method and device for target following, an electronic device, and a storage medium. Background Art

[0002] The dynamic following of a conveyor belt has extensive applications in industrial sites. In scenarios such as assembly lines where workpieces are dense and high requirements are placed on the operation rhythm, the dynamic following function is particularly important. Compared with static operations, the dynamic following of a conveyor belt greatly improves efficiency. However, the common conveyor belt dynamic following solutions have the following problems: (1) Ideally, the conveyor belt moves at a constant speed. However, due to factors such as servo following performance and the mechanical structure of the conveyor belt, the conveyor belt does not move at a constant speed during actual operation, and its speed fluctuates irregularly. It is difficult to guarantee the following accuracy of a following solution that assumes the conveyor belt speed is a constant speed; (2) The trajectory planning method for dynamic following is relatively complex. It is necessary to obtain the robot joint speeds through Jacobian matrix transformation, perform speed planning for each robot joint separately (with a non-zero end speed), and then perform time synchronization; (3) Usually, during dynamic following, variable speed or sudden stop of the conveyor belt is not supported. Summary of the Invention

[0003] The main purpose of the embodiments of the present disclosure is to propose a trajectory planning method and device for target following, an electronic device, and a storage medium to improve the accuracy of the following trajectory.

[0004] To achieve the above object, a first aspect of the embodiments of the present disclosure proposes a trajectory planning method for target following, including:

[0005] Obtaining a first position and a first speed of the target to be followed in a reference coordinate system;

[0006] Calculating position following data and speed following data according to the first position and the first speed;

[0007] Determining an estimated completion status of a following task according to preset following parameters, the position following data, and the speed following data;

[0008] Completing the target following task by superimposing the position following motion and the speed following motion according to the position following data and the speed following data.

[0009] In some embodiments, the method further includes:

[0010] Comparing the position following data and the speed following data with a preset accuracy;

[0011] If the position following data and the speed following data reach the preset accuracy, control to grasp the target to be followed or perform relevant process processing.

[0012] In some embodiments, the method further includes:

[0013] If the estimated completion status of the following task is a completable status, according to the position following data and the speed following data, superimpose the position following motion and the speed following motion to complete the target following task; if the estimated completion status of the following task is not a completable status, do not execute the target following task.

[0014] In some embodiments, estimating the real-time speed of the target to be followed includes:

[0015] Calibrate the encoder pulse equivalent of the conveyor belt;

[0016] Calculate the real-time position of the target to be followed according to the encoder pulse equivalent;

[0017] Estimate the real-time speed of the target to be followed according to the real-time position of the target to be followed.

[0018] In some embodiments, estimating the real-time speed of the target to be followed further includes:

[0019] Perform online low-pass filtering on the real-time speed of the target to be followed.

[0020] In some embodiments, the first speed is the end speed of the speed following motion, the speed following data includes the speed following time and the maximum speed of speed following, the first position is the end position of the position following motion, the position following data includes the position following time and the maximum speed of position following, and calculating the position following data and the speed following data according to the first position and the first speed includes:

[0021] Calculate the position following time and the maximum speed of position following according to the current position of the actuator and the first position;

[0022] Calculate the speed following time and the maximum speed of speed following according to the relationship that the motion distance of the end of the actuator is equal to the motion distance of the following target;

[0023] Correct the speed following time;

[0024] Correct the maximum speed of speed following according to the corrected speed following time.

[0025] To achieve the above object, a second aspect of the embodiments of the present disclosure provides a trajectory planning device for target following, including:

[0026] An estimation module, configured to estimate the real-time speed of the target to be followed;

[0027] An acquisition module, configured to acquire the first position and the first speed of the target to be followed in a reference coordinate system;

[0028] A calculation module, configured to calculate position following data and speed following data according to the first position and the first speed;

[0029] A following task determination module, configured to determine the estimated completion status of the following task according to preset following parameters, the position following data, and the speed following data;

[0030] A following task execution module, configured to superimpose the position following motion and the speed following motion according to the position following data and the speed following data to complete the target following task.

[0031] To achieve the above object, a third aspect of the embodiments of the present disclosure provides an electronic device, including:

[0032] At least one memory;

[0033] At least one processor;

[0034] At least one program;

[0035] The program is stored in the memory, and the processor executes the at least one program to implement the method as described in the first aspect above.

[0036] To achieve the above object, a fourth aspect of the embodiments of the present disclosure provides a computer-readable storage medium, the computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to cause a computer to execute:

[0037] The method as described in the first aspect above.

[0038] The trajectory planning method and device, electronic device, and storage medium for target following proposed by the embodiments of the present disclosure estimate the real-time speed of the target to be followed, acquire the first position and the first speed of the target to be followed in a reference coordinate system, calculate position following data and speed following data according to the first position and the first speed, determine the estimated completion status of the following task according to preset following parameters, position following data, and speed following data, and superimpose the position following motion and the speed following motion according to the position following data and the speed following data to complete the target following task, thereby improving the accuracy of the following trajectory. Description of the Drawings

[0039] Figure 1It is a flowchart of the trajectory planning method for target following provided by an embodiment of the present disclosure.

[0040] Figure 2 Yes Figure 1 It is a flowchart of step 101 in

[0041] Figure 3 It is a schematic diagram of the trajectory planning method for target following provided by an embodiment of the present disclosure applied to a specific application scenario.

[0042] Figure 4 It is a graph of the relationship between speed and time in speed following in the trajectory planning method for target following provided by an embodiment of the present disclosure.

[0043] Figure 5 It is a flowchart of the trajectory planning method for target following provided by an embodiment of the present disclosure applied to a specific application scenario. Detailed implementation manners

[0044] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0045] It should be noted that although functional module division is performed in the device schematic diagram and the logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in a different order from the module division in the device or the flowchart. Terms such as "first" and "second" in the specification, claims and the above-mentioned drawings are used to distinguish similar objects and do not necessarily need to describe a specific order or sequence.

[0046] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs. The terms used herein are only for the purpose of describing embodiments of the present invention and are not intended to limit the present invention.

[0047] Embodiments of the present disclosure are commonly used in industrial production lines or laboratories, etc. In an actual production line, it is usually necessary for a robot to perform operations such as picking up, spraying, and gluing on a target to be followed (such as a workpiece) on a conveyor belt. Therefore, it is necessary to perform dynamic following on the target object on the conveyor belt. Due to factors such as servo following performance and the mechanical structure of the conveyor belt, the conveyor belt does not move at a constant speed during actual operation, and the speed has irregular fluctuations. The positions and speeds of moving target objects may both change in real time, and it is difficult to guarantee the accuracy of a following scheme that assumes the conveyor belt speed is a uniform motion. Moreover, usually during dynamic following, the conveyor belt may change speed or suddenly stop. Therefore, it is necessary to design a reasonable robot following trajectory.

[0048] Based on this, the embodiments of the present disclosure provide a trajectory planning method and device for target following, an electronic device, and a computer-readable storage medium, which will be specifically described through the following embodiments. First, the trajectory planning method for target following in the embodiments of the present disclosure will be described.

[0049] The trajectory planning method for target following provided by the embodiments of the present disclosure can be applied to a terminal, or to a server side, or can also be software running on a terminal or a server side. In some embodiments, the terminal can be a smart phone, a tablet computer, a laptop computer, a desktop computer, or a smart watch, etc.; the server side can be configured as an independent physical server, or can be configured as a server cluster or a distributed system composed of multiple physical servers, or can also be configured as a cloud server providing basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communications, middleware services, domain name services, security services, CDN, and big data and artificial intelligence platforms; the software can be an application that implements the trajectory planning method for target following, etc., but is not limited to the above forms.

[0050] Figure 1 is an optional flowchart of the trajectory planning method for target following provided by the embodiments of the present disclosure. Figure 1 The method in [[ ]] includes steps 101 to 105.

[0051] Step 101, estimate the real-time speed of the target to be followed;

[0052] Step 102, obtain the first position and the first speed of the target to be followed in the reference coordinate system;

[0053] Step 103, calculate the position following data and the speed following data according to the first position and the first speed;

[0054] Step 104, determine the estimated completion status of the following task according to the preset following parameters, the position following data, and the speed following data;

[0055] Step 105, perform motion superposition of the position following motion and the speed following motion according to the position following data and the speed following data to complete the target following task.

[0056] In some embodiments, the target to be followed can be a target object on a conveyor belt, such as a workpiece. The reference coordinate system can be a user coordinate system. In this embodiment, the user coordinate system will be taken as an example for description.

[0057] Before executing step 101, the trajectory planning method for target following further includes:

[0058] Calibrate the conveyor belt encoder pulse equivalent, calibrate the camera, calibrate the user coordinate system, and set process parameters;

[0059] Trigger the camera to take a picture.

[0060] In some embodiments, the camera may be an industrial camera, and the embodiments of the present disclosure do not limit this. The process parameters may include the process time. In this embodiment, the process time is taken as an example for illustration.

[0061] Please refer to Figure 2 , in some embodiments, step 101 includes steps 201 to 203.

[0062] Step 201: Calibrate the encoder pulse equivalent of the conveyor belt;

[0063] Step 202: Calculate the real-time position of the target to be followed according to the encoder pulse equivalent;

[0064] Step 203: Estimate the real-time speed of the target to be followed according to the real-time position of the target to be followed.

[0065] In some embodiments, step 101 further includes step 204.

[0066] Step 204: Perform online low-pass filtering on the real-time speed of the target to be followed. Since the speed of the conveyor belt will fluctuate, an online low-pass filter is used to perform real-time filtering on the speed of the target to be followed, thereby improving the accuracy of calculating the real-time speed of the target to be followed.

[0067] In step 201 of some embodiments, the encoder pulse equivalent is the calibrated encoder pulse equivalent.

[0068] In step 202 of some embodiments, the real-time position of the workpiece is converted by using the calibrated encoder pulse equivalent.

[0069] In step 203 of some embodiments, numerical differentiation calculation is performed on the real-time position of the target to be followed to estimate the real-time speed of the target to be followed.

[0070] Please refer to Figure 3 , assuming that when the camera is triggered to take a picture of the target to be followed, the position of the target to be followed is A (the inverse solution corresponding to the robot is q A ), the speed is v e , and the end position of the robot is B (the inverse solution corresponding to the robot is q B ). The dynamic following task is decomposed into position following and speed following. Among them, the position following is a point-to-point movement with the starting point being B, the ending point being A, and both the starting speed and the ending speed being 0; the speed following is a linear interpolation movement with the starting point being A, the starting speed being v s , and the ending speed being v e .

[0071] In some embodiments, the shape of the conveyor belt can be an arc, a ring, or other shapes, which are not limited in the embodiments of the present disclosure. If the conveyor belt is in the shape of an arc, only the linear interpolation in the velocity following needs to be replaced with circular interpolation.

[0072] In some embodiments, the robot can be a serial horizontal multi-joint robot, a serial vertical multi-joint robot, or a parallel multi-joint robot, which are not limited in the embodiments of the present disclosure.

[0073] In step 103 of some embodiments, velocity planning is performed on the main axis of the point-to-point motion to obtain the position following time, velocity planning is performed on the linear motion to obtain the velocity following time, and the longer of the two times is taken as the current total following time. According to the preset dynamic grasping area, process parameters, and the real-time speed of the conveyor belt, it is predicted whether the current dynamic following task can be completed. Among them, the dynamic grasping area and process parameters can be set according to user needs.

[0074] In some embodiments, the first velocity is the end velocity v of the velocity following motion e , the velocity following data includes the velocity following time and the maximum velocity of velocity following, the first position is the end position q of the position following motion A , the position following data includes the position following time and the maximum velocity of position following, and step 103 includes:

[0075] Calculate the position following time and the maximum velocity of position following according to the current joint position of the robot and the joint position of the robot corresponding to the workpiece position;

[0076] Calculate the velocity following time and the maximum velocity of velocity following according to the relationship that the moving distance of the end tool center of the robot is equal to the moving distance of the workpiece;

[0077] Correct the velocity following time;

[0078] Correct the maximum velocity of velocity following according to the corrected velocity following time.

[0079] Among them, the current position of the actuator is the starting position of position following, and the first position is the end position of position following.

[0080] Please refer to Figure 4 , in some embodiments, taking the linear motion of the conveyor belt as an example for illustration, the maximum velocity of velocity following is the linear maximum velocity, the acceleration is the linear acceleration, the deceleration is the linear deceleration, and the principle of velocity following is as follows:

[0081] If v s < v e, according to the fact that the linear motion distance of the robot's end - effector tool center is equal to the linear motion distance of the workpiece, the formula (1) is obtained:

[0082]

[0083] Where, T is the velocity - following time, acc is the linear acceleration, dec is the linear deceleration, and v m is the maximum linear velocity.

[0084] Simplify formula (1) to obtain formula (2):

[0085]

[0086] Regard the above - mentioned formula (2) as a quadratic equation of one variable about v m . To ensure that the equation always has real roots, formula (3) must be satisfied:

[0087]

[0088] Simplify the above - mentioned formula (3) to obtain formula (4):

[0089]

[0090] Solve to obtain formula (5): Where, discard the negative value,

[0091]

[0092] According to formula (5), obtain the minimum value At this time, there are two equal real roots in the above - mentioned formula (2). Solve to obtain formula (6):

[0093]

[0094] Similarly, if v s > v e , the above - mentioned calculation method can be referred to for calculation.

[0095] Combining the two cases of v s < v e and v s > v e , introduce the sign function to combine the two cases and calculate the velocity - following time T, referring to formula (7):

[0096]

[0097] Calculate the maximum linear velocity v m according to formula (8):

[0098]

[0099] According to formula (9), the speed following time T is corrected to an integer multiple of the interpolation period dt:

[0100]

[0101] According to formula (10), correct the maximum linear speed v m :

[0102]

[0103] If the robot can complete the current dynamic following task, then the point-to-point motion and the linear interpolation motion are combined to obtain the joint positions of the robot, referring to formula (11):

[0104] q(t) = q B + s(t)(q A - q B ) / L + (q temp (t) - q B ) (11)

[0105] where q B is the joint position of the robot corresponding to the position of the tool center point B at the end of the robot, qA is the joint position of the robot corresponding to the target position A to be followed, L is the main axis length of the point-to-point motion, s(t) is the main axis position of the point-to-point motion, and q temp (t) is the position obtained by linear interpolation and then converted into the joint position of the robot by the inverse kinematics of the robot.

[0106] When L is small, the position following has reached the set accuracy. At this time, there is only speed following, and the joint position of the robot is expressed as formula (12):

[0107] q(t) = q temp (t) (12)

[0108] In some embodiments, the trajectory planning method for target following further includes:

[0109] If the estimated completion status of the following task is a completable status, then according to the position following data and the speed following data, the position following motion and the speed following motion are superimposed to complete the target following task;

[0110] If the estimated completion status of the following task is not a completable status, then the target following task is not executed.

[0111] In some embodiments, the trajectory planning method for target following further includes:

[0112] Compare the position following data, the speed following data with the preset accuracy;

[0113] If the position following data and the speed following data reach the preset accuracy, control the robot to grasp the target to be followed or perform relevant process operations.

[0114] In the embodiments of the present disclosure, by continuously performing position following and speed following until the position following and speed following meet the preset accuracy, it is considered that the tool center at the end of the robot is synchronized with the workpiece on the conveyor belt; and after synchronization, the robot can grasp the target to be followed or perform relevant process operations. Among them, the relevant process operations can be spraying operations; the relevant process operations can also be gluing operations.

[0115] The trajectory planning method for target following provided by the embodiments of the present disclosure estimates the real-time speed of the target to be followed, obtains the first position and the first speed of the target to be followed in the reference coordinate system, calculates the position following data and the speed following data according to the first position and the first speed, determines the estimated completion status of the following task according to the preset following parameters, the position following data, and the speed following data, and superimposes the position following motion and the speed following motion according to the position following data and the speed following data to complete the target following task. The technical solution provided by the embodiments of the present disclosure is highly efficient and accurate. It can be achieved through the above method without the need to additionally increase hardware devices, nor does the robot need to dynamically follow the workpiece for a certain distance and then the conveyor belt decelerates and stops and finally grasps the workpiece. It can consider the speed fluctuation of the conveyor belt and support the conveyor belt to change speed during the following process.

[0116] Figure 5 It is a flowchart in an optional application scenario of the trajectory planning method for target following provided by the embodiments of the present disclosure, where the target to be followed is a workpiece and the camera is an industrial camera. Figure 5 The trajectory planning method for target following shown includes the following steps:

[0117] Calibrate the encoder pulse equivalent of the conveyor belt, calibrate the camera, calibrate the user coordinate system, and preset the process time.

[0118] Trigger the camera to take a picture.

[0119] Obtain the real-time position of the conveyor belt encoder in real time, calculate the real-time position of the workpiece according to the encoder pulse equivalent, perform numerical differentiation on the real-time position of the workpiece, estimate the real-time speed of the workpiece, and perform online low-pass filtering processing.

[0120] Record the position and speed of the workpiece in the user coordinate system.

[0121] Obtain the position following duration and the speed following duration, and calculate the current total following time according to the position following duration and the speed following duration; specifically, perform speed planning on the main axis of the point-to-point motion to obtain the position following duration, perform speed planning on the linear motion to obtain the speed following duration, and take the longer of the two times as the current total following time.

[0122] Predict whether the current dynamic following task can be completed according to the preset dynamic grabbing area, process time, and real-time speed of the conveyor belt;

[0123] If it can be completed, the point-to-point motion and linear interpolation motion are synthesized, and the robot dynamically follows the workpiece on the conveyor belt; otherwise, execute the above step of "triggering the camera to take a picture";

[0124] Judge whether the position following and speed following reach the preset accuracy;

[0125] If it is judged that both the position following and speed following reach the preset accuracy, grab the workpiece or perform relevant process processing through superimposed motion; otherwise, execute the above step of "triggering the camera to take a picture";

[0126] In the trajectory planning method for target following provided by the embodiments of the present disclosure, the dynamic following of the target is regarded as the synthesis of point-to-point motion (position following) and linear motion (speed following). Perform speed planning on the main axis of the point-to-point motion, perform speed planning on the linear motion, and take the longer of the duration of the point-to-point motion and the duration of the linear motion as the current total following time. Determine the estimated completion status of the following task according to the preset dynamic grabbing area, process time, and real-time speed of the conveyor belt. If the estimated completion status is a completable status, the point-to-point motion and linear interpolation motion (taking the conveyor belt as an example of linear motion for illustration) are synthesized, and the robot dynamically follows the target to be followed on the conveyor belt (such as a workpiece). Continuously perform motion iteration, following the position and speed of the workpiece. When the position following and speed following reach the set accuracy, grab the workpiece or perform relevant process processing.

[0127] In the technical solution of the trajectory planning method for target following provided by the embodiments of the present disclosure, during the dynamic following process, the robot never stops, the speed is smooth and coherent, and the following action is completed in one go, so the speed is fast; considering the speed fluctuation of the conveyor belt, a low-pass online filter is used to perform real-time filtering on the speed of the workpiece, and the real-time position and speed of the workpiece are accurately calculated. The dynamic following is decomposed into position following and speed following. Once there is a deviation between the position or speed of the tool center at the end of the robot and the position or speed of the workpiece on the conveyor belt, position following or speed following is automatically performed. Since the speed of the conveyor belt will fluctuate, ordinary single following is difficult to ensure the following accuracy. The present invention continuously iterates the motion of the robot, continuously follows and approaches the position and speed of the workpiece, with high accuracy; it is applicable to the conveyor belt dynamic following operations of various configurations of automated equipment, and can basically be applicable to the conveyor belt dynamic following operations of all current configurations of automated equipment, with good versatility; it can be used for conveyor belts of different shapes. If the conveyor belt is arc-shaped, only the linear interpolation in the speed following module needs to be replaced with arc interpolation, so program modularization can be achieved; the dynamic following process supports variable speed or sudden stop of the conveyor belt.

[0128] The embodiments of the present disclosure also provide a trajectory planning device for target following, which can implement the above-mentioned trajectory planning method for target following. The device includes:

[0129] An estimation module for estimating the real-time speed of the target to be followed;

[0130] An acquisition module for acquiring the first position and the first speed of the target to be followed in the reference coordinate system;

[0131] A calculation module for calculating the position following data and the speed following data according to the first position and the first speed;

[0132] A following task determination module for determining the estimated completion status of the following task according to the preset following parameters, the position following data, and the speed following data;

[0133] A following task execution module for superimposing the position following motion and the speed following motion according to the position following data and the speed following data to complete the target following task.

[0134] The embodiments of the present disclosure also provide a trajectory planning device for target following according to another embodiment, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the computer program, the method steps 101 to 105 described above are implemented, Figure 1 the method steps 201 to 204 in, Figure 2 the method steps in. Figure 5 The embodiments of the present disclosure also provide an electronic device, including:

[0135] At least one memory;

[0136] At least one processor;

[0137] At least one program;

[0138] The program is stored in the memory, and the processor executes the at least one program to implement the above-mentioned trajectory planning method for target following of the present disclosure. The electronic device can be any intelligent terminal including a mobile phone, a tablet computer, a personal digital assistant (Personal Digital Assistant, abbreviated as PDA), a point of sales (Point of Sales, abbreviated as POS), an in-vehicle computer, etc.

[0139] The embodiments of the present disclosure also provide a storage medium, which is a computer-readable storage medium, and the computer-executable instructions are used to execute the above-mentioned trajectory planning method for target following.

[0140]

[0141] ​The trajectory planning method for target following, the trajectory planning device for target following, the electronic device, and the storage medium proposed in the embodiments of the present disclosure estimate the real-time speed of the target to be followed, obtain the first position and the first speed of the target to be followed in the reference coordinate system, calculate the position following data and the speed following data according to the first position and the first speed, determine the estimated completion status of the following task according to the preset following parameters, the position following data, and the speed following data, and superimpose the position following motion and the speed following motion according to the position following data and the speed following data to complete the target following task. The technical solution proposed in the embodiments of the present disclosure can improve the accuracy of the following trajectory; and regard the dynamic following of the target as the synthesis of point-to-point motion (position following) and linear motion (speed following); perform speed planning on the main axis of the point-to-point motion and perform speed planning on the linear motion; determine the estimated completion status of the following task according to the preset dynamic grasping area, process time, and the real-time speed of the conveyor belt. If the estimated completion status is a completable status, the point-to-point motion and the linear interpolation motion (taking the conveyor belt as an example of the linear motion) are synthesized, and the robot dynamically follows the target to be followed (such as a workpiece) on the conveyor belt. Through continuous motion iteration, following the position and speed of the workpiece, when the position following and the speed following reach the preset accuracy, the workpiece is grasped or relevant process processing is performed.

[0142] As a non-transitory computer-readable storage medium, the memory can be used to store non-transitory software programs and non-transitory computer-executable programs. In addition, the memory can include high-speed random access memory, and can also include non-transitory memory, such as at least one magnetic disk storage device, a flash memory device, or other non-transitory solid-state storage devices. In some embodiments, the memory can optionally include a memory remotely set relative to the processor, and these remote memories can be connected to the processor through a network. Examples of the above network include but are not limited to the Internet, an enterprise intranet, a local area network, a mobile communication network, and combinations thereof.

[0143] The embodiments described in the embodiments of the present disclosure are for more clearly illustrating the technical solutions of the embodiments of the present disclosure, and do not constitute a limitation on the technical solutions provided by the embodiments of the present disclosure. Those skilled in the art can know that with the evolution of technology and the emergence of new application scenarios, the technical solutions provided by the embodiments of the present disclosure are equally applicable to similar technical problems.

[0144] Those skilled in the art can understand that Figure 1-2 and Figure 5 the trajectory planning method for target following shown in

[0145] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0146] Those of ordinary skill in the art can understand that all or some of the steps in the methods disclosed above, and the functional modules / units in the systems and devices, can be implemented as software, firmware, hardware, and their appropriate combinations.

[0147] It should be understood that in this application, the terms "first", "second", "third", "fourth", etc. (if any) in the specification and the above-mentioned drawings are used to distinguish similar objects and do not necessarily need to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged under appropriate circumstances so that the embodiments of this application described here can be implemented in an order other than those illustrated or described here. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products, or devices.

[0148] It should be understood that in this application, "at least one (item)" means one or more, and "a plurality" means two or more. "And / or" is used to describe the association relationship of associated objects and indicates that there can be three relationships. For example, "A and / or B" can mean: only A exists, only B exists, and both A and B exist at the same time. Among them, A and B can be singular or plural. The character " / " generally means that the associated objects before and after are in an "or" relationship. "At least one (one) of the following" or its similar expression refers to any combination of these items, including any combination of single item (one) or plural items (ones). For example, at least one (one) of a, b, or c can mean: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", where a, b, and c can be single or multiple.

[0149] In several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of the units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed coupling, direct coupling, or communication connection between each other can be through some interfaces. The indirect coupling or communication connection of the devices or units can be in electrical, mechanical, or other forms.

[0150] The units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they may be located in one place, or may be distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0151] In addition, in each embodiment of the present application, the functional units can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above-mentioned integrated units can be implemented in the form of hardware or in the form of software functional units.

[0152] If the above-mentioned integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, in essence, or the part that contributes to the prior art, or all or part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes multiple instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in each embodiment of the present application. The foregoing storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROM), random access memories (RAM), magnetic disks, or optical discs that can store programs.

[0153] The preferred embodiments of the present disclosure have been described above with reference to the accompanying drawings, but this does not limit the scope of rights of the present disclosure. Any modifications, equivalent replacements, and improvements made by those skilled in the art without departing from the scope and essence of the present disclosure should be within the scope of rights of the present disclosure.

Claims

1. A trajectory planning method for target following, characterized in that, it includes: Estimate the real-time speed of the target to be followed on the conveyor belt; wherein, the conveyor belt is used for linear motion; Obtain the first position and the first speed of the target to be followed in the reference coordinate system; Calculate the position following data and the speed following data according to the first position and the first speed; According to the position following data and the speed following data, superimpose the position following motion and the speed following motion to complete the target following task; Wherein, the first speed is the end speed of the speed following motion, the speed following data includes the speed following time and the maximum speed of speed following, the first position is the position following end position of the position following motion, the position following data includes the position following time and the maximum speed of position following, and calculating the position following data and the speed following data according to the first position and the first speed includes: According to the current joint position of the robot and the position following end position, calculate the absolute value of the position change of each joint of the robot respectively, and perform speed planning on the absolute value of the position change of each joint of the robot respectively to obtain the motion time and the maximum speed of each joint of the robot. The longest motion time of each joint of the robot is the position following time, and the maximum speed of the corresponding speed planning is the maximum speed of position following; wherein, the current joint position of the robot is the position following start position; Obtain the first formula according to the relationship that the moving distance of the end tool center of the robot is equal to the moving distance of the following target: ， Among them, is the starting speed of the speed following motion, is the ending speed of the speed following motion, is the maximum speed of the speed following, is the time of the speed following, is the acceleration of the speed following, is the deceleration of the speed following; The second formula obtained by simplifying the first formula is: ， Wherein, the second formula is a quadratic equation with one variable, the quadratic equation has real roots, and the condition satisfied by the discriminant of the roots of the quadratic equation is the third formula: ， The fourth formula obtained by simplifying the third formula is: ， Calculate the speed following time to obtain the fifth formula: ， Among them, , ; Calculate the maximum speed of speed following to obtain the sixth formula: ， wherein, is the sign function; The speed is corrected according to the time and corrected to an integer multiple of the interpolation period of the control system to obtain a seventh formula: ， wherein, is the ceiling function; Correct the maximum speed of speed following according to the corrected speed following time to obtain the eighth formula: 。 2. The method according to claim 1, characterized in that, it further includes: Compare the position following data and the speed following data with a preset accuracy; If the position following data and the speed following data reach the preset accuracy, control to grasp the target to be followed or perform relevant process treatment.

3. The method according to claim 1, characterized in that, it further includes: If the estimated completion status of the following task is a completable status, then according to the position following data and the speed following data, superimpose the position following motion and the speed following motion to complete the target following task; If the estimated completion status of the following task is not a completable status, then do not execute the target following task.

4. The method according to claim 1, characterized in that, estimating the real-time speed of the target to be followed includes: Calibrate the pulse equivalent of the conveyor belt encoder; Calculate the real-time position of the target to be followed according to the encoder pulse equivalent; Estimate the real-time speed of the target to be followed based on the real-time position of the target to be followed, and perform online low-pass filtering processing on the real-time speed of the target to be followed.

5. A trajectory planning device for target following, characterized in that, it includes: An estimation module for estimating the real-time speed of the target to be followed located on the conveyor belt; wherein, the conveyor belt is used for linear motion; An acquisition module for acquiring the first position and the first speed of the target to be followed in the reference coordinate system; A calculation module for calculating position following data and speed following data according to the first position and the first speed; A following task execution module for superimposing the position following motion and the speed following motion according to the position following data and the speed following data to complete the target following task; wherein, the first speed is the end speed of the speed following motion, the speed following data includes the speed following time and the maximum speed of speed following, the first position is the position following end position of the position following motion, the position following data includes the position following time and the maximum speed of position following, and the calculation module is used for calculating the position following data and the speed following data according to the first position and the first speed, including: According to the current joint position of the robot and the position following end position, calculate the absolute value of the position change of each joint of the robot respectively, and perform speed planning on the absolute value of the position change of each joint of the robot respectively to obtain the motion time and the maximum speed of each joint of the robot. The longest motion time of each joint of the robot is the position following time, and the corresponding maximum speed of the speed planning is the maximum speed of position following; wherein, the current joint position of the robot is the position following start position; Obtain the first formula according to the relationship that the moving distance of the end tool center of the robot is equal to the moving distance of the following target: ， Among them, is the starting speed of the speed following motion, is the ending speed of the speed following motion, is the maximum speed of the speed following, is the time of the speed following, is the acceleration of the speed following, is the deceleration of the speed following; The second formula obtained by simplifying the first formula is: ， wherein, the second formula is a quadratic equation of one variable, the quadratic equation of one variable has real roots, and the condition satisfied by the discriminant of the roots of the quadratic equation of one variable is the third formula: ， The fourth formula obtained by simplifying the third formula is: ， Calculate the speed following time to obtain the fifth formula: ， Among them, , ; Calculate the maximum speed of speed following to obtain the sixth formula: ， wherein, is the sign function; The speed is corrected according to the time and corrected to an integer multiple of the interpolation period of the control system to obtain a seventh formula: ， wherein, is the ceiling function; Correct the maximum speed of speed following according to the corrected speed following time to obtain the eighth formula: 。 6. An electronic device, characterized in that, it includes: At least one memory; At least one processor; At least one program; The program is stored in the memory, and the processor executes the at least one program to implement the method according to any one of claims 1 to 4.

7. A storage medium, the storage medium is a computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to cause a computer to execute: The method according to any one of claims 1 to 4.

Images