Servo control method and system based on multiple sensors

Abstract

The invention relates to the technical field of servo control, in particular to a multi-sensor-based servo control method and system, and the method comprises the following steps: obtaining sensor data to execute consistency verification, recognizing and marking a trend abnormal channel, calculating a difference value to determine a main / standby path, judging a switching condition to complete path replacement, and collecting an increment sequence to output a control path. According to the invention, by dynamically identifying the sensors which do not accord with the direction trend and marking the sensors as the safe stop state, control deviation caused by abnormal signal interference is avoided, channel optimization is realized by calculating the amplitude change difference value of each effective channel, so that the control path better accords with the change trend of the target instruction; a main and standby path automatic switching mechanism is used for ensuring the continuity and consistency of an output path, path reference judgment is performed by collecting a continuous period increment sequence, a complementary path is reserved to form a bidirectional constraint mechanism for a control signal, and reliable output of a control system is still ensured under the condition that multi-sensor input is unstable.

Description

Technical Field

[0001] The present invention relates to the technical field of servo control, and in particular to a servo control method and system based on multiple sensors. Background Art

[0002] The field of servo control technology involves the precise control of the position, speed or force of the actuators in the mechanical system, including the feedback control theory, the composition of the servo mechanism and the demand for high-response and high-precision control in the application scenarios. It covers a variety of control methods such as position servo, speed servo and force servo, and combines different types of actuators such as electric, hydraulic and pneumatic to build a closed-loop control system to achieve fine adjustment and tracking control of the target. Among them, the traditional multi-sensor based servo control method refers to a control method that obtains information by fusing multiple physical quantity sensors to assist the servo system control output. It mainly improves the response accuracy and stability of the servo system in a highly dynamic and complex environment. It usually uses the values ​​measured by the acceleration sensor, angular velocity sensor and position encoder, and sends them to the PID controller for state estimation through the Kalman filter to complete the position, speed or force control. It relies on synchronous sampling between sensors and recursive calculation based on time series to obtain higher state estimation accuracy, which is then used to drive the servo actuator to achieve motion control.

[0003] The existing servo control process relies heavily on synchronous sampling between sensors and recursive estimation based on time series. When short-term abnormalities or directional deviations occur in individual sensor channels, the interference signals cannot be eliminated immediately, which can easily lead to inaccurate state estimation and thus cause malfunction of the control actuator. In a high-dynamic environment, the acceleration and angular velocity sensors change dramatically, and the filtering results are easily interfered with by noise and produce deviations. In particular, if there are signals with inconsistent instantaneous directions or drastic amplitude jumps in certain cycles, the system cannot actively isolate the abnormal path. Fixed filtering calculations alone are not enough to ensure the reliability of the path, resulting in a decrease in the stability of the output control instructions and affecting the accurate tracking and adjustment of the target execution state. Summary of the Invention

[0004] The purpose of the present invention is to solve the shortcomings in the prior art and to propose a servo control method based on multiple sensors.

[0005] In order to achieve the above object, the present invention adopts the following technical solution: a servo control method based on multiple sensors, comprising the following steps: S1: Obtain sensor data from each channel within the periodic task, perform direction consistency check, and perform statistical marking processing to generate the number of channels with consistent directions; S2: Based on the number of channels with consistent directions, identify sensors whose direction trends are inconsistent in two consecutive cycles channel by channel, mark the safe stop state channels according to the redundancy protocol, accumulate the number of channels, and generate the number of frozen state channels; S3: Based on the number of frozen channels, counting the amplitude change of each valid channel sensor, calculating the difference between the amplitude change and the target control amplitude change, determining the main control path and the backup path according to the difference, and generating the main and backup control path channel numbers; S4: Verify the current primary control path according to the primary and backup control path channel numbers, match the current primary control path with the target control direction symbol, determine whether the primary and backup control path switching condition is triggered, replace the backup path with the new primary control path, and generate a primary path replacement confirmation status record; S5: Based on the main path replacement confirmation status record, collect a continuous period increment sequence from the current main control path, determine it as the current period control signal output reference path, retain the original complementary path, and generate a multi-sensor servo control path orientation result.

[0006] As a further solution of the present invention, the number of direction-consistent channels includes a channel number mark, a direction consistency counting result, and a security level mark; the number of frozen state channels includes a failed channel mark, a cycle count label, and a redundant deactivation status record; the primary and standby control path channel numbers include a minimum difference number and a sub-minimum difference number; the primary path replacement confirmation status record includes a replacement confirmation mark, a trend consistency label, and a path switching instruction; the multi-sensor servo control path orientation result includes a control signal reference path, a complementary path retention record, and a path orientation number record.

[0007] As a further solution of the present invention, the step of obtaining the number of channels with consistent directions is specifically as follows: S111: Acquire sensor data corresponding to all channels in the periodic task, determine the relative difference between the direction value corresponding to each channel data and the initial direction reference value, compare the relative difference with the direction consistency offset threshold, obtain the direction consistency determination status of each channel in the current cycle, and generate a channel direction consistency status record; S112: Based on the channel direction consistency state record, filter the channel state values ​​with valid direction consistency state in the current cycle, compare the number of filtered channels with the total number of channels, determine the direction state matching status of the current cycle, and generate a direction consistency matching trend record; S113: According to the directional consistency matching trend record, the number of directional consistent channels in the current cycle is marked, and the sequence distribution information of the directional consistent channels in the cycle is obtained in combination with the channel sequence number. The periodic task ID and the period sequence number are read to establish an associated mapping to generate the number of directional consistent channels.

