Walking and grabbing control method and system applied to weather observation field inspection robot and storage medium

Abstract

The application provides a walking and grabbing control method and system applied to a meteorological observation field inspection robot and a storage medium, in a chassis suspension non-locking state, through a base inertial measurement unit, angular rate and acceleration disturbance time series containing wind disturbance and ground surface elastic deformation are collected, the disturbance is decomposed into a high-frequency gust component and a low-frequency ground surface creep component through adaptive frequency domain separation, a feedforward force compensation channel and a position compensation channel are respectively constructed, force compensation instructions and position compensation instructions are generated, combined with real-time joint configuration of a mechanical arm and Jacobian mapping relationship, through force-velocity admittance conversion and position-velocity gain conversion, joint speed space superposition and forward mapping are carried out, end speed compensation instructions are formed, after the end speed compensation instructions are superposed with task pre-planning speed instructions, a walking driving mechanism and a mechanical arm joint actuator are synchronously controlled, and instrument grabbing operation in the non-locking suspension state is realized. The application improves high dynamic adaptability and end positioning accuracy of mobile operation in a non-structured environment.

Description

Technical Field

[0001] This invention relates to the field of data processing, and in particular to a walking and grasping control method, system and storage medium for a meteorological observation field inspection robot. Background Technology

[0002] Meteorological observation field inspection robots need to walk along preset paths and perform grasping operations on observation instruments in outdoor environments. Their walking and grasping control involves the coordinated movement of the chassis and the operation of the robotic arm. In existing technologies, inspection robots typically use a locked chassis suspension to obtain a rigid operating base when performing grasping operations, suppressing the impact of external disturbances on the positioning of the robotic arm's end effector by eliminating the flexibility of the suspension system. However, in the actual operating scenarios of meteorological observation fields, gusts of wind and elastic deformation of the ground surface continuously act on the inspection robot's base. While locking the chassis suspension can reduce disturbance transmission to some extent, frequent locking and unlocking operations reduce the smoothness and efficiency of the inspection operation. Furthermore, the chassis in the locked state cannot adapt to sudden wind disturbances, causing momentary shaking and quasi-static displacement of the robotic arm's end effector, affecting the reliable execution of grasping operations in unstructured outdoor environments. Summary of the Invention

[0003] This invention provides a walking and grasping control method, system, and storage medium for a meteorological observation field inspection robot.

[0004] In a first aspect, embodiments of the present invention provide a walking and grasping control method for a meteorological observation field inspection robot, comprising:

[0005] The inertial sensing data stream collected by the base inertial measurement unit during the process of the inspection robot traveling along the preset inspection path in the meteorological observation field, when the chassis suspension of the inspection robot remains in an unlocked state, includes the timing of the base angular rate disturbance and the timing of the base acceleration disturbance caused by wind disturbance and surface elastic deformation.

[0006] Adaptive frequency domain separation is performed on the timing of the base angular rate disturbance and the timing of the base acceleration disturbance. The inertial sensing data stream is decomposed into high-frequency disturbance components and low-frequency disturbance components through time-varying frequency division boundaries. The high-frequency disturbance components include instantaneous angular rate fluctuations and instantaneous acceleration fluctuations caused by gusts, while the low-frequency disturbance components include quasi-static angle drift and quasi-static acceleration slow change caused by surface creep.

[0007] A feedforward force compensation channel is constructed based on high-frequency disturbance components. The instantaneous acceleration fluctuations in the high-frequency disturbance components are processed by acceleration feedforward to generate force compensation commands. A position compensation channel is constructed based on low-frequency disturbance components. The quasi-static angle drift in the low-frequency disturbance components is processed by angle integration to generate position compensation commands.

[0008] The robot acquires real-time joint configuration data of the robotic arm, calls the Jacobian mapping relationship corresponding to the real-time joint configuration data, converts the force compensation command into joint force compensation velocity contribution through the preset force-velocity admittance conversion relationship, converts the position compensation command into joint position compensation amount through the inverse mapping of the Jacobian mapping relationship, and then converts the joint position compensation amount into joint position compensation velocity contribution through the preset position compensation gain. After superimposing the joint force compensation velocity contribution and the joint position compensation velocity contribution in the joint velocity space, the robot generates the end-effector velocity compensation command through the forward mapping of the Jacobian mapping relationship.

[0009] The terminal velocity compensation command is superimposed with the pre-planned speed command of the task at the same timestamp to obtain the compensated terminal velocity command. Based on the compensated terminal velocity command, the walking drive mechanism and the mechanical arm joint execution mechanism of the inspection robot are synchronously controlled so that the inspection robot can perform the grasping operation of the target observation instrument in the meteorological observation field while the chassis suspension is kept unlocked.

[0010] Secondly, embodiments of the present invention provide a computer system, the computer device including a processor and a memory, the memory storing a computer program, the computer program being loaded and executed by the processor to implement the above-mentioned walking and grasping control method applied to a meteorological observation field inspection robot.

[0011] Thirdly, embodiments of the present invention provide a computer-readable storage medium storing a computer program, which is loaded and executed by a processor to implement the above-described walking and grasping control method for a meteorological observation field inspection robot.

[0012] The embodiments of the present invention have the following beneficial effects:

[0013] This invention, while maintaining the chassis suspension of the inspection robot in an unlocked state, directly senses angular rate and acceleration disturbances caused by wind disturbances and surface elastic deformation through the base inertial measurement unit. It then performs adaptive frequency domain separation processing on the inertial sensing data stream, decomposing the disturbance signal into a high-frequency disturbance component reflecting gusts and a low-frequency disturbance component reflecting surface creep, thereby achieving differentiated identification of different disturbance sources. For the high-frequency disturbance component, acceleration feedforward processing is performed along the feedforward force compensation channel to generate force compensation commands, quickly suppressing instantaneous end-effector jitter. For the low-frequency disturbance component, angle integration processing is performed along the position compensation channel to generate position compensation commands, continuously offsetting quasi-static offsets, thus forming a parallel compensation architecture. Furthermore, the Jacobian mapping relationship corresponding to the real-time joint configuration data of the robotic arm is obtained, and the force compensation commands and position compensation commands are uniformly transformed into end-effector velocity compensation commands, which are then superimposed on the pre-planned speed commands for the task. This enables the walking drive mechanism and the robotic arm joint actuators to collaboratively perform the grasping operation of the target observation instrument while the chassis is in an unlocked, flexible suspension state. This method abandons the conventional approach of providing a rigid operating base through a locked chassis suspension. Instead, it uses frequency domain separation and Jacobi mapping transformation to uniformly express the multidimensional disturbance compensation in the terminal velocity space, achieving efficient suppression of the combined disturbances of wind and surface deformation. This enhances the anti-disturbance precision operation capability and grasping stability of the meteorological observation field inspection robot in complex environments. Attached Figure Description

[0014] Figure 1 This is a schematic diagram of the application environment provided in the embodiments of the present invention;

[0015] Figure 2 This is a logic diagram of the walking and grasping control method for a meteorological observation field inspection robot provided in an embodiment of the present invention;

[0016] Figure 3 This is a flowchart illustrating the walking and grasping control method for a meteorological observation field inspection robot provided in an embodiment of the present invention.

[0017] Figure 4 This is a structural block diagram of the computer system provided in an embodiment of the present invention. Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be described in further detail below with reference to the accompanying drawings.

[0019] In some embodiments, the walking and grasping control method for meteorological observation field inspection robots provided in this invention is applied to, for example... Figure 1 The application environment shown. For example, as... Figure 1As shown, the application environment includes an inspection robot 10 and a computer system 20.

[0020] Computer system 20 provides analysis and control for inspection robot 10. Computer system 20 can be a single server, a server cluster consisting of multiple servers, or a cloud computing service center. It should be noted that the above... Figure 2 The descriptions provided are merely exemplary and explanatory. In exemplary embodiments, the functions of the inspection robot 10 and the computer system 20 can be flexibly configured and adjusted, and the embodiments of the present invention do not limit this.

[0021] Please combine Figure 2 and Figure 3 For reference, the walking and grasping control method for a meteorological observation field inspection robot provided by this invention includes the following steps S10~S500:

[0022] Step S100: Acquire the inertial sensing data stream collected by the base inertial measurement unit when the inspection robot is moving along the preset inspection path in the meteorological observation field and the chassis suspension of the inspection robot is kept in an unlocked state. The inertial sensing data stream includes the timing sequence of base angular rate disturbance and base acceleration disturbance caused by wind disturbance and surface elastic deformation.

[0023] Inspection robots refer to mobile intelligent equipment with autonomous navigation, environmental perception, and robotic arm operation capabilities. In this solution, they specifically refer to wheeled or tracked mobile platforms deployed at meteorological observation sites to perform automated inspection and target observation instrument grasping tasks.

[0024] A meteorological observation field is, for example, a standardized open-air site equipped with various meteorological observation instruments and equipment. These instruments include Stevenson screens, rain gauges, anemometers, radiometers, etc., arranged in rows or grids according to meteorological observation standards. The site surface is typically natural soil or artificial turf, subject to elastic deformation caused by factors such as temperature and humidity changes, rainfall, and freeze-thaw cycles. The preset inspection path is an optimal patrol route pre-planned based on the distribution of the observation instruments. This path consists of several waypoints connected sequentially. As the inspection robot travels along this path, it can sequentially cover all the observation instruments to be inspected and operated. The chassis suspension refers to the elastic support and vibration damping system that connects the inspection robot's walking wheel set to the vehicle chassis. It consists of components such as coil springs, hydraulic shock absorbers, and swing arm linkages. Its non-locking state means that the suspension system is in normal working mode, where the elastic elements can be freely compressed and rebounded, and the dampers can absorb vibration energy according to the preset damping coefficient. This state helps to reduce the impact of uneven terrain on the vehicle body during walking, but at the same time, it will cause the vehicle body base to produce a complex motion of low-frequency swaying and high-frequency vibration under external force disturbance.

[0025] The base-mounted inertial measurement unit (IMU) is an inertial sensing device rigidly connected to the chassis base of the inspection robot. It integrates a three-axis microelectromechanical system (MEMS) gyroscope and a three-axis MEMS accelerometer. The three axes correspond to the forward, lateral, and vertical axes of the robot's body coordinate system. The gyroscope detects the instantaneous angular rate of rotation along each axis using the Coriolis vibration principle, while the accelerometer detects the instantaneous linear acceleration along each axis using capacitive displacement or piezoresistive principles. The IMU continuously outputs digital signals after analog-to-digital conversion at a preset fixed sampling frequency. The inertial sensing data stream is a multi-dimensional time series continuously output by the base-mounted IMU at fixed time intervals. Each sampling frame contains measurement data from six channels, including three-axis angular rate values ​​and three-axis acceleration values. The data rate is typically set between 200 Hz and 1000 Hz to ensure full-band capture of dynamic disturbance signals. Wind disturbance refers to the aerodynamic load changes of natural wind acting on the inspection robot's body and onboard robotic arm. When gusts occur, the instantaneous wind pressure on the windward side of the vehicle causes transient angular rate fluctuations and linear acceleration fluctuations around multiple axes in the base. Surface elastic deformation refers to the slow, continuous deformation of the surface at the meteorological observation site caused by the combined effects of the inspection robot's moving load, changes in soil moisture content, and temperature gradients. The surface elastically sinks at the point where the wheels roll over it and gradually rebounds after the robot leaves. This deformation is transmitted to the suspension system through the tire contact surface, causing quasi-static angular drift and gradual changes in translational acceleration of the base. The base angular rate disturbance time sequence is a three-dimensional vector sequence composed of the measured values ​​from the three-axis gyroscope output channel in the inertial sensor data stream, arranged chronologically. Each element carries a timestamp label of the sampling time, reflecting the instantaneous attitude change rate of the base under the combined action of wind disturbance torque and uneven surface support torque. The base acceleration disturbance time series is a three-dimensional vector sequence composed of the measured values ​​of the three-axis accelerometer output channels in the inertial sensing data stream arranged in chronological order. It also carries a timestamp tag and reflects the instantaneous translational acceleration change of the base under the combined action of wind pressure load and surface deformation reaction force.

[0026] Step S200: Adaptive frequency domain separation is performed on the base angular rate disturbance time series and the base acceleration disturbance time series. The inertial sensing data stream is decomposed into high-frequency disturbance components and low-frequency disturbance components through time-varying frequency division boundaries. The high-frequency disturbance components include instantaneous angular rate fluctuations and instantaneous acceleration fluctuations caused by gusts, and the low-frequency disturbance components include quasi-static angle drift and quasi-static acceleration slow change caused by surface creep.

[0027] In one implementation, step S200 specifically includes the following steps S210 to S260:

[0028] Step S210: Perform complex wavelet transform on the base angular rate perturbation time series to generate an angular rate time-frequency distribution matrix with time scale resolution. Search for the wavelet coefficient modulus maxima path along the scale axis in the angular rate time-frequency distribution matrix to extract the instantaneous frequency trajectory of the angular rate and simultaneously obtain the instantaneous amplitude sequence of the angular rate.

[0029] Complex wavelet transform is a signal processing tool that maps one-dimensional time-domain signals to a two-dimensional time-scale plane. Compared with the traditional real wavelet transform, complex wavelet basis functions have two orthogonal components, real and imaginary, enabling simultaneous analysis of the instantaneous amplitude and phase information of the signal. The complex wavelet basis function used in this step is the Morlet complex wavelet, whose mother wavelet function is the product of a complex exponential carrier function and a Gaussian envelope function. Its frequency domain response is unimodal and without sidelobe leakage, exhibiting optimal uncertainty resolution in time-frequency localization characteristics, making it suitable for analyzing perturbation signals containing multiple frequency components. When performing complex wavelet transform on the base angular rate perturbation time series, the base angular rate perturbation time series is convolved along the time axis with complex wavelet basis functions of different scaling scales one by one. The convolution results form a two-dimensional complex matrix with rows corresponding to the time axis coordinates and columns corresponding to the scale axis coordinates; this matrix is ​​the angular rate time-frequency distribution matrix. In the angular velocity time-frequency distribution matrix, the magnitude of each element represents the wavelet coefficient amplitude of the corresponding frequency component at that time and scale coordinates, and the argument of the element represents the instantaneous phase of the corresponding frequency component. The row direction of the matrix can be converted into a time axis, and the column direction can be converted into a frequency axis according to the correspondence between scale and frequency. Thus, this matrix has dual-domain resolution capability of time and frequency. The wavelet coefficient magnitude maxima path refers to the continuous curve formed by searching for local magnitude maxima points along the scale axis on the magnitude surface of the angular velocity time-frequency distribution matrix and connecting them sequentially along the time axis. The magnitude maxima position on the scale axis at each moment indicates the scale value corresponding to the dominant instantaneous frequency at that moment.

