Motion track optimization method for mechanical arm and storage medium

Abstract

The invention provides a motion trail optimization method for a mechanical arm and a storage medium. The method comprises the following steps that a charging mechanical arm model used for force control and an interaction dynamics model between a charging plug at the tail end of the charging mechanical arm and a vehicle charging jack are established; designing an optimization target and a constraint condition of a force position track of a charging plug at the tail end of the charging mechanical arm to obtain a force position track optimization problem of the charging mechanical arm; the force position trajectory optimization problem obtained in the problem description step is decoupled into an independent force position trajectory tracking problem and a mechanical arm trajectory planning problem with convex constraint, and an alternating direction multiplier method is adopted for solving; and a tail end force-position hybrid model prediction controller of the charging mechanical arm is designed, and a joint torque control instruction of the mechanical arm at the current moment is obtained by combining the current system state, so that each joint driver of the charging mechanical arm executes tail end force-position hybrid control according to the obtained control instruction.

Description

Technical Field

[0001] The present invention relates to the field of industrial robots, and in particular to a motion trajectory optimization method and a storage medium for a robotic arm. Background Art

[0002] Many robotic arm tasks require motion planning when making contact in a constrained environment. For example, in the robotic arm automatic charging task, the environment is severely constrained and it is crucial to achieve safe and stable interaction. In this case, the motion of the robotic arm is coupled with the reaction forces exerted on the environment. Trajectory optimization methods that use environmental interaction models as constraints have been widely studied in the field of robotic arms (for example, the method and device for real-time force interaction control of an ultrasonic probe on a contact surface disclosed in Chinese invention patent application publication number CN117731324A). By incorporating contact forces and joint states into the optimization, contact-dynamics consistent motions can be planned for complex robotic behaviors, such as dynamic motion or dexterous object manipulation. Although the above work has shown impressive results in automatically discovering contact sequences, the optimized contact forces have not been used for control, but only as a way to explore physical interactions with objects.

[0003] This paper aims to accurately track the desired force trajectory as well as the end-effector path, which is a common requirement in safety-critical tasks but has not been fully explored. In addition, since complex contact environments such as automatic charging of vehicles are prone to disturbances and difficult to model or predict, it is necessary to solve the optimization problem online by updating the contact state information. However, efficiently solving highly constrained manipulators and contact dynamics still requires further research. Summary of the invention

[0004] The purpose of the present invention is to provide a motion trajectory optimization method for a robotic arm in view of the shortcomings of the prior art. The method constructs the constraints of the trajectory optimization problem of the end of the charging robotic arm by introducing an environmental dynamics model, taking into account the requirements of solving the optimization problem accuracy and real-time control of the charging robotic arm.

[0005] According to a first aspect of the present invention, a motion trajectory optimization method for a robot arm is provided, the method comprising the following steps: a model building step, which establishes a charging robot arm model suitable for force control and an interactive dynamics model between a charging plug at the end of the charging robot arm and a charging socket of a vehicle; a problem description step, which designs an optimization target and constraint conditions of the force-position trajectory of the charging plug at the end of the charging robot arm based on the charging robot arm model and the interactive dynamics model, and obtains a force-position trajectory optimization problem of the charging robot arm based on the optimization target and constraint conditions; a problem solving step, which solves the force-position trajectory optimization problem obtained in the problem description step. The invention discloses a charging robot arm which is coupled with an independent force-position trajectory tracking problem and a robot arm trajectory planning problem with convex constraints, and adopts the alternating direction multiplier method to solve them; an end jack force-position hybrid control step, which designs an end force-position hybrid model predictive controller of the charging robot arm based on the optimization problem solving method used in the problem solving step, and obtains the joint torque control instruction of the robot arm at the current moment in combination with the current system state, so that each joint driver of the charging robot arm performs the end force-position hybrid control according to the obtained joint torque control instruction, wherein the system state is represented by at least one of the robot arm joint angle, joint angular velocity and joint torque.

