Hierarchical robot coordinated precision contact job control method and system

Abstract

The application discloses a layered robot coordinated precise contact operation control method and system, relates to the field of robot control, and comprises the following steps: according to a current system state, an upper model predictive controller is utilized to generate an operation target reference trajectory, a reference contact force and a time-varying feedback gain sequence in a rolling horizon optimization mode; the upper model predictive controller adopts an iterative linear quadratic Gaussian algorithm to optimize the trajectory and utilizes a rigid contact model to analyze the reference contact force; according to the operation target reference trajectory, the reference contact force and the time-varying feedback gain sequence, a bottom force-position coordination controller is utilized to track the contact force in a three-level priority mode; the bottom force-position coordination controller and the upper model predictive controller realize data interaction through an asynchronous spline interpolation interface; finally, a contact force PD feedback compensation term is utilized to compensate joint speed instructions. The application realizes precise force-position coordination control of a dual-arm robot on an operation target under the premise of guaranteeing real-time performance.

Description

Technical Field

[0001] This application relates to the field of robot control, and in particular to a hierarchical robot coordinated precision contact operation control method and system. Background Technology

[0002] During the operation of a dual-arm robot, the robot and the target form a kinematic closed chain, resulting in a strong coupling relationship between the contact force and the end effector position. Simultaneously, the frictional contact means that the end effectors can only apply positive pressure to the target and cannot generate tension, further introducing unidirectional drive constraints. The combined effect of these multiple constraints poses a significant challenge to the coordinated and precise contact control of the dual arms.

[0003] Current research on precise control of coordinated dual-arm operation mainly focuses on two types of methods: model predictive control and quadratic programming control. However, these methods generally suffer from the following shortcomings: (1) The Model Predictive Control (MPC) method avoids incorporating contact force into the system state (which would greatly increase the state dimension and severely reduce the solution rate). It usually adopts a simplified contact model, which makes it difficult to achieve high-precision contact force prediction while ensuring real-time computation.

[0004] (2) Although the force feedback control method based on quadratic programming (QP) has a very high control frequency, its reference trajectory and reference contact force usually depend on the upper-level planner. If the quality of the upper-level planner is insufficient, the control effect of the lower-level QP will be severely limited.

[0005] (3) In the existing two-layer control architecture, the reference trajectory handover between the upper MPC and the lower QP lacks a systematic time-varying feedback gain mechanism, resulting in insufficient robustness to external disturbances.

[0006] To address the aforementioned issues, there is an urgent need for a dual-arm robot contact operation control method that can ensure real-time performance while simultaneously achieving precise force-position coordination control and satisfying multiple constraints. Summary of the Invention

[0007] The purpose of this application is to provide a hierarchical robot coordinated precision contact operation control method and system, which can achieve precise force-position coordination control of a dual-arm robot on the target under the premise of ensuring real-time performance.

[0008] To achieve the above objectives, this application provides the following solution: In a first aspect, this application provides a hierarchical robot coordinated precision contact operation control method, including: Acquire the current system status and the measured values ​​of the contact forces at the ends of both arms; the current system status includes the target pose, target velocity, and joint angles and angular velocities of both arms. Based on the current system state, the upper-level model predictive controller generates the reference trajectory, reference contact force, and time-varying feedback gain sequence of the operation target using a rolling time-domain optimization method. The upper-level model predictive controller uses an iterative linear quadratic Gaussian algorithm for trajectory optimization and uses a rigid contact model to analyze the reference contact force. The rigid contact model is used to analyze and calculate the reference contact force based on the nominal trajectory generated by the dual-arm robot and the multi-body system model constrained by the operation target. Based on the operational target reference trajectory, the reference contact force, and the time-varying feedback gain sequence, the contact force is tracked using a three-level priority form by a low-level force-position coordination controller to obtain joint velocity commands; the low-level force-position coordination controller and the upper-level model prediction controller achieve data interaction through an asynchronous spline interpolation interface. The joint speed command is compensated using a contact force PD feedback compensation term based on the error between the reference contact force and the measured contact force at the ends of the two arms to obtain the final joint speed.

[0009] Secondly, this application provides a hierarchical robot coordinated precision contact operation control system, comprising: The acquisition module is used to acquire the current system status and the measured values ​​of the contact forces at the ends of the two arms; the current system status includes the target pose, the target velocity, the joint angles and joint angular velocities of the two arms; The upper-level control module is used to generate the reference trajectory, reference contact force, and time-varying feedback gain sequence of the operation target using the upper-level model predictive controller based on the current system state in a rolling time-domain optimization manner; the upper-level model predictive controller uses an iterative linear quadratic Gaussian algorithm for trajectory optimization and uses a rigid contact model to analyze the reference contact force; the rigid contact model is used to analyze and calculate the reference contact force based on the nominal trajectory generated by the dual-arm robot and the multi-body system model constrained by the operation target. The lower-level control module is used to perform contact force tracking in a three-level priority manner using the lower-level force-position coordination controller based on the operation target reference trajectory, the reference contact force, and the time-varying feedback gain sequence to obtain joint speed commands; the lower-level force-position coordination controller and the upper-level model prediction controller achieve data interaction through an asynchronous spline interpolation interface; The compensation feedback module is used to compensate the joint speed command based on the error between the reference contact force and the measured value of the contact force at the end of the two arms using the contact force PD feedback compensation term, so as to obtain the final joint speed.

[0010] According to the specific embodiments provided in this application, the following technical effects are disclosed: This application provides a hierarchical robot coordinated precision contact operation control method and system. In the upper-level model predictive controller, a rigid contact model is used to analyze the reference contact force, which reduces the computational dimension while maintaining the force prediction accuracy. The iterative linear quadratic Gaussian algorithm can achieve efficient trajectory optimization, meeting the requirements of the upper-level planning frequency for real-time coordinated control of the dual-arm robot. The lower-level force-position coordination controller and the upper-level model predictive controller realize data interaction through an asynchronous spline interpolation interface, further improving the system's robustness to state errors. Thus, while ensuring real-time performance, the system achieves precise force-position coordination control of the dual-arm robot on the operation target. Attached Figure Description

