A robot dynamics planning method based on reachable set guided heuristic search

Abstract

The application discloses a robot dynamics planning method based on reachable set guiding heuristic search, and belongs to the field of industrial robot systems.The application solves the problems of low trajectory planning efficiency, easy falling into local optimal value and long trajectory execution time of the existing multi-joint serial industrial robot motion planning method.The application starts from the starting point of motion, discretizes the control input into a limited number, determines the next preferred exploration state according to the reachable set cost sorting of each state, finds a state space trajectory with the lowest cost through state space traversal search, connects the trajectory to the target state of the robot, and generates a reference path point sequence which is more in line with the dynamics constraints of the industrial robot than the traditional kinematics planning, and the sequence is a state space sequence, which is easier for further trajectory generation than the joint space position sequence.The method can be applied to robot trajectory planning.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of industrial robot system, and particularly relates to a robot dynamics planning method based on reachable set guided heuristic search. BACKGROUND

[0002] With the beginning of the negative growth of the total population and the working-age population, the acceleration of the aging process of the population, and the increase of labor production cost, industrial robots begin to replace manual labor in more and more production scenarios and become an indispensable important role in modern industrialized production. Industrial robots can not only replace manual labor to complete some dangerous and complex operations, but also can work efficiently for 24 hours, which is the main factor for the continuous popularization and application of industrial robots. However, the current industrial robots are not highly intelligent, and the required motion process is mainly dependent on manual teaching, and the efficiency of industrial robot motion needs to be improved.

[0003] Currently, there are mainly two kinds of motion planning methods for multi-joint serial industrial robots: one kind is a numerical optimization method that simultaneously considers the kinematic constraints, dynamic constraints and external geometric obstacle constraints of the robot, parameterizes the trajectory of the robot as a spline curve function form, takes the internal and external constraints as inequality constraints that need to be satisfied at each point on the curve, and models the motion time, energy consumption, and minimum jump of the industrial robot as a function of the curve parameters. The optimal solution of the trajectory is solved by using a numerical optimization method. Due to the limitation of multiple complex constraints, the solving process is time-consuming, so the efficiency of planning the trajectory is low, and it is easy to fall into a local optimal trap; the other kind of planning method is to decouple the geometric kinematic constraints and dynamic constraints of the robot, that is, to first consider the geometric factors such as the limit of the robot joint and the collision with the external obstacle, to plan a sequence of robot joint space path points according to the heuristic guidance of the path planning target, and then to consider the dynamic model of the robot according to the sequence of path points, and to generate an executable trajectory of the robot using interpolation and fitting methods. However, in the previous stage, the sequence of path points planned by only relying on the kinematic model of the robot will greatly affect the efficiency of planning the trajectory and the execution time of the trajectory in the subsequent process, so the dynamic model of the industrial robot also needs to be considered in the previous stage.

[0004] In summary, the existing multi-joint serial industrial robot motion planning method has low trajectory planning efficiency, is easy to fall into a local optimal value, and has a long trajectory execution time, so it is necessary to propose a new motion planning method. SUMMARY

[0005] The application aims to solve the problems of low trajectory planning efficiency, easy to fall into local optimal value and long trajectory execution time of the existing multi-joint serial industrial robot motion planning method, and proposes a robot dynamics planning method based on reachable set guided heuristic search.

[0006] The technical scheme adopted by the application to solve the above technical problems is:

[0007] A robot dynamics planning method based on reachable set guided heuristic search, the method specifically comprises the following steps:

[0008] Step 1, establishing the dynamics model of the multi-joint serial industrial robot considering the joint angle limit Q lim , the speed limit QD lim , the torque limit τ lim , the mass and rotational inertia of each link of the robot;

[0009] Step 2, inputting the start state S start and the target state S goal of the motion planning of the multi-joint serial industrial robot;

[0010] Step 3, taking the start point S start =(Q1,D1) as the state space point in the first state layer, wherein QD1 is the speed of the start state, and Q1 is the position of the start state, and initializing the state layer number i=1;

[0011] Step 4, judging whether the number of state space points in the i-th state layer is less than or equal to N max ;

[0012] If the number of state space points in the i-th state layer is less than or equal to N max , all the state space points in the i-th state layer constitute a set S low , and the set S left is an empty set;

