A multi-sensor fusion control system for high-precision robot joints

Abstract

The application relates to the technical field of robot control, in particular to a multi-sensor fusion control system for high-precision robot joints, which comprises a marking unit, a time sequence unit, a sensor data acquisition unit, a preset unit and an execution monitoring unit; the marking unit carries out hierarchical marking rules based on a base, an arm part, an execution part and a joint node, and carries out standardized marking on the base, the multi-stage arm part, the execution part and the joint node between adjacent components; in the scheme, through a task type driven differentiated time sequence and fusion strategy, the adaptation of a repetitive task predefined time length and a non-repetitive task dynamically calculated time length is realized, and the accuracy and efficiency are improved in combination with Kalman filtering, particle filtering and other algorithms; through multi-sensor cooperative acquisition and full-process data association, the sensors are arranged according to the joint characteristics and the data marks are bound, so that the sensing accuracy and traceability are ensured; through dynamic feedback correction and threshold iterative optimization, the deviation is corrected in real time and the threshold is optimized in the long term.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a fusion control system for multiple sensors in high-precision robot joints. Background Technology

[0002] Multi-sensor fusion control of robot joints is a core technology for achieving precise movement and stable operation in high-precision robots (such as semiconductor packaging robots, surgical robots, and precision assembly robots). By integrating multi-dimensional sensor data such as joint torque, angular velocity, angle, and position, and combining fusion algorithms and control strategies, it achieves synergistic optimization of joint motion accuracy, dynamic response, and anti-interference capability. It is a key support for meeting high-end requirements such as ±0.003mm positional accuracy in semiconductor packaging and ±0.05N force control accuracy in surgical robots.

[0003] However, traditional robot joint control systems have significant shortcomings: First, they suffer from poor task adaptability, often employing a "fixed timing + single fusion algorithm," which fails to account for the differences between repetitive and non-repetitive tasks. Repetitive tasks (such as assembly line grasping) do not utilize historical data patterns, leading to a high error rate (often exceeding 5%). Non-repetitive tasks (such as assembling unknown workpieces) lack dynamic duration calculation mechanisms, making them prone to accuracy degradation (position errors exceeding ±0.01mm) due to redundancy or insufficiency in duration. Second, sensor data coordination is weak. Sensor deployment is not designed according to the differences in joint characteristics (e.g., using the same type of angular velocity sensor for the upper arm and end effector joints), and data is not bound to "time period-joint" tags, resulting in timestamp misalignment during acquisition (error exceeding 0.005s). This prevents the formation of a complete joint state perception chain, leading to low fault location efficiency (often requiring more than 30 minutes). Third, the accuracy and stability are insufficient. Deviation correction relies solely on real-time proportional adjustment and no long-term threshold optimization mechanism has been established. When the equipment ages (such as joint wear) or the operating conditions change (such as temperature fluctuations), the accuracy decays rapidly (annual decay rate exceeds 10%), and the abnormal response is delayed (exceeding 0.05s), making it difficult to meet the requirements of long-term high-precision operation.

[0004] To address this, this invention proposes a multi-sensor fusion control system for high-precision robot joints. Through "task-type-driven differentiated timing and fusion strategies," it adapts the predefined duration of repetitive tasks to the dynamic calculation duration of non-repetitive tasks, combining Kalman filtering, particle filtering, and other algorithms to improve accuracy and efficiency. Through "multi-sensor collaborative acquisition and full-process data association," sensors are deployed according to joint characteristics and data tags are bound to ensure accurate perception and traceability. Through "dynamic feedback correction and threshold iterative optimization," deviations are corrected in real time and thresholds are optimized over the long term, ultimately achieving a joint control accuracy of ±0.003mm, anomaly response time ≤0.01s, and an annual accuracy decay rate reduced to 4%, thus solving the problems of poor adaptability, ambiguous perception, and unstable accuracy in traditional systems. Summary of the Invention

[0005] Technical problems to be solved: The traditional system suffers from poor adaptability, ambiguous perception, and unstable accuracy, making it difficult to meet the requirements of high-precision joint control.

[0006] To address the shortcomings of existing technologies, this invention provides a fusion control system for multiple sensors in high-precision robot joints, thereby solving the technical problems mentioned in the background section.

[0007] To achieve the above objectives, the present invention provides the following technical solution:

[0008] A fusion control system for multiple sensors in high-precision robot joints;

[0009] It includes a marking unit, a timing unit, a sensor data acquisition unit, a preset unit, and an execution monitoring unit;

[0010] The marking unit uses hierarchical marking rules based on the base, arm, actuator, and joints to standardize the marking of the robot base, multi-level arm, actuator, and joints between adjacent components;

[0011] The timing unit determines the total task duration and divides the time periods corresponding to independent actions;

[0012] The sensor data acquisition unit collects joint sensing data at different time periods;

[0013] The preset unit identifies the task and plans the joint position changes for each time period;

[0014] The execution monitoring unit drives task execution, monitors sensor values, and performs time-period data fusion and comparison.

[0015] In one possible implementation, the timing unit determines the task duration R, which refers to the total time from the robot receiving the "task start command" to the "task completion command feedback," in seconds. The determination method can be either predefined or dynamically calculated.

[0016] For repetitive tasks, R is determined by averaging the duration of multiple tasks based on historical task statistics, with a deviation of ≤10%.

[0017] For non-repetitive tasks, R is calculated using the "Task Complexity Analysis Algorithm"; the algorithm inputs are "number of task steps and estimated difficulty coefficient of each step", and the output is... Step i Basic Duration Difficulty level i .

