Trajectory planning method, device, computer equipment and storage medium for singular region

Abstract

The present application relates to a trajectory planning method, device, computer equipment, storage medium and computer program product for a singular region. The method comprises: obtaining the original planned trajectory of the robot, determining multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory, determining the entry boundary point, exit boundary point and entry joint posture sequence of the singular point when there is a singular point in the original planned trajectory, determining the exit joint posture sequence according to the entry joint posture sequence, performing minimum vibration interpolation on each to-be-corrected trajectory point in the original planned trajectory according to the entry joint posture sequence and the exit joint posture sequence, obtaining a joint posture sequence with smooth transition of the to-be-corrected trajectory point, and performing redundant degree of freedom planning on the joint posture sequence with smooth transition, obtaining a corrected planned trajectory avoiding singular points, so that the joint motion can smoothly transition from the entry joint posture sequence to the exit joint posture sequence without losing position accuracy.

Description

Technical Field

[0001] The present application relates to the field of robotics, and in particular to a trajectory planning method, device, computer equipment and storage medium for a singular area. Background Art

[0002] The robotic arm is a complex system with high precision, multiple inputs and multiple outputs, high nonlinearity, and strong coupling. Due to its unique operational flexibility, it has been widely used in industrial assembly, safety and explosion-proof and other fields.

[0003] A singular point refers to when the robot's fifth axis joint approaches 0°, and the fourth and sixth axes are on the same straight line. At this point, the robot's degrees of freedom will degenerate, causing the angular velocity of some joints to approach infinity, leading to loss of control. Mathematically, singular points are usually caused by the linear coupling of the joint rotation of the robot arm. There is a type of singular point that always corresponds to a specific angle of a specific joint of the robot. The corresponding joint angle will produce two mirrored joint postures on the left and right sides. These two postures are called the robot's mirror solution for a certain type of singularity.

[0004] In practical applications, since the motion near the singularity point often causes over-limit and abnormality of the collaborative robot controller, it is very necessary to establish motion protection near the singularity point. Summary of the invention

[0005] Based on this, it is necessary to provide a trajectory planning method, device, computer equipment, computer-readable storage medium and computer program product that can avoid singular areas in order to solve the above technical problems.

[0006] In a first aspect, the present application provides a trajectory planning method for a singular region. The method comprises:

[0007] Obtaining an original planned trajectory of the robot, and determining multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory;

[0008] When it is determined that the original planned trajectory has a singular point according to the plurality of groups of candidate joint posture sequences of each trajectory point, determining an entry boundary point and an exit boundary point of the singular point;

[0009] Determine an entry joint posture sequence of the entry boundary point, and determine an exit joint posture sequence in a candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence;

[0010] According to the entry joint posture sequence and the exit joint posture sequence, minimum vibration interpolation is performed on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with smooth transition of the to-be-corrected trajectory point;

[0011] Redundant degrees of freedom planning is performed on the joint posture sequence of the smooth transition of the trajectory points to be corrected to obtain a corrected planned trajectory.

[0012] In one embodiment, the method of determining whether the original planned trajectory has a singular point according to the multiple groups of candidate joint posture sequences of each trajectory point includes:

[0013] For each of the trajectory points, determining a target joint posture sequence from the multiple groups of candidate joint posture sequences; the target joint posture sequence includes the joint postures of each joint point of the robot;

[0014] Determine a target joint posture in the target joint posture sequence of each of the trajectory points;

[0015] The trajectory points where the target joint posture is less than a first threshold are determined as singular points.

[0016] In one embodiment, determining a target joint posture sequence from the multiple groups of candidate joint posture sequences comprises:

[0017] The joint posture sequence that is closest to the target joint posture sequence at the previous moment among the multiple groups of candidate joint posture sequences is determined as the target joint posture sequence.

[0018] In one embodiment, determining the entry boundary point and the exit boundary point of the singular point includes:

[0019] Find the target joint posture in the target joint posture sequence of trajectory points within a preset range before and after the singular point;

[0020] Determine the trajectory point corresponding to the target joint posture whose target joint posture is less than the second threshold and whose speed is less than 0 as the entry boundary point;

[0021] The trajectory point corresponding to the target joint posture whose target joint posture is greater than the second threshold and whose speed is greater than 0 is determined as the exit boundary point.

[0022] In one embodiment, determining the entry joint posture sequence of the entry boundary point, and determining the exit joint posture sequence in the candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence, comprises:

[0023] Determining the target joint posture sequence of the entry boundary point as an entry joint posture sequence;

[0024] Among the candidate joint posture sequences of the exit boundary points, the joint posture sequence with the smallest weighted joint angle with the entry joint posture sequence is determined as the exit joint posture sequence.

[0025] In one embodiment, the method of performing minimum vibration interpolation on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory according to the entry joint posture sequence and the exit joint posture sequence to obtain a joint posture sequence with smooth transition of the to-be-corrected trajectory point comprises:

[0026] According to the entry joint posture sequence and the exit joint posture sequence, a fifth-order polynomial is used to perform minimum vibration interpolation on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with a smooth transition of the to-be-corrected trajectory point.

[0027] In one embodiment, performing redundant degree of freedom planning on the joint posture sequence of the smooth transition of the trajectory points to be corrected to obtain the corrected planned trajectory includes:

[0028] According to the joint posture sequence of the trajectory points to be corrected with smooth transition, the trajectory speed of the joint posture sequence of the trajectory points to be corrected with smooth transition is obtained;

[0029] Determining a position increment according to the trajectory velocity and the Jacobian matrix;

[0030] A corrected planned trajectory is determined according to the position increment.

[0031] In a second aspect, the present application also provides a robot control device. The device comprises:

[0032] A candidate joint posture sequence determination module is used to obtain the original planned trajectory of the robot and determine multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory;

[0033] An entry boundary point and an exit boundary point determination module, for determining an entry boundary point and an exit boundary point of the singular point when it is determined that the original planned trajectory has a singular point according to the plurality of groups of candidate joint posture sequences of each trajectory point;

[0034] An exit joint posture sequence determination module is used to determine an entry joint posture sequence of the entry boundary point, and determine an exit joint posture sequence in a candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence;

[0035] A minimum vibration interpolation module is used to perform minimum vibration interpolation on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory according to the entry joint posture sequence and the exit joint posture sequence, so as to obtain a joint posture sequence with smooth transition of the to-be-corrected trajectory point;

[0036] The correction planning trajectory module is used to plan and determine the redundant degrees of freedom of the joint posture sequence with smooth transition of the trajectory points to be corrected, so as to obtain the correction planning trajectory.

[0037] In a third aspect, the present application further provides a computer device. The computer device includes a memory and a processor, the memory stores a computer program, and the processor implements the following steps when executing the computer program:

[0038] Obtaining an original planned trajectory of the robot, and determining multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory;

[0039] When it is determined that the original planned trajectory has a singular point according to the plurality of groups of candidate joint posture sequences of each trajectory point, determining an entry boundary point and an exit boundary point of the singular point;

