Model predictive control and artificial potential field fused mechanical arm path planning method

Abstract

The invention discloses a model predictive control and artificial potential field fused mechanical arm path planning method, and belongs to the field of mechanical arm control. Comprising the following steps: 1) establishing a mechanical arm state model; 2) distance solving; 3) establishing a cost function, namely a cost function of a path planning algorithm fusing model predictive control and an artificial potential field; and 4) a constraint condition is established, and the cost function is combined with the constraint condition, so that a final optimization problem is formed, and the mechanical arm is guided to safely and efficiently move in a complex environment. According to the method, the repulsive force field function and the gravitational field function of the artificial potential field are introduced into the cost function, accurate control over the position of the mechanical arm tail end actuator is achieved, and therefore the accuracy of path planning of the mechanical arm tail end is ensured; meanwhile, the influence of the obstacle and the target point on the end effector is fully considered in the path planning by reasonably adjusting system parameters, so that the planning efficiency and stability are ensured.

Description

Technical Field

[0001] The present invention relates to the field of robot arm control, and in particular to a robot arm path planning method integrating model predictive control with artificial potential field. Background Art

[0002] In the field of robotic arm research, the path planning of the end effector has always occupied a core position. The most challenging problem is how to accurately plan a safe and efficient path so that the end effector can move freely in a complex working environment and effectively avoid obstacles, thereby achieving barrier-free movement from the starting point to the end point. In recent years, a series of path planning algorithms have emerged, such as the Artificial Potential Field Method (APF) and the Rapidly-exploring Random Trees (RRT) and their derivatives. These methods can avoid collisions to a certain extent and build an obstacle-free path for the end effector of the robotic arm. However, the inherent defects of these traditional methods cannot be ignored - the completeness of the solution is limited, the quality of the planned path is uneven, and it is easy to cross the target point and then return when approaching the target point. Model Predictive Control (MPC) is a control strategy that uses a prediction model to predict the system state in the future and optimize and control based on the prediction results. The forward-looking planning characteristics of MPC enable it to significantly reduce the risk of falling into a local optimal solution when approaching the target point when dealing with the motion control of the robot arm. Therefore, the obstacle avoidance algorithm based on MPC has received widespread attention in recent years.

[0003] Although MPC has demonstrated outstanding capabilities in the field of robotic arm motion control, it still faces two major difficulties in practical applications: "how to properly deal with strict physical constraints" and "how to appropriately select the prediction horizon". In order to overcome these challenges of MPC, researchers at home and abroad are working to combine common obstacle avoidance methods with MPC to make up for the shortcomings of MPC. Zhu Tianqi, Mao Jianliang and others proposed a cascaded nonlinear MPC robot manipulator dynamic obstacle avoidance algorithm based on artificial potential field. By introducing the repulsive field function of the artificial potential field in the cost function, the excitation optimizer helps the end effector avoid obstacles, providing an innovative idea for robot arm obstacle avoidance. However, the cascaded nonlinear MPC robot arm dynamic obstacle avoidance algorithm based on artificial potential field still has some limitations. When applying the artificial potential field method, the algorithm mainly considers the influence of repulsion, but fails to fully consider the role of gravity. This imbalance leads to instability during the movement of the robot, especially when approaching the target point, which brings challenges to system control and optimization. Summary of the invention

[0004] In order to overcome the shortcomings of the prior art, the present invention provides a robot arm path planning method that integrates model predictive control and artificial potential field, aiming to solve problems such as instability and backtracking that may occur in the movement of the robot arm. By introducing repulsive field and gravitational field, the position of the end effector of the robot arm is precisely controlled to achieve precise control of the path of the end of the robot arm. In addition, by reasonably adjusting the system parameters, it is ensured that path planning not only takes into account the influence of obstacles, but also takes into account the attractiveness of the target point, thereby ensuring the efficiency and stability of the planning. The present invention is expected to improve the efficiency and reliability of the robot arm during movement.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is as follows:

[0006] A robot arm path planning method integrating model predictive control and artificial potential field, characterized in that the method comprises the following steps:

[0007] 1) Establishing the state model of the robot arm: In order to facilitate the prediction and control of the movement of the robot arm, the system state containing the position and velocity information of the joint is defined as x, which is a vector containing the position and velocity information of the joint, specifically expressed as where q represents the position of the joint, and represents the speed of the corresponding joint; for this model, a speed response controller will be designed, as shown in formula (1-1):

[0008]

[0009] Among them, x k and x k+1 are the state vectors at the kth and k+1th sampling moments, u k is the input vector at the kth sampling moment, f k (·) is the discrete state equation function, t s is the sampling time interval;

