A sampling-based time-optimal trajectory planning algorithm for robotic arms

Abstract

The application discloses a kind of sampling-based mechanical arm time optimal trajectory planning algorithm, it includes: the modeling of industrial robot body;Industrial robot is dynamically modeled, extract the relationship between joint space and corresponding joint angular acceleration, as the drawing condition of trajectory planning map of high-dimensional joint space;The obtained dynamic model is preprocessed, and the joint space sampling probability map is obtained with the reference standard of target joint angular acceleration by space discretization of trajectory planning map;The improved RRT algorithm is used to complete the motion trajectory planning optimization design of mechanical arm.The application can improve the joint utilization of industrial robot as a whole, realize the trajectory planning optimization of mechanical arm with time optimal orientation.

Description

TECHNICAL FIELD

[0001] The present application relates to an industrial robot trajectory planning method, in particular to a sampling-based time-optimal trajectory planning algorithm for a mechanical arm. BACKGROUND

[0002] Industrial robots play an important role in modern industrial production. With the slowing down of population growth and the decrease of working-age population, the increase of labor cost highlights the importance of industrial robots. However, traditional robot trajectory planning methods mostly only consider kinematics and ignore dynamics, resulting in low torque utilization rate of some joints and inability to achieve time-optimal planning.

[0003] Traditional trajectory planning methods are single-joint trajectory planning in joint space independently, but the overall planning effect cannot be optimal due to the change of moment of inertia caused by the change of other joints. Current trajectory planning methods include graph search, ant colony algorithm, artificial potential field algorithm and sampling-based algorithm, among which the sampling-based algorithm is of great concern due to its autonomous planning ability in high-dimensional space. Due to the very complex workspace of a six-degree-of-freedom mechanical arm, the use of traditional algorithms often faces problems such as insufficient computing power. The sampling-based algorithm has the advantages of fast planning speed, less calculation and wide application, but the random sampling method lacks guidance, the obtained path has strong randomness and the expansion is disorderly, which cannot be directly used for trajectory planning of the mechanical arm. SUMMARY

[0004] The present application is a sampling-based time-optimal trajectory planning algorithm for a mechanical arm to overcome the prior art. The present application can improve the overall joint utilization rate of an industrial robot and achieve time-optimal oriented trajectory planning optimization of a mechanical arm.

[0005] A sampling-based time-optimal trajectory planning algorithm for a mechanical arm comprises:

[0006] S1, modeling the industrial robot body;

[0007] S2, modeling the industrial robot dynamics, extracting the relationship between the joint space and the corresponding joint angular acceleration as the condition for drawing the trajectory planning map of the high-dimensional joint space;

[0008] S3, preprocessing the obtained dynamics model, discretizing the trajectory planning map space and obtaining the joint space sampling probability map with the target joint angular acceleration as the reference standard;

[0009] S4, using the improved RRT algorithm to complete the motion trajectory planning optimization of the mechanical arm.

[0010] Further, S41, determining the positions of the starting point and the target point of the mechanical arm.

[0011] S42, determine search duration or maximum search number, search step d_1, sampling step d_2;

[0012] S43, set initial random tree G;

[0013] S44, sampling in joint space

[0014] If the number of sampled points < maximum sampling point number x 0.4; execute

[0015] S45: probability sampling in joint space;

[0016] If the number of sampled points >= maximum sampling point number x 0.4; execute

[0017] S46: random sampling in joint space;

[0018] S47: P_parent and P_new are added to G, repeat S44 until the distance between G child node and target point is less than a certain threshold, end G expansion, complete the trajectory planning optimization of the robot arm.

[0019] The beneficial effects of the present application compared with the prior art are:

[0020] The present application adds a dynamic model to the kinematic planning of the robot arm in the joint space, and uses an improved algorithm to more efficiently and accurately optimize the trajectory planning of the robot arm. When planning a single joint, the influence of other joints on the target joint is considered, so that the target joint can maintain a high torque utilization state for as long as possible and reach the target as soon as possible, thereby improving the overall joint torque utilization efficiency and overall planning time of the industrial robot.

[0021] The technical solutions of the present application will be further described below in conjunction with the drawings and examples: DRAWINGS

[0022] Figure 1 It is the overall flowchart of the present application;

[0023] Figure 2 It is a space sampling probability diagram;

[0024] Figure 3 It is a space sampling probability top view;

[0025] Figure 4 It is a new node selection diagram of the improved RRT algorithm of the present application;

[0026] Figure 5 It is a path diagram generated by the ordinary RRT algorithm in the joint space;