[0018] In one possible implementation, the time-segment data fusion comparison of the execution monitoring unit includes differential fusion of two task types:

[0019] For repetitive tasks with small data fluctuations and reproducible error patterns, a fusion algorithm of "Kalman filter (KF) + historical data weighting" is adopted, which focuses on comparing actual data with "theoretical value + historical average value" in two dimensions.

[0020] For non-repetitive tasks with large data fluctuations and no fixed error pattern, the "Particle Filter (PF) + Dynamic Weight Allocation" approach is adopted. It focuses on the real-time dynamic comparison between actual data and theoretical values, and pays special attention to sudden deviations.

[0021] In one possible implementation, the time-segment data fusion and comparison for repetitive tasks is as follows:

[0022] Data pool initialization: When the task is executed for the first time, sensor data for each time period of 20 consecutive cycles is collected, and the "historical average value μ and standard deviation σ" for each time period and each parameter are calculated to form an initial historical data pool; subsequently, the historical data pool is iteratively updated every 10 cycles, and new data is incorporated using the "weighted average method";

[0023] KF model fusion: Constructing a Kalman filter model using theoretical data as predictions and real-time data as observations, defining a state vector. ,in, For torque, Angular velocity, For joint angle, Given the location coordinates, the optimal actual value after fusion is calculated using the prediction equation and the update equation;

[0024] Two-dimensional comparison and threshold judgment: Calculate the absolute deviation between the optimal actual value and the theoretical value, and the deviation rate from the historical average, based on the standard deviation. A two-level dynamic threshold is set, including thresholds for normal torque and position coordinates, and deviation rate; the upper limit of the warning threshold deviation is twice the normal threshold, and the corresponding processing is triggered based on the threshold to determine whether it is normal, slightly deviated or abnormal.

[0025] In one possible implementation, the time-segment data fusion and comparison for non-repetitive tasks is as follows:

[0026] Particle filter fusion: defining the state vector X=[T ， w ， θ ， P]T Observation vector To preprocess the sensor data in real time, 1000 particles with a fluctuation range of ±5% based on the preset theoretical value are initialized for each time period. The particle weights are updated according to the Gaussian probability density function. After removing particles with a weight < 0.001, the weighted average is calculated to obtain the optimal actual value.

[0027] Segmented threshold settings: Threshold ranges are set for the position, torque, and angular velocity of the coarse adjustment period, the position, torque, and angular velocity of the fine adjustment period, and the torque and position of the actuator action period, respectively, according to the time period type;

[0028] Dynamic deviation comparison and anomaly judgment: Particle filter fusion is performed every 0.01s to calculate the deviation between the optimal actual value and the theoretical value and the deviation growth rate. If the deviation is ≤ the corresponding time period threshold and the deviation growth rate is ≤ 0.05mm / s, it is judged as normal. If the deviation exceeds the threshold but the growth rate is ≤ 0.05mm / s, it is judged as slow deviation and the parameters are fine-tuned. If the deviation exceeds the threshold or the growth rate is > 0.05mm / s, it is judged as abnormal and the backtracking is paused.

[0029] Beneficial effects compared to existing technologies:

[0030] 1. This solution achieves a dynamic balance between high precision and high efficiency through a task-type-driven differentiated timing and fusion strategy. The system designs an adaptive solution for the differences in characteristics between repetitive and non-repetitive tasks: for repetitive tasks, predefined durations are calculated using historical data (e.g., a predefined 8s duration for assembly line capture, with a deviation ≤10%), combined with Kalman filtering and weighted fusion of historical data, utilizing data patterns to reduce the error rate to below 0.5%; for non-repetitive tasks, the duration is dynamically calculated using "step base duration × difficulty coefficient" (e.g., 9.5s for an unknown assembly task), and particle filtering is used to capture parameter mutations. Simultaneously, timing is allocated according to "action range + precision requirements" (e.g., 4s for upper arm extension, 2s for forearm fine-tuning), avoiding resource waste and achieving joint control precision of ±0.003mm. Task execution efficiency is improved by 30% compared to traditional fixed timing solutions.

[0031] 2. In this solution, "multi-sensor collaborative acquisition and full-process data association achieve precise perception and traceability of joint status." The system selects four types of sensors—torque, angular velocity, angle, and position—and deploys them differently according to joint characteristics (magnetic-electric angular velocity sensors are used for vibration resistance in the boom, and photoelectric sensors are used for accuracy improvement in the end effector). It adopts a "time-period node triggering + 100Hz real-time acquisition" mode to ensure data integrity. All sensor data is bound to "time period ID + joint point marker," aligned with the preset theoretical data timestamp (error ≤ 0.001s), and stored in the "coordinate-marker-time series database" after fusion and comparison. This not only allows for real-time monitoring of joint torque, position, and other parameter deviations (e.g., immediate warning if torque deviation exceeds 0.3 N·m in time period R1), but also enables tracing historical data by task type and joint point, generating deviation trend reports (e.g., indicating joint wear), improving fault location efficiency by 60%.

[0032] 3. In this solution, "dynamic feedback correction and threshold iterative optimization" achieve continuous stability and long-term iteration of control accuracy. After system fusion and comparison, minor deviations are corrected in real time with "proportional compensation" (e.g., the command increases by 0.0012mm when the position deviation is 0.001mm) to avoid overshoot. Every 50 tasks, the threshold is optimized based on historical data (e.g., when 95% deviation ≤ 70% of the threshold, the threshold is lowered by 80% to increase accuracy), adapting to equipment aging and changes in operating conditions. For example, in repetitive tasks, the historical data pool is weighted and updated every 10 cycles (new data weight 0.3) to ensure accurate threshold calculation; for non-repetitive tasks, segmented thresholds are set according to coarse / fine adjustment periods (position deviation ≤ 0.002mm during fine adjustment periods), balancing accuracy and fault tolerance. Ultimately, the long-term accuracy decay rate of the system is reduced from the traditional 10% / year to 4% / year, and the abnormal response time is ≤ 0.01s. Attached Figure Description