[0006] As a preferred solution, in the motion trajectory optimization method according to the present invention, in the model building step, the charging robot arm model is expressed by the following formula:

[0007]

[0008] Where q represents the joint angle vector of the charging robot arm, represents the joint angular velocity vector of the charging robot arm, represents the joint angular acceleration vector of the charging robot arm, τ u represents the joint torque vector of the charging robot arm; M(q) represents the inertia matrix of the charging robot arm, represents the Coriolis force matrix of the charging robot arm, G(q) represents the gravity matrix of the charging robot arm, fe represents the end force vector of the robot arm, and J(q) represents the Jacobian matrix of the contact point at the end of the charging robot arm; and the interactive dynamics model between the charging plug at the end of the charging robot arm and the charging socket of the vehicle is expressed by the following formula:

[0009]

[0010] Among them, n z Represents the normal unit vector of the contact surface between the charging plug and the charging socket, n v represents the tangential unit vector of the contact surface, E represents the simplified Young's modulus of the contact surface, R represents the maximum deformation of the contact between the charging plug and the charging socket, and F zrepresents the normal force applied by the charging robot arm on the contact surface, d represents the maximum static deformation between the charging robot arm and the charging socket, μ represents the friction coefficient of the contact surface, ν represents the Poisson's ratio of the contact surface, k d represents the damping coefficient of the charging robot along the moving direction, v e Indicates the velocity of the contact point.

[0011] As a preferred solution, in the motion trajectory optimization method according to the present invention, in the problem description step (S200), the optimization target of the force position trajectory of the charging plug at the end of the charging robot arm is expressed by the following formula:

[0012]

[0013] Among them, l F represents the expected force trajectory tracking accuracy index, l τ represents the minimum joint torque index, l x Represents the expected end position trajectory tracking accuracy indicator.

[0014] As a preferred solution, in the motion trajectory optimization method according to the present invention, in the problem description step (S200), the constraint conditions of the force and position trajectory of the charging plug at the end of the charging robot arm include at least one of the interactive dynamics constraint, the initial state constraint, the mechanical constraint of the robot arm joint, and the contact constraint between the charging robot arm and the charging socket, and the force and position trajectory optimization problem of the charging robot arm is expressed by the following formula:

[0015]

[0016] Where N is the number of prediction steps of the trajectory optimization algorithm, i represents the time, φ[i] is the decision vector of the trajectory optimization problem at time i, δF[i] = F z [i]-F d [i] is the normal force tracking error vector at time i, τ u [i] is the joint torque vector of the charging robot arm at time i, x[i] is the state vector of the charging robot arm at time i, represents the forward kinematic transformation function of the charging manipulator, is the expected trajectory vector of the charging gun end of the charging robot arm at time i, κ[i] is the expected motion trajectory curvature of the end at time i, qlower and qupper represent the lower limit and upper limit of the joint space position limit of the charging robot arm, respectively. and They represent the lower limit and upper limit of the joint torque limit of the charging robot arm, respectively, x0 represents the initial state of the charging robot arm, is the normal mapping vector of the end force of the charging robot arm, QF, R and Wp are the desired force trajectory tracking gain matrix, the joint torque output minimization gain matrix and the desired end position trajectory tracking gain matrix respectively.

[0017] As a preferred solution, in the motion trajectory optimization method according to the present invention, the problem solving step further includes the following steps: a problem decoupling step, which decouples the trajectory optimization problem constructed in the problem description step into three parallel sub-problems by defining distributed optimization variables and corresponding consistency constraints, so as to convert it into a consistent distributed constraint optimization problem; a constraint combining step, which adopts the augmented Lagrangian method to combine the consistency constraints into the three parallel sub-problems respectively; an initialization step, which initializes the decision vectors of the three parallel sub-problems and the formal variables related to the consistency constraints to zero vectors respectively; an iterative solution step, which iteratively solves the trajectory optimization problem; and an output step, which calculates the original residual of the kth iteration, and outputs the decision vector when the calculated original residual meets the iteration termination condition.