[0027] Figure 6This is the path diagram generated in the joint space according to the present invention;

[0028] Figure 7 This is a path map generated on the sampling probability map for multiple experiments. Detailed Implementation

[0029] The embodiments of the technical solution of the present invention will now be described in detail with reference to the accompanying drawings. Unless otherwise stated, the technical or scientific terms used in this application should have the ordinary meaning understood by those skilled in the art.

[0030] Definitions of abbreviations involved in this implementation method:

[0031] RRT: Rapidly-exploring Random Tree (RRT) was proposed by Lavalle and is a random sampling algorithm that grows incrementally.

[0032] RBDL: Rigid Body Dynamics Library, containing code for forward and inverse dynamics of kinematic chains and branching models.

[0033] See Figures 1-2 As shown, a sampling-based time-optimal trajectory planning algorithm for a robotic arm in this embodiment includes:

[0034] S1. Model the industrial robot body;

[0035] Furthermore, this step considers that traditional six-axis robotic arm pose modeling involves establishing a coordinate system on each joint and using homogeneous coordinate transformation to achieve the transformation of multiple joint coordinates. This method requires six parameters to uniquely represent the robotic arm pose. In contrast, the DH method establishes a coordinate system for each link and uses a homogeneous transformation matrix to achieve the coordinate transformation of multiple connected links.

[0036] Therefore, in this step, S11, we use the CoppeliaSim simulator to model the industrial robot body.

[0037] S12. Model the pose of the six-axis robotic arm using the DH method. In this sub-step, the pose modeling uses a homogeneous transformation matrix to realize the coordinate transformation of multiple series links.

[0038] S2. Perform dynamic modeling on the industrial robot and extract the relationship between the joint space and the corresponding joint angular acceleration as the conditions for drawing the trajectory planning map in the high-dimensional joint space.

[0039] Since the influence of the industrial robot joint angle on the robot body moment of inertia is mainly reflected in the first three joints, in this step, the present example takes the influence of different angles of joint two and joint three on the maximum angular acceleration of joint one as an example (where joint three, joint two and joint one are defined from the arm to the gripper direction), and uses the RBDL library to model the dynamics of the robot arm.

[0040] S3, preprocessing the obtained dynamics model, discretizing the trajectory planning map space and obtaining a joint space sampling probability map with the target joint angular acceleration as the reference standard;

[0041] In this step, a two-dimensional map is established, with dimensions corresponding to joint two and joint three; the joint space map is discretized, with a discrete array of 628x628, and each discrete point corresponds to 1° in the joint space; the center of the discrete region is taken as the corresponding region data collection point, and the dynamics model obtained in step S1 is used to establish a high-dimensional image of the joint space and the corresponding angular acceleration and perform normalization processing, thereby obtaining the corresponding space sampling probability image, as shown in the specific images of Figure 2 and Figure 3 .

[0042] S4, using the improved RRT algorithm to complete the motion trajectory planning optimization of the robot arm.

[0043] Determine the positions of the starting point and the target point of the robot arm, determine the search duration or the maximum search number, and determine the search step and the sampling step; specifically:

[0044] S41, determine the positions of the starting point and the target point of the robot arm;

[0045] S42, determine the search duration or the maximum search number, the search step d_1, and the sampling step d_2;

[0046] S43, set the initial random tree G;

[0047] S44, sample in the joint space

[0048] If the number of sampled points < maximum number of sampled points x 0.4;

[0049] S45: probability sampling in the joint space;

[0050] If the number of sampled points ≥ maximum number of sampled points x 0.4;

[0051] S46: random sampling in the joint space;

[0052] S47: add P_parent and P_new to G, repeat S44 until the distance between the child nodes of G and the target point is less than a certain threshold, end the expansion of G, and complete the trajectory planning optimization design of the robot arm.

[0053] Embodiment

[0054] The technical solutions of the application will be further described below in combination with specific embodiments:

[0055] A sampling-based trajectory planning optimization design method for a mechanical arm comprises:

[0056] S1, modeling an industrial robot body;

[0057] Specifically, S11, modeling the industrial robot body by using a CoppeliaSim simulator;

[0058] S12, modeling the pose of a six-axis mechanical arm by using a D-H method;

[0059] S2, modeling the dynamics of the industrial robot, extracting the relationship between the joint space and the corresponding joint angular acceleration, and taking the relationship as a drawing condition of a trajectory planning map in a high-dimensional joint space;

[0060] S21, modeling the dynamics of the mechanical arm by using an RBDL library, extracting the relationship between the joint space of the first three joints and the corresponding joint angular acceleration, and saving the data;

