Anti-sloshing motion control method and system for body-attached robot

Abstract

The application discloses a body-combined robot anti-swing motion control method and system, and relates to the technical field of body-combined robot motion control. The method performs time alignment on visual, inertial and force sensation observation data in a unified control cycle, and constructs a swing feedback state at a load interface. Input shaping compensation instructions and chassis-mechanical arm collaborative compensation instructions are synchronously generated based on the same swing feedback state, and the two compensation branches are updated in linkage according to the residual swing amount after control. When the residual swing amount continuously exceeds a safety threshold and is accompanied by abnormal detection results, the method switches to degradation control. The scheme is used for load anti-swing closed-loop control in a mobile and operation coupling scene.

Description

Technical Field

[0001] This invention relates to the field of motion control technology for embodied composite robots, and more specifically, to a method and system for anti-sway motion control of embodied composite robots. Background Technology

[0002] Hybrid robots typically consist of a mobile chassis, a robotic arm, an end effector, and a controller. They can perform composite tasks that involve moving and operating simultaneously in scenarios such as inspection, handling, grasping, assembly, warehousing, liquid transfer, and precision collaborative operations.

[0003] In the aforementioned application scenarios, when the embodied composite robot handles flexible components, suspended components, long workpieces, or liquid containers, the chassis's starting, braking, steering, obstacle crossing, and posture fluctuations, as well as the extension, retraction, lifting, and rapid posture switching of the robotic arm, will all induce load swaying. This swaying will further affect the end-effector positioning accuracy, liquid surface stability, grasping stability, and travel safety.

[0004] One existing control scheme primarily focuses on chassis trajectory tracking or robotic arm end-effector trajectory tracking, prioritizing ensuring the chassis reaches the target position or the robotic arm reaches the target pose, without introducing load sway as an independent control state variable into the closed loop. This type of scheme can meet basic control requirements under light load or low speed conditions, but in scenarios with significant load sway, it is prone to trajectory error accumulation and secondary end-effector oscillations.

[0005] Another approach attempts to suppress swaying solely by performing pose corrections at the robotic arm end, or by reducing vibration input solely by slowing down the chassis. While these approaches can reduce swaying under localized conditions, the lack of a unified control link between the chassis disturbance source and the robotic arm compensation source often leads to a situation where compensation at one end results in re-excitation at the other, thus limiting the overall vibration suppression effect.

[0006] Another approach uses either a visual sensor or an inertial sensor to detect the swaying state. However, single visual detection is easily affected by occlusion, lighting, reflection, and image blurring, while single inertial detection is easily affected by impact noise, integral drift, and installation errors, making it difficult to stably provide sway feedback that can be directly used for control under complex operating conditions.

[0007] In high-speed handling and liquid-containing operations, load sway is not only manifested as an increased swing angle, but also as an increased rate of change of swing angle, a shift in the dominant frequency, a change in the position of the equivalent center of mass, and a slower decay of residual oscillations. If the control system only processes the load based on a single swing angle threshold, it will be difficult to fully reflect the current sway risk.

[0008] Existing technologies include schemes that coordinate the control of the mobile chassis and the robotic arm to reduce vibration, schemes that use input shaping to suppress residual vibration of the robotic arm, and schemes that use multi-sensor information such as vision, inertia, and force sensing for state estimation. However, these schemes typically focus on one aspect of chassis-robotic arm coordination, fixed parameter shaping, or independent perception estimation, and have not yet established a continuous closed loop around the actual sway response at the load interface. This loop is driven by the same sway feedback state to uniformly drive input shaping and chassis-robotic arm collaborative compensation, and is triggered by the same residual sway amount to jointly update parameters. Even in schemes that use input shaping, they are mostly limited to suppressing local residual vibration of the robotic arm, without considering the path segment risks, chassis access space, and robotic arm joint margins during movement for collaborative constraints.

[0009] In complex ground environments, when the chassis is affected by slopes, joints, speed bumps, or sudden changes in the coefficient of friction, the end load will generate additional vibration. If only chassis deceleration is relied upon, the operation cycle will be significantly reduced. If only end-effector compensation is relied upon, it may lead to increased joint impact and energy consumption.

[0010] Therefore, how to unify visual, inertial, and force information into a sway feedback state that can be directly used for control solutions, without relying on a pre-stored environmental vibration database or additional passive compliant mechanisms, and how to enable this same sway feedback state to simultaneously drive the input shaping branch and the chassis-manipulator collaborative compensation branch within the same control cycle, and further enable the same residual sway amount to jointly trigger the dual-branch parameter linkage update and abnormal degradation control, thus distinguishing it from simple collaborative control, fixed parameter input shaping, or independent multi-source sensing estimation schemes, has become an urgent technical problem to be solved in the anti-sway motion control of embodied composite robots. Summary of the Invention

[0011] To address the problems of inconsistent compensation timing, fragmented control links, and inability to perform compensation in confined spaces caused by the separation of sensing, compensation, and parameter updating in existing solutions, this invention proposes an anti-sway motion control method and system for an embodied composite robot. Instead of treating input shaping, end-effector compensation, and anomaly handling as isolated functions, this invention establishes a unified closed loop around the actual sway response at the load interface. This allows the same sway feedback state to simultaneously drive two compensation branches, and the same residual sway quantity serves as the common update basis for both compensation branches.

[0012] According to a first aspect of the present invention, a method for anti-sway motion control of an embodied composite robot is provided. This method performs time alignment and unified modeling of visual, inertial, and force observation data within a unified control cycle to obtain a sway feedback state that can be directly used for control solution. Subsequently, input shaping compensation and chassis-robotic arm collaborative compensation are executed synchronously within the same control cycle, and after control output, the fusion weights, shaping parameters, compensation allocation coefficients, and compensation gain are updated in a linked manner based on the residual sway.

[0013] In this method, the input shaping branch is used to weaken the excitation component to be introduced during the control command generation stage, while the chassis-robotic arm collaborative compensation branch is used to suppress the sway response that has already occurred during the execution stage. Both branches share the same sway feedback state instead of operating independently based on different estimation results. This reduces compensation conflicts caused by inconsistent state estimations and lowers the risk of re-excitation due to inconsistent compensation directions.

[0014] In one embodiment, the chassis compensation allocation coefficient is determined jointly based on the passable space index and the robotic arm joint margin index, so that the compensation output is simultaneously limited by the environmental executable space and the robotic arm motion constraints; when the residual sway is in different threshold ranges, the controller updates the dual-branch parameters with different intensities, and switches to degradation control when the abnormality continues and the residual sway continuously exceeds the safety threshold.

[0015] According to a second aspect of the present invention, an anti-sway motion control system for a unibody composite robot is provided. The system includes a time alignment module, a sway state construction module, a multi-source fusion estimation module, a fusion weight update module, an input shaping module, a chassis-manipulator collaborative compensation module, a path segment pre-adjustment module, a residual sway evaluation and parameter linkage update module, a drive feedback monitoring module, and a safety degradation control module, for executing the above-described method. According to a third aspect of the present invention, an unibody composite robot is provided, comprising a mobile chassis, a manipulator, an end effector, and the unibody composite robot anti-sway motion control system, to implement closed-loop suppression of sway at the load interface during the coupling process of movement and operation.

[0016] In one implementation, the swaying state vector is defined as: xs(k) = [theta_x(k), theta_y(k), dtheta_x(k), dtheta_y(k), omega_s(k), delta_c(k)]T.

[0017] In one implementation, the multi-source fusion estimation can be expressed as: xh(k) = Wv(k)·xv(k) + Wi(k)·xi(k) + Wf(k)·xf(k).

[0018] In one implementation, the adaptive input shaping compensation instruction can be expressed as: u_sh(t) = Σ(A_r·u_0(t-T_r)), r = 1, 2, ... , M.