[0040] Determine an entry joint posture sequence of the entry boundary point, and determine an exit joint posture sequence in a candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence;

[0041] According to the entry joint posture sequence and the exit joint posture sequence, minimum vibration interpolation is performed on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with smooth transition of the to-be-corrected trajectory point;

[0042] Redundant degrees of freedom planning is performed on the joint posture sequence of the smooth transition of the trajectory points to be corrected to obtain a corrected planned trajectory.

[0043] In a fourth aspect, the present application further provides a computer-readable storage medium. The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the following steps are implemented:

[0044] Obtaining an original planned trajectory of the robot, and determining multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory;

[0045] When it is determined that the original planned trajectory has a singular point according to the plurality of groups of candidate joint posture sequences of each trajectory point, determining an entry boundary point and an exit boundary point of the singular point;

[0046] Determine an entry joint posture sequence of the entry boundary point, and determine an exit joint posture sequence in a candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence;

[0047] According to the entry joint posture sequence and the exit joint posture sequence, minimum vibration interpolation is performed on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with smooth transition of the to-be-corrected trajectory point;

[0048] Redundant degrees of freedom planning is performed on the joint posture sequence of the smooth transition of the trajectory points to be corrected to obtain a corrected planned trajectory.

[0049] In a fifth aspect, the present application further provides a computer program product. The computer program product includes a computer program, and when the computer program is executed by a processor, the following steps are implemented:

[0050] Obtaining an original planned trajectory of the robot, and determining multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory;

[0051] When it is determined that the original planned trajectory has a singular point according to the plurality of groups of candidate joint posture sequences of each trajectory point, determining an entry boundary point and an exit boundary point of the singular point;

[0052] Determine an entry joint posture sequence of the entry boundary point, and determine an exit joint posture sequence in a candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence;

[0053] According to the entry joint posture sequence and the exit joint posture sequence, minimum vibration interpolation is performed on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with smooth transition of the to-be-corrected trajectory point;

[0054] Redundant degrees of freedom planning is performed on the joint posture sequence of the smooth transition of the trajectory points to be corrected to obtain a corrected planned trajectory.

[0055] The trajectory planning method, device, computer equipment, storage medium and computer program product for the singular area above obtain the original planned trajectory of the robot, determine multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory, and when it is determined that there are singular points in the original planned trajectory based on the multiple groups of candidate joint posture sequences of each trajectory point, for the original planned trajectory with the singular point, determine the entry boundary point, exit boundary point and entry joint posture sequence, determine the exit joint posture sequence based on the entry joint posture sequence, and achieve minimum vibration interpolation through the entry joint posture sequence and the exit joint posture sequence to obtain a joint posture sequence with a smooth transition of the trajectory points to be corrected, perform redundant degree of freedom planning on the joint posture sequence with a smooth transition, and obtain a corrected planned trajectory that avoids singular points, so that the joint movement can smoothly transition from the entry joint posture sequence to the exit joint posture sequence without losing position accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 A diagram showing an application environment of a trajectory planning method for a singular area in an embodiment;

[0057] Figure 2 A schematic flow chart of a trajectory planning method for a singular region in one embodiment;

[0058] Figure 3 A schematic diagram of a trajectory boundary in an embodiment;

[0059] Figure 4 A schematic diagram of a process for determining a singular point in another embodiment;

[0060] Figure 5 A schematic diagram of a process for determining an entry boundary point and an exit boundary point in another embodiment;

[0061] Figure 6 A schematic diagram of a flow chart for determining a modified planned trajectory in another embodiment;

[0062] Figure 7 A schematic flow chart of a trajectory planning method for a singular region in one embodiment;

[0063] Figure 8 A schematic flow chart of a trajectory planning method for a singular area in another embodiment;

[0064] Fig. 9 A schematic diagram of comparing joint positions and velocities of an original planned trajectory and a revised planned trajectory of a robot in one embodiment;

[0065] Fig.10 is a graph of position / posture error in one embodiment;

[0066] Fig.11 is a structural block diagram of a robot control device in one embodiment;

[0067] Fig.12 FIG. 4 is a diagram showing the internal structure of a computer device in one embodiment. DETAILED DESCRIPTION

[0068] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with 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.

[0069] The trajectory planning method for a singular region provided in the embodiment of the present application can be applied to Figure 1In the robot shown. Among them, the robot 102 can realize the above control through its own module, and the robot 102 can also communicate with the server 104 through the network. The data storage system can store the data that the server 104 needs to process. The data storage system can be integrated on the server 104, or it can be placed on the cloud or other network servers. The server or robot obtains the original planning trajectory of the robot, and determines the multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planning trajectory; when it is determined that there are singular points in the original planning trajectory according to the multiple groups of candidate joint posture sequences of each trajectory point, the entry boundary point and the exit boundary point of the singular point are determined; the entry joint posture sequence of the entry boundary point is determined, and the exit joint posture sequence is determined in the candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence; according to the entry joint posture sequence and the exit joint posture sequence, the minimum vibration interpolation is performed on each trajectory point to be corrected within the range of the entry boundary point and the exit boundary point in the original planning trajectory to obtain the joint posture sequence of the smooth transition of the trajectory point to be corrected; the joint posture sequence of the smooth transition of the trajectory point to be corrected is subjected to redundant degree of freedom planning to obtain a corrected planning trajectory. The server 104 can be implemented with an independent server or a server cluster consisting of multiple servers. Robots can be industrial robots, sweeping robots, agricultural robots, special robots, etc.

[0070] In one embodiment, Figure 2 As shown, a trajectory planning method for singular regions is provided. The method is applied to Figure 1 The robot in the example includes the following steps:

[0071] Step 202, obtaining the original planned trajectory of the robot, and determining multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory.

[0072] The original planned trajectory refers to the path planned by the trajectory planning algorithm, which can be expressed as Tr t , t=0,1…n.Tr t Indicates the trajectory position of the robot at time t. Trajectory planning can be performed using free space method, grid method, quadtree method, geometric representation of vector field flow, etc.

[0073] After obtaining the original planned trajectory of the robot, the position of the robot at any time can be obtained according to the original planned trajectory of the robot. The position of the robot at each time corresponds to a trajectory point.

[0074] The candidate joint posture sequence is obtained by inverting each trajectory point of the original planned trajectory of the robot, and the multiple sets of inverse solutions of the trajectory point are used to represent the posture of each joint of the robot at the trajectory point. Therefore, the joint posture sequence can be a multi-dimensional vector, and each dimensional vector corresponds to the position of a joint. Taking the robot as a six-joint manipulator as an example, the joint posture sequence is a six-dimensional vector, which represents the positions corresponding to the six joints of the robot.

[0075] Specifically, by inverting each trajectory point, we can obtain multiple sets of inverse solutions corresponding to each trajectory point, that is, multiple sets of candidate joint posture sequences, each set of candidate joint posture sequences represents the posture of each joint of the robot at this trajectory point. Multiple sets of candidate joint posture sequences can be obtained by invkine, that is, [q1, q2…q8] = invkine(Tr t ).

