Method for planning tail-end trajectory of series robot on basis of bidirectional greedy search algorithm

Abstract

A method for planning a tail-end trajectory of a series robot on the basis of a bidirectional greedy search algorithm. The method comprises the following steps: step S1, using a geometrical relationship of a series robot to acquire an inverse kinematic model of the robot; step S2, obtaining a physical feasible region of a joint pose of the robot on the basis of a model of the robot and of the environment; step S3, sorting spatial point locations according to a distance relationship, so as to obtain a spatial point location operation sequence of the robot; step S4, uniformly dividing the point locations into two parts, respectively starting from a start point and an end point to sequentially input the spatial point locations of the series robot into the inverse kinematic model of the robot, and planning semi-trajectories of the robot, so as to obtain two semi-trajectories; and step S5, connecting the two semi-trajectories to finally obtain a complete trajectory of the robot. By means of the method, the time used for planning a tail-end trajectory of a robot is reduced, a search space is reduced, and the operation efficiency of the robot is improved.

Description

Trajectory planning method for serial robot terminal based on bidirectional greedy search algorithm Technical Field

[0001] The present invention relates to the field of robot control technology, in particular to the field of robot trajectory planning, and more specifically to a method for planning the terminal trajectory of a serial robot based on a bidirectional greedy search algorithm. Background Art

[0002] Serial robots are widely used in industrial production for tasks such as measurement, welding, and assembly due to their flexible workspace and strong scalability. These tasks can be described as requiring the robot's end-user to pass through a set sequence of locations, completing the specified task at each location, without collision. Due to the multi-solution nature of robot inverse kinematics, each end-user position does not necessarily correspond to a single position in joint space. Furthermore, due to the robot's collision constraints, a straight trajectory in joint space may not always be achievable.

[0003] At present, the commonly used method for trajectory planning of the robot end is based on random search, but the trajectory obtained is generally long, the robot operation efficiency is low, and the repeatability of the results obtained each time is poor.

[0004] Summary of the Invention

[0005] In order to solve the problems and shortcomings existing in the above-mentioned prior art, the present invention proposes a terminal trajectory planning method for a serial robot based on a bidirectional greedy search algorithm. Through the bidirectional greedy search trajectory planning method of the starting point and the end point, the two-process parallel method is used to reduce the time used for robot terminal trajectory planning. At the same time, the greedy method is adopted in the posture selection of the serial robot, which reduces the search space and improves the operation efficiency of the robot, and has good industrial application value.

[0006] In order to achieve the above-mentioned object of the invention, the technical solution of the present invention is as follows:

[0007] A method for trajectory planning of a serial robot terminal based on a bidirectional greedy search algorithm includes the following steps:

[0008] Step S1. Obtaining an inverse kinematics model of the robot using the geometric relationship of the serial robot;

[0009] Step S2. Obtaining the physical feasible domain of the robot joint pose based on the robot and environment models;

[0010] Step S3. Sort the spatial points according to their distance relationship to obtain the spatial point operation sequence of the robot;

[0011] Step S4. Divide the points into two equal parts, starting from the starting point and the end point respectively, input the spatial points of the serial robot into the robot inverse kinematics model in sequence, and plan the robot half-trajectory to obtain two half-trajectories;

[0012] Step S5: Connect the two half trajectories to finally obtain the complete trajectory of the robot.

[0013] Preferably, the step S1 specifically includes:

[0014] Step S11. Obtaining the robot geometric relationship through the CAD model or robot calibration method;

[0015] Step S12. Establishing a robot kinematic model using the robot geometric relationship;

[0016] Step S13: Establishing a robot inverse kinematics model through the robot kinematics model.

[0017] Preferably, the step S2 specifically includes:

[0018] Step S21. Randomly select a large number of joint poses in the robot joint pose space;

[0019] Step S22. In the selected joint pose, the positions of the robot joints and the positions of the environment components corresponding to the joint pose are calculated, and collision detection is performed on the CAD models. If a collision occurs, the joint pose is considered infeasible, otherwise it is considered feasible.

[0020] Step S23: Obtain the physical feasible region of the entire serial robot by interpolation.

[0021] Preferably, step S3 specifically includes:

[0022] Step S31. Obtain the starting point, end point, and intermediate space points that the robot needs to pass through according to the robot task;

[0023] Step S32: Project the intermediate spatial points onto the line connecting the starting point and the end point, divide the spatial points equally along the line, and use their order as the two original sequences;

[0024] Step S33. Starting from the first point in the original sequence, add it to the output sequence and delete the point in the original sequence;