[0033] The above description is merely an overview of the technical solution of the present invention. In order to better understand the technical means of the present invention and to implement it in accordance with the contents of the specification, the preferred embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0034] Figure 1 This is a schematic diagram of the overall structure of the present invention;

[0035] Figure 2 This is a system framework diagram of the present invention. Detailed Implementation

[0036] Preferred embodiments of the present invention will be described in detail with reference to the accompanying drawings. However, the present invention can also be implemented in various different forms, and therefore the present invention is not limited to the embodiments described below.

[0037] The technical solution in this application embodiment is to solve the problems mentioned in the background art, and the overall idea is as follows:

[0038] Example:

[0039] Please refer to Figure 1-2 As shown in the figure, this embodiment introduces a fusion control system for multiple sensors in high-precision robot joints, including:

[0040] The marking unit is based on "base (A) + arm (B) + actuator (C) + joint", where:

[0041] The base (A) is the fixed foundation of the robot, which includes rotational or translational joints (such as the base rotational joint of an articulated robot and the guide rail translational joint of a linear robot).

[0042] The arm (B) is the core component for transmitting motion and load, following a hierarchical sequence of motion transmission (first-level arm B1, second-level arm B2, third-level arm B3..., nth-level arm B). n The numerical subscripts strictly correspond to the "motion transmission priority" (i.e., B1 is the direct load transmission component of A, B2 is the load transmission component of B1, and so on).

[0043] The actuator (C) is the component that directly performs the task (gripper, screwdriver, suction cup, etc.). If there is a multi-actuator switching mechanism (such as a dual-gripper or a "gripper + screwdriver" quick-change mechanism), it is expanded to "C0, C1...C". m (m is the number of actuators); joint markers are represented by "a combination of markers of two adjacent components", in the format "(preceding component marker.subsequent component marker)";

[0044] For example, the joint between the base (A) and the first-stage arm B1 can be marked as (A.B1), the joint between the first-stage arm B1 and the second-stage arm B2 can be marked as (B1.B2), and the joint between the final-stage arm and the actuator (C) can be marked as (Bn.C).

[0045] Based on the above, for articulated robots (such as six-axis industrial robots), A is the base rotary joint, B1-B5 are multi-stage rotary / swing arms, B6 is the final stage arm, and C is the end effector. The joint points (A.B1), (B1.B2), ... (B6.C) correspond to the connection centers of each axis. For Cartesian coordinate robots (such as three-axis linear modular robots), A is the X-axis translation base, B1 is the Y-axis translation arm, B2 is the Z-axis translation arm, and C is the end effector gripper. The joint points (A.B1), (B1.B2), and (B2.C) correspond to the connection / motion centers of each axis guide rail and slider. For multi-actuator switching robots (such as flexible assembly robots), A is the fixed base, B1-B3 are multi-stage arms, C0 is the gripper, and C1 is the electric screwdriver. The joint points (B3.C0) and (B3.C1) correspond to the connection centers of different actuators and the final stage arm.

[0046] The timing unit first defines the task duration R, and then divides R into timing sequences:

[0047] Task definition (defining task duration R): Task duration R refers to the total time from "task start command trigger" to "task completion command feedback" for the robot, in seconds (s); its definition can be either predefined or dynamically calculated.

[0048] For repetitive tasks (such as material grabbing on an assembly line), R is determined by statistical analysis of historical tasks (taking the average duration of multiple tasks with a deviation of ≤10%); for example, if the average duration of grabbing the same material multiple times is 8s, then R is predefined as 8s.

[0049] For non-repetitive tasks (such as assembling unknown workpieces), R is calculated using the "Task Complexity Analysis Algorithm"; the algorithm inputs are "number of task steps and estimated difficulty coefficient of each step", and the output is... Step i Basic Duration Difficulty level i For example, if the assembly task includes "grab (base duration 2s, difficulty coefficient 1.2)", "align (base duration 3s, difficulty coefficient 1.5)" and "insert (base duration 2s, difficulty coefficient 1.3)", then R = 2 × 1.2 + 3 × 1.5 + 2 × 1.3 = 9.5s.

[0050] The time sequence is divided into time periods based on the logical stages of the robot's task actions. Each time period corresponds to "one independent and continuous action" (such as "extending the upper arm", "rotating the forearm", and "closing the gripper"). It satisfies the requirement of action continuity, where within the same time period, the robot executes "single-type, continuous, and uninterrupted" actions (e.g., time period R1 only executes "extending the upper arm from the initial position to the target position"). Simultaneously, it satisfies the requirement of reasonable time allocation, where the sum of the durations of all time periods equals the task duration R, and the duration of a single time period is t. i It needs to match the "range of motion and precision requirements of the action" (actions with a large range of motion and high precision requirements have a longer duration).

[0051] The task allocation method employs a "task action decomposition + time allocation algorithm" approach. Task action decomposition breaks down the task into "continuous actions of the smallest granularity." For example, the task of "grabbing a workpiece and placing it on the workbench" is decomposed into six actions: "arm extension (A1)," "forearm rotation (A2)," "gripper closure (A3)," "arm retraction (A4)," "forearm adjustment (A5)," and "gripper opening (A6)." The time allocation algorithm assigns time to each decomposed action, using the following formula: ,in The range of motion of an action (such as the distance the upper arm extends). For "accuracy coefficient" (the higher the accuracy requirement, the higher the accuracy coefficient). The larger the value, the more likely it is to be (1.2-2.0). The maximum safe speed for this movement (determined by joint performance);