[0030] When searching for the path to the maximum value of wavelet coefficient modulus, each time sampling column of the angular rate time-frequency distribution matrix is ​​traversed. The wavelet coefficient modulus values ​​are compared row by row along the scale axis from fine to coarse. Extreme points whose modulus values ​​are greater than the corresponding modulus values ​​of the two adjacent scales above and below and exceed a preset modulus noise floor threshold are marked. Then, based on the proximity and frequency continuity of extreme points on the scale axis, a greedy matching pursuit algorithm is used to connect isolated extreme points into multiple temporally continuous ridge trajectories. From all ridges, the main ridge is selected as the instantaneous frequency trajectory of the angular rate based on the proportion of signal energy. The instantaneous frequency trajectory of the angular rate is a one-dimensional function with time as the independent variable and the instantaneous frequency value corresponding to that moment as the dependent variable. The instantaneous frequency value at each moment is obtained by converting the scale value corresponding to the main ridge at that moment through a scale-frequency conversion relationship, which is determined by the ratio of the center frequency of the complex wavelet basis function to the sampling interval. The instantaneous amplitude sequence of angular rate is a sequence of wavelet coefficient magnitudes extracted along the instantaneous frequency trajectory of angular rate at each moment. Its physical meaning is the magnitude of the angular rate perturbation at the corresponding instantaneous frequency at each moment, and the unit is radians per second or degrees per second.

[0031] Step S220: Perform complex wavelet transform on the base acceleration disturbance time series to generate an acceleration time-frequency distribution matrix. Extract the instantaneous frequency trajectory of acceleration from the acceleration time-frequency distribution matrix by ridge tracking and simultaneously obtain the instantaneous amplitude sequence of acceleration.

[0032] This step applies a complex wavelet transform to the base acceleration perturbation time series using the same mathematical framework as step S210. The complex wavelet basis function is the Morlet complex wavelet, and its mother wavelet parameters remain completely consistent with those in step S210 to ensure the consistency of the scale-frequency mapping relationship between the angular rate channel and the acceleration channel in time-frequency analysis. The base acceleration perturbation time series is then continuously convolved with complex wavelet basis functions at different scaling scales to generate a two-dimensional complex matrix consisting of a time axis and a scale axis, i.e., the acceleration time-frequency distribution matrix. The magnitude of the matrix elements represents the energy distribution of the acceleration signal at each time and scale, and the argument of the matrix elements represents the instantaneous phase information of the acceleration signal. Ridge tracing is a technique for extracting the path of the main frequency components of a signal over time from a time-frequency distribution matrix. The specific algorithm for ridge tracing in this step is consistent with the method used in step S210 for searching for the magnitude maxima path; both are greedy matching tracing based on magnitude maxima detection and continuity constraints. However, the term "ridge tracing" emphasizes the extraction of continuous curves along the frequency direction, which aligns with the technical connotation of matrix magnitude maxima search, indicating the same type of algorithm operation. During ridge tracing, each time slice in the acceleration time-frequency distribution matrix is ​​traversed, and maxima points exceeding the local neighborhood threshold are identified along the scale axis. For the set of maxima points identified between adjacent time slices, a dynamic programming algorithm is used to solve for the optimal scale path under scale jump penalty constraints, resulting in a set of time-continuous optimal ridge trajectories. From multiple sets of ridges, the ridge with the highest energy concentration is selected as the instantaneous frequency trajectory of the acceleration according to the energy weighting factor. The instantaneous frequency trajectory of acceleration is a mapping function from time to instantaneous frequency values. The instantaneous frequency value at each moment is obtained from the scale value of the optimal ridge at that moment through the scale-frequency conversion relationship, reflecting the time-varying characteristics of the dominant frequency component in the acceleration disturbance. The instantaneous amplitude sequence of acceleration is the wavelet coefficient modulus extracted along the instantaneous frequency trajectory of acceleration at each moment. Its physical unit is meters per second squared or a fraction of the standard gravitational acceleration, reflecting the change of the amplitude of the acceleration disturbance at the dominant frequency over time.

[0033] Step S230: Input the instantaneous frequency trajectory of angular velocity and the instantaneous frequency trajectory of acceleration into the preset frequency division boundary judgment set respectively. Based on the comparison results of the frequency values ​​at each moment on the instantaneous frequency trajectory of angular velocity with the frequency division boundary, generate the frequency attribute label sequence of angular velocity, and generate the frequency attribute label sequence of acceleration in the same way.

[0034] The preset frequency division boundary determination set is a set of rules containing time-varying frequency division boundary parameters. This set defines the dividing line between the high-frequency band and the low-frequency band at any given time. The frequency division boundary itself is adaptively adjusted over time, and its adjustment is based on the frequency attribute marking statistics of the previous control cycle and the energy spectrum change trend of the environmental disturbance at the current time. In one embodiment of this scheme, the frequency division boundary determination set adopts a two-layer structure design: the bottom layer is a frequency division boundary time history curve generator, which receives the energy ratio of the high-frequency component and the low-frequency component of the previous cycle as feedback input, calculates the offset of the frequency division boundary in the current cycle through a proportional-integral adjustment algorithm, and outputs the time-varying frequency division boundary value at the current time after superimposing the offset with the preset reference frequency division boundary; the upper layer is a frequency attribute marking determiner, which compares the input instantaneous frequency value with the frequency division boundary value. If the instantaneous frequency value is greater than the frequency division boundary value, it outputs a mark as a gust disturbance frequency band; if the instantaneous frequency value is less than or equal to the frequency division boundary value, it outputs a mark as a creep disturbance frequency band. The instantaneous frequency trajectory of angular velocity is sampled point by point and input into a frequency attribute labeling determiner. The determiner extracts the instantaneous frequency value at each sampling moment and compares it with the time-varying frequency division boundary value at the same moment. According to the determination rules, the sampling moment is assigned a gust disturbance frequency band label or a creep disturbance frequency band label. The frequency attribute labels of all sampling moments are arranged in chronological order to generate the angular velocity frequency attribute label sequence. The generation process of the acceleration frequency attribute label sequence is exactly the same. The instantaneous frequency trajectory of acceleration is sampled point by point and input into the same frequency attribute labeling determiner. It is compared with the frequency division boundary value at the same moment, and the frequency attribute label of each sampling point is output and organized into a sequence in chronological order. Each label in the frequency attribute label sequence is a binary state variable. The gust disturbance frequency band label corresponds to the high-frequency component, and the creep disturbance frequency band label corresponds to the low-frequency component.

[0035] Step S240: Based on the angular rate frequency attribute marking sequence, extract the segments marked as gust disturbance frequency bands in the instantaneous angular rate amplitude sequence as high-frequency amplitude segments of angular rate; based on the acceleration frequency attribute marking sequence, extract the segments marked as gust disturbance frequency bands in the instantaneous acceleration amplitude sequence as high-frequency amplitude segments of acceleration.

[0036] The angular rate frequency attribute marker sequence is a one-dimensional discrete state sequence generated in step S230. Each element of the sequence corresponds one-to-one with a sampling point in the instantaneous amplitude sequence of angular rate in time, carrying state information of the gust disturbance frequency band marker or creep disturbance frequency band marker. The instantaneous amplitude sequence of angular rate is the instantaneous amplitude data of the angular rate channel synchronously acquired in step S210. The extraction operation uses the frequency attribute marker sequence as the basis for conditional filtering: iterate through all sampling positions of the frequency attribute marker sequence. When the marker corresponding to a certain sampling position is a gust disturbance frequency band, copy the element of the instantaneous amplitude sequence of angular rate corresponding to that sampling position to the corresponding time position of the high-frequency amplitude segment of angular rate; when the marker corresponding to a certain sampling position is a creep disturbance frequency band, set the element of the high-frequency amplitude segment corresponding to that position to zero or a blank marker. The high-frequency amplitude segments of angular rate obtained after traversal processing are a set of discrete sequences of the same length as the original sequence, but retaining the original amplitude only at the marked positions in the gust disturbance frequency band, with the remaining positions having zero or invalid values. This segment completely preserves the amplitude time distribution of the gust high-frequency disturbance component in the angular rate channel. The extraction of high-frequency amplitude segments of acceleration follows the same logic: using the acceleration frequency attribute marked sequence as the filtering condition, traversing all sampling positions of the marked sequence, and extracting the instantaneous acceleration amplitude sequence elements corresponding to the positions marked as gust disturbance frequency bands to form high-frequency amplitude segments of acceleration. The high-frequency amplitude segments of acceleration retain the instantaneous acceleration fluctuation amplitude information caused by gusts in the acceleration channel, and remove the low-frequency slowly varying amplitude components caused by surface creep.

[0037] Step S250: Perform amplitude envelope consistency detection on the high-frequency amplitude segments of angular rate and acceleration. When the cross-correlation coefficient of the amplitude envelopes of the two is lower than the preset cross-correlation threshold, perform bidirectional correction processing on the inconsistent time markers in the frequency attribute marker sequences of angular rate and acceleration.

[0038] In one implementation, step S250 specifically includes the following steps S251 to S256:

[0039] Step S251: Perform Hilbert transform on the high-frequency amplitude segment of angular rate to generate the analytic envelope of angular rate signal, and perform Hilbert transform on the high-frequency amplitude segment of acceleration to generate the analytic envelope of acceleration signal.

[0040] The Hilbert transform is an integral transform that converts a real signal into a complex analytic signal. For a given real-valued discrete-time sequence, the Hilbert transform obtains its orthogonal components by convolving the sequence with the impulse response function. These orthogonal components are 90 degrees out of phase with the original sequence. The high-frequency amplitude segment of the angular rate is transformed by the Hilbert transform to obtain the imaginary part sequence of the angular rate. The complex sequence formed by taking the original high-frequency amplitude segment of the angular rate as the real part and the imaginary part sequence of the angular rate as the imaginary part is the analytic angular rate signal. The magnitude of the analytic angular rate signal is calculated point-by-point, which is the arithmetic square root of the sum of the squares of the real and imaginary parts. The result is the envelope of the analytic angular rate signal. This envelope curve gives the contour of the instantaneous amplitude of the high-frequency amplitude segment of the angular rate at each sampling time. The generation of the acceleration analytical signal envelope adopts the same processing flow: perform Hilbert transform on the high-frequency amplitude segment of acceleration to obtain the imaginary part sequence of acceleration, construct the acceleration analytical signal with the original high-frequency amplitude segment of acceleration as the real part and the imaginary part sequence of acceleration as the imaginary part, and then take the modulus of the acceleration analytical signal point by point to generate the acceleration analytical signal envelope. The envelope curve reflects the instantaneous amplitude profile of the high-frequency amplitude segment of acceleration.

[0041] Step S252: Calculate the cross-correlation function of the angular rate analytical signal envelope and the acceleration analytical signal envelope within a preset sliding time window, and extract the maximum cross-correlation coefficient and the corresponding time delay within each time window.

[0042] The preset sliding time window refers to the time segmentation parameters for capturing signal segments by moving segment by segment along the time axis with a fixed window length and a fixed step size. The window length is usually chosen to match the basic period of the gust disturbance, and the step size is chosen to be smaller than the window length to ensure overlap between adjacent windows and avoid analytical omissions caused by boundary effects. The cross-correlation function is a statistical function that describes the correlation between two signals at different time offsets. For two real discrete sequences, the value of the cross-correlation function at each time delay index is equal to the normalized result of multiplying one sequence by the other sequence point by point after time delay shifting and then summing the results. Within each sliding time window, the angular velocity analytical signal envelope and the acceleration analytical signal envelope are each extracted into a subsequence corresponding to the window length. The correlation coefficient sequence is calculated for these two subsequences according to the cross-correlation function definition within the entire preset time delay range. The coefficient value with the largest absolute value is searched from the correlation coefficient sequence as the maximum cross-correlation coefficient for that time window. The time delay value corresponding to the maximum cross-correlation coefficient is the time delay. The physical meaning of the time delay is the amount by which the angular velocity envelope leads or lags behind the acceleration envelope within that time window.

[0043] Step S253: Compare the maximum cross-correlation coefficient of each time window with a preset cross-correlation threshold, and mark the time windows that are lower than the preset cross-correlation threshold as suspicious inconsistency intervals.

[0044] The preset cross-correlation threshold is a judgment threshold determined comprehensively based on the physical mechanism of gust disturbance and the dynamic coupling characteristics of the robot's structure. A reasonable setting of this threshold enables the algorithm to effectively distinguish between low correlation caused by natural differences in wind disturbance and low correlation caused by labeling errors. The maximum cross-correlation number calculated within each sliding time window is extracted and compared with the preset cross-correlation threshold. If the maximum cross-correlation value is less than the preset threshold, the start and end times of that time window are recorded as a suspicious inconsistency interval, indicating an abnormal matching degree of wind disturbance amplitude envelope between the angular rate and acceleration channels during that period, and that at least one channel in the angular rate frequency attribute labeling sequence or the acceleration frequency attribute labeling sequence may have a labeling error.

[0045] Step S254: Within the suspected inconsistency interval, compare the angular rate frequency attribute label sequence and the acceleration frequency attribute label sequence point by point to identify the sampling points with conflicting labels.

[0046] For each suspected inconsistency interval marked in step S253, extract the angular rate frequency attribute marker sequence segment and the acceleration frequency attribute marker sequence segment within the time range of that interval, and compare the marker values ​​of the two segments at the same timestamp sampling position point by point. If the marker values ​​of the two marker sequences at a certain sampling point are both in the gust disturbance frequency band or both in the creep disturbance frequency band, then the markers of that sampling point are consistent and no processing is required; if the marker of the angular rate frequency attribute marker sequence at a certain sampling point is in the gust disturbance frequency band while the marker of the acceleration frequency attribute marker sequence is in the creep disturbance frequency band, or the angular rate is in the creep disturbance frequency band while the acceleration is in the gust disturbance frequency band, then that sampling point is identified as a marker conflict sampling point, and the timestamp of the sampling point and the marker content of each conflicting party are recorded.

[0047] Step S255: For the sampling points with conflicting markers, trace back the original frequency values ​​from the instantaneous frequency trajectories of angular velocity and acceleration, respectively. Based on the distance between the original frequency value and the frequency division boundary and the instantaneous signal-to-noise ratio of the inertial sensing data stream at the sampling point, determine the priority confidence marker.

[0048] For each sampling point with conflicting labels identified in step S254, frequency value backtracking and confidence assessment are performed to determine the correct label to be used. The instantaneous frequency trajectory of angular rate is the instantaneous frequency time function dominated by the angular rate channel extracted in step S210, and the instantaneous frequency trajectory of acceleration is the instantaneous frequency time function dominated by the acceleration channel extracted in step S220. Both trajectories store the original frequency value corresponding to each sampling point. The distance between the original frequency value and the frequency division boundary refers to the absolute difference between the instantaneous frequency value at the sampling point and the time-varying frequency division boundary value at the same moment. The larger the distance, the farther the frequency component of the sampling point is from the frequency division boundary, and the higher the certainty of the frequency attribute determination; the smaller the distance, the closer the frequency component of the sampling point is to the frequency division boundary, and the higher the risk of misjudgment. Instantaneous signal-to-noise ratio (SNR) is an indicator describing the relative intensity of the effective disturbance signal and the noise floor measurement noise in the inertial sensing data stream at a sampling point. The estimated instantaneous SNR value for the angular rate channel is obtained by taking the logarithm of the ratio of the average energy of the non-principal ridge scale interval to the principal ridge energy in the angular rate time-frequency distribution matrix. The estimated instantaneous SNR value for the acceleration channel is calculated from the acceleration time-frequency distribution matrix in the same way. The priority confidence flag is the flag with higher confidence selected from the angular rate frequency attribute flag and the acceleration frequency attribute flag after comprehensive evaluation based on frequency distance and instantaneous SNR. This flag will be used to replace the flag of the conflicting counterpart.

