Trajectory planning method and apparatus

Abstract

Embodiments of the present disclosure relate to the technical field of robots, and provide a trajectory planning method and apparatus. The method comprises: upon receipt of a pause instruction in a movement process of a robot, determining a first target position of a first target trajectory point, the current positional velocity of the tail of the robot, and the current position, wherein in an original positional trajectory corresponding to the current movement, the execution moment of the current trajectory point is earlier than the execution moment of the first target trajectory point; and according to the requirement that the positional velocity at the first target trajectory point is 0, on the basis of the current movement, the current positional velocity, the current position, and the first target position, making planning to obtain pause position trajectory planning information corresponding to a pause process, wherein the start point of a pause position trajectory is the current position, the end point is the first target position, and the pause position trajectory planning information comprises correspondences between times and positions. In this way, accidents such as tail tool collision and robot collision can be effectively avoided, and the operation stability and safety of the robot are greatly improved.

Description

Trajectory planning method and device

[0001] CROSS-REFERENCE TO RELATED APPLICATIONS

[0002] This disclosure claims priority to Chinese patent application number 2023111746749, entitled “Trajectory Planning Method and Device,” filed with the State Intellectual Property Office of China on September 12, 2023, the entire contents of which are incorporated by reference into this disclosure. Technical Field

[0003] The present disclosure relates to the field of robotics technology, and in particular to a trajectory planning method and device. Background Art

[0004] During normal robot operation, some requirements may necessitate the robot pausing its current motion. If each joint is paused by reducing its velocity to zero through maximum acceleration, and each joint is controlled independently, there's no guarantee that the Cartesian trajectory (i.e., the actual position trajectory during the pause) will be consistent with the original position trajectory during normal motion corresponding to the pause (i.e., the segment of the original position trajectory corresponding to the current motion that corresponds to the time range of the pause). This can cause the robot's position trajectory to change, potentially leading to accidents such as end-of-line tool collisions (especially when the end-of-line tool contacts the workpiece) and robot collisions (especially when the robot is operating in a confined space or surrounded by objects).

[0005] Summary of the Invention

[0006] The embodiments of the present disclosure provide a trajectory planning method, device, electronic device and readable storage medium, which can ensure the consistency of the pause position trajectory and the original position trajectory segment corresponding to the pause period on the basis of achieving pause, without any change in the motion trajectory, and can effectively avoid the occurrence of accidents such as end tool collision and robot collision, thereby greatly improving the stability and safety of robot operation.

[0007] The embodiments of the present disclosure may be implemented as follows:

[0008] In a first aspect, an embodiment of the present disclosure provides a trajectory planning method, the method comprising:

[0009] When a pause instruction is received during the robot motion, determining a first target position, a current position speed, and a current position of the robot end, wherein the first target position is a position of a first target trajectory point in an original position trajectory corresponding to the current motion performed by the robot, and in the original position trajectory, an execution time of the current trajectory point is earlier than an execution time of the first target trajectory point;

[0010] With the requirement that the first target position speed of the first target trajectory point is 0, according to the current motion, current position speed, current position and first target position, the pause position trajectory planning information corresponding to the pause process is planned, wherein the starting point of the pause position trajectory corresponding to the pause position trajectory planning information is the current position and the end point is the first target position, and the pause position trajectory planning information includes the correspondence between time and position.

[0011] In a second aspect, an embodiment of the present disclosure provides a trajectory planning device, the device comprising:

[0012] an information acquisition module, configured to determine, upon receiving a pause instruction during robot motion, a first target position, a current position velocity, and a current position of the robot end, wherein the first target position is a position of a first target trajectory point in an original position trajectory corresponding to the current motion performed by the robot, and an execution time of the current trajectory point in the original position trajectory is earlier than an execution time of the first target trajectory point;

[0013] A planning module is configured to obtain pause position trajectory planning information corresponding to the pause process based on the requirement that the first target position speed of the first target trajectory point is 0, according to the current motion, current position speed, current position and first target position, wherein the pause position trajectory corresponding to the pause position trajectory planning information has a starting point of the current position and an end point of the first target position, and the pause position trajectory planning information includes a correspondence between time and position.

[0014] In a third aspect, an embodiment of the present disclosure provides an electronic device, including a processor and a memory, wherein the memory stores machine-executable instructions that can be executed by the processor, and the processor can execute the machine-executable instructions to implement the trajectory planning method described in the aforementioned embodiment.

[0015] In a fourth aspect, an embodiment of the present disclosure provides a readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the trajectory planning method as described in the aforementioned embodiment.

[0016] The trajectory planning method, device, electronic device and readable storage medium provided by the embodiments of the present disclosure determine the first target position, the current position speed and current position of the robot end when a pause command is received during the movement of the robot. The first target position is the position of the first target trajectory point in the original position trajectory corresponding to the current movement executed by the robot. In the original position trajectory, the execution time of the current trajectory point is earlier than the execution time of the first target trajectory point. With the first target position speed of the first target trajectory point being 0 as the requirement, the pause position trajectory planning information corresponding to the pause process is planned based on the current movement, current position speed, current position and first target position. The starting point of the pause position trajectory corresponding to the pause position trajectory planning information is the current position and the end point is the first target position. The pause position trajectory planning information includes the correspondence between time and position. In this way, the consistency of the pause position trajectory and the original position trajectory segment corresponding to the pause period is guaranteed on the basis of achieving the pause, and there will be no change in the motion trajectory. This can effectively avoid accidents such as end tool collision and robot collision, and greatly improve the stability and safety of the robot operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the technical solutions of the embodiments of the present disclosure, the following briefly introduces the drawings required for use in the embodiments. It should be understood that the following drawings only illustrate certain embodiments of the present disclosure and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other relevant drawings can be obtained based on these drawings without creative work.

[0018] FIG1 is a schematic diagram of an original position trajectory segment and a pause position trajectory segment;

[0019] FIG2 is a block diagram of an electronic device provided by an embodiment of the present disclosure;

[0020] FIG3 is a flow chart of a trajectory planning method according to an embodiment of the present disclosure;