[0010] 2) Distance solution: When planning the motion trajectory of the robot arm, obstacles in the actual operating environment must be considered; in order to effectively integrate obstacle information into the control strategy, the geometric structure of the obstacle is represented by an open set, as shown in formula (1-2):

[0011]

[0012] in, Represents the point set of the j-th obstacle in three-dimensional space coordinates;

[0013] The geometric shape of the robot arm is modeled as a joint description form of a convex body, as shown in formula (1-3):

[0014]

[0015] in, Indicates that in x k The point set of the robot arm in the three-dimensional space coordinates in the state;

[0016] The minimum distance solution function between the robot arm and the environmental obstacles is defined as formula (1-4):

[0017]

[0018] Where dist(·) is the obstacle point set Point set with robotic arm The distance solving function between them;

[0019] 3) Establishing a cost function, i.e., the cost function of the path planning algorithm integrating model predictive control and artificial potential field;

[0020] 4) Establish constraints and combine the cost function with the constraints to form the final optimization problem to guide the safe and efficient movement of the robot in a complex environment.

[0021] Furthermore, in step 3), the repulsive field and the gravitational field are combined into the cost function, and the cost function of the path planning algorithm integrating model predictive control and artificial potential field is formula (1-5):

[0022]

[0023] Among them, U k is the prediction input at the current k moment, N is the prediction range, is the terminal cost, is the stage cost, P(x k ) is the repulsive field function, G(x k ) is the gravitational field function;

[0024] The stage cost refers to the cost associated with the reference trajectory tracking error, which is defined as shown in formula (1-6):

[0025]

[0026] Among them, p k is the translation term, C k Refers to the rotation matrix in the homogeneous transformation matrix of the end effector at the kth moment, R k , Q poal , Q coc,k is the weight factor;

[0027] The function of the repulsive field function is to enable the robot arm to react in advance when approaching an obstacle, ensuring that the robot arm avoids the obstacle smoothly. The definition is shown in formula (1-7):

[0028]

[0029] Among them, A o is the weight parameter of the repulsive potential field, For x k State end effector position T EE With obstacles collection The distance between p,lim is the upper limit distance at which the repulsion function works, and α is a constant value used to prevent significant cost changes when the distance changes are small;

[0030] The function of the gravitational field function is to ensure that the robot arm can smoothly reach the target point when the end effector approaches the target point, and to prevent the phenomenon of crossing the target point and then returning, as shown in formula (1-8):

[0031]

[0032] Among them, ζ is the weight parameter of the gravitational field, dist(T EE ,T goal ) is in x k State end effector position T EE With the target point T goal The distance function between g,lim is the upper limit distance at which the gravitational field function takes effect;

[0033] Furthermore, in step 4), since the working space of the robot arm is limited and the range of motion and speed of its joints are limited by hardware, it is necessary to clearly set the constraints of the joint position and speed; the constraints are defined as equations (1-9) and (1-10):

[0034] q i,min ≤q i ≤q i,max (1-9)

[0035] u i,min ≤u i ≤u i,max (1-10)

[0036] Among them, q i,min and represent the minimum and maximum q of the i-th joint respectively. i,max Joint position; u i,min and u i,max Respectively represent the minimum and maximum joint velocity of the i-th joint;

[0037] Another issue that needs to be considered in robot arm operation is self-collision. The core of self-collision avoidance is to ensure that each joint always maintains a safe distance. Mathematically, this can be converted into the minimum distance between critical points, as shown in formula (1-11):

[0038] s i,j ≥s i,j,min (1-11)

[0039] Among them, s i,j and i,j,min Respectively represent the distance between the i-th critical point and the j-th critical point and the minimum constraint distance;

[0040] In order to reduce the amount of calculation for solving the minimum distance between critical points, the self-collision distance is further converted into a position constraint for a specific joint, and the corresponding joint position constraint is set as formula (1-12):

[0041] q i,self,min ≤q i ≤q i,self,max (1-12)

[0042] In the above formula, q i,self,min and q i,self,max They represent the minimum and maximum joint positions of the i-th joint to avoid self-collision;

[0043] In addition, the collision between the robot arm and the base and the ground can be achieved by limiting the height of the critical point from the ground to a certain range, as shown in formula (1-13):

[0044] g i ≥g i,min (1-13)

[0045] Among them, g i and g i,min They represent the height between the i-th critical point and the ground and the minimum safe height respectively.