[0018] As a preferred solution, in the motion trajectory optimization method according to the present invention, in the problem decoupling step, the three parallel sub-problems are: the dynamic constraint sub-problem; the inverse kinematics sub-problem; and the joint constraint mapping sub-problem.

[0019] As a preferred scheme, in the motion trajectory optimization method according to the present invention, the augmented Lagrangian method used in the constraint combination step further includes: an augmented form of the dynamic constraint subproblem containing consistency constraints; an augmented form of the inverse kinematics subproblem containing consistency constraints; and an augmented form of the joint constraint mapping subproblem containing consistency constraints.

[0020] As a preferred solution, in the motion trajectory optimization method according to the present invention, the iterative solution step further includes: using a differential dynamic programming method to solve the dynamic constraint subproblem containing the consistency constraint to obtain a decision vector φk+1 of the trajectory optimization problem; using a gradient descent method to solve the inverse kinematics subproblem containing the consistency constraint to obtain a decision vector φk+1 of the inverse kinematics subproblem The gradient descent method is used to solve the joint constraint mapping subproblem containing the consistency constraint, and the decision variables of the joint constraint mapping subproblem are obtained. (S343); and updating the formal variables associated with the consistency constraint and (S344).

[0021] As a preferred solution, in the motion trajectory optimization method according to the present invention, the end force-position hybrid control step further includes the following steps: a control instruction calculation step, which calculates the robot arm joint torque control instruction of the charging robot arm at time t according to the trajectory optimization decision variables solved in the problem solving step; a control instruction input step, which inputs the robot arm joint torque control instruction calculated in the control instruction calculation step into the charging robot arm; and a loop step, returning to the initialization step (S330) at time t+1 until the gun insertion task of the charging interface is completed.

[0022] According to a second aspect of the present invention, there is provided a non-temporary storage medium storing a computer program, which, when executed by a processor, can implement the motion trajectory optimization method described in the first aspect of the present invention.

[0023] The beneficial effects of the present invention are: in automated charging tasks that require physical contact, a contact interaction model between the charging gun and the charging interface is designed, a distributed trajectory optimization framework is proposed, and it is executed in the form of a model predictive controller, which can handle the desired operating effects and contact constraints, and integrate the interactive dynamics model into the trajectory optimization task to ensure the safe execution of the automatic charging task. The motion trajectory optimization of the present invention significantly improves the motion and force tracking accuracy in both static and dynamic environments. Compared with other trajectory optimization methods, the trajectory optimization method of the present invention has better real-time performance, and has the advantages of strong versatility and ease of use. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 A flow chart of a motion trajectory optimization method for a robot arm according to the present invention is illustrated.

[0025] Figure 2 The flowchart of the sub-process of the problem solving step in the motion trajectory optimization method for a robot arm according to the present invention is illustrated.

[0026] Figure 3 The flowchart illustrates a sub-process of the iterative solution step in the motion trajectory optimization method for a robot arm according to the present invention.

[0027] Figure 4 The flowchart illustrates a sub-process of the end force-position hybrid control step in the motion trajectory optimization method for a robot arm according to the present invention. DETAILED DESCRIPTION

[0028] Exemplary embodiments of the present invention are described in detail below with reference to the accompanying drawings. It should be noted that the relative arrangement of components, numerical expressions and numerical values ​​described in these embodiments do not limit the scope of the present invention unless otherwise specifically stated.

[0029] The motion trajectory optimization method for a robot arm of the present invention can be implemented by a processor in the robot arm or a processor in a robot equipped with / mounted with the robot arm executing a computer program stored in a memory in the robot arm or the robot. As an alternative, the robot arm or the robot equipped with / mounted with the robot arm can communicate with a server, and the processor in the server executes a computer program stored in the server or the cloud and feeds back the program execution results to the robot arm or the robot in real time.

[0030] As an example, in the following embodiments, a scenario in which a charging robot arm operates a charging gun to insert or pull out a charging gun from a vehicle's charging port is used as an example for explanation. The charging robot arm does not refer to a charging type robot arm. Obviously, the motion trajectory optimization method of the present invention can also be applied to a non-charging type robot arm.