[0021] FIG4 is a flowchart of one of the sub-steps included in step S120 in FIG3 ;

[0022] FIG5 is a schematic flow chart of the sub-steps included in sub-step S122 in FIG4 ;

[0023] FIG6 is a schematic diagram of a position planning curve provided by an embodiment of the present disclosure;

[0024] FIG7 is a schematic diagram of a speed planning curve provided by an embodiment of the present disclosure;

[0025] FIG8 is a second flow chart of the sub-steps included in step S120 in FIG3 ;

[0026] FIG9 is a schematic flow chart of the sub-steps included in sub-step S126 in FIG8 ;

[0027] FIG10 is a second flow chart of the trajectory planning method provided by an embodiment of the present disclosure;

[0028] FIG11 is a third flow chart of the trajectory planning method provided in an embodiment of the present disclosure;

[0029] FIG12 is a fourth flow chart of the trajectory planning method provided in an embodiment of the present disclosure;

[0030] FIG13 is a block diagram of a trajectory planning device according to an embodiment of the present disclosure;

[0031] FIG14 is a second block diagram of the trajectory planning device provided in an embodiment of the present disclosure.

[0032] Icon: 100 - electronic device; 110 - memory; 120 - processor; 130 - communication unit; 200 - trajectory planning device; 210 - information acquisition module; 220 - planning module; 230 - control module. DETAILED DESCRIPTION

[0033] To make the objectives, technical solutions, and advantages of the embodiments of the present disclosure more clear, the technical solutions of the embodiments of the present disclosure will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present disclosure. Obviously, the described embodiments are only some of the embodiments of the present disclosure, but not all of them. Generally, the components of the embodiments of the present disclosure described and shown in the drawings herein can be arranged and designed in various different configurations.

[0034] Therefore, the following detailed description of the embodiments of the present disclosure provided in the accompanying drawings is not intended to limit the scope of the present disclosure as claimed, but merely represents selected embodiments of the present disclosure. Based on the embodiments of the present disclosure, all other embodiments obtained by those skilled in the art without making any creative efforts shall fall within the scope of protection of the present disclosure.

[0035] It should be noted that relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus comprising a series of elements includes not only those elements, but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of additional identical elements in the process, method, article, or apparatus comprising the element.

[0036] During normal robot operation, when the robot needs to stop temporarily, a pause operation is usually triggered. Currently, the pause is achieved by reducing the speed of each joint to zero through maximum acceleration. However, since each joint is controlled separately, this method cannot guarantee the consistency of the Cartesian space trajectory (i.e., the actual position trajectory of the pause process) with respect to the original position trajectory segment (i.e., the trajectory segment corresponding to the time range of the pause process in the original position trajectory corresponding to the current movement). This will lead to changes in the robot's motion trajectory, which can easily cause accidents such as end-tool collisions (especially when the end tool contacts the workpiece) and robot collisions (especially when the robot is operating in a narrow space or there are objects around the robot).

[0037] As shown in Figure 1, position trajectory segment 1 and position trajectory segment 2 are the original position trajectories. Assuming that the robot receives a pause operation at point M, it is necessary to plan trajectory segment 3 as the pause position trajectory segment for achieving the pause. Since the consistency of the path corresponding to the pause position trajectory segment 3 and the path corresponding to the position trajectory segment 2 cannot be guaranteed, that is, if the path corresponding to the pause position trajectory segment 3 is different from the path corresponding to the position trajectory segment 2, accidents such as collisions may occur. Among them, the trajectory segments 1, 2, and 3 shown in Figure 2 are only used to represent different trajectory segments, and are not used to represent specific path shapes. For example, trajectory end 3 is only used to represent the pause position trajectory segment, and is not used to limit the path corresponding to the trajectory segment to a straight line.

[0038] In response to the above situation, the embodiments of the present disclosure provide a trajectory planning method, device, readable storage medium and electronic device, which ensure the consistency of the pause position trajectory and the original position trajectory segment corresponding to the pause period on the basis of achieving pause, and will not cause changes in the motion trajectory. It can effectively avoid accidents such as end tool collision and robot collision, and greatly improve the stability and safety of robot operation.

[0039] The following describes some embodiments of the present disclosure in detail with reference to the accompanying drawings. In the absence of conflict, the following embodiments and features therein may be combined with each other.

[0040] Please refer to Figure 2, which is a block diagram of an electronic device 100 provided in an embodiment of the present disclosure. The electronic device 100 may be, but is not limited to, a computer, a robot control unit, etc. The electronic device 100 may include a memory 110, a processor 120, and a communication unit 130. The memory 110, processor 120, and communication unit 130 are electrically connected to each other directly or indirectly to enable data transmission or exchange. For example, these components may be electrically connected to each other via one or more communication buses or signal lines.

[0041] The memory 110 is used to store programs or data. The memory 110 may be, but is not limited to, a random access memory (RAM), a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), etc.

[0042] The processor 120 is used to read / write data or programs stored in the memory 110 and execute corresponding functions. For example, the memory 110 stores a trajectory planning device 200, which includes at least one software function module that can be stored in the memory 110 in the form of software or firmware. The processor 120 executes various functional applications and data processing by running software programs and modules stored in the memory 110, such as the trajectory planning device 200 in the embodiment of the present disclosure, thereby implementing the trajectory planning method in the embodiment of the present disclosure.

[0043] The communication unit 130 is used to establish a communication connection between the electronic device 100 and other communication terminals through a network, and to send and receive data through the network.

[0044] It should be understood that the structure shown in FIG2 is merely a schematic diagram of the structure of the electronic device 100, and the electronic device 100 may also include more or fewer components than shown in FIG2, or have a configuration different from that shown in FIG2. Each component shown in FIG2 may be implemented using hardware, software, or a combination thereof.

[0045] Please refer to Figure 3, which is a flow chart of a trajectory planning method provided in an embodiment of the present disclosure. The method can be applied to the electronic device 100 described above. The specific process of the trajectory planning method is described in detail below. In this embodiment, the method may include steps S110 to S120.