[0013] Otherwise, if the number of state space points in the i-th state layer is greater than N max , for any state space point in the i-th state layer, the cost of each forward reachable state in the forward reachable state set of the state space point to the target state is calculated, and the sum of the costs of the forward reachable states to the target state is calculated as the cost of the state space point; after traversing each state space point in the i-th state layer, the cost of each state space point in the i-th state layer is obtained; after sorting the state space points in the i-th state layer in the order of cost from small to large, the first N max state space points constitute a set S low , and the remaining state space points constitute a set Sleft ;

[0014] where N max is the maximum searchable number of each state layer;

[0015] Step 5, for the state space point (Q low , D i ) with the minimum cost in the set S i , calculate the maximum acceleration amax i and the minimum acceleration amin i allowed on the state space point (Q i , D i ) according to the established dynamic model;

[0016] Step 6, select n discrete acceleration values as control variables in the acceleration interval [amin i , amax i ], n represents the degree of control variable discretization (the specific value can be set according to actual requirements);

[0017] Step 7, for any selected acceleration value a j , j = 1, 2, …, n, after the acceleration value a j is used as a control variable on the state point (Q i , D i ), the speed change ΔQD i,j and the position change ΔQ i,j are obtained, and then the new state space point S i,j (Q i,j , D i ) is obtained according to ΔQD i , ΔQ new and the state point (Q i,j , D i,j );

[0018] Similarly, according to each selected acceleration value, the corresponding new state space point is obtained;

[0019] Step 8, the state space points in the set S low are processed in order from small to large cost, that is, the processes of steps 5 to 7 are performed for each state space point in the set S low , and all new state space points obtained based on the set S low form the i+1th state layer together with the state space points in the set S left ;

[0020] Step 9, let i = i+1, and return to step 4;

[0021] Step 10, repeat the process of step 4 to step 9 until the latest state layer exists a state space point meeting the end condition, backtracking the parent node of the state space point meeting the end condition (i.e. the Euclidean distance from the state space point to the target state is less than the threshold value) in the previous state layer, and constantly backtracking until the starting state of the motion is recursively reached, the state space node sequence formed by the state space point meeting the end condition, all state space points backtracked in the backtracking process and the starting point as the reference trajectory of the motion planning.

[0022] Further, the dynamics model of the multi-joint serial industrial robot is established by Newton-Euler method or Lagrange method.

[0023] Further, the maximum acceleration amax i is the acceleration allowed on the state point (Q i , D i ) under the action of the maximum positive driving force; the minimum acceleration amin i is the acceleration allowed on the state point (Q i , D i ) under the action of the maximum reverse driving force.

[0024] Further, the specific process of step 5 is as follows:

[0025] Starting from the endpoint amin i of the acceleration interval [amin i , amax i ], a value of acceleration is selected every time the value increases by (amax i - amin i ) / (n-1) until the endpoint amax i of the acceleration interval [amin i , amax i ] is selected, and the selected n values of acceleration are sequentially taken as the control quantity.

[0026] Further, the speed change ΔQD i,j is obtained by integrating the control quantity.

[0027] Further, the position change ΔQ i,j is obtained by trapezoidal integration of the speed.

[0028] Further, the time T of the control quantity acting on the state point (Q i , D i ) is a random number within [0, T max ], and T max is the integral time step.

[0029] Further, the cost of each forward reachable state in the set of forward reachable states to the target state is:

[0030]

[0031] wherein Cost i,j is the cost of the jth state space point (Q' i,j D' i,j ) in the set of forward reachable states, QD i,j is the velocity of the jth state space point in the set of forward reachable states, Q' i,j is the position of the jth state space point in the set of forward reachable states, QDgoal is the velocity of the target state, and Qgoal is the position of the target state.

[0032] Further, the ith state layer contains a number of state space points:

[0033]

[0034] wherein f(i) is the number of state space points contained in the ith state layer, m = i', i' satisfies: n i′-1 <N max and n i′ >N max .

[0035] Still further, the end condition is that the Euclidean distance of the state space point to the target state is less than a threshold value.

[0036] The present application has the following beneficial effects:

[0037] The present application proposes a robot dynamics planning method based on reachable set guided heuristic search, which discretizes the control input into a finite number from the starting point of the motion, sorts the reachable set cost of each state, and determines the state to be explored next, thereby significantly improving the search efficiency. Through state space traversal search, a state space trajectory with the lowest cost is found to connect to the target state of the robot, thereby generating a reference path point sequence that is more consistent with the industrial robot dynamics constraints than the traditional kinematics planning, and avoiding the problem that the traditional method is prone to local optimal value. Moreover, the sequence is a state space sequence, which is easier to generate a further trajectory than the joint space position sequence, and reduces the robot trajectory execution time through trajectory optimization. Through the improvement of search efficiency, the efficiency of trajectory planning is improved. BRIEF DESCRIPTION OF DRAWINGS

[0038] Figure 1 is a flowchart of a robot dynamics planning method based on reachable set guided heuristic search of the present application;

[0039] Fig. 2(a) is a comparison chart of the number of states of the reachable set heuristic search and the traditional breadth-first search when the control discrete quantity n = 2 and the maximum searchable state Nmax= 1000;

[0040] Fig. 2(b) is a comparison chart of the number of states of the reachable set heuristic search and the traditional breadth-first search when the control discrete quantity n = 2 and the maximum searchable state Nmax= 10000000;

[0041] Fig. 2(c) is a comparison chart of the number of states of the reachable set heuristic search and the traditional breadth-first search when the control discrete quantity n = 3 and the maximum searchable state Nmax= 100;

[0042] Fig. 2(d) is a comparison chart of the number of states of the reachable set heuristic search and the traditional breadth-first search when the control discrete quantity n = 4 and the maximum searchable state Nmax= 100;

[0043] Fig. 3(a) is a phase plane vector diagram of a fully driven single-joint linkage mechanism under the action of +-5Nm torque;

[0044] In the figure, the thickened curve part is the optimal trajectory obtained according to theoretical analysis. The optimal trajectory is to first apply a +5Nm driving force in the positive direction to accelerate movement, and then decelerate with a -5Nm driving force;

[0045] Fig. 3(b) is a phase plane vector diagram of an underactuated single-joint linkage mechanism under the action of +-1Nm torque;

[0046] In the figure, the thickened short curve is an infeasible trajectory generated without considering the dynamic characteristics of trajectory planning, and the thickened long curve is the optimal trajectory obtained according to theoretical analysis. The optimal trajectory is to first apply a -1Nm driving force in the negative direction to move, which will first accelerate and then decelerate due to insufficient driving force. At the position where the negative direction movement stops, a +1Nm driving force is applied in the positive direction to move. With the change of the load torque, it will first accelerate, then decelerate, and then accelerate again, and then decelerate with a -1Nm driving force until the target state is reached;

[0047] Fig. 4(a) is a schematic diagram of the single-layer state set after the first search in the state space trajectory search of the fully driven single-joint linkage mechanism and the state space trajectory obtained by tracing the state set;

[0048] In the figure, the horizontal coordinate is the joint position, and the vertical coordinate is the joint velocity;

[0049] Fig. 4(b) is a schematic diagram of the single-layer state set after the second search in the state space trajectory search of the fully driven single-joint linkage mechanism and the state space trajectory obtained by tracing the state set;

[0050] Fig. 4(c) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the third search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0051] Fig. 4(d) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the eighth search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0052] Fig. 4(e) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the ninth search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0053] Fig. 4(f) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the tenth search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0054] Fig. 4(g) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the thirteenth search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0055] Fig. 4(h) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the fourteenth search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0056] Fig. 4(i) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the fifteenth search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0057] Fig. 4(j) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the fifteenth search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0058] Fig. 4(k) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the fifteenth search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0059] Fig. 5(a) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the first search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0060] Fig. 5(b) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the second search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0061] Fig. 5(c) is a schematic diagram of the state space trajectory of the single-joint linkage mechanism after the third search of the state space trajectory search of the single-joint linkage mechanism and the state space trajectory obtained by tracing back the state space trajectory;

[0062] Fig. 5(d) is a schematic diagram of the state space trajectory of the single layer state set after the 8th search and its tracing back in the state space trajectory search of the underactuated single jointed link mechanism;

[0063] Fig. 5(e) is a schematic diagram of the state space trajectory of the single layer state set after the 12th search and its tracing back in the state space trajectory search of the underactuated single jointed link mechanism;

[0064] Fig. 5(f) is a schematic diagram of the state space trajectory of the single layer state set after the 14th search and its tracing back in the state space trajectory search of the underactuated single jointed link mechanism;