[0031] First, refer to Figure 1 To illustrate the motion trajectory optimization method for a robot arm according to the present invention, the method includes the following steps S100 to S400:

[0032] In the model building step S100, a model of the dynamics of the charging robot arm system is built. Specifically, a charging robot arm model suitable for force control and an interactive dynamics model between the charging plug at the end of the charging robot arm and the vehicle charging socket are established.

[0033] First, preferably, by integrating the dynamics and kinematics of the robot arm, a charging robot arm model suitable for force control is established. The charging robot arm model is preferably expressed by the following formula (1):

[0034]

[0035] Where q represents the joint angle vector of the charging robot arm, represents the joint angular velocity vector of the charging robot arm, represents the joint angular acceleration vector of the charging robot arm, τ u represents the joint torque vector of the charging robot arm, M(q) represents the inertia matrix of the charging robot arm, represents the Coriolis force matrix of the charging robot arm, G(q) represents the gravity matrix of the charging robot arm, and f e represents the end force vector of the robot arm, and J(q) represents the Jacobian matrix of the contact point at the end of the charging robot arm.

[0036] Secondly, preferably by combining Hertz theory, an interactive dynamics model between the charging plug at the end of the charging robot arm and the vehicle charging socket is established. The interactive dynamics model is preferably expressed by the following formula (2):

[0037]

[0038] Among them, n z Represents the normal unit vector of the contact surface between the charging plug and the charging socket, n y represents the tangential unit vector of the contact surface, E represents the simplified Young's modulus of the contact surface, R represents the maximum deformation of the contact between the charging plug and the charging socket, and F z represents the normal force applied by the charging robot arm on the contact surface, d represents the maximum static deformation between the charging robot arm and the charging socket, μ represents the friction coefficient of the contact surface, ν represents the Poisson's ratio of the contact surface, and k d represents the damping coefficient of the charging robot along the moving direction, v e Indicates the velocity of the contact point.

[0039] Furthermore, the maximum static deformation d between the charging robot arm and the charging socket in the above formula (2) is:

[0040]

[0041] Normal force F applied by the charging robot on the contact surface z for:

[0042] F z =F e cosθ F (4)

[0043] Among them, θ F is the angle between the contact force direction and the normal of the contact surface. The velocity v of the contact point e for:

[0044]

[0045] Next, the problem description step S200 is entered. In step S200, the charging robot arm force-position hybrid trajectory optimization problem description is performed. Specifically, firstly, based on the charging robot arm model and the interactive dynamics model, the optimization objectives and constraints of the charging plug force-position trajectory at the end of the charging robot arm are designed. Among them, the optimization objective is expressed by the following formula (6):

[0046]

[0047] Among them, l F represents the expected force trajectory tracking accuracy index, l r represents the minimum joint torque index, l x Represents the expected end position trajectory tracking accuracy indicator.

[0048] As a preferred solution, the constraint conditions of the force trajectory of the charging plug at the end of the charging robot arm include at least one of interactive dynamics constraints, initial state constraints, robot arm joint mechanical constraints and contact constraints between the charging robot arm and the charging socket.

[0049] Secondly, based on the above optimization objectives and various constraints, the force-position trajectory optimization problem of the charging robot arm is obtained.

[0050] The force position trajectory optimization problem obtained in step S200 can be expressed by the following formula (7):

[0051]

[0052] Where N is the number of prediction steps of the trajectory optimization algorithm, i represents the time, φ[i] is the decision vector of the trajectory optimization problem at time i, δF[i] = F z [i]-F d [i] is the normal force tracking error vector at time i, τ u [i] is the joint torque vector of the charging robot arm at time i, x[i] is the state vector of the charging robot arm at time i, represents the forward kinematic transformation function of the charging manipulator, is the expected trajectory vector of the charging gun end of the charging robot arm at time i, κ[i] is the expected motion trajectory curvature of the end at time i, q lower and q upper Respectively represent the lower limit and upper limit of the joint space position limit of the charging robot arm, and They represent the lower limit and upper limit of the joint torque limit of the charging robot arm, respectively, x0 represents the initial state of the charging robot arm, is the normal mapping vector of the end force of the charging manipulator, Q F , R and W p They are the desired force trajectory tracking gain matrix, the joint torque output minimization gain matrix, and the desired end position trajectory tracking gain matrix.