[0046] In step 4), the cost function constructed in step 3) is combined with the constraint conditions to obtain the optimization problem of path planning and obstacle avoidance, as shown in formula (1-14):

[0047]

[0048] The technical concept of the present invention is as follows: the present invention improves the algorithm and introduces a gravitational field function. By adding the influence of gravity, the stability and real-time performance of the robot in the obstacle avoidance process can be improved, thereby achieving more efficient motion control and planning. Specifically, the present invention proposes a strategy for the fusion of MPC and APF: first, the repulsive field generated by the obstacle is integrated into the MPC framework, giving the robot end effector a keen obstacle avoidance capability; secondly, in order to solve the backtracking problem that may occur when the robot approaches the target point, a gravitational field is configured near the target point. Doing so can not only guide the robot to approach the target in a smooth manner, but also avoid excessive focus on the local optimal solution, ensuring the optimization of the global path and the continuity of the motion.

[0049] The beneficial effects of the present invention are mainly manifested in: by introducing the repulsive field function and the gravitational field function in the artificial potential field into the cost function, the convergence of the system at the target point is effectively enhanced, the possible backtracking phenomenon is solved, and the real-time performance of the system is significantly improved; this method can not only make the robot arm more stable and efficient when performing tasks, but also effectively reduce the instability of the planned path, ensuring that the robot arm can also perform well in complex workspaces; the method of the present invention provides new ideas for the design and optimization of robot arm control systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 It is a block diagram for implementing the method of the present invention.

[0051] Figure 2 It is the X, Y, and Z position information of the end effector of the verification result of the method of the embodiment of the present invention, wherein (a) represents the X position change, (b) represents the Y position change, and (c) represents the Z position change. DETAILED DESCRIPTION

[0052] The present invention will be further described in detail below in conjunction with specific embodiments.

[0053] Reference Figure 1-2 , a robot arm path planning method integrating model predictive control and artificial potential field, the method comprising the following steps:

[0054] 1) Establishing the state model of the robot arm: In order to facilitate the prediction and control of the movement of the robot arm, the system state containing the position and velocity information of the joint is defined as x, which is a vector containing the position and velocity information of the joint, specifically expressed as where q represents the position of the joint, and represents the speed of the corresponding joint; for this model, a speed response controller will be designed, as shown in formula (1-1):

[0055]

[0056] Among them, x k and x k+1 are the state vectors at the kth and k+1th sampling moments, u k is the input vector at the kth sampling moment, f k (·) is the discrete state equation function, t s is the sampling time interval; in this example, t s =0.5s;

[0057] 2) Distance solution: When planning the motion trajectory of the robot arm, obstacles in the actual operating environment must be considered; in order to effectively integrate obstacle information into the control strategy, the geometric structure of the obstacle is represented by an open set, as shown in formula (1-2):

[0058]

[0059] in, Represents the point set of the j-th obstacle in three-dimensional space; the starting point is set to [0.6, 0.2, 0.2], the end point is set to [0.55, -0.2, 0.2], and the obstacle is a sphere with a radius of 0.15 and a center of [0.55, 0, 0.15];

[0060] The geometric shape of the robot arm is modeled as a joint description form of a convex body, as shown in formula (1-3):

[0061]

[0062] in, Indicates that in x k The point set of the robot arm in the three-dimensional space coordinates in the state;

[0063] The minimum distance solution function between the robot arm and the environmental obstacles is defined as formula (1-4):

[0064]

[0065] Where dist(·) is the obstacle point set Point set with robotic arm The distance solving function between them;

[0066] 3) Establishing the cost function: The cost function in the MPC-APF algorithm is defined as shown in formula (1-5):

[0067]

[0068] Among them, U k is the prediction input at the current k moment, N is the prediction range, is the terminal cost, is the stage cost, P(x k ) is the repulsive field function, G(x k ) is the gravitational field function;

[0069] The stage cost refers to the cost associated with the reference trajectory tracking error, which is defined as shown in formula (1-6):

[0070]

[0071] Among them, p k is the translation term, C k Refers to the rotation matrix in the homogeneous transformation matrix of the end effector at the kth moment, R k , Q poal , Q coc,k is the weight factor; in this example, the prediction horizon parameter N is set to 5, R k , Q poal , Q coc,k The weight factors are all set to diagonal matrices, but the values ​​on the main diagonal are different, and the rest of the matrix elements are 0, R k The elements of the main diagonal are [3,3,3], Q poal The elements of the main diagonal are [5,5,5], Q coc,k The elements of the main diagonal are [1,1,1];