[0065] Fig. 5(g) is a schematic diagram of the state space trajectory of the single layer state set after the 20th search and its tracing back in the state space trajectory search of the underactuated single jointed link mechanism;

[0066] Fig. 5(h) is a schematic diagram of the state space trajectory of the single layer state set after the 22nd search and its tracing back in the state space trajectory search of the underactuated single jointed link mechanism;

[0067] Fig. 5(i) is a schematic diagram of the state space trajectory of the single layer state set after the 25th search and its tracing back in the state space trajectory search of the underactuated single jointed link mechanism;

[0068] Fig. 5(j) is a schematic diagram of the state space trajectory of all the states whose distance to the target is less than the threshold value and its tracing back in the state space trajectory search of the underactuated single jointed link mechanism after the 25th search;

[0069] Fig. 5(k) is a schematic diagram of all the state points explored in the 25th search in the state space trajectory search of the underactuated single jointed link mechanism;

[0070] Fig. 6(a) is a path for trajectory tracking control without considering dynamics;

[0071] Fig. 6(b) is a tracking result diagram of the trajectory planning when the joint driving torque of the robot is not enough to provide a large enough acceleration and speed to each joint after overcoming the link gravity;

[0072] Fig. 6(c) is a schematic diagram of the trajectory planning completely invalid state when the joint driving torque of the robot is not enough to provide the required gravity to overcome the robot to execute the trajectory, resulting in the actual state lagging behind the planned trajectory;

[0073] Fig. 7(a) is a path for trajectory tracking control considering dynamics;

[0074] Fig. 7(b) is a tracking process when the path actively changes the joint state combination considering dynamics, reduces the gravity that the robot configuration needs to overcome, and makes the joint driving torque sufficient to complete the trajectory tracking control process Figure 1 ;

[0075] Fig. 7(c) is a tracking process diagram two when the path actively changes the joint state combination considering dynamics, reduces the gravity that the robot configuration needs to overcome, and makes the joint driving torque sufficient to complete the trajectory tracking control process

[0076] Fig. 7(d) is a tracking process diagram three when the path actively changes the joint state combination considering dynamics, reduces the gravity that the robot configuration needs to overcome, and makes the joint driving torque sufficient to complete the trajectory tracking control process

[0077] Fig. 7(e) is a tracking process diagram four when the path actively changes the joint state combination considering dynamics, reduces the gravity that the robot configuration needs to overcome, and makes the joint driving torque sufficient to complete the trajectory tracking control process

[0078] Fig. 7(f) is a tracking process diagram five when the path actively changes the joint state combination considering dynamics, reduces the gravity that the robot configuration needs to overcome, and makes the joint driving torque sufficient to complete the trajectory tracking control process DETAILED DESCRIPTION

[0079] The application will be described in further detail below with specific embodiments in conjunction with the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the application, rather than all the embodiments. Based on the embodiments in the application, other embodiments obtained by those skilled in the art without creative labor fall within the protection scope of the application.

[0080] Specific embodiment one, combined Figure 1 This embodiment describes a robot dynamics planning method based on reachable set guided heuristic search. The method specifically includes the following steps:

[0081] Step 1: Based on the SolidWorks model of the industrial robot, the connection relationship between the robot joints and the connecting rods is established using the URDF plug-in, the relative pose transformation relationship of each joint coordinate is set, and the robot dynamics model is exported as a.urdf text format. Using compilation tools such as cmake, qmake, etc., in vscode, qtcreator, visualstudio editor, using c++ language to call dynamics libraries such as rbdl, kdl, pinocchio, according to the parameter data of the URDF model, using Newton-Euler method or Lagrange method to establish joint angle restriction Q lim , speed limit QD lim , torque limit τlim , the robot each link mass and the inertia of rotation dynamics model of multi-joint serial industrial robot;

[0082] Step 2, input the starting state S start and target state S goal of multi-joint serial industrial robot motion planning;

[0083] Step 3, take the starting point S start =(Q1, D1) as the first layer search state, wherein QD1 is the velocity of the starting state, and Q1 is the position of the starting state, and initialize the state layer number i=1;

[0084] Step 4, judge whether the number of state space points in the ith state layer is less than or equal to N max ;

[0085] If less than or equal to N max , all state space points in the ith state layer constitute a set S low , and let the set S left be empty set;