[0049] In one implementation, step S255 specifically includes the following steps S2551 to S2556:

[0050] Step S2551: Obtain the original frequency value of the instantaneous frequency trajectory of the angular velocity at the marked collision sampling point, and the original frequency value of the instantaneous frequency trajectory of the acceleration.

[0051] The instantaneous angular velocity frequency value, i.e., the raw angular velocity frequency value, is retrieved from the table in the instantaneous angular velocity frequency trajectory data structure by looking up the timestamp index of the conflicting sampling point. The unit is Hertz. Similarly, the instantaneous acceleration frequency value, i.e., the raw acceleration frequency value, is retrieved from the table in the instantaneous acceleration frequency trajectory data structure by looking up the same timestamp. The raw angular velocity frequency value and the raw acceleration frequency value each represent the true values ​​of the dominant frequency components of the angular velocity and acceleration disturbances at that sampling point, respectively, without any marking processing.

[0052] Step S2552: Calculate the angular rate frequency distance between the original angular rate frequency value and the frequency division boundary, and the acceleration frequency distance between the original acceleration frequency value and the frequency division boundary.

[0053] Extract the time-varying frequency division boundary value corresponding to the timestamp of the conflicting sampling point from the frequency division boundary judgment set. The absolute value of the difference between the original angular rate frequency value and the frequency division boundary value is the angular rate frequency distance. This distance measures the degree of deviation of the angular rate frequency component from the division boundary, with Hertz as the unit. Similarly, the absolute value of the difference between the original acceleration frequency value and the frequency division boundary value is used to obtain the acceleration frequency distance. The larger the frequency distance value, the farther the frequency point is from the frequency division boundary, and the more robust the frequency attribute judgment of the corresponding channel at that moment; the smaller the frequency distance value, the closer the judgment is to the ambiguity boundary.

[0054] Step S2553: Obtain the instantaneous signal-to-noise ratio estimates of the angular rate channel and the acceleration channel of the inertial sensing data stream at the marked collision sampling points.

[0055] The modulus distribution of all wavelet coefficients along the scale axis at the sampling moment is extracted from the angular rate time-frequency distribution matrix. The sum of squared moduli at the scale corresponding to the main ridge and its neighboring scales is taken as the signal energy, and the sum of squared moduli in the remaining scale intervals is taken as the noise energy. The ratio of signal energy to noise energy is taken as the common logarithm and then multiplied by 10 to obtain the instantaneous signal-to-noise ratio estimate of the angular rate channel, in decibels. The calculation method for the instantaneous signal-to-noise ratio estimate of the acceleration channel is the same. The modulus distribution is extracted from the acceleration time-frequency distribution matrix according to the same time index. The sum of energy at the main ridge and its neighborhood is taken as the signal, and the sum of energy at the remaining scales is taken as the noise. The ratio is calculated, and then the logarithm is taken and multiplied by 10 to obtain the instantaneous signal-to-noise ratio estimate of the acceleration channel.

[0056] Step S2554: Input the estimated values ​​of angular rate frequency distance and instantaneous signal-to-noise ratio of angular rate channel into the preset angular rate confidence evaluation relationship to generate an angular rate tag confidence score, and input the estimated values ​​of acceleration frequency distance and instantaneous signal-to-noise ratio of acceleration channel into the preset acceleration confidence evaluation relationship to generate an acceleration tag confidence score.

[0057] The pre-defined angular rate confidence assessment relationship refers to the mapping relationship between two factors, frequency distance and instantaneous signal-to-noise ratio (SNR), and a single confidence score. In one implementation of this scheme, this mapping relationship adopts a combination of a two-dimensional lookup table function and a weighted combination: First, based on the numerical ranges of frequency distance and instantaneous SNR, the corresponding sub-item confidence scores are looked up in a pre-calibrated confidence assessment table. Then, the two sub-item scores are weighted and summed according to the frequency distance weight factor and the instantaneous SNR weight factor. The frequency distance weight factor and the instantaneous SNR weight factor are obtained through offline calibration. The calibration method involves injecting a synthetic perturbation signal with known frequency components and known SNR into the test platform, and determining the optimal weight combination by minimizing the overall labeling error rate through grid search. The angular rate label confidence score is a quantitative score of the credibility of the angular rate frequency attribute label at the conflict sampling point. The higher the score, the greater the probability that the label is judged as correct. The acceleration confidence assessment relationship employs a two-dimensional lookup table and weighted combination mapping method with the exact same structure as the angular rate confidence assessment relationship. Its lookup table content and weighting factors can share the same set of calibration parameters as the angular rate channel, because the measurement error characteristics of the angular rate and acceleration channels are statistically consistent on the same physical platform. The acceleration marker confidence score is a quantitative score of the confidence level of the acceleration frequency attribute marker at conflict sampling points.

[0058] Step S2555: Compare the confidence scores of the angular rate marker and the acceleration marker, and select the frequency attribute marker corresponding to the one with the higher confidence score as the priority confidence marker.

[0059] The confidence scores of the angular rate marker and the acceleration marker calculated in step S2554 are compared. If the confidence score of the angular rate marker is higher than that of the acceleration marker, the marker of the angular rate frequency attribute marker sequence at that sampling point is selected as the priority confidence marker; if the confidence score of the acceleration marker is higher than that of the angular rate marker, the marker of the acceleration frequency attribute marker sequence at that sampling point is selected as the priority confidence marker; if the two scores are equal, the marker of the one farther from the frequency division boundary is selected as the priority confidence marker.

[0060] Step S2556: After determining the priority confidence markers for multiple consecutive sampling points with conflicting markers, perform continuous smoothing on the obtained priority confidence marker sequence to eliminate isolated jump markers.

[0061] After processing steps S2551 to S2555, each of the consecutive conflicting sampling points is assigned a priority confidence marker. These priority confidence markers are arranged in chronological order to form a priority confidence marker sequence. The logic of the continuity smoothing operation is as follows: the priority confidence marker sequence is input into a sliding window mid-range filter processor, with the window width set to a preset small odd number, such as 3 or 5. At each window position, the occurrence frequency of gust disturbance frequency band markers and creep disturbance frequency band markers within the window is counted. The marker with the higher occurrence frequency replaces the marker at the center position of the window, thereby eliminating isolated jump markers of one or two sampling points caused by fluctuations in instantaneous signal-to-noise ratio or frequency distance estimation, ensuring reasonable continuity of the markers in time. The priority confidence marker sequence after continuity smoothing is the final marker sequence that can be used for correction.

[0062] Step S256: Assign the priority confidence flag to the corresponding conflict sampling point, update the angular rate frequency attribute flag sequence and the acceleration frequency attribute flag sequence, and re-extract the angular rate high-frequency amplitude segment and the acceleration high-frequency amplitude segment based on the updated flag sequence.

[0063] For each conflicting sampling point, the previously disputed angular rate frequency attribute marker or acceleration frequency attribute marker at that sampling point is replaced with the priority confidence marker determined in step S255, ensuring consistency between the two marker sequences at that sampling point. After replacing all conflicting sampling points, the angular rate frequency attribute marker sequence and the acceleration frequency attribute marker sequence become the updated marker sequences. These two sequences are consistent across all sampling point locations and both reflect the optimal frequency attribute determination after confidence assessment and continuity smoothing correction. Subsequently, following the same extraction logic as in step S240, the instantaneous amplitude sequence of angular rate is re-extracted based on the updated angular rate frequency attribute marker sequence to obtain the updated high-frequency amplitude segment of angular rate; similarly, the instantaneous amplitude sequence of acceleration is re-extracted based on the updated acceleration frequency attribute marker sequence to obtain the updated high-frequency amplitude segment of acceleration. The updated two high-frequency amplitude segments eliminate the amplitude loss or mis-inclusion problems caused by marker conflicts, providing more accurate input for subsequent signal reconstruction.

[0064] Step S260: Align the high-frequency amplitude segments of angular rate and acceleration after bidirectional correction in the time domain. Reconstruct the high-frequency amplitude segments of angular rate into a high-frequency reconstruction time sequence of angular rate using inverse wavelet transform, and reconstruct the high-frequency amplitude segments of acceleration into a high-frequency reconstruction time sequence of acceleration. The high-frequency perturbation components are formed by the high-frequency reconstruction time sequences of angular rate and acceleration. At the same time, extract the segments marked as creep perturbation frequency bands in the instantaneous amplitude sequence of angular rate and reconstruct them into low-frequency perturbation components.

[0065] After bidirectional correction, the high-frequency amplitude segments of angular rate and acceleration have completely consistent marking states and timestamp alignment bases on the time axis. Time-domain alignment involves checking the sampling timestamp arrays of the two amplitude segments to ensure there is no time offset or sampling point misalignment. If micro-timeline differences are detected due to potential introduction during correction, linear interpolation resampling is used to strictly align them to the same time reference grid point. Inverse wavelet transform is the process of reconstructing the original time-domain signal from wavelet coefficients processed in the time-frequency domain. Mathematically, it involves treating the non-zero amplitude retained at each sampling moment in the high-frequency amplitude segment of angular rate as wavelet coefficients, multiplying them by the complex wavelet basis functions at the corresponding time and scale, and summing and superimposing them across all scales to obtain the reconstructed high-frequency time series of angular rate. This time series completely reconstructs the time-domain waveform of the high-frequency gust disturbance component in the angular rate channel, removing the low-frequency creep component and background noise interference from the original signal. The high-frequency amplitude segment of acceleration is subjected to inverse wavelet transform in the same way. The non-zero amplitude at each time point in the high-frequency amplitude segment of acceleration is multiplied by the complex wavelet basis function and summed on all effective scales to reconstruct the high-frequency reconstruction time series of acceleration. This time series completely reconstructs the time-domain waveforms of the high-frequency disturbance component of gust in the acceleration channel.

[0066] The high-frequency reconstruction time series of angular rate and acceleration together constitute the high-frequency disturbance component. This component fully characterizes the multi-dimensional instantaneous dynamic impact of wind disturbance on the robot base, serving as the input for the subsequent feedforward force compensation channel. For the reconstruction of the low-frequency disturbance component: based on the updated angular rate frequency attribute labeling sequence, segments labeled as creep disturbance frequency bands, i.e., low-frequency amplitude segments of angular rate, are extracted from the instantaneous amplitude sequence of angular rate. These segments are then reconstructed into the low-frequency reconstruction time series of angular rate using inverse wavelet transform. Similarly, based on the updated acceleration frequency attribute labeling sequence, segments labeled as creep disturbance frequency bands, i.e., low-frequency amplitude segments of acceleration, are extracted from the instantaneous amplitude sequence of acceleration. These segments are then reconstructed into the low-frequency reconstruction time series of acceleration using inverse wavelet transform. The low-frequency reconstruction time series of angular rate and acceleration together constitute the low-frequency disturbance component. This component fully characterizes the quasi-static attitude drift and translational gradual change impact of ground creep on the robot base, serving as the input for the subsequent position compensation channel.

[0067] Step S300: Construct a feedforward force compensation channel based on the high-frequency disturbance component, perform acceleration feedforward processing on the instantaneous acceleration fluctuation in the high-frequency disturbance component to generate a force compensation command, and construct a position compensation channel based on the low-frequency disturbance component, perform angle integration processing on the quasi-static angle drift in the low-frequency disturbance component to generate a position compensation command.

[0068] The high-frequency disturbance component is a set of angular rate high-frequency reconstruction timing and acceleration high-frequency reconstruction timing output in step S260. The former reflects the instantaneous angular rate fluctuation of the base caused by wind disturbance, and the latter reflects the instantaneous linear acceleration fluctuation of the base caused by wind disturbance. The feedforward force compensation channel is a feedforward control path that takes the high-frequency disturbance component as input and the force compensation command of the robot arm joint space as output. The working mechanism of this channel is to convert the detected base acceleration disturbance into the compensation torque required by each joint in advance through dynamic mapping. Before the disturbance affects the robot arm end effector and causes pose deviation, the active force is applied to cancel it out. This belongs to the category of open-loop feedforward compensation. Acceleration feedforward processing refers to the entire process of using the acceleration high-frequency reconstruction timing as the calculation basis for the feedforward control quantity and converting it into the joint torque feedforward quantity through the robot arm dynamic model. It includes processing steps such as acceleration to force mapping, Coriolis force and centrifugal force compensation, phase correction, and torque limiting distribution. The force compensation command is a set of torque command values ​​that should be applied to each joint of the robotic arm, which is the final output of the feedforward force compensation channel. This command exists in the form of a multi-dimensional vector, where the vector dimension equals the number of degrees of freedom of the robotic arm. Each element corresponds to the torque compensation amount of a joint, and the physical unit is Newton-meters (Nm). The low-frequency disturbance component is a set of low-frequency reconstruction timing sequences for angular rate and acceleration, output from step S260. The former reflects the quasi-static angular drift rate of the base caused by ground creep, and the latter reflects the gradual change in quasi-static acceleration of the base caused by ground creep. The position compensation channel is a feedback compensation path that takes the low-frequency disturbance component as input and the position compensation command for the robotic arm's operating space as output. This channel converts the detected low-frequency angular drift and gradual acceleration changes into a position offset that should be applied to the end effector of the robotic arm to counteract the adverse effects of the slow drift of the base attitude on the absolute positioning accuracy of the robotic arm's end effector. Angle integration processing refers to the process of continuously integrating the low-frequency reconstruction timing of the angular rate along the time axis to obtain the cumulative angular offset. The integrated angular drift is then mapped using forward kinematics to obtain the estimated value of the end effector position drift. The position compensation command is a compensation vector, output by the position compensation channel, containing the three-dimensional linear and three-dimensional angular displacements of the robotic arm's end effector in the operating space, in meters and radians.

[0069] In one implementation, step S300 specifically includes the following steps S310 to S360:

[0070] Step S310: Input the high-frequency reconstruction timing of acceleration in the high-frequency disturbance component into the acceleration force mapping stage of the feedforward force compensation channel. In the acceleration force mapping stage, combine the joint space inertia matrix of the robotic arm dynamics model to convert the high-frequency reconstruction timing of acceleration into the joint space feedforward torque timing.