[0011] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0012] Figure 1 This is a flowchart of a hierarchical robot coordinated precision contact operation control method according to an embodiment of this application; Figure 2 A general framework diagram of a hierarchical robot coordinated precision contact operation control method provided in an embodiment of this application; Figure 3 This is a schematic diagram of the functional modules of a hierarchical robot coordinated precision contact operation control system provided in an embodiment of this application. Detailed Implementation

[0013] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0014] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0015] In one exemplary embodiment, such as Figure 1 As shown, a hierarchical robot coordinated precision contact operation control method is provided, including the following steps.

[0016] Step 101: Obtain the current system status and the contact force measurement values ​​at the ends of both arms. The current system status includes the target pose, target velocity, joint angles and angular velocities of both arms; the measurement values ​​from the six-dimensional force / torque sensors at the ends of the left and right arms are spliced ​​together to form a twelve-dimensional contact force measurement vector.

[0017] Step 102: Based on the current system state, the upper-level model predictive controller generates the operation target reference trajectory, reference contact force, and time-varying feedback gain sequence using a rolling time-domain optimization method; the upper-level model predictive controller uses an iterative linear quadratic Gaussian algorithm for trajectory optimization and uses a rigid contact model to analyze the reference contact force; the rigid contact model is used to analyze and calculate the reference contact force based on the nominal trajectory generated by the dual-arm robot and the operation target constrained multi-body system model.

[0018] Step 103: Based on the operation target reference trajectory, the reference contact force, and the time-varying feedback gain sequence, the contact force is tracked using a three-level priority form using a bottom-level force-position coordination controller to obtain joint speed commands; the bottom-level force-position coordination controller and the upper-level model prediction controller achieve data interaction through an asynchronous spline interpolation interface.

[0019] Step 104: Based on the error between the reference contact force and the measured contact force at the ends of both arms, the joint speed command is compensated using the contact force PD feedback compensation term to obtain the final joint speed.

[0020] In an exemplary embodiment, the upper-level model predictive controller optimizes the target trajectory tracking error and the operability of the two arms, with rigid contact acceleration constraints, friction cone linearization constraints, and collision avoidance constraints as explicit constraints. During the forward unfolding of the nominal trajectory, it applies joint position, joint velocity, and joint torque constraints and uses an iterative linear quadratic Gaussian algorithm to optimize the rolling trajectory.

[0021] In an exemplary embodiment, the optimal control problem solved in the upper-level model predictive controller is: .

[0022] in, To predict the sequence of biarm joint angular accelerations at discrete time points in the time domain, To predict the number of time-domain steps; and The first The predicted state and reference state of the operation target for each step; For the measurement of bi-arm operability, These are the weighting coefficients; The task space mass matrix of the constrained multibody system consisting of the operational target and the end effectors of the two arms. For the corresponding stacked generalized acceleration, The generalized contact force generated by rigid contact constraints. For unconstrained generalized forces such as control force, Coriolis force, and gravity; and Rigid contact acceleration constraints The constraint matrix and velocity-related bias terms, superscript Indicates matrix transpose; For the linearized matrix of the friction cone, The force vector is the force vector of the two arms in contact. To determine the Jacobian matrix of the collision avoidance distance for joint motion, This is the upper limit for collision avoidance calculated based on the current safe distance and control cycle. To constrain the dynamic equations of a multibody system, In order to be in Closed-form solution of rigid contact force obtained under the given conditions; For the linearization constraint of the friction cone, To avoid collisions, joint position, joint velocity, and joint torque are limited by model constraints and control limits during each MuJoCo forward deployment.

[0023] In an exemplary embodiment, based on the current system state, the upper-level model predictive controller generates the operational target reference trajectory, reference contact force, and time-varying feedback gain sequence using a rolling time-domain optimization method, specifically including: Obtain the current system state, use the current system state as the initial state, and use the nominal action sequence of the previous control cycle as the initial nominal action sequence of the current cycle to perform a hot start.

[0024] Using the initial nominal motion sequence after a hot start as input, the nominal state trajectory and nominal motion trajectory are obtained by forward expansion of the joint simulation model of the dual-arm robot and the target being operated. The discrete dynamic Jacobian matrix is ​​then calculated using finite difference based on the nominal state trajectory and nominal motion trajectory.

[0025] Based on the nominal state trajectory and nominal action trajectory as input, a Gaussian-Newton second-order approximation is applied to the risk-sensitive cost function, and the action improvement amount and time-varying feedback gain matrix at each time step are obtained through Riccati backward recursion using the discrete dynamics Jacobian matrix. The risk-sensitive cost function is obtained by risk-sensitive transformation of the basic cost function, which is composed of a weighted sum of task residual terms. The action improvement amount is used to apply feedforward correction to the nominal action sequence, and the time-varying feedback gain matrix is ​​used to apply feedback correction to the nominal action sequence.

[0026] The nominal policy is updated by performing a parallel forward search with multiple step sizes and combining it with adaptive regularization. The nominal policy includes a nominal state trajectory and a nominal action trajectory.

[0027] Extract the target reference trajectory and joint reference trajectory from the updated nominal state trajectory, calculate the reference contact force time-by-time using the rigid contact force closed solution, and output the time-varying feedback gain matrix sequence.

[0028] In an exemplary embodiment, the construction process of the asynchronous spline interpolation interface specifically includes: Obtain the current system state, current time, and upper-level strategy object; the upper-level strategy object includes nominal state trajectory, nominal joint torque sequence, operation target reference trajectory, joint reference trajectory, reference contact force, and time-varying feedback gain matrix sequence.

[0029] Based on the current time, zero-order hold, linear interpolation, or cubic Hermite spline interpolation are performed on the nominal state trajectory, nominal joint torque sequence, operational target reference trajectory, joint reference trajectory, reference contact force, and time-varying feedback gain matrix sequence, respectively. The generalized state deviation is calculated based on the nominal state obtained by interpolation and the current system state, and the nominal joint torque is corrected using the time-varying feedback gain matrix at the current time. The corrected nominal joint torque is limited to the actuator setting range, and the high-frequency operational target reference trajectory, joint reference trajectory, and reference contact force obtained by interpolation are used as the reference inputs of the underlying force-position coordination controller.