[0025] Step S34: Select a series of points in the original sequence whose distance from the starting point in the measurement direction is less than a threshold to obtain a local sequence. If the length of the local sequence is less than or equal to 1, return to step S33;

[0026] Step S35. Sort the local sequence in ascending order of spatial distance from the starting point, traverse the local sequence, and if the current traversed point is not the first point of the original sequence, add it to the output sequence; otherwise, stop traversing;

[0027] Step S36: Loop steps S33-S35 until the original sequence is empty. At this time, the output sequence is the planned point operation sequence.

[0028] Preferably, the step S4 specifically includes:

[0029] Step S41: Use a dual process to process two point operation sequences respectively, starting from the starting point and the end point respectively, and input the robot spatial points in the two point sequences into the robot inverse kinematics model in sequence to obtain two joint space sequences;

[0030] Step S42. Establish two half-trajectories with the starting point and the end point of the two joint space sequences as the first point, and traverse the two feasible space sequences respectively;

[0031] Step S43. Loop and take the starting point of the previous position, randomly select a certain amount of postures from the points in the current joint space sequence that are not added to the half-trajectory as the end point, take the straight line trajectory of the starting and end point postures in the posture space, interpolate, and calculate whether each interpolation point collides. If all points do not collide, then the straight line trajectory is called a straight line feasible trajectory, and this trajectory is used as the feasible trajectory of the robot. Select the robot trajectory with the shortest running time from all feasible trajectories, add the half-trajectory, and continue the loop. Otherwise, jump to step S44.

[0032] Step S44. Use the random search tree method to search for a feasible trajectory in the joint pose space, then try to optimize the trajectory by jump connection, and use the trajectory as the robot trajectory and add it to the half-trajectory sequence.

[0033] Preferably, step S5 specifically includes:

[0034] Step S51. Select the end points of the two half trajectories in step S4 as the middle points;

[0035] Step S52: Take the straight line trajectory of the middle point in the joint pose space, perform interpolation, and calculate whether each interpolation point has a collision. If all points do not collide, then the trajectory is used as the robot's middle trajectory;

[0036] Step S53: Connect the starting half track, the middle track, and the ending half track in sequence to finally obtain the complete robot track.

[0037] Preferably, the step S22 specifically includes:

[0038] Step S22.1. Obtain accurate CAD models of each joint of the robot and its environment;

[0039] Step S22.2. Triangulate the CAD model and obtain all vertices and vertex normals of the triangulated CAD model;

[0040] Step S22.3. Shift the coordinates of the vertices of the triangulated CAD model by a fixed value along the vertex normal to obtain the outer envelope of the CAD model;

[0041] Step S22.4. Shift the coordinates of the vertices of the triangulated CAD model by a fixed value in the opposite direction of the vertex normal to obtain the inner envelope of the CAD model;

[0042] Step S22.5. Combining the triangulated CAD model, the CAD model outer envelope, and the CAD model inner envelope to obtain an extended CAD model;

[0043] Step S22.6. Calculate the minimum convex envelope of the extended CAD model;

[0044] Step S22.7. Represent the position of each component as a homogeneous matrix, multiply the homogeneous matrix by the second coordinate of the minimum convex body envelope of each component, and calculate the world coordinate of the minimum convex body envelope of each component;

[0045] Step S22.8. Use the minimum convex body envelopes of each model in the world coordinate system to perform intersection detection between each two models. If they intersect, it means a collision has occurred; otherwise, it means no collision has occurred.

[0046] Preferably, for a complete robot, the solution of the inverse kinematics model is a segregated solution, and the geometrically feasible space is an empty set or a finite set; for a redundant robot, the solution of the inverse kinematics model is a continuous solution, which is represented by a spatial range.

[0047] Preferably, the step S43 specifically includes:

[0048] Step S43.1. Randomly select an endpoint pose in the joint space sequence based on its weighted Euclidean distance to the starting point. Calculate whether the pose is within the physically feasible domain. If so, add it to the feasible point sequence. Otherwise, remove its neighbors within the physically feasible domain. Repeat the selection process until the length of the feasible endpoint sequence reaches a preset value or the physically feasible domain is empty.

[0049] Step S43.2. Traverse the feasible endpoint sequence {T i}, select the end point T i =(θ i，1 ,θ i，2 ,…,θ i，n ) and starting point Perform linear interpolation to obtain N intermediate points Calculate whether the N intermediate points are in the physical feasible domain. If not, a collision occurs. Otherwise, no collision occurs. i，1 ,θ i，2 ,…,θ i，n are the coordinates of each joint at the end point, are the coordinates of each joint at the starting point, M ij Indicates the coordinates of the middle point.

[0050] Preferably, the step S44 specifically includes:

[0051] Step S44.1. Establish a random search tree with the starting point as the root;

[0052] Step S44.2. Randomly select a point in the physical feasible domain using a certain probability distribution function. With this randomly selected point as the center, sort all nodes in the random search tree from smallest to largest according to their Euclidean distance to this point to obtain a corresponding node sequence. Traverse this node sequence and use the interpolation and verification method of step S43 to check whether there is a straight-line feasible trajectory from the current point to all nodes in the current tree as described in step S43. If so, make it a child of this node, and terminate the loop.

[0053] The distance is calculated as follows: let the randomly selected point in the physical feasible region be (α1, α2, ..., α n ), the nodes in the random search tree are (β1, β2, ..., β n ), the distance d is calculated as: Among them, (α1,α2,…,α n ) and (β1, β2, …, β n ) represent the joint coordinates of the points.

[0054] Step S44.3. Traverse the search tree starting from the root node, and use the interpolation and verification method of step S43 to try whether other searched nodes are linearly feasible with the nodes on the current trajectory. If feasible and the distance is shorter, replace the current trajectory with it.

[0055] Beneficial effects of the present invention:

[0056] (1) Aiming at the problem of low efficiency of robot terminal trajectory planning in the prior art, the present invention proposes a trajectory planning method with a two-way greedy search from the starting point and the end point. The time used for robot terminal trajectory planning is reduced by a two-process parallel method. At the same time, a greedy method is adopted in the posture selection of the serial robot, which reduces the search space and improves the operation efficiency of the robot. It has good industrial application value.

[0057] (2) This invention addresses the problem of long search trajectories and low operating efficiency in existing technologies for robot terminal trajectory planning. It proposes a method that combines straight-line trajectory search with random search tree search. In the straight-line random search, a method is proposed to select as short a trajectory as possible by designing a distribution function. In the random search tree search, a method is proposed to optimize the trajectory by combining a random search tree with a graph optimization method. This reduces the robot's search trajectories and improves operating efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] The foregoing and following detailed description of the present invention will become more apparent when read in conjunction with the following drawings, in which:

[0059] FIG1 is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0060] In order to enable those skilled in the art to better understand the technical solutions of the present invention, the following will further illustrate the technical solutions for achieving the purpose of the present invention through several specific embodiments. It should be noted that the technical solutions claimed for protection by the present invention include but are not limited to the following embodiments. Based on the embodiments of the present invention, 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 invention.

[0061] Serial robots are widely used in industrial production for tasks such as measurement, welding, and assembly due to their flexible workspace and strong scalability. These tasks can be described as requiring the robot's end-user to pass through a set sequence of locations, completing the specified task at each location, without collision. Due to the multi-solution nature of robot inverse kinematics, each end-user position does not necessarily correspond to a single position in joint space. Furthermore, due to the robot's collision constraints, a straight trajectory in joint space may not always be achievable.

[0062] At present, the commonly used method for trajectory planning of the robot end is based on random search, but the trajectory obtained is generally long, the robot operation efficiency is low, and the repeatability of the results obtained each time is poor.

[0063] Based on this, an embodiment of the present invention proposes a method for terminal trajectory planning of a serial robot based on a bidirectional greedy search algorithm. The trajectory planning method based on bidirectional greedy search from the starting point and the end point reduces the time used for terminal trajectory planning of the robot through a two-process parallel method. At the same time, a greedy method is used in the posture selection of the serial robot, which reduces the search space and improves the operation efficiency of the robot, and has good industrial application value.

[0064] This embodiment discloses a method for planning the trajectory of a serial robot terminal based on a bidirectional greedy search algorithm. Referring to FIG1 of the specification, the method is described in detail as follows.

[0065] Step S1: Obtain the inverse kinematics model of the robot using the geometric relationship of the serial robot.

[0066] In this embodiment, the joint coordinates of the robot can be expressed as end coordinates through step S1. The specific steps are as follows:

[0067] Step S11: Obtain the geometric relationship of the robot through the CAD model or robot calibration method.

[0068] In this embodiment, the CAD models are robot joint and environment CAD models, where the robot joints refer to all joints, including tools, etc.; and the environment refers to fixed tooling, measurement / operation objects, etc.

[0069] Step S12: Establishing a robot kinematic model using the acquired robot geometric relationship.

[0070] Step S13: Inversely solve the robot kinematics model to establish an inverse kinematics model of the robot.

[0071] As an embodiment of the present invention, the robot is a robot with six rotation joints, and its kinematic model can be expressed as follows:

[0072] Furthermore, its inverse kinematics model can be expressed as eight branch functions, that is, for any spatial position There are eight possible positions:

[0073] Among them, f j It is an inverse kinematics model; Represents the transfer matrix from the first joint of the robot to the base; Represents the transfer matrix from the second joint to the first joint of the robot; Represents the transfer matrix from the third joint to the second joint of the robot; Represents the transfer matrix from the fourth joint to the third joint of the robot; Represents the transfer matrix from the fifth joint to the fourth joint of the robot; represents the transfer matrix from the sixth joint to the fifth joint of the robot; θ1 represents the rotation angle of the first joint of the robot; θ2 represents the rotation angle of the second joint of the robot; θ3 represents the rotation angle of the third joint of the robot; θ4 represents the rotation angle of the fourth joint of the robot; θ5 represents the rotation angle of the fifth joint of the robot; θ6 represents the rotation angle of the sixth joint of the robot; θ j represents the set of robot joint coordinates; θ j，1 ,θ j，2 ,θ j，3 ,θ j，4 ,θj，5 ,θ j，6 Indicates the coordinates of the six joints of the robot corresponding to different solutions; j indicates the number of solutions; The coordinates of any spatial position P, x, y, z represent the position, Represents direction.

[0074] Furthermore, when it has a real number solution, it means that the feasible position exists, otherwise it means that the position does not exist.

[0075] As another embodiment of the present invention, the robot is a robot with six rotational joints mounted on a two-degree-of-freedom translation slide. In this case, its kinematic model can be expressed as:

[0076] in, Represents the transfer matrix from the translation slide to the world coordinate system;

[0077] For a given slide position d1, d2, its inverse kinematics solution can also be expressed as eight branch functions, that is, for any given spatial position and the slide positions d1, d2, there are eight possible positions:

[0078] If a real number solution exists, the feasible position exists; otherwise, it does not exist. By giving different values ​​of d1 and d2 and combining them with interpolation methods, the robot's inverse kinematic solution can be obtained. Unlike the previous invention example, this solution is a subspace of the joint space and is not a segregated solution.

[0079] Step S2: Using the robot joints and the environment CAD model, obtain the physical feasible domain of the robot joint pose.

[0080] In this embodiment, step S2 specifically includes the following contents.

[0081] Step S21: Randomly select a large number of joint poses in the robot joint pose space.

[0082] In this embodiment, it's important to note that the robot has two spaces: the joint space and the operation space. The operation space can be understood as the three-dimensional coordinates (position and orientation) in the world coordinate system, while the joint space is the coordinates of each joint of the robot. The robot's kinematic model is simply the transformation matrix between the operation space and the joint space. In other words, if the operation space moves to a certain position, then how should the robot's joints move to achieve that position?

[0083] Step S22. In the selected joint pose, calculate the positions of each joint of the robot and the positions of each component of the environment corresponding to the joint pose, and perform collision detection on each CAD model; if a collision occurs, it is an infeasible joint pose, otherwise it is a feasible joint pose.

[0084] In this embodiment, step S22 further includes the following contents.

[0085] Step S22.1. Obtain accurate CAD models of each joint of the robot and its environment;

[0086] Step S22.2. Triangulate the CAD model, and obtain all vertices and vertex normals of the triangulated CAD model;

[0087] Step S22.3. Shift the vertex coordinates of the triangulated CAD model by a fixed value along the vertex normal to obtain the outer envelope of the CAD model;

[0088] Step S22.4. Shift the vertex coordinates of the triangulated CAD model by a fixed value in the opposite direction of the vertex normal to obtain the inner envelope of the CAD model;

[0089] Step S22.5. Combining the triangulated CAD model, the CAD model outer envelope, and the CAD model inner envelope to obtain an extended CAD model;

[0090] Step S22.6. Calculate the minimum convex envelope of the extended CAD model;

[0091] Step S22.7. Represent the position of each component as a homogeneous matrix, multiply the homogeneous matrix by the homogeneous coordinates of the minimum convex body envelope of each component, and calculate the world coordinates of the minimum convex body envelope of each component;

[0092] Step S22.8. Use the minimum convex body envelopes of each model in the world coordinate system to perform intersection detection between each two. If they intersect, it means a collision has occurred; otherwise, it means no collision has occurred.

[0093] In this embodiment, it should be noted that the various components refer to the robot joints and the CAD models in the environment after being processed in steps S22.1-S22.6.

[0094] Step S23: Obtain the physical feasible region of the entire serial robot by interpolation.

[0095] In this embodiment, interpolation refers to kinematic interpolation, which is a technical means well known to those skilled in the art and will not be described in detail here.