[0071] The high-frequency acceleration reconstruction timing sequence is the high-frequency reconstruction signal of the acceleration channel in the high-frequency disturbance component output in step S260. Its data form is a three-axis acceleration time series, with physical units of meters per second squared. Each sampling frame contains three linear acceleration components along the forward axis, lateral axis, and vertical axis of the robot's body coordinate system. This timing sequence completely preserves the instantaneous translational acceleration fluctuation characteristics caused by gusts acting on the base. The feedforward force compensation channel is a complete control path for dynamic feedforward compensation of high-frequency disturbances. This path is composed of an acceleration force mapping stage, a joint friction torque compensation stage, and a force limiting stage connected in series. The acceleration force mapping stage is the first-level processing stage of the feedforward force compensation channel, responsible for converting the base acceleration fluctuations into the driving torque required by each joint of the robotic arm. Its core lies in establishing a quantitative mapping relationship between acceleration and torque using the joint space inertia matrix in the robotic arm dynamics model. A robotic arm dynamics model is a mathematical model describing the relationship between the motion of each link and the driving torque of the joints. For example, a rigid body dynamics model based on the Newton-Euler recursion method can be used. This model can calculate three core dynamic parameters under any joint configuration: the joint space inertia matrix, the Coriolis force and centrifugal force coefficient matrix, and the gravitational torque vector. The joint space inertia matrix is ​​an important output of the robotic arm dynamics model. It is a symmetric positive definite square matrix with a dimension equal to the number of degrees of freedom of the robotic arm multiplied by the number of degrees of freedom of the robotic arm. The diagonal elements of the matrix represent the torque coefficients corresponding to the moment of inertia that each joint needs to overcome during its own acceleration motion. The off-diagonal elements represent the coupling torque coefficients that are transmitted to the other joint through mechanical coupling when two different joints accelerate. The physical dimensions of each element in the matrix are kilogram-square meters or kilogram-meters. The joint space feedforward torque time series is a multidimensional torque time series output by the acceleration force mapping stage. Each sampling frame contains the same number of torque values ​​as the number of degrees of freedom of the robotic arm. Each element corresponds sequentially to the feedforward amount of the driving torque of each joint of the robotic arm from the base end to the end end. The physical unit is Newton-meter.

[0072] In one implementation, step S310 specifically includes the following steps S311 to S316:

[0073] Step S311: Based on the real-time joint configuration data, extract the joint space inertia matrix corresponding to the current configuration from the robotic arm dynamics model. The joint space inertia matrix includes the self-inertia term of each joint and the inter-joint coupling inertia term.

[0074] Real-time joint configuration data is a vector composed of joint angular position values ​​sampled and output by encoders or position sensors at each joint of the robotic arm at the current moment. The vector dimension is equal to the number of degrees of freedom of the robotic arm, and each element is the current angular position of the corresponding joint, in radians. The specific method for extracting the joint space inertia matrix from the robotic arm dynamics model is as follows: Real-time joint configuration data is input as joint angle variables into the extrapolation iteration stage of the Newton-Euler recursive algorithm. The linear velocity, angular velocity, linear acceleration, and angular acceleration variables of each link are calculated recursively from the base end to the end end. Then, in the introverting iteration stage from the end end to the base end, the inertia coefficient term in the joint torque expression is calculated recursively for each link, finally assembling a symmetric positive definite joint space inertia matrix. The self-inertia term of the joint space inertia matrix refers to the inertia coefficient corresponding to each joint on the main diagonal of the matrix. Its physical meaning is the total rotational inertia that the joint needs to overcome when it accelerates with a unit angular acceleration while other joints remain stationary, referred to as the moment of inertia of its own links and subsequent links on the joint axis. The coupled inertia term refers to the inertial coupling coefficient between two different joints on the off-diagonal of the matrix. Its physical meaning is the coupling torque effect generated on one joint through the mechanical structure when one joint accelerates.

[0075] Step S312: Multiply the acceleration vector at each moment in the high-frequency acceleration reconstruction time series with the joint space inertia matrix to generate the initial joint space feedforward torque time series. The initial joint space feedforward torque time series contains torque components contributed by linear acceleration and angular acceleration, respectively.

[0076] Each sampling frame in the high-frequency acceleration reconstruction timing sequence is a vector containing three linear acceleration components. The reference coordinate system of this vector is the body coordinate system where the base inertial measurement unit is located. The multiplication operation specifically refers to performing a matrix-vector product calculation on the acceleration vector and the sub-blocks related to translational acceleration in the joint space inertial matrix. At the same time, combined with the positive kinematics transfer relationship of the current configuration of the robotic arm, the acceleration of the base body system is mapped to the center of mass of each link. Then, it is multiplied with the mass parameters of each link and converted to joint torque through the lever arm relationship. Finally, a set of joint torque values ​​at each sampling moment is generated. The torque values ​​at all sampling moments are arranged in chronological order to form the initial joint space feedforward torque timing sequence. The torque components in the initial joint space feedforward torque timing sequence include two contribution sources: the torque contributed by linear acceleration refers to the inertial reaction torque generated on the joint axis by the translational acceleration of the base through the mass of each link of the robotic arm, and the torque contributed by angular acceleration refers to the inertial reaction torque generated on the joint axis by the rotational acceleration of the base through the rotational inertia of each link. These two torque contributions are considered simultaneously and linearly superimposed in the matrix-vector product calculation.

[0077] Step S313: Obtain the Coriolis force and centrifugal force coefficient matrix of each joint of the robotic arm under the current configuration, calculate the Coriolis force and centrifugal force compensation torque according to the current motion speed, and superimpose the Coriolis force and centrifugal force compensation torque into the initial joint space feedforward torque timing sequence to generate the dynamically compensated feedforward torque timing sequence.

[0078] The Coriolis force and centrifugal force coefficient matrix is ​​a three-dimensional coefficient array in the robotic arm dynamics model that correlates the joint velocity product and joint torque. Its specific data is extracted from the internal extrapolation stage of the Newton-Euler recursive algorithm, and this coefficient array is fixed under a given joint configuration. The current motion velocity refers to the angular velocity value of each joint obtained from real-time joint configuration data through differential operations, with the physical unit being radians per second. The process of calculating the Coriolis force and centrifugal force compensation torque is as follows: the angular velocity values ​​of each joint are multiplied and accumulated pairwise according to the structure of the Coriolis force and centrifugal force coefficient matrix. That is, the elements in the joint angular velocity vector are multiplied one by one with the coefficients of the corresponding joint and corresponding velocity product channels in the coefficient array and summed to obtain the Coriolis force and centrifugal force compensation torque value for each joint. The compensation torque values ​​of all joints constitute a compensation torque vector. This compensation torque vector is then superimposed frame by frame onto the corresponding sampling frame of the initial joint space feedforward torque timing sequence to obtain the dynamically compensated feedforward torque timing sequence. The dynamically compensated feedforward torque timing sequence compensates for the torque demand generated by the Coriolis effect and centrifugal force effect during the joint movement of the robotic arm on the basis of the initial feedforward torque, thereby improving the dynamic accuracy of feedforward compensation during high-speed movement or large-amplitude attitude changes.

[0079] Step S314: Phase advance is performed on the feedforward torque timing after dynamic compensation. The response delay of the feedforward force compensation channel is compensated by the differential prediction link to generate the phase-corrected feedforward torque timing.

[0080] The feedforward force compensation channel experiences transmission and computation delays throughout its entire link, from sensor signal acquisition and signal processing to torque command output. This delay causes the feedforward torque to be applied later than the actual disturbance occurs, resulting in a compensation phase lag. The differential prediction stage refers to a prediction algorithm that extrapolates future signal values ​​using current and historical signal values. In this scheme, a linear extrapolation differential predictor based on first-order Taylor expansion is used. Its structure is as follows: the dynamically compensated feedforward torque value at the current sampling time, the feedforward torque value at the previous sampling time, and the time interval between them are taken. The torque change rate, i.e., the differential value, is calculated. This change rate is then multiplied by the estimated channel delay time to obtain the prediction increment. The current torque value is added to the prediction increment to obtain the phase-advanced feedforward torque value at the prediction time. After performing the differential prediction operation on all sampled frames in sequence, a phase-corrected feedforward torque timing sequence is generated. The phase of this timing sequence is ahead of the original feedforward torque timing sequence in the frequency domain. The amount of phase advance is matched with the channel delay time, so that the disturbance and the feedforward torque are aligned in time during physical execution.

[0081] Step S315: Input the phase-corrected feedforward torque timing sequence into the joint torque distribution stage. In the joint torque distribution stage, based on the instantaneous acceleration requirements and torque load margin of each joint, the phase-corrected feedforward torque timing sequence is redistributed between joints to generate the distributed joint space feedforward torque timing sequence.

[0082] The joint torque distribution module is an optimized redistribution processing module designed to address the issue of feedforward torque potentially exceeding the torque output capacity of certain joints. The instantaneous acceleration requirement of each joint refers to the approximately calculated joint angular acceleration requirement obtained by dividing the torque component of each joint in the phase-corrected feedforward torque timing sequence by its corresponding inertia coefficient. This reflects the acceleration capability required for the joint to complete feedforward compensation. Torque load margin refers to the difference between the peak torque that each joint drive motor can additionally provide under the current motion state and its currently output torque. This information is obtained from the real-time monitoring data of the joint servo drive unit, and the physical unit is Newton-meters (Nm). The execution logic for joint torque redistribution is as follows: Iterate through each joint. If the phase correction feedforward torque requirement of a joint exceeds its torque load margin, the excess portion is distributed to other joints with inertial coupling according to the coupling inertia coefficient ratio in the joint space inertia matrix. These other joints, still with torque margins, share this torque requirement. After redistribution, it ensures that the torque commands of each joint are within their torque load margin range. If, after redistribution, there are still cases exceeding the total capacity, the overall torque command is scaled proportionally according to the margin ratio of each joint to avoid torque saturation. The distributed joint space feedforward torque sequence is the feedforward torque sequence optimized by torque margin constraints.

[0083] Step S316: Output the allocated joint space feedforward torque timing as the joint space feedforward torque timing to the subsequent processing stage.

[0084] The allocated joint space feedforward torque timing has satisfied the torque execution capability constraints of each joint of the robotic arm at the torque limiting allocation level. This timing is then handed over to the subsequent joint friction torque compensation stage for further torque compensation superposition.

[0085] Step S320: Obtain the current motion speed of each joint of the robotic arm, query the preset joint friction characteristic curve based on the current motion speed, obtain the joint friction torque compensation amount, and superimpose the joint friction torque compensation amount with the joint space feedforward torque timing sequence to generate the initial force compensation torque timing sequence.

[0086] The current motion speed, i.e., the real-time angular velocity values ​​of each joint of the robotic arm, is obtained from joint angular position sensor data after numerical difference and low-pass filtering, with the physical unit being radians per second. The preset joint friction characteristic curves are joint friction models established offline through joint friction identification experiments. This model describes the mapping relationship between joint friction torque and joint motion speed. The friction model uses a composite friction model including Coulomb friction, viscous friction, and Stribeck friction terms. The Coulomb friction term provides a constant amplitude dry friction torque related to the sign of velocity; the viscous friction term provides a viscous drag torque linearly proportional to the magnitude of velocity; and the Stribeck friction term provides an exponential transition characteristic where friction decreases with increasing velocity in the low-speed region. The three friction terms are superimposed to form a complete joint friction characteristic curve. The query operation involves substituting the current motion speed of each joint into the corresponding joint's friction characteristic curve, and retrieving the corresponding Coulomb friction amplitude, viscous friction product coefficient, and Stribeck friction transition value from each component of the curve according to the velocity value position. The sum of these three components yields the friction torque compensation amount for that joint at the current speed. The friction torque compensation of each joint is added frame by frame and element by element to the torque value of the corresponding joint in the joint space feedforward torque timing sequence to obtain the initial force compensation torque timing sequence. This timing sequence includes both the dynamic torque of the base acceleration feedforward and the compensation torque of the joint's own friction characteristics.

[0087] Step S330: Input the initial force compensation torque timing sequence into the force limiting stage. In the force limiting stage, based on the torque output peak constraints of each joint of the robotic arm, the initial force compensation torque timing sequence is clipped to generate the limited force compensation torque timing sequence, and the limited force compensation torque timing sequence constitutes the force compensation command.

[0088] The force limiting stage is a processing unit that uses the maximum torque output of each joint's servo drive system as a constraint to safely limit the amplitude of the compensation torque command. The peak torque output constraint for each joint refers to the maximum output torque value allowed by the peak torque capability curve of each joint's servo motor at the current speed. This value is determined by the joint driver specifications and the current bus voltage, and typically decreases gradually as the speed increases. This peak constraint value is read in real-time from the joint servo drive unit. The specific operation of amplitude clipping is as follows: for each sampling frame of the initial force compensation torque timing sequence, the torque value of each joint is checked one by one. If the torque value of a certain joint exceeds the peak torque output constraint at the corresponding speed, the torque value is clipped to the peak constraint value. The clipping method is to keep the torque sign direction unchanged and limit its amplitude within the absolute value of the peak constraint; if the torque value is within the constraint range, the original value remains unchanged. The amplitude-limited force compensation torque timing sequence generated after amplitude clipping satisfies the torque output capability limit of the servo system at any sampling time and at any joint. The force compensation command is directly composed of the force compensation torque timing after the amplitude is limited, and this command is the final output of the feedforward force compensation channel.

[0089] Step S340: Input the low-frequency reconstruction timing of the angular rate in the low-frequency disturbance component into the angle integrator of the position compensation channel, and perform continuous time-domain integration on the low-frequency reconstruction timing of the angular rate through the angle integrator to generate the angle drift estimation timing.

[0090] The angular rate low-frequency reconstruction timing sequence is the low-frequency reconstruction signal of the angular rate channel in the low-frequency disturbance component output in step S260. Its data form is a three-axis angular rate time series, with the physical unit being radians per second. Each sampling frame contains angular rate components around the three coordinate axes of the robot's body coordinate system. This timing sequence completely preserves the angular rate of the quasi-static attitude change of the base caused by ground creep. The position compensation channel is the control path for position feedback compensation of low-frequency disturbances. This path consists of an angle integrator, a drift compensator, and an acceleration position transfer and fusion stage connected in series. The angle integrator is the front-end numerical integration processing stage of the position compensation channel. It performs recursive numerical integration along the time axis on the input angular rate low-frequency reconstruction timing sequence. The integration algorithm adopts the trapezoidal integration rule, that is, within each integration step, the arithmetic mean of the current sampled angular rate value and the previous sampled angular rate value is multiplied by the sampling time interval to obtain the angle increment. This increment is accumulated to the angle estimate value of the previous moment and the current moment's angle estimate value is output. An angle drift estimation time series is generated by independently integrating the angular rate components of each axis one by one using an integrator. This time series is represented in the form of a three-dimensional angle sequence, where each element is the cumulative angle drift of the corresponding axis since the start of integration, and the physical unit is radians.

[0091] Step S350: Input the angle drift estimation timing into the drift compensator, and use the preset drift correction relationship in the drift compensator to perform nonlinear compensation on the angle drift estimation timing to generate the corrected angle drift timing.

[0092] As one implementation method, step S350 specifically includes the following steps S351 to S356:

[0093] Step S351: Obtain the pose feedback data of the robotic arm end effector in the previous control cycle, and calculate the end effector pose deviation vector based on the deviation between the pose feedback data and the desired pose.