[0076] In this embodiment, offline singularity avoidance allows the trajectory planning module and the singularity avoidance module to be decoupled and executed sequentially, which simplifies the design and testing of the planning algorithm to a certain extent.

[0077] Step 204 , when it is determined that there are singular points in the original planned trajectory according to the multiple groups of candidate joint posture sequences of each trajectory point, the entry boundary point and the exit boundary point of the singular point are determined.

[0078] Most 6-axis articulated robots, due to mechanical or soft limit limitations, will have inverse kinematics solutions in their motion space, that is, the coordinate-based planned motion cannot be clearly reversed into the rotation angles of the robot's joint axes. These points in the robot's workspace where inverse kinematics solutions are not available are called singular points. Figure 3 As shown in the figure, given a trajectory, if there is a joint posture that enters the middle judgment area (the target joint posture is less than the first threshold), it means that there is a singular point in the trajectory, and the area where the target joint posture is less than the first threshold constitutes the singular area. The intersection of the range of the two side areas and the trajectory represents the entry boundary point and exit boundary point of the trajectory. The range of the entry boundary point and the exit boundary point will be slightly larger than the singular area. This is because the singular area is set too large, which will cause too many trajectories to be judged as singular.

[0079] Specifically, an inverse solution is obtained for each trajectory point to obtain multiple sets of candidate joint posture sequences corresponding to each trajectory point. According to the principle of continuity, one set of candidate joint posture sequences is selected from the multiple sets of candidate joint posture sequences, and the selected set of joint posture sequences is called the target joint posture. According to the target joint posture sequence, it is determined whether there is a singular point in the trajectory. According to the target joint posture sequence, when it is determined that there is a singular point in the trajectory, the entry boundary point and exit boundary point of the singular point are determined through the multiple sets of candidate joint posture sequences corresponding to each trajectory point and the entry boundary conditions and exit boundary conditions. Among them, the entry boundary point of the singular point is denoted as Trin , the exit boundary point of the singular point is denoted as Tr out .

[0080] Specifically, the entry boundary condition may be that the target joint posture is less than the second threshold value, and the speed is less than 0; the exit boundary condition may be that the target joint posture is greater than the second threshold value, and the speed is greater than 0. In other words, when the target joint posture sequence corresponding to a trajectory point meets the entry boundary condition, the trajectory point is an entry boundary point; when the target joint posture sequence corresponding to a trajectory point meets the exit boundary condition, the trajectory point is an exit boundary point.

[0081] Step 206, determining an entry joint posture sequence of the entry boundary point, and determining an exit joint posture sequence in the candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence.

[0082] Among them, the entry joint posture sequence is the target joint posture sequence of the entry boundary point, and the exit joint posture sequence is the target joint posture sequence of the exit boundary point. The entry joint posture sequence of the entry boundary point is denoted as q in , the exit joint posture sequence of the exit boundary point is recorded as q out .

[0083] Specifically, for the exit boundary point Tr out Find the inverse solution and obtain multiple sets of candidate joint pose sequences of the exit boundary point. Among the multiple sets of candidate joint pose sequences, determine the entry joint pose sequence q in The candidate joint posture sequence corresponding to the solution with the minimum weight is the exit joint posture sequence q of the exit boundary point out . Exit joint pose sequence Among them, the weight q dif =q in -q i ,q i is the candidate joint pose sequence corresponding to the exit boundary point. The exit joint pose sequence is a mirror solution of the entry joint pose sequence about the singular point.

[0084] Step 208, based on the entry joint posture sequence and the exit joint posture sequence, minimum vibration interpolation is performed on each trajectory point to be corrected within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with smooth transition of the trajectory point to be corrected.

[0085] Among them, the trajectory points to be corrected are the trajectory points within the range of the entry boundary point and the exit boundary point. Hogan proposed in 1984 that smoothness can be quantitatively analyzed through the vibration function. Vibration is the third-order derivative of the trajectory, that is, vibration is the derivative of acceleration with respect to time. Therefore, vibration is also the third differential of position with respect to time. Minimum vibration (Minimun JerkTrajectory) will directly constrain the position, velocity and acceleration of the head and tail. There are 6 equality constraints, so the optimization parameters must provide more than 6 degrees of freedom, and the fifth-order polynomial has 6 parameters. Therefore, the minimum order of the polynomial that meets the requirements is 5, that is, higher-order polynomials are also acceptable. Specifically, the order of the polynomial can be determined according to specific planning requirements.

[0086] Specifically, based on the principle of minimum vibration, interpolation is performed through trajectory polynomials. For example, a fifth-order trajectory polynomial x(t)=a0+a1t+a2t can be selected. 2 +a3t 3 +a4t 4 +a5t 5 According to the joint posture sequence q in and the exit joint pose sequence q out , determine the six parameters in the quintic polynomial and obtain the fifth-order polynomial of the trajectory. The fifth-order polynomial equation can achieve the smoothest relationship. Therefore, by sampling the trajectory at the time corresponding to the original planned trajectory, the joint posture sequence with smooth transition of the trajectory points to be corrected can be obtained, that is, q sam =x(t sam ), {t sam =t in , t in +δt,...,t out}

[0087] Step 210 , performing redundant degree of freedom planning on the joint posture sequence with smooth transition of the trajectory points to be corrected, to obtain a corrected planned trajectory.

[0088] Among them, redundant degrees of freedom are extra degrees of freedom. When a robot has redundant degrees of freedom, it can reach the same position through various different positions.

[0089] Redundant degree of freedom planning is to establish task redundancy, that is, to mark some orthogonal degrees of freedom in the task space as redundant degrees of freedom, and to ensure the control accuracy of non-redundant degrees of freedom by sacrificing the accuracy of some degrees of freedom to meet the conditions of redundant control.

[0090] Specifically, the corrected trajectory is determined according to the joint posture sequence obtained in the previous step, the Cartesian linear velocity of the original planned trajectory, and the Jacobian matrix representing the mapping relationship between the robot end effector velocity and the joint velocity.

[0091] In this embodiment, it is determined whether there is a singularity point through the original planned trajectory. For the original planned trajectory with a singularity point, the entry boundary point, the exit boundary point and the entry joint posture sequence are determined, and a mirror solution that is a mirror image of the entry joint posture sequence about the singularity point is determined as the exit joint posture sequence. Through the entry joint posture sequence and the exit joint posture sequence, minimum vibration interpolation is achieved to obtain a joint posture sequence with a smooth transition of the trajectory points to be corrected. Redundant degrees of freedom are planned for the joint posture sequence with a smooth transition to obtain a corrected planned trajectory that avoids singularities, so that the joint movement can smoothly transition from the entry joint posture sequence to the exit joint posture sequence without losing position accuracy.

[0092] In one embodiment, Figure 4 As shown, the method of determining whether there are singular points in the original planned trajectory based on multiple sets of candidate joint posture sequences of each trajectory point includes:

[0093] Step 402: for each trajectory point, determine a target joint posture sequence from multiple groups of candidate joint posture sequences; the target joint posture sequence includes the joint postures of each joint point of the robot.

[0094] The candidate joint posture sequence is used to represent the posture of each joint of the robot at this trajectory point. The target joint posture sequence is a set of candidate joint posture sequences.

[0095] Specifically, an inverse solution is obtained for each trajectory point on the original planned trajectory to obtain multiple sets of inverse solutions for each trajectory point, that is, multiple sets of candidate joint posture sequences. The target joint posture sequence is determined according to the target joint posture sequence at the previous moment and the candidate joint posture sequence at the current moment. Specifically, the modulus length of the two norm corresponding to each candidate joint posture sequence can be obtained by calculating the modulus length of the two norm of the difference between each candidate joint posture sequence and the target joint posture sequence, and the candidate joint posture sequence corresponding to the smallest modulus length of the two norm is the target joint posture sequence at the current moment.

[0096] Step 404: determine the target joint posture in the target joint posture sequence of each trajectory point.

[0097] The joint posture sequence is a six-dimensional vector, and the target joint point is the fifth scalar in the target joint posture sequence. The target joint posture is recorded as

[0098] Specifically, according to the target joint posture sequence of each trajectory point, the fifth scalar in each target joint posture sequence is determined as the target joint point of each trajectory of the target joint posture sequence.

[0099] Step 406: determine the trajectory points whose target joint posture is less than the first threshold as singular points.

[0100] The first threshold is a preset value, which can be set to 10 degrees or 20 degrees.

[0101] The relationship between the target joint posture and the first threshold is determined. When the target joint posture is less than the first threshold, the trajectory point corresponding to the target joint posture is a singular point.

[0102] In this embodiment, the target joint posture sequence is determined from the candidate joint posture sequence by the continuity principle, and the singular points in the trajectory are determined by judging the singular points of the target joint postures.

[0103] In one embodiment, determining a target joint posture sequence from multiple groups of candidate joint posture sequences includes: determining a joint posture sequence in the multiple groups of candidate joint posture sequences that is closest to a target joint posture sequence at a previous moment as the target joint posture sequence.

[0104] Among them, the target joint posture sequence at the previous moment is recorded as q t-1 , the target joint posture sequence is recorded as q t There are 8 groups of candidate joint posture sequences. The closest distance means the smallest modulus of the two norms.

[0105] Specifically, the inverse solution is obtained for each trajectory point to obtain multiple sets of candidate joint posture sequences [q1, q2…q8]. t-1 and the candidate joint posture sequence [q1, q2…q8] at the current moment, determine the modulus length of the two-norm of the difference between each candidate joint posture sequence and the target joint posture sequence, and obtain the modulus length of the two-norm corresponding to each candidate joint posture sequence, among which the candidate joint posture sequence corresponding to the smallest modulus length of the two-norm is the target joint posture sequence at the current moment. That is, the target joint posture sequence at the current moment is q t =argmin q；，i＝1.8 (||q t-1 -q i ||2).

[0106] In this embodiment, the target joint posture sequence at the current moment is determined by calculating the modulus length of the two-norm difference between each candidate joint posture sequence and the target joint posture sequence at the previous moment through multiple sets of candidate joint posture sequences at the current moment and the target joint posture sequence at the previous moment. According to the above method, the target joint posture sequence corresponding to each trajectory point is determined in turn.

[0107] In one embodiment, Figure 5 As shown, the entry boundary point and exit boundary point of the singular point are determined, including:

[0108] Step 502 , searching for a target joint posture in a target joint posture sequence of trajectory points within a preset range before and after the singular point.

[0109] The preset range is a pre-set range, the target joint posture sequence is a multi-dimensional vector, the target joint posture is one of the scalars in the target joint posture sequence, usually the fifth scalar in the target joint sequence, and the target joint posture is recorded as

[0110] According to the trajectory points within the preset range before and after the singular point, the target joint posture sequence corresponding to each trajectory point is determined, thereby determining the target joint posture in the target joint sequence of each trajectory point.

[0111] Step 504 : Determine the trajectory point corresponding to the target joint posture whose target joint posture is less than the second threshold and whose speed is less than 0 as the entry boundary point.

[0112] Among them, velocity is the derivative of the target joint posture with respect to time.

[0113] The target joint posture in the target joint posture sequence of the trajectory points within the preset range before and after the singular point is judged. When the target joint posture of a certain trajectory point is less than the second threshold and the speed is less than 0, the trajectory point corresponding to the target joint posture is determined as the entry boundary point, and the entry boundary point is recorded as Tr in .

[0114] Step 506: Determine the trajectory point corresponding to the target joint posture whose target joint posture is greater than the second threshold and whose speed is greater than 0 as the exit boundary point.

[0115] The target joint posture in the target joint posture sequence of the trajectory points within the preset range before and after the singular point is judged. When the target joint posture of a certain trajectory point is greater than the second threshold and the speed is greater than 0, the trajectory point corresponding to the target joint posture is determined as the exit boundary point, and the exit boundary point is recorded as Tr out .

[0116] In this embodiment, the entry boundary point and exit boundary point of the singularity point are determined by the target joint posture of the trajectory points within a preset range before and after the singularity point, the second threshold and the speed of the target joint posture, thereby providing conditions for avoiding the singularity point.

[0117] In one embodiment, an entry joint posture sequence of an entry boundary point is determined, and an exit joint posture sequence is determined in a candidate joint posture sequence of an exit boundary point based on the entry joint posture sequence, including: determining a target joint posture sequence of the entry boundary point as the entry joint posture sequence; and determining a joint posture sequence in the candidate joint posture sequence of the exit boundary point that has the smallest weighted joint angle with the entry joint posture sequence as the exit joint posture sequence.

[0118] Among them, the joint posture sequence is recorded as q in, the exit joint posture sequence is recorded as q out The entry joint posture sequence is the target joint posture sequence of the entry boundary points in the previous embodiment.

[0119] For the exit boundary point Tr in the previous embodiment out Find the inverse solution and obtain multiple candidate joint posture sequences, which are recorded as [q 10 ,q 20 …q 80 ]=invkine(Tr out ), the exit joint posture sequence is the joint posture sequence with the smallest weight with the entry joint posture sequence in the candidate joint posture sequence, that is, Among them, the weight Wq dif =[5, 1, 2, 0.1, 2], q dif =q in -q i ,q i is the candidate joint posture sequence corresponding to the exit boundary point. In other words, the difference between the 8 candidate joint posture sequences and the entry posture sequence is calculated to obtain 8 q dif , perform weighted summation on them, and the posture sequence corresponding to the minimum value among the 8 results is the exit joint posture sequence.

[0120] In this embodiment, by entering the joint posture sequence, a mirror solution that is a mirror image of the entering posture sequence about the singular point is obtained from multiple candidate joint posture sequences of the exit boundary point as the exit joint posture sequence, thereby providing conditions for avoiding the singular point.