[0061] S3, preprocessing the obtained dynamics model, discretizing the trajectory planning map space, and obtaining a joint space sampling probability map with the target joint angular acceleration as a reference standard;

[0062] This step specifically comprises: S31, discretizing the trajectory planning map in the high-dimensional joint space, and discretizing the corresponding joint space by 1°;

[0063] S32, taking the center of the discrete region as a corresponding region data collection point, and establishing a high-dimensional image of the joint space and the corresponding angular acceleration;

[0064] S33, normalizing the angular acceleration of the target joint in the entire joint space to obtain the corresponding space sampling probability;

[0065] S4, completing the motion trajectory planning optimization of the mechanical arm by using an improved RRT algorithm.

[0066] This step specifically comprises: S41, determining the positions of the starting point and the target point of the mechanical arm;

[0067] S42, determining the search duration or the maximum search number, the search step d_1, and the sampling step d_2; in this example, the maximum search number is 8000, the search step d_1 is 10, and the sampling step d_2 is 150;

[0068] S43, setting an initial random tree G;

[0069] S44, sampling in joint space

[0070] In the beginning of sampling stage, probability sampling is performed in joint space, guiding the random tree to expand to high probability area as much as possible; in the later stage of sampling, random sampling is performed in joint space, at this time the random tree has basically explored the high probability area, and random sampling can improve the space exploration speed and complement the exploration of low probability space. Through the algorithm, the target joint can stay in a state with higher torque utilization rate for as long as possible;

[0071] If the number of sampled points < maximum number of sampled points x 0.4;

[0072] S45: probability sampling in joint space;

[0073] S451, sampling in probability space to obtain the sampling probability threshold P* of this time;

[0074] S452, sampling in joint space to obtain joint space sampling points and the corresponding sampling probability P obtained from the joint space sampling probability map obtained in step S3;

[0075] S453, if P >= P*, take this point as the sampling point of this time; if P < P*, discard this sampling point and repeat step S451 to sample;

[0076] S454, traverse the distance between all nodes in G and the sampling point to find the shortest distance d, and record the point as P_parent;

[0077] S455, if the path passes through an obstacle or an unreachable area, discard the sampling point and repeat step S454;

[0078] S456, if d > d_2, discard the sampling point and repeat S451 to sample; if d_1 < d < d_2, select a point with a length of d_1 in this direction as the sampling point, record it as P_new; if d < d_1, take this point as the sampling point of this time, record it as P_new;

[0079] If the number of sampled points >= maximum number of sampled points x 0.4, execute step S46, otherwise return to step S45 to continue probability sampling;

[0080] S46: random sampling in joint space;

[0081] S461, traverse the distance between all nodes in G and the sampling point to find the shortest distance d, and record the point as P_parent;

[0082] S462, if the path passes through an obstacle or an unreachable area, discard the sampling point and repeat S461;

[0083] S463. If d_1 < d, select a point with a length of d_1 in this direction as the sampling point, denoted as P_new; if d < d_1, take this point as the sampling point for this time, denoted as P_new.

[0084] S47. Add P_parent and P_new to G, and repeat S44 until the distance between the child nodes of G and the target point is less than a certain threshold, end the expansion of G, and complete the optimal design of the manipulator trajectory planning.

[0085] The advantage of this embodiment is that it can add a dynamic model to the kinematic planning of the manipulator in the joint space. When planning a single joint, the influence of other joints on the target joint is considered, so that the target joint can maintain a high torque utilization rate state for as long as possible and reach the target as soon as possible, thereby improving the overall joint torque utilization efficiency and overall optimal planning of the industrial robot.

[0086] In this embodiment, a two-dimensional map is established in the Python 3.9 environment in combination with the dynamic model, and the corresponding spatial sampling probability image of step S2 is obtained. The specific image is as Figure 2 and Figure 3 shown. Combining the probability sampling model with the Figure 4 shown sampling method for trajectory planning ( Figure 4 where p_sample represents the sampling point, p_sample_fixed represents the corrected sampling point, and abandoned represents the abandoned sampling point), the path shown in Figure 6 is obtained. Compared with the path shown in Figure 5 (ordinary RRT planning), Figure 6 the path stays in the high torque utilization rate area for a significantly longer time. Figure 7 This is a multiple verification of the improved RRT algorithm of this embodiment. The experimental results show that the optimized design method of this embodiment can indeed improve the overall joint utilization rate of the industrial robot and achieve the optimal design of the manipulator trajectory planning with time-optimal guidance.