[0030] In one embodiment, the underlying force-position coordination controller uses hierarchical quadratic programming to solve the first priority, second priority, and third priority sub-problems sequentially. When solving the next priority sub-problem, the optimal constraint relaxation amount or optimal task value of the previous priority is fixed, thereby ensuring that low-priority tasks do not disrupt high-priority tasks.

[0031] The first priority is to transform the joint constraint, collision avoidance constraint, and contact friction cone constraint into a linear inequality with respect to the joint angular acceleration, and solve the highest priority feasibility subproblem with the goal of minimizing constraint relaxation, thereby obtaining the optimal feasible set of the first priority. .

[0032] The second priority is: when the joint angular acceleration belongs to the optimal feasible set of the first priority. Under these conditions, the target acceleration is calculated from the upper reference trajectory by operating the target trajectory tracking impedance controller. The second priority quadratic programming (QP) subproblem is solved with the objective of minimizing the L2 norm of the difference between the actual target acceleration and the target acceleration, thus obtaining the joint angular acceleration. Its core QP subproblem is: .

[0033] .

[0034] in, , These are the stiffness matrix and damping matrix of the spring-damped contact model, respectively. , These represent the end positions and velocities of the two arms, respectively. , These represent the position and velocity of the contact point of the target operation; To predict the sequence of biarm joint angular accelerations at discrete moments in the time domain; This is the constraint matrix obtained after transforming the joint constraint into a linear inequality with respect to joint angular acceleration; This is the corresponding upper bound vector for joint constraints; The task space mass matrix of the constrained multibody system consisting of the operational target and the end effectors of the two arms. For the corresponding stacked generalized acceleration, The generalized contact force generated by rigid contact constraints. For unconstrained generalized forces such as control forces, Coriolis forces, and gravity; the second priority quadratic programming subproblem inherits the optimal feasible set of the first priority subproblem. Joint constraints, collision avoidance, and friction cone conditions; target trajectory acceleration. Calculated from the upper reference trajectory using an impedance controller: .

[0035] in, , These are the stiffness and damping parameter matrices of the impedance controller, respectively. Use the target acceleration as a reference. The actual speed of the operating target. For the target operating speed, The actual pose of the target being operated on. The reference pose for the operation target.

[0036] The third priority is: maintaining the optimal feasible set of the first priority. Under the condition that the target acceleration value of the second priority optimal operation remains unchanged, the third priority QP subproblem is solved with the goal of minimizing the joint angle acceleration tracking error; through equations... Keep the second priority task unchanged, among which For the Jacobian matrix at the ends of both arms; To predict the sequence of biarm joint angular accelerations at discrete time points in the time domain, The first optimal joint angular acceleration obtained for the second priority subproblem. The time derivative of the Jacobian matrix at the ends of both arms; target joint acceleration. Seeking Then according to The joint velocity command is obtained by integration. The second optimal joint angular acceleration obtained for the third priority subproblem The joint speed command output by H-QP; For joint reference acceleration, In the middle, D q Here is the damping gain matrix for joint trajectory tracking, and q is the actual joint angular velocity of both arms. r For the joint reference angular velocity, In the middle, K q This is the stiffness gain matrix for joint trajectory tracking. The actual joint angular velocities of both arms at the current moment. This is the underlying control cycle.

[0037] In an exemplary embodiment, contact force tracking is performed using a low-level force-position coordination controller with three priority levels based on the operation target reference trajectory, the reference contact force, and the time-varying feedback gain sequence to obtain joint velocity commands, specifically including: Inputting the current joint state, the joint constraint, collision avoidance constraint, and contact friction cone constraint are transformed into linear inequalities with respect to joint angular acceleration. The highest priority feasibility problem is solved to obtain the first priority optimal feasible set. ; in the first priority optimal feasible set Within the system, using the reference trajectory of the target being manipulated as input, the target acceleration is calculated through a target trajectory tracking impedance controller. The second priority subproblem is then solved with the objective of minimizing the L2 norm of the difference between the actual acceleration of the target and the target acceleration, yielding the first optimal joint angular acceleration. Under the constraint of keeping the second priority optimal task value unchanged, the third priority sub-problem is solved with the objective of minimizing the tracking error between the target joint acceleration generated by the joint reference trajectory through the joint impedance controller and the actual joint angular acceleration, to obtain the second optimal joint angular acceleration. ;according to The joint velocity command is obtained by integration; wherein, the time-varying feedback gain sequence is used to perform state feedback correction on the nominal joint torque through the asynchronous spline interpolation interface; the reference contact force is used to subtract the contact force measurement value of the two arms end to form the contact force error, so as to generate the contact force PD feedback compensation term.

[0038] In one exemplary embodiment, the final joint velocity is: .

[0039] in, , These are the proportional gain matrix and the differential gain matrix of the contact force error, respectively; For the Jacobian matrix at the ends of both arms, For the Moore-Penrose generalized inverse of the Jacobian matrix at the ends of the two arms; The twelve-dimensional vector is formed by splicing the reference contact forces / torques of the left and right arms. A twelve-dimensional vector is formed by splicing together the measurement values ​​of the six-dimensional force / torque sensors at the ends of the left and right arms; The joint speed command output by H-QP. To superimpose the final joint velocity command after contact force feedback compensation, For time.

[0040] This application employs a two-layer control architecture consisting of four parts: upper-layer trajectory planning, asynchronous strategy transmission, lower-layer constraint coordination, and contact force feedback compensation. The upper layer uses a model predictive controller based on Iterative Linear Quadratic Gaussian (iLQG) to generate nominal state trajectories, nominal joint torque sequences, target reference trajectories, joint reference trajectories, reference contact forces, and time-varying feedback gain matrix sequences on a co-simulation model of the dual-arm robot and the multi-body system constrained by the target. An asynchronous interface interpolates the upper-layer results to 1000Hz and uses time-varying feedback gain to correct the nominal joint torques. The lower layer uses Hierarchical Quadratic Programming (H-QP) to sequentially satisfy safety constraints, track the target trajectory, and track the joint trajectory without violating high-priority constraints and the task, outputting joint velocity commands. Finally, contact force PD feedback compensation based on a six-dimensional force / torque sensor at the end of the dual arms is independently superimposed to obtain the final joint velocity commands.

