A method and system for compliant control of an industrial robot without external force sensor based on enhanced extended Kalman filter and offline optimal scheduling

Abstract

The application relates to a compliant control method and system of an industrial robot without an external force sensor. The specific steps comprise the following: fusing motor current and reducer deformation double observation information, synchronously estimating joint external torque and environment stiffness by using an enhanced extended Kalman filter; according to the estimated environment stiffness, mapping online scheduling of admittance control parameters through pre-computed parameters; constructing an admittance control model in the normal direction and introducing adaptive compensation to correct environment deviation; designing a sliding mode controller in the tangential direction to realize robust tracking and dynamic decoupling; synthesizing normal and tangential control instructions, generating joint torque to drive the robot to work through inverse kinematics and dynamics calculation. The application is suitable for force control and trajectory tracking tasks such as precise assembly, curved surface polishing and deburring of industrial robots, and can significantly improve force control accuracy, environment adaptability and system real-time performance under the condition of no external force sensor through multi-source information fusion, environment adaptive parameter scheduling and task space decoupling control.

Description

Technical Field

[0001] This invention relates to the field of industrial robot control technology, and in particular to a compliant control method and system for industrial robots without external force sensors. Background Technology

[0002] Industrial robot technology plays an increasingly important role in complex tasks involving interaction with the environment, such as grinding, polishing, and assembly. In these contact-based operations, industrial robots require compliant control capabilities, meaning they must maintain the desired contact force or torque while achieving precise trajectory tracking. Traditional compliant control relies on force / torque sensors mounted at the end effector of the industrial robot to provide direct feedback. However, this not only increases system cost and hardware complexity but may also reduce system dynamic performance and structural stiffness due to the added mass and flexibility. Therefore, developing compliant control technology that does not require external force sensors is of great significance for improving the economy, reliability, and adaptability of industrial robot systems.

[0003] Currently, the core challenge of compliant control without external force sensors lies in how to accurately and robustly estimate the interaction torque between the end effector of an industrial robot and the environment, as well as the dynamic characteristics of the environment. Existing methods typically estimate torque and environmental parameters through dynamic models based on information such as motor current and joint position. However, the coupling effect of the nonlinear characteristics of the transmission links and environmental disturbances means that the estimation results of single data sources or simple linear models still contain errors, especially in high-dynamic or variable-stiffness contact scenarios where accuracy drops significantly. In addition, industrial robots need to simultaneously achieve force control in the contact normal direction and high-precision trajectory tracking in the tangential plane. Traditional unified control frameworks struggle to achieve high-precision trajectory tracking while ensuring force tracking accuracy, necessitating a compliant control strategy that can achieve task-space decoupling.

[0004] Environmental stiffness is a key dynamic parameter affecting the interaction performance of industrial robots, and its variation makes it difficult for fixed controller parameters to maintain optimal performance. Furthermore, the precise location of the environmental surface in actual tasks is often unknown or variable, leading to a steady-state deviation between the expected contact force and the actual contact force. Therefore, it is necessary to address the challenges of simultaneously and accurately estimating external torque and contact stiffness during robot-environment interaction, as well as the dynamic nature of the environment. This paper proposes a sensorless compliant control method for industrial robots that integrates enhanced extended Kalman filtering and online scheduling using an offline optimal mapping table. This method achieves high-precision synchronous estimation of interaction torque and environmental stiffness, as well as adaptive optimal adjustment of controller parameters, thereby improving the compliant control performance and stability of the system in unknown or variable stiffness environments. Summary of the Invention

[0005] To address the problems of insufficient torque and stiffness estimation accuracy, poor environmental adaptability, and difficulty in achieving real-time optimal control in existing sensorless compliant control methods, this invention provides a sensorless compliant control method and system for industrial robots based on enhanced extended Kalman filtering and offline optimal scheduling. The method first designs an enhanced extended Kalman filter, fusing multi-source information such as motor current and torsional deformation of the harmonic reducer to simultaneously estimate the external torque at the end effector of the industrial robot and the stiffness of the contact environment. Then, based on the estimated environmental stiffness, it queries a controller parameter mapping table generated offline according to the robot-environment coupled dynamics model and the optimal criterion of the linear quadratic regulator, and schedules the optimal admittance control parameters online. In the task space, the contact dynamics are decoupled, implementing adaptive admittance force control in the normal direction and sliding mode trajectory tracking control in the tangential direction, achieving dynamic decoupling of force control and trajectory tracking. This method significantly improves the force tracking accuracy, trajectory tracking performance, and overall stability of industrial robots in environments with unknown or time-varying stiffness while ensuring real-time computation.

[0006] To achieve the above objectives, the technical solution provided by this invention is as follows:

[0007] A sensorless compliant control method and system for industrial robots based on enhanced extended Kalman filtering and offline optimal scheduling, characterized by comprising:

[0008] Step S1: By fusing the dual observation information of motor current dynamics and reducer deformation, the joint external torque and environmental stiffness are estimated simultaneously using an enhanced extended Kalman filter.

[0009] Step S2: Based on the estimated environmental stiffness, the admittance control parameters are scheduled online through the pre-calculated optimal parameter mapping table, and a lookup table method is used to reduce the computational load when the stiffness changes gradually.

[0010] Step S3: Construct an admittance control model in the normal direction, convert the force tracking error into a position adjustment amount, and introduce an adaptive compensation mechanism to correct the deviation caused by unknown or changing environmental position.