[0019] In one embodiment, the chassis micro-translation compensation amount, the robotic arm end-effector pose compensation amount, and the robotic arm joint compensation amount respectively satisfy: Delta_x_base(k) = eta(k)·(Kb1·theta(k) + Kb2·dtheta(k)); Delta_x_end(k) = (1-eta(k))·(Ke1·theta(k) + Ke2·dtheta(k)); Delta_q_arm(k) = J#(Delta_x_end(k) - G·Delta_x_base(k)).

[0020] Therefore, this invention does not separate input shaping, execution compensation, and parameter updating into independent local functions. Instead, it enables the two compensation branches to share the same sway feedback state and the same residual sway amount. The compensation allocation relationship is constrained by the passable space index and the joint margin index, thereby suppressing sway at the load interface while reducing the risk of secondary excitation caused by asynchronous compensation and the risk of non-execution in confined space.

[0021] Compared with the prior art, the present invention has at least the following beneficial effects: First, by uniformly aligning and mapping visual, inertial, and force observation data to the same state space, the input shaping branch and the cooperative compensation branch can operate based on the same sway feedback state, thereby reducing control inconsistencies caused by the estimated states of different branches.

[0022] Second, by synchronously generating input shaping compensation commands and chassis-robotic arm collaborative compensation commands within the same control cycle, new excitation inputs can be weakened and existing swaying can be suppressed, thereby reducing the compensation lag caused by relying solely on deceleration or solely on end-effector reverse correction.

[0023] Third, by using the controlled residual sway as a common feedback trigger and updating the fusion weight, shaping parameters, chassis compensation allocation coefficient and compensation gain in a coordinated manner, the parameter updates can directly correspond to the current residual sway level, thereby reducing the secondary excitation caused by the asynchronous updates of the two branches.

[0024] Fourth, by jointly determining the chassis compensation distribution coefficient based on the passable space index and the robotic arm joint margin index, the output ratio of chassis compensation and robotic arm compensation can be adjusted under the conditions of limited space and joint constraints, thereby taking into account both vibration damping capability and feasibility.

[0025] Fifth, by combining the residual sway amount exceeding the threshold with the abnormal detection result as the degradation trigger condition, and maintaining the constrained degradation control mode before the recovery condition is met, the risk of control instability caused by sensing distortion or drive deviation can be reduced. Attached Figure Description

[0026] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying 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.

[0027] Figure 1 This is a schematic diagram of the overall structure of the anti-sway motion control closed-loop system for the embodied composite robot of the present invention.

[0028] Figure 2 This is a flowchart of the shaking state sensing and multi-source fusion process of the present invention.

[0029] Figure 3 This is a flowchart of the adaptive input shaping and compensation instruction generation process of the present invention.

[0030] Figure 4 This is a flowchart of the chassis-robotic arm collaborative anti-sway control process of the present invention.

[0031] Figure 5 This is a block diagram of the motion control closed-loop logic of the present invention.

[0032] Figure 6 This is a flowchart illustrating the safety degradation control process of this invention. Detailed Implementation

[0033] To make the technical solution, implementation method, and beneficial effects of the present invention clearer, the present invention will be further described below with reference to the accompanying drawings and embodiments. It should be noted that the following embodiments are only for illustrating the present invention and are not intended to limit the scope of protection of the present invention.

[0034] like Figure 1As shown, the closed-loop system of the present invention is applied to a hybrid robot. The hybrid robot includes a mobile chassis 1, a robotic arm 2, an end effector 3, an end effector inertial measurement unit 4, a six-dimensional force sensor 5, a vision sensor 6, a controller 7, a path planning interface 10, a chassis driver 8, and a robotic arm joint driver 9. The end effector inertial measurement unit 4, the six-dimensional force sensor 5, and the vision sensor 6 are mounted at the end of the robotic arm or rigidly connected to the end effector 3. The chassis driver 8 and the robotic arm joint driver 9 return execution status feedback to the controller 7.

[0035] In one embodiment, the mobile chassis 1 can be a wheeled chassis, a tracked chassis, or an omnidirectional chassis; the robotic arm 2 can be a six-degree-of-freedom or seven-degree-of-freedom robotic arm; and the end effector 3 can be a gripper, a suction device, a liquid tray, a suspension device, or a special tooling. This invention can be applied wherever there is a risk of load swaying.

[0036] like Figure 2 As shown, the sway state perception and fusion process first collects chassis pose, chassis speed, joint position, joint speed, end effector attitude, end effector angular velocity, end effector linear acceleration, end effector force, and visual image sequence. Controller 7 estimates the current load's sway trend based on changes in the visual target contour, end effector attitude, and force. Its focus is on the actual sway response at the load interface, rather than solely relying on ground environment vibration data to perform feedforward adjustments to the chassis or robotic arm.

[0037] In one implementation, the swaying state vector xs(k) is defined as: xs(k) = [theta_x(k), theta_y(k), dtheta_x(k), dtheta_y(k), omega_s(k), delta_c(k)]T.

[0038] Where theta_x(k) and theta_y(k) represent the swing angles of the load in two mutually perpendicular planes, respectively; dtheta_x(k) and dtheta_y(k) represent the corresponding swing angle change rates, respectively; omega_s(k) represents the equivalent excitation frequency obtained within the most recent preset sliding time window based on the peak value of the sway state sequence spectrum, the zero crossover period, or the dominant mode shape identification result; delta_c(k) represents the offset of the load's center of mass relative to the nominal gripping position or the nominal supporting position. For liquid loads, delta_c(k) can be used to characterize the equivalent mass offset of the liquid surface relative to the center of the container; for suspended loads, delta_c(k) can be used to characterize the offset of the load's center of mass relative to the suspension reference line; for long flexible workpieces, delta_c(k) can be used to characterize the equivalent offset trend of the overall center of mass and the main bending segment.

[0039] In one implementation, the vision sensor 6 obtains the visual estimation state xv(k) by tracking the load edge, liquid surface boundary, suspension point projection, or target feature point; the end effector inertial measurement unit 4 obtains the inertial estimation state xi(k) by integrating angular velocity and linear acceleration and filtering; and the six-dimensional force sensor 5 obtains the force estimation state xf(k) by inverting the changes in end force and torque. Specifically, when the visual observation is the liquid surface tilt angle, suspension point swing angle, or workpiece end offset, the controller 7 maps it to observations of theta_x(k), theta_y(k), or delta_c(k); when the force observation is the change in end torque or low-frequency force fluctuation, the controller 7 maps it to supplementary observations of dtheta_x(k), dtheta_y(k), or delta_c(k), so that the observation results under different load types can all be classified into the same swaying state space.

[0040] The fusion estimation stage can be expressed as follows: xh(k) = Wv(k)xv(k) + Wi(k)xi(k) + Wf(k)xf(k).

[0041] Wherein, Wv(k), Wi(k), and Wf(k) satisfy Wv(k) + Wi(k) + Wf(k) = 1. This formula is only one implementation method, used to illustrate that visual, inertial, and force information can be fused according to weights in a unified state space, and does not limit the use of Kalman filtering, complementary filtering, factor graph optimization, or other fusion algorithms that can achieve equivalent functions.