[0094] The pose feedback data of the robotic arm's end effector is obtained from the robotic arm joint encoder through forward kinematics calculation or measured by an external vision positioning system. This data represents the actual pose of the end effector in the operating space during the previous control cycle, including six dimensions: three-dimensional position coordinates and three-dimensional attitude angles. The desired pose is the target pose that the end effector should be in at this moment, as set according to the current task plan. It is also represented by a six-dimensional vector of three-dimensional position and three-dimensional attitude. Calculating the end effector pose deviation vector involves element-wise subtraction between the desired pose vector and the pose feedback vector. The position deviation is a three-dimensional translation vector, while the attitude deviation can be expressed using Euler angle deviation or equivalent axis angle deviation. This scheme uses the equivalent axis angle expression to convert the attitude deviation into a three-dimensional rotation vector. The direction of the rotation vector represents the equivalent rotation axis, and the magnitude of the vector represents the rotation angle. Thus, the position deviation and attitude deviation together constitute a six-dimensional end effector pose deviation vector.

[0095] Step S352: Project the end pose deviation vector onto the joint space through the inverse Jacobian mapping to generate the joint angle deviation feedback quantity, and synchronize and align the joint angle deviation feedback quantity with the angle drift estimation time series.

[0096] Inverse Jacobian mapping refers to the mathematical transformation that maps the end-effector's spatial velocity or micro-displacement to the joint spatial velocity or micro-displacement. Its matrix form is the pseudo-inverse of the robotic arm's Jacobian matrix. In this step, the end-effector pose deviation vector is treated as a micro-displacement, multiplied by the damped pseudo-inverse of the Jacobian matrix corresponding to the current configuration, to obtain the angular deviation feedback for each joint. This feedback represents the additional angle value required for each joint to eliminate the end-effector pose deviation. Synchronization alignment refers to matching the joint angular deviation feedback to the sampling frames at the corresponding timestamps in the angle drift estimation time series. If the sampling rates are inconsistent, interpolation or downsampling is used to unify them to the same time grid, ensuring that subsequent calculations are performed on corresponding data at the same time.

[0097] Step S353: Perform trend analysis on the angle drift estimation time series and extract the monotonic drift trend component and oscillating drift component from the angle drift estimation time series.

[0098] Trend analysis is a time series decomposition process performed separately for each axis component of the angle drift estimation time series. This scheme employs the moving window midpoint trend decomposition method: for each axis's angle drift estimation time series, a portion of the time series is covered by a sliding window with a length greater than the oscillation period. The median of the data within the window is calculated as the trend value at the center time of that window. After the window slides along the time axis, the monotonic drift trend component curve is obtained. The residual sequence obtained by subtracting the monotonic drift trend component from the original angle drift estimation time series is the oscillating drift component, which mainly reflects the cyclical fluctuations caused by factors such as surface elastic rebound and temperature fluctuations.

[0099] Step S354: Based on the slope parameter of the monotonic drift trend component and the amplitude parameter of the oscillating drift component, call the preset nonlinear compensation function to generate a compensation increment timing sequence opposite to the angular drift direction.

[0100] The slope parameter of the monotonic drift trend component is the local slope value obtained by differentiating the trend component curve, representing the monotonic rate of change of the drift angle. The amplitude parameter of the oscillating drift component is the amplitude range from the peak to the trough of the oscillating component within a local time window, representing the intensity of the drift oscillation. The preset nonlinear compensation function is a nonlinear bivariate function with slope and amplitude as dual inputs and compensation increment as output. This function is implemented using a radial basis function network structure: the input layer of the network contains two nodes corresponding to the normalized slope and amplitude, respectively; the hidden layer consists of several Gaussian radial basis function neurons, and the center vector and width parameters of each neuron are determined by an offline training clustering algorithm; the output layer linearly combines the output of the hidden layer and denormalizes it to obtain the compensation increment value, the direction of which is opposite to the current angle drift direction. The nonlinear compensation function is called sequentially for each local time segment of the monotonic drift trend component and the oscillating drift component to generate a compensation increment time series, which gives the correction amount to be superimposed on the angle drift estimate at each time step.

[0101] Step S355: The compensation increment timing sequence and the angle drift estimation timing sequence are superimposed point by point, and the superposition result is constrained and corrected by the joint angle deviation feedback to generate the corrected angle drift timing sequence.

[0102] Point-by-point overlay involves algebraically adding the compensation increment timing sequence generated in step S354 to the angle drift estimation timing sequence frame by frame at corresponding time sampling points to obtain the initially corrected angle drift value. Constraint correction uses the joint angle deviation feedback quantity generated in step S352 as a supervisory constraint term. The principle is as follows: the joint angle deviation feedback quantity is considered a closed-loop verification signal for the angle drift estimation correction effect. If a significant difference in amplitude or trend is observed between the initially corrected angle drift value and the joint angle deviation feedback quantity, the initial correction value is adjusted according to the amplitude direction of the deviation feedback quantity, ensuring that the corrected angle drift value is physically consistent with the pose deviation feedback of the robotic arm's end effector. The sequence after constraint correction is the corrected angle drift timing sequence.

[0103] Step S356: Perform boundary smoothing on the corrected angle drift timing to ensure that the corrected angle drift timing changes continuously within the motion range constraints of the robotic arm joints.

[0104] Boundary smoothing refers to the smoothing and limiting of drift angle values ​​that exceed the motion range constraints of each joint in the corrected angle drift timing, preventing angle drift compensation commands from exceeding the physical limits of the joints and causing execution abnormalities. Motion range constraints are the safe range between the maximum allowed forward and reverse rotation angles for each joint. The processing logic of the boundary smoother is as follows: when the corrected angle drift value approaches the joint motion range boundary, a smoothing limiting function with continuous first-order derivative characteristics gently confines the drift value within the boundary. The smoothing limiting function can use a family of hyperbolic tangent functions, and its transition zone width is preset according to the joint motion range and safety margin, ensuring that the output drift angle changes continuously and smoothly within the motion range constraints without abrupt changes.

[0105] Step S360: The corrected angle drift timing is fused with the acceleration low-frequency reconstruction timing in the low-frequency disturbance component. The acceleration low-frequency reconstruction timing is converted into displacement drift through double integration. The displacement drift is then vector-synthesized with the end displacement drift obtained by forward kinematic mapping from the corrected angle drift timing to obtain the comprehensive position drift. The position compensation command is then constructed based on the reverse compensation amount of the comprehensive position drift.

[0106] The low-frequency reconstruction timing sequence of acceleration is the low-frequency reconstructed signal of the acceleration channel in the low-frequency disturbance component output in step S260. The data format is a three-axis acceleration time series, with physical units of meters per second squared, describing the gradual change in quasi-static acceleration of the base caused by surface creep. Double integration refers to performing two consecutive numerical integrations along the time axis on the low-frequency reconstruction timing sequence of acceleration: the first integration uses the trapezoidal rule to integrate the linear acceleration to obtain the gradual change in linear velocity; the second integration again uses the trapezoidal rule to integrate the obtained gradual change in linear velocity to obtain the linear displacement drift. Both integrations require setting a reasonable initial zero reference at the beginning of the integration. The displacement drift obtained from the double integration of the low-frequency reconstruction timing sequence of acceleration reflects the positional shift of the base in the translational direction caused by surface creep. Forward kinematic mapping refers to transferring the angular drift at each moment in the corrected angular drift time series to the end effector of the robotic arm through a kinematic chain calculation. This yields the equivalent displacement drift of the end effector in the workspace caused by the base attitude drift. The calculation process involves multiplying homogeneous transformation matrices from the base to the end effector and converting the attitude angle to translational deviation using lever arm conversion. Vector synthesis involves adding the base translational displacement drift obtained from double integration with the end effector displacement drift caused by the base attitude drift obtained from forward kinematic mapping in the workspace, combining them into a comprehensive position drift containing three-dimensional displacement. The reverse compensation amount of the comprehensive position drift is a displacement vector formed by taking the opposite sign of each component of the comprehensive position drift. This reverse compensation amount is the position compensation command, which indicates that the end effector should add a small displacement in the workspace to offset the comprehensive position drift of the base and maintain the relative pose stability of the end effector with respect to the target observation instrument.

[0107] Step S400: Obtain the real-time joint configuration data of the inspection robot's robotic arm, call the Jacobian mapping relationship corresponding to the real-time joint configuration data, convert the force compensation command into the joint force compensation velocity contribution through the preset force-velocity admittance conversion relationship, convert the position compensation command into the joint position compensation amount through the inverse mapping of the Jacobian mapping relationship, and then convert the joint position compensation amount into the joint position compensation velocity contribution through the preset position compensation gain. After superimposing the joint force compensation velocity contribution and the joint position compensation velocity contribution in the joint velocity space, generate the end-effector velocity compensation command through the forward mapping of the Jacobian mapping relationship.

[0108] In one implementation, step S400 specifically includes the following steps S410 to S460:

[0109] Step S410: Extract the angular position values ​​of each joint from the real-time joint configuration data, obtain the joint angular velocity values ​​through differential operation, and use the joint angular position values ​​and joint angular velocity values ​​as joint state variables.

[0110] Real-time joint configuration data is typically uploaded by the robotic arm joint servo drivers via a fieldbus at fixed communication intervals. Each joint channel in the data frame contains angular position values ​​converted from encoder counts. Extracting the angular position values ​​of each joint involves arranging the angular position values ​​of each channel in the data frame into an angular position vector according to their joint index. Differential operation involves subtracting the angular position vector from the previous sampling time from the current sampling time's angular position vector, and dividing the resulting vector difference by the sampling period to obtain the joint angular velocity vector. This operation is performed in real-time each time new joint configuration data is received. The angular position vector and angular velocity vector are combined as a joint state variable, which fully describes the robotic arm's joint spatial motion state at the current moment.

[0111] Step S420: Input the joint state variables into the geometric Jacobian construction stage. In the geometric Jacobian construction stage, calculate the linear velocity Jacobian component and angular velocity Jacobian component column by column according to the kinematic parameters of the robot arm, and combine them to generate the Jacobian matrix of the current configuration.

[0112] The geometric Jacobian construction step is a processing module that calculates the mapping matrix from joint velocity to end effector velocity under the current configuration based on the kinematic model of the robotic arm. The kinematic parameters of the robotic arm include the length of each link, the rotation axis direction vector of each joint, and the fixed translation vector between the coordinate systems of each link. These parameters are pre-stored in the form of a Dienergetic Hartenberg parameter table or a modified Dienergetic Hartenberg parameter table. The construction process is carried out sequentially according to the joint index: For the i-th rotary joint, the linear velocity Jacobian component of its Jacobian matrix in the i-th column is calculated by the cross product of the rotation axis direction vector of the joint and the position vector pointing from the origin of the joint to the origin of the end effector. This component represents the instantaneous linear velocity vector generated at the end effector when the joint rotates at a unit angular velocity; the angular velocity Jacobian component in the i-th column is the rotation axis direction vector of the joint itself, representing the instantaneous angular velocity vector generated at the end effector when the joint rotates at a unit angular velocity. After all joint columns are calculated, they are spliced ​​together to form a Jacobian matrix with 6 rows and N columns representing the robot arm's degrees of freedom. The top 3 rows of the matrix correspond to linear velocity mapping, and the bottom 3 rows correspond to angular velocity mapping.

[0113] Step S430: Perform singular value decomposition on the Jacobian matrix, calculate the matrix condition number based on the singular values ​​obtained from the decomposition, and apply damping correction to the Jacobian matrix when the condition number exceeds a preset singularity threshold to obtain the damped Jacobian matrix and its pseudo-inverse matrix.

[0114] In one implementation, step S430 specifically includes the following steps S431 to S436:

[0115] Step S431: Perform singular value decomposition on the Jacobian matrix to obtain the left singular vector matrix, the singular value diagonal matrix, and the right singular vector matrix. Extract all non-zero singular values ​​from the singular value diagonal matrix.

[0116] Singular value decomposition (SVD) is implemented using a standard numerical linear algebra algorithm based on Haushold transform and Givens rotation. The algorithm first transforms the Jacobian matrix into an upper double-diagonal form through bidiagonalization, and then obtains the singular values ​​through implicit symmetric QR iteration. The decomposition output is denoted as matrix U, a 6×6 real orthogonal matrix; the singular value diagonal matrix is ​​denoted as matrix Sigma, a 6×N real rectangular diagonal matrix; and the right singular vector matrix is ​​denoted as matrix V, an N×N real orthogonal matrix. All positive numbers greater than a preset small tolerance are extracted row-wise from the diagonal positions of matrix Sigma as non-zero singular values, and arranged in descending order to form a singular value sequence.

[0117] Step S432: Calculate the ratio of the maximum singular value to the minimum singular value as the matrix condition number, and compare the matrix condition number with the preset singularity threshold.

[0118] From the singular value sequence obtained in step S431, extract the first element (maximum singular value) and the last element (minimum non-zero singular value), and calculate the ratio of the maximum singular value to the minimum singular value to obtain the matrix condition number. Compare this matrix condition number with a preset singularity threshold. If the condition number is greater than the singularity threshold, a damping correction process is triggered. If the condition number is less than or equal to the singularity threshold, the Jacobian matrix is ​​in the normal configuration range, and no damping correction is required.

[0119] Step S433: When the matrix condition number exceeds the preset singularity threshold, calculate the damping correction amount based on the minimum singular value and the preset damping coefficient, add the damping correction amount to each non-zero singular value, and generate the corrected singular value diagonal matrix.

[0120] The calculation of the damping correction follows the least squares principle of damping: the damping correction is the difference between the square of the preset damping coefficient divided by the sum of the minimum singular value and the preset damping coefficient, plus the square root of the minimum singular value itself, and the minimum singular value. The preset damping coefficient is selected based on the robot arm's task accuracy requirements and joint servo bandwidth. Its selection principle is to ensure that the damping correction introduces sufficient singular direction damping only when approaching the singular configuration, while the damping effect almost disappears when far from the singular configuration. The calculated damping correction is then superimposed onto each non-zero singular value of the singular value diagonal matrix, i.e., each non-zero singular value is added with the damping correction, resulting in the corrected singular value diagonal matrix.

[0121] Step S434: The damped Jacobian matrix is ​​reconstructed using the left singular vector matrix, the corrected singular value diagonal matrix, and the right singular vector matrix.

[0122] The reconstruction process is the inverse of matrix multiplication: first, multiply the left singular vector matrix U with the corrected singular value diagonal matrix to obtain an intermediate matrix; then, multiply the intermediate matrix with the transpose of the right singular vector matrix V. The product is the damped Jacobian matrix. Mathematically, the damped Jacobian matrix is ​​still a 6xN matrix; its difference from the original Jacobian matrix lies in the change in information content after the singular values ​​are damped and corrected.

[0123] Step S435: Solve for the pseudo-inverse of the damped Jacobian matrix by performing a standard pseudo-inverse operation on the damped Jacobian matrix, and use iterative optimization in the solution process to maintain numerical stability.