[0046] Step S110 , when a pause instruction is received during the movement of the robot, a first target position, a current position speed and a current position of the end of the robot are determined.

[0047] In this embodiment, the robot receives a pause command while executing its current motion. Optionally, the pause command can be triggered by a user manually operating an interface (e.g., an app interface), or by a user via a teach pendant or a host computer. Alternatively, the pause command can be automatically generated and sent to the electronic device by another device. The specific method for generating the pause command is not limited herein. Upon receiving the pause command, the planned motion command can be paused, and the first target position, the current position velocity V0 of the robot end point, and the current position can be obtained.

[0048] The first target position is the position of the first target trajectory point in the original position trajectory corresponding to the current motion of the robot. In the original position trajectory, the execution time of the current trajectory point is earlier than the execution time of the first target trajectory point, that is, the robot reaches the current trajectory point before the first target trajectory point.

[0049] The first target position can be determined based on a pause instruction. For example, the pause instruction directly includes the first target position. Alternatively, including the moment N1 specified by the user, the sum of the current moment N and the moment N1 can be used as the first target moment, and then the position corresponding to the first target moment in the original position trajectory can be used as the first target position. The moment N1 can be configured by the user through an interface, a teach pendant or a host computer, and carried in the pause instruction. Among them, the original position trajectory planning information corresponding to the original position trajectory can include the correspondence between the moment and the position, and after determining the first target moment, the position corresponding to the first target moment in the original position trajectory can be determined based on the correspondence.

[0050] The first target position can also be determined automatically. For example, a correspondence between different position speed ranges and the required pause time is pre-set. It is understandable that the greater the position speed, the longer the required pause time. The required time corresponding to the current position speed can be determined based on the correspondence between the current position speed, different position speed ranges and the required pause time, and then the first target moment is calculated based on the current moment and the determined required time, and the position corresponding to the first target moment in the original position trajectory is used as the first target position. Alternatively, a pause required time is directly pre-set, and when a pause instruction is received, the first target moment is directly calculated based on the pause required time and the current moment, and then the position corresponding to the first target moment in the original position trajectory is used as the first target position. Among them, the specific method of determining the first target position based on the first target moment can be the same as the above method, which will not be repeated here.

[0051] It is worth noting that the specific method for determining the first target position described above is for illustrative purposes only and can be determined based on actual needs. The robot supports reaching the first target position from the current position, and the position velocity is 0 at the first target position; that is, the robot's acceleration performance supports the robot reaching the first target position from the current position, and the position velocity is 0 at the first target position. In other words, for example, when setting the value of N1, N1 can be set based on the maximum velocity and maximum acceleration of the robot end point, and the value of N1 will not cause the position velocity of the first target position to be non-zero.

[0052] Step S120 , requiring that the first target position speed of the first target trajectory point be 0, and planning to obtain pause position trajectory planning information corresponding to the pause process according to the current motion, current position speed, current position and first target position.

[0053] Next, the first target position speed V1 of the first target trajectory point can be set to 0 as a requirement, and according to the first target position, the current position speed V0 of the robot end and the current position, combined with the current motion, the pause position trajectory planning information corresponding to the pause process can be planned. The starting point of the pause position trajectory corresponding to the pause position trajectory planning information is the current position and the end point is the first target position, and the pause position trajectory planning information includes the correspondence between time and position. That is, the pause position trajectory and the trajectory segment that has been executed when the pause command is received belong to the same motion, and the starting point of the pause position trajectory is the end point of the trajectory segment that has been executed when the pause command is received, and the end point of the pause position trajectory is the point that has not been reached in the original position trajectory when the pause command is received.

[0054] It can be understood that the correspondence between the moment and the position in the pause position trajectory planning information may be different from the correspondence between the moment and the position in the pause period (i.e., the time period used to achieve the pause) in the original position trajectory planning information, but the corresponding position trajectories are the same.

[0055] In this way, by performing trajectory consistency planning in the pause segment, the consistency of the pause position trajectory and the original position trajectory segment corresponding to the pause period can be guaranteed on the basis of achieving the pause, and there will be no change in the motion trajectory. It can effectively avoid accidents such as end tool collision and robot collision, and greatly improve the stability and safety of the robot operation.

[0056] Optionally, when performing pause planning, the pause path length information can be calculated based on the current motion, current position, and first target position, and then combined with the current position velocity V0 to obtain corresponding pause position trajectory planning information. Optionally, a pause can be achieved directly using uniform deceleration motion, motion that accelerates first and then decelerates, or other motion methods, which are not specifically limited here.

[0057] Optionally, the current motion is a linear motion. In this case, the pause position trajectory planning information can be obtained as shown in Figure 4. Please refer to Figure 4, which is a flowchart of one of the sub-steps included in step S120 in Figure 3. In this embodiment, step S120 may include sub-steps S121 and S122.

[0058] Sub-step S121 , calculating a pause distance based on the current motion, the current position and the first target position.

[0059] When the current motion is linear motion, the pause distance can be calculated using the following formula:

[0060] Among them, (x0, y0, z0) represents the current position p0, (x1, y1, z1) represents the first target position p1, and d1 represents the pause distance.

[0061] In sub-step S122 , the pause position trajectory planning information is obtained based on the current motion, pause distance, and current position speed, with the first target position speed being 0 as a requirement.

[0062] When the pause distance is obtained, the pause position trajectory planning information can be obtained by adopting uniform deceleration motion or acceleration followed by deceleration motion according to the pause distance, current motion and current position speed to achieve a speed of 0 at the first target position.

[0063] Optionally, due to the ease of calculation of uniform deceleration motion, as a possible implementation, uniform deceleration motion is employed during the pause process, as shown in FIG5 . Referring to FIG5 , FIG5 is a schematic flow diagram of the substeps included in substep S122 in FIG4 . In this embodiment, substep S122 may include substeps S1221 to S1223 .

[0064] Sub-step S1221 , calculating the target deceleration under uniform deceleration motion according to the pause distance and the current position speed.