[0008] As a further solution of the present invention, the step of obtaining the number of frozen state channels is specifically as follows: S211: Based on the number of channels with consistent directions, extract the direction consistency status flag of each channel in two consecutive cycles, compare the direction consistency flags of adjacent cycles based on the channel number to see if there is a change, identify the set of channel numbers whose direction consistency states reverse in consecutive cycles, count the number of channels, and generate the number of channels with inconsistent direction trends; S212: Based on the number of channels with inconsistent directional trends, a redundant channel configuration table is read, and the identified channel numbers are matched one by one with the parallel channel information in the redundant structure. The directional consistency status of the paired channels in the current cycle is compared. When any paired channel group is identified as inconsistent in the same cycle, the corresponding channel number is written into the safe stop flag set to establish the number of channels in the safe stop state. S213: According to the number of channels in the safe stop state, perform a periodic freeze counter accumulation operation on the channel numbers marked as the safe stop state to generate the number of channels in the frozen state.

[0009] As a further solution of the present invention, the step of obtaining the channel number of the primary and standby control paths is specifically as follows: S311: Based on the number of frozen channels, count the number of all remaining unfrozen channels in the current cycle, retain channels with stable direction consistency in two consecutive cycles and not marked as safe stops, extract the numbers to form a valid channel sequence, and generate the number of valid channels; S312: Based on the channel numbers recorded in the number of valid channels, respectively read the amplitude change value of the current cycle and the amplitude change value of the previous cycle corresponding to the sampling signal of each channel sensor, obtain the instantaneous amplitude difference of each channel, set the target control amplitude change value and calculate the amplitude difference evaluation value, sort all channels, record the channel number and difference corresponding to the minimum difference, and establish the channel number and amplitude difference of the main control path; S313: Based on the main control path channel number and amplitude difference, after excluding the channel with the minimum amplitude difference from all sorted channels, select the channel corresponding to the next minimum amplitude difference, record the number as the alternative path channel, and merge the main path channel number and the alternative path channel number into a dual-path configuration structure to establish the main and backup control path channel numbers.

[0010] As a further solution of the present invention, the step of obtaining the primary path replacement confirmation status record is specifically as follows: S411: Based on the main control path channel number, extract the trend of the amplitude change of consecutive sampling points of the main control channel in the current cycle and the previous cycle, compare the difference between two adjacent sampling values, and mark the direction as positive if the latter value is greater than the former value, otherwise mark it as negative, forming a direction symbol sequence for each cycle, recording the number of positions with inconsistent direction changes, calculating the proportion of the corresponding position number in the total number of sampling points, and outputting the trend direction inconsistency rate; S412: Based on the trend direction inconsistency rate, a set target control direction symbol sequence is read and the directions of the sampling points of the current period are compared item by item. If the direction of the sampling point is inconsistent with the target direction, it is counted as a deviation point. The total number of deviation points is accumulated and divided by the total number of sampling points to obtain the target direction deviation ratio of the current period. The deviation ratio is compared with the set deviation judgment threshold. If the deviation ratio is less than the threshold, the current main path is retained to obtain the main path trend deviation judgment result. S413: Based on the main path trend deviation judgment result, analyze whether the main and backup path switching conditions are met. If the judgment value exceeds the deviation threshold, read the corresponding alternative path channel number and current direction trend data, package the timestamp of the replacement action, the replaced channel number, and the new channel number to obtain the main path replacement confirmation status record.

[0011] As a further solution of the present invention, the steps of obtaining the multi-sensor servo control path orientation result are specifically as follows: S511: Based on the main path replacement confirmation status record, read the main channel number of the new cycle, and initialize the signal acquisition sequence of the current path accordingly, perform differential processing on the voltage differences between sampling points in each cycle, extract the amplitude changes of adjacent sampling points, and retain all incremental values ​​corresponding to the channel in each cycle to generate a continuous cycle incremental sequence; S512: Based on the continuous periodic increment sequence, calculate the mean of the incremental values ​​of all sampling points in each period, check whether the incremental directions at all points are consistent, calculate the proportion of direction mutations, mark the channel as an available control path for the current period, and obtain the control output record of the current period; S513: According to the current cycle control output record, register the corresponding channel as the main control path of the current cycle, bind it to the channel number and archive it, and at the same time retain the complementary channel number, freeze the status and do not participate in the current cycle control output process, write the main channel number, complementary channel number and their associated identifiers into the servo control path configuration mapping table, and establish the multi-sensor servo control path orientation result.

[0012] Multi-sensor based servo control system, including: The direction consistency check module obtains the sensor data of each channel, performs direction consistency check, and generates statistics on the number of channels with consistent directions; The trend conflict screening module judges the difference in the direction trend of the consistent channel for two consecutive periods based on the number of channels with consistent directions, counts the number of abnormal channels, and generates the number of frozen state channels; The primary and backup path screening module obtains the amplitude change value of the valid channel sensor based on the number of frozen state channels, calculates the difference between the amplitude change value and the target amplitude, and sorts the values ​​by difference. The channel with the smallest difference is recorded as the primary channel, and the channel with the second smallest difference is recorded as the backup channel, and generates the channel numbers of the primary and backup control paths; The path symbol matching module extracts the main channel direction symbol according to the main and standby control path channel numbers and compares it with the target symbol. If there is an inconsistent trend, the channel is switched and a main path replacement confirmation status record is generated. The control path orientation module collects a new main channel continuous incremental sequence as a reference path based on the main path replacement confirmation status record, retains complementary path information, and generates a multi-sensor servo control path orientation result.

[0013] Compared with the prior art, the advantages and positive effects of the present invention are: In the present invention, sensors that do not conform to the directional trend are dynamically identified and marked as a safe stop state to avoid control deviations caused by abnormal signal interference. Channel optimization is achieved by calculating the amplitude change difference of each effective channel, so that the control path is more in line with the target instruction change trend. The main and standby path automatic switching mechanism is used to ensure the continuity and consistency of the output path. After identifying the trend anomaly, the path update is completed in time to enhance the system's adaptability in periodic fluctuations. Path benchmark judgment is performed by collecting continuous periodic incremental sequences, and complementary paths are retained to form a two-way constraint mechanism for the control signal. In the case of unstable multi-sensor input, the reliable output of the control system and the stable maintenance of the motion state are still guaranteed. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 It is a flow chart of the main steps of the present invention; Figure 2 A flow chart for obtaining the number of channels consistent with the direction of the present invention; Figure 3 A flow chart for obtaining the number of channels in the frozen state according to the present invention; Figure 4 A flow chart for obtaining the channel numbers of the primary and standby control paths of the present invention; Figure 5 This is a flow chart for obtaining the main path replacement confirmation status record of the present invention; Figure 6 This is a flow chart for obtaining the path orientation results of the multi-sensor servo control of the present invention. DETAILED DESCRIPTION