[0053] Next, the problem solving step S300 is entered, in which the force-position hybrid trajectory optimization problem is solved. Specifically, the force-position trajectory optimization problem in the problem description step S200 is decoupled into an independent force-position trajectory tracking problem and a robot trajectory planning problem with convex constraints, and the alternating direction multiplier method is used for the solution.

[0054] The following reference Figure 2 The problem solving step S300 in the motion trajectory optimization method for a robot arm according to the present invention is described in detail. Figure 2As shown, the problem solving step S300 includes the following sub-steps S310 to S370.

[0055] First, in the problem decoupling step S310, the trajectory optimization problem constructed in the problem description step S200 is decoupled into three parallel sub-problems by defining distributed optimization variables and corresponding consistency constraints, and then converted into a consistent distributed constraint optimization problem.

[0056] The consistent distributed constrained optimization problem is expressed as follows:

[0057]

[0058] in, is the joint angular velocity and end force joint vector at time i, and is the distributed optimization variable of the joint angle vector q[i] at time i, is the distributed optimization variable of the joining vector λ[i] at time i, is the distributed optimization variable of the joint vector u[i] at time i, is the obstacle function form of the mechanical constraint of the robot joint and the contact constraint between the charging robot and the charging socket, which is specifically expressed by the following formula (9):

[0059]

[0060] Furthermore, the three parallel sub-problems decoupled in step S310 are:

[0061] (3.1.1) The dynamic constraint subproblem is expressed as follows:

[0062]

[0063] (3.1.2) The inverse kinematics problem is expressed as follows:

[0064]

[0065] (3.1.3) The joint constraint mapping sub-problem is expressed as follows:

[0066]

[0067] Next, the process enters the constraint combining step S320. In step S320, the augmented Lagrangian method is used to combine the consistency constraints into three parallel sub-problems respectively, so as to obtain the following augmented form of the sub-problems including the consistency constraints:

[0068] (3.2.1) The augmented form of the dynamic constraint subproblem including the consistency constraint is expressed as follows:

[0069]

[0070] In the formula, ρ j , u and ρ f are the optimization step parameters for each consistency constraint, v j , and v f are formal variables related to consistency constraints respectively.

[0071] (3.2.2) The augmented form of the inverse kinematics subproblem including the consistency constraint is expressed as follows:

[0072]

[0073] In the formula, v ik are formal variables associated with consistency constraints.

[0074] (3.2.3) The augmented form of the joint constraint mapping subproblem including the consistency constraint is expressed as follows:

[0075]

[0076] Next, the initialization step S330 is entered. In step S330, the decision vectors of the three parallel sub-problems and the formal variables related to the consistency constraint are initialized to zero vectors. Specifically, according to the state of the charging robot at the current time t, when the iteration step k = 0, the decision vector φ of the dynamic constraint sub-problem is initialized to zero vectors. 0 , the decision vector of the inverse kinematics subproblem and the decision vector of the joint constraint mapping subproblem Initialize them to zero vectors respectively, and set the formal variables related to the consistency constraints and They are initialized to zero vectors respectively, and then the process enters the iterative solution step S340.

[0077] In step S340, the above trajectory optimization problem is solved iteratively. Figure 3 The iterative solution step S340 in the motion trajectory optimization method for a robot arm according to the present invention is described in detail. Figure 3 As shown, the iterative solution step S340 includes the following sub-steps S341 to S344.

[0078] First, in step S341, the differential dynamic programming method is used to solve the dynamic constraint subproblem (3.2.1) including the consistency constraint, and the decision vector φ of the trajectory optimization problem is obtained. k+1 ;