[0065] Sub-step S1222: Calculate a first pause time according to the target deceleration and the current position speed.

[0066] Sub-step S1223 , obtaining the pause position trajectory planning information according to the target deceleration and the current position within the first pause time range.

[0067] Based on the pause distance, the target deceleration, first pause time, and current movement distance can be calculated in the following way:

[0068] Among them, a 11 Indicates the target deceleration, T 11 Indicates the first pause time (i.e. deceleration time), s 11 Indicates the current movement distance during the pause. Then, the pause position trajectory planning information is as follows:

[0069] When the current motion is linear motion, the pause position trajectory corresponding to the pause position trajectory planning information is

[0070] The trajectory segments corresponding to substeps S1221 to S1223 are shown in Figure 6. After receiving the pause command at p0, a pause position trajectory is planned, with p0 as the starting point and p1 (a point in the original position trajectory) as the end point, with the velocity at p1 being 0 and uniform deceleration being used, to achieve a linear pause position trajectory. This means that the velocity at p1 is reached through deceleration planning. The velocity during deceleration planning is shown in Figure 7, with the velocity V0 becoming 0 through uniform deceleration.

[0071] Optionally, the current motion is a full circle motion or an arc motion. In this case, the pause position trajectory planning information can be obtained as shown in Figure 8. Please refer to Figure 8, which is a second flow diagram of the sub-steps included in step S120 in Figure 3. In this embodiment, step S120 may include sub-steps S124 to S126.

[0072] Sub-step S124, obtaining a first vector according to the center position of the circle corresponding to the current motion and the current position, and obtaining a second vector according to the center position of the circle and the first target position.

[0073] Sub-step S125 , calculating the pause center angle according to the first vector and the second vector.

[0074] In this embodiment, when the current motion is a full circle motion or an arc motion, the pause center angle can be calculated by the following method:

[0075] Among them, (x0, y0, z0) represents the current position p0, o represents the center of the circle, (x r ,y r ,z r ) represents the center position of the circle, represents the first vector, (x1, y1, z1) represents the first target position p1, represents the second vector, and θ1 represents the pause circle center angle.

[0076] In sub-step S125 , the pause position trajectory planning information is obtained according to the current motion, the pause center angle and the current position speed, with the first target position speed being 0 as a requirement.

[0077] When the pause center angle is obtained, the center angle can be used as a uniform deceleration motion or other motion method based on the pause center angle, current motion and current position speed to achieve a speed of 0 at the first target position, thereby obtaining the pause position trajectory planning information.

[0078] Optionally, due to the uniform deceleration motion calculation method, as a possible implementation method, the pause position trajectory planning information when the current motion is a full circle motion or circular arc motion can be obtained through the method described in Figure 9. Please refer to Figure 9, which is a flowchart of the substeps included in substep S126 in Figure 8. In this embodiment, substep S126 can include substeps S1261 to S1263.

[0079] Sub-step S1261, calculating the target angular deceleration under uniform deceleration motion according to the radius corresponding to the current motion, the pause center angle and the current position speed.

[0080] Sub-step S1262: Calculate a second pause time according to the target angular deceleration, radius, and current position speed.

[0081] Sub-step S1263, within the second pause time range, obtaining the pause position trajectory planning information according to the radius corresponding to the current motion, the current position speed and the target angular deceleration.

[0082] Based on the pause center angle, the target angular deceleration, second pause time, and current motion center angle can be calculated using the following method:

[0083] Among them, R represents the radius, a 12 represents the target angular deceleration, T 12 Indicates the second pause time, s 12 Indicates the current motion center angle.

[0084] Optionally, in order to quickly obtain the pause position trajectory planning information, the pause position trajectory planning information can be obtained by rotating the three-dimensional vector around any axis within the second pause time range according to the plane normal vector corresponding to the current motion, the first vector, the radius corresponding to the current motion, the current position speed and the target angular deceleration. The pause position trajectory planning information is as follows:

[0085] in, Represents the arc plane normal vector or the circle plane normal vector.

[0086] The trajectory segments corresponding to the above sub-steps S1261 to S1263 are shown in FIG6 , and the position and speed segments are shown in FIG7 , which will not be described in detail here.

[0087] When a pause command is received, the pause posture trajectory planning information corresponding to the pause position trajectory planning information can also be planned to complete the posture pause, thereby ensuring safety. Please refer to Figure 10, which is a second flow chart of the trajectory planning method provided in an embodiment of the present disclosure. In this embodiment, the method can also include steps S130 to S140.

[0088] Step S130: upon receiving the pause instruction, determining a first target posture, a current posture speed, and a current posture of the robot end.

[0089] In this embodiment, when a pause instruction is received, the current posture speed V of the robot end can be recorded at the same time. r0and the current posture, that is, the position in the current posture of the robot end is the current position, and the posture is the current posture. The original trajectory planning information corresponding to the current motion includes the original position trajectory planning information and the original posture trajectory planning information. The original trajectory corresponding to the original trajectory planning information includes the original position trajectory and the original posture trajectory. The position trajectory corresponding to the original position trajectory planning information is the original position trajectory, and the posture trajectory corresponding to the original posture trajectory planning information is the original posture trajectory. The first target posture is the posture corresponding to the first target trajectory point in the original posture trajectory corresponding to the original position trajectory. That is, based on the original trajectory planning information, the posture of the robot end at the first target trajectory point in the original plan is determined, and the posture is used as the first target posture.

[0090] Step S140 , requiring that the first target posture speed of the first target trajectory point is 0, and obtaining pause posture trajectory planning information corresponding to the pause process according to the current posture speed, the current posture and the first target posture.

[0091] In this embodiment, the first target posture speed V of the first target trajectory point can be r1 As a requirement, posture planning is performed based on the current posture speed, current posture, and first target posture speed to obtain pause posture trajectory planning information corresponding to the pause process. The pause posture trajectory corresponding to the pause posture trajectory planning information has a starting posture of the current posture and an ending posture of the first target posture. The pause posture trajectory planning information includes a correspondence between time and posture.