[0086] Otherwise, for any state space point in the ith state layer, calculate the cost of each forward reachable state in the forward reachable state set of the state space point to the target state, and then sum the cost of each forward reachable state to the target state, and take the sum as the cost of the state space point; After traversing each state space point in the ith state layer (for the state space point whose cost has been calculated, it is not necessary to repeat the calculation), the cost of each state space point in the ith state layer is obtained respectively; After sorting the state space points in the ith state layer according to the cost from small to large, the first N max state space points constitute a set S low , and the remaining state space points constitute a set S left ;

[0087] Wherein, N max is the maximum searchable number of each state layer; for restricting the number of states in a state layer that can be used as the starting point for further search;

[0088] The method for calculating the forward reachable state set is the same as the subsequent actual state search, but the time step should be longer than that set in the actual search in step 7, so as to quickly move to a farther point in the state space and represent the search potential of the current state space point with the cost of a farther sub-state. By the cost of the forward reachable state set and the cost of the current state space point, the better state space point can be selected to expand first, and the overall search efficiency is higher;

[0089] The cost of each forward reachable state in the set of forward reachable states to the target state is:

[0090]

[0091] wherein Cost i,j is the cost of the jth state space point (Q' i,j ,D' i,j ) in the set of forward reachable states, QD' i,j is the velocity of the jth state space point in the set of forward reachable states, Q' i,j is the position of the jth state space point in the set of forward reachable states, QDgoal is the velocity of the target state, and Qgoal is the position of the target state.

[0092] Step 5, for the state space point (Q low ,D i ) with the minimum cost in the set S i , the maximum acceleration amax i and the minimum acceleration amin i allowed on the state space point (Q i ,D i ) are calculated according to the established dynamic model;

[0093] The maximum acceleration amax i is the acceleration allowed on the state space point (Q i ,D i ) under the action of the maximum forward driving force; and the minimum acceleration amin i is the acceleration allowed on the state space point (Q i ,D i ) under the action of the maximum reverse driving force.

[0094] Step 6, n discrete acceleration values are selected in the acceleration interval [amin i ,amax i ] as control quantities;

[0095] That is, a control input quantity discretization degree parameter n is set, and n discrete control quantities a i are evenly selected in the acceleration interval [amin i ,amax i ] according to the interval (amax i -amin n ) / (n-1); the time T during which the control quantity acts on the state space point (Q i ,D i ) is a random number in [0,T max ], and T max is the integral time step, and the integral time step Tmax The smaller, the higher the positive dynamic calculation frequency, the higher the fitting degree of the dynamic model constraint, but the corresponding search efficiency will also be reduced;

[0096] Step 7, for any selected acceleration value a j , j = 1, 2, …, n, the acceleration value a j acts on the state point (Q i , D i ) as a control variable, then the speed change ΔQD i,j and the position change ΔQ i,j are obtained, and then the new state space point S new (Q i,j , D i,j ) is obtained according to ΔQD i,j , ΔQ i,j and the state point (Q i , D i );

[0097] The speed change ΔQD i,j is obtained by integrating the control variable, and the position change ΔQ i,j is obtained by trapezoidal integration of the speed.

[0098] Similarly, according to each selected acceleration value, the corresponding new state space point is obtained;

[0099] Step 8, according to the order of cost from small to large, the state space points in the set S low are processed in turn, that is, the process of steps 5 to 7 is performed for each state space point in the set S low , and all new state space points obtained based on the set S low form the i+1th state layer together with the state space points in the set S left .

[0100] The set S left is directly retained to the next state layer to participate in cost calculation and sorting again, in order to avoid the search from falling into a local optimal region, and the global optimality of the search algorithm is guaranteed through the state retention;

[0101] Starting from the i th state layer, based on the action of n discrete control variables, the i+1th state layer will be generated, according to the rules of the traditional breadth-first search algorithm, the number of states contained in the i+1th state layer will be n iThe number of states will exponentially increase with the increase of the number n of discrete control amounts and the number i of iteration search layers, which greatly reduces the efficiency of subsequent search; the state reachable set is used to guide the search instead of the traditional breadth-first search, so that the number of state expansion in each search is reduced, and the search efficiency is improved.