[0042] In one implementation, the controller 7 adaptively adjusts the weights based on the visual confidence score Cv(k), the inertial noise intensity Ni(k), and the force residual Rf(k). The visual confidence score Cv(k) can be obtained by combining the target tracking score, feature point retention rate, and image sharpness; the inertial noise intensity Ni(k) can be obtained by the root mean square value, variance, or saturation number of the angular velocity or linear acceleration residual within the most recent preset time window; the force residual Rf(k) can be expressed as the deviation norm between the measured end force / torque vector Fm(k) and the force / torque vector Fhat(k) predicted based on the current motion state and load model, i.e., Rf(k) = ||Fm(k) - Fhat(k)||. After normalizing Cv(k), Ni(k), and Rf(k), controller 7 can construct visual reliability qv(k), inertial reliability qi(k), and force reliability qf(k), and set Wv(k) = qv(k) / (qv(k) + qi(k) + qf(k)), Wi(k) = qi(k) / (qv(k) + qi(k) + qf(k)), and Wf(k) = qf(k) / (qv(k) + qi(k) + qf(k)). When the visual occlusion rate increases, the illumination is insufficient, or the image is blurred, qv(k) is reduced; when the end is subjected to a sudden impact or the inertial noise increases, qi(k) is reduced; when the six-dimensional force sensor drifts zero or the installation is loose, resulting in an increase in the force residual, qf(k) is reduced.

[0043] After obtaining the fused wobbling feedback state xh(k), as follows Figure 3 As shown, controller 7 enters the input shaping and compensation generation stage. Traditional trajectory control often directly interpolates the original trajectory command u_0(t), while this invention first generates an adaptive input shaping and compensation command based on the current excitation frequency omega_s(k) and damping characteristics.

[0044] In one implementation, the input shaping compensation command satisfies: u_sh(t) = Σ(A_r·u_0(t-T_r)), r = 1, 2, ... , M.

[0045] Where A_r represents the weight of the r-th pulse, T_r represents the corresponding delay, and M represents the number of pulses. Controller 7 dynamically sets A_r and T_r based on the current load principal vibration period, damping ratio, and desired shaping accuracy to minimize the residual excitation of the control command on the target vibration mode after shaping. In one embodiment, T_r can be set as T_r = lambda_r·2pi / omega_s(k), where lambda_r is a timing coefficient related to the shaping structure; A_r is determined based on the estimated damping ratio, the target residual excitation ratio, and the number of pulses M.

[0046] For example, when the embodied composite robot is handling a liquid container, the main frequency of the liquid surface sloshing is low and the damping is small, so a multi-pulse shaping with a longer time window can be used; when the embodied composite robot is handling a short suspended component, the main frequency of the oscillation is high, so the shaping pulse interval can be shortened; when the oscillation decay time is prolonged or the main frequency shifts within the last few control cycles, the controller 7 can increase the number of pulses M or extend the interval between adjacent pulses; when the load stiffness is increased and the decay is accelerated, the controller 7 can shorten the shaping time window. Thus, adaptive input shaping is no longer a fixed template, but is reconstructed in real time based on the current sloshing state and recent decay characteristics.

[0047] like Figure 4 As shown, after input shaping, the present invention further employs chassis-robotic arm collaborative compensation. The controller 7 does not only cause the robotic arm 2 to perform end-effector reverse correction, nor does it only cause the chassis 1 to slow down. Instead, it distributes the remaining sway to the chassis micro-translation compensation channel and the robotic arm compensation channel. The robotic arm compensation channel first solves for the end-effector pose compensation amount and then maps it to the robotic arm joint compensation amount.

[0048] In one embodiment, the chassis micro-translation compensation amount, the robotic arm end-effector pose compensation amount, and the robotic arm joint compensation amount respectively satisfy: Delta_x_base(k) = eta(k)·(Kb1·theta(k) + Kb2·dtheta(k)); Delta_x_end(k) = (1-eta(k))·(Ke1·theta(k) + Ke2·dtheta(k)); Delta_q_arm(k) = J#(Delta_x_end(k) - G·Delta_x_base(k)).

[0049] Wherein, theta(k) is composed of theta_x(k) and theta_y(k), dtheta(k) is composed of dtheta_x(k) and dtheta_y(k), Delta_x_end(k) represents the end-effector pose compensation amount, Delta_q_arm(k) represents the joint compensation amount of the robotic arm obtained by solving based on the end-effector pose compensation amount, J# represents the inverse or generalized inverse matrix of the Jacobian matrix of the end-effector of the robotic arm. When the robotic arm is a non-redundant robotic arm, J# is the inverse Jacobian matrix; when the robotic arm is a redundant robotic arm, J# is the Moore-Penrose generalized inverse matrix of the Jacobian matrix, G represents the mapping matrix of chassis displacement to the end-effector coordinate system, and eta(k) represents the chassis compensation allocation coefficient. Through this relationship, chassis compensation and robotic arm compensation share the same sway feedback target, avoiding mutual interference due to independent control.

[0050] In one implementation, the controller 7 adaptively calculates eta(k) based on the passable space index Spass(k) and the joint margin index Sjoint(k). Spass(k) can be obtained by normalizing the minimum remaining clearance dclr(k) between the chassis outer envelope and the obstacle in the current or next path segment. For example, it can be calculated as Spass(k)=clip((dclr(k)-dmin) / (dpref-dmin),0,1), where dmin represents the minimum clearance that allows safe passage, dpref represents the desired clearance, both of which are positive numbers and dmin is less than dpref. clip(·) represents limiting the clearance to the interval between 0 and 1. Sjoint(k) can be obtained by normalizing the remaining amount of the upper limit of the relative position, velocity or acceleration constraints of each joint and taking the minimum value. For example, calculate the position margin rqi(k), velocity margin rvi(k) and acceleration margin rai(k) for the i-th joint, and then let Sjoint(k)=min_i{min[rqi(k),rvi(k),rai(k)]}. In one embodiment, eta(k) can be calculated as eta(k) = clip(beta0 + beta1·Spass(k) + beta2·(1-Sjoint(k)), etamin, etamax), where beta0 is the baseline value of the chassis compensation allocation coefficient, ranging from 0.3 to 0.7; beta1 is the weighting coefficient of the passable space index, ranging from 0.2 to 0.5; beta2 is the weighting coefficient of the joint margin index, ranging from 0.2 to 0.5; etamin is the lower limit of the chassis compensation allocation coefficient, ranging from 0.1 to 0.3; and etamax is the upper limit of the chassis compensation allocation coefficient, ranging from 0.7 to 0.9, with etamin being less than etamax. Therefore, when Spass(k) decreases, eta(k) decreases accordingly; when Sjoint(k) decreases, eta(k) increases accordingly. When the swing angle is small but the swing angle change rate is large and Spass(k) is large, the weight of chassis smoothing and vibration reduction compensation is increased first; when the swing angle is large but Spass(k) is small or the chassis is currently limited by a narrow space and cannot deviate significantly, the weight of robotic arm end-effector reverse posture compensation is increased first; when Sjoint(k) decreases, controller 7 synchronously reduces the robotic arm compensation ratio to avoid the robotic arm approaching a singular configuration or speed limit.

[0051] like Figure 5As shown, the closed-loop control logic of this invention is not a simple trajectory tracking closed loop, nor is it the same as the single-threshold vibration suppression logic that only implements deceleration or stopping based on the end vibration threshold. Instead, it is composed of task trajectory input, sway state feedback, input shaping branch, cooperative compensation branch, drive feedback monitoring branch, and residual sway evaluation branch. The controller 7 performs at least the following operations in each control cycle: updating the sway feedback state, calculating shaping parameters, generating chassis and robotic arm compensation amounts and chassis compensation allocation coefficients, issuing control commands, collecting drive execution feedback, and evaluating residual sway.

[0052] In one embodiment, the residual sloshing can be composed of one or more of the following: the control-induced swing angle, the rate of change of the swing angle, the liquid surface tilt angle deviation, the end position deviation, and the force-feel oscillation component. Any sloshing tendency that persists after control can be used as an evaluation index for residual sloshing.

[0053] For example, residual sway can be expressed as: Jr(k) = alpha_1·||theta(k)|| + alpha_2·||dtheta(k)|| + alpha_3·e_p(k) + alpha_4·e_f(k).