[0124] The standard pseudo-inverse operation selects the computation path based on the matrix dimension relationship: when the number of rows in the damped Jacobian matrix is ​​less than the number of columns, the right pseudo-inverse formula is used, i.e., the product matrix of the damped Jacobian matrix and its own transpose is first calculated, the inverse of this product matrix is ​​then multiplied by the transpose of the damped Jacobian matrix; when the number of rows is greater than the number of columns, the left pseudo-inverse formula is used. The inverse operation uses the Choleski decomposition method, utilizing the symmetric positive definiteness of the matrix to accelerate the solution. Iterative optimization refers to using the pseudo-inverse matrix result of the previous control cycle as the initial value for the current cycle in the pseudo-inverse calculation, and performing optimization updates in a finite number of steps through Newton iteration or fixed-point iteration to improve the time continuity of the pseudo-inverse matrix and reduce the computational cost per iteration.

[0125] Step S436: Output the damping Jacobian matrix and its pseudo-inverse to the force-velocity mapping stage and the position-velocity mapping stage, respectively.

[0126] The damping Jacobian matrix is ​​used for the forward mapping in step S460, and the pseudo-inverse matrix of the damping Jacobian matrix is ​​used for the position and velocity mapping in step S440. The two matrices are derived from the same singular value decomposition and damping correction results, ensuring mathematical consistency between the forward and inverse mappings.

[0127] Step S440: Input the force compensation command to the force-velocity mapping stage. In the force-velocity mapping stage, the force compensation command is converted into a joint force compensation velocity contribution using a preset joint space admittance matrix. At the same time, the position compensation command is input to the position-velocity mapping stage. First, the position compensation command is converted into a joint position compensation offset using the pseudo-inverse of the damping Jacobian matrix. Then, the joint position compensation offset is converted into a joint position compensation velocity contribution using a preset position compensation gain coefficient.

[0128] The force-velocity mapping unit is the conversion unit from force compensation commands to velocity compensation commands. Its core preset joint space admittance matrix is ​​an N-order diagonal matrix, where N is the number of degrees of freedom of the robotic arm, and the diagonal elements are the virtual admittance coefficients of each joint. The physical meaning of the virtual admittance coefficient is the steady-state angular velocity response of the joint when subjected to a unit external torque. The conversion operation involves right-multiplying the force compensation command vector by the joint space admittance matrix to obtain the joint force compensation velocity contribution vector. Each element in this vector represents the motion velocity required by the corresponding joint to adapt to the wind disturbance torque. The position-velocity mapping unit is executed in two steps: First, the position compensation command, i.e., the six-dimensional displacement compensation vector in the operation space, is left-multiplied by the pseudo-inverse of the damped Jacobian matrix to obtain the joint position compensation offset vector. This vector represents the angular offset value that each joint needs to rotate to generate the corresponding end-effector displacement compensation. Then, the joint position compensation offset vector is multiplied by a preset position compensation gain coefficient matrix. This coefficient matrix is ​​usually a diagonal matrix, with each diagonal element being a gain coefficient whose dimension is the reciprocal of the frequency. The result of the multiplication is the joint position compensation velocity contribution vector.

[0129] Step S450: The joint force compensation velocity contribution and the joint position compensation velocity contribution are superimposed in the same dimension in the joint velocity space to generate a joint space composite velocity vector, and a zero space self-motion component is introduced into the joint space composite velocity vector.

[0130] Same-dimensional superposition refers to algebraically adding the joint force compensation velocity contribution vector and the joint position compensation velocity contribution vector element by element according to the same joint index order. The result of the summation is the main part of the joint space synthesized velocity vector. The null space self-motion component refers to using the null space of the Jacobian matrix, that is, the velocity subspace in which the joint velocity is mapped to a zero vector in the operation space, to construct an internal motion velocity that does not produce end-effector motion but only changes the joint's own configuration. Superimposing this self-motion velocity onto the joint space synthesized velocity vector can optimize the joint configuration of the robotic arm without affecting the end-effector compensation velocity, keeping it away from joint limit boundaries or singular configuration regions.

[0131] In one implementation, step S450 specifically includes the following steps S451 to S456:

[0132] Step S451: Obtain the current angular position of each joint of the robotic arm, and calculate the joint displacement optimization gradient direction based on the deviation between the current angular position and the preset intermediate position of each joint.

[0133] The current angular position of each joint of the robotic arm is obtained from real-time joint configuration data. The preset intermediate position of each joint is a pre-defined center position of the joint's range of motion or a reference configuration that optimizes the mechanical performance of the robotic arm. This preset intermediate position is determined comprehensively based on the midpoint of the joint's range of motion and task requirements. The method for calculating the joint displacement optimization gradient direction is as follows: subtract the current angular position vector from the preset intermediate position vector to obtain the joint displacement deviation vector. Keep the sign of each element of this deviation vector unchanged. The normalized direction of this deviation vector is the joint displacement optimization gradient direction, which points to the steepest descent direction that causes each joint to regress to its preset intermediate position.

[0134] Step S452: Project the optimized gradient direction of the joint displacement onto the null space of the Jacobian matrix to obtain the self-motion velocity direction in the null space. The null space consists of a set of vectors that satisfy the projection relationship of the Jacobian matrix into the null space.

[0135] The null space of the Jacobian matrix refers to the set of all N-dimensional vectors that satisfy the condition that multiplying the Jacobian matrix by the vector results in zero, where N is the number of degrees of freedom of the robotic arm. The mathematical projection matrix of the null space is obtained by subtracting the product of the pseudo-inverse of the Jacobian matrix and the Jacobian matrix from the identity matrix. Multiplying the joint displacement optimization gradient direction vector by this null space projection matrix yields the null space self-motion velocity direction vector. This vector lies entirely within the null space in the joint velocity space, and the resulting joint motion does not affect the velocity output of the robotic arm's end effector in the operating space.

[0136] Step S453: Calculate the joint limit margin weight matrix based on the proximity of the current configuration of the robotic arm to the joint limit boundary. Each diagonal element in the joint limit margin weight matrix is ​​associated with the angular margin of the joint from the limit boundary.

[0137] Joint limit boundaries include the upper and lower angle values ​​of the joint's range of motion. The angular margin of a joint from the limit boundary is defined as the absolute value of the angle difference between the current joint angle position and the nearest limit boundary. The joint limit margin weight matrix is ​​an N-order diagonal matrix. Its diagonal elements are calculated as follows: the angular margin of each joint is input into a limit margin weight function. When the angular margin is large, the function outputs a value close to 1, indicating that the joint can move freely in zero space; when the angular margin shrinks to near zero, the output value rapidly decays to 0, preventing the joint from continuing to move towards the limit boundary. This function is implemented using an S-shaped decreasing function or a piecewise polynomial smooth decay function.

[0138] Step S454: Multiply the zero-space self-motion velocity direction with the joint constraint margin weight matrix to generate the weighted zero-space self-motion velocity.

[0139] The zero-space self-motion velocity direction vector obtained in step S452 is multiplied by the joint limit margin weight matrix obtained in step S453. The result of the multiplication is the weighted zero-space self-motion velocity vector. The closer the joint is to the limit boundary, the more significantly its self-motion velocity component is attenuated, thus avoiding self-motion driving the joint into the limit danger zone.

[0140] Step S455: Superimpose the weighted null space self-motion velocity with the joint space synthesized velocity vector to generate a modified joint space synthesized velocity vector that includes the null space component.

[0141] The weighted null space self-motion velocity vector output in step S454 is added element by element to the joint space synthesized velocity vector generated in the previous step S450 to obtain the corrected joint space synthesized velocity vector. This vector retains the velocity components required for end-effector compensation while adding null space self-motion velocity components, which can be used to perform joint configuration self-optimization without affecting end-effector motion.

[0142] Step S456: Monitor the amplitude of the modified joint space synthesized velocity vector. When the velocity amplitude of any joint exceeds the preset velocity limit of that joint, scale the modified joint space synthesized velocity vector proportionally.

[0143] The preset speed limit is the maximum allowable angular velocity value for each joint, determined by the upper limit of the joint servo drive and reduction mechanism. Amplitude monitoring iterates through each element of the corrected joint space composite velocity vector, finds the maximum absolute value, and compares it with the preset speed limit of the corresponding joint. If the maximum value exceeds the limit, a scaling factor is calculated by dividing the preset speed limit value by the maximum value. All elements in the entire corrected joint space composite velocity vector are multiplied by this scaling factor to achieve proportional scaling, ensuring that the speed commands of all joints do not exceed their respective speed limits.

[0144] Step S460: The joint space synthesized velocity vector after introducing the zero-space self-motion component is positively mapped to the operation space through the Jacobian matrix to generate an end velocity compensation command containing linear velocity compensation components and angular velocity compensation components.

[0145] The joint space synthesized velocity vector after introducing the zero-space self-motion component is the corrected joint space synthesized velocity vector output in step S450. The Jacobian matrix forward mapping refers to left-multiplying this vector by the damped Jacobian matrix output in step S430. The product is a six-dimensional operational space velocity vector, where the first three dimensions are linear velocity compensation components in meters per second, and the last three dimensions are angular velocity compensation components in radians per second. This six-dimensional velocity vector is the end-effector velocity compensation command, reflecting the comprehensive compensation velocity of the robotic arm end effector required to simultaneously counteract wind disturbance and ground creep.

[0146] Step S500: Overlay the end-effector speed compensation command with the pre-planned speed command of the task at the same timestamp to obtain the compensated end-effector speed command. Synchronously control the walking drive mechanism and the mechanical arm joint execution mechanism of the inspection robot according to the compensated end-effector speed command, so that the inspection robot can perform the grasping operation of the target observation instrument in the meteorological observation field while the chassis suspension is kept unlocked.

[0147] The pre-planned speed command is the end-effector velocity command pre-calculated offline by the inspection robot based on the preset inspection path and operation task, or generated in real time by the upper-level task planner. This command includes the pre-planned end-effector linear velocity timing and the pre-planned end-effector angular velocity timing, which respectively describe the translational and rotational velocities that the robotic arm end-effector should execute in the operating space under ideal conditions without considering disturbance compensation. Simultaneous timestamp overlay refers to algebraically adding the linear velocity compensation component and angular velocity compensation component in the end-effector velocity compensation command frame-by-frame with the sampled values ​​of the same timestamp in the pre-planned end-effector linear velocity timing and the pre-planned end-effector angular velocity timing, according to their respective sampling time timestamps. This allows the compensation speed correction caused by wind disturbance and ground creep to be directly added to the pre-planned motion speed, generating the compensated end-effector velocity command. Synchronous control of the walking drive mechanism and the robotic arm joint actuator refers to the joint scheduling of the inspection robot chassis moving platform and the vehicle-mounted multi-joint robotic arm according to the compensated end-effector velocity command. This ensures that the chassis movement and the robotic arm end-effector movement are coordinated, suppressing the influence of base disturbances while precisely guiding the robotic arm end-effector to the location of the target observation instrument to complete the grasping operation. The chassis suspension remains in an unlocked state, which is the working mode of the suspension system that is maintained throughout the aforementioned steps. In this mode, the robot has good terrain adaptability but significant base disturbance. The entire control method of this solution is designed for this contradictory working condition, and achieves high-precision operation capability of the robotic arm end in the unlocked suspension state through online disturbance compensation.

[0148] As one implementation method, step S500 specifically includes the following steps S510 to S560:

[0149] Step S510: Parse the pre-planned end linear velocity timing and pre-planned end angular velocity timing from the pre-planned speed command, and align the linear velocity compensation component and angular velocity compensation component in the end velocity compensation command with the pre-planned end linear velocity timing and pre-planned end angular velocity timing according to the timestamp.

[0150] Task pre-planned velocity commands are typically stored or transmitted as a sequence of structured data frames. Each frame contains a timestamp field, a three-dimensional linear velocity field, and a three-dimensional angular velocity field. The parsing operation extracts the timestamp array, linear velocity array, and angular velocity array from all data frames according to the data protocol, forming the pre-planned end-point linear velocity timing sequence and the pre-planned end-point angular velocity timing sequence, respectively. The end-point velocity compensation command is also a six-dimensional velocity vector sequence carrying timestamps. Alignment is achieved through timestamp matching: for each timestamp in the end-point velocity compensation command, a corresponding sampling frame within a preset tolerance range is found in the pre-planned timing sequence. If the timestamps are completely identical, they are directly matched; if there is a slight deviation, the pre-planned velocity value is linearly interpolated to obtain the velocity value at the alignment time, unifying the sampling times of both to the same time reference grid.

[0151] Step S520: Superimpose the aligned linear velocity compensation component onto the pre-planned end linear velocity timing sequence to generate the compensated end linear velocity timing sequence; superimpose the aligned angular velocity compensation component onto the pre-planned end angular velocity timing sequence to generate the compensated end angular velocity timing sequence.

[0152] The aligned linear velocity compensation component and the pre-planned terminal linear velocity timing are unified to the same time grid. Vector addition is performed frame-by-frame at each sampling time, adding the corresponding three-dimensional elements of the linear velocity compensation component to the three-dimensional elements of the pre-planned linear velocity to obtain the compensated terminal linear velocity timing. Similarly, the angular velocity compensation component and the pre-planned terminal angular velocity timing are vector-by-vector added frame-by-frame to obtain the compensated terminal angular velocity timing. The superimposed velocity timing integrates the pre-planned motion trend of the task and the real-time perturbation compensation correction.

[0153] Step S530: Combine the compensated end linear velocity timing and the compensated end angular velocity timing into a compensated end velocity command, and input the compensated end velocity command into the decoupling distributor.

[0154] The combined operation concatenates the three-dimensional compensated linear velocity and three-dimensional compensated angular velocity at the same moment into a six-dimensional vector. The six-dimensional vectors at all moments are arranged in time sequence to form the compensated end-effector velocity command. The decoupling distributor is a functional module used to decompose the end-effector composite velocity command into chassis walking speed and robotic arm end-effector motion speed. Its decomposition is based on the chassis kinematic model of the inspection robot and the coordinate transformation relationship between the robotic arm and the chassis.

[0155] Step S540: In the decoupling distributor, the compensated end-effector speed command is decomposed into a chassis travel speed command and a robotic arm end-effector execution speed command according to the chassis kinematic constraints of the inspection robot. The chassis travel speed command includes the differential control amount of the left and right wheel sets.

[0156] The chassis kinematic constraints of the inspection robot include a differential drive kinematic model and a maximum speed and acceleration limit for the chassis. The differential drive chassis consists of two independently driven wheel sets, left and right. Steering is achieved by controlling the speed difference between the left and right wheels, and straight-line movement is achieved by controlling the common mode speed. The decoupling distributor uses the differential drive kinematic model of the chassis to allocate the portion of the compensated end-effector speed that can be achieved by chassis motion to the walking drive mechanism, and allocates the remaining portion that must be executed by the robotic arm to the robotic arm joint actuators.

[0157] In one implementation, step S540 specifically includes the following steps S541 to S546:

[0158] Step S541: Obtain the relative pose transformation relationship between the chassis coordinate system and the robot arm base coordinate system of the inspection robot. Based on the relative pose transformation relationship, transform the operation space velocity vector in the compensated end-effector velocity command to the chassis coordinate system to obtain the chassis system velocity vector.

