Motion trajectory optimization method and storage medium for robotic arm

Abstract

The present invention provides a motion trajectory optimization method and storage medium for a robotic arm, the method comprising the following steps: establishing a charging robotic arm model for force control and an interactive dynamics model between a charging plug at the end of the charging robotic arm and a charging socket of a vehicle; designing optimization objectives and constraints for the force-position trajectory of the charging plug at the end of the charging robotic arm to obtain a force-position trajectory optimization problem for the charging robotic arm; decoupling the force-position trajectory optimization problem obtained in the problem description step into an independent force-position trajectory tracking problem and a robotic arm trajectory planning problem with convex constraints, and solving them using an alternating direction multiplier method; designing a force-position hybrid model predictive controller for the end of the charging robotic arm, and combining the current system state to obtain a joint torque control instruction of the robotic arm at the current moment, so that each joint driver of the charging robotic arm performs 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 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 achieving safe and stable interaction is crucial. 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 demonstrated 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 and end-effector path, a common yet underexplored requirement in safety-critical tasks. Furthermore, since complex contact environments, such as those for autonomous vehicle charging, are susceptible to disturbances and difficult to model or predict, an online optimization problem with updated contact state information is required. However, efficiently solving highly constrained manipulator and contact dynamics remains a research area. Summary of the Invention

[0004] The purpose of the present invention is to address the shortcomings of the existing technology and provide a motion trajectory optimization method for a robotic arm. This 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 robotic arm is provided, the method comprising the following steps: a model construction step, which establishes a charging robotic arm model suitable for force control and an interactive dynamics model between a charging plug at the end of the charging robotic arm and a charging socket of a vehicle; a problem description step, which designs the optimization objectives and constraints of the force trajectory of the charging plug at the end of the charging robotic arm based on the charging robotic arm model and the interactive dynamics model, and obtains the force trajectory optimization problem of the charging robotic arm based on the optimization objectives and constraints; a problem solving step, which solves the force trajectory optimization problem obtained in the problem description step. The invention relates to a method for coupling an independent force-position trajectory tracking problem and a robot 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 the end force-position hybrid model prediction 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 manipulator, represents the joint angular acceleration vector of the charging manipulator, τ u represents the joint torque vector of the charging manipulator; M(q) represents the inertia matrix of the charging manipulator, 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 interaction 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:

[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, and 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 Indicates the expected end position trajectory tracking accuracy index.

[0014] As a preferred solution, in the motion trajectory optimization method according to the present invention, in the problem description step (S200), the constraints of the force and position 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 mechanical constraint of the robot arm joint, and a contact constraint between the charging robot arm and the charging socket. In addition, 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 manipulator at time i, x[i] is the state vector of the charging manipulator 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 and upper limits of the joint space position 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, x0 represents the initial state of the charging robot arm, is the normal mapping vector of the end force of the charging manipulator, 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 solution, 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 the differential dynamic programming method to solve the dynamic constraint subproblem containing the consistency constraint to obtain the decision vector φk+1 of the trajectory optimization problem; using the gradient descent method to solve the inverse kinematics subproblem containing the consistency constraint to obtain the 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 manipulator joint torque control instruction of the charging manipulator at time t based on the trajectory optimization decision variables solved in the problem solving step; a control instruction input step, which inputs the manipulator joint torque control instruction calculated in the control instruction calculation step into the charging manipulator; 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, a non-transitory storage medium is provided, which stores a computer program. When the computer program is executed by a processor, it 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 as follows: 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 integrates 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 The flowchart of the 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 robotic 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, unless otherwise specifically stated, the relative arrangement of components, numerical expressions and numerical values ​​described in these embodiments do not limit the scope of the present invention.

[0029] The motion trajectory optimization method for a robotic arm of the present invention can be implemented by a processor in the robotic arm or a processor in a robot equipped with / mounted on the robotic arm executing a computer program stored in a memory in the robotic arm or the robot. Alternatively, the method can be implemented by the robotic arm or the robot equipped with / mounted on the robotic arm communicating with a server, with the processor in the server executing a computer program stored on the server or the cloud and feeding back the program execution results to the robotic 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 remove 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 non-charging type robot arms.

[0031] First, refer to Figure 1 The motion trajectory optimization method for a robotic arm according to the present invention is described, and the method includes the following steps S100 to S400:

[0032] In the model building step S100 , a model of the charging robot arm system dynamics 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, a charging manipulator model suitable for force control is established by integrating the manipulator dynamics and kinematics. The charging manipulator 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 manipulator, represents the joint angular acceleration vector of the charging manipulator, τ u represents the joint torque vector of the charging manipulator, M(q) represents the inertia matrix of the charging manipulator, represents the Coriolis force matrix of the charging manipulator, G(q) represents the gravity matrix of the charging manipulator, 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, F z represents the normal force applied by the charging manipulator on the contact surface, d represents the maximum static deformation between the charging manipulator 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 exerted 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, we proceed to step S200 of problem description. In step S200, we describe the optimization problem of the force-position hybrid trajectory of the charging manipulator. Specifically, based on the charging manipulator model and the interactive dynamics model, we first design the optimization objectives and constraints of the force-position trajectory of the charging plug at the end of the charging manipulator. The optimization objective is expressed as the following equation (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 Indicates the expected end position trajectory tracking accuracy index.

[0048] As a preferred solution, the constraint conditions of the force-position 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 as follows (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 manipulator at time i, x[i] is the state vector of the charging manipulator 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, 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, we proceed to problem-solving step S300 , where we solve the force-position hybrid trajectory optimization problem. Specifically, we decouple the force-position trajectory optimization problem in problem description step S200 into an independent force-position trajectory tracking problem and a robot trajectory planning problem with convex constraints, and solve them using the alternating direction multiplier method.

[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 joint 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 manipulator joint and the contact constraint between the charging manipulator 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 subproblem 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 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] Where, ρ 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.

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

[0072]

[0073] Where, v ik are formal variables related to 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 current state of the charging robot at time t, when the iteration number k=0, the decision vector φ of the dynamic constraint sub-problem is initialized to 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 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, the process proceeds to output step S350. In step S350, the raw residual for the kth iteration is calculated. Then, in step S360, it is determined whether the calculated raw residual for the kth iteration meets the iteration termination condition. If so, the process proceeds to step S370, where the decision vector obtained in steps S341 to S343 is output. Otherwise, the process returns to step S340.

[0084] The original residual is expressed as follows:

[0085]

[0086] The iteration termination condition is expressed as follows:

[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-point jack force-position hybrid control step S400 is performed. Specifically, based on the optimization problem-solving method used in the problem-solving step S300, a force-position hybrid model predictive controller for the end-point jack of the charging manipulator is designed. Combined with the current system state, the joint torque control instructions for the manipulator at the current moment are obtained. The joint drivers of the charging manipulator then execute the obtained joint torque control instructions to implement end-point force-position hybrid control. The system state can be represented by parameters such as the manipulator joint angles, joint angular velocities, and / or joint torques.

[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 manipulator joint torque control command of the charging manipulator at time t is calculated based on the trajectory optimization decision variables solved in problem solving step S300. The manipulator joint torque control command of the charging manipulator 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 step S420, the robot arm joint torque control instruction calculated in step S410 is input into the charging robot arm. Then, the process proceeds to step S430. At time t+1, the process returns to step S330 until the charging interface gun insertion 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 integrates 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 a computer of a system or device that reads and executes computer-executable instructions (e.g., one or more programs) recorded on a storage medium (also more fully referred to as a "non-transitory computer-readable storage medium") to perform one or more functions of the above-described embodiments, and / or includes one or more circuits (e.g., application-specific integrated circuits (ASICs)) for performing one or more functions of the above-described embodiments. Furthermore, the embodiments of the present invention may be implemented using a method in which the computer of the system or device, for example, reads and executes the computer-executable instructions from the storage medium to perform one or more functions of the above-described embodiments, and / or controls the one or more circuits to perform one or more functions of the above-described 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 disc (such as a compact disc (CD), a digital versatile disc (DVD), or a Blu-ray disc (BD) TM ), one or more of a flash memory device and a memory card, etc.

[0098] Although the present invention has been described above with reference to exemplary embodiments, the above embodiments are intended only to illustrate the technical concepts and features of the present invention and are not intended to limit the scope of protection of the present invention. Any equivalent variations or modifications made based on the spirit and essence of the present invention shall be included within the scope of protection 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 the charging plug at the end of the charging robot arm and the vehicle charging socket; Problem description step: Based on the charging manipulator model and the interactive dynamics model, the optimization objectives and constraints of the force-position trajectory of the charging plug at the end of the charging manipulator are designed, and based on the optimization objectives and constraints, the force-position trajectory optimization problem of the charging manipulator is obtained; 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; The end-plug force-position hybrid control step is based on the alternating direction multiplier method used in the problem-solving step to design the end-plug force-position hybrid model predictive controller of the charging robot arm, and combined with the current system state to obtain the joint torque control instructions of the robot arm at the current moment, so that the joint drivers of the charging robot arm perform the end-plug force-position hybrid control according to the obtained joint torque control instructions. The system state is represented by at least one of the robot arm joint angle, joint angular velocity, and joint torque. Furthermore, in the problem description step, the optimization objective 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 Indicates the expected end position trajectory tracking accuracy index.

2. The motion trajectory optimization method according to claim 1, wherein: In the model building step, 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 manipulator, represents the joint angular acceleration vector of the charging manipulator, τ u represents the joint torque vector of the charging manipulator; M(q) represents the inertia matrix of the charging manipulator, represents the Coriolis force matrix of the charging manipulator, G(q) represents the gravity matrix of the charging manipulator, and f e represents the end force vector of the manipulator, and J(q) represents the Jacobian matrix of the contact point at the end of the charging manipulator; 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, and 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, the constraint conditions of the force-position 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. Furthermore, 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 manipulator at time i, x[i] is the state vector of the charging manipulator 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, 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.

4. The motion trajectory optimization method according to claim 1, wherein: The problem-solving step further comprises the following steps: The problem decoupling step 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, thereby transforming it into a consistent distributed constraint optimization problem. In a constraint combining step, the augmented Lagrangian method is used 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; as well as The output step calculates the original residual of the kth iteration and outputs the decision vector when the calculated original residual meets the iteration termination condition.

5. The motion trajectory optimization method according to claim 4, wherein: In the problem decoupling step, the three parallel sub-problems are: Dynamic constraint subproblem; Inverse kinematics student problem; and Joint constraint mapping subproblem.

6. The motion trajectory optimization method according to claim 4, wherein: The augmented Lagrangian method used in the step of combining constraints further includes: The augmented form of the dynamic 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.

7. The motion trajectory optimization method according to claim 6, wherein: The iterative solution step further comprises: 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 ; The gradient descent method is used to solve the inverse kinematics subproblem containing the consistency constraint and obtain the decision vector 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. as well as Update the formal variables associated with the consistency constraints and 8. The motion trajectory optimization method according to claim 4, wherein: The terminal force-position hybrid control step further comprises the following steps: a control instruction calculation step, which calculates the manipulator joint force control instruction of the charging manipulator at time t based on the trajectory optimization decision variables solved in the problem solving step; a control instruction input step of inputting the manipulator arm joint torque control instruction calculated in the control instruction calculation step into the charging manipulator arm; as well as The loop steps return to the initialization step at time t+1 until the charging port plugging task is completed.

9. A non-transitory 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 8.

Images