[0054] When the chassis is about to start from a standstill, the controller 7 detects that there is an excitation component in the preset speed curve that may cause the liquid surface to sway. Therefore, it first generates an input shaping compensation command u_sh(t) based on the currently estimated excitation frequency to make the edge of the chassis speed command smoother and reduce the excitation input during the start-up phase.

[0055] After starting, if the vision sensor 6 detects that the liquid surface tilt angle is still increasing and the peak angular velocity given by the inertial measurement unit 4 exceeds the threshold, the controller 7 calculates the chassis micro-translation compensation amount Delta_x_base(k) based on the current theta(k) and dtheta(k), causing the chassis to produce a small amplitude and opposite lateral correction; at the same time, it solves the robot arm end pose compensation amount Delta_x_end(k) and maps it to obtain the robot arm joint compensation amount Delta_q_arm(k), causing the end tray posture to undergo reverse fine adjustment.

[0056] During the turning phase, the liquid surface in the container begins to slosh again due to the increased yaw rate of the chassis. At this point, controller 7 re-estimates the dominant excitation frequency omega_s(k) and updates the shaping pulse delay T_r and weight A_r. In other words, the input shaping parameters are not set all at once at the start of the task, but are updated in real time as the operating conditions change.

[0057] When the embodied composite robot crosses a ground seam, the end effector inertial measurement unit 4 detects a sudden increase in impact acceleration, and the visual image becomes blurred for a short time, causing a decrease in confidence. Therefore, the controller 7 reduces the visual weight Wv(k) and increases the inertial weight Wi(k) and force weight Wf(k) to maintain the stability of the sway feedback state and avoid control misjudgments caused by a single visual distortion.

[0058] During the deceleration phase approaching the target position, if the residual sway assessment value Jr(k) is still higher than the stability threshold, the controller 7 increases the end-effector attitude compensation ratio and simultaneously reduces the chassis deceleration rate of change to reduce new deceleration excitation input. In this way, the system simultaneously handles both "existing sway" and "new excitation to be introduced".

[0059] If the task scenario changes to handling a suspended workpiece, the vision sensor 6 can track the workpiece's swing angle, the end effector inertial measurement unit 4 can detect the swing angular velocity, and the six-dimensional force sensor 5 can invert the force changes at the suspension point. The controller 7 can also construct a sway state vector based on the above information and use the same closed-loop framework to implement input shaping and collaborative compensation.

[0060] If the task scenario is changed to handling long, flexible workpieces, then delta_c(k) and the force oscillation components become more critical for vibration suppression decisions. In this case, controller 7 can increase the weight of the force feedback branch and appropriately extend the input shaping time window to reduce the continuous shaking caused by the flexible vibration mode of the workpiece.

[0061] In one embodiment, the present invention can reduce residual oscillation in high-speed transport tasks while maintaining a substantially constant work cycle. Because it weakens vibrations through input shaping before control commands are generated and compensates for them through the combined efforts of the chassis and robotic arm during execution, it can reduce oscillations without solely relying on significant speed reduction.

[0062] In another embodiment, the present invention can be used for attitude stabilization control before precision insertion or alignment. The controller 7 can reduce the residual wobble Jr(k) to a more stringent threshold before the end-effector approaches the target, thereby reducing pose error and additional impact at the moment of contact.

[0063] In one implementation, the number of shaping pulses M, weight A_r, delay T_r, fusion weight, and compensation gain can all be calibrated according to the task type. For example, liquid handling tasks prioritize reducing liquid surface sloshing, flexible rod handling tasks prioritize reducing end-end sway angle, and precision insertion tasks prioritize reducing end-end attitude change rate.

[0064] In one embodiment, the invention can work in conjunction with a path planning module. When the planning module identifies sharp turns, slopes, joints, or narrow passages in advance, the controller 7 can pre-increase the sampling frequency of the swaying state or pre-shorten the shaping update time, and pre-compress the adjustable range of the chassis compensation distribution coefficient eta(k) or limit the upper limit of the compensation displacement to enhance the response capability to potential excitation sections and avoid introducing new collision risks in confined spaces.

[0065] Under test conditions in one implementation, by explicitly introducing the swaying state at the load interface into the control closed loop, and combining it with multi-source fusion of vision, inertia, and force perception, adaptive input shaping, and chassis-robotic arm collaborative compensation, the residual swaying of the embodied composite robot in high-speed handling tasks can be reduced, and the stability of the liquid surface in liquid-carrying operations can be improved. The above description is used to illustrate the feasibility of the present invention and does not constitute a limitation on the scope of protection.

[0066] In one implementation, the system initialization phase first performs sensor calibration and coordinate system alignment. Specifically, the controller 7 reads the extrinsic parameter relationships between the body coordinate system of the mobile chassis 1, the base coordinate system of the robotic arm 2, the tool coordinate system of the end effector 3, and the end load coordinate system. It then performs static zero-bias correction on the end-effector inertial measurement unit 4, performs no-load tare on the six-dimensional force sensor 5, and uses the vision sensor 6 to perform hand-eye calibration on the calibration plate or calibration fixture. Through these steps, the subsequent visual estimation state xv(k), inertial estimation state xi(k), and force estimation state xf(k) can have a unified reference benchmark, avoiding distortion of the sway feedback state xh(k) caused by inconsistencies in the reference systems of different sensors.

[0067] In one implementation, the sampling and time synchronization of sway sensing are achieved by combining a fixed control cycle with asynchronous sensor buffering. The end-effector inertial measurement unit 4 and the six-dimensional force sensor 5 can sample at frequencies higher than the control cycle frequency, while the vision sensor 6 can acquire data asynchronously according to the image frame rate. At the arrival of each control cycle, the controller 7 extracts the data sample closest to the current timescale from each buffer and performs timestamp alignment or extrapolation compensation on the visual data. Thus, the relatively long processing delay of the vision link can be taken into account while maintaining the consistency of sway state estimation within the control cycle.

[0068] In one implementation, the vision sensor 6 employs different visual feature extraction strategies for different load types. When the load is a liquid container, it can extract the liquid surface boundary line, the liquid surface normal vector, or the tilt angle of the liquid surface relative to the container edge; when the load is a suspended workpiece, it can extract the projection of the suspension point, feature points at the end of the workpiece, or the direction of the workpiece's main axis; when the load is a long, flexible workpiece, it can extract the workpiece's centerline, end offset, or the deformation of the target cross-section. The controller 7 then maps the extracted visual features to measurements of the swing angle, the rate of change of the swing angle, or the center of mass offset, thereby improving adaptability to different types of swaying objects.

[0069] In one implementation, the unified control cycle of controller 7 can be set jointly based on the execution bandwidth of the robot platform and the load sway frequency. For example, for a chassis-arm coupled handling task, the control cycle can be selected from 5ms to 20ms. The sampling frequency of the end effector inertial measurement unit 4 and the six-dimensional force sensor 5 can be higher than the control cycle frequency, and the image frame rate of the vision sensor 6 can be lower than the sampling frequency of the inertial branch. In each control cycle, controller 7 prioritizes reading the most recently valid inertial, force, and visual buffer data and records the deviation between each data sample and the current control time scale. When the visual data delay exceeds the preset allowable delay threshold, the vision branch only participates in state correction and does not directly participate in the fast compensation output of the current cycle, so as to reduce the impact of asynchronous delay on closed-loop stability.