[0011] Step S4: Design a sliding mode controller in the tangential direction to achieve robust trajectory tracking by suppressing chattering, and achieve dynamic decoupling from the normal direction control.

[0012] Step S5: Synthesize normal and tangential control commands, generate joint torques including force compensation through inverse kinematics and dynamics calculations, and drive the industrial robot to complete compliant operations.

[0013] Preferably, step S1 involves: constructing an observer A based on motor current and an observer B based on the torsional deformation of the harmonic reducer, respectively.

[0014] For observer A, based on the n-degree-of-freedom robot dynamics model, the joint driving torque is calculated from the motor current, and then the external joint torque is estimated. Its calculation formula is:

[0015]

[0016] Where, τ m =K i ·i represents the motor output torque. Let M(q) be the joint friction torque. G(q) represents the inertia matrix, the Coriolis force matrix, and the gravitational torque vector, respectively.

[0017] For observer B, based on the torsion angle Δθ = θ - q of the harmonic reducer (where θ is the motor-side angle and q is the output-side angle) and its nonlinear stiffness model, the transmission-side torque τ of the harmonic reducer is calculated. fs Then, estimate the external joint torque T. ext，t The calculation formula is as follows:

[0018]

[0019] Where, τ f The joint friction torque is calculated using the same friction model as observer A but estimated independently; Δθ error K1 represents the kinematic error of the harmonic reducer; K2 and K1 are the coefficients of the nonlinear stiffness model.

[0020] The outputs τ of the two observers ext，m With τ ext，t As independent observables that are complementary in the frequency domain, an enhanced extended Kalman filter is designed for fusion. The filter state vector is defined as follows:

[0021]

[0022] For the i-th joint, the state vector is denoted as

[0023]

[0024] Where θ m,i and These represent the angular displacement and angular velocity of the motor side, θ. l，i and These are the angular displacement and angular velocity on the load side, respectively, T ext，i For external torque, K e，i For environmental stiffness.

[0025] By updating the state covariance matrix online and calculating the Kalman gain, a high-precision estimate of the joint external torque is output after fusion. Compared with the estimated environmental stiffness Finally, the joint torques are mapped to estimated end-effector contact forces using the robot's Jacobian matrix J(q).

[0026]

[0027] Environmental stiffness K e The observability is guaranteed by relating the contact deformation Δx between the robot end effector and the environment to the contact force F. e The linear relationship F between them e =K e • Δx is incorporated as an observation equation into the update process of the enhanced extended Kalman filter (EKF), thereby utilizing the coupling relationship between force and deformation to achieve an assessment of the environmental stiffness K. e With contact force F e The joint estimate.

[0028] Preferably, step S2 involves: in the Cartesian task space, based on the current contact environment stiffness estimate obtained in step S1. Query the controller parameter mapping table that has been pre-calculated offline based on the robot-environment coupled dynamics model and the linear quadratic regulator (LQR) optimality criterion. Online scheduling of optimal admittance control parameters under the current environment, including virtual mass M d With virtual damping Dd; when the rate of change of the stiffness estimate Below the preset threshold δ K In this case, a lookup table method or linear interpolation method can be used to directly retrieve the data from the mapping table. The parameters are obtained from the mapping table. This is generated by solving the following LQR optimal control problem.

[0029]

[0030] subject to:

[0031] Where x is the displacement vector of the robot end effector in the contact normal direction, u is the control input, Q and R are weight matrices, and K... e =diag(K) e ) represents the environmental stiffness matrix;

[0032] By analyzing the stiffness K under different environments e Solving the corresponding algebraic Riccati equation yields the optimal state feedback gain matrix.

[0033] K LQR (K e )=[K p (K e ), K v (K e )]

[0034] in, These are the position and velocity feedback gains, respectively. Based on the equivalent relationship between the optimal state feedback gain and admittance parameter in LQR, the optimal admittance model parameters can be analytically derived.

[0035]

[0036] Among them, M eq and B eq Let K be the equivalent natural inertia and damping coefficient of the robot's end effector in the normal direction. p (K e ) and K v (K e ) represents the optimal position and velocity feedback gain for LQR.

[0037] This mapping relationship is based on the admittance model. Wherein, the state vector With impedance control closed loop The inverse equivalence in the sense of force-motion transfer function is derived and discretized and stored as a mapping table. Used for online real-time parameter scheduling.

[0038] When the rate of change of the stiffness estimate Below the preset threshold δ K In this case, a lookup table method or linear interpolation method can be used to directly retrieve the data from the mapping table. Parameters are obtained from the data; when the rate of change of the stiffness estimate is higher than or equal to the threshold δ. K At this time, parameter updates are paused and the parameter values ​​from the previous time step are maintained. Simultaneously, the process noise covariance of the enhanced extended Kalman filter is adaptively increased to accelerate the convergence estimation of the stiffness of the new environment.

[0039] Preferably, step S3 involves: establishing a moving target coordinate system in the task space with the contact point normal direction as the normal, and based on the current environmental stiffness estimate obtained in step S2... and the corresponding optimal admittance parameter pair This is then updated as the current admittance control parameter.

[0040] An admittance control model is constructed based on this parameter.

[0041]

[0042] in This is the correction amount for the reference position of the normal scalar. F is the optimal admittance parameter obtained in step S2. d For the desired contact force, The normal contact force estimated in step S1;

[0043] Introducing an adaptive law based on the force tracking error integral term

[0044]