[0087] The present invention has been disclosed above with preferred embodiments. However, it is not intended to limit the present invention. Any person skilled in the art can make some changes or modifications to the above-disclosed structure and technical content to form equivalent embodiments of equivalent changes within the scope of the technical solution of the present invention.

Claims

1. A sampling-based time-optimal trajectory planning algorithm for a robotic arm, characterized in that: It includes: S1. Model the industrial robot body; S2. Conduct dynamic modeling on the industrial robot, extract the relationship between the joint space and the corresponding joint angular acceleration, and use it as the drawing condition for the trajectory planning map of the high-dimensional joint space; S3. Preprocess the obtained dynamic model, discretize the trajectory planning map space, and obtain the joint space sampling probability map with the target joint angular acceleration as the reference standard; S4. Use the improved RRT algorithm to complete the optimization of the manipulator's motion trajectory.

2. The sampling-based time-optimal trajectory planning algorithm for a robotic arm according to claim 1, characterized in that: Step S1 is specifically as follows: S11. Use the CoppeliaSim simulator to model the industrial robot body; S12. Use the D-H method to model the pose of the six-axis manipulator.

3. The sampling-based time-optimal trajectory planning algorithm for a robotic arm according to claim 1, characterized in that: Step S2 is to conduct dynamic modeling on the manipulator using the RBDL library.

4. The sampling-based time-optimal trajectory planning algorithm for a robotic arm according to claim 1, characterized in that: Step S3 is specifically as follows: S31. Discretize the trajectory planning map of the high-dimensional joint space. The discrete array is 628×628, and the discrete division corresponds to 1° in the joint space; S32. Use the center of the discrete area as the data collection point for the corresponding area, and establish a high-dimensional image of the joint space and the corresponding angular acceleration; S33. Normalize the angular acceleration of the target joint in the entire joint space to obtain the corresponding space sampling probability.

5. The sampling-based time-optimal trajectory planning algorithm for a robotic arm according to claim 1, characterized in that: Step S4 includes: S41. Determine the positions of the manipulator's working starting point and target point; S42. Determine the search duration or the maximum number of searches, the search step size d_1, and the sampling step size d_2; S43. Set the initial random tree G; S44. Sample in the joint space If the number of sampled points < 0.4 × the maximum number of sampled points; S45: Conduct probability sampling in the joint space; S451. Sample in the probability space to obtain the sampling probability threshold P* for this sampling; S452. Sample in the joint space to obtain the joint space sampling point and obtain the corresponding sampling probability P from the joint space sampling probability map obtained in step S3; S453. If P >= P*, take this point as the sampling point for this time; if P < P*, abandon this sampling point and repeat the sampling; S454. Traverse the distances between all nodes in G and the sampling point, find the shortest distance d, and mark this point as P_parent; S455. If this path passes through an obstacle or an inaccessible area, abandon this sampling point and repeat step S454; S456. If d > d_2, abandon this sampling point and repeat sampling in S451; if d_1 < d < d_2, select a point with a length of d_1 in this direction as the sampling point and mark it as P_new; if d < d_1, take this point as the sampling point for this time and mark it as P_new; If the number of sampled points ≥ 0.4 × the maximum number of sampled points, execute step S46; otherwise, return to step S45 to continue probability sampling; S46: Conduct random sampling in the joint space; S461. Traverse the distances between all nodes in G and the sampling point, find the shortest distance d, and mark this point as P_parent; S462. If this path passes through an obstacle or an inaccessible area, abandon this sampling point and repeat S461; S463. If d_1 < d, select a point with a length of d_1 in this direction as the sampling point, denoted as P_new; if d < d_1, take this point as the sampling point for this time, denoted as P_new; S47: Add P_parent and P_new to G, and repeat S44 until the distance between the child node of G and the target point is less than a certain threshold, end the expansion of G, and complete the optimization of the manipulator trajectory planning.

6. The sampling-based time-optimal trajectory planning algorithm for a robotic arm according to claim 3, characterized in that: Under the environment of Python 3.9, a two-dimensional map is established in combination with the dynamic model, and the corresponding spatial sampling probability image of step S2 is obtained.

7. The sampling-based time-optimal trajectory planning algorithm for a robotic arm according to claim 2, characterized in that: In the pose modeling of step 12, the homogeneous transformation matrix is used to realize the coordinate transformation of multiple series-connected linkages.

8. The sampling-based time-optimal trajectory planning algorithm for a robotic arm according to claim 5, characterized in that: In S42, the number of search times is 8000, the search step size d_1 = 10, and the sampling step size d_2 = 150.

Images