[0070] In one implementation, the load type parameter template, in addition to limiting the adjustable range of fusion weights, shaping parameters, and compensation gains, further limits the maximum update step size per cycle and the output limiting boundary. Specifically, the single-cycle changes of the visual weight Wv(k), inertial weight Wi(k), and force weight Wf(k) can be limited to preset upper limits, the update amount of the pulse delay T_r does not exceed a preset proportion of the current vibration cycle, and the chassis compensation displacement and the robotic arm end-effector pose compensation are constrained by the chassis safety lateral margin and the end-effector attitude error tolerance range, respectively. By simultaneously limiting the parameter range and the change step size, new excitations or control saturation caused by parameter mutations under boundary conditions can be avoided.

[0071] In one implementation, the controller 7's assessment of the residual sway Jr(k) is used not only for parameter updates in the next cycle but also, together with the work phase identifier, to determine the threshold level. When the task is in the starting, braking, turning, obstacle crossing, precision alignment, or pre-end contact phase, the controller 7 reads the corresponding threshold correction coefficients from the parameter template and performs phased corrections on the stability threshold Jstb, safety threshold Jsafe, and degradation trigger cycle number Nsafe. For the handling phase, which allows short-term dynamic sway but requires rapid final convergence, the instantaneous sway angle constraint can be appropriately relaxed and the decay time constraint strengthened. For the precision alignment and pre-contact phases, the constraint levels on end position deviation and force oscillation deviation are increased to ensure that the vibration suppression logic is consistent with the actual work objectives.

[0072] In one implementation, the anomaly detection result can be generated by a joint determination of multiple conditions. Specifically, when the visual confidence level is lower than the visual threshold for several consecutive cycles, the inertial noise intensity is consistently higher than the noise threshold, the force perception residual is consistently higher than the residual threshold, or the execution deviation returned by the chassis driver 8 and the robotic arm joint driver 9 exceeds the corresponding execution error threshold, the controller 7 writes the corresponding anomaly flag into the anomaly state register area and determines whether to enter degradation control based on the duration of continuous over-threshold of Jr(k). If only a single short-term anomaly occurs and Jr(k) does not continuously exceed the safety threshold, the controller 7 only suppresses the weight of the abnormal branch and maintains the normal closed loop to avoid over-responding to transient disturbances.

[0073] In one implementation, after the degradation control exits, the controller 7 does not immediately revert to all standard control parameters. Instead, it gradually restores the visual branch weights, chassis speed limit, robotic arm compensation gain, and input shaping update time window according to a preset recovery slope. Preferably, the conventional speed limit under the inertial-force closed loop can be restored first, and then the visual branch participation ratio and collaborative compensation gain can be restored in stages over several consecutive control cycles. When Jr(k) rises rapidly again or abnormal detection results are repeatedly triggered during the recovery process, the controller 7 terminates the recovery process and reverts to the degradation state. By setting a staged recovery mechanism, the risk of secondary oscillation caused by immediate full recovery immediately after the anomaly is resolved can be reduced.

[0074] In one implementation, before generating the input shaping parameters, the controller 7 can estimate the damping characteristics of the dominant vibration mode based on the sway state sequence within the most recent control cycles. When a decrease in damping ratio, an increase in oscillation decay time, or a shift in the excitation frequency is detected, the controller 7 increases the number of shaping pulses M or extends the interval between adjacent pulses; when an increase in load stiffness, an increase in dominant frequency, or an acceleration in decay is detected, the controller 7 shortens the shaping time window. In this way, the input shaping not only depends on the instantaneous excitation frequency omega_s(k) but also takes into account the recent oscillation decay characteristics, thereby reducing the problem of insufficient response of fixed parameter shaping to changes in operating conditions.

[0075] In one implementation, the controller 7 can classify and manage the characteristics of future path segments. These characteristics can be provided by the path planning module, local map module, or environmental perception module, and may include at least path curvature, embankment height, joint width, speed bump location, remaining width of narrow passages, straight-line acceleration segments, braking segments, sharp turn segments, embankment crossing segments, joint crossing segments, narrow passage segments, and precision alignment segments. For different segments, the controller 7 employs a differentiated pre-tuning strategy: for sharp turn segments, it prioritizes increasing the sampling frequency of yaw-related state components; for embankment, joint, or speed bump segments, it prioritizes shortening the update cycle of input shaping parameters and pre-lowering the compensation action limit; for precision alignment segments, it prioritizes increasing the constraint level of the residual sway threshold. This allows the system to adjust the control chain parameters before potential excitation occurs, rather than passively responding after swaying occurs.

[0076] In one implementation, the adaptive allocation of the chassis and robotic arm compensation ratio can be further corrected by incorporating task space constraints. For example, when the embodied robot is located in a narrow passage, a wall-side workstation, or a human-robot mixed area, the controller 7 can calculate eta(k) based on Spass(k) and Sjoint(k), and then apply an upper limit constraint etamap,max to eta(k) based on the environmental lateral safety margin. When the robotic arm approaches a singular configuration, the joint speed approaches the upper limit, or the remaining joint stroke is insufficient, the controller 7 can apply a lower limit increase to eta(k) or directly increase the chassis compensation weight. In one implementation, etamap,max can be determined based on the area safety level, obstacle density, or passage width classification, so that the compensation allocation coefficient is affected by both the swaying state and the actual executable space limitation. Thus, a dynamic balance can be maintained between vibration damping capability and executability.

[0077] In one implementation, the residual sway Jr(k) is used not only to determine the parameter update for the next cycle but also to trigger control constraints in a tiered manner. When Jr(k) is below the stability threshold Jstb, the controller 7 maintains the normal shaping parameters and normal compensation gain. When Jr(k) is above the stability threshold Jstb but below or equal to the safety threshold Jsafe, the controller 7 enters an enhanced vibration suppression mode, increasing the fusion update frequency and appropriately increasing the compensation gain within the limits defined by the load type parameter template. When Jr(k) is above the safety threshold Jsafe for N consecutive control cycles and the anomaly persists, the controller 7 enters a safety degradation mode. In one implementation, the load type parameter template can further limit the allowable step size of each parameter in the enhanced vibration suppression mode, including the maximum adjustment of the fusion weight per cycle, the maximum adjustment of the shaping pulse delay, the maximum adjustment of the chassis compensation allocation coefficient, and the maximum adjustment of the compensation gain, to avoid introducing new excitation due to parameter mutations in boundary states. By setting different threshold ranges and different update step sizes, the frequent switching of control modes in boundary states can be avoided.

[0078] In one implementation, safety degradation control can also employ an exit hysteresis mechanism. That is, when visual obstruction, communication anomalies, zero drift exceeding limits, or external shocks cause the system to enter a degradation state, the controller 7 does not immediately resume the normal closed loop upon detecting the disappearance of the anomaly. Instead, it requires the visual confidence level, inertial noise intensity, force residual, and residual sway to simultaneously recover to normal ranges within consecutive Mrec control cycles, and then gradually restores the visual feedback branch, the normal speed upper limit, and the standard compensation gain. This hysteresis mechanism can prevent the system from repeatedly entering and exiting the degradation state within the abnormal edge region.

[0079] In one embodiment, the present invention can also be deployed through a combination of offline calibration and online self-tuning. During the offline calibration phase, the number of shaping pulses M, initial weight A_r, initial delay T_r, and initial gain values ​​for various tasks are preset according to the types of typical liquid loads, suspended loads, and flexible workpieces. During the online operation phase, these parameters are then updated within a small range based on the actually observed omega_s(k), delta_c(k), Jr(k), and environmental segment characteristics. This method can shorten on-site deployment time while retaining adaptability to different robot platforms, chassis types, and load types.