[0052] For example, the task of "a six-axis industrial robot grabbing materials from the production line and assembling them into equipment" has a total task duration of R=12s. Through motion decomposition and time allocation algorithms, it is finally divided into 4 time periods:

[0053] R1 (upper arm extension): Duration 4s, target of movement is level 1 arm B1, range of motion 300mm, accuracy coefficient 1.3, maximum safe speed 75mm / s. It was initially calculated according to the formula t1=5.2s, and then corrected to 4s based on actual debugging experience (because there are acceleration / deceleration phases in actual movement, it is not necessary to run at the theoretical speed exactly).

[0054] R2 (Forearm Adjustment + Gripper Closure): Duration 3s, Action targets are secondary arm B2 and actuator C, forearm range of motion 50mm (accuracy coefficient 1.8, maximum safe speed 20mm / s), basic gripper closure action duration 0.5s, comprehensive calculation yields t2=5s, adjusted to 3s after actual testing (because gripper closure and forearm adjustment can be partially synchronized, reducing the total duration);

[0055] R3 (upper arm retraction + forearm rotation): Duration 3s, action targets B1 and B2, upper arm retraction range 280mm (accuracy coefficient 1.2, maximum safe speed 80mm / s), forearm rotation angle 60° (corresponding linear range 80mm, accuracy coefficient 1.5, maximum safe speed 30mm / s), theoretically calculated duration 8.2s, actually adjusted to 3s (because upper arm retraction and forearm rotation can move in tandem, no need to execute in sequence);

[0056] R4 (Forearm Adjustment + Gripper Opening): Duration 2s, Action targets B2 and C, Forearm fine-tuning range 30mm (precision coefficient 2.0, maximum safe speed 15mm / s), Gripper opening base duration 0.3s, theoretical calculation duration 4.3s, actual adjustment 2s (fine-tuning can use higher precision low-speed movement to shorten ineffective waiting time).

[0057] The sensor data acquisition unit includes sensor selection and deployment, data acquisition and synchronization.

[0058] The sensors include:

[0059] Joint rotation torque sensors are installed at the power output end of each driven joint (such as between the harmonic reducer and the joint shaft). They are strain gauge or magnetoelastic torque sensors with a range that covers 1.2–1.5 times the maximum rated torque of the joint, a measurement accuracy of no less than ±0.1%FS, and a resolution of 0.01 N·m. For example, if the rated torque of joint (A.B1) is 20 N·m, then a torque sensor with a range of 25 N·m is selected to ensure accurate measurement even under sudden load changes.

[0060] The joint rotational angular velocity sensor is coaxially mounted with the joint motor. It adopts a high-precision magnetoelectric or photoelectric speed sensor, outputs pulse signals or analog voltage signals, and has a measurement range covering 0–300 rpm with a resolution of not less than 1 rpm and a dynamic response time of less than 1 ms. In six-axis robots, the B1 and B2 arm joints often choose magnetoelectric type to resist vibration interference, while the end effector B6 joint can choose photoelectric type to improve the measurement accuracy at low speeds.

[0061] The joint rotatable angle sensor is directly integrated on the joint shaft and uses a 16-bit or higher absolute encoder to ensure that the angle information is maintained even after power failure. The measurement range is set according to the joint mechanical limit (e.g., ±170°), with an accuracy of ±0.01°, and supports multi-turn measurement to adapt to continuous rotating joints.

[0062] Position sensors: A position sensor is set at the robot's vision point as the origin coordinate (0, 0, 0), and position sensors are added at each joint point. For articulated robots, position sensors can be laser rangefinders or high-precision displacement sensors, which are installed on the fixed and moving parts of the joints to measure the position changes of the joints in space. For Cartesian coordinate robots, linear encoders can be used in conjunction with position sensors to accurately capture the position coordinates of each axis during translation. Through these position sensors, the position coordinates of each joint point can be fed back in real time at various time periods.

[0063] Sensor data acquisition adopts a time-period node triggering mode, that is, it only occurs at the beginning of each time period (t). start ) and end time (t) end The sensor data is collected once, and at the same time, real-time data is collected at a certain frequency (such as 100Hz) within the time period to ensure that the collection point is completely aligned with the action stage in the "timing unit" and can provide real-time feedback on the position of each joint.

[0064] Data collection begins during the time period (t) start The system records the initial torque, angular velocity, angle, and initial position coordinates of the joint as the baseline values ​​for the motion during that time period; it continuously acquires dynamic change data of torque, angular velocity, angle, and position coordinates in real time during the time period; and it collects data at the end of the time period (t). endRecord the final torque, angular velocity, angle, and final position coordinates of the joint as the actual result values ​​of the motion during that period;

[0065] The trigger signal source is a hard trigger signal (such as a rising edge of GPIO) output by the task scheduler of the "timing unit". At the same time, it triggers four types of sensors, namely torque, angular velocity, angle and position, to sample synchronously, ensuring that the four parameters are captured at the same time. Each time the data is collected, it is automatically bound to the current time period ID (such as R1, R2) and the corresponding key point mark (such as (B1.B2)) and stored in the "coordinate-mark-timing database" for easy comparison and traceability later. If the data collection of a certain time period node fails due to interference or hardware failure, the system will immediately re-collect the data in the next control cycle and mark it as "re-collected data" to ensure data integrity.