[0015] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0016] In the description of the present invention, it should be understood that the terms "length," "width," "up," "down," "front," "back," "left," "right," "vertical," "horizontal," "top," "bottom," "inside," "outside," and the like, indicating positions or relationships, are based on the positions or relationships shown in the accompanying drawings and are intended only to facilitate the description of the present invention and simplify the description. They do not indicate or imply that the devices or elements referred to must have a specific orientation, be constructed, or operate in a specific orientation. Therefore, they should not be construed as limiting the present invention. Furthermore, in the description of the present invention, "plurality" means two or more, unless otherwise expressly and specifically defined.

[0017] See also Figure 1 , a servo control method based on multiple sensors, comprising the following steps: S1: Acquire sensor data from each channel within a periodic task (task cycle (1-100ms) defined by the programmable controller standard), perform directional consistency check (machine safety standard PL level verification), and perform statistical marking processing to generate the number of channels with consistent direction; S2: Based on the number of channels with consistent directions, identify sensors whose direction trends do not match in two consecutive cycles, channel by channel, mark the safe stop state channel according to the redundancy protocol (parallel redundancy protocol (dual channel zero switching time)), and perform cycle counter accumulation to generate the number of frozen state channels; S3: Based on the number of frozen channels, count the remaining valid channels, obtain the amplitude change of each valid channel sensor, calculate the difference between the amplitude change and the target control amplitude change, use the channel with the minimum difference as the main control path, record the corresponding channel number and difference amplitude, select the channel with the second minimum difference as the backup path, and generate the channel numbers of the main and backup control paths; S4: Based on the channel numbers of the primary and backup control paths, the current primary control path is verified and matched with the target control direction symbol. If there is a trend inconsistency between the two cycles, the primary and backup switching conditions are triggered, the backup path is replaced with the new primary control path, and a primary path replacement confirmation status record is generated; S5: Based on the main path replacement confirmation status record, a continuous cycle increment sequence is collected from the current main control path, and the reference path for the current cycle control signal output is determined, and the original complementary path is retained to generate a multi-sensor servo control path orientation result.

[0018] The number of direction-consistent channels includes the channel number mark, direction consistency counting result, and safety level mark; the number of frozen state channels includes the failed channel mark, cycle count label, and redundant deactivation status; the master and standby control path channel numbers include the minimum difference number and the second minimum difference number; the master path replacement confirmation status record includes the replacement confirmation mark, trend consistency label, and path switching instruction; the multi-sensor servo control path orientation result includes the control signal reference path, complementary path retention record, and path orientation number record.

[0019] See also Figure 2 , S1 step is: S111: Acquire sensor data corresponding to all channels in the periodic task, determine the relative difference between the direction value corresponding to each channel data and the initial direction reference value, compare the relative difference with the direction consistency offset threshold, obtain the direction consistency determination status of each channel in the current cycle, and generate a channel direction consistency status record; Within the task cycle range defined by the periodic task, the sensor signal sampling data corresponding to each channel number is obtained. The specific sampling frequency is set to 10ms per cycle. Assume that there are 16 channels in the system, and each channel is connected to a group of direction sensors. The collected content is the voltage signal returned by the sensor. The system presets the sensor voltage value (such as 2.5V) to be mapped to the standard direction angle of 0°. Each channel fluctuates with an accuracy of ±0.01V, corresponding to a range of ±1.2°. The direction data of each channel in the first stable working state in the starting reference cycle T0 is set as the initial direction reference value, for example The initial value of channel 3 is 2.48V, so the reference direction angle is set to -2.4°. Similarly, the system then determines the difference between the real-time voltage value collected in each new cycle and the reference value. For example, if channel 3 reads 2.45V in the current cycle, the relative difference is -3.6°. This difference is compared with the preset direction consistency offset threshold. The direction consistency offset threshold is set to ±5° in the design according to the safety standard ISO13849. This value is derived from the standard deviation statistics of the direction offset during multiple test runs of the robot arm, as shown in the table below. Therefore, the direction of this channel in this cycle is consistent. Table 1 Direction offset test statistics

[0020] As shown in Table 1, by comparing the angular offset between the initial direction and the current direction channel by channel, if the offset angle is within the range of ±5°, the channel is considered to meet the direction consistency requirements and the status is marked as consistent. Otherwise, it is inconsistent. The system stores the status in a binary manner (1 for consistent and 0 for inconsistent) in the direction consistency status list for the next step of quantity trend identification operation. This process generates a channel direction consistency status record.

[0021] S112: Based on the channel direction consistency state record, filter the channel state values ​​with valid direction consistency state in the current cycle, compare the number of filtered channels with the total number of channels, determine the direction state matching status of the current cycle, and generate a direction consistency matching trend record; Based on the channel directional consistency status records obtained above, the system sequentially reads the status value of each channel in the current cycle and performs a filtering operation. The filtering rule is to retain only records with a status value of 1, which indicates a set of channel numbers with consistent direction. For example, if channels 1, 3, 5, and 6 are consistent in direction in this cycle, the number of consistent channels is 4. The total number of channels in the system is set to 16, which means the directional consistency ratio in this cycle is 25%. By reading the directional consistency determination benchmark value, this benchmark value is set to 70% based on statistical experience with stable equipment operation. This setting process is based on a distribution histogram analysis of the number of consistent channels in more than 100 sets of periodic operation data. The 70% ratio is determined to be the lower bound of the safe operating area for dynamic error convergence of the equipment. If the current cycle ratio is far below the 70% benchmark value, the cycle is classified as a low consistency interval. The trend direction is marked based on the matching trend of the past three cycles. Since only four channels meet the directional consistency requirements, the matching trend is marked as decreasing, and a directional consistency matching trend record is finally generated.