[0080] In one embodiment of liquid container handling, the end effector 3 is a pallet-type support mechanism. The vision sensor 6 takes the upper edge of the container and the liquid surface boundary as the main observation objects. The end inertial measurement unit 4 is used to output the angular velocity and linear acceleration corresponding to the container's attitude change. The six-dimensional force sensor 5 is used to detect the force change on the pallet. When the mobile chassis 1 performs starting, braking, or turning actions, the controller 7 first sets the shaping pulse parameters according to the liquid surface main vibration frequency identified in the most recent several cycles. Then, it calculates the chassis micro-translation compensation amount and the robotic arm end pose compensation amount based on the liquid surface tilt angle and the pallet attitude deviation, and maps the robotic arm end pose compensation amount to the robotic arm joint compensation amount. If the visual characteristics of the liquid surface are unstable for a short time due to reflection, the controller 7 reduces the weight of the visual branch and maintains short-term closed-loop control with the inertial and force branches. After the visual confidence recovers, the weight of the visual branch is increased again.

[0081] In one embodiment of suspended workpiece handling, the end effector 3 is a hanging clamp, and the load is connected to the end effector 3 via a flexible sling. At this time, the vision sensor 6 continuously tracks the projected position of the workpiece's center of mass or the workpiece's swing envelope, the end inertial measurement unit 4 outputs inertial measurements related to the swing angular velocity, and the six-dimensional force sensor 5 outputs the force and torque changes at the suspension point. The controller 7 constructs a sway state vector xs(k) based on the swing angle obtained from visual observation and the force deviation obtained from force inversion. During the turning phase, it prioritizes increasing the lateral compensation weight, and during the braking phase, it prioritizes reducing the rate of change of velocity and increasing the input shaping smoothness, thereby avoiding amplified swing amplitude when the suspended workpiece undergoes longitudinal deceleration and lateral turning.

[0082] In a long-length flexible workpiece transfer embodiment, in addition to overall swaying, flexible bending vibration modes may also occur when the load accelerates / decelerates on the chassis 1 and switches the posture of the robotic arm 2. Besides using the vision sensor 6 to extract the workpiece centerline and end offset, the controller 7 can also estimate the workpiece's flexible oscillation trend based on the low-frequency force fluctuations output by the six-dimensional force sensor 5, and increase the weights of delta_c(k) and the force-feeling oscillation component in the residual sway Jr(k). When the slow decay of the flexible vibration mode is detected, the controller 7 appropriately extends the input shaping time window, reduces the chassis speed change rate, and limits the high-frequency components of the robotic arm's end-effector posture compensation to avoid re-excitation of the workpiece's flexible vibration mode.

[0083] In one implementation, the controller 7 can communicate with the chassis driver 8 and the robotic arm joint driver 9 via a real-time industrial communication bus or an industrial Ethernet control link. The end effector inertial measurement unit 4, the six-dimensional force sensor 5, and the vision sensor 6 share a unified time reference or timestamp synchronization mechanism with the controller 7. Within each control cycle, the controller 7 first completes sensor sampling alignment, sway state construction, and fusion estimation. Then, it outputs the obtained sway feedback state in parallel to the input shaping solution branch, the chassis-robotic arm collaborative compensation solution branch, and the residual sway evaluation and parameter linkage update branch. The chassis-robotic arm collaborative compensation solution branch first calculates the robotic arm end effector pose compensation amount and then maps it to the robotic arm joint compensation command. The update result output by the residual sway evaluation and parameter linkage update branch is further fed back to the fusion weight update, input shaping, and collaborative compensation solution stages, thus forming a closed-loop data flow centered on the same sway feedback state and the same residual sway amount. Subsequently, the controller 7 issues smoothed chassis control commands to the chassis driver 8 and joint compensation commands to the robotic arm joint driver 9 in a predetermined order. If a limiting constraint, communication delay anomaly, or driver execution status feedback anomaly is detected in the current cycle, the controller 7 will prioritize the execution of the safety degradation logic and record the anomaly type and trigger time for subsequent recovery determination and maintenance traceability.

[0084] In one embodiment, to verify the deployment effect of the present invention, two sets of comparative tests can be performed, one with anti-sway control enabled and the other with anti-sway control enabled, using the same path, load, and work cycle. Evaluation indicators include at least the residual sway Jr(k), end-effector position deviation, liquid surface tilt angle deviation, arrival stabilization time, chassis speed fluctuation, and robotic arm joint compensation amplitude. If, after enabling the present invention, at least one or more of the above indicators are improved while maintaining a substantially unchanged work cycle, it demonstrates that the unified anti-sway closed loop constructed by the present invention is engineering-feasible for actual operations. The above experimental process is used to illustrate the verification method of the present invention and does not constitute a limitation on the scope of protection.

[0085] In one implementation, a correspondence between task types and parameter templates can be established during system deployment. The parameter template includes at least: initial fusion weight, upper limit of the number of shaping pulses, upper limit of the shaping time window, upper limit of chassis compensation displacement, upper limit of robotic arm compensation speed, upper and lower limits of chassis compensation allocation coefficients, stability threshold Jstb, safety threshold Jsafe, stability judgment cycle number Nstb, degradation trigger cycle number Nsafe, degradation exit confirmation cycle number Mrec, allowable force fluctuation range, allowable end-effector attitude error, correction coefficients corresponding to the path segment risk level, and normalized baseline values ​​for navigable space indicators and joint margin indicators. For liquid load tasks, the parameter template prioritizes limiting the liquid surface tilt angle and longitudinal and lateral velocity abrupt changes; for suspended load tasks, the parameter template prioritizes limiting the peak swing angle and swing angle change rate; for long-size flexible workpiece tasks, the parameter template prioritizes limiting the duration of flexible oscillation and end-effector position deviation. The controller 7 reads the corresponding template before the task begins and performs parameter updates only within the template's defined range during operation. This avoids the parameter drift risk associated with fully online self-tuning while retaining adaptability to different task conditions.

[0086] In one implementation, the controller 7 can also write each parameter update, degradation trigger, degradation exit, threshold exceedance type, and driver execution feedback into the operation log. The operation log can be used for offline analysis of the sway characteristic distribution under different loads, different path segments, and different ground conditions, and accordingly correct the parameter template or safety threshold for the next round of tasks. Preferably, during the offline analysis phase, the peak distribution of Jr(k), the duration of continuous threshold exceedance, the distribution of Spass(k), the distribution of Sjoint(k), and the frequency of degradation triggers are statistically analyzed according to the load type number, environmental segment number, and task number. Based on this, the candidate value ranges of Jstb, Jsafe, Nsafe, Nstb, and Mrec are recalibrated, and after manual confirmation, they are written back to the parameter template. By combining online control with offline review, the parameter adaptation efficiency and long-term operational stability of the system in repetitive operation scenarios can be further improved.

[0087] In one implementation, when the controller 7 normalizes the visual confidence Cv(k), inertial noise intensity Ni(k), and force residual Rf(k), it first performs scale unification based on the upper and lower limits corresponding to their respective dimensions. Then, it performs limiting and smoothing by combining the mean or variance over the most recent control cycles to suppress the direct impact of single-cycle spike disturbances on the fusion weights. By following the processing sequence of normalization, smoothing, and limiting, the weight updates can reflect changes in sensor reliability without causing abrupt changes in Wv(k), Wi(k), and Wf(k) due to instantaneous noise.

[0088] In one implementation, the preferred processing sequence within the same control cycle is: sampling alignment, state construction, fusion estimation, input shaping solution, collaborative compensation solution, drive deployment, residual sway evaluation, and parameter linkage update. Both the input shaping branch and the chassis-robotic arm collaborative compensation branch use the sway feedback state xh(k) at the same time scale as input, thus ensuring that the two compensation branches correspond to the same sway snapshot at the same load interface, rather than asynchronous state quantities from different time sections.