[0066] For example, in the task of "six-axis industrial robot grasping materials", during the R1 time period (0s~4s), tstart=0s, the following data is collected (B1, B2): joint torque = 0 N·m, angular velocity = 0° / s, angle = 0°, and position coordinates (0, 0, 0). Real-time data collection occurs within this time period, with data collected every 0.01s, recording changes in torque, angular velocity, angle, and position coordinates. end =4s, collected torque =18N·m, angular velocity =2.0° / s, rotation angle corresponding to angle =300mm, position coordinates (300, 0, 0); R2 time period (4s~7s), t start =4s, torque =18N·m, angular velocity =2.0° / s, angle = initial angle, position coordinates (300, 0, 0); real-time data acquisition during the time period; t end =7s, torque =22.5N·m, angular velocity =0.5° / s, angle =6.0°, position coordinates (300, 50, 0).

[0067] The preset unit is mainly used for task instruction recognition and task time planning:

[0068] Task recognition primarily involves acquiring task instructions. These instructions can be input in various ways, such as through a human-machine interface, receiving instructions from a higher-level control system, or reading stored task files. Once an instruction is input, the system parses it to extract key task information, such as task type (grabbing, assembly, handling, etc.), task object (material shape, size, position, etc.), and task objective (target position, orientation, etc.). For example, when receiving the task instruction "grab the cylindrical material located at coordinates (100, 200, 300) and place it at coordinates (500, 600, 700)," the system will parse the task type as grabbing and placing, the task object as a cylindrical material, the initial position as (100, 200, 300), and the target position as (500, 600, 700).

[0069] Time-segment planning involves pre-planning the positional changes required for each joint within each time segment. After task identification, the system divides the task into time segments based on key task information and the robot's kinematic model, and plans the positional changes of each joint within each time segment. First, the system decomposes the task into a series of continuous actions, such as "arm movement," "forearm rotation," "end-effector grasping," "end-effector movement," and "end-effector release." Then, for each decomposed action, its corresponding time segment is determined, and the required position coordinates of each joint within that time segment are planned. For example, the grasping and placing task described above can be decomposed into the following actions and corresponding time-segment planning:

[0070] Time period R1 (0s-3s): The boom moves from the initial position to bring the end effector close to the material. The position changes of each joint are planned so that the end effector reaches the gripping preparation position (120, 220, 280) above the material at 3s.

[0071] Period R2 (3s-6s): The forearm rotates and controls the end effector to descend, planning the position changes of each joint point so that the end effector reaches the material position (100, 200, 300) and completes the gripping action at 6s.

[0072] Time period R3 (6s-9s): The upper arm and lower arm move together to move the end effector that has grabbed the material to above the target position. The position changes of each joint are planned so that the end effector reaches above the target position (520, 620, 680) at 9s.

[0073] Period R4 (9s-12s): The end effector descends and releases the material, and the position changes of each joint are planned so that the end effector reaches the target position (500, 600, 700) and completes the release action at 12s.

[0074] When planning time periods, the system considers the robot's kinematic constraints (such as joint range of motion and speed limits), dynamic constraints (such as torque and acceleration limits), and task accuracy requirements to ensure that the planned position changes are feasible and accurate. Simultaneously, the system stores the position changes of each joint within each planned time period in a "preset position database" for subsequent comparison and adjustment by the monitoring unit.

[0075] The monitoring unit mainly includes the robot's task execution, sensor data acquisition and monitoring during different time periods of task execution, and then data fusion and comparison across time periods.

[0076] Task execution refers to the robot sequentially executing actions for each time period according to the time periods and position changes of each joint planned by the preset unit. The system sends control commands to the robot's joint controllers based on the task duration and time period division determined by the "time sequence unit" and the position changes of each joint within each time period planned by the "preset unit," driving the robot's joints to move and complete the task. During task execution, the system receives various sensor data collected by the "sensor data acquisition unit" in real time, including torque, angular velocity, angle, and position coordinates, providing data support for subsequent monitoring and adjustment.

[0077] During each time period of task execution, the system monitors the values ​​collected by the sensors in real time. The monitored content includes the real-time values ​​of parameters such as torque, angular velocity, angle, and position coordinates of each joint. The system compares these real-time values ​​with the theoretical values ​​of each parameter planned in the "preset unit" for that time period to determine whether the robot's actual movement is consistent with the planned movement. For example, in time period R1 (arm extension), the preset arm position coordinates should gradually change from the initial position (0, 0, 0) to (300, 0, 0), and the torque, angular velocity, and other parameters should also have corresponding theoretical change curves. The system monitors the arm position coordinates collected by the position sensor, as well as the torque and angular velocity values ​​collected by the torque and angular velocity sensors, in real time to see if they conform to the preset change patterns.

[0078] Time-segment data fusion and comparison is a core step connecting "sensor data acquisition" and "execution deviation correction." It requires leveraging the differentiated characteristics of task types (repetitive / non-repetitive), combining real-time sensor data from different time periods with preset theoretical data, and employing a multi-dimensional fusion algorithm to achieve "accurate difference identification." Simultaneously, dynamic thresholds are established to determine the execution status, ensuring the accuracy and stability of robot joint movements. The specific implementation process is as follows:

[0079] Data Sources and Preprocessing

[0080] Actual data: provided by the sensor data acquisition unit, including "t" for each time period. start Initial value, real-time sampled value at 100Hz within the time period, t end The "final value" covers four core parameters: torque (N·m), angular velocity (° / s), joint angle (°), and spatial position coordinates (mm), and is bound to "time period ID + joint point marker" (e.g., joint data for time period R1 (B1.B2)).