[0045] The reference position deviation caused by unknown or changing environmental surface position is estimated online; finally, the position correction amount generated by admittance control is superimposed with the adaptive compensation amount to obtain the normal position adjustment command.

[0046] x n，cmd =x c +δ

[0047] To achieve high-precision tracking of normal contact force and online compensation for unknown environmental positions, the scalar command will be converted into a three-dimensional position adjustment vector in step S5.

[0048] Preferably, step S4 involves defining the trajectory tracking error of the robot's end effector in the tangential direction within the task space tangential coordinate system as:

[0049] e act =x act -x act,d

[0050] And construct the sliding surface

[0051]

[0052] in, It is a positive definite diagonal matrix;

[0053] The tangential space dimension 5 corresponds to the remaining degrees of freedom (3D posture + 2D tangential translation) of the 6-DOF robot end effector under contact constraints. The dimension is adjusted accordingly for different degrees of freedom robots or task constraints.

[0054] Based on tangential dynamics model

[0055]

[0056] Design a tangential sliding mode controller with control input F. act By switching control item -K sat sat(s act / Φ), equivalent control items and normal-tangential dynamic coupling feedforward compensation term Together they constitute. Among them, K... sat The gain matrix is ​​ss, sat(·) is the saturation function, and Φ is the boundary layer thickness vector; this controller switches the gain matrix via the sliding surface ss. actA reaching law is applied to ensure that the system state reaches the sliding surface within a finite time, thereby achieving fast and accurate trajectory tracking; while the normal-tangential dynamic coupling feedforward compensation term... It is specifically designed to compensate for the off-diagonal block coupling effect of the Coriolis force matrix caused by the selection of task space coordinates, effectively improving the control performance of the system.

[0057] Normal force control and tangential trajectory control are decoupled at the kinematic level through orthogonal coordinate selection, and at the dynamic level through the aforementioned feedforward term to achieve active compensation of residual dynamic coupling. Together, they constitute a complete decoupled control strategy.

[0058] Preferably, step S5 involves: adjusting the scalar normal position generated in step S3 within the task space. Convert to normal position adjustment vector The desired tangential pose is obtained from the original trajectory planning or the output of step S4. To synthesize, where For tangential plane position, As the attitude, it constitutes the complete expected pose trajectory in Cartesian space. Its position component p d satisfy

[0059] p d =p t -R t (q)·Δp n

[0060] Where, p t =[x t y t ,0] T For the desired tangential position (normal component initially zero), Δp n Containing only the z-axis component, the sum of the two yields the complete desired three-dimensional position, R. t (q)∈SO(3) is the rotation matrix from the target coordinate system of the contact point movement established in step S3 to the robot base coordinate system, which is obtained by calculating the current joint angle q through forward kinematics; the desired posture φ d Take it directly as φ t .

[0061] Subsequently, the desired pose x is obtained by solving the robot's inverse kinematics. d Convert into desired angle commands for each joint. The solution process must satisfy the kinematic constraints and joint limits of the robotic arm.

[0062] Finally, the required joint driving torque is calculated based on the robot dynamics model.

[0063]

[0064] Where M(q) is the inertia matrix, Let G(q) be the Coriolis and centrifugal force matrix, and G(q) be the gravity term. Let J(q) be the frictional torque, and J(q) be the Jacobian matrix of the robotic arm. The contact force estimated in step S1 is used to compensate for the effect of the contact force on the joint torque.

[0065] Finally, the calculated combined joint torque command τ is... cmd The data is sent to the underlying joint servo driver to drive the industrial robot to achieve high-precision, compliant interactive operations.

[0066] The multi-source information fusion and synchronous estimation module is configured to design an enhanced extended Kalman filter, use motor current and harmonic reducer torsional deformation data as observation input, and based on the robot joint dynamics and transmission nonlinear model, to estimate the external torque and contact environment stiffness of the industrial robot end effector online with high precision.

[0067] The optimal controller parameter mapping table generation and storage module is configured to calculate and generate an optimal controller parameter mapping table indexed by environmental stiffness offline based on the robot-environment coupled dynamics model and the optimal criterion of the linear quadratic regulator, and then store it.

[0068] The online parameter scheduling and compliant control module is configured to receive the real-time environmental stiffness estimate and external torque estimate output by the synchronous estimation module, query the parameter mapping table to dynamically schedule the corresponding optimal control parameters online, perform normal admittance control based on the scheduled parameters, and perform tangential sliding mode robust tracking control based on the estimated external torque, so as to realize high-precision force tracking and trajectory tracking of industrial robots in unknown or time-varying stiffness environments.

[0069] The estimation-control closed-loop performance enhancement module is configured to monitor force tracking error and state estimation residual in real time, construct performance indicators, dynamically adjust the process noise and observation noise covariance matrix of the enhanced EKF based on these indicators, and fine-tune the query interpolation strategy of the parameter mapping table online to suppress model mismatch and external disturbances, thereby improving the robustness and adaptability of the entire estimation-control closed-loop system.

[0070] Compared with the prior art, the present invention has the following beneficial effects:

[0071] 1. This invention achieves synchronous, high-precision online estimation of the external torque and contact environment stiffness of the industrial robot's end effector by designing an enhanced extended Kalman filter and fusing multi-source information such as motor current and torsional deformation of the harmonic reducer. Based on this estimation result, it queries an offline pre-generated controller parameter mapping table according to the robot-environment coupled dynamics model and LQR optimality criterion, realizing real-time optimal scheduling of controller parameters. This scheme transforms the complex optimal control problem into an efficient online query problem, effectively solving the problems of heavy online computation burden, poor real-time performance, and decreased control performance under model mismatch conditions in traditional methods while ensuring optimal system control performance.