[0121] In one embodiment, according to the entry joint posture sequence and the exit joint posture sequence, minimum vibration interpolation is performed on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with a smooth transition of the trajectory points to be corrected, including: according to the entry joint posture sequence and the exit joint posture sequence, a quintic polynomial is used to perform minimum vibration interpolation on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with a smooth transition of the trajectory points to be corrected.

[0122] The trajectory point to be corrected is a trajectory point between the entry boundary point and the exit boundary point.

[0123] Specifically, based on the input joint posture sequence q in and the exit joint pose sequence q out The principle of minimum vibration is used to interpolate through a quintic polynomial. Using the joint pose sequence q in and the exit joint pose sequence q out, for the quintic polynomial x(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 The six parameters in are solved. t0 is the time of entering the boundary point, t f is the time to exit the boundary point, To enter the joint posture sequence q in . To find the first derivative of a quintic polynomial, To find the second derivative of a quintic polynomial.

[0124] x(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5

[0125]

[0126]

[0127]

[0128]

[0129] The six parameters of the quintic polynomial are obtained through the above six equations. The trajectory is sampled using the time corresponding to the original planned trajectory point to obtain the smooth transition joint posture sequence of the planned trajectory point. The sampled joint posture sequence is recorded as q sam =x(t sａn ), {t sam =t in , t in +δt,...,t out}.

[0130] In this embodiment, by entering a joint posture sequence and exiting a joint posture sequence, minimum vibration interpolation is performed on the original planned trajectory points within the range of the entry boundary point and the exit boundary point to obtain a joint posture sequence with a smooth transition.

[0131] In one embodiment, Figure 6 As shown, redundant degrees of freedom planning is performed on the joint posture sequence with smooth transition of the trajectory points to be corrected to obtain the corrected planning trajectory, including:

[0132] Step 602, according to the joint posture sequence of the smooth transition of the trajectory points to be corrected, obtain the trajectory speed of the joint posture sequence of the smooth transition of the trajectory points to be corrected.

[0133] Specifically, the trajectory velocity of the smooth transition joint posture sequence is the first-order derivative of the smooth transition joint posture sequence with respect to time, denoted as

[0134] Step 604, determining the position increment according to the trajectory velocity and the Jacobian matrix.

[0135] The position increment is

[0136]

[0137] The Jacobian matrix is ​​the mapping relationship between the robot end effector velocity and the joint velocity. In this embodiment, J is the row of the Jacobian matrix Jac corresponding to the linear motion, that is, rows 1 to 3 of the Jacobian matrix. I is a 3x3 unit matrix. represents the Cartesian linear velocity of the original trajectory, Represents the trajectory velocity of the corrected sampled joint sequence. Both velocities are a sequence from the time corresponding to the entry boundary point to the time corresponding to the exit boundary point, and the obtained Δq is also a sequence.

[0138] Step 606, determining a corrected planned trajectory according to the position increment.

[0139] The corrected planning trajectory is

[0140]

[0141] Where Δq is the position increment, Correct the planned trajectory for the previous moment, is the corrected planned trajectory at the current moment. That is, the corrected planned trajectory at the current moment is obtained by the corrected planned trajectory at the previous moment and the calculated position increment.

[0142] In this embodiment, redundant degree of freedom planning is used to merge the original planned trajectory with the interpolated trajectory within the range of the entry boundary point and the exit boundary point, that is, after exiting the joint posture sequence, it can smoothly connect to the original planned trajectory. Redundant degree of freedom planning allows singular avoidance to be achieved without affecting the accuracy and response speed of the Cartesian position, while ensuring the position accuracy.

[0143] In one embodiment, Figure 7 As shown in FIG, a trajectory planning method for a singular region mainly includes the steps of inputting the original trajectory, judging the singular point, searching for the entry boundary and the exit boundary, determining the exit joint posture sequence, minimum vibration interpolation, redundant degree of freedom planning and outputting the corrected planned trajectory. Specifically, Figure 8 As shown, the above-mentioned trajectory planning method for the singular area includes the following steps:

[0144] Step 802, obtaining the original planned trajectory of the robot, and determining multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory.

[0145] Specifically, the original planning trajectory is Tr t , t=0,1...n.Tr t Represents the trajectory position of the robot at time t. After obtaining the original planned trajectory of the robot, the position of the robot at any time can be obtained according to the original planned trajectory of the robot. The position of the robot at each time corresponds to a trajectory point. Each trajectory point corresponds to multiple sets of joint posture sequences, called candidate joint posture sequences. Multiple sets of candidate joint posture sequences can be obtained by invkine, that is, [q1, q2…q8] = invkine(Tr t ).

[0146] Step 804 : for each trajectory point, determine a target joint posture sequence from multiple groups of candidate joint posture sequences.

[0147] According to the target joint posture sequence at the previous moment and the candidate joint posture sequence at the current moment, the modulus length of the two norm of the difference between each candidate joint posture sequence and the target joint posture sequence is determined, and the modulus length of the two norm corresponding to each candidate joint posture sequence is obtained, among which the candidate joint posture sequence corresponding to the smallest modulus length of the two norm is the target joint posture sequence at the current moment.

[0148] Step 806, judging whether there are singular points in the original planned trajectory according to the target joint posture sequence of each trajectory point.

[0149] According to the target joint posture sequence of each trajectory point, the target joint posture of the target joint posture sequence of each trajectory point is determined, and the relationship between the target joint posture and the first threshold is judged. When the target joint posture is less than the first threshold, the trajectory point corresponding to the target joint posture is a singular point.

[0150] Step 808: When there is a singular point in the original planned trajectory, determine the entry boundary point and the exit boundary point of the singular point.

[0151] According to the trajectory points within the preset range before and after the singular point, the target joint posture sequence corresponding to each trajectory point is determined, thereby determining the target joint posture in the target joint sequence of each trajectory point, that is, the fifth scalar of the target joint sequence. When the target joint posture of a certain trajectory point is less than the second threshold value and the speed is less than 0, the trajectory point corresponding to the target joint posture is determined to be an entry boundary point. When the target joint posture of a certain trajectory point is greater than the second threshold value and the speed is greater than 0, the trajectory point corresponding to the target joint posture is determined to be an exit boundary point.

[0152] Step 810, determining an entry joint posture sequence of an entry boundary point, and determining an exit joint posture sequence in a candidate joint posture sequence of an exit boundary point according to the entry joint posture sequence.

[0153] Exit boundary point Tr out Find the inverse solution and obtain multiple candidate joint posture sequences, which are recorded as [q 10 ,q 20 …q 80 ]=invkine(Tr out ), the exit joint posture sequence is the joint posture sequence with the smallest weight with the entry joint posture sequence in the candidate joint posture sequence, that is, Among them, the weight Wq dif =[5, 1, 2, 0.1, 2], q dif =q in -q i ,q i is the candidate joint pose sequence corresponding to the exit boundary point.

[0154] Step 812, based on the entry joint posture sequence and the exit joint posture sequence, use a quintic polynomial to perform minimum vibration interpolation on each trajectory point to be corrected within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with a smooth transition of the trajectory points to be corrected.

[0155] Step 814, performing redundant degree of freedom planning on the joint posture sequence with smooth transition of the trajectory points to be corrected, to obtain a corrected planned trajectory.

[0156] Step 816, performing error evaluation based on the original planned trajectory and the revised planned trajectory.

[0157] The corrected robot joint angle q mod The corrected robot terminal position Tr is obtained by solving the positive kinematics mod

[0158] T d =fkin(q mod )

[0159] The position and attitude errors are calculated using the following method:

[0160] P(e Tr )=||p d -p t ||

[0161]

[0162] Where P(e Tr ) represents the spatial position error, R(e Tr) represents the axis angle error of the spatial attitude. d and R d The second transformation matrix Tr is d The offset vector and rotation vector of . Where log(·) is the log transformation of the matrix, and the superscript v is the vectorization of the angular velocity matrix.

[0163] In order to verify the effect of correcting the planned trajectory of this application, the robot was tested to avoid wrist joint singularity. The test results are as follows Fig. 9 and Fig.10 As shown in FIG. Only the difference between joint 4 before and after the avoidance is shown, where the solid line represents the movement of joint 4 before the avoidance, and the dotted line represents the corresponding movement after the avoidance. Fig. 9 As shown in Figure 1, the joint 4 was deflected by nearly 180 degrees (3 rads) in the singular area before the correction, and the trajectory passed the singular area with a smaller deflection. The corresponding acceleration was also improved. Fig.10 As shown, the position error is only 10 -7 m, that is, there is almost no position error, and the attitude error is within 0.14rad, which is approximately equal to 8.4deg. It can be seen that the corrected planning trajectory obtained in this application has a good effect on avoiding singular points.

[0164] In this embodiment, it is determined whether there is a singularity point through the original planning trajectory. For the original planning trajectory with a singularity point, the entry boundary point, the exit boundary point and the entry joint posture sequence are determined, and a mirror solution that is a mirror image of the entry joint posture sequence about the singularity point is determined as the exit joint posture sequence. Interpolation of the singular area is achieved through the entry joint posture sequence, the exit joint posture sequence and redundant degree of freedom planning, so that the joint movement can smoothly transition from the entry posture to the corresponding mirror posture without losing position accuracy.

[0165] It should be understood that, although the steps in the flowcharts involved in the above embodiments are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps is not strictly limited in order, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above embodiments may include multiple steps or multiple stages, and these steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the steps or stages in other steps.

[0166] Based on the same inventive concept, the embodiment of the present application also provides a robot control device for implementing the trajectory planning method for the singular area involved above. The implementation solution provided by the device to solve the problem is similar to the implementation solution recorded in the above method, so the specific limitations in one or more robot control device embodiments provided below can refer to the limitations of the trajectory planning method for the singular area above, and will not be repeated here.

[0167] In one embodiment, Fig.11 As shown, a robot control device is provided, including a candidate joint posture sequence determination module 1102, an entry boundary point and an exit boundary point determination module 1104, an exit joint posture sequence determination module 1106, a minimum vibration interpolation module 1108 and a correction planning trajectory module 1110, wherein:

[0168] The candidate joint posture sequence determination module 1102 is used to obtain the original planned trajectory of the robot and determine multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory.

[0169] The entry boundary point and exit boundary point determination module 1104 is used to determine the entry boundary point and exit boundary point of the singular point when it is determined that the original planned trajectory has a singular point based on multiple groups of candidate joint posture sequences of each trajectory point.

[0170] The exit joint posture sequence determination module 1106 is used to determine the entry joint posture sequence of the entry boundary point, and determine the exit joint posture sequence in the candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence.

[0171] The minimum vibration interpolation module 1108 is used to perform minimum vibration interpolation on each trajectory point to be corrected within the range of the entry boundary point and the exit boundary point in the original planned trajectory according to the entry joint posture sequence and the exit joint posture sequence, so as to obtain a joint posture sequence with smooth transition of the trajectory points to be corrected.

[0172] The revised planned trajectory module 1110 is used to plan and determine redundant degrees of freedom for the joint posture sequence with smooth transition of the trajectory points to be revised, so as to obtain a revised planned trajectory.

[0173] In one embodiment, the robot control device also includes a singularity point determination module, which is used to determine a target joint posture sequence from multiple groups of candidate joint posture sequences for each trajectory point; the target joint posture sequence includes the joint postures of each joint point of the robot; determine the target joint posture in the target joint posture sequence of each trajectory point; and determine the trajectory point whose target joint posture is less than a first threshold as a singularity point.

[0174] In one embodiment, the singular point determination module further includes a target joint posture sequence module, which is used to determine the joint posture sequence closest to the target joint posture sequence at the previous moment in the multiple groups of candidate joint posture sequences as the target joint posture sequence.

[0175] In one embodiment, an entry boundary point and exit boundary point determination module is used to find the target joint posture in the target joint posture sequence of trajectory points within a preset range before and after a singularity point; the trajectory point corresponding to the target joint posture whose target joint posture is less than a second threshold and whose speed is less than 0 is determined as an entry boundary point; the trajectory point corresponding to the target joint posture whose target joint posture is greater than the second threshold and whose speed is greater than 0 is determined as an exit boundary point.

[0176] In one embodiment, the exit joint posture sequence determination module is used to determine the target joint posture sequence of the entry boundary point as the entry joint posture sequence; and determine the joint posture sequence with the smallest weighted joint angle with the entry joint posture sequence among the candidate joint posture sequences of the exit boundary point as the exit joint posture sequence.

[0177] In one embodiment, a minimum vibration interpolation module is used to perform minimum vibration interpolation on each to-be-corrected trajectory point within the range of an entry boundary point and an exit boundary point in the original planned trajectory according to an entry joint posture sequence and an exit joint posture sequence, using a quintic polynomial to obtain a joint posture sequence with a smooth transition of the to-be-corrected trajectory points.

[0178] In one embodiment, a corrected planned trajectory module is used to obtain a trajectory velocity of a joint posture sequence with smooth transition of trajectory points to be corrected according to a joint posture sequence with smooth transition of trajectory points to be corrected; determine a position increment according to the trajectory velocity and the Jacobian matrix; and determine a corrected planned trajectory according to the position increment.

[0179] Each module in the above robot control device can be implemented in whole or in part by software, hardware or a combination thereof. Each module can be embedded in or independent of a processor in a computer device in the form of hardware, or can be stored in a memory in a computer device in the form of software, so that the processor can call and execute operations corresponding to each module.

[0180] In one embodiment, a computer device is provided. The computer device may be a server, and its internal structure diagram may be as follows: Fig.12As shown. The computer device includes a processor, a memory and a network interface connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store trajectory data. The network interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a trajectory planning method for a singular area is implemented.

[0181] Those skilled in the art will understand that Fig.12 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0182] In one embodiment, a computer device is provided, including a memory and a processor, wherein a computer program is stored in the memory, and when the processor executes the computer program, the following steps are implemented:

[0183] Obtain the original planned trajectory of the robot, and determine multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory;

[0184] When it is determined that there are singular points in the original planned trajectory according to the multiple sets of candidate joint posture sequences of each trajectory point, an entry boundary point and an exit boundary point of the singular point are determined;

[0185] Determine an entry joint posture sequence of the entry boundary point, and determine an exit joint posture sequence in a candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence;

[0186] According to the entry joint posture sequence and the exit joint posture sequence, the minimum vibration interpolation is performed on each trajectory point to be corrected within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with smooth transition of the trajectory point to be corrected;

[0187] Redundant degrees of freedom planning is performed on the joint posture sequence with smooth transition of the trajectory points to be corrected to obtain the corrected planned trajectory.

[0188] In one embodiment, when the processor executes the computer program, the processor further implements the following steps:

[0189] For each trajectory point, a target joint posture sequence is determined from multiple groups of candidate joint posture sequences; the target joint posture sequence includes the joint postures of each joint point of the robot; the target joint posture in the target joint posture sequence of each trajectory point is determined; and the trajectory point whose target joint posture is less than a first threshold is determined as a singular point.

[0190] In one embodiment, when the processor executes the computer program, the processor further implements the following steps:

[0191] The joint posture sequence that is closest to the target joint posture sequence at the previous moment among the multiple groups of candidate joint posture sequences is determined as the target joint posture sequence.

[0192] In one embodiment, when the processor executes the computer program, the processor further implements the following steps:

[0193] Search for the target joint posture in the target joint posture sequence of trajectory points within a preset range before and after the singular point; determine the trajectory point corresponding to the target joint posture whose target joint posture is less than a second threshold and whose speed is less than 0 as the entry boundary point; determine the trajectory point corresponding to the target joint posture whose target joint posture is greater than the second threshold and whose speed is greater than 0 as the exit boundary point.

[0194] In one embodiment, when the processor executes the computer program, the processor further implements the following steps:

[0195] The target joint posture sequence of the entry boundary point is determined as the entry joint posture sequence; and the joint posture sequence with the smallest weighted joint angle with the entry joint posture sequence among the candidate joint posture sequences of the exit boundary point is determined as the exit joint posture sequence.

[0196] In one embodiment, when the processor executes the computer program, the processor further implements the following steps:

[0197] According to the entry joint posture sequence and the exit joint posture sequence, the minimum vibration interpolation is performed on each trajectory point to be corrected within the range of the entry boundary point and the exit boundary point in the original planned trajectory using a quintic polynomial to obtain a joint posture sequence with a smooth transition of the trajectory points to be corrected.

[0198] In one embodiment, when the processor executes the computer program, the processor further implements the following steps:

[0199] According to the joint posture sequence of the smooth transition of the trajectory points to be corrected, the trajectory speed of the joint posture sequence of the smooth transition of the trajectory points to be corrected is obtained; according to the trajectory speed and the Jacobian matrix, the position increment is determined; according to the position increment, the correction planning trajectory is determined.

[0200] In one embodiment, a computer readable storage medium is provided, on which a computer program is stored, and when the computer program is executed by a processor, the following steps are implemented:

[0201] Obtain the original planned trajectory of the robot, and determine multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory;

[0202] When it is determined that there are singular points in the original planned trajectory according to the multiple sets of candidate joint posture sequences of each trajectory point, an entry boundary point and an exit boundary point of the singular point are determined;

[0203] Determine an entry joint posture sequence of the entry boundary point, and determine an exit joint posture sequence in a candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence;

[0204] According to the entry joint posture sequence and the exit joint posture sequence, the minimum vibration interpolation is performed on each trajectory point to be corrected within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with smooth transition of the trajectory point to be corrected;

[0205] Redundant degrees of freedom planning is performed on the joint posture sequence with smooth transition of the trajectory points to be corrected to obtain the corrected planned trajectory.

[0206] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0207] For each trajectory point, a target joint posture sequence is determined from multiple groups of candidate joint posture sequences; the target joint posture sequence includes the joint postures of each joint point of the robot; the target joint posture in the target joint posture sequence of each trajectory point is determined; and the trajectory point whose target joint posture is less than a first threshold is determined as a singular point.