[0096] In one embodiment of the present invention, the robot is a robot with six rotating joints, and the environment is a frame and a workpiece to be processed. First, accurate CAD models of the six joints of the robot and the workpiece are established, and then the method is used to obtain the minimum convex body envelope of each model (a total of eight). The eight minimum convex body envelope models can be represented as a series of key points {S ij}, i = 1, 2, ..., 8. In the world coordinates, the transformation matrix from the gantry to the world coordinates is obtained by calibration and registration. And the transformation matrix from the workpiece to the world coordinates This gives the transformation relationship of each component, including the robot:

[0097] frame:

[0098] Artifacts:

[0099] in, Represents the coordinates of a space point in the world coordinate system;

[0100] The physical feasible region is then calculated using the method described above.

[0101] Step S3: Sort the spatial points according to the distance relationship to obtain the spatial point operation sequence of the robot.

[0102] In this embodiment, step S3 specifically includes the following contents.

[0103] Step S31. Obtain the starting point, end point, and intermediate space points that the robot needs to pass through according to the robot task;

[0104] Step S32: Project the intermediate spatial points onto the line connecting the starting point and the end point, equally divide the spatial points along the line, and use their order as two original sequences, referred to as the starting point original sequence and the end point original sequence. The following steps are performed on each of the two original sequences.

[0105] Step S33. Starting from the first point in the original sequence, add it to the output sequence and delete the point in the original sequence;

[0106] Step S34: Select a series of points in the original sequence whose distance from the starting point in the measurement direction is less than a threshold to obtain a local sequence. If the length of the local sequence is less than or equal to 1, return to step S33;

[0107] Step S35. Sort the local sequence in ascending order of spatial distance from the starting point, traverse the local sequence, and if the current traversed point is not the first point of the original sequence, add it to the output sequence; otherwise, stop traversing;

[0108] Step S36: Loop steps S33-S35 until the original sequence is empty. At this time, the output sequence is the planned point operation sequence.

[0109] In this embodiment, the threshold value of step S34 is generally related to the size of the robot and the size of the workspace, and can be 200 mm in this example.

[0110] In this embodiment, the starting point original sequence and the ending point original sequence in step S32 correspond to the starting point running sequence and the ending point running sequence respectively.

[0111] Step S4. Divide the points into two parts, starting from the starting point and the end point respectively, input the spatial points of the serial robot into the robot inverse kinematics model in sequence, plan the robot half-trajectory, and obtain two half-trajectories.

[0112] In this embodiment, step 4 specifically includes the following contents.

[0113] Step S41: Divide the robot spatial point sequence obtained in step S3 into two equal parts. Use a dual process to process the two point operation sequences separately, starting from the starting point and the end point respectively. Input the robot spatial points in the two point sequences into the robot inverse kinematics model in sequence to obtain two joint spatial sequences.

[0114] Step S42. Establish two half-trajectories with the starting point and the end point of the two joint space sequences as the first point, and traverse the two feasible space sequences respectively;

[0115] Step S43. Loop and take the previous position as the starting point, randomly select a certain number of postures from the points in the current joint space sequence that are not added to the half-trajectory as the end point, take the straight line trajectory of the starting and end postures in the posture space, interpolate, and calculate whether each interpolation point collides. If all points do not collide, then the straight line trajectory is called a straight line feasible trajectory, and this trajectory is used as the feasible trajectory of the robot. Select the robot trajectory with the shortest running time from all feasible trajectories, add the half-trajectory, and continue the loop. Otherwise, jump to step S44.

[0116] Furthermore, the step S43 specifically includes the following contents:

[0117] Step S43.1. Randomly select an endpoint pose in the joint space sequence based on its weighted Euclidean distance to the starting point, and calculate whether the pose is within the physically feasible domain described in step S2. If so, add it to the feasible point sequence; otherwise, remove its neighbors within the physically feasible domain. Repeat the selection process until the length of the feasible endpoint sequence reaches a preset value or the physically feasible domain is empty.

[0118] Step S43.2. Traverse the feasible endpoint sequence {T i}, select the end point Ti=(θ i，1 ,θ i，2 ,…,θ i，n ) and starting point Perform linear interpolation to obtain N intermediate points Calculate whether the N intermediate points are in the physical feasible domain of step S2. If not, a collision occurs; otherwise, no collision occurs. i，1 ,θ i，2 ,…,θ i，n are the coordinates of each joint at the end point, are the coordinates of each joint at the starting point, M ij Indicates the coordinates of the middle point.

[0119] In one embodiment of the present invention, the robot is a robot with six rotational joints, and the inverse kinematics model can be expressed as:

[0120] The physical feasible region is the set of real function values ​​of eight functions, and the starting point

[0121] θ s =(θ s，1 ,θ s，2 ,θ s，3 ,θ s，4 ,θ s，5 ,θ s，6 ) can be expressed as:

[0122] Among them, θ j，1 ,θ j，2 ,θ j，3 ,θ j，4 ,θ j，5 ,θ j，6 and θ s，1 ,θ s，2 ,θ s，3 ,θ s，4 ,θ s，5 ,θ s，6 are the joint coordinates of any point and the starting point respectively.

[0123] Sort all feasible solutions in ascending order according to the Euclidean distance, and verify their feasibility one by one using the linear interpolation method described in step S43.2.

[0124] Step S44. Use the random search tree method to search for a feasible trajectory in the joint pose space, then try to optimize the trajectory by jump connection, and use the trajectory as the robot trajectory and add it to the half-trajectory sequence.

[0125] Furthermore, the step S44 specifically includes the following contents.

[0126] Step S44.1. Establish a random search tree with the starting point as the root;

[0127] Step S44.2. Randomly select a point in the physical feasible domain using a certain probability distribution function. With this random point as the center, sort all nodes in the random search tree in ascending order of distance to this point, and establish a node sequence. Traverse this node sequence, using the interpolation and verification method of step S43 to check whether there is a straight line feasible trajectory from the current point to all nodes in the current tree as described in step S43. If so, make it a child of this node, terminate the loop, and use this point as the end point. The line connecting it and the starting point is the current trajectory.

[0128] Step S44.3. Traverse the search tree starting from the root node, and use the interpolation and verification method of step S43 to try whether other searched nodes are linearly feasible with the nodes on the current trajectory. If feasible and the distance is shorter, replace the current trajectory with it.

[0129] In one embodiment of the present invention, the robot is a robot having six rotational joints, and the inverse kinematics model can be expressed as:

[0130] The feasible physical domain is the set of real function values ​​of eight functions. The point obtained in each search can be expressed as ψ t =(ψ t，1 , ψ t，2 , ψ t，3 , ψ t，4 , ψ t，5 , ψ t，6 ), represents the coordinates of the point. Based on the random search tree, a companion kd tree is established. Take the companion kd tree to ψ t Nodes with a distance less than a given value d0 establish a node sequence, any point to ψ t The distance d j It can be expressed as:

[0131] Among them, ψ t，1 , ψ t，2 , ψ t，3 , ψt，4 , ψ t，5 , ψ t，6 and All represent the coordinates of a point;

[0132] Traverse the node sequence and use the interpolation and verification method of step S43 to detect whether there is a straight line feasible trajectory from the current point to all nodes of the current tree as described in step S43. If so, make it a child node of the node, end the loop, and make it the end point. The line connecting it and the starting point is the current trajectory.

[0133] Step S5: Connect the two half trajectories to finally obtain the complete trajectory of the robot.

[0134] In this embodiment, step S5 specifically includes the following contents.

[0135] Step S51: Select the end points of the two half trajectories in step S4 as the middle points.

[0136] Step S52. Take the straight line trajectory of the middle point in the joint posture space, perform interpolation, and calculate whether each interpolation point has a collision. If all points do not collide, then the trajectory is used as the middle trajectory of the robot.

[0137] Step S53: Connect the starting half track, the middle track, and the ending half track in sequence to finally obtain the complete robot track.

[0138] Based on the same inventive concept, an embodiment of the present invention further proposes a serial robot device suitable for the above-mentioned bidirectional greedy search algorithm, the robot device comprising:

[0139] The motion component module contains more than six movable joints, and the end has six complete spatial degrees of freedom, including three translational degrees of freedom and three rotational degrees of freedom;

[0140] Power drive module, each movable joint has a corresponding power drive module, which can drive the movable joint to rotate along the axis with a single degree of freedom or to move in a single degree of freedom;

[0141] The control module can control the power drive module to move to a given position at a given speed in the form of instructions.

[0142] In order to implement the trajectory planning method described in the above embodiment, an embodiment of the present invention further proposes a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the serial robot terminal trajectory planning method described in the above embodiment can be implemented.

[0143] It should be noted that the computer-readable medium described above in the present invention may be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or component, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to, an electrical connection having one or more conductors, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof. In the present invention, a computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, device, or component. In the present invention, a computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. This propagated data signal may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. A computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium that can transmit, propagate, or transport a program for use by or in conjunction with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium may be transmitted using any suitable medium, including but not limited to wires, optical cables, RF (radio frequency), etc., or any suitable combination thereof.

[0144] Furthermore, an embodiment of the present invention also provides an electronic device, which includes a processor, a memory, and a computer program stored in the memory and runnable on the processor. When the processor executes the computer program, the steps of the serial robot terminal trajectory planning method described in the above embodiment are implemented.

