Motion planning method and system for multi-degree-of-freedom robot in dynamic environment

Abstract

The present invention relates to the field of robot control technology, and in particular to a multi-degree-of-freedom robot dynamic environment motion planning method and system thereof. The multi-degree-of-freedom robot dynamic environment motion planning method comprises a discrete posture generation step, a robot-robot collision detection step and a dynamic path collision detection step, and the corresponding system comprises a discrete posture generation module, a robot-robot collision detection module and a dynamic path collision detection module. The multi-degree-of-freedom robot dynamic environment motion planning method and system thereof provided by the present invention, by optimizing the dynamic path collision detection algorithm, not only reduces the amount of calculation of dynamic path collision detection, but also increases the space for improving hardware resource utilization through FPGA dedicated accelerator, thereby improving hardware utilization and operation speed, and is worthy of promotion.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a multi-degree-of-freedom robot dynamic environment motion planning method and system thereof. Background Art

[0002] In the process of multi-degree-of-freedom robot motion planning, collision detection will inevitably be required when using the random landmark method based on probability sampling. When there are multiple robotic arms in the scene, the remaining robotic arms will become obstacles to the robotic arm that is planning the path. Collision detection is required in path planning. The traditional collision detection method uses the axial bounding box method or the directional bounding box method. For multi-degree-of-freedom robotic arms, there are also improved bounding sphere and capsule models based on this. The connecting rod of the robotic arm is regarded as a capsule with length and radius, and the obstacle is wrapped with a sphere. The collision detection process is ultimately simplified to the calculation of the shortest distance between the line segment and the point in space. By judging whether the distance between the line segment and the point is greater than the sum of the capsule and sphere radii, it is judged whether the two collide. For collision detection between robotic arms, according to the same principle, it is simplified to the calculation of the shortest distance between line segments in space. This type of model-based collision detection method has a large number of complex mathematical formula operations, including a large number of multiplications and multiplication and accumulation operations, which will consume a large amount of computing resources of general-purpose processors. If a dedicated accelerator design based on FPGA can be adopted, it will effectively improve computing efficiency, save computing resources and reduce computing time. Summary of the invention

[0003] In order to solve the problems existing in the prior art, the present invention provides a multi-degree-of-freedom robot dynamic environment motion planning method and system thereof, which improves the computing efficiency by adopting an FPGA dedicated accelerator and optimizing a collision detection algorithm.

[0004] The technical solution adopted by the present invention is as follows:

[0005] The method for motion planning of a multi-degree-of-freedom robot in a dynamic environment comprises the following steps:

[0006] The discrete posture generation step generates a series of discrete static postures of the planned robot arm according to the path starting point and path end point of the robot arm, and generates a timestamp. According to the predicted timestamp, the posture of the obstacle robot arm at the corresponding time is calculated;

[0007] The robot-robot collision detection step detects whether the planning robot and the obstacle robot collide according to each timestamp, and outputs the detection result as a Boolean value;

[0008] Dynamic path collision detection step,After obtaining the static robot-robot collision detection results, if any group collides, the entire path will be considered to have collided.

[0009] Preferably, the robot-robot collision detection step comprises the following sub-steps:

[0010] The trigonometric function solver step converts the input radians of each joint of the robotic arm into corresponding trigonometric function values;

[0011] The forward kinematics solver step maps the robot arm from the joint space to the Cartesian space through forward kinematics calculation;

[0012] The connecting rod shortest distance calculation sub-step regards the two connecting rods involved in the detection as two line segments in space and calculates the shortest connection length between the two line segments;

[0013] The robot-robot collision determination sub-step determines whether the connecting rods collide by comparing the shortest connection length between the two robot arms with a preset collision detection threshold, and outputs the determination result as a Boolean value.

[0014] Preferably, the multi-degree-of-freedom robot dynamic environment motion planning method further comprises a discrete posture generation step, and the discrete posture generation step is used to discretize the dynamic continuous robot path into a plurality of static robot postures.

[0015] Preferably, the multi-degree-of-freedom robot dynamic environment motion planning method further comprises a dynamic path collision detection step, and the dynamic path collision detection step is used to determine whether the current path collides with the path being executed by the obstacle robot arm.

[0016] Preferably, the number of discrete static postures generated in the discrete posture generating step is 32.

[0017] The multi-degree-of-freedom robot dynamic environment motion planning system is characterized by comprising the following modules:

[0018] The discrete posture generation module generates a series of discrete static postures of the planned robot arm according to the path starting point and path end point of the robot arm, and generates a timestamp. According to the predicted timestamp, the posture of the obstacle robot arm at the corresponding time is calculated;

[0019] The robot-robot collision detection module detects whether the planning robot and the obstacle robot collide according to each timestamp and outputs the detection result as a Boolean value;

[0020] Dynamic path collision detection module, after obtaining the static robot-robot collision detection results, if any group collides, the entire path will be considered to have collided.

[0021] Preferably, the robot-robot collision detection module includes the following submodules:

[0022] The trigonometric function solver module converts the input radians of each joint of the robotic arm into corresponding trigonometric function values;

[0023] The forward kinematics solver module maps the robot arm from joint space to Cartesian space through forward kinematics calculation;

[0024] The connecting rod shortest distance calculation submodule regards the two connecting rods involved in the detection as two line segments in space and calculates the shortest connection length between the two line segments;

[0025] The robot-robot collision judgment submodule determines whether the connecting rods collide by comparing the shortest connection length between the two robot arms with a preset collision detection threshold, and outputs the judgment result as a Boolean value.

[0026] Preferably, the multi-degree-of-freedom robot dynamic environment motion planning system further comprises a discrete posture generation module, and the discrete posture generation module is used to discretize the dynamic continuous robot path into a plurality of static robot postures.

[0027] Preferably, the multi-degree-of-freedom robot dynamic environment motion planning system further includes a dynamic path collision detection module, which is used to determine whether the current path collides with the path being executed by the obstacle robot arm.

[0028] Preferably, the number of discrete static gestures generated in the discrete gesture generation module is 32.

[0029] Beneficial technical effects:

[0030] The multi-degree-of-freedom robot dynamic environment motion planning method and system provided by the present invention not only reduce the amount of dynamic path collision detection calculations by optimizing the dynamic path collision detection algorithm, but also increase the space for improving hardware resource utilization through FPGA dedicated accelerators, thereby improving hardware utilization and computing speed, and is worthy of promotion. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 It is a schematic diagram of two six-degree-of-freedom robotic arms installed face to face;

[0032] Figure 2 It is a schematic diagram of the mathematical model analysis of two six-degree-of-freedom robotic arms installed face to face;

[0033] Figure 3 It is a schematic diagram of the motion planning system of a multi-degree-of-freedom robot in a dynamic environment. DETAILED DESCRIPTION

[0034] In order to make the objectives, technical solutions and advantages of the present invention more clear, further detailed description will be given below in conjunction with the accompanying drawings and specific embodiments.

[0035] This embodiment uses a case where two six-degree-of-freedom robotic arms are installed face to face for illustration. The solution of the present invention is not limited to a six-degree-of-freedom robotic arm, nor is it limited to a system of two robotic arms.

[0036] Mechanical model analysis simplification:

[0037] See also Figure 1-Figure 2 , a combination of two six-degree-of-freedom robotic arms installed face to face, where the base origin coordinates of one robotic arm are (-325mm, 0mm, -264mm), hereinafter referred to as robotic arm A, and the base origin coordinates of the other robotic arm are (325mm, 0mm, -264mm), hereinafter referred to as robotic arm B. The installation method of the two robotic arms is as follows Figure 1 The robot arm used in this embodiment is composed of 6 rotating joints. The spatial orientation angle of the end point of the connecting rod has no effect on the static robot arm-robot arm collision detection. The 3rd and 4th connecting rods can be simplified to the same connecting rod. The 5th and 6th connecting rods and the grasped objects can be simplified to the same connecting rod for calculation. The model of the 6-DOF robot arm in the system is simplified to 4 connecting rods as shown in Figure 2 According to the robot forward kinematics principle, the coordinates of the five endpoints of the four connecting rods (LD0, LD1, LD2, LD3, LD4) are obtained by solving the states of the first five joints.

[0038] The number of collision checks is simplified:

[0039] Due to the structure of the robot arms and the constraints of the installation model, all the links of one robot arm will not collide with the link 1 (LD0-LD1) of another robot arm, and the link 2 (LD1-LD2) of one robot arm will not collide with the link 2 of another robot arm. Therefore, only 8 groups of links need to be collided for the 4 links of each of the two robots.

[0040] Basic functional unit design:

[0041] The multi-DOF robot dynamic environment motion planning system is composed of multiple basic functional units. Some basic functional units will be implemented in FPGA in hardware form, using hardware parallelism and DSP (Digital Signal Processor) Slice to accelerate the execution speed of key operations, thereby accelerating dynamic path collision detection. In order to facilitate the exchange of data between software and hardware, each basic functional unit mainly uses Q format to exchange data during design. The basic functional units include: discrete posture generation module, trigonometric function solution module, forward kinematics solution module, connecting rod shortest distance calculation module, robot-robot collision judgment module, and dynamic path collision detection module.

[0042] Trigonometric function solving:

[0043] Before the forward kinematics solution, the input radians of each joint need to be converted into the corresponding trigonometric function values. In order to reduce the amount of calculation in the process of solving the trigonometric function values, the sine table is saved in RAM and the results are obtained by table lookup. Since the strokes of each joint of the robot arm have upper and lower limits, the periodicity, parity, and phase difference of the sine and cosine functions of the trigonometric functions can be used to complete the solution of the sine and cosine functions with a small amount of RAM.

[0044] The trigonometric function solving module first determines the value range of each joint radian input, aligns each joint radian to the corresponding sine and cosine addresses on the RAM through phase shift, and reads the table lookup results of each joint radian in turn through a pipeline method. Finally, according to the value range of each joint radian, the positive and negative signs of the calculation results are corrected.

[0045] The trigonometric function calculation module inputs the radian values ​​of the five joints of a robot arm each time, using 16-bit, Q13 format, which can represent radians between -4rad and +3.9999rad, covering the travel of each joint of the robot arm. The output result of the trigonometric function calculation module is the sine and cosine values ​​of the corresponding joints, using 8-bit, Q7 format, which can represent trigonometric function values ​​ranging from -1 to +0.9921875.

[0046] Forward kinematics solution:

[0047] The forward kinematics solution module can map the robot from joint space to Cartesian space. Through forward kinematics calculation, the coordinates of the five connecting rod nodes of the robot are obtained.

[0048] The Cartesian coordinates of LD0 and LD1 are constants, which directly generate results. The forward kinematics solution module simplifies the calculation process, adjusts the calculation order, and uses some pre-calculated results based on the DH parameters of the six-DOF robot arm used by the system. It makes full use of the functional structure of the DSP Slice of the Artix-7 series FPGA to solve the coordinates of LD2, LD3, and LD4 in sequence without affecting the circuit timing.

[0049] The input data of the forward kinematics solver module is the 10 trigonometric function values ​​required for solving the forward kinematics of a robot arm, represented in 8-bit, Q7 format. The output data of the forward kinematics solver module is the Cartesian coordinates of the 5 endpoints of the 4 connecting rods of the robot arm, represented in 16-bit, Q3 format, which can represent Cartesian coordinates of -1024mm to +1024mm.

[0050] Calculation of the shortest distance of the connecting rod:

[0051] The connecting rod shortest distance calculation module regards the two connecting rods involved in the detection as two line segments in space (L s , L t ), where L s The starting point and end point are S0, S1, L t The starting point and end point are T0 and T1 respectively. Using the principles of spatial analytic geometry, the shortest distance between two line segments is solved. The solution process is as follows:

[0052] Define two lines l in space s , l t They are line segments L s and L t The shortest distance points of the two line segments are S x and T x , the direction vectors are:

[0053]

[0054] Let the shortest distance between two points on the straight line be s j ,t j , their corresponding scalar is s c ,t c , then:

[0055]

[0056] make

[0057]

[0058] Then we have:

[0059]

[0060] When two lines are not coincident and parallel, The straight line is the only common perpendicular line between the two straight lines, that is:

[0061]

[0062] Then we can obtain:

[0063]

[0064] make

[0065]

[0066] Then we have:

[0067]

[0068] When ac-d2=0, the two straight lines are parallel. By taking any point on the straight lines as the common perpendicular line, the foot of the common perpendicular line on the other straight line can be found.

[0069] Due to the different relative positions of the two line segments in space, the position of the shortest distance point needs to be discussed in 9 cases. When sc or tc is less than 0, sj or tj is outside the starting point of the line segment, and s0 or t0 is the shortest distance point; when sc or tc is greater than 1, sj or tj is outside the end point of the line segment, and s1 or t1 is the shortest distance point; when sc or tc is between 0 and 1, the foot of the common perpendicular line of the two straight lines sj, tj is the shortest distance point sx, tx of the two line segments, and the shortest distance between the two line segments is:

[0070] d(L s ,L t )=|s x -t x |

[0071] The connecting rod shortest distance calculation module first performs vector calculation based on the coordinates of the two connecting rods, and preliminarily determines the positional relationship of the shortest distance points of the two connecting rods; if the shortest distance point is on the line segment, a 6-bit precision division operation is performed to calculate the position of the shortest distance point on the line segment; finally, the specific coordinates of the two shortest distance points are calculated, and the square of the shortest distance is calculated for subsequent robot-robot collision judgment.

[0072] The input data of the connecting rod shortest distance calculation module is the Cartesian coordinates of the four endpoints of the two connecting rods that need to be determined, expressed in 16-bit, Q3 format; the output data is the square of the shortest distance expressed in 18-bit after reducing the precision.

[0073] Robot arm-robot arm collision determination:

[0074] After obtaining the shortest distances of the eight links, the robot-robot collision judgment module compares each shortest distance with the corresponding threshold according to the radius and safety threshold of the links to determine whether the links collide, and generates a Boolean value for the judgment result: 1 for collision and 0 for no collision. Finally, the eight judgment results are logically "or" operated. Only when the distances of all link pairs are legal (output logic 0), the robot-robot collision judgment is legal (output logic 0).

[0075] The input value of the robot-robot collision judgment module is the square of the 8 shortest distances, represented by 18 bits; the output value is a 1-bit Boolean value, indicating whether a collision occurs.

[0076] Discrete static pose generation:

[0077] In the process of dynamic path collision detection, the discrete static posture generation module needs to generate 32 discrete static postures of the planned manipulators according to the path starting point and the path end point, and calculate the posture of the obstacle manipulator at the corresponding time according to the predicted timestamp. Finally, the generated discrete static postures are combined to perform static manipulator-manipulator collision detection. The safety of the dynamic path is indirectly detected through collision detection between discrete static postures.

[0078] In order to carry out subsequent detection, after completing the motion planning, the discrete static posture generation module needs to write the start timestamp, end timestamp and each static discrete posture point of the planned final route into the lookup table.

[0079] The discrete static posture generation module needs to input the 5 joint arc values ​​of the path start and end of the planned robot arm when working, using 16-bit, Q13 format. At the same time, in order to predict the timestamp of each discrete static posture, the timestamp of the path start needs to be input in 32-bit format. The discrete static posture generation module outputs 2 sets of discrete static postures each time, using 16-bit, Q13 format.

[0080] The discrete static posture generation module needs to input no more than 700 discrete static postures in the initialization state, using 16-bit, Q13 format. At the same time, in order to calculate the search address of the obstacle robot arm, it is also necessary to input the start and end timestamps of the entire planned route, respectively using 32-bit.

[0081] Dynamic Path Collision Detection:

[0082] After obtaining 32 sets of static robot-robot collision judgment results, the dynamic path collision detection module performs a logical "OR" operation on the Boolean values ​​of all judgment results. Only when all static robot-robot collision judgment results are legal (output logic 0), the dynamic path collision detection result is legal (output logic 0).

[0083] The input data of the dynamic path collision detection module is a 32-bit Boolean value indicating whether each posture has a collision, and the output data is a 1-bit Boolean value indicating whether the path has a collision.

[0084] The same functional subunits run in parallel:

[0085] See also Figure 3For basic functional units that need to be called multiple times during motion planning, increasing the number of functional units and allowing several identical functional units to execute in parallel can reduce execution time overhead, but at the same time hardware resource overhead will increase. For an algorithm that requires a module to be executed i*n times repeatedly to complete, if the module is instantiated n times in hardware and implemented in parallel, without considering synthesizer optimization, the total hardware resources it needs to consume are n times the basic resources; and the total execution time overhead is i times the time of a single hardware execution. For modules that are implemented synchronously by software and hardware, its hardware resource overhead is determined by the number of hardware parallel implementations, and the total execution time is related to the implementation form that takes the longest time. When multiple parallel implementations have the situation where the results are returned in advance, the execution time overhead should be calculated according to the longest situation.

[0086] By adopting a dedicated FPGA-based accelerator design, the upper limit of hardware resource constraints can be increased. The maximum delay that the actual system real-time can tolerate will constrain the upper limit of the execution time overhead.

[0087] Solve multi-objective optimization problems using any method, eliminate dominant solutions and implement the system:

[0088] In multi-objective optimization problems, there are conflicts and incomparable situations: a solution is optimal in one objective but not in another; when optimizing one objective, the other objective will be weakened. Such solutions are called non-dominated solutions. In this implementation, because the model is small and there are fewer configurations to choose from, all non-dominated solutions can be obtained within a few minutes using the exhaustive traversal method. All non-dominated solution configurations are optimal configurations, and any optimal configuration is selected for the actual system implementation.

[0089] In this embodiment, by optimizing the dynamic path collision detection algorithm, not only the amount of dynamic path collision detection calculations is reduced, but also the space for improving hardware resource utilization through FPGA dedicated accelerators is increased, thereby improving hardware utilization and calculation speed.

[0090] It should be noted that in the above embodiment, the case of two six-degree-of-freedom manipulators installed face to face is used for illustration, but the solution of the present invention is not limited to six-degree-of-freedom manipulators, nor is it limited to a system of two manipulators. For embodiments with larger models and more optional configurations, there are situations where the exhaustive traversal method cannot quickly obtain all non-dominated solutions. In this case, other algorithms can be used to obtain non-dominated solutions, including but not limited to genetic algorithms with elite decision-making (NSGA-II), fuzzy algorithms (FUZZY), simulated annealing algorithms, etc.

[0091] The above are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A motion planning method for a multi-degree-of-freedom robot in a dynamic environment, characterized in that: The steps include: The discrete posture generation step generates a series of discrete static postures of the planned robot arm according to the path starting point and path end point of the robot arm, and generates a timestamp. According to the predicted timestamp, the posture of the obstacle robot arm at the corresponding time is calculated; The robot-robot collision detection step detects whether the planning robot and the obstacle robot collide according to each timestamp, and outputs the detection result as a Boolean value; Dynamic path collision detection step, after obtaining the static robot-robot collision detection results, if any group collides, the entire path will be considered to have collided; The robot-robot collision detection step comprises the following sub-steps: The trigonometric function solver step converts the input radians of each joint of the robotic arm into corresponding trigonometric function values; The forward kinematics solver step maps the robot arm from the joint space to the Cartesian space through forward kinematics calculation; The connecting rod shortest distance calculation sub-step regards the two connecting rods involved in the detection as two line segments in space and calculates the shortest connection length between the two line segments; A robot-robot collision determination substep, which determines whether the connecting rods collide by comparing the shortest connection length between the two robot arms with a preset collision detection threshold, and outputs the determination result as a Boolean value; The discrete posture generating step is used to discretize the dynamic continuous robot path into a plurality of static robot postures; The dynamic path collision detection step is used to determine whether the current path collides with the path being executed by the obstacle robot arm.

2. The method for motion planning of a multi-degree-of-freedom robot in a dynamic environment according to claim 1, characterized in that: The number of discrete static postures generated in the discrete posture generating step is 32.

3. A multi-degree-of-freedom robot dynamic environment motion planning system, characterized in that: Includes the following modules: The discrete posture generation module generates a series of discrete static postures of the planned robot arm according to the path starting point and path end point of the robot arm, and generates a timestamp. According to the predicted timestamp, the posture of the obstacle robot arm at the corresponding time is calculated; The robot-robot collision detection module detects whether the planning robot and the obstacle robot collide according to each timestamp and outputs the detection result as a Boolean value; Dynamic path collision detection module, after obtaining the static robot-robot collision detection results, if any group collides, the entire path will be considered to have collided; The robot-robot collision detection module includes the following submodules: The trigonometric function solver module converts the input radians of each joint of the robotic arm into corresponding trigonometric function values; The forward kinematics solver module maps the robot arm from joint space to Cartesian space through forward kinematics calculation; The connecting rod shortest distance calculation submodule regards the two connecting rods involved in the detection as two line segments in space and calculates the shortest connection length between the two line segments; The robot-robot collision judgment submodule determines whether the connecting rods collide by comparing the shortest connection length between the two robot arms with a preset collision detection threshold, and outputs the judgment result as a Boolean value; The discrete posture generation module is used to discretize the dynamic continuous robot path into a plurality of static robot postures; The dynamic path collision detection module is used to determine whether the current path collides with the path being executed by the obstacle robot arm.

4. The multi-degree-of-freedom robot dynamic environment motion planning system according to claim 3, characterized in that: The number of discrete static gestures generated in the discrete gesture generation module is 32.

Images