[0089] In one implementation, the chassis compensation allocation coefficient eta(k) can be generated using a two-level constraint method: first, candidate allocation values ​​are obtained based on the passable space index Spass(k) and the robotic arm joint margin index Sjoint(k); then, the candidate allocation values ​​are constrained by amplitude and slope by combining the chassis lateral safety margin, the robotic arm singular configuration margin, and the maximum change step size per single cycle. By setting amplitude and rate of change boundaries for eta(k), control jitter caused by frequent switching of dominance between chassis compensation and robotic arm compensation under boundary conditions can be reduced.

[0090] In one implementation, the anomaly detection result is preferably generated by a combination of duration and amplitude conditions. Specifically, when a single sensing branch experiences a one-time noise spike, only the anomaly count is recorded without immediately triggering degradation control; a stable anomaly marker is formed only when at least one of the visual confidence level, inertial noise intensity, force residual, or drive execution deviation exceeds the corresponding threshold for several consecutive control cycles; the controller 7 outputs a degradation trigger signal only when the stable anomaly marker and the residual sway both continuously exceed the safety threshold. This joint determination method helps avoid misjudging short-term disturbances as systemic anomalies requiring a switch in control mode.

[0091] In one implementation, the load type parameter template, in addition to specifying the value ranges of shaping parameters, fusion weights, and compensation gains, can also specify the default sampling frequency, default threshold level, and default recovery slope corresponding to different load types. For example, liquid load tasks preferably employ stricter liquid surface tilt angle constraints and gentler velocity change rate limits, suspension load tasks preferably employ higher swing angle change rate weights, and long-sized flexible workpiece tasks preferably employ longer shaping time windows and stricter high-frequency compensation limiting. By loading the parameter template corresponding to the load type before the task begins, the online tuning time can be shortened, and the adaptability of initial parameter values ​​under different working conditions can be improved.

[0092] In one embodiment, the present invention can be deployed on an industrial controller, vehicle controller, or industrial computer platform. The controller 7 interacts with the chassis drive 8 and the robotic arm joint drive 9 via real-time industrial Ethernet, fieldbus, or other deterministic communication links. For implementation scenarios with a control cycle of 5ms to 20ms, the sensor sampling frequency, communication refresh cycle, and parameter update step size can be pre-matched and set according to the platform bandwidth and the load's main oscillation frequency to achieve closed-loop suppression of sway at the load interface without adding an additional passive compliance mechanism.

[0093] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit the scope of protection of the present invention. Equivalent substitutions, modifications, or combinations made by those skilled in the art to the present invention without departing from the concept of the present invention should all fall within the scope of protection of the present invention.

Claims

1. A method for anti-sway motion control of a unibody composite robot, applied to a unibody composite robot comprising at least a mobile chassis, a robotic arm mounted on the mobile chassis, an actuator disposed at the end of the robotic arm, and a controller, for implementing closed-loop suppression of swaying at the load interface during the coupling of movement and operation, characterized in that, Includes the following steps: S1. During the current control cycle, collect the chassis motion state, robotic arm motion state, end-effector multi-source observation data, and task trajectory instructions of the embodied composite robot. The end-effector multi-source observation data includes visual observation data, inertial observation data, and force observation data for the sway response at the load interface. S2. The multi-source observation data at the end are timestamped according to a unified control cycle. The load swing angle, swing angle change rate, equivalent excitation frequency, and offset information of the load center of mass relative to the nominal grasping position or nominal support position are extracted to construct a sway state vector. The visual estimation state, inertial estimation state, and force estimation state are mapped to a unified state space and then multi-source fusion estimation is performed to obtain the sway feedback state of the current control cycle. S3. Based on the same sway feedback state and the same task trajectory instruction, input shaping compensation instruction and chassis-robotic arm collaborative compensation instruction are generated synchronously within the same control cycle. The chassis-robotic arm collaborative compensation instruction includes chassis micro-translation compensation amount, robotic arm end-effector pose compensation amount and robotic arm joint compensation amount mapped from the robotic arm end-effector pose compensation amount. The chassis compensation allocation coefficient is determined jointly based on the passable space index and the robotic arm joint margin index. S4. The input shaping compensation command and the chassis-robotic arm collaborative compensation command are applied together to the chassis driver and the robotic arm joint driver to perform closed-loop motion control, and the residual sway is calculated based on the sway response at the load interface after control, the end position deviation and the force oscillation deviation. S5. The residual sway is used as the common feedback trigger for the input shaping compensation branch and the chassis-robotic arm collaborative compensation branch. When the residual sway is higher than the stability threshold Jstb and lower than or equal to the safety threshold Jsafe, the multi-source fusion adaptive weight, input shaping parameters, chassis compensation allocation coefficient, and compensation gain corresponding to chassis compensation and robotic arm compensation are updated in the next control cycle. When the residual sway is higher than the safety threshold Jsafe for N consecutive control cycles and there are abnormal detection results, degradation control is switched to.

2. The anti-sway motion control method for the embodied composite robot according to claim 1, characterized in that, The swaying state vector constructed in step S2 satisfies: xs(k) = [theta_x(k), theta_y(k), dtheta_x(k), dtheta_y(k), omega_s(k),delta_c(k)]T; Where, theta_x(k) and theta_y(k) represent the load swing angle in the two orthogonal planes at the current control time, dtheta_x(k) and dtheta_y(k) represent the swing angle change rate, omega_s(k) represents the equivalent excitation frequency corresponding to the main frequency peak or the main mode frequency obtained by spectrum identification of the sway state sequence at the load interface within the most recent preset sliding time window, and delta_c(k) represents the offset of the load center of mass relative to the nominal gripping position or the nominal supporting position; The shaking feedback state satisfies: xh(k) = Wv(k)xv(k) + Wi(k)xi(k) + Wf(k)xf(k); Where xh(k) is the shaking feedback state as described in claim 1, xv(k) is the visual estimation state, xi(k) is the inertial estimation state, xf(k) is the force estimation state, and Wv(k), Wi(k) and Wf(k) are adaptive weights that satisfy the condition that the sum is 1.

3. The anti-sway motion control method for the embodied composite robot according to claim 2, characterized in that, The adaptive weights are updated based on the visual confidence Cv(k), inertial noise intensity Ni(k), and force perception residual Rf(k). The controller normalizes Cv(k), Ni(k), and Rf(k) to construct visual reliability qv(k), inertial reliability qi(k), and force perception reliability qf(k), and updates Wv(k), Wi(k), and Wf(k) according to the proportion of each reliability in the total reliability. When the reliability of the visual branch, inertial branch, or force perception branch decreases, the weight of the corresponding branch is reduced.

4. The anti-sway motion control method for the embodied composite robot according to claim 2, characterized in that, The input shaping compensation instruction in step S3 satisfies: u_sh(t) = Σ(A_r·u_0(t-T_r)), r = 1, 2, ... , M; Where u_0(t) is the original trajectory command, u_sh(t) is the shaped trajectory command, A_r is the weight of the r-th pulse, T_r is the delay of the r-th pulse, M is the number of pulses, and the vibration period corresponding to the equivalent excitation frequency omega_s(k) according to claim 2 is adaptively set; the controller further updates A_r, T_r and M according to the damping characteristics, oscillation decay time or main frequency offset identified in the most recent preset control cycle, so that the updated A_r, T_r and M participate in the dual-branch linkage update as the input shaped pulse parameters according to claim 1.