[0159] The chassis coordinate system is a vehicle coordinate system fixed to the chassis, with the geometric center of the inspection robot chassis or the axle of the drive wheel set as the origin, the forward direction as the X-axis, and the direction perpendicular to the ground as the Z-axis. The robot arm base coordinate system is the reference coordinate system at the robot arm mounting base, connected to the chassis via a fixed mechanical interface. The relative pose transformation relationship is a homogeneous transformation matrix, containing a rotation matrix and a translation vector. This transformation relationship is obtained during robot factory calibration through dimensional measurements and mounting surface geometric calibration and is stored as a fixed parameter. The transformation operation process is as follows: the operational space velocity vector in the compensated end-effector velocity command is mapped from the robot arm base coordinate system to the chassis coordinate system through an adjoint transformation. The adjoint transformation matrix is ​​composed of the rotation matrix and its cross product antisymmetric matrix from the relative pose transformation relationship, combined with the translation vector. After mapping, the chassis system velocity vector is obtained, which represents the equivalent description of the robot arm end-effector velocity in the chassis coordinate system.

[0160] Step S542: Based on the linear velocity component in the chassis velocity vector and the differential drive kinematic model of the inspection robot, calculate the speed difference distribution ratio and common mode speed of the left and right wheel sets, and generate the differential control quantity of the left and right wheels.

[0161] The differential drive kinematic model of the inspection robot describes the quantitative relationship between the chassis linear velocity, angular velocity, and the rotational speeds of the left and right wheels: the chassis linear velocity equals the average rotational speed of the left and right wheels multiplied by the wheel assembly radius; the chassis angular velocity equals the difference between the right wheel rotational speed and the left wheel rotational speed multiplied by the wheel assembly radius and then divided by the wheelbase. The rotational speeds of the left and right wheels can be inversely calculated from the horizontal linear velocity component and the angular velocity component about the vertical axis in the chassis system velocity vector: the common mode rotational speed equals the linear velocity divided by the wheel assembly radius; the rotational speed difference distribution ratio equals the angular velocity multiplied by half the wheelbase and then divided by the wheel assembly radius. The differential speed control quantities for the left and right wheels are obtained by subtracting and adding the rotational speed difference distribution ratio from the common mode rotational speed, respectively, in radians per second. This control quantity is the core content of the chassis travel speed command.

[0162] Step S543: Compare the velocity vector of the operating space with the velocity vector of the chassis system, extract the end-effector velocity component contributed only by the robotic arm, and generate a pure robotic arm end-effector velocity vector.

[0163] The vector comparison operation involves subtracting the original operational space velocity vector in the robot arm base coordinate system from the chassis system velocity vector, which is then transformed back to the robot arm base coordinate system through an adjoint transformation. The result of the subtraction is the pure robot arm end-effector velocity vector. This vector represents the end-effector velocity component that must be independently achieved by the robot arm joint motion after removing the velocity portion covered by chassis motion.

[0164] Step S544: Perform singular configuration avoidance on the end effector velocity vector of the pure robotic arm. When the end effector of the robotic arm approaches a singular configuration, adjust the velocity component along the singular direction in the end effector velocity vector of the pure robotic arm.

[0165] The singular configuration avoidance adopts a direction-selective velocity reduction strategy in conjunction with the damping correction in step S430: when the condition number of the Jacobian matrix exceeds another preset avoidance threshold, the singular value direction decomposition is performed on the pure robotic arm end velocity vector, and it is projected onto each direction of the left singular vector of the Jacobian matrix. The singular direction along which the corresponding minimum singular value is located is identified, and the velocity component in the singular direction is multiplied by a reduction factor less than 1. The reduction factor is dynamically calculated according to the degree to which the condition number exceeds the threshold. The higher the condition number, the closer the reduction factor is to zero. The velocity components in the non-singular directions retain their original values. The adjusted directional components are resynthesized to obtain the pure robotic arm end velocity vector after the singular configuration avoidance process.

[0166] Step S545: Output the differential control value of the left and right wheels as the chassis travel speed command, and output the pure robotic arm end velocity vector after the singular configuration avoidance processing as the robotic arm end execution speed command.

[0167] The chassis travel speed command is output as a data structure of the left and right wheel differential control quantities, containing two scalar values: the target angular velocity of the left wheel and the target angular velocity of the right wheel, and is sent to the walking drive mechanism. The robotic arm end effector speed command is output as a pure robotic arm end effector velocity vector, containing six components: three-dimensional linear velocity and three-dimensional angular velocity, and is sent to the motion control calculation unit of the robotic arm joint actuator.

[0168] Step S546: Synchronize the chassis travel speed command and the robotic arm end effector execution speed command with timestamps so that they are executed under a unified time base.

[0169] The synchronization timestamp is a system clock timestamp written into the data frames of the two instructions. This timestamp corresponds to the sampling time when the compensated end-effector speed instruction is generated. After receiving the instruction, the controllers of the walking drive mechanism and the robotic arm joint actuator perform synchronous execution scheduling according to the timestamp, ensuring that the chassis movement and the robotic arm end-effector movement are coordinated in time.

[0170] Step S550: Send the chassis travel speed command to the speed following control unit of the walking drive mechanism to drive the left and right walking wheel sets to travel with differential speed control, and send the end effector speed command of the robotic arm to the joint servo control unit of the robotic arm joint actuator to drive each joint to move with the corresponding joint angular velocity command.

[0171] The speed control unit of the walking drive mechanism receives the differential speed control quantity of the left and right wheels from the chassis travel speed command, and drives the left and right walking wheel motors through a proportional-integral-derivative speed closed-loop control algorithm, so that the actual wheel speed tracks the command speed. The end effector speed command of the robotic arm is converted into the target angular velocity command of each joint through inverse Jacobian mapping in the joint servo control unit. Each joint servo driver executes proportional-integral speed closed-loop control according to the target angular velocity, driving each joint motor to move at the command angular velocity. Finally, the end effector of the robotic arm moves in the operating space according to the end effector speed command. The synchronous execution of chassis travel and robotic arm movement enables the on-board robotic arm end effector of the inspection robot to approach the target observation instrument at the speed and direction planned by the compensated end effector speed command.

[0172] Step S560: During the movement of the walking drive mechanism and the robotic arm joint actuator, the real-time inertial sensing data stream fed back by the base inertial measurement unit is continuously acquired, and the end-effector speed compensation command is cyclically updated based on the real-time inertial sensing data stream to form an online compensation closed loop.

[0173] The online compensation closed loop is a closed-loop feedback mechanism that runs through the entire inspection walking and grasping operation process. It continuously collects new inertial sensing data from the base inertial measurement unit and sends the new data into the complete processing pipeline of the aforementioned adaptive frequency domain separation, feedforward force compensation channel, position compensation channel and velocity compensation command generation. It updates the end velocity compensation command in real time, so that the compensation command continuously tracks the dynamic changes of base disturbance over time.

[0174] As one implementation method, step S560 specifically includes the following steps S561 to S566:

[0175] Step S561: Read the real-time angular rate value and real-time acceleration value from the base inertial measurement unit at a preset update cycle, and form a real-time inertial sensing data stream from the real-time angular rate value and real-time acceleration value.

[0176] The preset update cycle is consistent with the main cycle of the control system, and is usually set in the millisecond range to ensure the real-time performance of compensation commands. Within each update cycle, the latest set of angular rate and acceleration values ​​are read through the data interface of the base inertial measurement unit. This set of data contains six measurements of triaxial angular rate and triaxial acceleration, constituting a new frame of sampled data in the real-time inertial sensing data stream.

[0177] Step S562: Input the real-time inertial sensing data stream into the adaptive frequency domain separation processing stage to generate real-time high-frequency disturbance components and real-time low-frequency disturbance components.

[0178] The adaptive frequency domain separation process is the complete adaptive frequency domain separation process described in step S200. In each update cycle, the accumulated real-time inertial sensor data stream within the current time window is processed by complex wavelet transform, instantaneous frequency trajectory extraction, frequency attribute marking, amplitude segment extraction, consistency correction, and inverse wavelet transform reconstruction, and outputs the real-time high-frequency disturbance component and real-time low-frequency disturbance component corresponding to the current moment.

[0179] Step S563: Perform acceleration feedforward on the real-time high-frequency disturbance component along the feedforward force compensation channel to generate a real-time force compensation command, and perform angle integration on the real-time low-frequency disturbance component along the position compensation channel to generate a real-time position compensation command.

[0180] The acceleration feedforward processing is executed according to steps S310 to S330, converting the acceleration high-frequency reconstruction timing in the real-time high-frequency disturbance component into a limited force compensation torque timing as a real-time force compensation command. The angle integration processing is executed according to steps S340 to S360, converting the angular rate low-frequency reconstruction timing and acceleration low-frequency reconstruction timing in the real-time low-frequency disturbance component into a reverse compensation amount of the comprehensive position drift as a real-time position compensation command.

[0181] Step S564: Obtain the real-time joint configuration data at the current moment, call the current Jacobian mapping relationship corresponding to the real-time joint configuration data, perform unified transformation on the real-time force compensation command and the real-time position compensation command, and generate the real-time end velocity compensation command.

[0182] The real-time joint configuration data consists of the latest joint angle position values ​​uploaded by the robotic arm joint encoder within the current update cycle. The current Jacobian mapping relationship is reconstructed or uses the damped Jacobian matrix and its pseudo-inverse matrix according to step S400. The unified transformation, following steps S440 to S460, converts the real-time force compensation command and real-time position compensation command into joint force compensation velocity contribution and joint position compensation velocity contribution, respectively. These are then superimposed and forward-mapped to generate the real-time end-effector velocity compensation command.

[0183] Step S565: Overlay the real-time end-vehicle speed compensation command with the task pre-planned speed command of the current cycle to obtain the updated compensation end-vehicle speed command, and replace the compensation end-vehicle speed command of the previous cycle with the updated compensation end-vehicle speed command to complete the cyclic update of the end-vehicle speed compensation command.

[0184] The pre-planned speed command for the current cycle is obtained through real-time interpolation based on the current position of the inspection robot on the preset inspection path and the task timeline. The overlay operation is the same as step S520, overlaying the real-time end-effector speed compensation command component onto the pre-planned speed command according to the timestamp. The replacement operation updates the currently valid compensation end-effector speed command stored in the control system memory with the newly calculated command value, which is used by the walking drive mechanism and the robotic arm joint actuator during the downsampling cycle.

[0185] Step S566: Use the iterative residual of the online compensation closed loop as the judgment criterion. When the iterative residual is lower than the preset convergence tolerance for multiple consecutive cycles, maintain the current compensation state; otherwise, continue iterative updates.

[0186] The iterative residual of the online compensation closed loop is defined as the larger of the ratios of the energy of the high-frequency disturbance component and the energy of the low-frequency disturbance component in the most recent adaptive frequency domain separation output to the average energy in their respective historical windows, or as the difference modulus between the amplitude of the real-time end-vehicle velocity compensation command and the amplitude of the compensation command in the previous cycle. The iterative residual is compared with a preset convergence tolerance, which is set according to the robot's operational accuracy requirements. When the iterative residual is lower than the convergence tolerance for several consecutive control cycles, it indicates that the current disturbance compensation has converged to a steady state, and the control system maintains the current compensation command without significant updates to reduce computational overhead and command fluctuations. If the iterative residual exceeds the convergence tolerance in any cycle, the full-process iterative update is immediately resumed to ensure timely response to transient disturbances such as sudden gusts of wind or sudden surface deformation.

[0187] Please refer to Figure 4 This diagram illustrates the structural block diagram of a computer system 20 provided in an embodiment of the present invention. This computer system can be used to implement the functions of the aforementioned walking and grasping control method applied to a meteorological observation field inspection robot. Specifically:

[0188] The computer system 20 includes a central processing unit (CPU) 21, a system memory 24 including random access memory (RAM) 22 and read-only memory (ROM) 23, and a system bus 25 connecting the system memory 24 and the CPU 21. The computer device 20 also includes a basic input / output system (I / O system) 26 that facilitates information transfer between various devices within the computer, and a mass storage device 27 for storing the operating system 271.

[0189] The input / output system 26 may include a display for showing information and input devices such as a mouse and keyboard for user input. Both the display and the input devices are connected to the central processing unit 21 via an input / output controller connected to the system bus 25.

[0190] Mass storage device 27 is connected to central processing unit 21 via a mass storage controller (not shown) connected to system bus 25. Mass storage device 27 and its associated computer-readable media provide non-volatile storage for computer device 20. That is, mass storage device 27 may include computer-readable media (not shown) such as hard disk or CD-ROM (Compact Disc Read-Only Memory) drive.

[0191] Without loss of generality, computer-readable media can include computer storage media and communication media. Computer storage media includes volatile and non-volatile, removable and non-removable media implemented using any method or technology for storing information such as computer-readable instructions, data structures, program modules, or other data. Computer storage media includes RAM, ROM, EPROM (Erasable Programmable Read Only Memory), EEPROM (Electrically Erasable Programmable Read Only Memory), flash memory or other solid-state storage devices, CD-ROM, DVD (Digital Video Disc) or other optical storage, magnetic tape cassettes, magnetic tape, disk storage, or other magnetic storage devices. Of course, those skilled in the art will recognize that computer storage media are not limited to the above-mentioned types. The system memory 24 and mass storage device 27 described above can be collectively referred to as memory.

[0192] According to various embodiments of the present invention, the computer device 20 can also be connected to a remote computer on a network such as the Internet. That is, the computer device 20 can be connected to the network 29 via the network interface unit 28 connected to the system bus 25, or the network interface unit 28 can be used to connect to other types of networks or remote computer systems (not shown).

[0193] The memory also includes a computer program stored in the memory and configured to be executed by one or more processors to implement the above-described walking and grasping control method for a meteorological observation field inspection robot.

[0194] In an exemplary embodiment, a computer-readable storage medium is also provided, wherein a computer program is stored therein, which, when executed by a processor, implements the above-described walking and grasping control method applied to a meteorological observation field inspection robot, or implements the above-described training method for determining the lesion area.

[0195] Optionally, the computer-readable storage medium may include: ROM (Read Only Memory), RAM (Random Access Memory), SSD (Solid State Drives), or optical disc, etc. The random access memory may include ReRAM (Resistance Random Access Memory) and DRAM (Dynamic Random Access Memory).