[0081] Theoretical data: provided by the preset unit, including the "parameter change curves and key node values" planned for each time period, such as the theoretical curve of the position coordinates of the boom extension in the R1 time period (linearly changing from (0,0,0) to (300,0,0)), the theoretical threshold of torque (0~18N·m), and the theoretical range of angular velocity (0~2.0° / s).

[0082] Task type differentiation integration strategy

[0083] For repetitive tasks (such as pipeline crawling), where the data fluctuations are small and the error patterns are reproducible (e.g., the peak torque in each R1 period is 18±0.5 N·m), a fusion algorithm of "Kalman filter (KF) + historical data weighting" can be used, which focuses on comparing actual data with "theoretical value + historical average" in two dimensions.

[0084] For non-repetitive tasks (such as assembling unknown workpieces), the data fluctuates greatly and the error has no fixed pattern (such as the difference of ±5° in the angle adjustment range during the R2 time period when assembling different workpieces). "Particle filtering (PF) + dynamic weight allocation" can be used, which focuses on the real-time dynamic comparison between actual data and theoretical values, and pays special attention to sudden deviations.

[0085] Implementation of time-segment data fusion and comparison for repetitive tasks

[0086] The core advantage of repetitive tasks (such as grabbing the same material more than 1,000 times a day) is that "the historical data is rich and the error patterns can be extracted". The fusion comparison needs to combine "real-time data + historical data" to improve the accuracy of deviation identification and avoid misjudgment caused by occasional noise in a single cycle.

[0087] Data pool initialization: Upon the first execution of the task, sensor data for 20 consecutive cycles is collected, and the "historical average μ and standard deviation σ" for each parameter in each time period are calculated to form an initial historical data pool. For example, joint torque in time period R1 (B1, B2): , final value of position coordinates , ;

[0088] Dynamic update mechanism: Every 10 cycles, the historical data pool is iteratively updated, incorporating new data using a weighted average method (new data weight 0.3, old data weight 0.7) to prevent historical data from becoming invalid due to equipment aging (such as increased torque caused by joint wear). For example, when the peak torque reaches 18.2 N·m during the R1 period of a certain cycle, the updated data will... , .

[0089] A KF model is constructed using "theoretical data as predicted values ​​and real-time data as observed values," and the parameters are estimated by fusion.