[0041] This application constructs a constrained multibody system with the target object and a dual-arm robotic arm. It utilizes the closed-form solution of rigid contact acceleration constraints to quickly calculate the reference contact force along the upper nominal trajectory, maintaining force prediction efficiency without expanding the contact force into an independent state. The upper-layer iLQG achieves approximately 50Hz rolling optimization through parallel forward expansion, Riccati backward recursion, and parallel line search. The lower-layer H-QP operates at 1000Hz, solving for joint velocity commands sequentially according to safety constraints, target trajectory tracking, and joint trajectory tracking. The feedback from the six-dimensional force / torque sensor is superimposed only after the H-QP output through a PD compensation term to correct the uncertainty of the contact model parameters.

[0042] In another exemplary embodiment, such as Figure 2 As shown, this application adopts a two-layer hierarchical control architecture. The upper layer is a model predictive controller based on iLQG (operating frequency of about 50Hz), and the lower layer is a force-position coordination controller based on H-QP (operating frequency of 1000Hz). The two controllers interact with each other through an asynchronous spline interpolation interface.

[0043] Step 1: Construct a multi-body system model of the dual-arm robot and the target being manipulated.

[0044] Step 1 specifically includes: For the upper-level iLQG model predictive controller, a co-simulation model of the dual-arm robot and the manipulated target is constructed using the MuJoCo Modeling Format (MJCF). The constrained multibody system model is a mathematical description of the two arms, the manipulated target, and their rigid contact relationships. The MJCF co-simulation model is an executable implementation of this mathematical model in MuJoCo. The iLQG model predictive controller uses the discrete dynamics given by this co-simulation model. ( , The object is used as the forward unfolding object. The geometry, inertia, joint limits, and contact parameters of the left arm, right arm, and manipulated target are defined in the MJCF file. The generalized position vector is 21-dimensional, including the three-dimensional position of the box, the four-dimensional representation of the box's unit quaternion, and the 14-dimensional joint angles of the left and right arms; the generalized velocity vector is 20-dimensional, including the six-dimensional velocity of the box and the 14-dimensional joint angular velocities of the left and right arms. Therefore, the state vector stored in MuJoCo has a total of 41 dimensions. The generalized state difference used for feedback represents the quaternion attitude difference as a three-dimensional local rotation error, hence its dimension is 40. Control Input The 14-dimensional joint torques are represented for both arms. The iLQG planner performs parallel forward simulation and finite-difference Jacobian calculations via the MuJoCo Application Programming Interface (API), and calculates the reference contact force along the nominal trajectory using a rigid contact closed-form solution. The aforementioned 41-dimensional states belong to the upper-level simulation states, while the six-dimensional force / torque sensor measurements described later pertain to the contact force measurement of a single robotic arm end effector. These two dimensions have different uses and do not have a dimensional correspondence.

[0045] For the underlying H-QP force-position coordination controller, a Unified Robot Description Format (URDF) model of the dual-arm robot is loaded using the Pinocchio robot dynamics library. The mass matrix, Coriolis force vector, gravity vector, and end-effector Jacobian matrix are calculated in real time, providing dynamic data for various priority Quadratic Programming (QP) sub-problems. The kinematic and dynamic parameters of the manipulated target are calculated online using Euler-Newton equations, and the relative position vector of the contact point is updated in real time by the pose of the robotic arm's end effector to ensure the real-time performance and accuracy of the grasping matrix.

[0046] Step 2: Construct an upper-level model predictive controller based on the iLQG algorithm. Specifically, an upper-level model predictive controller is constructed based on the iLQG algorithm, generating the operational target reference trajectory, reference contact force, and time-varying feedback gain sequence using a rolling time-domain optimization method.

[0047] The task space dynamics of the operational target, the left arm end effector, and the right arm end effector are combined into a unified constrained multibody system dynamics. and with Describe the uniformity of acceleration at the rigid contact point; from these two, obtain the closed-form solution of the rigid contact force. At each iLQG forward expansion node, the co-simulation model is used to calculate the next state and cost residuals, and the rigid contact closed solution is used to calculate the reference contact force and verify the friction cone constraint, thus forming the constrained multibody system model, the co-simulation model, and the iLQG optimizer.

[0048] The upper-level optimal control problem aims to optimize the tracking error of the target trajectory and the operability metric of the two arms. It explicitly employs rigid contact acceleration constraints and their closed-form solution for contact force, friction cone linearization constraints, and collision avoidance constraints. Furthermore, it imposes constraints on joint position, joint velocity, and joint torque during the MuJoCo forward unfolding. Each iteration sequentially performs nominal trajectory forward unfolding and finite-difference linearization, the Gauss-Newton second-order approximation of the risk-sensitive cost function, Riccati backward recursion, parallel line search, and adaptive regularization. The upper layer operates at approximately 50Hz, outputting the target reference trajectory, joint reference trajectory, reference contact force, and a time-varying feedback gain matrix sequence composed of the feedback gain matrices at each time step.

[0049] like Figure 2 As shown, the upper-level model predictive controller operates within a rolling time-domain optimization framework, employing the iterative linear quadratic Gaussian (iLQG) algorithm to solve the following optimal control problem: .

[0050] in, and The first The predicted state and reference state of the operation target for each step. For the measurement of bi-arm operability, These are the weighting coefficients. To predict the number of time-domain steps; To constrain the dynamic equations of a multibody system, For rigid contact acceleration constraints, This is the closed-form solution of the rigid contact force obtained from the two. For the linearization constraint of the friction cone, To avoid collisions, joint position, joint velocity, and joint torque are limited each time MuJoCo is deployed forward.

[0051] Each iteration of the upper-layer iLQG controller includes the following sub-steps: (1) Forward expansion of nominal trajectory: from the current system state Starting from this point, the MuJoCo physics engine is used to perform a forward simulation of the current nominal policy to obtain the nominal state trajectory. and nominal action trajectory ;in For the first The nominal state at each discrete moment. This corresponds to the 14-dimensional nominal joint torque. The discrete dynamic Jacobian matrix is ​​calculated using finite difference. and .

[0052] .

[0053] in, The discrete state transition function given for the co-simulation model. For system status, This refers to joint torque movements; Let be the state Jacobian matrix, representing the local effect of the state perturbation on the state at the next time step; Let be the action Jacobian matrix, representing the local effect of joint torque disturbance on the state at the next moment.

[0054] (2) Second-order expansion of the cost function: The first-order gradient of the cost function with respect to the state and action is calculated using the Gauss-Newton approximation. , and the second-order Hessian matrix , , These quantities represent the first-order and second-order local sensitivities of the cost to the state and action, respectively, and serve as inputs to Riccati's backward recursion. The cost function takes a risk-sensitive form: 。

[0055] 。

[0056] in, The basic cost function represents The sum of the weighted norms of the residuals of each task; For the first The weights of each residual term, For the corresponding norm, For the residual vector, The number of residual terms; For risk transformation function, As a risk-sensitive parameter, Time limit Take a risk-neutral form; This is the stage cost function after risk transformation.

[0057] (3) Riccati backward propagation: Propagate from the terminal time to the initial time and calculate the gradient of the value function at each time. With Hessian matrix Action improvement amount and feedback gain matrix Each It is a matrix that maps 40-dimensional generalized state biases to 14-dimensional joint torque corrections, covering the entire prediction time domain. A time-varying feedback gain matrix sequence is constructed, and the following time-varying linear feedback strategy is generated.

[0058] 。

[0059] in, For the first The nominal joint torque at any given moment. In nominal state, This is the candidate forward unfolding state. For motion feedforward improvement, Let be the feedback gain matrix at that moment. Step length, The candidate joint torque is obtained by applying feedforward improvement and state feedback correction.

[0060] (4) Parallel line search: for multiple different time lengths Perform parallel forward expansion, evaluate the total reward of each candidate trajectory, and select the optimal trajectory to update the nominal policy.

[0061] (5) Adaptive regularization: The regularization coefficient in Riccati backpropagation is adaptively adjusted based on the ratio of the improvement amount to the expected improvement amount (surprise index) to ensure the numerical stability of backpropagation.

[0062] After selecting the optimal candidate through parallel line search, the optimal nominal state trajectory directly provides the reference position and velocity of the target, as well as the reference angles and angular velocities of the left and right arm joints; the reference acceleration is calculated from adjacent discrete states or model dynamics; at each nominal trajectory node, the state, acceleration, and unconstrained generalized force are substituted into the closed-form solution of the rigid contact force to obtain the reference contact force. Riccati's backward recursion directly provides the feedback gain matrix at each time step. Therefore, the upper-level iLQG controller outputs the target reference trajectory, joint reference trajectory, reference contact force, and time-varying feedback gain matrix sequence. .

[0063] Step 3: Construct an asynchronous interface between the upper and lower level controllers. Specifically, this involves constructing an asynchronous spline interpolation interface between the upper and lower level controllers to transmit the planning results from the upper level to the lower level in a high-frequency manner.

[0064] The underlying controller queries the upper-layer policy object at a frequency of 1000Hz, passing in the current system state. With current time Current system status This includes the target's 3D position, unit quaternion pose, 6D velocity, and 14D joint angles and 14D joint angular velocities of the left and right arms. The policy object performs cross-frequency interpolation on the upper-layer discrete output to obtain the nominal state, nominal 14D joint torque, target reference trajectory, joint reference trajectory, reference contact force, and feedback gain matrix at the current moment. The nominal reference action refers to the optimal nominal action sequence of the upper-layer iLQG. The 14-dimensional joint torques were obtained through interpolation. (Using...) A linear feedback correction is applied to the 40-dimensional generalized deviation of the current state relative to the nominal state, and the result is subjected to joint torque limiting. The interface finally outputs a high-frequency reference data packet, including the operation target reference trajectory, joint reference trajectory, reference contact force, feedback gain matrix, and the corrected joint torque after limiting. The underlying H-QP directly uses the first three types of reference quantities.

[0065] Step 3 specifically includes: The upper-level iLQG controller operates asynchronously at approximately 50Hz, while the lower-level H-QP controller operates at 1000Hz. The upper-level policy object stores discrete nominal state trajectories, nominal joint torque sequences, target reference trajectories, joint reference trajectories, reference contact forces, and feedback gain matrix sequences. Splines are only used by the lower-level controller to query these discrete results across frequencies and do not alter the iLQG's optimization parameterization of the direct action sequence. The current system state is passed in during the lower-level query. and current time The reference value at the current time is obtained by using zero-order preservation, linear interpolation, or cubic Hermite splines, and then... Calculate state feedback correction: 。

[0066] in, This is a spline interpolation function for cross-frequency lookup of the nominal joint moment sequence. For spline node time series, This corresponds to the nominal joint torque node value; Obtained by interpolation of the feedback gain matrix sequence; Defined from nominal state To the current state The 40-dimensional generalized state difference, i.e. The quaternion pose difference is represented by a three-dimensional local rotation vector. To correct and limit the 14-dimensional joint torques, the strategy object also synchronously outputs the interpolated operation target reference trajectory, joint reference trajectory, and reference contact force for direct use by the underlying H-QP.

[0067] Step 4: Construct a low-level force-potential coordination controller based on the H-QP framework. Specifically, a low-level force-potential coordination controller is constructed based on the hierarchical quadratic programming (H-QP) framework, which implements constraint satisfaction and trajectory tracking in a three-level priority manner.

[0068] Based on the target trajectory, joint reference trajectory, and reference contact force output from the upper-level controller and interpolated via an asynchronous interface, a hierarchical quadratic programming (H-QP) framework is used to solve for the joint angular acceleration and integrate it into the joint velocity command. The H-QP stage uses a spring-damped contact model to describe the contact relationship, but sensor PD compensation is not superimposed at this stage. The feedback from the six-dimensional force / torque sensors at the ends of the dual arms is superimposed as an independent compensation term after the H-QP output in step 5.

[0069] The underlying H-QP employs a three-level priority: first, it solves the minimum relaxation feasibility problem of joint constraints, collision avoidance, and contact friction cone constraints, and fixes their optimal relaxation values; second, it minimizes the target acceleration tracking error within the optimal feasible set of the first priority; finally, it minimizes the joint reference acceleration tracking error while keeping the optimal values ​​of the first two levels unchanged. The joint angular acceleration is then obtained. The joint speed command is obtained by integration, and the control frequency is 1000Hz.

[0070] The underlying H-QP controller operates at a control frequency of 1000Hz and employs a hierarchical priority quadratic programming framework, with the specific priority division as follows: Priority 1 (highest): Rewrite the joint constraint, collision avoidance constraint, and contact friction cone constraint as linear inequalities with respect to joint angular acceleration, introduce necessary non-negative relaxation variables, and minimize the relaxation amount to obtain the optimal feasible set of the first priority. Subsequent priorities must not increase this optimal relaxation amount.

[0071] Priority 2: In Under these conditions, the target acceleration is calculated from the upper reference trajectory by operating the target trajectory tracking impedance controller, and the QP subproblem is solved with the objective of minimizing the L2 norm of the difference between the actual acceleration of the operated target and the target acceleration, thus obtaining the joint angular acceleration. Its core QP subproblem is: in, The stiffness and damping matrix of the spring-damped contact model are respectively; the target trajectory acceleration is... Calculated from the upper reference trajectory using an impedance controller: .

[0072] in, , These are the stiffness and damping parameter matrices of the impedance controller, respectively.

[0073] Priority 3: In And satisfy Under these conditions, the QP subproblem is solved with the goal of minimizing the joint angular acceleration tracking error, thus maintaining the second-priority operational objective task; the target joint acceleration is determined by... Calculate and obtain Then according to The joint velocity command is obtained by integration.

[0074] Step 5: Implement contact force PD feedback compensation. Implement contact force PD feedback compensation, mapping the difference between the six-dimensional force sensor measurement value and the reference contact force into a joint speed correction value, which is then superimposed and sent to the robot arm's underlying controller.

[0075] Step 5 specifically includes: Considering the uncertainty of the contact model parameters, a contact force PD feedback compensation term is independently superimposed on the joint velocity command output in step 4. The measurement values ​​from the six-dimensional force / torque sensors at the ends of the left and right arms are spliced ​​together to form a twelve-dimensional contact force measurement vector. , and the corresponding twelve-dimensional upper-level reference contact force vector The difference, after passing through the Moore-Penrose generalized inverse mapping of the Jacobian matrix at the PD controller and the dual-arm end effectors, yields the additional joint velocity correction: .

[0076] in, , These are the proportional gain matrix and the differential gain matrix of the contact force error, respectively; For the Moore-Penrose generalized inverse of the Jacobian matrix at the ends of the two arms; The twelve-dimensional vector is formed by splicing the reference contact forces / torques of the left and right arms. A twelve-dimensional vector is formed by splicing together the measurement values ​​of the six-dimensional force / torque sensors at the ends of the left and right arms; The joint speed command output by H-QP. To superimpose the final joint velocity command after contact force feedback compensation, For time.

[0077] This application employs the iLQG algorithm to construct an upper-level model predictive controller operating at approximately 50Hz. A co-simulation model is used to perform forward expansion of the constrained multibody system dynamics, and the closed-form solution of rigid contact acceleration constraints is used to calculate the reference contact force time-by-time. Riccati backward recursion generates motion improvement quantities and a time-varying feedback gain matrix sequence. An asynchronous interface interpolates the target reference trajectory, joint reference trajectory, reference contact force, and feedback gain matrix sequence to 1000Hz, and the nominal joint torque is corrected by the feedback gain matrix. The lower-level H-QP solves for joint velocity commands according to safety constraints, target trajectory tracking, and joint trajectory tracking. Subsequently, the contact force errors of the six-dimensional force / torque sensors at the left and right arm ends are mapped to additional joint velocities through PD compensation, yielding the final joint velocity commands.

[0078] Based on the same inventive concept, this application also provides a hierarchical robot coordinated precision contact operation control system for implementing the hierarchical robot coordinated precision contact operation control method described above. The solution provided by this system is similar to the implementation scheme described in the above method; therefore, the specific limitations of one or more embodiments of the hierarchical robot coordinated precision contact operation control system provided below can be found in the limitations of the hierarchical robot coordinated precision contact operation control method described above, and will not be repeated here.

[0079] In one exemplary embodiment, such as Figure 3 As shown, a hierarchical robot coordinated precision contact operation control system is provided, comprising: The acquisition module is used to acquire the current system status and the measured values ​​of the contact forces at the ends of the two arms; the current system status includes the target pose, the target velocity, the joint angles and joint angular velocities of the two arms.

[0080] The upper-level control module is used to generate the reference trajectory, reference contact force, and time-varying feedback gain sequence of the operation target using the upper-level model predictive controller based on the current system state in a rolling time-domain optimization manner; the upper-level model predictive controller uses an iterative linear quadratic Gaussian algorithm for trajectory optimization and uses a rigid contact model to analyze the reference contact force; the rigid contact model is used to analyze and calculate the reference contact force based on the nominal trajectory generated by the dual-arm robot and the multi-body system model constrained by the operation target.

[0081] The lower-level control module is used to perform contact force tracking in a three-level priority manner using the lower-level force-position coordination controller based on the operation target reference trajectory, the reference contact force, and the time-varying feedback gain sequence to obtain joint speed commands; the lower-level force-position coordination controller and the upper-level model prediction controller realize data interaction through an asynchronous spline interpolation interface.

[0082] The compensation feedback module is used to compensate the joint speed command based on the error between the reference contact force and the measured value of the contact force at the end of the two arms using the contact force PD feedback compensation term, so as to obtain the final joint speed.

[0083] In an exemplary embodiment, the upper-level model predictive controller optimizes the target trajectory tracking error and the operability of the two arms, with rigid contact acceleration constraints, friction cone linearization constraints, and collision avoidance constraints as explicit constraints. During the forward unfolding of the nominal trajectory, it applies joint position, joint velocity, and joint torque constraints and uses an iterative linear quadratic Gaussian algorithm to optimize the rolling trajectory.

[0084] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0085] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A hierarchical robot coordinated precision contact operation control method, characterized in that, include: Acquire the current system status and the measured values ​​of the contact forces at the ends of both arms; the current system status includes the target pose, target velocity, and joint angles and angular velocities of both arms. Based on the current system state, the upper-level model predictive controller generates the operational target reference trajectory, reference contact force, and time-varying feedback gain sequence using a rolling time-domain optimization method; the upper-level model predictive controller uses an iterative linear quadratic Gaussian algorithm for trajectory optimization and uses a rigid contact model to analyze the reference contact force; The rigid contact model is used to analytically calculate the reference contact force based on the nominal trajectory generated by the multibody system model constrained by the dual-arm robot and the operation target. Based on the operational target reference trajectory, the reference contact force, and the time-varying feedback gain sequence, the contact force is tracked using a three-level priority form by a low-level force-position coordination controller to obtain joint velocity commands; the low-level force-position coordination controller and the upper-level model prediction controller achieve data interaction through an asynchronous spline interpolation interface. The joint speed command is compensated using a contact force PD feedback compensation term based on the error between the reference contact force and the measured contact force at the ends of the two arms to obtain the final joint speed.