[0196] The above description is merely an exemplary embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A walking and grasping control method for a meteorological observation field inspection robot, characterized in that, The method includes: The inertial sensing data stream collected by the base inertial measurement unit during the process of the inspection robot traveling along the preset inspection path in the meteorological observation field, when the chassis suspension of the inspection robot is kept in an unlocked state, includes the base angular rate disturbance time sequence and base acceleration disturbance time sequence caused by wind disturbance and surface elastic deformation. Adaptive frequency domain separation is performed on the angular rate disturbance time series and the acceleration disturbance time series of the base. The inertial sensing data stream is decomposed into high-frequency disturbance components and low-frequency disturbance components through time-varying frequency division boundaries. The high-frequency disturbance components include instantaneous angular rate fluctuations and instantaneous acceleration fluctuations caused by gusts, and the low-frequency disturbance components include quasi-static angle drift and quasi-static acceleration slow change caused by surface creep. A feedforward force compensation channel is constructed based on the high-frequency disturbance component, and acceleration feedforward processing is performed on the instantaneous acceleration fluctuation in the high-frequency disturbance component to generate a force compensation command. A position compensation channel is constructed based on the low-frequency disturbance component, and angle integration processing is performed on the quasi-static angle drift in the low-frequency disturbance component to generate a position compensation command. The real-time joint configuration data of the robotic arm of the inspection robot is obtained. The Jacobian mapping relationship corresponding to the real-time joint configuration data is called. The force compensation command is converted into a joint force compensation velocity contribution through a preset force-velocity admittance conversion relationship. The position compensation command is converted into a joint position compensation amount through the inverse mapping of the Jacobian mapping relationship. Then, the joint position compensation amount is converted into a joint position compensation velocity contribution through a preset position compensation gain. After the joint force compensation velocity contribution and the joint position compensation velocity contribution are superimposed in the joint velocity space, the end-effector velocity compensation command is generated through the forward mapping of the Jacobian mapping relationship. The terminal velocity compensation command and the pre-planned speed command are superimposed with the same timestamp to obtain the compensated terminal velocity command. The walking drive mechanism and the mechanical arm joint execution mechanism of the inspection robot are synchronously controlled according to the compensated terminal velocity command, so that the inspection robot can perform the grasping operation of the target observation instrument in the meteorological observation field while the chassis suspension is kept unlocked.

2. The method according to claim 1, characterized in that, The adaptive frequency domain separation of the base angular rate disturbance timing and the base acceleration disturbance timing, and the decomposition of the inertial sensing data stream into high-frequency disturbance components and low-frequency disturbance components through time-varying frequency division boundaries, includes: The base angular rate perturbation time series is subjected to complex wavelet transform to generate an angular rate time-frequency distribution matrix with time-scale resolution. The wavelet coefficient modulus maxima path is searched along the scale axis in the angular rate time-frequency distribution matrix to extract the instantaneous frequency trajectory of the angular rate and simultaneously obtain the instantaneous amplitude sequence of the angular rate. The acceleration perturbation time series of the base is subjected to complex wavelet transform to generate an acceleration time-frequency distribution matrix. The instantaneous frequency trajectory of acceleration is extracted from the acceleration time-frequency distribution matrix by ridge tracking, and the instantaneous amplitude sequence of acceleration is obtained synchronously. The instantaneous frequency trajectory of angular velocity and the instantaneous frequency trajectory of acceleration are respectively input into a preset frequency division boundary judgment set. Based on the comparison results of the frequency values ​​at each moment on the instantaneous frequency trajectory of angular velocity with the frequency division boundary, an angular velocity frequency attribute label sequence is generated, and an acceleration frequency attribute label sequence is generated in the same way. Based on the angular rate frequency attribute marking sequence, the segments marked as gust disturbance frequency bands in the instantaneous angular rate amplitude sequence are extracted as high-frequency amplitude segments of angular rate; based on the acceleration frequency attribute marking sequence, the segments marked as gust disturbance frequency bands in the instantaneous acceleration amplitude sequence are extracted as high-frequency amplitude segments of acceleration. Amplitude envelope consistency detection is performed on the high-frequency amplitude segments of angular velocity and acceleration. When the cross-correlation coefficient of the amplitude envelopes of the two segments is lower than a preset cross-correlation threshold, bidirectional correction processing is performed on the inconsistent time markers in the frequency attribute marker sequences of angular velocity and acceleration. The high-frequency amplitude segments of angular velocity and acceleration, after bidirectional correction processing, are aligned in the time domain. The high-frequency amplitude segments of angular velocity are reconstructed into a high-frequency reconstruction time sequence of angular velocity using inverse wavelet transform, and the high-frequency amplitude segments of acceleration are reconstructed into a high-frequency reconstruction time sequence of acceleration. The high-frequency perturbation component is formed by the high-frequency reconstruction time sequence of angular velocity and the high-frequency reconstruction time sequence of acceleration. At the same time, the segments marked as creep perturbation frequency bands in the instantaneous amplitude sequence of angular velocity are extracted and reconstructed into the low-frequency perturbation component.

3. The method according to claim 2, characterized in that, The step of performing amplitude envelope consistency detection on the high-frequency amplitude segments of angular velocity and acceleration, and when the cross-correlation coefficient of their amplitude envelopes is lower than a preset cross-correlation threshold, performing bidirectional correction on inconsistent time markers in the frequency attribute labeling sequences of angular velocity and acceleration, includes: Perform a Hilbert transform on the high-frequency amplitude segment of the angular velocity to generate an analytical signal envelope of the angular velocity, and perform a Hilbert transform on the high-frequency amplitude segment of the acceleration to generate an analytical signal envelope of the acceleration. Within a preset sliding time window, calculate the cross-correlation function of the angular rate analytical signal envelope and the acceleration analytical signal envelope, and extract the maximum cross-correlation coefficient and the corresponding time delay within each time window; The maximum cross-correlation number of each time window is compared with the preset cross-correlation threshold, and time windows that are lower than the preset cross-correlation threshold are marked as suspicious inconsistency intervals; Within the suspected inconsistency interval, the angular rate frequency attribute label sequence and the acceleration frequency attribute label sequence are compared point by point to identify the sampling points with conflicting labels. For the sampling points with conflicting markers, the original frequency values ​​are traced back from the instantaneous frequency trajectory of the angular rate and the instantaneous frequency trajectory of the acceleration, respectively. Based on the distance between the original frequency value and the frequency division boundary and the instantaneous signal-to-noise ratio of the inertial sensing data stream at the sampling point, a priority confidence marker is determined. The priority confidence flag is assigned to the corresponding conflict sampling point, the angular rate frequency attribute flag sequence and the acceleration frequency attribute flag sequence are updated, and the angular rate high-frequency amplitude segment and the acceleration high-frequency amplitude segment are re-extracted based on the updated flag sequence.

4. The method according to claim 2, characterized in that, The step of determining the priority confidence marker based on the distance between the original frequency value and the frequency division boundary and the instantaneous signal-to-noise ratio of the inertial sensing data stream at the sampling point includes: Obtain the original frequency value of the instantaneous frequency trajectory of the angular velocity at the sampling point where the collision occurs, and the original frequency value of the instantaneous frequency trajectory of the acceleration. Calculate the angular rate frequency distance between the original angular rate frequency value and the frequency division boundary, and the acceleration frequency distance between the original acceleration frequency value and the frequency division boundary; The instantaneous signal-to-noise ratio (SNR) estimates of the angular rate channel and the instantaneous SNR estimates of the acceleration channel of the inertial sensing data stream at the marked conflict sampling points are obtained. The angular rate frequency distance and the estimated instantaneous signal-to-noise ratio of the angular rate channel are input into a preset angular rate confidence evaluation relationship to generate an angular rate tag confidence score. The acceleration frequency distance and the estimated instantaneous signal-to-noise ratio of the acceleration channel are input into a preset acceleration confidence evaluation relationship to generate an acceleration tag confidence score. By comparing the confidence scores of the angular rate marker and the confidence scores of the acceleration marker, the frequency attribute marker corresponding to the one with the higher confidence score is selected as the preferred confidence marker; After determining the priority confidence markers at multiple consecutive sampling points with conflicting markers, the resulting priority confidence marker sequence is continuously smoothed to eliminate isolated jump markers.

5. The method according to claim 1, characterized in that, The step of constructing a feedforward force compensation channel based on the high-frequency disturbance component, performing acceleration feedforward processing on the instantaneous acceleration fluctuations in the high-frequency disturbance component to generate force compensation commands, and constructing a position compensation channel based on the low-frequency disturbance component, performing angle integration processing on the quasi-static angle drift in the low-frequency disturbance component to generate position compensation commands, includes: The acceleration high-frequency reconstruction timing sequence in the high-frequency disturbance component is input to the acceleration-force mapping stage of the feedforward force compensation channel. In the acceleration-force mapping stage, the joint space inertia matrix of the robotic arm dynamics model is combined to convert the acceleration high-frequency reconstruction timing sequence into the joint space feedforward torque timing sequence. The current motion speed of each joint of the robotic arm is obtained. Based on the current motion speed, a preset joint friction characteristic curve is queried to obtain the joint friction torque compensation amount. The joint friction torque compensation amount is then superimposed with the joint space feedforward torque timing sequence to generate the initial force compensation torque timing sequence. The initial force compensation torque timing sequence is input to the force limiting stage. In the force limiting stage, the amplitude of the initial force compensation torque timing sequence is clipped according to the peak torque output constraint of each joint of the robotic arm to generate the limited force compensation torque timing sequence. The limited force compensation torque timing sequence constitutes the force compensation command. The angular rate low-frequency reconstruction timing sequence in the low-frequency disturbance component is input to the angle integrator of the position compensation channel. The angle integrator performs continuous time-domain integration on the angular rate low-frequency reconstruction timing sequence to generate the angle drift estimation timing sequence. The angle drift estimation timing is input to the drift compensator, and the angle drift estimation timing is nonlinearly compensated in the drift compensator using a preset drift correction relationship to generate a corrected angle drift timing. The corrected angle drift timing is fused with the acceleration low-frequency reconstruction timing in the low-frequency disturbance component. The acceleration low-frequency reconstruction timing is converted into a displacement drift through double integration. This displacement drift is then vector-synthesized with the end displacement drift obtained by forward kinematic mapping from the corrected angle drift timing to obtain a comprehensive position drift. The position compensation command is then constructed based on the reverse compensation amount of the comprehensive position drift.

6. The method according to claim 5, characterized in that, The step of inputting the high-frequency acceleration reconstruction timing from the high-frequency disturbance components into the acceleration-force mapping stage of the feedforward force compensation channel, and in the acceleration-force mapping stage combining the joint space inertia matrix of the robotic arm dynamics model, converting the high-frequency acceleration reconstruction timing into the joint space feedforward torque timing, includes: Based on the real-time joint configuration data, the joint space inertia matrix corresponding to the current configuration is extracted from the robotic arm dynamics model. The joint space inertia matrix includes the self-inertia term of each joint and the inter-joint coupling inertia term. The acceleration vector at each moment in the high-frequency reconstruction timing sequence is multiplied with the joint space inertia matrix to generate an initial joint space feedforward torque timing sequence, which includes torque components contributed by linear acceleration and angular acceleration respectively. Obtain the Coriolis force and centrifugal force coefficient matrix of each joint of the robotic arm under the current configuration, calculate the Coriolis force and centrifugal force compensation torque according to the current motion speed, and superimpose the Coriolis force and centrifugal force compensation torque into the initial joint space feedforward torque timing sequence to generate the dynamically compensated feedforward torque timing sequence; The feedforward torque timing sequence after dynamic compensation is phase-advanced, and the response delay of the feedforward force compensation channel is compensated by a differential prediction stage to generate a phase-corrected feedforward torque timing sequence. The phase-corrected feedforward torque timing is input to the joint torque allocation stage. In the joint torque allocation stage, the phase-corrected feedforward torque timing is redistributed between joints according to the instantaneous acceleration requirements and torque load margin of each joint, thereby generating the allocated joint space feedforward torque timing. The allocated joint space feedforward torque timing is output as the joint space feedforward torque timing to the subsequent processing stage.

7. The method according to claim 5, characterized in that, The step of inputting the angle drift estimation time series to the drift compensator, and performing nonlinear compensation on the angle drift estimation time series using a preset drift correction relationship in the drift compensator to generate a corrected angle drift time series includes: Obtain the pose feedback data of the robotic arm end effector in the previous control cycle, and calculate the end effector pose deviation vector based on the deviation between the pose feedback data and the desired pose. The end pose deviation vector is projected onto the joint space through an inverse Jacobian mapping to generate a joint angle deviation feedback quantity, and the joint angle deviation feedback quantity is synchronized and aligned with the angle drift estimation time series. Trend analysis is performed on the angle drift estimation time series to extract the monotonic drift trend component and oscillatory drift component from the angle drift estimation time series; Based on the slope parameter of the monotonic drift trend component and the amplitude parameter of the oscillating drift component, a preset nonlinear compensation function is invoked to generate a compensation increment timing sequence that is opposite to the angular drift direction. The compensation increment timing sequence is superimposed point by point with the angle drift estimation timing sequence, and the superposition result is constrained and corrected by the joint angle deviation feedback amount to generate the corrected angle drift timing sequence. The corrected angle drift timing is smoothed at the boundary so that the corrected angle drift timing changes continuously within the range of motion constraints of the robotic arm joint.

8. The method according to claim 1, characterized in that, The process involves acquiring real-time joint configuration data of the robotic arm of the inspection robot, calling the Jacobian mapping relationship corresponding to the real-time joint configuration data, converting the force compensation command into a joint force compensation velocity contribution through a preset force-velocity admittance conversion relationship, converting the position compensation command into a joint position compensation amount through the inverse mapping of the Jacobian mapping relationship, converting the joint position compensation amount into a joint position compensation velocity contribution through a preset position compensation gain, superimposing the joint force compensation velocity contribution and the joint position compensation velocity contribution in the joint velocity space, and generating an end-effector velocity compensation command through the forward mapping of the Jacobian mapping relationship. This includes: The angular position values ​​of each joint are extracted from the real-time joint configuration data, and the joint angular velocity values ​​are obtained through differential operation. The joint angular position values ​​and the joint angular velocity values ​​are used as joint state variables. The joint state variables are input into the geometric Jacobian construction stage. In the geometric Jacobian construction stage, the linear velocity Jacobian component and the angular velocity Jacobian component are calculated column by column according to the kinematic parameters of the robotic arm, and combined to generate the Jacobian matrix of the current configuration. The Jacobian matrix is ​​subjected to singular value decomposition, and the matrix condition number is calculated based on the singular values ​​obtained from the decomposition. When the condition number exceeds a preset singularity threshold, a damping correction is applied to the Jacobian matrix to obtain a damped Jacobian matrix and its pseudo-inverse matrix. The force compensation command is input to the force-velocity mapping stage. In the force-velocity mapping stage, the force compensation command is converted into a joint force compensation velocity contribution using a preset joint space admittance matrix. At the same time, the position compensation command is input to the position-velocity mapping stage. First, the position compensation command is converted into a joint position compensation offset using the pseudo-inverse of the damping Jacobian matrix. Then, the joint position compensation offset is converted into a joint position compensation velocity contribution using a preset position compensation gain coefficient. The joint force compensation velocity contribution and the joint position compensation velocity contribution are superimposed in the same dimension in the joint velocity space to generate a joint space composite velocity vector, and a zero space self-motion component is introduced into the joint space composite velocity vector. The joint space synthesized velocity vector after introducing the zero-space self-motion component is positively mapped to the operation space through the Jacobian matrix to generate the end-effector velocity compensation command containing linear velocity compensation components and angular velocity compensation components.

9. A computer system, characterized in that, The computer system includes a processor and a memory, the memory storing a computer program, which is loaded and executed by the processor to implement the walking and grasping control method for a meteorological observation field inspection robot as described in any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, which is loaded and executed by a processor to implement the walking and grasping control method for a meteorological observation field inspection robot as described in any one of claims 1 to 8.

Images