[0090] State vector definition: ( For torque, Angular velocity, For joint angle, (position coordinates);

[0091] Prediction equation: ( Here is the state transition matrix. To control the input matrix, (For joint drive current command).

[0092] Update equation: ( For Kalman gain, For the observation matrix, To observe the noise covariance (determined by sensor accuracy), the final output is the fused "optimal actual value". ;

[0093] Two-dimensional comparison:

[0094] The first dimension (real-time vs. theoretical): Calculate the "absolute deviation" between the optimal actual value and the theoretical value after fusion, such as the time period t in R1. end Absolute torque deviation at all times absolute deviation of position coordinates ;

[0095] The second dimension (real-time vs. historical): Calculate the "deviation rate" between the best actual value after fusion and the historical average, such as the torque deviation rate. Position deviation rate .

[0096] Based on historical data standard deviation In accordance with the accuracy requirements of the task, a "two-level dynamic threshold" is established:

[0097] Normal threshold (first-level threshold): Sets the upper limit of deviation for each parameter, such as torque. (Approximately 0.15 N·m), location coordinates (Approximately 0.0009 mm), Deviation rate ;

[0098] Warning threshold (secondary threshold): The upper limit of deviation is twice the normal threshold, such as torque. (Approximately 0.3 N·m), location coordinates (Approximately 0.0018 mm), Deviation rate .

[0099] State determination rules:

[0100] If all parameters meet the "≤ normal threshold" condition, the execution is considered normal, and the next time period action continues;

[0101] If any parameter is greater than the normal threshold and less than or equal to the warning threshold, it is judged as a "minor deviation", the deviation data is recorded (e.g., torque deviation of 0.2 N·m during the R1 period), and a compensation amount is added to the control command of the next period (e.g., fine-tuning the motor current by 0.1 A).

[0102] If any parameter is greater than the warning threshold, it is determined to be "execution abnormal". The pause mechanism is immediately triggered (pause response time ≤ 0.01s) and an abnormal report is generated (including time period ID, joint point, deviation parameter and value). For example, if the joint position deviation of the R1 time period (B1.B2) is 0.003mm (>0.0018mm), it is determined to be abnormal.

[0103] Taking the joint data of the R1 time period (B1.B2) of a certain pipeline grasping task (repetitive task) as an example, the actual fused torque Theoretical value T theo =18.0 N·m, historical average ΔT=0N·m, η T =0.44%, all ≤ normal threshold; actual fusion location P^=(299.9985, 0, 0) mm, theoretical value Ptheo=(300, 0, 0) mm, historical average μ P =(299.998, 0, 0)mm, ΔP=0.0015mm, η P =0.00025%, all of which are ≤ the normal threshold; the execution is finally determined to be normal, and the process enters the R2 period.

[0104] Realizing the fusion and comparison of time-segment data for non-repetitive tasks

[0105] The core characteristics of non-repetitive tasks (such as customized assembly of workpieces of unknown size) are "no historical data reference and large parameter fluctuation range". The fusion comparison needs to focus on "the dynamic matching degree between real-time data and theoretical data", capture parameter mutations through particle filtering (PF) algorithm, and at the same time adopt "segmented threshold" to adapt to the accuracy requirements of different time periods.

[0106] To address the "nonlinear and highly fluctuating" characteristics of non-repetitive task data, particle filtering is used to achieve multi-parameter fusion.

[0107] State Vector and Observation Model: State Vector X=[T ， w ， θ ， P]T Observation vector For real-time preprocessing of sensor data, observation model ( For the observation matrix, To observe noise, it is dynamically calculated from the real-time error of the sensor, such as the noise of a force sensor during the assembly stage. );

[0108] Particle generation and weight update: 1000 particles are initialized in each time period. The initial particle value is randomly generated based on the "±5% fluctuation range" of the preset theoretical value (e.g., the theoretical angle of the forearm adjustment in time period R2 is 6°, and the particle range is 5.7~6.3°). The weight is updated according to the "deviation between the actual observed value and the predicted particle value". The smaller the deviation, the greater the weight (the weight calculation uses the Gaussian probability density function).

[0109] Resampling and fusion output: The "roulette wheel method" is used to remove low-weight particles with a weight of <0.001, retain the effective particles and calculate the weighted average to obtain the "optimal actual value" after fusion, ensuring that the fusion result can track parameter mutations in real time (such as the sudden increase in torque caused by workpiece jamming during assembly).

[0110] For non-repetitive tasks, the "accuracy requirements and motion characteristics" vary significantly across different time periods, necessitating the setting of segmented thresholds based on time period type.

[0111] Coarse adjustment period (e.g., R1 boom extension, R3 boom retraction): large range of motion (≥200mm), low precision requirement (±0.01mm), and relatively lenient threshold setting: position deviation ΔP≤0.008mm, torque deviation ΔT≤1.0N·m, angular velocity deviation Δw≤0.3° / s;

[0112] Fine-tuning period (such as R2 forearm adjustment, R4 end-effector positioning): small range of motion (≤50mm), high precision requirements (±0.003mm), strict threshold setting: position deviation ΔP≤0.002mm, torque deviation ΔT≤0.3N·m, angular velocity deviation Δw≤0.1° / s;

[0113] During the actuator's action period (e.g., R2 gripper closing, R4 gripper opening): with "action completion" as the core, the threshold setting focuses on "force and position coordination": when the gripper is closed, the torque deviation ΔT ≤ 0.5 N·m, and the position coordinates must meet the "gripping range threshold" (e.g., material center ± 0.5 mm).

[0114] Fusion comparison and anomaly judgment process: First, real-time fusion calculation is performed. Particle filtering fusion is performed every 0.01s within each time period to output the optimal actual value. For example, during joint fine-tuning in time period R2 (B2.C), 100 fusion results are generated per second to track the coordinated changes in position and force in real time. Then, dynamic deviation comparison is performed. The optimal actual value after fusion is compared with the preset theoretical value point by point, and the "deviation growth rate" (the difference between the current deviation and the deviation at the previous moment / time interval) is calculated. If the deviation growth rate is >0.1mm / s (e.g., the position deviation suddenly increases from 0.001mm to 0.003mm in a time interval of 0.01s, with a growth rate of 0.2mm / s), then the data point is marked.

[0115] Anomaly detection rules:

[0116] If the deviation is less than or equal to the threshold for the corresponding time period and the deviation growth rate is less than or equal to 0.05 mm / s, the execution is considered normal. If the deviation is greater than the threshold but the deviation growth rate is less than or equal to 0.05 mm / s, the deviation is considered slow. In the next sampling cycle, the control parameters are fine-tuned (e.g., the forearm rotation speed is reduced by 5%). If the deviation is greater than the threshold or the deviation growth rate is greater than or equal to 0.05 mm / s, the execution is considered abnormal. The action is immediately paused and "abnormal backtracking" is initiated. This involves retrieving the fused data from the previous 5 sampling points for that time period, analyzing the source of the deviation (e.g., a sudden increase in torque may be due to workpiece jamming, and a positional deviation may be due to sensor drift), and outputting correction suggestions (e.g., reverse fine-tuning by 0.002 mm, or restarting the sensor).

[0117] Taking the joint data of the R2 time period (B2.C) of an unknown workpiece assembly task (non-repetitive task) as an example, the forearm rotates 6° and the gripper closes to grasp the workpiece (theoretical torque 5~8 N·m, position coordinates (300,50,0) mm); at 3.5s, the fusion angle is 5.8° (deviation 0.2°, > fine-tuning time period angle threshold 0.1°), the fusion torque is 8.2 N·m (deviation 0.2 N·m, ≤ torque threshold 0.3 N·m), and the deviation growth rate is 0.03° / s (≤0.05° / s); the judgment result is a slow deviation, and in the next sampling period (3.51s), the fine-tuning motor rotation angle command increases by 0.02°, and at 3.6s, the fusion angle reaches 5.98° (deviation 0.02°, ≤ threshold), and normal execution resumes.

[0118] Application and feedback of fusion comparison results

[0119] Real-time deviation correction: For "minor deviations" and "slow deviations," the system sends real-time correction commands to the joint controller through the "execution monitoring unit." Based on the position deviation value of the fused comparison, the joint movement commands are adjusted according to the "proportional compensation" principle. For example, when the deviation is 0.001mm, the command increases by 0.0012mm (compensation coefficient 1.2). The motor drive current is adjusted according to the direction of the deviation. For example, when the torque deviation is -0.5N·m (actual value is lower than theoretical value), the current increases by 0.2A (based on the motor torque constant K). t =2.5 N·m / A (calculated).

[0120] For data storage and traceability, after each task is completed, the system stores the "fusion comparison results of each time period" (including actual value, theoretical value, deviation value, and judgment result) in the "coordinate-tag-time series database" and associates it with the task ID and execution time. At the same time, historical data can be queried by "task type / time period ID / joint point", such as querying the deviation distribution of the R2 time period of the last 10 non-repetitive tasks. A "deviation trend report" is automatically generated. For example, if the torque deviation of a joint increases for 3 consecutive tasks in the R1 time period (from 0.1 N·m to 0.3 N·m), it will prompt "Joint wear may occur, it is recommended to calibrate the torque sensor".

[0121] Threshold dynamic optimization

[0122] After every 50 tasks, the system optimizes the thresholds for each time period based on historical fusion and comparison data. This includes: if 95% or more of the historical deviation for a certain time period is less than or equal to 70% of the current threshold, the threshold is lowered to 80% of the original threshold (e.g., the position threshold for time period R1 is lowered from 0.0009mm to 0.00072mm) to improve accuracy; if 10% or more of the historical deviation for a certain time period is greater than the current threshold, and it is confirmed that the deviation is not due to execution abnormalities (e.g., normal deviation increase caused by equipment aging), the threshold is raised to 1.2 times the original threshold (e.g., the torque threshold for time period R3 is raised from 0.3N·m to 0.36N·m) to avoid misjudgments.

[0123] Finally, it should be noted that the above embodiments are merely examples for clearly illustrating the present invention and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. A fusion control system for multiple sensors in high-precision robot joints, characterized in that: It includes a marking unit, a timing unit, a sensor data acquisition unit, a preset unit, and an execution monitoring unit; The marking unit uses hierarchical marking rules based on the base, arm, actuator, and joints to standardize the marking of the robot base, multi-level arm, actuator, and joints between adjacent components; The timing unit determines the total task duration and divides the time periods corresponding to independent actions; The timing unit determines the total task duration R, which refers to the total time from the triggering of the task start instruction to the feedback of the task completion instruction, in seconds; Its determination method is divided into two types: predefined and dynamic calculation. For repetitive tasks, R is determined by averaging the duration of multiple tasks based on historical task statistics, with a deviation of ≤10%. For non-repetitive tasks, R is calculated using a task complexity analysis algorithm; the algorithm inputs are the number of task steps and the estimated difficulty coefficient of each step, and the output is... Step i Basic Duration Difficulty level i ; The sensor data acquisition unit collects joint sensing data at different time periods; The preset unit identifies the task and plans the joint position changes for each time period; The execution monitoring unit drives task execution, monitors sensor values, and performs time-period data fusion and comparison. The time-period data fusion and comparison of the execution monitoring unit includes differential fusion of two task types: For repetitive tasks with small data fluctuations and reproducible error patterns, a Kalman filter and historical data weighted fusion algorithm are used, which focuses on comparing actual data with theoretical values ​​and historical averages in two dimensions. For non-repetitive tasks with large data fluctuations and no fixed error pattern, particle filtering and dynamic weight allocation are used. The focus is on the real-time dynamic comparison between actual data and theoretical values, with particular attention to abrupt deviations.

2. The fusion control system for high-precision robot joints based on multiple sensors as described in claim 1, characterized in that, The specific details of the time-segment data fusion comparison for repetitive tasks are as follows: Data pool initialization: When the task is executed for the first time, sensor data for each time period of 20 consecutive cycles is collected, and the historical average value μ and standard deviation σ of each parameter for each time period are calculated to form an initial historical data pool; subsequently, the historical data pool is iteratively updated every 10 cycles, and new data is incorporated using a weighted average method. KF model fusion: Constructing a Kalman filter model using theoretical data as predictions and real-time data as observations, defining a state vector. ,in, For torque, Angular velocity, For joint angle, Given the location coordinates, the optimal actual value after fusion is calculated using the prediction equation and the update equation; Two-dimensional comparison and threshold judgment: Calculate the absolute deviation between the optimal actual value and the theoretical value, and the deviation rate from the historical average, based on the standard deviation. A two-level dynamic threshold is set, including thresholds for normal torque and position coordinates, and deviation rate; the upper limit of the warning threshold deviation is twice the normal threshold, and the corresponding processing is triggered based on the threshold to determine whether it is normal, slightly deviated or abnormal.

3. The fusion control system for high-precision robot joints based on multiple sensors as described in claim 1, characterized in that, The specific details of the time-segment data fusion comparison for non-repetitive tasks are as follows: Particle filter fusion: defining state vectors Observation vector For real-time preprocessing of sensor data, among which and For the observation matrix, To observe noise, 1000 particles with a fluctuation range of ±5% based on the preset theoretical value are initialized for each time period. The particle weights are updated according to the Gaussian probability density function. After removing particles with a weight < 0.001, a weighted average is calculated to obtain the optimal actual value. Segmented threshold settings: Threshold ranges are set for the position, torque, and angular velocity of the coarse adjustment period, the position, torque, and angular velocity of the fine adjustment period, and the torque and position of the actuator action period, respectively, according to the time period type; Dynamic deviation comparison and anomaly judgment: Particle filter fusion is performed every 0.01s to calculate the deviation between the optimal actual value and the theoretical value and the deviation growth rate. If the deviation is ≤ the corresponding time period threshold and the deviation growth rate is ≤ 0.05mm / s, it is judged as normal. If the deviation exceeds the threshold but the growth rate is ≤ 0.05mm / s, it is judged as slow deviation and the parameters are fine-tuned. If the deviation exceeds the threshold or the deviation growth rate is > 0.05mm / s, it is judged as abnormal and the backtracking is paused.

Images