A method and system for closed-chain multi-contact robot's internal load suppression optimization control
Patent Information
- Application Number
- CN202611067450.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-07-17
- Publication Date
- 2026-09-25
AI Technical Summary
[0005]本发明旨在提供一种用于闭链多接触机器人的内载荷抑制优化控制方法及系统，以解决现有技术中多接触闭链支撑切换引起的六维力/力矩分配跳变、切换振荡以及内载荷增长问题，并在不依赖关节力矩直接控制的情况下实现稳定的六维力/力矩调节
[0061](1)通过抓取映射零空间惩罚，抑制闭链内载荷增长，降低对抗性输出与执行器负担；
Smart Images
Figure CN122807893A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot control technology, and in particular to a control method, device, robot, and storage medium for a closed-chain multi-contact robot to achieve end-effector force / torque adjustment and suppress load growth within the closed chain and improve support switching stability under position servo joint conditions. Background Technology
[0002] In scenarios such as climbing, on-orbit assembly, multi-arm collaborative support, and handling, closed-chain multi-contact robots often require multiple end effectors to simultaneously form rigid or near-rigid connections with the environment. Due to contact redundancy, while satisfying the task-equivalent net six-dimensional force / torque, multiple contact six-dimensional forces / torques are also allowed to have zero-space components that do not change the net six-dimensional force / torque. These components will circulate within the closed chain and form internal loads, leading to increased structural stress, antagonistic output of actuators, and increased energy consumption. In severe cases, this can cause overcurrent or mechanical damage.
[0003] On the other hand, multi-contact motion is often accompanied by support switching, which leads to constraint reconstruction, thereby causing abrupt changes in the feasible six-dimensional force / torque set and the zero-space structure of the grasping mapping. Even if the net six-dimensional force / torque commands of the task are continuous, the six-dimensional force / torque allocated to each contact end may jump, thereby inducing vibration of the lightly damped structure and amplifying the internal load peaks.
[0004] Furthermore, many engineering robots employ high-reduction-ratio position servo joints, whose joint torques are difficult to control directly with high bandwidth, making traditional whole-body force control methods relying on joint torque interfaces unsuitable for application. Using only impedance / admittance shaping also struggles to simultaneously address the coupling problem of abrupt changes in the feasible domain of support switching and the zero-space accumulation of internal loads. Therefore, a closed-loop, multi-contact, six-dimensional force / torque control scheme is needed that can be implemented under position servo conditions while simultaneously considering contact feasibility, internal load suppression, and stable switching execution. Summary of the Invention
[0005] The present invention aims to provide an internal load suppression optimization control method and system for closed-chain multi-contact robots, in order to solve the problems of six-dimensional force / torque distribution jump, switching oscillation and internal load growth caused by multi-contact closed-chain support switching in the prior art, and to achieve stable six-dimensional force / torque adjustment without relying on direct joint torque control.
[0006] An internal load suppression optimization control method for a closed-loop multi-contact robot includes the following steps:
[0007] S1, for the working process of the closed-chain multi-contact robot, acquire the robot's motion state information, the six-dimensional force / torque of each support end and the support contact set information;
[0008] The robot's motion state information includes the expected and actual poses and velocities of the robot's body and each branch;
[0009] The support contact set information includes at least: the set of support end numbers currently in the support contact state, the number of support contacts, the contact object identifier corresponding to each support end, the contact state of each support end, the contact geometry and coordinate transformation information corresponding to each contact, and the information on the addition, deletion and change of the support contact set at adjacent time points; wherein, the contact state is used to characterize whether the support is established, maintained or released.
[0010] S2, using the body pose error feedback and equivalent dynamic model to generate the expected task equivalent net six-dimensional force / torque command;
[0011] S3, under the contact feasibility constraint, is distributed to the expected six-dimensional force / moment at each support end through quadratic programming with a zero-space penalty term for grasping mapping, thereby suppressing the load within the closed chain from the source.
[0012] The crawling mapping relationship is as follows: in This is a stacked vector of six-dimensional forces / torques at each support end. The grasping mapping matrix between the net six-dimensional force / moment and the six-dimensional force / moment at each support end is determined by the contact geometry and coordinate transformation; This represents the actual amount of net six-dimensional force / torque acting on the robot's body;
[0013] Quadratic programming is based on satisfying the set of contact feasible constraints. Under the premise of this, what is the expected six-dimensional force / torque at each support end? The optimization problem is to construct the following objective function:
[0014]
[0015] The first term is the net six-dimensional force / torque tracking term, which makes... Tracking the expected task equivalent net six-dimensional force / torque command The second term is the internal load suppression term, used to penalize... exist The component in the null space direction, To capture the mapping matrix The zero-space basis satisfies ; This is the net six-dimensional force / torque tracking weight matrix. This is the internal load suppression weight matrix.
[0016] The set of contact feasible constraints It includes at least: upper and lower limit constraints of the six-dimensional force / moment components at the end of each support, and minimum clamping force / preload constraints in the support direction;
[0017] Contact Feasible Constraint Set It also includes: a linear inequality constraint determined by the structural strength and position servo capability of the gripper, and the minimum clamping force / preload constraint in the support direction is satisfied by the force components at each support end along the normal direction of the contact alignment coordinate system. .
[0018] Finally, the solution is obtained The result is obtained by splitting the support end. .
[0019] S4. Determine whether the robot has switched supports. If so, determine the transition window corresponding to the support switching event and execute S5. Otherwise, the robot is in a steady state. Perform steady-state conventional filtering and error construction on the expected six-dimensional force / torque and execute S6.
[0020] The criteria for determining whether a support switching event has occurred in the robot are: a support switching event is determined to have occurred when a change in the support contact set information is detected.
[0021] The error construction specifically involves filtering and aligning the measured six-dimensional force / torque values with a coordinate system to obtain the six-dimensional force / torque error. The coordinate system alignment is achieved by mapping the measured six-dimensional force / torque values to an end-aligned coordinate system consistent with the desired six-dimensional force / torque reference through coordinate transformation before filtering.
[0022] S5. When the robot switches supports, the six-dimensional force / torque at each support end after the second planning is bandwidth-limited and progressively shaped in the transition window to obtain the expected six-dimensional force / torque reference at each support end.
[0023] The gradual shaping process includes: applying second-order filtering or bandwidth-limited filtering to the desired six-dimensional force / torque within the transition window, and introducing gradual-in / gradual-out coefficients to the six-dimensional force / torque reference. This ensures that the six-dimensional force / torque feedback gain and / or reference amplitude change smoothly during the support establishment and disengagement phases, and The desired six-dimensional force / torque reference at each support end is obtained. .
[0024] At the same time, the measured six-dimensional force / torque at each support end will be... Mapped to coordinates After aligning the coordinate system with consistent end points, filtering is performed to obtain filtered measurements. And construct a six-dimensional force / torque error
[0025] S6 employs a variable impedance admittance law to perform six-dimensional force / torque adjustment at each support end.
[0026] For each support end Based on six-dimensional force / torque error End-effector pose error With end velocity error Construct a variable impedance admittance law to generate the desired generalized acceleration at each support end. Or equivalent micro-displacement / micro-velocity commands.
[0027] The variable impedance admittance law is: in For virtual inertia, For six-dimensional force / torque feedback gain, and These are the damping and stiffness matrices, respectively. This is the disturbance estimator.
[0028] The damping and stiffness are in the form of variable impedance.
[0029]
[0030] And on , Apply projection / limiting to ensure that the parameters are bounded.
[0031] S7 uses an online disturbance estimator to estimate the combined disturbance caused by closed-chain coupling and non-ideal contact, and compensates it to a variable impedance admittance control law.
[0032] The online estimator uses a radial basis function network (RBFNN) in its form:
[0033]
[0034] Where input This includes quantities related to branch joint status, end-effector measurements, and errors. These are the basis function vectors. Weights Adaptive updates with leakage terms are used to suppress weight drift:
[0035]
[0036] in For measurable residuals, The learning rate matrix, The leakage coefficient is denoted as .
[0037] S8, based on the admittance port power criterion, injects damping into the admittance output to make the port output power non-positive, thereby suppressing switching-induced oscillation;
[0038] Assume the admittance port output without damping is Port power is defined as
[0039] When detected At that time, along Directional injection damping, resulting in Until the port output power is not positive; where It is numerically robust to small quantities.
[0040] S9 maps the end command output by the admittance to the joint micro-displacement increment through damped least squares inverse mapping, and superimposes it on the nominal trajectory position command to generate the final joint position command.
[0041] Map the end command of the admittance output to joint acceleration. The incremental joint displacement is obtained by performing discrete integration. Superimposed on the nominal trajectory position instruction Generate final joint position commands .
[0042] The mapping from end-point commands to joints employs damped least-squares inverse (DLS) to enhance the numerical stability of near-singular configurations.
[0043]
[0044] in For the terminal Jacobian matrix, This is the task weight matrix. is the damping coefficient.
[0045] Final output:
[0046]
[0047] S10 outputs the final joint position command to the robot, enabling force / torque control of each support end during support switching.
[0048] An internal load suppression optimization control system for a closed-loop multi-contact robot includes:
[0049] The data acquisition module is used to acquire body pose / velocity, end effector pose / velocity, end effector six-dimensional force / torque measurement values, and support contact assembly information;
[0050] The net six-dimensional force / torque generation module is used to calculate the desired net six-dimensional force / torque command. ;
[0051] The crawl mapping and allocation module is used to construct the crawl mapping matrix. And solve the optimization problem with null space penalty under contact feasibility constraints, and output the expected six-dimensional force / moment at each support end;
[0052] The switching shaping module is used to perform bandwidth limiting and fade-in / fade-out shaping on the desired six-dimensional force / torque reference within the support switching window;
[0053] Admittance execution module, used to generate end-effector desired generalized acceleration command and / or micro-displacement command based on six-dimensional force / torque error and pose / velocity error;
[0054] The damping injection module is used to inject damping into the admittance output based on the port power criterion so that the output power is not positive.
[0055] The disturbance estimation and compensation module is used to estimate and compensate closed-chain coupling disturbances online.
[0056] The micro-displacement mapping module is used to map the end-effector command into joint micro-increments using damped least squares inverse and then superimpose and output them to the position servo joint.
[0057] The optimization objectives of the grasping mapping and allocation module include a net six-dimensional force / torque tracking term and a grasping mapping null space penalty term, and the contact feasibility constraint includes a minimum clamping force constraint.
[0058] The disturbance estimation and compensation module employs a radial basis function network with leakage terms to suppress weight drift.
[0059] The switching and shaping module sets a transition window length for support establishment and dissolution, and performs second-order filtering on the reference six-dimensional force / torque within the window.
[0060] Compared with the prior art, the present invention has at least the following beneficial effects:
[0061] (1) By capturing the mapping null space penalty, the load growth in the closed chain is suppressed, and the adversarial output and actuator burden are reduced;
[0062] (2) By supporting the switching window reference shaping and port power constraint damping injection, the switching impact is reduced and the oscillation is suppressed, and the recovery speed after switching is improved;
[0063] (3) Through micro-displacement mapping, multi-contact six-dimensional force / torque adjustment can be realized on the position servo joint, reducing the dependence on the joint torque interface;
[0064] (4) In the prototype experiment, it can be observed that the actuator power index is reduced by about 15% compared with pure position control and the six-dimensional force / torque error within the switching window is improved. Attached Figure Description
[0065] Figure 1 This is a structural schematic diagram of the closed-loop multi-contact position servo robot to which this invention is addressed.
[0066] Figure 2This is the overall block diagram of the internal load suppression optimization control framework for the closed-loop multi-contact robot of the present invention;
[0067] Figure 3 This is a flowchart of the internal load suppression optimization control method for the closed-loop multi-contact robot of the present invention;
[0068] Figure 4 This is a schematic diagram of the closed-loop multi-contact position servo robot supporting the switching window in this invention;
[0069] Figure 5 This is a logical schematic diagram of the port power criterion and damping injection in this invention;
[0070] Figure 6 This is a schematic diagram of the online perturbation estimator structure in this invention;
[0071] Figure 7 This is a schematic diagram of the damped least squares inverse mapping from the end command to the joint micro-increment in this invention.
[0072] The meanings of the labels in the diagram are as follows:
[0073] 1—Robot body / body; 2—Leg / arm multiplex branch; 3—Joint module (position servo joint); 4—Support end effector (rigid gripper / gripper); 5—External support structure (truss node / column / tooling base); 6—Six-dimensional force / torque sensor; 7—Support contact end / rigid locking connection; 8—Body coordinate system {B}; 9—End alignment coordinate system ;
[0074] 11—Admittance Port Speed ;12—Admittance port output without damping injection ; 13-port power and its calculation module; 14—damping coefficient (Calculated based on port power criterion); 15—Control output after damping injection ;
[0075] S1—Acquire / estimate state and contact information; S2—Generate net six-dimensional force / torque; S3—Construct grasping map; S4—QP feasible allocation and internal load suppression; S5—Reference shaping, measurement filtering and coordinate alignment (including error construction); S6—Variable impedance admittance control; S7—Online disturbance estimator (including weight adaptive update); S8—End-joint mapping and position servo command generation. Detailed Implementation
[0076] To make the technical solution, implementation process, and beneficial effects of the present invention clearer, the present invention will be further described below with reference to the accompanying drawings and specific embodiments. Those skilled in the art should understand that the embodiments described in this specification are only for explaining the present invention and are not intended to limit the scope of protection of the present invention; the technical features of each embodiment can be combined with each other without conflict. In accordance with patent document drafting specifications, the specific embodiments should describe in detail the preferred methods of implementing the present invention, and, where necessary, provide examples and refer to the accompanying drawings.
[0077] This embodiment uses a closed-loop multi-contact position servo robot as an example for illustration. Figure 1 As shown, the robot includes a body 1 and four reusable leg arms 2. Each arm has a rigid gripper / gripper 4 at its end for rigid connection with truss nodes or tooling bases 5. Each joint of the robot is a high-reduction-ratio position servo actuator 3. The joint does not directly output torque commands but receives position / incremental position commands to achieve motion control. To achieve interactive force control, each support end is equipped with a six-dimensional force / torque sensor 6 to measure the three-dimensional force and three-dimensional torque interacting with the external environment and form a six-dimensional force / torque vector. The closed-chain multi-contact structure forms rigid geometric constraints during the support phase. During the switching phase, the support set is reconstructed, causing abrupt changes in the feasible six-dimensional force / torque set and the internal load subspace structure, which can easily lead to the growth of transient loads and closed-chain counteracting internal loads.
[0078] In this embodiment, the following variables and coordinate system are established: Let the robot's total joint variables be... For the support set Each support end Its six-dimensional contact force / torque is denoted as The six-dimensional force / torque is composed of three-dimensional force components. With three-dimensional torque components Composition, represented as
[0079]
[0080] Stack all the six-dimensional forces / torques at the support ends into
[0081]
[0082] The equivalent net six-dimensional force / torque acting on the fuselage during the mission is denoted as: And satisfy the crawling mapping relationship in It is determined by contact geometry and coordinate transformation.
[0083] When support redundancy exists (e.g.) and When in contact with six-dimensional force / torque It can be decomposed into net six-dimensional force / moment components and internal load components. The internal load components are located in... The null space direction remains unchanged. However, it will loop within the closed chain and cause structural loads to counteract joint outputs. This redundancy can be written as... in It is a false rebellion. It is a zero-space basis, satisfying , These are the internal load parameters. Therefore, this embodiment, while satisfying net six-dimensional force / torque tracking, further suppresses the internal load components in the zero space to reduce the sudden increase in internal load and the antagonistic joint load during support switching.
[0084] like Figure 2 As shown, this embodiment employs a layered closed-loop control pipeline: the whole-body layer generates the net six-dimensional force / torque command equivalent to the desired task; the multi-contact layer distributes the six-dimensional force / torque to each support end under feasible constraints and suppresses the load inside the zero space; the limb layer converts the desired six-dimensional force / torque into joint micro-displacement increments by switching a stable variable impedance admittance law, thereby realizing the adjustment of six-dimensional force / torque under position servo conditions.
[0085] The implementation method of the present invention is as follows Figure 3 As shown, the specific steps are as follows:
[0086] (1) Generation of net six-dimensional force / torque in the whole body layer
[0087] Let the desired and measured fuselage poses be respectively... and In Li Qun Upper definition of pose error:
[0088]
[0089] By combining the error velocity to construct state variables, the fuselage reference acceleration is obtained through error state feedback. .
[0090] In one implementation, the controller uses an equivalent dynamic model to map the reference acceleration into the desired net six-dimensional force / torque. The equivalent dynamic model may include equivalent inertia terms, velocity-related terms, and gravity equivalent terms; when an unloading device or equivalent gravity compensation is present, an unloading coefficient may be introduced. Scaling the gravity equivalent term yields... As a target for multi-contact layer allocation.
[0091] (2) Feasible allocation of multiple contact layers and internal load suppression
[0092] Based on crawl map In satisfying the set of contact feasible constraints Under the premise of this, we construct an optimization problem to solve for the desired six-dimensional force / moment at each support end. The solution is obtained Then, it is split according to the support end to obtain .
[0093] In a preferred implementation, the optimization problem is a quadratic programming problem, and the objective function includes at least:
[0094] (a) Net six-dimensional force / torque tracking term, making track ;
[0095] (b) Internal load suppression term, used to penalize exist The component in the null space direction.
[0096] It can be written as:
[0097]
[0098] in , This is the weight matrix.
[0099] The set of contact feasible constraints This may include, but is not limited to: upper and lower bound constraints of the six-dimensional force / torque components at each support end, minimum normal preload constraint, and linear inequality constraints determined by the structural strength and servo capability of the gripper.
[0100] (3) Switch window reference shaping and measurement alignment
[0101] Because support switching causes constraint reconstruction, the feasible six-dimensional force / moment set and the zero-space structure undergo abrupt changes, resulting in a jump trend in the allocation results before and after the switch. To reduce the injection of high-frequency excitation into the structure and servo system by this jump, this embodiment, after detecting the support set switching event, performs [further action] within the switching transition window. Reference shaping and bandwidth limiting are performed to obtain the filtered reference. The transition window in the switching event is as follows: Figure 4 As shown. Simultaneously, the measured six-dimensional force / torque will be... Mapped to coordinates After aligning the coordinate system with consistent end points, filtering is performed to obtain filtered measurements. And construct a six-dimensional force / torque error
[0102] In one implementation, the reference shaping may employ a first-order or second-order low-pass filter, and fade-in / fade-out coefficients may be introduced. Smooth start-stop force control function.
[0103] (4) Limb layer switching stable variable impedance admittance control and damping injection
[0104] For each support end Based on six-dimensional force / torque error End-effector pose error With end velocity error Construct the admittance law to generate the terminal desired generalized acceleration. (or equivalent micro-displacement / micro-velocity commands). For example, the admittance law can be written as:
[0105]
[0106] in For virtual inertia, For six-dimensional force / torque feedback gain, and These are the damping and stiffness matrices, respectively. This is a disturbance estimate. The damping and stiffness can be expressed in the form of variable impedance.
[0107]
[0108] And on , Apply projection / limiting to ensure that the parameters are bounded.
[0109] To suppress the sensitivity of the rigid closed chain to high-frequency excitation and improve the recovery speed after switching, this embodiment introduces a damped injection based on the port power criterion at the admittance port. For example... Figure 5 As shown, the admittance port output is assumed to be without damping. Port power is defined as When detected At that time, along Directional injection damping, resulting in in
[0110]
[0111] It is a numerically robust small quantity. This ensures that the port output power after injection is not positive, thereby suppressing switching-induced oscillations and improving recovery performance.
[0112] (5) Online disturbance estimation and compensation
[0113] Considering that factors such as closed-chain coupling, friction, and non-ideal contact manifest as bounded lumped perturbations in end-of-line admittance dynamics, this implementation can establish an online perturbation estimator for each support end and store the perturbation estimate... Injecting admittance laws improves switching stability and steady-state tracking accuracy. For example... Figure 6 As shown, in one implementation, the perturbation estimator takes the form of a radial basis function network (RBFNN):
[0114]
[0115] Where input This may include branch joint status, end-effector measurements, and error-related quantities. These are the basis function vectors. Weights Adaptive updates with leakage terms can be used to suppress weight drift:
[0116]
[0117] in For measurable residuals, The learning rate matrix, The leakage coefficient is denoted as .
[0118] (6) End-effector command to joint micro-displacement mapping and position servo implementation
[0119] Since the joint is a position servo actuator, it cannot directly output joint torque. Therefore, this embodiment outputs the end effector admittance (e.g., the desired generalized acceleration at the end effector). Mapped to joint acceleration The joint micro-displacement increment is obtained by integration. Superimposed on the nominal trajectory position instruction Generate final joint position commands .
[0120] like Figure 7 As shown, in one implementation, the end-to-joint mapping employs damped least squares inverse (DLS) to enhance the numerical stability of the near-singular configuration:
[0121]
[0122] in For the terminal Jacobian matrix, This is the task weight matrix. is the damping coefficient.
[0123] right By performing discrete integration, we can obtain And set amplitude and rate limits to meet the feasibility and safety requirements of the servo position loop.
[0124] Final output:
[0125]
[0126] (7) Control cycle and engineering implementation example
[0127] In one engineering implementation, the controller operating frequency can be selected from 100 Hz to 500 Hz; multi-contact layer optimization and limb layer admittance cycling can be updated within the same control cycle. Support switching events can be triggered by changes in the support set given by the gait planner, or by a joint determination of the gripper state (locked / unlocked) and force sensor threshold. Reference shaping filter parameters, transition window length, DLS damping coefficient, and admittance impedance parameters can be calibrated offline under the target structure / load condition, and different parameters can be used in the support and transition phases to balance steady-state compliance and switching vibration suppression recovery requirements.
[0128] In a prototype experiment, a truss climbing robot equipped with a six-dimensional force / torque sensor was used, and a microgravity environment was simulated through a ground unloading device. The controller operated at a frequency of 200 Hz, and the multi-contact layer secondary planning allocation and limb layer admittance control were updated within the same cycle; the support switching transition window was taken 1 s before and after the support and swing boundaries. The complete method of this invention and pure position control were compared under the same trajectory, the same unloading coefficient, and the same cycle conditions. An instantaneous electrical power proxy index was constructed based on the sum of the absolute values of the bus voltage and the current of each joint, and the total electrical power proxy index was obtained by integrating it. Experimental results show that the total electrical power proxy index of the method of this invention is 14036.71 J, while that of pure position control is 16618.00 J, a reduction of 15.5%. Furthermore, under the same diagonal gait support switching conditions, the root mean square error of the vertical force component tracking error of the method of the present invention within the switching window is 6.46 N; when the switching sensing reference processing or the online disturbance compensation is removed, the root mean square error increases to 8.44 N and 8.56 N, respectively, indicating that the switching sensing reference processing and online disturbance compensation help reduce the transient impact of switching and improve the force tracking performance during the switching phase.
[0129] Without departing from the spirit of this invention, the optimization objectives and constraints of the multi-contact layer can be extended according to the gripper's capabilities, structural strength, or task requirements; the disturbance estimator can be replaced with an extended state observer or other adaptive estimators; the damping injection strategy can be replaced with passive methods such as energy tanks; the end-to-joint mapping can be combined with redundancy to introduce secondary tasks (e.g., moving away from joint limits, reducing joint speed, etc.). All the above modifications should fall within the protection scope of this invention.
Claims
1. An internal load suppression optimization control method for closed-chain multi-contact robots, characterized in that, Includes the following steps: S1, for the working process of the closed-chain multi-contact robot, acquire the robot's motion state information, the six-dimensional force / torque of each support end and the support contact set information; S2, using the body pose error feedback and equivalent dynamic model to generate the expected task equivalent net six-dimensional force / torque command; S3, under the contact feasibility constraint, is distributed to the expected six-dimensional force / moment at each support end through quadratic programming with a zero-space penalty term for grasping mapping, thereby suppressing the load within the closed chain; Quadratic programming is based on satisfying the set of contact feasible constraints. Under the premise of this, what is the expected six-dimensional force / torque at each support end? The optimization problem is to construct the following objective function: The first term is the net six-dimensional force / torque tracking term, which makes... Tracking the expected task equivalent net six-dimensional force / torque command The second term is the internal load suppression term, used to penalize... exist The component in the null space direction, To capture the mapping matrix The zero-space basis satisfies ; This is the net six-dimensional force / torque tracking weight matrix. This is the internal load suppression weight matrix; Finally, the solution is obtained The result is obtained by splitting the support end. ; S4. Determine whether the robot has switched supports. If so, determine the transition window corresponding to the support switching event and execute S5. Otherwise, the robot is in a steady state. Perform steady-state conventional filtering and error construction on the expected six-dimensional force / torque and execute S6. S5. When the robot switches supports, the six-dimensional force / torque at each support end after the second planning is bandwidth-limited and progressively shaped in the transition window to obtain the expected six-dimensional force / torque reference at each support end. The gradual shaping process includes: applying second-order filtering or bandwidth-limited filtering to the desired six-dimensional force / torque within the transition window, and introducing gradual-in / gradual-out coefficients to the six-dimensional force / torque reference. This ensures that the six-dimensional force / torque feedback gain and / or reference amplitude change smoothly during the support establishment and disengagement phases, and The desired six-dimensional force / torque reference at each support end is obtained. ; At the same time, the measured six-dimensional force / torque at each support end will be... Mapped to coordinates After aligning the coordinate system with consistent end points, filtering is performed to obtain filtered measurements. And construct a six-dimensional force / torque error ; S6, uses a variable impedance admittance law to perform six-dimensional force / torque adjustment at each support end; For each support end Based on six-dimensional force / torque error End-effector pose error With end velocity error Construct a variable impedance admittance law to generate the desired generalized acceleration at each support end. Or equivalent micro-displacement / micro-velocity commands; S7 uses an online disturbance estimator to estimate the combined disturbance caused by closed-chain coupling and non-ideal contact, and compensates it to a variable impedance admittance control law; S8, based on the admittance port power criterion, injects damping into the admittance output to make the port output power non-positive, thereby suppressing switching-induced oscillation; Assume the admittance port output without damping is Port power is defined as When detected At that time, along Directional injection damping, resulting in Until the port output power is not positive; where For numerical robustness to small quantities; S9 maps the end command output by the admittance to the joint micro-displacement increment through damped least squares inverse mapping, and superimposes it on the nominal trajectory position command to generate the final joint position command. Map the end command of the admittance output to joint acceleration. The incremental joint displacement is obtained by performing discrete integration. Superimposed on the nominal trajectory position instruction Generate final joint position commands ; S10 outputs the final joint position command to the robot, enabling force / torque control of each support end during support switching.
2. The internal load suppression optimization control method for a closed-loop multi-contact robot according to claim 1, characterized in that, The support contact set information includes at least: the set of support end numbers currently in the support contact state, the number of support contacts, the contact object identifier corresponding to each support end, the contact state of each support end, the contact geometry and coordinate transformation information corresponding to each contact, and the information on the addition, deletion and change of the support contact set at adjacent time points; wherein, the contact state is used to characterize whether the support is established, maintained or released.
3. The internal load suppression optimization control method for a closed-loop multi-contact robot according to claim 1, characterized in that, The crawling mapping relationship is as follows: in This is a stacked vector of six-dimensional forces / torques at each support end. The grasping mapping matrix between the net six-dimensional force / moment and the six-dimensional force / moment at each support end is determined by the contact geometry and coordinate transformation; This represents the actual amount of net six-dimensional force / torque acting on the robot's body.
4. The internal load suppression optimization control method for a closed-loop multi-contact robot according to claim 1, characterized in that, The set of contact feasible constraints This includes: upper and lower limit constraints for the six-dimensional force / torque components at each support end; minimum clamping force / preload constraint in the support direction; and linear inequality constraints determined by the structural strength and position servo capability of the gripper, wherein the minimum clamping force / preload constraint in the support direction is satisfied by the force components at each support end along the normal direction of the contact alignment coordinate system. .
5. The internal load suppression optimization control method for a closed-loop multi-contact robot according to claim 1, characterized in that, The error construction specifically involves filtering and aligning the measured six-dimensional force / torque values with a coordinate system to obtain the six-dimensional force / torque error. The coordinate system alignment is achieved by mapping the measured six-dimensional force / torque values to an end-aligned coordinate system consistent with the desired six-dimensional force / torque reference through coordinate transformation before filtering.
6. The internal load suppression optimization control method for a closed-loop multi-contact robot according to claim 1, characterized in that, The variable impedance admittance law is: in For virtual inertia, For six-dimensional force / torque feedback gain, and These are the damping and stiffness matrices, respectively. For disturbance estimators; Damping and stiffness are achieved using variable impedance. And on , Apply projection / limiting to ensure that the parameters are bounded.
7. The internal load suppression optimization control method for a closed-loop multi-contact robot according to claim 1, characterized in that, The online estimator uses a radial basis function network: Where input This includes quantities related to the condition of the branch joint, end-effector measurements, and errors. The basis function vector; weights Adaptive updates with leakage terms are used to suppress weight drift: in For measurable residuals, The learning rate matrix, The leakage coefficient is denoted as .
8. The internal load suppression optimization control method for a closed-loop multi-contact robot according to claim 1, characterized in that, The process of generating the final joint position command is as follows: The mapping from end-effector commands to joints uses damped least-squares inverse mapping: in For the terminal Jacobian matrix, This is the task weight matrix. The damping coefficient; Final output:
9. An internal load suppression optimization control system for a closed-loop multi-contact robot, used to implement the control method according to any one of claims 1 to 8, characterized in that, include: The data acquisition module is used to acquire body pose / velocity, end effector pose / velocity, end effector six-dimensional force / torque measurement values, and support contact assembly information; The net six-dimensional force / torque generation module is used to calculate the desired net six-dimensional force / torque command. ; The crawl mapping and allocation module is used to construct the crawl mapping matrix. And solve the optimization problem with null space penalty under contact feasibility constraints, and output the expected six-dimensional force / moment at each support end; The switching shaping module is used to perform bandwidth limiting and fade-in / fade-out shaping on the desired six-dimensional force / torque reference within the support switching window; Admittance execution module, used to generate end-effector desired generalized acceleration command and / or micro-displacement command based on six-dimensional force / torque error and pose / velocity error; The damping injection module is used to inject damping into the admittance output based on the port power criterion so that the output power is not positive. The disturbance estimation and compensation module is used to estimate and compensate closed-chain coupling disturbances online. The micro-displacement mapping module is used to map end-effector commands into joint micro-increments using damped least squares inverse mapping and then superimpose and output them to the position servo joint.