[0102] The forward reachable states of the current state point are also obtained based on the discretization processing, and the similar process of state expansion is performed, the allowable acceleration range on the current state point is calculated according to the positive dynamics, and n discrete control amounts are generated; the reachable state set is calculated according to the n different control amounts and the action time t of each state, the current potential cost of each point in the reachable state set is calculated, that is, the cost to be paid for moving to the terminal point, and the sum of the calculated costs is obtained as the cost of the current state point, each state point in the current state layer is calculated in turn, and then the state points in the current state layer are sorted according to the cost value;

[0103] The non-expandable state set is set due to the high cost of the reachable set at a certain time, and at a later time, the expandable state at a certain time, after a period of search, if the cost of the subsequent state of the reachable set does not decrease but increases, when it exceeds the cost of some non-expandable states, the remaining non-expandable states are reconverted into expandable states through the adjustment of the cost sorting, and some state space regions with high early-stage cost are also searched, so as to ensure the probability completeness and optimality of the dynamic programming problem in the state space;

[0104] Step 9, let i = i + 1, and return to step 4;

[0105] Step 10, repeat the process of steps 4 to 9 until the state space point meeting the end condition exists in the latest state layer (we start the search from the state space point with small cost, and once the state space point meeting the end condition is searched in the latest state layer, the search is ended, and the subsequent state space points are not searched), backtrack the parent node of the state space point meeting the end condition (i.e., the Euclidean distance from the state space point to the target state is less than the threshold) in the previous state layer, and continuously backtrack until the starting state of the motion is reached, and the state space node sequence formed by the state space point meeting the end condition, all the state space points backtracked in the backtracking process and the starting point are taken as the reference trajectory of the motion planning.

[0106] The state space trajectory search results of the fully driven single-joint linkage mechanism after each iteration are shown in Figures 4(a) to 4(k) The state space trajectory search results of the underactuated single-joint linkage mechanism after each iteration are shown in Figures 5(a) to 5(k)It can be seen that the search process is sparser and the feasible solution can be found faster in the under-actuated case.

[0107] In the present application, the number of state space points contained in the ith state layer is:

[0108]

[0109] Wherein, f(i) is the number of state space points contained in the ith state layer, m=i', i' satisfies: n i′-1 <N max And n i′ ≥N max .

[0110] Different control variable discretization coefficients will also greatly affect the speed of state increase. The number of states generated by the traditional breadth-limited search and the heuristic search method guided by the reachable set under different control variable discretization coefficients and iteration numbers are shown in Figures 2(a), 2(b), 2(c) and 2(d). Figures 3(a) and 3(b) are the phase plane vector diagrams of a fully-actuated single-joint linkage mechanism and an under-actuated single-joint linkage mechanism, respectively.

[0111] In the present application, since the time at which all state points in each state layer start to move relative to the starting state is consistent, when a state point that can be connected to the target state first appears in the latest state layer, the state space sequence point trajectory experienced by the state point is the time-optimal trajectory. In the generation process of the state point sequence, not only the kinematic constraints of the robot (joint limits, upper speed limit), but also the dynamic characteristics of the robot (nonlinear acceleration constraints provided by the maximum torque of the joints under different configurations of the robot) are considered, which are used as the reference trajectory of the robot for trajectory tracking control, so that the overall motion performance of the industrial robot can be better played. As shown in Figure 4(a), it is a trajectory tracking schematic diagram without considering dynamics, and as shown in Figure 4(b), it is a trajectory tracking schematic diagram considering dynamics. Figures 6(a) to 6(c) Figures 7(a) to 7(f) As can be seen, the method of the present application can obtain better trajectory tracking control effect.

[0112] The above examples of the present application are only used to illustrate the calculation model and calculation process of the present application, and are not intended to limit the embodiments of the present application. Those skilled in the art can make other different forms of changes or modifications on the basis of the above description, and it is impossible to exhaust all embodiments here. Any obvious changes or modifications derived from the technical solutions of the present application still fall within the protection scope of the present application.​

Claims