2. The hierarchical robot coordinated precision contact operation control method according to claim 1, characterized in that, The upper-level model predictive controller optimizes the target trajectory tracking error and the operability of the two arms, with rigid contact acceleration constraints, friction cone linearization constraints, and collision avoidance constraints as explicit constraints. It also applies joint position, joint velocity, and joint torque constraints during the forward unfolding of the nominal trajectory and uses an iterative linear quadratic Gaussian algorithm to optimize the rolling trajectory.

3. The hierarchical robot coordinated precision contact operation control method according to claim 1, characterized in that, The optimal control problem solved in the upper-level model predictive controller is: ； in, To predict the sequence of biarm joint angular accelerations at discrete time points in the time domain, To predict the number of time-domain steps; and The first The predicted state and reference state of the operation target for each step; For the measurement of bi-arm operability, These are the weighting coefficients; The task space mass matrix of the constrained multibody system consisting of the operational target and the end effectors of the two arms. For the corresponding stacked generalized acceleration, The generalized contact force generated by rigid contact constraints. For unconstrained generalized forces such as control force, Coriolis force, and gravity; and Rigid contact acceleration constraints The constraint matrix and velocity-related bias terms, superscript Indicates matrix transpose; For the linearized matrix of the friction cone, The force vector is the force vector of the two arms in contact. To determine the Jacobian matrix of the collision avoidance distance for joint motion, The upper limit for collision avoidance is calculated based on the current safe distance and control cycle; To constrain the dynamic equations of a multibody system, In order to be in Closed-form solution of rigid contact force obtained under the given conditions; For the linearization constraint of the friction cone, To avoid collisions, joint position, joint velocity, and joint torque are limited by model constraints and control limits during each MuJoCo forward deployment.