[0079] Then, in step S342, the gradient descent method is used to solve the inverse kinematics subproblem (3.2.2) including the consistency constraint, and the decision vector of the inverse kinematics subproblem is obtained.

[0080] Next, in step S343, the gradient descent method is used to solve the joint constraint mapping subproblem (3.2.3) including the consistency constraint, and the decision variables of the joint constraint mapping subproblem are obtained.

[0081] Finally, in step S344, the formal variables related to the consistency constraint are updated using the following formula (16): and

[0082]

[0083] After step S340 is completed, the process enters the output step S350. In step S350, the original residual of the k-th iteration is calculated, and then in step S360, it is determined whether the calculated original residual of the k-th iteration meets the iteration termination condition. If it meets, the process enters step S370 to output the decision vector obtained in steps S341 to S343; otherwise, it returns to step S340.

[0084] The original residual is expressed as follows:

[0085]

[0086] The iteration termination condition is expressed by the following formula (18):

[0087] ||r k+1 ||2<∈ p (18)

[0088] In formula (18), ∈ p It is the iteration termination threshold, which can be adjusted according to the actual solution needs, for example, it can be adjusted according to the trajectory tracking accuracy required by the task.

[0089] Next, the end-plug force-position hybrid control step S400 is entered, in which the end-plug force-position hybrid control of the charging robot arm is performed. Specifically, based on the optimization problem solving method used in the problem solving step S300, the end-plug force-position hybrid model predictive controller of the charging robot arm is designed, and combined with the current system state, the joint torque control instruction of the robot arm at the current moment is obtained, so that the joint drivers of the charging robot arm execute the obtained joint torque control instruction to realize the end-plug force-position hybrid control. Among them, the system state can be represented by parameters such as the robot arm joint angle, joint angular velocity and / or joint torque.

[0090] The following reference Figure 4 The end force-position hybrid control step S400 in the motion trajectory optimization method for a robot arm according to the present invention is described in detail. Figure 4 As shown, the terminal force-position hybrid control step S400 includes the following sub-steps S410 to S430.

[0091] First, in step S410, the robot joint torque control command of the charging robot arm at time t is calculated according to the trajectory optimization decision variables solved in the problem solving step S300. The robot joint torque control command of the charging robot arm at time t is expressed by the following formula (19):

[0092]

[0093] Among them, K is the positive constant gain matrix, C is the positive constant compliance matrix, is the joint angle measurement value of the charging robot arm at time t, is the measured value of the external force applied to the end of the charging robot arm at time t.

[0094] Next, the process proceeds to step S420, in which the robot arm joint torque control instruction calculated in step S410 is input into the charging robot arm, and then the process proceeds to step S430. At time t+1, the process returns to step S330 until the charging port plugging task is completed and the process ends.

[0095] In summary, in automated charging tasks that require physical contact, a contact interaction model between the charging gun and the charging interface is designed, a distributed trajectory optimization framework is proposed, and it is executed in the form of a model predictive controller, which can handle the desired operating effects and contact constraints, and integrate the interactive dynamics model into the trajectory optimization task to ensure the safe execution of the automatic charging task. The motion trajectory optimization of the present invention significantly improves the motion and force tracking accuracy in both static and dynamic environments. Compared with other trajectory optimization methods, the trajectory optimization method of the present invention has better real-time performance, and has the advantages of strong versatility and ease of use.

[0096] [Other embodiments]