[0092] Optionally, the pause time T1 in the position pause plan (i.e. the first pause time T 11 Or the second pause time T 12 ), the pause posture trajectory planning information is obtained through quaternion planning. That is, the moment corresponding to the pause posture trajectory planning is within the pause time range used in the pause position trajectory planning. In this way, the relationship between position, posture and time within the pause time T1 can be obtained, and the pause trajectory planning information is obtained based on the pause posture trajectory planning information and the pause position trajectory planning information. The starting trajectory point in the pause trajectory corresponding to the pause trajectory planning information is the current point (the position is the current position, the position speed is the current position speed V0, the posture is the current posture, and the posture speed is V r1 ), the end trajectory point is the first target trajectory point (the position is the first target position, the position speed is 0, the posture is the first target posture, and the posture speed is 0).

[0093] Optionally, when the pause trajectory planning information is obtained, the Cartesian space pose instruction can be obtained Then, the interpolation instruction of each control cycle is obtained through inverse kinematics calculation and sent to the driver for execution, thereby achieving pause. Indicates location, Indicates posture.

[0094] Please refer to Figure 11, which is a third flow chart of the trajectory planning method provided by an embodiment of the present disclosure. In this embodiment, after executing the pause, the method may further include steps S210 to S230.

[0095] Step S210: upon receiving the recovery instruction, determining a second target position.

[0096] In this embodiment, when the robot needs to continue moving, the electronic device can receive a resume instruction. The resume instruction can be used to determine time N2. The second target time (i.e., time N+time N1+time N2 when the pause instruction is received) can be calculated based on the time N, time N1, and time N2 of receiving the stop instruction. Then, based on the correspondence between time and position in the original position trajectory planning information, the position corresponding to the second target time is determined and used as the second target position. The second target position is the position of the second target trajectory point in the original position trajectory, and in the original position trajectory, the execution time of the first target trajectory point is earlier than the execution time of the second target trajectory point.

[0097] Optionally, time N2 can be input by the user when triggering the recovery command, for example, through a user interface, a teach pendant, or a host computer configuration. Alternatively, it can be a pre-set fixed value, i.e., time N2 is used to determine the second target position whenever the recovery command is received. It is understood that when setting the value of N2, it is necessary to ensure that the robot's maximum end-point acceleration supports the robot reaching the corresponding position and velocity requirements at the second target time.

[0098] Step S220 , obtaining a second target position speed of the second target trajectory point according to the original position trajectory planning information corresponding to the original position trajectory.

[0099] In this embodiment, the original position trajectory planning information includes the correspondence between position and time. The second target position speed of the second target trajectory point can be analyzed based on the original position trajectory planning information, that is, the position speed at the second target position in the original plan can be analyzed and obtained.

[0100] Optionally, as a possible implementation manner, one or more positions near the second target position and the time corresponding to each position may be determined according to the original position trajectory planning information, and then the second target position speed may be calculated.

[0101] Optionally, a third target position corresponding to a third target moment (the current moment N0 + moment N1 + moment N2-1 at which the pause instruction is received, i.e., the moment before the second target moment) can be determined based on the original position trajectory planning information. The second target position speed can then be calculated based on the distance between the second and third target positions and the specific duration represented by a moment. The length of a moment defined by different manufacturers can vary, for example, 1ms or 8ms.

[0102] Step S230 , planning and obtaining restoration position trajectory planning information corresponding to the restoration process according to the second target position speed, the current motion, the first target position, and the second target position.

[0103] Next, the position speed of the second target trajectory point can be required to be the second target position speed V2, and the recovery position trajectory planning information corresponding to the recovery process can be planned according to the current motion, the first target position and the second target position. The starting point of the recovery position trajectory corresponding to the recovery position trajectory planning information is the first target position and the end point is the second target position, and the recovery position trajectory planning information includes the correspondence between time and position. That is, the recovery position trajectory and the original position trajectory segment to be executed after recovery belong to the same motion, and the starting point of the recovery position trajectory is the end point of the previously paused position trajectory, and the end point of the recovery position trajectory is the point that has not yet been reached in the original position trajectory.

[0104] It is understood that the correspondence between time and position in the restored position trajectory planning information may be different from the correspondence between time and position during the recovery period (i.e., the time period used for recovery) in the original position trajectory planning information, but the corresponding position trajectories are the same. This method allows you to directly connect to the original trajectory after recovery without the need for replanning.

[0105] In this way, the aforementioned pause-resume trajectory consistency control method performs consistency planning during both the pause and resume segments, ensuring consistency between the original position trajectory segment and the newly planned position trajectory segment, preventing trajectory changes. This effectively prevents accidents such as end-of-line tool collisions and robot collisions, significantly improving the smoothness and safety of robot operation. This approach allows the robot to continue its previously unfinished trajectory after receiving the pause and resume commands, rather than starting over from the beginning.

[0106] In this embodiment, the process of obtaining the resume position trajectory planning information is similar to the process of obtaining the pause position trajectory planning information. For specific instructions, please refer to the above description. Here, only two specific methods are introduced for illustration.

[0107] When the current motion is a linear motion, the recovery distance can be calculated according to the current motion, the first target position, and the second target position using the following formula:

[0108] Among them, (x1, y1, z1) represents the first target position p1, (x2, y2, z2) represents the second target position p2, and d2 represents the recovery distance.

[0109] When the current motion is linear and uniform acceleration is used for recovery, the target acceleration, first recovery time, and current motion distance can be calculated as follows:

[0110] Wherein, V2 represents the second target position speed, a 21 represents the target acceleration, T 21 Indicates the first recovery time (i.e. acceleration time), s 21 Indicates the current movement distance in recovery. Then, the recovery position trajectory planning information is as follows:

[0111] When the current motion is linear motion, the recovery position trajectory corresponding to the recovery position trajectory planning information is

[0112] When the current motion is a full circle motion or an arc motion, the restoration center angle can be calculated using the following method:

[0113] Among them, (x1, y1, z1) represents the first target position p1, o represents the center of the circle, (x2, y2, z2) represents the second target position, (x r ,y r ,z r ) represents the center position of the circle, represents the second vector, (x2, y2, z2) represents the second target position p2, represents the third vector, and θ2 represents the restored center angle.

[0114] When the center angle of the circle is used as a uniform deceleration motion, the target angular acceleration speed, the second recovery time, and the current motion center angle can be calculated using the following formula:

[0115] Among them, R represents the radius, a 22represents the target angular acceleration, T 22 represents the second recovery time, s 22 Indicates the current motion center angle.

[0116] Optionally, in order to quickly obtain the restoration position trajectory planning information, the restoration position trajectory planning information can be obtained by rotating the three-dimensional vector around any axis within the second restoration time range based on the plane normal vector corresponding to the current motion, the second vector, the radius corresponding to the current motion, the second target position speed, and the target angular acceleration. The restoration position trajectory planning information is as follows:

[0117] in, Represents the arc plane normal vector or the circle plane normal vector.

[0118] The trajectory of the above recovery process can be shown in Figure 6, and the position and speed are shown in Figure 7, which will not be repeated here. That is, in the case of linear motion, when p1 receives the recovery command, it takes p1 as the starting point of the trajectory, p2 (a point in the original position trajectory) as the end point of the trajectory, and the position speed at p2 is V2. Uniform acceleration is used to plan a recovery position trajectory that is still a linear motion, that is, to reach p2 through acceleration planning. The speed situation in the acceleration planning is shown in Figure 7, and the position speed changes from 0 to V2 through uniform acceleration. The recovery process of circular motion or full circle motion is the same.

[0119] Similarly, when a recovery instruction is received, the recovery posture trajectory planning information corresponding to the recovery position trajectory planning information can also be planned to complete the posture update, facilitating direct connection to the original trajectory. Please refer to Figure 12, which is a fourth flow chart of the trajectory planning method provided in an embodiment of the present disclosure. In this embodiment, the method can also include steps S240 to S260.

[0120] Step S240: upon receiving the restoration instruction, determining a second target posture.

[0121] The second target posture is the posture corresponding to the second target trajectory point in the original posture trajectory corresponding to the original position trajectory; that is, the posture of the robot end at the second target trajectory point in the original plan is the second target posture. The second target posture can be determined by determining the first target posture, that is, the posture corresponding to the third target time (time N + time N1 + time N2 when the pause instruction is received) in the original posture trajectory planning information is used as the second target posture.

[0122] Step S250: Obtain a second target posture velocity corresponding to the second target trajectory point according to the original position trajectory planning information.

[0123] Alternatively, the second target posture speed can be calculated using the method for calculating the second target position speed. In this method, the third target time (time N when the pause instruction is received + time N1 + time N2-1) can be calculated first, and then the posture corresponding to the third target time is determined based on the original trajectory planning information. Then, the second target posture speed is calculated based on the second target posture speed, the posture corresponding to the third target time, and the specific duration corresponding to one moment.

[0124] Step S260 , planning and obtaining restoration posture trajectory planning information corresponding to the restoration process according to the second target posture, the second target posture speed, and the first target posture.

[0125] In this embodiment, the posture speed of the second target trajectory point can be the second target posture speed V r2 As a requirement, based on the second target posture and the first target posture, restoration posture trajectory planning information corresponding to the restoration process is planned. The restoration posture trajectory corresponding to the restoration posture trajectory planning information has a starting posture of the first target posture and an ending posture of the second target posture, and the restoration posture trajectory planning information includes a correspondence between time and posture.

[0126] Optionally, the recovery time T2 (i.e., the first recovery time T 21 Or the second recovery time T 22 ), the restoration posture trajectory planning information is obtained through quaternion planning. That is, the moment corresponding to the restoration posture trajectory planning is within the restoration time range used in the restoration position trajectory planning. In this way, the relationship between position, posture and time within the recovery time T2 can be obtained, and the restoration trajectory planning information is obtained based on the restoration posture trajectory planning information and the restoration position trajectory planning information. The starting trajectory point in the restoration trajectory corresponding to the restoration trajectory planning information is the first target trajectory point (the position is the first target position, the position speed is 0, the posture is the first target posture, and the posture speed is 0), and the end trajectory point is the second target trajectory point (the position is the second target position, the position speed is the second target position speed V2, the posture is the second target posture, and the posture speed is the second target posture speed V r2 ).

[0127] Optionally, when the recovery trajectory planning information is obtained, the Cartesian space pose instruction can be obtained Then, the interpolation instruction of each control cycle is obtained through inverse kinematics calculation and sent to the driver for execution, thereby achieving recovery. Indicates location, Indicates posture.

[0128] In order to execute the corresponding steps in the above-mentioned embodiments and various possible methods, a method for implementing a trajectory planning device 200 is given below. Optionally, the trajectory planning device 200 can adopt the device structure of the electronic device 100 shown in Figure 2 above. Further, please refer to Figure 13, which is one of the block diagrams of the trajectory planning device 200 provided in the embodiment of the present disclosure. It should be noted that the basic principles and technical effects of the trajectory planning device 200 provided in this embodiment are the same as those in the above-mentioned embodiments. For the sake of brief description, for parts not mentioned in this embodiment, please refer to the corresponding contents in the above-mentioned embodiments. In this embodiment, the trajectory planning device 200 may include: an information acquisition module 210 and a planning module 220.

[0129] In this embodiment, the information acquisition module 210 is configured to determine a first target position, the current position velocity, and the current position of the robot end point when a pause command is received during robot motion. The first target position is the position of a first target trajectory point in an original position trajectory corresponding to the current motion of the robot, where the execution time of the current trajectory point in the original position trajectory is earlier than the execution time of the first target trajectory point.

[0130] The planning module 220 is configured to obtain pause position trajectory planning information corresponding to the pause process based on the current motion, current position velocity, current position, and first target position, with the first target position velocity of the first target trajectory point being zero. The pause position trajectory corresponding to the pause position trajectory planning information has a starting point at the current position and an end point at the first target position, and the pause position trajectory planning information includes a correspondence between time and position.