[0072] The function of the repulsive field function is to enable the robot arm to react in advance when approaching an obstacle, ensuring that the robot arm avoids the obstacle smoothly. The definition is shown in formula (1-7):

[0073]

[0074] Among them, A o is the weight parameter of the repulsive field, For x k State end effector position T EE With obstacles collection The distance between the solution function, d p,lim is the upper limit distance of the repulsion function. The constant value α is used to reduce the impact of significant cost changes caused by small distance changes. The weight parameter A is set o =1, the upper limit distance d of the repulsive field p,lim =25cm, value α=0.05;

[0075] The function of the gravitational field function is to ensure that the robot arm can smoothly reach the target point when the end effector approaches the target point, and to prevent the phenomenon of crossing the target point and then returning. The definition is shown in formula (1-8):

[0076]

[0077] Among them, ζ is the weight parameter of the gravitational field, dist(T EE ,T goal ) is in x k State end effector position T EE With the target point T goal The distance function between g,lim is the upper limit distance of the gravitational field function; set the weight parameter ζ = 7.2, the upper limit distance d of the gravitational field g,lim =30cm;

[0078] 4) Establish constraint conditions: Since the working space of the robot arm is limited, and the range of motion and speed of its joints are limited by hardware, it is necessary to clearly set the constraints of joint position and speed; the constraints are defined as equations (1-9) and (1-10):

[0079] q i,min ≤q i ≤q i,max (1-9)

[0080] u i,min ≤u i ≤u i,max (1-10)

[0081] Among them, q i,min and represent the minimum and maximum q of the i-th joint respectively. i,max Joint position; u i,min and u i,max Represent the minimum and maximum joint speeds of the i-th joint respectively; set the minimum joint position to q i,min =[-174.53, -2.2000, -174.53, -2.5656, -174.53, -2.0500, -174.53], the maximum joint position is q i,max =[174.53, 2.2000, 174.53, 2.5656, 174.53, 2.0500, 174.53]; Set the minimum joint speed to u i,min =[-0.8927,-0.8727,-0.8927,-0.8727,-0.8927,-0.8727,-0.8927], the maximum joint speed is u i,max =[0.8927,0.8727,0.8927,0.8727,0.8927,0.8727,0.8927,0.8927];

[0082] Another issue that needs to be considered in robot arm operation is self-collision. The core of self-collision avoidance is to ensure that each joint always maintains a safe distance. Mathematically, it can be converted into the minimum distance between critical points, as shown in formula (1-11):

[0083] s i,j ≥s i,j,min (1-11)

[0084] Among them, s i,j and i,j,min Respectively represent the distance between the i-th critical point and the j-th critical point and the minimum constraint distance;

[0085] In order to reduce the amount of calculation for optimizing the minimum distance between critical points, the self-collision distance can be further converted into a position constraint of a specific joint, and the corresponding joint position constraint can be set as formula (1-12);

[0086] q i,self,min ≤q i ≤q i,self,max (1-12)

[0087] In the above formula, q i,self,min and q i,self,max They represent the minimum and maximum joint positions of the i-th joint to avoid self-collision;

[0088] In addition, the collision between the robot arm and the base and the ground can be achieved by limiting the height of the critical point from the ground to a certain range, as shown in formula (1-13):

[0089] g i ≥g i,min (1-13)

[0090] Among them, g i and g i,min Respectively represent the height between the i-th critical point and the ground and the minimum safe height;

[0091] According to the cost function constructed in step 3) and the constraint conditions constructed in step 4), the optimization problem of path planning and obstacle avoidance is obtained, as shown in formula (1-14):

[0092]

[0093] In this embodiment, the end effector of the robotic arm is selected to perform the task of avoiding obstacles in order to verify the method proposed in this embodiment. During the movement of the robotic arm according to the specified end task, we set obstacles in the workspace to verify the safety of the end effector. First, in step 1), a state model of the robotic arm is established. Subsequently, in step 2), mathematical models of obstacles and robotic arms are set up respectively, and a solution formula for the minimum distance between the robotic arm and the obstacle is constructed. In step 3), objective functions (i.e., cost functions) of repulsive fields, gravitational fields, and nonlinear costs are established respectively, and they are fused. Afterwards, in step 4), the physical constraints of the robotic arm and the constraints of itself and the base are set, and these constraints are combined with the cost function, ultimately forming an optimization problem for path planning and obstacle avoidance. The optimization problem is solved by a solver, and the optimal input of the system is obtained, which is used to guide the movement of the end controller of the robotic arm. The experimental results are as follows: Figure 2 As shown, Figure 2 Shows the X, Y, and Z information of the end-effector path trajectory planned by our method.