1. A method of robot dynamics planning based on reachable set guided heuristic search, characterized in that, The method specifically comprises the following steps: Step 1, Establishing joint angle limit Q lim , velocity limit QD lim , torque limit τ lim , mass and moment of inertia of each link of the robot Dynamics model of a multi-joint serial industrial robot Step 2, input the start state S of the multi-joint series industrial robot motion planning start and the target state S goal ; Step 3, with the starting point S start = (Q1, QD1) as a state space point in the 1st state layer, where QD1 is the velocity of the starting state and Q1 is the position of the starting state, and initialize the state layer number i = 1; Step 4, judge whether the number of state space points in the i-th state layer is less than or equal to N max ; If the number of state space points in the i-th state layer is less than or equal to N max , all the state space points in the i-th state layer constitute a set S low , and the set S left is an empty set. Otherwise, the number of state space points in the i-th state layer is greater than N max Then, for any state space point in the i-th state layer, the cost of each forward reachable state in the forward reachable state set of the state space point to the target state is calculated, and the costs are summed up to obtain the cost of the state space point; after traversing each state space point in the i-th state layer, the cost of each state space point in the i-th state layer is obtained respectively; after sorting the state space points in the i-th state layer according to the cost from small to large, the first N max state space points form a set S low , and the remaining state space points form a set S left . where N max is the maximum searchable number of each state layer; Step 5, for the set S low the cost-minimal state space point (Q i , QD i ), the maximum acceleration amax i and the minimum acceleration amin i allowed on the state space point (Q i , QD i ) are calculated according to the established dynamic model; Step 6, n discrete acceleration values are selected as control variables in the acceleration interval [amin i , amax i ], n representing the discrete degree of the control variables; Step 7, for any selected acceleration value a j , j = 1, 2,..., n, the acceleration value a j is applied to the state point (Q i , QD i ) as a control variable, then the velocity change ΔQD i,j and the position change ΔQ i,j are obtained, and then the new state space point S i,j (Q i,j , QD i ) is obtained according to ΔQD i , ΔQ new and the state point (Q i，j , QD i,j ). Similarly, according to each selected acceleration value, a corresponding new state space point is obtained; Step 8, process the state space points in set S low in turn according to the order from small to large of the cost, that is, perform the processes of steps 5 to 7 for each state space point in set S low , and form the i+1th state layer with all the new state space points obtained based on set S low and the state space points in set S left . Step 9: let i = i + 1, and return to step 4; Step 10: repeat the process of steps 4 to 9 until there is a state space point in the latest state layer that meets the end condition, backtrack the parent node of the state space point meeting the end condition in the previous state layer, and continuously backtrack until the starting state of the motion is reached, and the state space node sequence formed by the state space point meeting the end condition, all state space points backtracked in the backtracking process and the starting point are taken as a reference trajectory of motion planning.

2. The method of claim 1, wherein, The dynamics model of the multi-joint serial industrial robot is established by using Newton-Euler method or Lagrange method.

3. The method of claim 2, wherein, said maximum acceleration amax i is the acceleration allowed on the state point (Q i , QD i ) under the action of the maximum positive driving force; said minimum acceleration amin i is the acceleration allowed on the state point (Q i , QD i ) under the action of the maximum negative driving force.

4. The method of claim 3, wherein, The specific process of step 5 is as follows: from the end point amin i of the acceleration interval [amin i , amax i ], a value is selected for each increase of (amax i - amin i ) / (n-1) until the end point amax i of the acceleration interval [amin i , amax i ] is reached, the n values selected being used in turn as the control variable.

5. The method of claim 4, wherein, The speed variation amount AQD i,j Obtained by integrating the control amount.

6. The method of claim 5, wherein, The position change amount ΔQ i,j Obtained by trapezoidal integration of the velocity.

7. The method of claim 6, wherein, The control quantity acts on the state point (Q i , QD i ) at time T which is a random number in [0, T max ] and T max is the integration time step.

8. The method of claim 7, wherein, The cost of each forward reachable state in the forward reachable state set to the target state is: where Cost i,j is the cost of the jth state space point (Q' i，j , QD' i，j ) in the set of forward reachable states, QD' i，j is the velocity of the jth state space point in the set of forward reachable states, Q' i，j is the position of the jth state space point in the set of forward reachable states, QDgoalis the velocity of the goal state, and Qgoalis the position of the goal state.

9. The method of claim 8, wherein, The number of state space points contained in the i th state layer is: where f(i) is the number of state space points contained in the i-th state layer, m = i', i' satisfies: n i-1 <N max and n i′ ≥N max .

10. The method of claim 9, wherein, The end condition is that the Euclidean distance of the state space point to the target state is less than a threshold.

Images