[0022] S113: Based on the directional consistency matching trend record, the number of channels with consistent direction in the current cycle is marked, and the sequence distribution information of the channels with consistent direction in the cycle is obtained in combination with the channel sequence number. The periodic task ID and the period sequence number are read to establish an association mapping, and the number of channels with consistent direction is generated; According to the direction consistency matching trend records obtained above, the channel numbers of the direction-consistent channels are rearranged and marked, that is, the consistent channels are arranged in ascending order in the channel number set. For example, channel numbers 3, 5, 6, and 12 are assigned bit sequence labels 1 to 4 respectively. On this basis, their labels are mapped to the period sequence numbers of the periodic tasks to form a corresponding mapping table. For example, the direction-consistent channels in period T1 are 3, 5, 6, and 12, and in T2 are 2, 4, 7, 9, and 10, respectively. They are archived in the data structure with task ID "P12345". At the same time, a channel-period mapping distribution structure is generated. This structure can be used to subsequently count the continuous trend of each channel maintaining direction consistency in multiple periods. For example, channel 5 has been consistent in direction for three consecutive periods, and it is recorded in the trend growth label group. The final direction-consistent channel number and its statistical number are written into the structure table item to generate the number of direction-consistent channels.

[0023] See also Figure 3 , step S2 is: S211: Based on the number of channels with consistent directions, extract the direction consistency status flag of each channel in two consecutive cycles, compare the direction consistency flags of adjacent cycles based on the channel number to see if there is a change, identify the set of channel numbers whose direction consistency states reverse in consecutive cycles, count the number of channels, and generate the number of channels with inconsistent direction trends; Based on the number of channels with consistent direction, the direction consistency status identifier of all channels in the current cycle and the previous cycle is first obtained. This identifier is the data processing result provided by the sensor in the front-end acquisition system. Its value is 0 or 1, representing inconsistent and consistent directions respectively. Assume that there are 16 channels in the system, numbered from C01 to C16. The direction consistency status of each channel in the two cycles is compared. Assume that C03 is 1 in cycle T1 and 0 in cycle T2, then it is determined that the direction trend of the channel has reversed. The system completes the above comparison process for all channels and constructs a channel direction change list. If there is a state reversal, its channel number is added to the identification set, and then the number of identified channels is counted. At the same time, channels that have not changed are eliminated, and only channel items whose state switches from 1 to 0 or from 0 to 1 are retained. Assume that the reversed channels identified in the final result are C03, C07, C11, and C12, a total of 4. The system writes this value as the number of channels with inconsistent direction trends into the internal register structure, see Table 2.

[0024] Table 2 Trend reversal channel identification table

[0025] As shown in Table 2, the system successfully identified the channels with reversals by comparing the changes in channel states in adjacent cycles and obtained the number of channels with inconsistent directional trends.

[0026] S212: Based on the number of channels with inconsistent directional trends, the redundant channel configuration table is read, and the identified channel numbers are matched one by one with the parallel channel information in the redundant structure. The direction consistency status of the paired channels in the current cycle is compared. If any paired channel group is identified as inconsistent in the same cycle, the corresponding channel number is written into the safe stop flag set to establish the number of channels in the safe stop state. Based on the number of channels with directional trend discrepancies, the system retrieves the paired channel structure for each channel in the redundant protocol configuration table to complete the comparison and identification of the redundant dual-channel configuration. The specific operation steps are to find the parallel channel pairing according to the channel number. For example, assume that C03 is paired with C04, C07 is paired with C08, and C11 is paired with C13. The system then retrieves the directional consistency status of these paired channels in the current cycle. When the main channel and its parallel channels both have directional trend discrepancies in the same cycle, it is considered that the channel group has lost its mutual redundancy and must be included in the safe stop process. If both C03 and C04 are in a trend discrepancy, the system marks their pairing as a "failed pair" and records it in the safe stop mark set. This process is processed one by one for each trend discrepancy channel. After completion, the number of channels included in the safe stop state is counted. Assuming that both pairs C03 / C04 and C11 / C13 meet the stop conditions, totaling four channels, the system outputs this value as the number of channels in the safe stop state.

[0027] S213: According to the number of channels in the safe stop state, perform a periodic freeze counter accumulation operation on the channel numbers marked as the safe stop state to generate the number of channels in the frozen state; According to the number of channels in the safe stop state, the system will write the number of the marked channel into the cycle freeze count mapping structure, read the freeze count value recorded in the structure of the channel in the previous cycle, and initialize its freeze count to 1 if the channel enters the safe stop state for the first time. If a frozen value already exists, the frozen value will be increased by 1 in the current cycle to complete the update of the cycle freeze count. Assuming that the frozen value of C03 in the previous cycle is 2 and the safe state has not been restored in this cycle, it will be updated to 3. If C04 enters the state for the first time, it will be counted as 1. Then the number of channels with frozen values ​​not equal to 0 will be traversed and counted as the total output of frozen channels in this cycle, which constitutes the number of channels in the frozen state.

[0028] See also Figure 4 , S3 steps are: S311: Based on the number of frozen channels, count the set of channel numbers remaining unfrozen in the current cycle, retain the channels with stable direction consistency in two consecutive cycles and not marked as safe stop, extract the numbers to form a valid channel sequence, and calculate the number of valid channels; According to the number of frozen state channels, the system first identifies the frozen state channel number set in the current cycle. The set comes from the channels whose frozen counters are greater than 0. Suppose the current frozen channels of the system are C03, C06, C08, and C14. Through logical screening, the system removes the remaining channels after these numbers as the valid channel set. The valid channels are C01, C02, C04, C05, C07, C09, C10, C11, C12, C13, C15, and C16. The system traverses the set and retrieves its current cycle amplitude data channel by channel. The amplitude data is defined as the difference between the maximum and minimum voltage values ​​in a single cycle. The sampling frequency of each channel is set to 10ms, and the total number of sampling points in each cycle is 20. The maximum and minimum voltage values ​​are extracted and the difference is calculated to form the amplitude value of the channel. For example, the maximum sampling value of channel C01 in the current cycle is 2.41V, and the minimum value is 2.22V, so the amplitude is 0.19V. Similarly, the system performs the same operation on the remaining 11 channels and writes the channel number and the corresponding amplitude value into the amplitude cache table for subsequent processing to generate the number of valid channels.