[0145] The electronic device may be a computing device such as a desktop computer, a notebook computer, a PDA, a cloud server, etc. The electronic device may include, but is not limited to, a processor and a memory. For example, the electronic device may also include input and output devices, network access devices, a bus, etc.

[0146] The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor, or the processor may be any conventional processor, etc. The memory may be an internal storage unit of the electronic device, such as a hard disk or memory of a terminal device. The memory may also be an external storage device of the electronic device, such as a plug-in hard disk equipped on the electronic device, a smart media card (SMC), a secure digital (SD) card, a flash memory card, etc. Furthermore, the memory may include both the internal storage unit of the electronic device and an external storage device. The memory is used to store the computer program and other programs and data required by the electronic device. The memory may also be used to temporarily store data that has been output or is to be output.

[0147] The computer-readable medium may be included in the electronic device or may exist independently without being incorporated into the electronic device. The computer-readable medium carries one or more programs, and when the one or more programs are executed by the electronic device, the electronic device executes the above-mentioned embodiments.

[0148] The above description is only a preferred embodiment of the present invention and does not constitute any form of obstruction to the present invention. Any simple modification or equivalent change made to the above embodiment based on the technical essence of the present invention shall fall within the scope of protection of the present invention.

Claims

1. A serial robot terminal trajectory planning method based on a bidirectional greedy search algorithm, characterized in that: The steps include: Step S1. Obtaining an inverse kinematics model of the robot using the geometric relationship of the serial robot; Step S2. Obtaining the physical feasible domain of the robot joint posture according to the model of the robot and the environment; Step S3. Sort the spatial points according to the distance relationship to obtain the spatial point operation sequence of the robot; Step S4. Divide the points into two parts, starting from the starting point and the end point respectively, input the spatial points of the serial robot into the robot inverse kinematics model in sequence, plan the robot half trajectory, and obtain two half trajectories; Step S5: Connect the two half trajectories to finally obtain the complete trajectory of the robot.

2. The method for terminal trajectory planning of a serial robot based on a bidirectional greedy search algorithm according to claim 1 is characterized in that: The step S1 specifically includes: Step S11. Obtaining the robot geometric relationship through a CAD model or a robot calibration method; Step S12. Establishing a robot kinematic model using the robot geometric relationship; Step S13: Establishing a robot inverse kinematics model through the robot kinematics model.

3. The method for terminal trajectory planning of a serial robot based on a bidirectional greedy search algorithm according to claim 2 is characterized in that: The step S2 specifically includes: Step S21. randomly selecting a large number of joint poses in the robot joint pose space; Step S22. In the selected joint pose, calculate the positions of each joint of the robot and the positions of each component of the environment corresponding to the joint pose, and perform collision detection on the CAD models in pairs; if a collision occurs, it is an infeasible joint pose, otherwise it is a feasible joint pose; Step S23: Obtain the physical feasible domain of the entire serial robot by interpolation.

4. The method for terminal trajectory planning of a serial robot based on a bidirectional greedy search algorithm according to claim 3 is characterized in that: The step S3 specifically includes: Step S31. Obtain the starting point, end point, and intermediate space points that the robot needs to pass through according to the robot task; Step S32: Project the intermediate spatial points onto the line connecting the starting point and the end point, divide the spatial points equally along the line, and use their order as two original sequences; Step S33. Starting from the first point in the original sequence as the starting point, adding it to the output sequence, and deleting the point in the original sequence; Step S34. Select a series of points in the original sequence whose distance from the starting point in the measurement direction is less than a threshold to obtain a local sequence. If the length of the local sequence is less than or equal to 1, return to step S33; Step S35. Sort the local sequence in ascending order according to the spatial distance from the starting point, traverse the local sequence, and if the current point of the traversal is not the current first point of the original sequence, add it to the output sequence, otherwise stop traversing; Step S36. Loop steps S33-S35 until the original sequence is empty, and the output sequence is the planned point operation sequence.

5. The method for terminal trajectory planning of a serial robot based on a bidirectional greedy search algorithm according to claim 4 is characterized in that: The step S4 specifically includes: Step S41. Use a dual process to process two point operation sequences respectively, starting from the starting point and the end point respectively, and input the robot space points in the two point sequences into the robot inverse kinematics model in sequence to obtain two joint space sequences; Step S42. Using the starting point and the end point of the two joint space sequences as the first point, two half-trajectories are established respectively, traversing the two feasible space sequences respectively; Step S43. Loop to take the starting point of the previous position, randomly select a certain amount of postures from the points in the current joint space sequence that are not added to the half-trajectory as the end point, take the straight line trajectory of the starting point and the end point posture in the posture space, interpolate, and calculate whether each interpolation point collides. If all points do not collide, then the straight line trajectory is called a straight line feasible trajectory, and this trajectory is used as the robot feasible trajectory. Select the robot trajectory with the shortest running time from all feasible trajectories, add the half-trajectory, and continue the loop, otherwise jump to step S44; Step S44. Use the random search tree method to search for a feasible trajectory in the joint pose space, then try to optimize the trajectory by jump connection, use the trajectory as the robot trajectory, and add it to the half-trajectory sequence.