4. The hierarchical robot coordinated precision contact operation control method according to claim 1, characterized in that, Based on the current system state, the upper-level model predicts the controller and generates the operational target reference trajectory, reference contact force, and time-varying feedback gain sequence using a rolling time-domain optimization method. Specifically, this includes: Obtain the current system state, use the current system state as the initial state, and use the nominal action sequence of the previous control cycle as the initial nominal action sequence of the current cycle to perform a hot start; Using the initial nominal action sequence after hot start as input, the nominal state trajectory and nominal action trajectory are obtained by forward expansion of the joint simulation model of the dual-arm robot and the target being operated. The discrete dynamic Jacobian matrix is ​​then calculated using finite difference based on the nominal state trajectory and nominal action trajectory. Based on the nominal state trajectory and nominal action trajectory as input, a Gaussian-Newton second-order approximation is applied to the risk-sensitive cost function, and the action improvement amount and time-varying feedback gain matrix at each time step are obtained through Riccati backward recursion using the discrete dynamics Jacobian matrix. The risk-sensitive cost function is obtained by risk-sensitive transformation of the basic cost function, which is composed of a weighted sum of task residual terms. The action improvement amount is used to apply feedforward correction to the nominal action sequence, and the time-varying feedback gain matrix is ​​used to apply feedback correction to the nominal action sequence. The nominal policy is updated by performing a parallel forward search with multiple step sizes and combining adaptive regularization. The nominal policy includes a nominal state trajectory and a nominal action trajectory. Extract the target reference trajectory and joint reference trajectory from the updated nominal state trajectory, calculate the reference contact force time-by-time using the rigid contact force closed solution, and output the time-varying feedback gain matrix sequence.