[0029] Table 3 Effective channel amplitude change value table

[0030] As shown in Table 3, through the maximum and minimum value difference processing, the system obtains the amplitude changes of the 12 valid channels, and based on this, determines that the total number of valid channels in the current cycle is 12, completing the statistics and output of the number of valid channels.

[0031] S312: Based on the channel number recorded in the number of valid channels, read the current cycle amplitude change value and the previous cycle amplitude change value corresponding to each channel sensor sampling signal, obtain the instantaneous amplitude difference of each channel, and set the target control amplitude change value using the formula: ; Calculate the amplitude difference evaluation value, sort all channels, record the channel number and difference corresponding to the minimum difference, and establish the main control path channel number and amplitude difference, where: Indicates the amplitude value of the i-th sampling point in the effective channel in period t, in volts, is the amplitude value of the previous cycle, in volts, Indicates the interference fluctuation value of the channel in the current and previous cycles, in volts. T indicates the target amplitude change value in volts, with a typical value of 2.0V. n indicates the number of sampling points per channel. The channel amplitude difference evaluation value, in volts; Based on the 12 channel numbers and their amplitude change values ​​constructed in the number of valid channels, the system reads the amplitude change value of each channel in the previous cycle in turn, and then averages it with the amplitude change value of the current cycle to obtain the average amplitude change of the channel in two consecutive cycles. Set the system target control amplitude setting value to: ; This value is the system static control reference value. The current cycle amplitude change of channel C01 is: ; The amplitude changes in the previous cycle are: ; The average amplitude change value is: ; With target value For reference, calculate the difference between the average amplitude and the target value and take the absolute value: ; Then read the channel The interference term of the current cycle is: ; The interference in the previous cycle is: ; The disturbance combination value is calculated as follows: ; The final amplitude difference evaluation value is: ; This operation processes all valid channels. Set the channels in the sorting The current cycle amplitude is: ; The previous cycle was: ; Its average value is: ; The corresponding difference is: ; The interference combination is ; Therefore, the final evaluation value is: ; After comparison, the amplitude difference evaluation value of C01 is the smallest, which is the optimal channel. The channel number and amplitude difference of the main control path are obtained.

[0032] The amplitude difference evaluation value indicates the degree of deviation between the actual control signal amplitude change of a certain effective channel in the current operation cycle and the system's preset target control amplitude, and is comprehensively quantified in combination with the interference fluctuation level of the channel in two consecutive cycles. The smaller the value, the closer the amplitude change of the channel is to the target value, and the lower the interference fluctuation, the more stable and reliable the overall control state is. This evaluation value not only reflects the control response accuracy of the channel, but also measures its anti-interference ability during dynamic operation. Therefore, this indicator can be used as an important criterion for judging whether the channel is suitable as the main control path.

[0033] The calculation logic of the amplitude difference evaluation formula is based on the joint measurement of control accuracy and interference steady state. Its core goal is to measure the closeness between the current state of each effective channel and the system's target control amplitude, while also considering the impact of interference noise on channel stability. The first part of the formula is: ; middle, Represents the average amplitude change of the channel between the current and previous weeks, used to quantify its fluctuation trend in the time series. This item is subtracted from the target amplitude T and the absolute value is taken to evaluate whether the current channel response is approaching the system set target value, thereby eliminating the interference of positive and negative fluctuation directions on the results. The second part: ; It represents the average composite amount of the interference amplitude of the channel in two cycles. The purpose of adding the squares and taking the square root is to unify the interference in different directions into a modulus value, that is, to obtain the length of the disturbance vector in a two-dimensional disturbance coordinate system, which is direction-independent. Finally, the two sub-items are added together to comprehensively evaluate the channel performance, so that while approaching the target amplitude, the smaller the disturbance fluctuation, the smaller the evaluation value, thereby reflecting the better adaptability of the channel in the control path.

[0034] S313: Based on the channel number and amplitude difference of the primary control path, after excluding the channel with the smallest amplitude difference from all sorted channels, select the channel corresponding to the next smallest amplitude difference, record the number as the backup path channel, and merge the primary path channel number and the backup path channel number into the dual path configuration structure to establish the primary and backup control path channel numbers; According to the main control path channel number and amplitude difference, the system reads the next-level channel number except the first-ranked channel number according to the channel difference sorting structure. Assuming that the difference of channel C01 is 1.864V as the minimum value, the second-ranked channel is C05 with a difference of 1.87V. The difference between the two is only 0.006V, which meets the alternative condition limit threshold Δ≤0.02V. The system binds the main channel C01 and the alternative channel C05, and writes them into the path identification configuration table to form the main and backup channel group identification. The number (C01, C05) is used as the effective control path within the cycle to generate the main and backup control path channel number.