[0208] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0209] The joint posture sequence that is closest to the target joint posture sequence at the previous moment among the multiple groups of candidate joint posture sequences is determined as the target joint posture sequence.

[0210] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0211] Search for the target joint posture in the target joint posture sequence of trajectory points within a preset range before and after the singular point; determine the trajectory point corresponding to the target joint posture whose target joint posture is less than a second threshold and whose speed is less than 0 as the entry boundary point; determine the trajectory point corresponding to the target joint posture whose target joint posture is greater than the second threshold and whose speed is greater than 0 as the exit boundary point.

[0212] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0213] The target joint posture sequence of the entry boundary point is determined as the entry joint posture sequence; and the joint posture sequence with the smallest weighted joint angle with the entry joint posture sequence among the candidate joint posture sequences of the exit boundary point is determined as the exit joint posture sequence.

[0214] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0215] According to the entry joint posture sequence and the exit joint posture sequence, the minimum vibration interpolation is performed on each trajectory point to be corrected within the range of the entry boundary point and the exit boundary point in the original planned trajectory using a quintic polynomial to obtain a joint posture sequence with a smooth transition of the trajectory points to be corrected.

[0216] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0217] According to the joint posture sequence of the smooth transition of the trajectory points to be corrected, the trajectory speed of the joint posture sequence of the smooth transition of the trajectory points to be corrected is obtained; according to the trajectory speed and the Jacobian matrix, the position increment is determined; according to the position increment, the correction planning trajectory is determined.