[0072] 2. The enhanced extended Kalman filter designed in this invention integrates a joint dynamics model that considers transmission nonlinearity and incorporates motor current information to construct a more comprehensive observation model. This design effectively overcomes the problem of insufficient accuracy caused by nonlinear factors such as friction and hysteresis, as well as environmental disturbances, when relying solely on current or a single model for estimation. It significantly improves the accuracy, convergence speed, and robustness of estimating interactive torque and environmental dynamic characteristics in unknown, time-varying, or complex contact environments.

[0073] 3. This invention provides a complete compliant control solution that does not rely on external force / torque sensors. Through algorithmic innovation, this method achieves force tracking and environmental adaptability close to sensor-based solutions without increasing hardware costs, altering the industrial robot's structure, or introducing additional flexible elements. This provides a lower-cost, easier-to-deploy, and more reliable technical path for industrial robots in tasks requiring high dynamics and variable stiffness force interaction, such as grinding, polishing, and precision assembly, and has significant engineering application value. Attached Figure Description

[0074] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings of the embodiments will be briefly described below.

[0075] Figure 1 This is a schematic diagram of a compliant control method for an industrial robot without an external force sensor provided in an embodiment of the present invention;

[0076] Figure 2 This is a control block diagram of a compliant control method for an industrial robot without an external force sensor, provided in an embodiment of the present invention. Detailed Implementation

[0077] The following describes the implementation of the present invention through specific embodiments and in conjunction with the accompanying drawings.

[0078] To address the problems of insufficient torque and stiffness estimation accuracy, poor environmental adaptability, and difficulty in achieving real-time optimal control in existing compliant control methods without external force sensors, this invention provides a compliant control method and system for industrial robots without external force sensors. It integrates multi-source information such as motor current and torsional deformation of harmonic reducers to achieve synchronous high-precision estimation of external torque and environmental stiffness, and schedules controller parameters online based on an offline optimal mapping table, enabling industrial robots to achieve high-precision force tracking and trajectory tracking in unknown or time-varying stiffness environments.

[0079] Figure 1 This is a schematic diagram of a sensorless industrial robot compliant control method provided in an embodiment of the present invention. Figure 2 The control block diagram of the compliant control method for an industrial robot without external force sensors provided in this embodiment of the invention is described in detail below.

[0080] The present invention provides a sensorless compliant control method and system for industrial robots based on enhanced extended Kalman filtering and offline optimal scheduling, comprising:

[0081] Design an enhanced extended Kalman filter (EKF) to fuse motor current and harmonic reducer torsional deformation data to simultaneously estimate the external torque and contact environment stiffness of the industrial robot end effector.

[0082] Before establishing the enhanced EKF, two complementary external torque observers are first constructed.

[0083] Observer A, based on the motor current and the complete dynamics model of the robot, calculates estimated joint torques using measurements from the motor-side encoder, reflecting the overall dynamic response of the motor and load, and is sensitive to low-frequency dynamics. Based on the n-DOF robot dynamics model, it calculates the joint driving torque from the motor current, and then estimates the external joint torque. The motor output torque is...

[0084] τ m =K t,i I m，i

[0085] Among them, K t，i I is the motor torque constant. m，i This represents the motor current.

[0086] Consider robot dynamics model

[0087]

[0088] Where M(q) is the inertia matrix, Let G(q) be the Coriolis force matrix and G(q) be the gravitational torque vector. Given the joint friction torque, the estimated external torque output by observer A is...

[0089]

[0090] The observer is sensitive to low-frequency dynamics, but is greatly affected by model uncertainties.

[0091] Observer B, based on the torsional deformation of the harmonic reducer, uses the differential measurement values ​​of the encoders on the motor side and the load side to independently calculate the torque estimate on the transmission side, reflecting the local deformation characteristics of the transmission link, and is sensitive to high-frequency disturbances.

[0092] Define the torsional deformation angle of the harmonic reducer

[0093] Δθ=θ m -N·θ l

[0094] Where, θ m θ is the angle on the motor side. l Where is the load-side angle, and N is the reduction ratio.

[0095] Based on the nonlinear stiffness model of the harmonic reducer, the transmission side torque is:

[0096]

[0097] Where K1 and K2 are the coefficients of the nonlinear stiffness model, and D is the damping coefficient.

[0098] Therefore, the estimated external torque output by observer B is...

[0099] τ ext，t =τ fs -τ fric,B -τ error

[0100] Where τ fric,B For the joint friction torque (using the same friction model as observer A but estimated independently), T error This refers to the kinematic error of the harmonic reducer.

[0101] This observer is sensitive to high-frequency disturbances and can capture the hysteresis and nonlinear stiffness characteristics unique to harmonic reducers.

[0102] Observer A and Observer B have complementary characteristics in the frequency domain. Observer A has a high signal-to-noise ratio in the low-frequency band but is affected by model error in the high-frequency band. Observer B has a fast response in the high-frequency band but drifts in the low-frequency band. Therefore, by fusing the outputs of the two observers as independent observation sources, high-precision torque estimation across the entire frequency band can be achieved.

[0103] Taking the i-th joint of the robot as an example, a discrete-time state-space model is established that integrates joint dynamics, nonlinear deformation of the harmonic reducer, and environmental interaction.