[0035] See also Figure 5 , step S4 is: S411: Based on the main control path channel number, extract the trend of the amplitude change of consecutive sampling points of the main control channel in the current cycle and the previous cycle, compare the difference between two adjacent sampling values, and mark the direction as positive if the latter value is greater than the former value; otherwise, mark the direction as negative. This forms a direction symbol sequence for each cycle, records the number of positions with inconsistent direction changes, calculates the proportion of the corresponding position number in the total number of sampling points, and outputs the trend direction inconsistency rate; Based on the channel number of the main control path, the system extracts the voltage data of each sampling point of channel C01 in the current cycle and the previous cycle. The system takes the difference between the two adjacent values ​​of each sampling point to determine its voltage change trend. If the voltage value increases, it is marked as +1, and if it decreases, it is marked as -1. After obtaining the periodic trend sequence, the system takes the difference between the signs of the corresponding positions of the same sampling point number in cycles t=1 and t=2. If the signs are consistent, they are marked as 0, and if they are inconsistent, they are marked as 1. The total number of inconsistent points is recorded and divided by the total number of sampling points (20). Assuming that there are 5 inconsistent points, the inconsistency rate is 5 / 20=0.25. This value is used to express the stability of the channel direction trend and serves as the basis for subsequent offset judgment. Specific data sampling and trend extraction are shown in the table below. Table 4 Statistics of inconsistent trend symbols

[0036] As shown in Table 4, the trend direction inconsistency rate can be obtained based on symbol comparison.

[0037] S412: Based on the trend direction inconsistency rate, the set target control direction symbol sequence is read and the directions of the sampling points in the current cycle are compared item by item. If the direction of the sampling point is inconsistent with the target direction, it is counted as a deviation point. The total number of deviation points is accumulated and divided by the total number of sampling points to obtain the target direction deviation ratio of the current cycle. This is compared with the set deviation judgment threshold. If the deviation ratio is less than the threshold, the current main path is retained, and the main path trend deviation judgment result is obtained. Based on the trend direction inconsistency rate of 0.25, the system calls the channel target control direction symbol sequence set in the standard control module. Assuming that the target symbol sequence is that each sampling point within the cycle should maintain a positive trend, that is, all are +1, the system reads the actual trend symbol of the current cycle item by item and compares it with the target symbol. If there is inconsistency, it is marked as a deviation point. The target direction deviation ratio is obtained by accumulating the number of deviation points and dividing it by the total number of sampling points. Assuming that there are three inconsistencies with the target direction in the current sampling trend symbol of channel C01, the deviation ratio is 3 / 20=0.15. The system compares this value with the deviation judgment threshold of 0.2 to determine whether it exceeds the limit threshold. The threshold source is the critical mean test of the multi-channel trend deviation ratio under different disturbance conditions in the control group experiment, with a test range of 0.15 to 0.3. The lower limit and the median are averaged as the setting reference, so the setting value is 0.2. This value is set empirically. The current value of 0.15 is lower than the threshold. The system determines that the current channel trend has not deviated beyond the limit and obtains the main path trend deviation judgment result.

[0038] S413: Based on the primary path trend deviation judgment result, analyze whether the primary-backup path switching condition is met. If the judgment value exceeds the deviation threshold, read the corresponding backup path channel number and current direction trend data, package the replacement action timestamp, replaced channel number, and new channel number, and obtain the primary path replacement confirmation status record; Based on the main path trend offset judgment result, the system determines whether the path replacement conditions are met. 0.15 is lower than the offset judgment threshold of 0.2, which does not meet the channel switching conditions. The system retains the current channel without replacing it. If the judgment value exceeds the threshold, the system automatically reads the previously registered alternative channel number and sets it to C05. At the same time, it collects the trend symbol sequence of the C05 channel in the current cycle and replaces the current main channel C01 with C05. The system records the timestamp of this replacement behavior as 2025-06-1114:23:01, with the channel number before replacement being C01 and after replacement being C05. The replacement reason is set to the judgment value exceeding the limit, with the reason code being TR001. The system writes the above four data items into the main and backup channel status structure to obtain the main path replacement confirmation status record.

[0039] See also Figure 6 , step S5 is: S511: Based on the main path replacement confirmation status record, read the main channel number of the new cycle and initialize the signal acquisition sequence of the current path accordingly. Perform differential processing on the voltage differences between sampling points in each cycle, extract the amplitude changes of adjacent sampling points, and retain all incremental values ​​corresponding to the channel in each cycle to generate a continuous cycle incremental sequence. Based on the main path replacement confirmation status record, the system first confirms the main control channel number that is currently active, sets the current main channel to C05, and starts the continuous cycle collection mechanism according to the channel identifier. The system goes back to the cycle , for C05 channel Sampling is performed within a total of 4 cycles of t. Assume that the sampling frequency is 20 Hz, there are 20 sampling points per cycle, and the total number of sampling points is 80. In each group of cycles, the adjacent difference processing is performed on the voltage values ​​of consecutive sampling points to extract the amplitude change. For example, the cycle The voltages of the first three sampling points are 2.01V, 2.04V, and 2.03V, so the corresponding incremental values ​​are +0.03V and −0.01V. The system processes all cycles in sequence, records the increments of all adjacent points to form a time series increment matrix, and establishes a sampling time index. The data of each cycle is further merged to form a complete channel operation sequence, in which the serial number information of all sampling points corresponds to the time index one by one. The incremental sequence data is recorded in the matrix in the form of a two-dimensional array, where the row represents the cycle position and the column represents the sampling point position. For example, the voltage of the 6th sampling point in the 2nd cycle is 2.36V, and the 5th sampling point is 2.31V, so the increment of this point is +0.05V. The system repeats the above operation to complete the full-cycle sampling increment extraction and data assembly for the C05 channel, and finally forms a continuous cycle incremental sequence data group; Table 5 Main channel continuous period sampling increment record table

[0040] As shown in Table 5, the system extracts the amplitude increment data of each period sampling point in time sequence to obtain a continuous period increment sequence.