[0131] Optionally, in this embodiment, when the pause instruction is received, the information acquisition module 210 is also used to determine the first target posture, the current posture speed and the current posture of the robot end, wherein the first target posture is the posture corresponding to the first target trajectory point in the original posture trajectory corresponding to the original position trajectory; the planning module 220 is also used to require that the first target posture speed of the first target trajectory point is 0, and according to the current posture speed, current posture and first target posture, plan the pause posture trajectory planning information corresponding to the pause process, wherein the starting posture of the pause posture trajectory corresponding to the pause posture trajectory planning information is the current posture, and the end posture is the first target posture, and the pause posture trajectory planning information includes the correspondence between time and posture.

[0132] Optionally, in this embodiment, the information acquisition module 210 is further used to determine a second target position upon receiving a recovery instruction, wherein the second target position is the position of the second target trajectory point in the original position trajectory, and in the original position trajectory, the execution time of the first target trajectory point is earlier than the execution time of the second target trajectory point; and the second target position speed of the second target trajectory point is obtained according to the original position trajectory planning information corresponding to the original position trajectory. The planning module 220 is further used to plan and obtain the recovery position trajectory planning information corresponding to the recovery process according to the second target position speed, the current motion, the first target position, and the second target position. The starting point of the recovery position trajectory corresponding to the recovery position trajectory planning information is the first target position, and the end point is the second target position, and the recovery position trajectory planning information includes the correspondence between time and position.

[0133] In this embodiment, the information acquisition module 210 is further used to: determine the second target posture when receiving the recovery instruction, wherein the second target posture is the posture corresponding to the second target trajectory point in the original posture trajectory corresponding to the original position trajectory; and obtain the second target posture speed corresponding to the second target trajectory point according to the original position trajectory planning information. The planning module 220 is further used to: plan and obtain the restoration posture trajectory planning information corresponding to the restoration process according to the second target posture, the second target posture speed and the first target posture, wherein the starting position of the restoration posture trajectory corresponding to the restoration posture trajectory planning information is the first target posture, and the end position is the second target posture, and the restoration posture trajectory planning information includes the correspondence between time and posture.

[0134] Please refer to FIG14 , which is a second block diagram of the trajectory planning device 200 provided in an embodiment of the present disclosure. In this embodiment, the trajectory planning device 200 may further include a control module 230 .

[0135] The control module 230 is configured to perform pause control according to the pause position trajectory planning information or the pause position trajectory planning information and the pause posture trajectory planning information to achieve pause.

[0136] The control module 230 is further configured to perform recovery control according to the recovery position trajectory planning information or the recovery position trajectory planning information and the recovery posture trajectory planning information to achieve recovery.

[0137] Optionally, the above modules may be stored in the memory 110 shown in FIG2 in the form of software or firmware or solidified in the operating system (OS) of the electronic device 100, and may be executed by the processor 120 in FIG2. At the same time, the data, program code, etc. required to execute the above modules may be stored in the memory 110.

[0138] An embodiment of the present disclosure further provides a readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the trajectory planning method is implemented.

[0139] In summary, the embodiments of the present disclosure provide a trajectory planning method, device, electronic device and readable storage medium. When a pause command is received during the movement of a robot, the method determines the first target position, the current position speed and current position of the robot end. The first target position is the position of the first target trajectory point in the original position trajectory corresponding to the current movement performed by the robot. In the original position trajectory, the execution time of the current trajectory point is earlier than the execution time of the first target trajectory point. With the first target position speed of the first target trajectory point being 0 as the requirement, the pause position trajectory planning information corresponding to the pause process is planned based on the current movement, current position speed, current position and first target position. The starting point of the pause position trajectory corresponding to the pause position trajectory planning information is the current position and the end point is the first target position. The pause position trajectory planning information includes the correspondence between time and position. In this way, the consistency of the pause position trajectory and the original position trajectory segment corresponding to the pause period is guaranteed on the basis of achieving the pause, and there will be no change in the motion trajectory. This can effectively avoid accidents such as end tool collision and robot collision, and greatly improve the stability and safety of the robot operation.

[0140] In the several embodiments provided in the present disclosure, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely illustrative. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architectures, functions and operations of the devices, methods and computer program products according to the multiple embodiments of the present disclosure. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a portion of code, and the module, program segment or a portion of code contains one or more executable instructions for implementing the specified logical functions. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in an order different from that marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart, can be implemented using a dedicated hardware-based system that performs the specified function or action, or can be implemented using a combination of dedicated hardware and computer instructions.

[0141] In addition, the functional modules in the various embodiments of the present disclosure may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

[0142] If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present disclosure, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for enabling 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 method described in each embodiment of the present disclosure. The aforementioned storage medium includes various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0143] The foregoing description is merely an optional embodiment of the present disclosure and is not intended to limit the present disclosure. Those skilled in the art will readily appreciate that the present disclosure is susceptible to various modifications and variations. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present disclosure shall be included within the scope of protection of the present disclosure. Industrial Applicability

[0144] The embodiments of the present disclosure provide a trajectory planning method and device, which relate to the field of robotics. The trajectory planning method includes: when a pause command is received during the movement of the robot, determining the first target position of the first target trajectory point, the current position speed and current position of the robot end, and in the original position trajectory corresponding to the current movement, the execution time of the current trajectory point is earlier than the execution time of the first target trajectory point; with the position speed of the first target trajectory point being 0 as the requirement, according to the current movement, current position speed, current position and first target position, the pause position trajectory planning information corresponding to the pause process is planned, the starting point of the pause position trajectory is the current position, and the end point is the first target position, and the pause position trajectory planning information includes the correspondence between time and position. In this way, accidents such as end tool collision and robot collision can be effectively avoided, greatly improving the stability and safety of robot operation.

[0145] Furthermore, it is understood that the trajectory planning method and apparatus disclosed herein are reproducible and can be applied in a variety of industrial applications. For example, the trajectory planning method and apparatus disclosed herein can be applied in the field of robotics.