[0218] In one embodiment, a computer program product is provided, comprising a computer program, which, when executed by a processor, implements the following steps:

[0219] Obtain the original planned trajectory of the robot, and determine multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory;

[0220] When it is determined that there are singular points in the original planned trajectory according to the multiple sets of candidate joint posture sequences of each trajectory point, an entry boundary point and an exit boundary point of the singular point are determined;

[0221] Determine an entry joint posture sequence of the entry boundary point, and determine an exit joint posture sequence in a candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence;

[0222] According to the entry joint posture sequence and the exit joint posture sequence, the minimum vibration interpolation is performed on each trajectory point to be corrected within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with smooth transition of the trajectory point to be corrected;

[0223] Redundant degrees of freedom planning is performed on the joint posture sequence with smooth transition of the trajectory points to be corrected to obtain the corrected planned trajectory.

[0224] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0225] For each trajectory point, a target joint posture sequence is determined from multiple groups of candidate joint posture sequences; the target joint posture sequence includes the joint postures of each joint point of the robot; the target joint posture in the target joint posture sequence of each trajectory point is determined; and the trajectory point whose target joint posture is less than a first threshold is determined as a singular point.

[0226] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0227] The joint posture sequence that is closest to the target joint posture sequence at the previous moment among the multiple groups of candidate joint posture sequences is determined as the target joint posture sequence.