[0041] S512: Based on the continuous cycle increment sequence, calculate the mean of the increment values ​​of all sampling points in each cycle, check whether the increment directions at all points are consistent, calculate the proportion of direction mutations, mark the channel as an available control path for the current cycle, and obtain the control output record of the current cycle; According to the continuous cycle increment sequence, the system averages the increment values ​​of all sampling points in each cycle, and sets the cycle The average value is 0.02V, is 0.03V, is 0.04V, t is 0.035V, the system calculates the difference between the means of each pair of adjacent cycles and records the deviation direction. If the difference is positive, it is marked as rising, and if it is negative, it is falling. Then, the trend change marks of a total of 60 points in the four groups of cycles are counted, and the trend consistency of the sampling points at the same position in adjacent cycles is judged. Set the cycle If the increments at the fifth point in t are +0.02V and +0.03V, the trends are consistent. If they are +0.03V and −0.01V, the trends are inconsistent. The system accumulates the total number of inconsistent trend points and calculates the inconsistency rate. It determines whether the ratio is less than 0.2 as the basis for trend stability. At the same time, the system reads the amplitude of the change in the mean value of the cycle increment and determines whether it is less than the set amplitude stability threshold. The threshold is set to 0.15V, which comes from the maximum stability boundary value extracted in the previous batch experiment. This value is the average of the maximum mean difference limits in 10 consecutive tests of the four channels under test loads of 5N, 10N, and 15N. The difference between the mean value and t is 0.005V, which is lower than 0.15V and meets the stability condition. The system determines that the channel is stable in terms of trend and amplitude change and obtains the control output record of the current cycle.

[0042] S513: Based on the current cycle control output record, the corresponding channel is registered as the main control path of the current cycle, bound to the channel number and archived. At the same time, the complementary channel number is frozen and retained, and does not participate in the current cycle control output process. The main channel number, complementary channel number and their associated identifiers are written into the servo control path configuration mapping table to establish the multi-sensor servo control path orientation result; According to the current cycle control output record, the system first writes the main channel number C05 corresponding to the trend value into the servo control path scheduling module as the main control path identifier of the current cycle, and then archives its corresponding output trend value. At the same time, it extracts the complementary channel number C01 recorded before the previous replacement, marks its status as a standby state, retains it in the servo structure but does not participate in the output control task of this cycle, and assigns path labels to C05 and C01 respectively, where C05 is marked as "main path" and C01 is marked as "standby path". Both are written into the path orientation structure table at the same time, and are accompanied by the current cycle index number t, channel valid mark 1 and standby mark 0. Finally, the channel pair structure update is completed, and the multi-sensor servo control path orientation result is established.

[0043] Multi-sensor based servo control system, including: The direction consistency check module obtains the sensor data of each channel, performs direction consistency check, and generates statistics on the number of channels with consistent directions; The trend conflict screening module determines the difference in the direction trend of the consistent channel over two consecutive cycles based on the number of channels with consistent directions, counts the number of abnormal channels, and generates the number of frozen channels. The active / standby path screening module obtains the amplitude change value of the valid channel sensor based on the number of frozen state channels, calculates the difference between the effective channel sensor and the target amplitude, and sorts them by difference. The channel with the smallest difference is recorded as the active channel, and the channel with the second smallest difference is recorded as the standby channel. The active / standby control path channel numbers are generated. The path symbol matching module extracts the main channel direction symbol based on the main and backup control path channel numbers and compares it with the target symbol. If there is an inconsistent trend, the module executes the channel switching and generates the main path replacement confirmation status record. The control path orientation module collects the new main channel continuous incremental sequence as the reference path based on the main path replacement confirmation status record, retains the complementary path information, and generates the multi-sensor servo control path orientation result.

[0044] The above are merely preferred embodiments of the present invention and do not limit the present invention in any other form. Any technician familiar with the profession may use the technical content disclosed above to change or modify it into an equivalent embodiment with equivalent changes and apply it to other fields. However, any simple modification, equivalent change and modification made to the above embodiment based on the technical essence of the present invention without departing from the content of the technical solution of the present invention shall still fall within the scope of protection of the technical solution of the present invention.

Claims

1. A servo control method based on multiple sensors, characterized in that: The following steps are involved: S1: Obtain sensor data from each channel within the periodic task, perform direction consistency check, and perform statistical marking processing to generate the number of channels with consistent directions; S2: Based on the number of channels with consistent directions, identify sensors whose direction trends are inconsistent in two consecutive cycles channel by channel, mark the safe stop state channels according to the redundancy protocol, accumulate the number of channels, and generate the number of frozen state channels; S3: Based on the number of frozen channels, counting the amplitude change of each valid channel sensor, calculating the difference between the amplitude change and the target control amplitude change, determining the main control path and the backup path according to the difference, and generating the main and backup control path channel numbers; S4: Verify the current primary control path according to the primary and backup control path channel numbers, match the current primary control path with the target control direction symbol, determine whether the primary and backup control path switching condition is triggered, replace the backup path with the new primary control path, and generate a primary path replacement confirmation status record; S5: Based on the main path replacement confirmation status record, collect a continuous period increment sequence from the current main control path, determine it as the current period control signal output reference path, retain the original complementary path, and generate a multi-sensor servo control path orientation result.

2. The multi-sensor based servo control method according to claim 1, characterized in that: The number of direction-consistent channels includes a channel number mark, a direction consistency counting result, and a security level mark; the number of frozen state channels includes a failed channel mark, a cycle count label, and a redundant deactivation status record; the primary and standby control path channel numbers include a minimum difference number and a sub-minimum difference number; the primary path replacement confirmation status record includes a replacement confirmation mark, a trend consistency label, and a path switching instruction; the multi-sensor servo control path orientation result includes a control signal reference path, a complementary path retention record, and a path orientation number record.

3. The multi-sensor based servo control method according to claim 1, characterized in that: The specific steps for obtaining the number of channels with consistent directions are: S111: Acquire sensor data corresponding to all channels in the periodic task, determine the relative difference between the direction value corresponding to each channel data and the initial direction reference value, compare the relative difference with the direction consistency offset threshold, obtain the direction consistency determination status of each channel in the current cycle, and generate a channel direction consistency status record; S112: Based on the channel direction consistency state record, filter the channel state values ​​with valid direction consistency state in the current cycle, compare the number of filtered channels with the total number of channels, determine the direction state matching status of the current cycle, and generate a direction consistency matching trend record; S113: According to the directional consistency matching trend record, the number of directional consistent channels in the current cycle is marked, and the sequence distribution information of the directional consistent channels in the cycle is obtained in combination with the channel sequence number. The periodic task ID and the period sequence number are read to establish an associated mapping to generate the number of directional consistent channels.