6. The method for terminal trajectory planning of a serial robot based on a bidirectional greedy search algorithm according to claim 5, characterized in that: The step S5 specifically includes: Step S51. Select the end points of the two half trajectories in step S4 as the middle points; Step S52. Take the straight line trajectory of the middle point in the joint posture space, perform interpolation, and calculate whether each interpolation point collides. If all points do not collide, use the trajectory as the middle trajectory of the robot; Step S53: Connect the starting half track, the middle track and the ending half track in sequence to finally obtain a complete robot track.

7. The method for terminal trajectory planning of a serial robot based on a bidirectional greedy search algorithm according to claim 3, characterized in that: The step S22 specifically includes: Step S22.

1. Obtain accurate CAD models of each joint of the robot and the environment; Step S22.

2. triangulate the CAD model, and obtain all vertices and vertex normals of the triangulated CAD model; Step S22.

3. Move the coordinates of the vertices of the triangulated CAD model by a fixed value along the vertex normal to obtain the outer envelope of the CAD model; Step S22.

4. Shift the coordinates of the vertices of the triangulated CAD model by a fixed value in the opposite direction of the vertex normal to obtain the inner envelope of the CAD model; Step S22.

5. Combining the triangulated CAD model, the CAD model outer envelope and the CAD model inner envelope to obtain an extended CAD model; Step S22.

6. Calculate the minimum convex body envelope of the extended CAD model; Step S22.

7. Represent the position of each component as a homogeneous matrix, multiply the homogeneous matrix by the second coordinate of the minimum convex body envelope of each component, and calculate the world coordinate of the minimum convex body envelope of each component; Step S22.

8. Use the minimum convex body envelopes of each model in the world coordinate system to perform intersection detection between each two. If they intersect, it means a collision has occurred; otherwise, it means no collision has occurred.

8. The method for terminal trajectory planning of a serial robot based on a bidirectional greedy search algorithm according to claim 1 or 5, characterized in that: For a complete robot, the solution of the inverse kinematics model is a segregated solution, and the geometrically feasible space is an empty set or a finite set; for a redundant robot, the solution of the inverse kinematics model is a continuous solution, represented by a spatial range.

9. The method for planning the terminal trajectory of a serial robot based on a bidirectional greedy search algorithm according to claim 5, characterized in that: The step S43 specifically includes: Step S43.

1. Randomly select the endpoint pose in the joint space sequence according to the weighted Euclidean distance to the starting point, calculate whether the pose is in the physical feasible domain, and if so, add it to the feasible point sequence, otherwise remove its neighborhood in the physical feasible domain, and repeat the selection until the length of the feasible endpoint sequence reaches a preset value or the physical feasible domain is empty; Step S43.

2. Traverse the feasible endpoint sequence {T i }, select the end point T i =(θ i，1 ,θ i，2 ,…,θ i，n ) and starting point Perform linear interpolation to obtain N intermediate points Calculate whether the N intermediate points are in the physical feasible domain. If not, a collision occurs. Otherwise, no collision occurs. i，1 ,θ i，2 ,…,θ i，n are the coordinates of each joint at the end point, are the coordinates of each joint at the starting point, M ij represents the coordinates of the middle point; j represents the sequence of N selected middle points.

10. The method for terminal trajectory planning of a serial robot based on a bidirectional greedy search algorithm according to claim 5, characterized in that: The step S44 specifically includes: Step S44.

1. Establish a random search tree with the starting point as the root; Step S44.

2. Randomly select a point in the physical feasible domain according to a certain probability distribution function, take the randomly selected point as the center, sort all the nodes in the random search tree from small to large according to the Euclidean distance to the point, obtain the corresponding node sequence, traverse the node sequence, use the interpolation and verification method of step S43 to detect whether there is a straight line feasible trajectory from the current point to all the nodes of the current tree as described in step S43, if there is, take it as the child node of the node, and end the loop; Step S44.

3. Traverse the search tree from the root node, and use the interpolation and verification method of step S43 to try whether other searched nodes are linearly feasible with the nodes on the current trajectory. If feasible and the distance is shorter, replace the current trajectory with it.

Images