[0104] Define the state vector as

[0105]

[0106] Where θ m，i and These represent the angular displacement and angular velocity of the motor side, θ. l，i and These represent the angular displacement and angular velocity on the load side, respectively, τ ext，i Ke is the equivalent component of the external torque acting at the end point on joint i. ，i Let be the equivalent stiffness of the environment in the direction of joint i.

[0107] The discrete-time nonlinear state equation of the system is

[0108] x i (k+1)=f(x i (k), u i (k))+w i (k)

[0109] Where u i (k)=[I m,i [(k)] represents the motor current input at time k, w i (k) represents the process noise, which is modeled as zero-mean Gaussian white noise, and its covariance matrix is ​​Qi.

[0110] The nonlinear function f(·) is integrated by the following dynamic sub-models, and the motor-side dynamics are as follows:

[0111]

[0112] J m，i K represents the total inertia of the motor rotor and the input side of the reducer. t，i T is the motor torque constant. h，i B is the torque transmitted by the harmonic reducer. m,i This is the viscous damping coefficient on the motor side.

[0113] The nonlinear torsional deformation model of the harmonic reducer considers stiffness nonlinearity and hysteresis effects, and the transmission side torque.

[0114]

[0115] Where Δθ=θ m -N·θ l Where is the torsional deformation angle, N is the reduction ratio, K1 and K2 are the coefficients of the nonlinear stiffness model, and D is the damping coefficient.

[0116] The load-side dynamics and environment interaction model is

[0117]

[0118] J l,i B is the equivalent inertia on the load side. l，i The load-side damping coefficient is given by the external torque T. ext，i With environmental stiffness K e，i The relationship is given by the linear elastic contact model.

[0119] τ ext，i =K e，i (θ l，i -θ e,i )

[0120] Where, θ e，i This represents the position of the environment when it is undeformed.

[0121] The system's observation model is based on the outputs of two observers and the directly measurable motor current and harmonic torsional deformation angle. The observation vector is defined as follows:

[0122]

[0123] in The external torque estimate calculated by observer A based on the motor current and dynamic model. For observer B, the external torque estimate is calculated based on the torsional deformation of the harmonic reducer. m，i (k) represents the measured value of the motor current, Δθ h，i (k) is the measured value of the torsional deformation angle of the harmonic reducer, v i (k) represents the observation noise, assumed to be zero-mean Gaussian white noise, with its covariance matrix R. i .

[0124] The specific form of the observation function h(·) is:

[0125]

[0126]

[0127] h3(x)=I m，i

[0128] h4(x)=Δθ h，i =θ m，i -N i θ l，i

[0129] By incorporating the torque estimates of both observers into the observation vector, the enhanced EKF can fully utilize the complementary characteristics of the two in the frequency domain. Through online adjustment of the Kalman gain, it automatically weights and fuses low-frequency and high-frequency information to achieve estimation accuracy superior to that of a single observer.

[0130] Based on the above model, the enhanced EKF is recursively estimated according to the following standard steps, and the state prediction is as follows:

[0131]

[0132] Error covariance prediction is

[0133]

[0134] in Let be the Jacobian matrix of the state transition function;

[0135] Kalman gain is calculated as

[0136]

[0137] in Let be the Jacobian matrix of the observation function;

[0138] Status updated to

[0139]

[0140] Error covariance updated to

[0141] P i (k|k)=[IK i (k)H i (k)]P i (k|k-1)

[0142] In the above EKF recursion process, the Kalman gain K i The calculation of (k) automatically achieves the optimal fusion of the outputs of observer A and observer B.

[0143] Error covariance matrix P i (k|k-1) and observation noise covariance R i The relative magnitudes of the two observers determine their weights in the state update, where the observation matrix H... i (k) contains the Jacobian matrix block for the outputs of the two observers.

[0144] Through online updates, EKF can adaptively adjust the estimation based on the current estimation uncertainty and observation noise level. and The trust weights are determined so that the system mainly relies on observer A in the low-frequency band and mainly relies on observer B in the high-frequency band, thereby achieving optimal torque estimation across the entire frequency band.

[0145] Through this enhanced EKF online recursion, the state vector can be obtained in real time. The optimal estimate. Extract from it. and This refers to the fused high-precision joint external torque estimate and environmental stiffness estimate.

[0146] This estimation result integrates information from observer A based on motor current and observer B based on harmonic deformation. Compared with a single observer, it offers advantages such as improved estimation accuracy, especially in the transition frequency band; faster convergence speed to sudden changes in environmental stiffness; and stronger robustness to model uncertainty and measurement noise. For multi-joint robots, the estimated values ​​of each joint can be mapped to Cartesian space using the robot's Jacobian matrix J(q) to obtain the total external torque acting on the robot's end effector. and contact environment stiffness matrix The estimation provides crucial information for subsequent online scheduling of controller parameters.

[0147] Based on the time-varying environmental stiffness estimated in step S1 To achieve optimal tracking control of the contact force in the normal direction, this step uses the robot-environment coupled dynamics model and the linear quadratic regulator (LQR) design criterion to calculate and generate the optimal controller parameter mapping table offline, and then queries and schedules it online.

[0148] First, a linearized robot-environment coupled dynamics model is established along the normal direction n of the contact surface. Assuming the contact is point contact and the environment is a linear elastic body, the coupled dynamics along the normal direction can be described by the following second-order system.

[0149]

[0150] Where, m n and b n Here, x represents the equivalent inertia and damping coefficient of the robot's end effector in the normal direction. n For the normal position, f c f represents the normal component of the control force generated by the robot joints. ext，n It is the normal contact force.

[0151] According to Hooke's Law, the contact force is related to the environmental stiffness K. e，n The relationship between the positions is

[0152] f ext，n =K e，n (x n -x e，n )

[0153] Where x e，n This represents the location when the environment is undeformed.

[0154] Define state vector

[0155]

[0156] Where x d,n To correspond to the desired contact force f d，n The expected position (by f) d，n =K e，n (x d,n -x e,n (Determination of static relationships).

[0157] Control input is

[0158] u = f c -f d，n

[0159] That is, the amount of change in control force around the desired force.

[0160] Linearizing the system at the equilibrium point (x = 0, u = 0) yields the standard state-space equations.

[0161]

[0162] in,

[0163]

[0164] To design a controller that optimizes both contact force tracking error and energy consumption, the following quadratic performance index is defined.

[0165]

[0166] Where Q = diag(q1, q2) is a semi-positive definite state weighting matrix used to penalize position error and velocity; R > 0 is a positive definite control weighting matrix used to limit the change of control force.

[0167] For a given environmental stiffness K e，n Optimal state feedback control law u * =-K lqr x can be obtained by solving the corresponding continuous-time algebraic Riccati equation.

[0168] A T P+PA-PBR -1 B T P+Q=0

[0169] The optimal feedback gain matrix is

[0170] K lqr=R -1 B T P

[0171] Here, P is a positive definite solution to the Riccati equation. This gain matrix is ​​directly related to the system matrix A, which in turn depends on the unknown environmental stiffness K. e，n .

[0172] To achieve online self-adaptation, the environmental stiffness K within the expected range needs to be determined in advance. e，n ∈[K min K max Discrete sampling is performed. For each sampling point... Solve the Riccati equations offline to calculate the corresponding optimal feedback gain. and the closed-loop system poles. The gain Or the equivalent admittance controller parameters calculated from it (such as the target inertia M) d Damping B d Stiffness K d Stored as a mapping table

[0173] During online operation, the environmental stiffness value is estimated in real time based on step S1. By looking up the table By employing linear interpolation, the optimal control parameters for the current moment can be obtained. This provides a basis for the online updating of the parameters of the normal adaptive admittance controller in step S3, ensuring that the system can achieve fast, stable and energy-efficient force tracking performance under different contact stiffnesses.

[0174] This method avoids the huge computational burden of solving the Riccati equations online, ensuring the real-time nature of the control.

[0175] Optimal state feedback gain based on scheduling in step S2 and the environmental stiffness estimated synchronously in step S1 With contact force A second-order target admittance dynamic model is constructed and updated online, and an adaptive law based on the force tracking error integral is designed to estimate the reference position deviation caused by the unknown or changing position of the environmental surface online.

[0176] The model will have the desired contact force f d With estimated contact force Error between Dynamically mapped to the robot end effector's position correction x in the normal direction. c .

[0177] The standard form of the admittance model is expressed in the Laplace domain as:

[0178] (Md s 2 +D d s+K d )X c (s)=ΔF(s)

[0179] Where M d D d K d These are the target inertia, damping, and stiffness parameters, which are the core parameters that need to be updated online.

[0180] To achieve adaptive adjustment based on the LQR optimal criterion, an analytical relationship is established between the admittance parameter and the optimal gain and environmental stiffness estimate obtained in step S2.

[0181] Let the optimal state feedback gain matrix be...

[0182] K = [K p K v ]

[0183] Where K p and K v These are the position and velocity feedback gains, respectively.

[0184] The formula for calculating the target admittance parameter can be obtained through the coefficient matching method.

[0185]

[0186] Where M eq and B eq Let be the equivalent natural inertia and damping coefficient of the robot end effector in the normal direction.

[0187] Meanwhile, to compensate for reference position deviations caused by unknown or changing environmental surface positions, an adaptive law based on the force tracking error integral term is designed.

[0188]

[0189] To estimate the reference position deviation caused by unknown or changing environmental surface location online; where K I This is the integral gain coefficient.

[0190] This adaptive law estimates the unknown deviation of the environmental surface position online by accumulating force tracking error.

[0191] The final position correction x generated by admittance control c When superimposed with the adaptive compensation amount δ, the normal position adjustment command x is obtained. n,cmd =x c +δ enables high-precision tracking of normal contact force and online compensation for unknown environmental positions.

[0192] During online operation, the following calculations are performed sequentially in each control cycle k: First, the current environmental stiffness estimate provided in step S1 is read. With contact force estimate Next, based on Query the optimal parameter mapping table maintained in step S2 Obtain the corresponding optimal gain Then, the updated admittance parameter M is calculated in real time using the above analytical relationship. d (k), D d (k), K d (k), and update the adaptive compensation amount.

[0193]

[0194] Where T s The sampling time is used. The continuous-time admittance model is discretized using the backward Euler method, and the difference equation is solved to obtain the position correction x at the current time. c (k)

[0195]

[0196] The adaptive compensation and admittance correction are superimposed to obtain the normal position adjustment command.

[0197] x n,cmd (k)=x c (k)+δ(k)

[0198] This yields the expected position after the normal vector is corrected.

[0199] x r (k)=x d (k)-x n,cmd (k)

[0200] This adaptive admittance controller ensures that the robot's end effector's dynamic compliant behavior in the normal direction always matches the changing environmental stiffness through online parameter updates and adaptive position compensation, and eliminates the impact of environmental position uncertainty on force tracking accuracy, thus providing core position commands for achieving high-precision contact force tracking.

[0201] Within the tangential space of the contact surface, the desired tangential trajectory of the robot's end effector is defined by the planning and denoted as . It includes five degrees of freedom: two-dimensional translation and three-dimensional pose in the tangential plane. The actual pose is denoted as...

[0202] The trajectory tracking error vector is defined as follows:

[0203] e t (t)=xd,t (t)-x t (t)

[0204] To suppress coupling disturbances that may arise from model uncertainties, unmodeled dynamics, and normal force interactions, an integral sliding surface is designed.

[0205]

[0206] Λ1 and Λ2 are positive definite diagonal matrices.

[0207] Considering the five tangential degrees of freedom (two-dimensional translation and three-dimensional pose), a simplified dynamic model of the robot's end effector in the tangential direction is presented.

[0208]

[0209] in Given the positive definite inertia matrix, This represents known terms such as Coriolis force, centrifugal force, and viscous damping. and This is the dynamic coupling term between normal and tangential motion. The lumped disturbance includes model parameter uncertainties, unmodeled dynamics, and coupling effects of normal and tangential motions, and is assumed to be bounded, i.e., ||d t ||≤D, The tangential control force to be designed.

[0210] To ensure that the system state reaches and remains on the sliding surface within a finite time, a sliding mode control law based on the reaching law is designed, employing an exponential reaching law.

[0211]

[0212] K1 and K2 are positive definite diagonal gain matrices.

[0213] To mitigate chattering, a saturation function sat(s / Φ) is used instead of the sign function, resulting in the final tangential robust trajectory tracking control law.

[0214]

[0215] By appropriately selecting the gain matrix and boundary layer thickness, this controller ensures that, under the presence of bounded disturbances, the tangential trajectory tracking error asymptotically converges to zero or a uniformly bounded small neighborhood, thereby achieving high-precision tangential trajectory tracking. This controller operates independently in the tangential space, decoupled from the adaptive admittance controller operating in the normal direction in step S3. Normal force control and tangential trajectory control are decoupled at the kinematic level through orthogonal coordinate selection, and at the dynamic level, the aforementioned feedforward compensation term achieves active compensation for residual dynamic coupling. Together, they constitute a complete decoupled control strategy, forming a hybrid force / position control architecture in the task space.

[0216] In the task space, based on the corrected normal position command x generated in steps S3 and S4 respectively. r (t) and the tangential trajectory command x for high-precision tracking t (t) is used to synthesize the complete desired pose of the robot end effector in Cartesian space.

[0217] First, a local coordinate system {C} is established at the contact point, with its z-axis aligned with the normal direction n of the contact surface, and its x-axis and y-axis located in the tangent plane and corresponding to the two orthogonal tangential unit vectors t1 and t2 defined in step S4.

[0218] Assume the original, unadmitted end-planned pose is represented in coordinate system {C} as follows:

[0219]

[0220] in For location, This is the attitude quaternion (or equivalent rotation matrix). The scalar position component of the original trajectory in the normal direction is... The tangential vector position component is

[0221]

[0222] The adaptive admittance controller output in step S3 is the normal position correction.

[0223] x c (t)=x d (t)-x r (t)

[0224] In coordinate system {C}, this correction directly applies to the normal position component. Simultaneously, the sliding mode controller in step S4 precisely tracks the desired tangential trajectory x. d,t (t), whose output is the tangential instruction x. t (t). Therefore, in the contact local coordinate system {C}, the fused desired position command The structure is as follows:

[0225]

[0226] in, The tangential position after fusion is directly given by the output of step S4; The normal position after fusion is obtained by subtracting the admittance correction from the original normal position.

[0227] For attitude commands, they are usually kept consistent with the original planned trajectory, i.e. To ensure the stability of the end effector's attitude relative to the contact surface, unless the task has specific attitude compliance requirements.

[0228] Next, the desired pose after fusion in the local coordinate system {C} needs to be... Transform back to the robot's base coordinate system {B} (or world coordinate system {W}). Let the homogeneous transformation matrix from coordinate system {C} to base coordinate system {B} be... B T C This matrix is ​​determined by the known or online estimated attitude of the contact surface.

[0229] The desired pose in the base coordinate system It can be calculated using the following formula

[0230] B T r (t)= B T C · c T r (t)

[0231] in, C T r (t) is derived from The homogeneous transformation matrix is ​​formed. The desired position in the base coordinate system is obtained from this. and expected posture (or the corresponding quaternion) ).

[0232] Finally, the Cartesian space desired pose command By solving the robot's inverse kinematics, the desired joint space angle command q is obtained. des (t)

[0233]

[0234] Where IK(·) represents the inverse kinematics solution function, which can be implemented using analytical methods, numerical iteration methods, or methods based on the Jacobian matrix.

[0235] The obtained joint angle command q des (t) and the expected joint velocity obtained by difference. acceleration The data is fed into the robot's underlying joint position / torque servo controller, which ultimately drives the motors of each joint of the robot to move, thereby achieving the complex interactive task of simultaneously satisfying normal contact force tracking and tangential trajectory tracking.

[0236] This step completes the closed loop from high-level task space instructions to low-level joint space instructions, and is the execution end of the entire control architecture.

[0237] Although exemplary embodiments of the present invention have been described in detail, this is not intended to limit the scope of the invention. Those skilled in the art can modify the invention in detail and form without departing from the scope and spirit disclosed in the claims. All such modifications should fall within the protection scope of the present invention, and the scope of the invention is not limited by the above technical solutions, but is defined by the claims.

Claims

1. A sensorless compliant control method for industrial robots based on enhanced extended Kalman filtering and offline optimal scheduling, characterized in that, include: Step S1: Construct two complementary external torque observers. Observer A is based on the motor current and the complete dynamic model of the robot. It uses the measured values ​​of the encoder on the motor side to calculate the estimated value of the joint torque, which reflects the overall dynamic response of the motor-load and is sensitive to low-frequency dynamics. Observer B is based on the torsional deformation of the harmonic reducer. It uses the differential measured values ​​of the encoders on the motor side and the load side to independently calculate the estimated value of the transmission side torque, which reflects the local deformation characteristics of the transmission link and is sensitive to high-frequency disturbances. For observer A, based on the n-degree-of-freedom robot dynamics model, the joint driving torque is calculated from the motor current, and then the external joint torque is estimated. Its calculation formula is: For observer B, based on the harmonic reducer twist angle Δθ=θ m -Nθ l Its nonlinear stiffness model is used to calculate the transmission-side torque τ of the harmonic reducer. fs Therefore, the external joint torque can be estimated. Its calculation formula is: The outputs of the two observers are treated as independent, complementary observations in the frequency domain. An enhanced extended Kalman filter is designed for fusion, and the filter state vector is defined as follows: By updating the state covariance matrix online and calculating the Kalman gain, a high-precision estimate of the joint external torque is output after fusion. Compared with the estimated environmental stiffness Finally, the joint torques are mapped to estimated end-effector contact forces using the robot's Jacobian matrix J. Step S2: Based on the environmental stiffness estimated in step S1 Query the controller parameter mapping table pre-calculated offline based on the robot-environment coupled dynamics model and the optimality criterion of the linear quadratic regulator. Online scheduling of optimal admittance control parameters under the current environment, including virtual mass M d Virtual damping D d With virtual stiffness K d The mapping table This is generated by solving the following LQR optimal control problem. subject to: By analyzing the stiffness K under different environments e Solving the corresponding algebraic Riccati equation yields the optimal state feedback gain matrix K. LQR =[K p K v Based on the equivalent relationship between the optimal state feedback gain and admittance parameters in LQR, the optimal admittance model parameters are analytically derived. Step S3: Establish a moving target coordinate system in the task space with the contact point normal direction as the normal. Based on the current optimal admittance parameter pair (M) obtained in step S2... d D d K d Construct an admittance control model Introducing an adaptive law based on the force tracking error integral term The final position correction x generated by admittance control c When superimposed with the adaptive compensation amount δ, the normal position adjustment command is obtained. x n,cmd =x c +δ Step S4: In the tangential coordinate system of the task space, define the trajectory tracking error of the robot's end effector in the tangential direction as e. t And construct the integral sliding surface Design a tangential sliding mode controller based on the tangential dynamics model. Normal force control and tangential trajectory control are decoupled at the kinematic level through orthogonal coordinate selection, and at the dynamic level, residual dynamic coupling is actively compensated through the aforementioned feedforward term. Step S5: Adjust the scalar normal position command x generated in step S3. n,cmd Convert to normal position adjustment vector Δp n = [0, 0, x n,cmd ] T The desired tangential pose x, as output by the original trajectory planning or step S4. t The composite material is then synthesized to form the complete desired pose trajectory X in Cartesian space. d By solving the robot's inverse kinematics, the desired pose is converted into the desired angle command q for each joint. d The required joint driving torque τ is calculated based on the robot dynamics model. Ultimately, this drives the motors of each joint of the robot to move, thereby achieving the complex interactive task of simultaneously satisfying normal contact force tracking and tangential trajectory tracking.

2. A sensorless compliant control system for industrial robots based on enhanced extended Kalman filtering and offline optimal scheduling, used to implement the sensorless compliant control method for industrial robots described in claim 1, characterized in that, include: The multi-source information fusion and synchronous estimation module is configured to design an enhanced extended Kalman filter, use motor current and harmonic reducer torsional deformation data as observation input, and based on the robot joint dynamics and transmission nonlinear model, to estimate the external torque and contact environment stiffness of the robot end effector online with high precision. The optimal controller parameter mapping table generation and storage module is configured to calculate and generate an optimal controller parameter mapping table indexed by environmental stiffness offline based on the robot-environment coupled dynamics model and the optimal criterion of the linear quadratic regulator, and then store it. The online parameter scheduling and compliant control module is configured to receive the real-time environmental stiffness estimate and external torque estimate output by the synchronous estimation module, query the parameter mapping table to dynamically schedule the corresponding optimal control parameters online, perform normal admittance control based on the scheduled parameters, and perform tangential sliding mode robust tracking control based on the estimated external torque, so as to realize high-precision force tracking and trajectory tracking of industrial robots in unknown or time-varying stiffness environments. The estimation-control closed-loop performance enhancement module is configured to monitor force tracking error and state estimation residual in real time and construct performance indicators. Based on this index, the process noise and observation noise covariance matrix of the enhanced EKF is dynamically adjusted, and the query interpolation strategy of the parameter mapping table is fine-tuned online to suppress model mismatch and external disturbances, thereby improving the robustness and adaptability of the entire estimation-control closed-loop system.

Images