4. The multi-sensor based servo control method according to claim 1, characterized in that: The steps for obtaining the number of frozen state channels are specifically as follows: S211: Based on the number of channels with consistent directions, extract the direction consistency status flag of each channel in two consecutive cycles, compare the direction consistency flags of adjacent cycles based on the channel number to see if there is a change, identify the set of channel numbers whose direction consistency states reverse in consecutive cycles, count the number of channels, and generate the number of channels with inconsistent direction trends; S212: Based on the number of channels with inconsistent directional trends, a redundant channel configuration table is read, and the identified channel numbers are matched one by one with the parallel channel information in the redundant structure. The directional consistency status of the paired channels in the current cycle is compared. When any paired channel group is identified as inconsistent in the same cycle, the corresponding channel number is written into the safe stop flag set to establish the number of channels in the safe stop state. S213: According to the number of channels in the safe stop state, perform a periodic freeze counter accumulation operation on the channel numbers marked as the safe stop state to generate the number of channels in the frozen state.

5. The multi-sensor based servo control method according to claim 1, characterized in that: The steps for obtaining the channel numbers of the primary and backup control paths are as follows: S311: Based on the number of frozen channels, count the number of all remaining unfrozen channels in the current cycle, retain channels with stable direction consistency in two consecutive cycles and not marked as safe stops, extract the numbers to form a valid channel sequence, and generate the number of valid channels; S312: Based on the channel numbers recorded in the number of valid channels, respectively read the amplitude change value of the current cycle and the amplitude change value of the previous cycle corresponding to the sampling signal of each channel sensor, obtain the instantaneous amplitude difference of each channel, set the target control amplitude change value and calculate the amplitude difference evaluation value, sort all channels, record the channel number and difference corresponding to the minimum difference, and establish the channel number and amplitude difference of the main control path; S313: Based on the main control path channel number and amplitude difference, after excluding the channel with the minimum amplitude difference from all sorted channels, select the channel corresponding to the next minimum amplitude difference, record the number as the alternative path channel, and merge the main path channel number and the alternative path channel number into a dual-path configuration structure to establish the main and backup control path channel numbers.

6. The multi-sensor based servo control method according to claim 1, characterized in that: The steps for obtaining the primary path replacement confirmation status record are specifically as follows: S411: Based on the main control path channel number, extract the trend of the amplitude change of consecutive sampling points of the main control channel in the current cycle and the previous cycle, compare the difference between two adjacent sampling values, and mark the direction as positive if the latter value is greater than the former value, otherwise mark it as negative, forming a direction symbol sequence in each cycle, recording the number of positions with inconsistent direction changes, calculating the proportion of the corresponding position number in the total number of sampling points, and outputting the trend direction inconsistency rate; S412: Based on the trend direction inconsistency rate, a set target control direction symbol sequence is read and the directions of the sampling points of the current period are compared item by item. If the direction of the sampling point is inconsistent with the target direction, it is counted as a deviation point. The total number of deviation points is accumulated and divided by the total number of sampling points to obtain the target direction deviation ratio of the current period. The deviation ratio is compared with the set deviation judgment threshold. If the deviation ratio is less than the threshold, the current main path is retained to obtain the main path trend deviation judgment result. S413: Based on the main path trend deviation judgment result, analyze whether the main and backup path switching conditions are met. If the judgment value exceeds the deviation threshold, read the corresponding alternative path channel number and current direction trend data, package the timestamp of the replacement action, the replaced channel number, and the new channel number to obtain the main path replacement confirmation status record.

7. The multi-sensor based servo control method according to claim 1, characterized in that: The steps for obtaining the multi-sensor servo control path orientation result are specifically as follows: S511: Based on the main path replacement confirmation status record, read the main channel number of the new cycle, and initialize the signal acquisition sequence of the current path accordingly, perform differential processing on the voltage differences between sampling points in each cycle, extract the amplitude changes of adjacent sampling points, and retain all incremental values ​​corresponding to the channel in each cycle to generate a continuous cycle incremental sequence; S512: Based on the continuous periodic increment sequence, calculate the mean of the incremental values ​​of all sampling points in each period, check whether the incremental directions at all points are consistent, calculate the proportion of direction mutations, mark the channel as an available control path for the current period, and obtain the control output record of the current period; S513: According to the current cycle control output record, register the corresponding channel as the main control path of the current cycle, bind it to the channel number and archive it, and at the same time retain the complementary channel number, freeze the status and do not participate in the current cycle control output process, write the main channel number, complementary channel number and their associated identifiers into the servo control path configuration mapping table, and establish the multi-sensor servo control path orientation result.

8. A servo control system based on multiple sensors, characterized in that: The system is used to implement the multi-sensor based servo control method according to any one of claims 1 to 7, comprising: The direction consistency check module obtains the sensor data of each channel, performs direction consistency check, and generates statistics on the number of channels with consistent directions; The trend conflict screening module judges the difference in the direction trend of the consistent channel for two consecutive periods based on the number of channels with consistent directions, counts the number of abnormal channels, and generates the number of frozen state channels; The primary and backup path screening module obtains the amplitude change value of the valid channel sensor based on the number of frozen state channels, calculates the difference between the amplitude change value and the target amplitude, and sorts the values ​​by difference. The channel with the smallest difference is recorded as the primary channel, and the channel with the second smallest difference is recorded as the backup channel, and generates the channel numbers of the primary and backup control paths; The path symbol matching module extracts the main channel direction symbol according to the main and standby control path channel numbers and compares it with the target symbol. If there is an inconsistent trend, the channel is switched and a main path replacement confirmation status record is generated. The control path orientation module collects a new main channel continuous incremental sequence as a reference path based on the main path replacement confirmation status record, retains complementary path information, and generates a multi-sensor servo control path orientation result.

Images