5. The anti-sway motion control method for the embodied composite robot according to claim 2, characterized in that, The chassis micro-translation compensation amount, the robotic arm end-effector pose compensation amount, and the robotic arm joint compensation amount in step S3 respectively satisfy: Delta_x_base(k) = eta(k)·(Kb1·theta(k) + Kb2·dtheta(k)); Delta_x_end(k) = (1-eta(k))·(Ke1·theta(k) + Ke2·dtheta(k)); Delta_q_arm(k) = J#(Delta_x_end(k) - G·Delta_x_base(k)), where J# represents the inverse or generalized inverse of the Jacobian matrix at the end of the robotic arm. When the robotic arm is non-redundant, J# is the inverse of the Jacobian matrix; when the robotic arm is redundant, J# is the Moore-Penrose generalized inverse of the Jacobian matrix. Where, theta(k) represents the swing angle vector composed of theta_x(k) and theta_y(k), dtheta(k) represents the swing angle rate vector, Delta_x_base(k) is the chassis compensation displacement vector, Delta_x_end(k) is the pose compensation amount of the robotic arm end effector, Delta_q_arm(k) is the joint compensation amount of the robotic arm obtained by solving based on the pose compensation amount of the robotic arm end effector, and J# represents the inverse matrix or generalized inverse matrix of the Jacobian matrix of the robotic arm end effector. When the robotic arm is a non-redundant robotic arm, J# is the inverse Jacobian matrix; when the robotic arm is a redundant robotic arm, J# is the inverse Jacobian matrix. The Moore-Penrose generalized inverse matrix is ​​defined as follows: G is the mapping matrix from chassis displacement to the end-effector coordinate system; Kb1, Kb2, Ke1, and Ke2 are compensation gains; and eta(k) is the chassis compensation allocation coefficient. eta(k) is adaptively adjusted based on the passable space index and the robotic arm joint margin index. The passable space index represents the minimum remaining clearance between the chassis envelope and obstacles, while the robotic arm joint margin index represents the remaining available amount of the upper limit of the relative position, velocity, or acceleration constraints of each joint. When the passable space index decreases, eta(k) is reduced; when the robotic arm joint margin index decreases, eta(k) is increased.

6. The anti-sway motion control method for the embodied composite robot according to claim 2, characterized in that, The residual sway in step S5 satisfies: Jr(k) = alpha_1·||theta(k)|| + alpha_2·||dtheta(k)|| + alpha_3·e_p(k)+ alpha_4·e_f(k); Where e_p(k) represents the end-effector position deviation, e_f(k) represents the force oscillation deviation, and alpha_1 to alpha_4 are weighting coefficients; the stability requirement is that Jr(k) is less than or equal to the stability threshold Jstb within N consecutive control cycles, and the stability threshold Jstb and the safety threshold Jsafe are set according to the load type parameter template, the end-effector allowable attitude error, the allowable force fluctuation, and the risk level of the path segment; when Jr(k) is higher than the stability threshold Jstb and lower than or equal to the safety threshold Jsafe, the adaptive weights, input shaping pulse parameters, chassis compensation allocation coefficients, and compensation gains corresponding to chassis compensation and robotic arm compensation are updated within the range defined by the load type parameter template; when Jr(k) is higher than the safety threshold Jsafe for N consecutive control cycles, and at least one of the following is detected simultaneously: sensor abnormality, drive execution deviation abnormality, or continuous external disturbance, the degradation control described in claim 7 is entered.

7. The anti-sway motion control method for a unibody composite robot according to claim 6, characterized in that, The controller synchronously acquires the characteristics of future path segments when receiving task trajectory instructions; when sharp turns, slopes, seams, speed bumps, or narrow passages are identified, the sampling frequency of the sway state is increased in advance and / or the update cycle of the input shaping parameters is shortened; the degradation control includes at least one of freezing the visual feedback branch and maintaining the inertial-force closed loop, reducing the maximum linear velocity and maximum angular velocity of the chassis and the maximum velocity of the robotic arm joints, performing short-term holding control based on the most recent effective sway feedback state, and triggering chassis slow-stop or whole-machine shutdown protection; when the visual confidence, inertial noise intensity, force residual, and residual sway amount recover to the corresponding threshold range within consecutive Mrec control cycles, the system exits the degradation control and resumes the dual-branch linkage update as described in claim 1.

8. A motion control system for an embodied composite robot to prevent swaying, characterized in that, It includes a controller, mobile chassis, robotic arm, end effector, end inertial measurement unit, six-dimensional force sensor, vision sensor, path planning interface, chassis driver, and robotic arm joint driver; The end-effector inertial measurement unit, the six-dimensional force sensor, and the vision sensor are connected to the controller and share a unified time reference or timestamp synchronization mechanism with the controller. The path planning interface is used to input task trajectory instructions and future path segment characteristics to the controller. The controller includes a time alignment module, a sway state construction module, a multi-source fusion estimation module, a fusion weight update module, an input shaping module, a chassis-robotic arm collaborative compensation module, a path segment pre-adjustment module, a residual sway evaluation and parameter linkage update module, a drive feedback monitoring module, and a safety degradation control module. The time alignment module is used to perform timestamp alignment or extrapolation compensation on asynchronously sampled inertial, force, and visual data, and output the aligned data to the shaking state construction module. The sway state construction module is used to extract load swing angle, swing angle change rate, equivalent excitation frequency and centroid offset information based on the aligned data, construct a sway state vector for the actual sway response at the load interface, and output the sway state vector to the multi-source fusion estimation module. The multi-source fusion estimation module and the fusion weight update module are used to map the visual estimation state, inertial estimation state and force estimation state to a unified state space and then perform fusion estimation to obtain the sway feedback state. The adaptive weights are updated according to the visual confidence, inertial noise intensity and force residual. The sway feedback state is output in parallel to the input shaping module and the chassis-robotic arm collaborative compensation module. The input shaping module is used to generate input shaping compensation instructions based on the sway feedback state and task trajectory instructions, and update the input shaping parameters according to the damping characteristics, oscillation decay time or main frequency offset identified in the most recent preset control cycle. The chassis-robotic arm collaborative compensation module is used to generate chassis micro-translation compensation amount, robotic arm end pose compensation amount and robotic arm joint compensation amount mapped from the robotic arm end pose compensation amount based on the sway feedback state. The chassis compensation allocation coefficient is determined together based on the passable space index and the robotic arm joint margin index. The path segment pre-adjustment module is used to pre-adjust the swaying state sampling frequency and the input shaping parameter update cycle according to the characteristics of the future path segment; The residual sway assessment and parameter linkage update module is used to calculate the residual sway amount based on the sway response at the controlled load interface, the end position deviation, and the force oscillation deviation, and uses the residual sway amount as the common feedback trigger amount of the input shaping module and the chassis-robotic arm collaborative compensation module; when the residual sway amount is higher than the stability threshold Jstb and lower than or equal to the safety threshold Jsafe, the multi-source fusion adaptive weight, input shaping parameters, chassis compensation allocation coefficient, and compensation gain corresponding to chassis compensation and robotic arm compensation are linked and updated; when the residual sway amount is higher than the safety threshold Jsafe for N consecutive control cycles and there are abnormal detection results, a degradation trigger signal is output to the safety degradation control module. The drive feedback monitoring module is used to collect the execution status feedback of the chassis drive and the robotic arm joint drive, and combine visual confidence, inertial noise intensity and force residual to generate the anomaly detection result; The safety degradation control module is used to perform degradation control when receiving the anomaly detection result and the degradation trigger signal. The degradation control includes at least one of freezing the visual feedback branch and maintaining the inertial-force perception closed loop, reducing the speed limit, performing short-term hold control, or triggering slow stop / shutdown protection.

9. A hybrid robot, characterized in that, The system includes a mobile chassis, a robotic arm, an end effector, and the anti-sway motion control system for the embodied composite robot as described in claim 8; wherein the mobile chassis, robotic arm, and end effector are all controlled and connected to the controller, and the end inertial measurement unit, six-dimensional force sensor, and vision sensor are installed at the end of the robotic arm or rigidly connected to the end effector to perform closed-loop suppression of swaying at the load interface.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the anti-sway motion control method for the embodied composite robot according to any one of claims 1 to 7.

Images