5. The hierarchical robot coordinated precision contact operation control method according to claim 1, characterized in that, The construction process of the asynchronous spline interpolation interface specifically includes: Obtain the current system state, current time, and upper-level strategy object; the upper-level strategy object includes nominal state trajectory, nominal joint torque sequence, operation target reference trajectory, joint reference trajectory, reference contact force, and time-varying feedback gain matrix sequence; Based on the current time, zero-order hold, linear interpolation, or cubic Hermite spline interpolation are performed on the nominal state trajectory, nominal joint torque sequence, operational target reference trajectory, joint reference trajectory, reference contact force, and time-varying feedback gain matrix sequence, respectively. The generalized state deviation is calculated based on the nominal state obtained by interpolation and the current system state, and the nominal joint torque is corrected using the time-varying feedback gain matrix at the current moment. The modified nominal joint torque is limited within the actuator setting range, while the interpolated operation target reference trajectory, joint reference trajectory, and reference contact force are used as reference inputs for the underlying force-position coordination controller.

6. The hierarchical robot coordinated precision contact operation control method according to claim 1, characterized in that, The underlying force-position coordination controller uses hierarchical quadratic programming to solve the first priority, second priority, and third priority subproblems in sequence; when solving the next priority subproblem, the optimal constraint relaxation amount or optimal task value of the previous priority is fixed. The first priority is to transform the joint constraint, collision avoidance constraint, and contact friction cone constraint into a linear inequality with respect to the joint angular acceleration, and solve the highest priority feasibility subproblem with the goal of minimizing constraint relaxation, thereby obtaining the optimal feasible set of the first priority. ; The second priority is: when the joint angular acceleration belongs to the optimal feasible set of the first priority. Under these conditions, the target acceleration is calculated from the upper reference trajectory by operating the target trajectory tracking impedance controller. The second priority quadratic programming subproblem is then solved with the objective of minimizing the L2 norm of the difference between the actual target acceleration and the target acceleration, thus obtaining the joint angular acceleration. The second priority quadratic programming subproblem is: ； ； in, , These are the stiffness matrix and damping matrix of the spring-damped contact model, respectively. , These represent the end positions and velocities of the two arms, respectively. , These represent the position and velocity of the contact point of the target operation; To predict the sequence of biarm joint angular accelerations at discrete moments in the time domain; This is the constraint matrix obtained after transforming the joint constraint into a linear inequality with respect to joint angular acceleration; This is the corresponding upper bound vector for joint constraints; The task space mass matrix of the constrained multibody system consisting of the operational target and the end effectors of the two arms. For the corresponding stacked generalized acceleration, The generalized contact force generated by rigid contact constraints. For unconstrained generalized forces such as control forces, Coriolis forces, and gravity; the second priority quadratic programming subproblem inherits the optimal feasible set of the first priority subproblem. Joint constraints, collision avoidance, and friction cone conditions; target trajectory acceleration. Calculated from the upper reference trajectory using an impedance controller: ； in, , These are the stiffness and damping parameter matrices of the impedance controller, respectively. Use the target acceleration as a reference. The actual speed of the operating target. For the target operating speed, The actual pose of the target being operated on. The target position is used as a reference pose. The third priority is: maintaining the optimal feasible set of the first priority. Under the condition that the target acceleration value of the second priority optimal operation remains unchanged, the third priority quadratic programming subproblem is solved with the goal of minimizing the joint angle acceleration tracking error; through Keep the second priority task unchanged, among which For the Jacobian matrix at the ends of both arms, To predict the sequence of biarm joint angular accelerations at discrete time points in the time domain, The first optimal joint angular acceleration obtained for the second priority subproblem. The time derivative of the Jacobian matrix at the ends of both arms; target joint acceleration. Seeking Then according to The joint velocity command is obtained through integration. The second optimal joint angular acceleration obtained for the third priority subproblem. The joint speed command output by H-QP; For joint reference acceleration, In the middle, D q Here is the damping gain matrix for joint trajectory tracking, and q is the actual joint angular velocity of both arms. r For the joint reference angular velocity, In the middle, K q This is the stiffness gain matrix for joint trajectory tracking. The actual joint angular velocities of both arms at the current moment. This is the underlying control cycle.