[0097] The embodiments of the present invention may also be implemented by reading and executing computer executable instructions (e.g., one or more programs) recorded on a storage medium (which may also be more completely referred to as a "non-transitory computer-readable storage medium") to perform one or more functions in the above-mentioned embodiments, and / or a computer of a system or device including one or more circuits (e.g., an application-specific integrated circuit (ASIC)) for performing one or more functions in the above-mentioned embodiments, and the embodiments of the present invention may be implemented by, for example, reading and executing the computer executable instructions from the storage medium by the computer of the system or device to perform one or more functions in the above-mentioned embodiments, and / or controlling the one or more circuits to perform one or more functions in the above-mentioned embodiments. The computer may include one or more processors (e.g., a central processing unit (CPU), a microprocessing unit (MPU)), and may include a network of separate computers or separate processors to read and execute the computer executable instructions. The computer executable instructions may be provided to the computer, for example, from a network or the storage medium. The storage medium may include, for example, a hard disk, a random access memory (RAM), a read-only memory (ROM), a memory of a distributed computing system, an optical disk (such as a compact disc (CD), a digital versatile disc (DVD), or a Blu-ray disc (BD) TM ), flash memory devices, memory cards, etc.

[0098] Although the present invention has been described above with reference to exemplary embodiments, the above embodiments are only for illustrating the technical concept and features of the present invention and cannot be used to limit the protection scope of the present invention. Any equivalent variations or modifications made according to the spirit and essence of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for optimizing the motion trajectory of a robotic arm, the method comprising the following steps: A model building step, which establishes a charging robot arm model suitable for force control and an interactive dynamics model between a charging plug at the end of the charging robot arm and a charging socket of a vehicle (S100); A problem description step, which designs the optimization target and constraint conditions of the force-position trajectory of the charging plug at the end of the charging robot arm based on the charging robot arm model and the interactive dynamics model, and obtains the force-position trajectory optimization problem of the charging robot arm based on the optimization target and constraint conditions (S200); a problem solving step, which decouples the force-position trajectory optimization problem obtained in the problem description step into an independent force-position trajectory tracking problem and a robot trajectory planning problem with convex constraints, and solves them using an alternating direction multiplier method (S300); The end jack force-position hybrid control step is based on the alternating direction multiplier method used in the problem solving step to design the end force-position hybrid model predictive controller of the charging robot arm, and combined with the current system state, obtain the joint torque control instruction of the robot arm at the current moment, so that each joint driver of the charging robot arm performs the end force-position hybrid control according to the obtained joint torque control instruction (S400), The system state is represented by at least one of the robot arm joint angle, joint angular velocity and joint torque.

2. The motion trajectory optimization method according to claim 1, wherein: In the model building step (S100), the charging robot arm model is expressed by the following formula: Where q represents the joint angle vector of the charging robot arm, represents the joint angular velocity vector of the charging robot arm, represents the joint angular acceleration vector of the charging robot arm, τ u represents the joint torque vector of the charging robot arm; M(q) represents the inertia matrix of the charging robot arm, represents the Coriolis force matrix of the charging robot arm, G(q) represents the gravity matrix of the charging robot arm, fe represents the end force vector of the robot arm, and J(q) represents the Jacobian matrix of the contact point at the end of the charging robot arm; The interactive dynamics model between the charging plug at the end of the charging robot arm and the vehicle charging socket is expressed by the following formula: Among them, n z Represents the normal unit vector of the contact surface between the charging plug and the charging socket, n v represents the tangential unit vector of the contact surface, E represents the simplified Young's modulus of the contact surface, R represents the maximum deformation of the contact between the charging plug and the charging socket, and F z represents the normal force applied by the charging robot arm on the contact surface, d represents the maximum static deformation between the charging robot arm and the charging socket, μ represents the friction coefficient of the contact surface, ν represents the Poisson's ratio of the contact surface, k d represents the damping coefficient of the charging robot along the moving direction, v e Indicates the velocity of the contact point.

3. The motion trajectory optimization method according to claim 1, wherein: In the problem description step (S200), the optimization target of the force position trajectory of the charging plug at the end of the charging robot arm is expressed by the following formula: Among them, l F represents the expected force trajectory tracking accuracy index, l τ represents the minimum joint torque index, l x Represents the expected end position trajectory tracking accuracy indicator.