[0094] The above are preferred embodiments of the present invention. Any changes made according to the technical solution of the present invention, as long as the resulting functions do not exceed the scope of the technical solution of the present invention, belong to the protection scope of the present invention.

Claims

1. A robot arm path planning method integrating model predictive control and artificial potential field, characterized in that: The method comprises the following steps: 1) Establish the robot state model: define the system state vector as x, which contains the position and velocity information of the joint, expressed as Where q represents the position of the joint, Represents the speed of the corresponding joint; for the robotic arm system, the controller is designed as a speed response controller as shown in formula (1-1); Among them, x k and x k+1 are the state vectors at the kth and k+1th sampling moments, u k is the input vector at the kth sampling moment, f k (·) is the discrete state equation function, t s is the sampling time interval; 2) Distance solution: Define the geometric structure of obstacles in the robot workspace as an open set O obs ; in, Represents the point set of the j-th obstacle in three-dimensional space coordinates; The geometric shape of the robot arm is modeled as a joint description of a convex body, as shown in formula (1-3): in, Indicates that in x k The point set of the robot arm in the three-dimensional space coordinates in the state; Formula (1-4) is the function for solving the minimum distance between the robot arm and the environmental obstacles; Where dist(·) is the obstacle point set Point set with robotic arm The distance solving function between them; 3) Establishing a cost function, i.e., the cost function of the path planning algorithm integrating model predictive control and artificial potential field; 4) Establish constraints and combine the cost function with the constraints to form the final optimization problem to guide the safe and efficient movement of the robot in a complex environment.

2. A robot arm path planning method integrating model predictive control and artificial potential field as claimed in claim 1, characterized in that: In step 3), the repulsive field and the gravitational field are combined into the cost function, and the cost function of the path planning algorithm integrating model predictive control and artificial potential field is formula (1-5): Among them, U k is the prediction input at the current k moment, N is the prediction range, l N (x N ) is the terminal cost, l k (x k ,u k ) is the stage cost, P(x k ) is the repulsive field function, G(x k ) is the gravitational field function; The stage cost function is defined as follows (1-6): In the formula, p k is the translation term, C k Refers to the rotation matrix in the homogeneous transformation matrix of the end effector at the kth moment, R k , Q poal , Q coc,k is the weight factor; Formula (1-7) defines the repulsive field function: Among them, A o is the weight parameter of the repulsive potential field, For x k State end effector position T EE With obstacles collection Distance between p,lim is the upper limit distance where the repulsion function takes effect, and α is a constant value; The gravitational field function is defined as shown in formula (1-8): Among them, ζ is the weight parameter of the gravitational field, For x k State end effector position T EE With the target point T goal Distance between g,lim This is the upper limit distance at which the gravitational field function takes effect.

3. The method for manipulator path planning by integrating model predictive control and artificial potential field as claimed in claim 2, characterized in that: In step 4), since the range of motion and speed of the robot arm joints are limited by hardware, it is necessary to clearly set the joint position constraints of formula (1-9) and the joint speed constraints of formula (1-10); q i,min ≤q i ≤q i,max (1-9) in i,min in i in i,max (1-10) Among them, q i,min and represent the minimum and maximum q of the i-th joint respectively. i,max Joint position; u i,min and u i,max Respectively represent the minimum and maximum joint velocity of the i-th joint; Another issue that needs to be considered in robot arm operation is self-collision. The core of self-collision avoidance is to ensure that each joint always maintains a safe distance. Mathematically, this can be converted into the minimum distance between critical points, as shown in formula (1-11): s i,j ≥s i,j,min (1-11) Among them, s i,j and i,j,min Respectively represent the distance between the i-th critical point and the j-th critical point and the minimum constraint distance; The distance constraint of self-collision detection is converted into a position constraint for a specific joint, which is defined as formula (1-12): q i,self,min ≤q i ≤q i,self,max (1-12) In the formula, q i,self,min and q i,self,max They represent the minimum and maximum joint positions of the i-th joint to avoid self-collision; To avoid collision between the robot arm and the base and the ground, a constraint on the height of the critical point from the ground is set as shown in formula (1-13); g i ≥g i,min (1-13) Among them, g i and g i,min They represent the height between the i-th critical point and the ground and the minimum safe height respectively.

4. The method for manipulator path planning by integrating model predictive control and artificial potential field as claimed in claim 3, characterized in that: In step 4), the final optimization problem formula (1-14) is:

Images