7. The hierarchical robot coordinated precision contact operation control method according to claim 1, characterized in that, Based on the operational target reference trajectory, the reference contact force, and the time-varying feedback gain sequence, the underlying force-position coordination controller performs contact force tracking in a three-level priority manner to obtain joint velocity commands, specifically including: Inputting the current joint state, the joint constraint, collision avoidance constraint, and contact friction cone constraint are transformed into linear inequalities with respect to joint angular acceleration. The highest priority feasibility problem is solved to obtain the first priority optimal feasible set. ; in the first priority optimal feasible set Within the system, using the reference trajectory of the target being manipulated as input, the target acceleration is calculated through a target trajectory tracking impedance controller. The second priority subproblem is then solved with the objective of minimizing the L2 norm of the difference between the actual acceleration of the target and the target acceleration, yielding the first optimal joint angular acceleration. Under the constraint of keeping the second priority optimal task value unchanged, the third priority sub-problem is solved with the objective of minimizing the tracking error between the target joint acceleration generated by the joint reference trajectory through the joint impedance controller and the actual joint angular acceleration, to obtain the second optimal joint angular acceleration. ;according to The joint velocity command is obtained by integration; wherein, the time-varying feedback gain sequence is used to perform state feedback correction on the nominal joint torque through the asynchronous spline interpolation interface; the reference contact force is used to subtract the contact force measurement value of the two arms end to form the contact force error, so as to generate the contact force PD feedback compensation term.

8. The hierarchical robot coordinated precision contact operation control method according to claim 1, characterized in that, The final joint velocity is: ； in, , These are the proportional gain matrix and the differential gain matrix of the contact force error, respectively; For the Jacobian matrix at the ends of both arms; For the Moore-Penrose generalized inverse of the Jacobian matrix at the ends of the two arms; The twelve-dimensional vector is formed by splicing the reference contact forces / torques of the left and right arms. A twelve-dimensional vector is formed by splicing together the measurement values ​​of the six-dimensional force / torque sensors at the ends of the left and right arms; The joint speed command output by H-QP To superimpose the final joint velocity command after contact force feedback compensation, For time.

9. A hierarchical robot coordinated precision contact operation control system, characterized in that, include: The acquisition module is used to acquire the current system status and the measured values ​​of the contact forces at the ends of the two arms; the current system status includes the target pose, the target velocity, the joint angles and joint angular velocities of the two arms; The upper-level control module is used to generate the operation target reference trajectory, reference contact force, and time-varying feedback gain sequence using the upper-level model predictive controller based on the current system state in a rolling time-domain optimization manner; the upper-level model predictive controller uses an iterative linear quadratic Gaussian algorithm for trajectory optimization and uses a rigid contact model to analyze the reference contact force; The rigid contact model is used to analytically calculate the reference contact force based on the nominal trajectory generated by the multibody system model constrained by the dual-arm robot and the operation target. The lower-level control module is used to perform contact force tracking in a three-level priority manner using the lower-level force-position coordination controller based on the operation target reference trajectory, the reference contact force, and the time-varying feedback gain sequence to obtain joint speed commands; the lower-level force-position coordination controller and the upper-level model prediction controller achieve data interaction through an asynchronous spline interpolation interface; The compensation feedback module is used to compensate the joint speed command based on the error between the reference contact force and the measured value of the contact force at the end of the two arms using the contact force PD feedback compensation term, so as to obtain the final joint speed.

10. The hierarchical robot coordinated precision contact operation control system according to claim 9, characterized in that, The upper-level model predictive controller optimizes the target trajectory tracking error and the operability of the two arms, with rigid contact acceleration constraints, friction cone linearization constraints, and collision avoidance constraints as explicit constraints. It also applies joint position, joint velocity, and joint torque constraints during the forward unfolding of the nominal trajectory and uses an iterative linear quadratic Gaussian algorithm to optimize the rolling trajectory.

Images