4. The motion trajectory optimization method according to claim 1, wherein: In the problem description step (S200), the constraint conditions of the force trajectory of the charging plug at the end of the charging robot arm include at least one of an interactive dynamics constraint, an initial state constraint, a robot arm joint mechanical constraint, and a contact constraint between the charging robot arm and the charging socket. In addition, the force position trajectory optimization problem of the charging robot arm is expressed by the following formula: Where N is the number of prediction steps of the trajectory optimization algorithm, i represents the time, φ[i] is the decision vector of the trajectory optimization problem at time i, δF[i] = F z [i]-F d [i] is the normal force tracking error vector at time i, τ u [i] is the joint torque vector of the charging robot arm at time i, x[i] is the state vector of the charging robot arm at time i, represents the forward kinematic transformation function of the charging manipulator, is the expected trajectory vector of the charging gun end of the charging robot arm at time i, κ[i] is the expected motion trajectory curvature of the end at time i, q lower and q upper Respectively represent the lower limit and upper limit of the joint space position limit of the charging robot arm, and They represent the lower limit and upper limit of the joint torque limit of the charging robot arm, respectively, x0 represents the initial state of the charging robot arm, is the normal mapping vector of the end force of the charging manipulator, Q F , R and W p They are the desired force trajectory tracking gain matrix, the joint torque output minimization gain matrix, and the desired end position trajectory tracking gain matrix.

5. The motion trajectory optimization method according to claim 1, wherein: The problem solving step (S300) further comprises the following steps: A problem decoupling step, which decouples the trajectory optimization problem constructed in the problem description step into three parallel sub-problems by defining distributed optimization variables and corresponding consistency constraints, so as to transform the problem into a consistent distributed constraint optimization problem (S310); In a constraint combining step, the augmented Lagrangian method is used to combine the consistency constraints into the three parallel sub-problems respectively (S320); An initialization step, which initializes the decision vectors of the three parallel sub-problems and the formal variables related to the consistency constraints to zero vectors respectively (S330); An iterative solution step, which iteratively solves the trajectory optimization problem (S340); as well as The output step calculates the original residual of the k-th iteration, and outputs a decision vector when the calculated original residual meets the iteration termination condition (S350 to S370).

6. The motion trajectory optimization method according to claim 5, wherein: In the problem decoupling step (S310), the three parallel sub-problems are: the dynamics constraint subproblem; The inverse kinematics subproblem; and The joint constraint mapping subproblem.

7. The motion trajectory optimization method according to claim 5, wherein: The augmented Lagrangian method used in the step of combining constraints (S320) further includes: The augmented form of the dynamics constraint subproblem including consistency constraints; An augmented form of the inverse kinematics subproblem that includes consistency constraints; and Augmented form of the joint constraint mapping subproblem including consistency constraints.

8. The motion trajectory optimization method according to claim 7, wherein: The iterative solution step (S340) further includes: The differential dynamic programming method is used to solve the dynamic constraint subproblem containing the consistency constraint, and the decision vector φ of the trajectory optimization problem is obtained. k+1 (S341); The gradient descent method is used to solve the inverse kinematics subproblem containing the consistency constraint, and the decision vector of the inverse kinematics subproblem is obtained. (S342); The gradient descent method is used to solve the joint constraint mapping subproblem containing the consistency constraint, and the decision variables of the joint constraint mapping subproblem are obtained. (S343); and Update the formal variables associated with the consistency constraints and (S344).

9. The motion trajectory optimization method according to claim 5, wherein: The terminal force-position hybrid control step (S400) further comprises the following steps: a control instruction calculation step, which calculates the manipulator joint force control instruction (S410) of the charging manipulator at time t according to the trajectory optimization decision variables solved in the problem solving step (S300); a control instruction input step, which inputs the robot arm joint torque control instruction calculated in the control instruction calculation step (S410) into the charging robot arm (S420); as well as The loop step returns to the initialization step (S330) at time t+1 until the task of plugging the charging port into the charging port is completed (S430).

10. A non-temporary storage medium storing a computer program, wherein the computer program, when executed by a processor, can implement the motion trajectory optimization method according to any one of claims 1 to 9.

Images