Claims

1. A trajectory planning method, characterized in that: The method comprises: In the case where a pause instruction is received during the movement of the robot, a first target position, a current position speed and a current position of the end of the robot are determined, wherein the first target position is the position of a first target trajectory point in an original position trajectory corresponding to the current movement performed by the robot, and in the original position trajectory, the execution time of the current trajectory point is earlier than the execution time of the first target trajectory point; Taking the first target position speed of the first target trajectory point as 0 as the requirement, according to the current motion, current position speed, current position and the first target position, the pause position trajectory planning information corresponding to the pause process is planned, wherein the starting point of the pause position trajectory corresponding to the pause position trajectory planning information is the current position and the end point is the first target position, and the pause position trajectory planning information includes the correspondence between time and position.

2. The method according to claim 1, characterized in that When the current motion is a linear motion, the first target position speed of the first target trajectory point is required to be 0, and according to the current motion, the current position speed, the current position and the first target position, the pause position trajectory planning information corresponding to the pause process is planned, including: Calculating a pause distance according to the current motion, the current position and the first target position; The first target position speed is required to be 0, and the pause position trajectory planning information is obtained according to the current motion, pause distance and current position speed.

3. The method according to claim 2, characterized in that The step of obtaining the pause position trajectory planning information based on the current motion, pause distance and current position speed with the first target position speed being 0 includes: According to the pause distance and the current position speed, the target deceleration under uniform deceleration motion is calculated; Calculating a first pause time according to the target deceleration and the current position speed; Within the first pause time range, the pause position trajectory planning information is obtained according to the target deceleration and the current position.

4. The method according to claim 1, characterized in that: When the current motion is a full circle motion or an arc motion, the first target position speed of the first target trajectory point is required to be 0, and according to the current motion, the current position speed, the current position and the first target position, the pause position trajectory planning information corresponding to the pause process is planned, including: Obtaining a first vector according to the center position of the circle corresponding to the current motion and the current position, and obtaining a second vector according to the center position of the circle and the first target position; Calculate the pause circle center angle according to the first vector and the second vector; The first target position speed is required to be 0, and the pause position trajectory planning information is obtained according to the current motion, the pause center angle and the current position speed.

5. The method according to claim 4, characterized in that The step of obtaining the pause position trajectory planning information based on the current motion, the pause center angle and the current position speed with the first target position speed being 0 as the requirement includes: Calculate the target angular deceleration under uniform deceleration motion according to the radius corresponding to the current motion, the pause circle center angle and the current position speed; Calculating a second pause time according to the target angular deceleration, radius and current position speed; Within the second pause time range, the pause position trajectory planning information is obtained according to the radius corresponding to the current motion, the current position speed and the target angular deceleration.

6. The method according to claim 5, characterized in that The step of obtaining the pause position trajectory planning information within the second pause time range according to the radius corresponding to the current motion, the current position speed and the target angular deceleration includes: By rotating a three-dimensional vector around an arbitrary axis, within the second pause time range, the pause position trajectory planning information is obtained according to the plane normal vector corresponding to the current motion, the first vector, the radius corresponding to the current motion, the current position speed and the target angular deceleration.

7. The method according to claim 1, characterized in that The method further comprises: In the case of receiving the pause instruction, determining a first target posture, a current posture speed and a current posture of the robot end, wherein the first target posture is a posture corresponding to the first target trajectory point in the original posture trajectory corresponding to the original position trajectory; Taking the first target posture speed of the first target trajectory point as 0 as the requirement, according to the current posture speed, the current posture and the first target posture, the pause posture trajectory planning information corresponding to the pause process is planned, wherein the starting posture of the pause posture trajectory corresponding to the pause posture trajectory planning information is the current posture, and the end posture is the first target posture, and the pause posture trajectory planning information includes the corresponding relationship between time and posture.

8. The method according to any one of claims 1 to 7, characterized in that: The method further comprises: In case of receiving the recovery instruction, determining a second target position, wherein the second target position is a position of a second target trajectory point in the original position trajectory, and in the original position trajectory, an execution time of the first target trajectory point is earlier than an execution time of the second target trajectory point; Obtaining a second target position speed of the second target trajectory point according to the original position trajectory planning information corresponding to the original position trajectory; According to the speed of the second target position, the current motion, the first target position and the second target position, the recovery position trajectory planning information corresponding to the recovery process is planned, wherein the starting point of the recovery position trajectory corresponding to the recovery position trajectory planning information is the first target position and the end point is the second target position, and the recovery position trajectory planning information includes the correspondence between time and position.

9. The method according to claim 8, characterized in that The method further comprises: In case of receiving the recovery instruction, determining a second target posture, wherein the second target posture is a posture corresponding to the second target trajectory point in the original posture trajectory corresponding to the original position trajectory; According to the original position trajectory planning information, obtaining a second target attitude speed corresponding to the second target trajectory point; According to the second target posture, the second target posture speed and the first target posture, the restoration posture trajectory planning information corresponding to the restoration process is planned, wherein the starting posture of the restoration posture trajectory corresponding to the restoration posture trajectory planning information is the first target posture, and the end posture is the second target posture, and the restoration posture trajectory planning information includes the correspondence between time and posture.

10. A trajectory planning device, characterized in that: The trajectory planning device comprises: an information acquisition module, for determining a first target position, a current position speed and a current position of the end of the robot when a pause command is received during the movement of the robot, wherein the first target position is the position of a first target trajectory point in an original position trajectory corresponding to the current movement performed by the robot, and in the original position trajectory, the execution time of the current trajectory point is earlier than the execution time of the first target trajectory point; A planning module is used to plan the pause position trajectory planning information corresponding to the pause process based on the requirement that the first target position speed of the first target trajectory point is 0, according to the current motion, current position speed, current position and first target position, wherein the starting point of the pause position trajectory corresponding to the pause position trajectory planning information is the current position and the end point is the first target position, and the pause position trajectory planning information includes the corresponding relationship between time and position.

Images