[0228] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0229] Search for the target joint posture in the target joint posture sequence of trajectory points within a preset range before and after the singularity point; determine the trajectory points corresponding to the target joint postures whose target joint postures are less than a second threshold and whose speeds are less than 0 as entry boundary points; and determine the trajectory points corresponding to the target joint postures whose target joint postures are greater than the second threshold and whose speeds are greater than 0 as exit boundaries.

[0230] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0231] The target joint posture sequence of the entry boundary point is determined as the entry joint posture sequence; and the joint posture sequence with the smallest weighted joint angle with the entry joint posture sequence among the candidate joint posture sequences of the exit boundary point is determined as the exit joint posture sequence.

[0232] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0233] According to the entry joint posture sequence and the exit joint posture sequence, the minimum vibration interpolation is performed on each trajectory point to be corrected within the range of the entry boundary point and the exit boundary point in the original planned trajectory using a quintic polynomial to obtain a joint posture sequence with a smooth transition of the trajectory points to be corrected.

[0234] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:

[0235] According to the joint posture sequence of the smooth transition of the trajectory points to be corrected, the trajectory speed of the joint posture sequence of the smooth transition of the trajectory points to be corrected is obtained; according to the trajectory speed and the Jacobian matrix, the position increment is determined; according to the position increment, the correction planning trajectory is determined.

[0236] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM). The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. Non-relational databases may include distributed databases based on blockchains, etc., but are not limited to this. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but are not limited to this.

[0237] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0238] The above-described embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the present application. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.

Claims

1. A trajectory planning method for a singular region, characterized in that: The method comprises: Obtaining an original planned trajectory of the robot, and determining multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory; For each of the trajectory points, a target joint posture sequence is determined from the multiple groups of candidate joint posture sequences; the target joint posture sequence includes the joint postures of each joint point of the robot; the target joint posture in the target joint posture sequence of each of the trajectory points is determined; and the trajectory point whose target joint posture is less than a first threshold is determined as a singular point; Searching for the target joint posture in the target joint posture sequence of trajectory points within a preset range before and after the singular point; determining the trajectory point corresponding to the target joint posture whose target joint posture is less than a second threshold and whose speed is less than 0 as an entry boundary point; determining the trajectory point corresponding to the target joint posture whose target joint posture is greater than the second threshold and whose speed is greater than 0 as an exit boundary point; wherein the second threshold is used to ensure that the range of the entry boundary point and the exit boundary point is slightly larger than the singular area corresponding to the singular point; Determine an entry joint posture sequence of the entry boundary point, and determine an exit joint posture sequence in a candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence; According to the entry joint posture sequence and the exit joint posture sequence, minimum vibration interpolation is performed on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with smooth transition of the to-be-corrected trajectory point; Redundant degrees of freedom planning is performed on the joint posture sequence of the smooth transition of the trajectory points to be corrected to obtain a corrected planned trajectory.

2. The method according to claim 1, characterized in that Determining a target joint posture sequence from the multiple groups of candidate joint posture sequences comprises: The joint posture sequence that is closest to the target joint posture sequence at the previous moment among the multiple groups of candidate joint posture sequences is determined as the target joint posture sequence.

3. The method according to claim 1, characterized in that The step of determining an entry joint posture sequence of the entry boundary point and determining an exit joint posture sequence in a candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence comprises: Determining the target joint posture sequence of the entry boundary point as an entry joint posture sequence; Among the candidate joint posture sequences of the exit boundary points, the joint posture sequence with the smallest weighted joint angle with the entry joint posture sequence is determined as the exit joint posture sequence.

4. The method according to claim 1, characterized in that: According to the entry joint posture sequence and the exit joint posture sequence, minimum vibration interpolation is performed on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with smooth transition of the to-be-corrected trajectory point, including: According to the entry joint posture sequence and the exit joint posture sequence, a fifth-order polynomial is used to perform minimum vibration interpolation on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory to obtain a joint posture sequence with a smooth transition of the to-be-corrected trajectory point.

5. The method according to claim 1, characterized in that The step of performing redundant degree of freedom planning on the joint posture sequence of the smooth transition of the trajectory points to be corrected to obtain a corrected planned trajectory includes: According to the joint posture sequence of the trajectory points to be corrected with smooth transition, the trajectory speed of the joint posture sequence of the trajectory points to be corrected with smooth transition is obtained; Determining a position increment according to the trajectory velocity and the Jacobian matrix; A corrected planned trajectory is determined according to the position increment.

6. A robot control device, characterized in that: The device comprises: A candidate joint posture sequence determination module is used to obtain the original planned trajectory of the robot and determine multiple groups of candidate joint posture sequences corresponding to each trajectory point of the original planned trajectory; An entry boundary point and exit boundary point determination module is used to determine a target joint posture sequence from the multiple groups of candidate joint posture sequences for each of the trajectory points; the target joint posture sequence includes the joint postures of each joint point of the robot; determine the target joint posture in the target joint posture sequence of each of the trajectory points; determine the trajectory point whose target joint posture is less than a first threshold as a singular point; search for the target joint posture in the target joint posture sequence of the trajectory points within a preset range before and after the singular point; determine the trajectory point corresponding to the target joint posture whose target joint posture is less than a second threshold and whose speed is less than 0 as an entry boundary point; determine the trajectory point corresponding to the target joint posture whose target joint posture is greater than the second threshold and whose speed is greater than 0 as an exit boundary point; wherein the second threshold is used to ensure that the range of the entry boundary point and the exit boundary point is slightly larger than the singular area corresponding to the singular point; An exit joint posture sequence determination module is used to determine an entry joint posture sequence of the entry boundary point, and determine an exit joint posture sequence in a candidate joint posture sequence of the exit boundary point according to the entry joint posture sequence; A minimum vibration interpolation module is used to perform minimum vibration interpolation on each to-be-corrected trajectory point within the range of the entry boundary point and the exit boundary point in the original planned trajectory according to the entry joint posture sequence and the exit joint posture sequence, so as to obtain a joint posture sequence with smooth transition of the to-be-corrected trajectory point; The correction planning trajectory module is used to plan and determine the redundant degrees of freedom of the joint posture sequence with smooth transition of the trajectory points to be corrected, so as to obtain the correction planning trajectory.

7. The device according to claim 6, characterized in that The entry boundary point and exit boundary point determination module is also used for: The joint posture sequence that is closest to the target joint posture sequence at the previous moment among the multiple groups of candidate joint posture sequences is determined as the target joint posture sequence.

8. The device according to claim 6, characterized in that The entry boundary point and exit boundary point determination module is also used for: Determining the target joint posture sequence of the entry boundary point as an entry joint posture sequence; Among the candidate joint posture sequences of the exit boundary points, the joint posture sequence with the smallest weighted joint angle with the entry joint posture sequence is determined as the exit joint posture sequence.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 5 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 5 are implemented.

Images