Movable two-arm body intelligent data acquisition platform

Abstract

This invention relates to the field of haptic feedback technology, specifically a mobile dual-arm intelligent data acquisition platform. The platform includes a frequency domain signal conversion module, which acquires vibration time-series signals from a haptic skin array, performs time-domain to frequency domain discrete signal decomposition and transformation, calculates the dominant spatial frequency value corresponding to the amplitude peak within the frequency domain signal sequence, extracts the energy spectral density value of the corresponding frequency band range of the frequency domain signal sequence, and combines the dominant spatial frequency value and the energy spectral density value. In this invention, haptic spectrum characteristics, force changes, geometric spacing, penetration direction, pose constraints, and trajectory reconstruction are linked into a closed loop. This enhances the detail of contact state recognition while reducing the probability of accidental collisions in close-range dual-arm collaboration, balancing acquisition accuracy, movement smoothness, and spatial safety. It improves data effectiveness and consistency in repetitive tasks for scenarios such as complex surface scanning, precise grasping and contact recording, and dynamic interactive data accumulation.

Description

Technical Field

[0001] This invention relates to the field of haptic feedback technology, and more particularly to a mobile dual-arm intelligent data acquisition platform. Background Technology

[0002] Haptic feedback technology researches how to enable machines or systems to perceive and reproduce information such as force, pressure, vibration, temperature and surface texture generated when objects come into contact through sensing, modeling and control methods, thereby achieving human-like tactile perception and interaction capabilities.

[0003] Existing haptic feedback technologies largely focus on the perception and reproduction of contact information itself, concentrating on acquiring and responding to tactile quantities such as force, pressure, vibration, temperature, and texture. When faced with complex tasks such as dual-arm movement, continuous trajectory acquisition, and close-range spatial collaboration, a common shortcoming is the lack of closer linkage and adjustment between the contact state and the movement process. In practice, while haptic information can reflect the presence and intensity of contact, it struggles to directly provide guidance on how the movement rhythm should contract, whether the collaborative space is approaching a danger boundary, and which direction of posture correction is more appropriate for local contact anomalies. The result is often that the perception and execution layers operate independently; contact feedback is visible but difficult to translate into timely trajectory updates. Therefore, improvements are needed. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of existing technologies and propose a mobile dual-arm intelligent data acquisition platform.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: a mobile dual-arm intelligent data acquisition platform comprising: The frequency domain signal conversion module acquires vibration time-series signals from the tactile skin array, performs time-domain to frequency domain discrete signal decomposition transformation, calculates the dominant spatial frequency value corresponding to the amplitude peak value in the frequency domain signal sequence, extracts the energy spectral density value of the corresponding frequency band range of the frequency domain signal sequence, and combines the dominant spatial frequency value and the energy spectral density value to generate a tactile frequency domain energy set. The feed rate control module performs inverse proportional mapping and correlation calculation based on the tactile frequency domain energy set, extracts the energy spectral density value within the tactile frequency domain energy set, constructs an inverse proportional correspondence between the energy spectral density value and the feed rate variable value, obtains the feed rate variable value, and sends the feed rate variable value to the trajectory planning controller to generate a rate control instruction set.

[0006] Preferably, the platform further includes: The spatial interference calculation module drives the joint servo motor according to the magnification control instruction set, extracts the force values ​​of the two arms, compares the force values ​​with the collision setting value, suspends the action command, reads the angle value of the joint servo motor, projects the two arms' enclosing box frame onto the Cartesian space, calculates the minimum vertex spacing value and the deepest penetration direction coordinate of the projection coordinates, and generates a spatial interference vector set. The obstacle avoidance path rewriting module sets a target term for minimizing pose deviation based on the spatial interference vector set, establishes an equality constraint term for constant end probe coordinates and an inequality constraint term for spacing greater than a set boundary, obtains the updated angle value by differentiation, performs interpolation on the updated angle value to generate waypoint coordinates, and places the waypoint coordinates into the front end of the action command to generate the obstacle avoidance reconstructed trajectory segment.

[0007] Preferably, the frequency domain signal conversion module includes: The discrete signal decomposition submodule acquires vibration time-series signals from the tactile skin array, performs time-series waveform truncation and discrete time-domain slicing, establishes a window corresponding to the signal timestamp, separates high- and low-frequency fluctuation components, extracts high-frequency vibration amplitude coordinates, transforms the time-domain vibration amplitude coordinates to the frequency-domain coordinate system, outputs the amplitude of each frequency component, and generates a frequency-domain signal sequence. The dominant frequency calculation submodule performs amplitude scanning and comparison of each frequency point in the frequency coordinate system according to the frequency domain signal sequence, extracts the value of the horizontal axis frequency point corresponding to the maximum amplitude value as the dominant spatial frequency value corresponding to the amplitude peak value, defines the frequency band boundary around the frequency point, performs square integral accumulation on the amplitude of each frequency point in the frequency band, obtains the energy spectral density value of the corresponding frequency band range, and generates spectral feature value pairs. The frequency domain energy combination submodule performs multi-dimensional numerical association binding based on the spectral feature values, extracts the dominant spatial frequency value corresponding to the amplitude peak and the corresponding frequency band range energy spectral density value, packs and aligns the two according to the time stamp order, establishes a composite structure containing spatial frequency and energy density, combines the dominant spatial frequency value and energy spectral density value, and generates a tactile frequency domain energy set.

[0008] Preferably, the feed rate control module includes: The inverse proportional mapping submodule performs inverse proportional mapping correlation calculation based on the tactile frequency domain energy set, extracts the energy spectral density value within the tactile frequency domain energy set, establishes a basic feed parameter constant, performs a division operation with the basic feed parameter constant as the numerator and the energy spectral density value as the denominator, constructs an inverse proportional correspondence between the energy spectral density value and the target feed variable, and generates a magnification mapping function term. The multiplier variable calculation submodule performs real-time parameter substitution and calculation based on the multiplier mapping function term, monitors the energy spectral density value increment under the current input state, substitutes the increased energy spectral density value into the denominator of the inverse proportional relationship, calculates the target feed ratio parameter after attenuation, and calculates the corresponding feed multiplier variable value. The control instruction generation submodule rewrites the underlying actuator parameters based on the feed rate variable value, reads the preset feed speed parameter, multiplies the preset feed speed parameter with the attenuated feed ratio parameter to obtain the corrected end-effector movement speed value, and sends the feed rate variable value to the trajectory planning controller to generate a rate control instruction set.

[0009] Preferably, the spatial interferometry calculation module includes: The collision force monitoring submodule drives the joint servo motor according to the multiplier control instruction set, extracts the torque feedback parameters of each node of the double arm body, calculates the force value of the double arm body, retrieves the preset collision force threshold constant, compares the force value with the collision setting value, cuts off the motor drive signal when the force value exceeds the collision setting value, suspends the action command, and generates an over-limit force status indicator. The bounding box projection submodule performs a positive kinematic space position transformation based on the over-limit force state indicator, reads the angle values ​​of the joint servo motors, retrieves the three-dimensional geometric shell size parameters of the two arms, constructs the bounding box frame of the two arms body along the axis of each link, and projects the bounding box frame of the two arms body onto Cartesian space in combination with the joint angle and link length parameters to generate a Cartesian projection coordinate system.

[0010] Preferably, the spatial interferometry calculation module further includes: The penetration measurement and positioning submodule performs iterative search of the spatial position distance of the vertices of the polygonal shell according to the Cartesian projection coordinate system, locates the three-dimensional coordinates of the two closest points between the two arm bounding box shells, calculates the minimum vertex spacing value of the projection coordinates, measures the overlap depth along the normal direction of the intersecting and overlapping area, calculates the coordinates of the deepest penetration direction, and generates a spatial interference vector set.

[0011] Preferably, the obstacle avoidance path rewriting module includes: The constraint equation establishment submodule sets the boundary conditions for the optimization solution variables based on the spatial interference vector set, sets the target term for minimizing the pose deviation, establishes the constraint term for the constant coordinates of the end probe, extracts the vertex spacing and penetration direction within the spatial interference vector set, restricts the distance between the two arms to be outside the safety threshold, establishes the constraint term for the distance being greater than the set boundary, and generates the obstacle avoidance constraint equation set. The updated angle calculation submodule performs multivariate function iterative differentiation calculation based on the obstacle avoidance constraint equation set. It combines the pose deviation minimization target term with the end probe coordinate constant equality constraint term and the spacing greater than the set boundary inequality constraint term, and calculates the derivative to obtain the safe avoidance pose parameters of each joint that meet the boundary conditions, i.e., the updated angle value, and generates the updated angle value.

[0012] Preferably, the obstacle avoidance path rewriting module further includes: The obstacle avoidance trajectory reconstruction submodule performs a time-dimensional polynomial curve fitting operation based on the updated angle value, performs interpolation calculation on the updated angle value to generate waypoint coordinates, advances the generated waypoint coordinates according to the time sequence, links the remaining action coordinate points in the original deferred queue, outputs a smooth collision-free execution sequence, and places the waypoint coordinates into the front end of the action command to generate the obstacle avoidance reconstruction trajectory segment.

[0013] Compared with the prior art, the advantages and positive effects of the present invention are as follows: In this invention, based on the vibration timing information collected by the tactile skin array, the dynamic changes during the contact process are first expressed in the frequency domain. The dominant spatial frequency corresponding to the amplitude peak and the energy spectral density within a specific frequency band are extracted synchronously from the discrete spectrum. These two are combined into a tactile frequency domain energy set that can characterize the detailed differences in the contact state. Then, based on the inverse proportional relationship between the energy spectral density and the feed rate, the movement speed is adaptively converged, so that the position with higher contact intensity and more significant vibration receives a more cautious execution rhythm, avoiding the accumulation of detection errors and amplification of surface disturbances caused by rough advancement. During the movement execution phase, the force values ​​of both arms are continuously read and compared with the impact... The collision threshold is compared, and the predetermined action is interrupted when the risk approaches. At the same time, the shape of the two arms is mapped to Cartesian space by combining the joint angle information, and the minimum vertex spacing and the deepest penetration direction are further calculated. This extends from contact perception to spatial relationship quantification, so that the interference judgment is no longer limited to the identification of single force anomalies, but has the ability to represent position, direction and penetration depth. In the path correction stage, the goal is to minimize the pose deviation, keep the coordinates of the end probe stable, and constrain the safety distance to always be above the boundary. Then, the updated angle results are interpolated to generate continuous waypoints and written into the action sequence in advance, so that the obstacle avoidance process maintains a high degree of continuity with the original data acquisition task. The above execution link connects tactile spectrum features, force changes, geometric spacing, penetration direction, pose constraints and trajectory reconstruction into a closed loop. This not only enhances the fineness of contact state recognition, but also reduces the probability of accidental collisions in close-range cooperation of the two arms. It takes into account the acquisition accuracy, action smoothness and spatial safety. It can improve the data effectiveness and consistency of repetitive operations for scenarios such as complex surface scanning, fine grasping contact recording, and dynamic interactive data accumulation. Attached Figure Description

[0014] Figure 1 This is a system flowchart of the present invention. Detailed Implementation

[0015] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0016] Please see Figure 1 The present invention provides a technical solution: a mobile dual-arm intelligent data acquisition platform comprising: The frequency domain signal conversion module acquires vibration time-series signals from the tactile skin array, performs time-domain to frequency domain discrete signal decomposition transformation, calculates the dominant spatial frequency value corresponding to the amplitude peak value in the frequency domain signal sequence, extracts the energy spectral density value of the corresponding frequency band range of the frequency domain signal sequence, and combines the dominant spatial frequency value and the energy spectral density value to generate a tactile frequency domain energy set. The feed rate control module performs inverse proportional mapping and correlation calculation based on the tactile frequency domain energy set, extracts the energy spectral density value within the tactile frequency domain energy set, constructs an inverse proportional correspondence between the energy spectral density value and the feed rate variable value, obtains the feed rate variable value, and sends the feed rate variable value to the trajectory planning controller to generate a rate control instruction set. The spatial interference calculation module drives the joint servo motors according to the magnification control instruction set, extracts the force values ​​of the two arms, compares the force values ​​with the collision setting values, suspends the action command, reads the angle values ​​of the joint servo motors, projects the bounding box frame of the two arms onto the Cartesian space, calculates the minimum vertex spacing value and the deepest penetration direction coordinate of the projection coordinates, and generates a spatial interference vector set. The obstacle avoidance path rewriting module sets a target term for minimizing pose deviation based on the spatial interference vector set, establishes an equality constraint term for constant end probe coordinates and an inequality constraint term for spacing greater than the set boundary, obtains the updated angle value by differentiation, performs interpolation on the updated angle value to generate waypoint coordinates, and puts the waypoint coordinates into the front end of the action command to generate the obstacle avoidance reconstructed trajectory segment.

[0017] The frequency domain signal conversion module includes: The discrete signal decomposition submodule acquires vibration time-series signals from the tactile skin array, performs time-series waveform truncation and discrete time-domain slicing, establishes a window corresponding to the signal timestamp, separates high- and low-frequency fluctuation components, extracts high-frequency vibration amplitude coordinates, transforms the time-domain vibration amplitude coordinates to the frequency-domain coordinate system, outputs the amplitude of each frequency component, and generates a frequency-domain signal sequence. The dominant frequency calculation submodule performs amplitude scanning and comparison of each frequency point in the frequency coordinate system based on the frequency domain signal sequence, extracts the value of the horizontal axis frequency point corresponding to the maximum amplitude value as the dominant spatial frequency value corresponding to the amplitude peak value, defines the frequency band boundary around the frequency point, performs square integral accumulation on the amplitude of each frequency point in the frequency band, obtains the energy spectral density value of the corresponding frequency band range, and generates spectral feature value pairs. The frequency domain energy combination submodule performs multi-dimensional numerical association and binding based on the spectral feature values, extracts the dominant spatial frequency value corresponding to the amplitude peak and the corresponding frequency band range energy spectral density value, packs and aligns the two according to the time stamp order, establishes a composite structure containing spatial frequency and energy density, combines the dominant spatial frequency value and energy spectral density value, and generates a tactile frequency domain energy set.

[0018] Specifically, vibration timing signals are acquired using a tactile skin array. The continuously acquired analog signals are converted from analog to digital, with a sampling frequency of 2000Hz. Voltage amplitudes at discrete time points are recorded. A Hamming window function is used to truncate the vibration timing signal and slice it in the discrete time domain. The sliding window length is set to 50 milliseconds with a step size of 25 milliseconds, establishing a window corresponding to the signal timestamp. A fourth-order Butterworth high-pass filter with a cutoff frequency of 50Hz is used to filter the data within the window, separating low-frequency mechanical noise components below 50Hz and extracting high-frequency vibration amplitude coordinates above 50Hz. A Fast Fourier Transform (FFT) is used to transform the time-domain vibration amplitude coordinates to the frequency-domain coordinate system. The amplitude of each frequency component is calculated using the Discrete Fourier Transform (DFT) formula. ,in Represents frequency index The corresponding complex values ​​in the frequency domain, Represents time index The corresponding time-domain signal amplitude, Represents the total number of sampling points within the window. Represents a frequency index variable. Represents a time index variable. Representing the imaginary unit, it calculates the modulus of the complex value to obtain the actual amplitude at each frequency point, maps the frequency index to the corresponding physical frequency value, arranges all frequency amplitude pairs in ascending order of physical frequency, outputs the amplitude of each frequency component, and generates a frequency domain signal sequence.

[0019] Based on the frequency domain signal sequence, the amplitude values ​​of all frequency points are scanned and compared in the frequency coordinate system. A traversal algorithm is used to compare the current frequency point amplitude with the recorded maximum amplitude variable. If the current value is larger, the maximum amplitude variable and its corresponding horizontal axis coordinate are updated. The horizontal axis frequency point value corresponding to the final maximum amplitude is extracted as the dominant spatial frequency value corresponding to the amplitude peak. This defines the frequency band boundary values ​​around the dominant spatial frequency. Specifically, the upper and lower limits of this frequency band boundary are set to 10% above and below the dominant spatial frequency. For example, if the current dominant spatial frequency is 200Hz, the lower limit is set to 180Hz and the upper limit to 220Hz. The frequency band boundary is defined around the frequency point. The amplitude values ​​of each frequency point within the frequency band are squared, and the results are integrated and accumulated to obtain the energy spectral density value of the corresponding frequency band range. The calculation formula is as follows: ,in Represents the energy spectral density value. The lower limit frequency represents the frequency band. Represents the upper limit frequency of the bandwidth. Representing frequency The corresponding amplitude, The frequency integral element is represented by discrete summation instead of continuous integration in actual calculations. The frequency points and their corresponding energy spectral densities are extracted and concatenated into a two-dimensional array structure. The first column stores the dominant spatial frequency values, and the second column stores the corresponding energy spectral density values, generating spectral feature value pairs.

[0020] Based on the spectral feature value pairs, the internal high-precision clock stamps recorded during the acquisition of the aforementioned high-frequency vibration timing signals are read. These clock stamps are appended to the attribute tags of each spectral feature value pair for multi-dimensional numerical correlation and binding. The dominant spatial frequency value corresponding to the amplitude peak and the corresponding frequency band energy spectral density value are extracted and sorted according to the order of the timestamps. The time difference between adjacent timestamps is retrieved, and it is determined whether the time difference is within the allowable time tolerance range. The benchmark value for the time tolerance is set by selecting the average time of one hundred consecutive signal processing cycles and adding three times the standard deviation. For example, if the average time is 10 milliseconds and the standard deviation is 1 millisecond, then the benchmark... The threshold is set to 13 milliseconds. If the time difference between adjacent timestamps is less than or equal to 13 milliseconds, the two timestamps are packaged and aligned according to their order. The dominant spatial frequency and energy density belonging to the same time window are stored in the same dictionary data structure. In the dictionary structure, a composite structure containing spatial frequency and energy density is established with the timestamp as the key. All spectral feature value pairs are processed in a loop. Isolated data points with discontinuous timestamps or exceeding the tolerance range are removed. The cleaned continuous dataset is merged and stored in the system's shared memory area. An array index is established for subsequent reading. The dominant spatial frequency value and energy spectral density value are combined to generate a tactile frequency domain energy set.

[0021] The feed rate control module includes: The inverse proportional mapping submodule performs inverse proportional mapping correlation calculation based on the tactile frequency domain energy set, extracts the energy spectral density value within the tactile frequency domain energy set, establishes a basic feed parameter constant, performs a division operation with the basic feed parameter constant as the numerator and the energy spectral density value as the denominator, constructs an inverse proportional correspondence between the energy spectral density value and the target feed variable, and generates a magnification mapping function term. The multiplier variable calculation submodule performs real-time parameter substitution and calculation based on the multiplier mapping function term, monitors the energy spectral density value increment under the current input state, substitutes the increased energy spectral density value into the denominator of the inverse proportional relationship, calculates the target feed ratio parameter after attenuation, and calculates the corresponding feed multiplier variable value. The control instruction generation submodule rewrites the underlying actuator parameters based on the feed rate variable value, reads the preset feed speed parameter, multiplies the preset feed speed parameter with the attenuated feed ratio parameter to obtain the corrected end-effector movement speed value, and sends the feed rate variable value to the trajectory planning controller to generate a rate control instruction set.

[0022] Specifically, based on the tactile frequency domain energy set, the energy spectral density values ​​within the tactile frequency domain energy set are extracted. The constant term for feed control is initialized, and the average energy spectral density during data acquisition on a smooth reference plane is set as the base energy threshold. This threshold is set by averaging the energy spectral densities of 1000 smooth surfaces, for example, an average of 0.5. The feed rate base parameter is set to 100%, and the product of the base energy threshold and the feed rate base parameter is used as the base feed parameter constant, for example, calculated as 50 here. This base feed parameter constant is then used for inverse proportional mapping calculations. The base feed parameter constant is used as the numerator, and the real-time acquired energy spectral density values ​​are used as the denominator for division. If the denominator is less than 0.01, it is forcibly assigned a value of 0.01. This establishes an inverse proportional relationship between the energy spectral density values ​​and the target feed variable. The calculation formula is as follows: ,in This represents the calculated target feed variable. Represents the basic feed parameter constant. The value represents the real-time input energy spectral density. When the surface is rough, the real-time energy spectral density increases to, for example, 2.0. The target feed variable is calculated as 50 divided by 2.0, which equals 25%. This functional relationship is used to convert the high-frequency energy of tactile perception into kinematic variables and generate a magnification mapping function term.

[0023] Based on the magnification mapping function, the latest data from the tactile frequency domain energy set within the current acquisition cycle is obtained. The increment of the energy spectral density value under the current input state is monitored, and the difference between the energy spectral density of the current cycle and the energy spectral density of the previous cycle is calculated. A difference greater than zero indicates increased vibration. The increased energy spectral density value is extracted and used for real-time parameter substitution calculation. The increased energy spectral density value is substituted into the denominator of the inverse proportional relationship, and division is performed. Boundary constraints are applied to the calculated initial proportional parameters, setting upper and lower thresholds for the feed magnification. The upper threshold is set as a percentage of the machine's maximum safe speed under no-load, i.e., 100%, and the lower threshold is set to ensure that the lead screw motor does not experience low speed. The percentage corresponding to the minimum stable speed of jitter is set by reading the minimum rated speed from the motor nameplate parameters and dividing it by the maximum rated speed. For example, the minimum speed of 300 rpm is divided by the maximum speed of 3000 rpm to get a lower threshold of 10%. If the calculated result is lower than 10%, 10% is forcibly output; if it is higher than 100%, 100% is forcibly output. The increased energy spectral density, such as 3.0, is substituted into the formula to calculate 16.6%. If the result is within the threshold range of 10% and 100%, 16.6% is retained. The target feed ratio parameter after attenuation is calculated, the constrained ratio parameter is converted into single-precision floating-point data format, and the corresponding feed rate variable value is calculated.

[0024] Based on the feed rate variable value, the original constant speed execution pipeline of the interceptor is intercepted, and the underlying actuator parameters are rewritten. The feed rate parameters preset for the current acquisition path are read from the CNC machining program. The preset feed rate parameters are determined by the hardness of the material of the machining surface. The recommended values ​​in the material standard manual are matched using a lookup table method. For example, if the material to be acquired is marked as a standard aluminum alloy plate, its basic running speed is obtained by indexing the material library and is calibrated to be 60 mm / s. The preset feed rate parameters are multiplied by the attenuated feed ratio parameters through the floating-point multiplication unit. For example, 60 mm / s is multiplied by 16. The feed rate variable value of 6% is used to obtain the corrected end-effector velocity value of 9.96 mm / s. This velocity value is packaged into a data frame format that conforms to the underlying bus communication protocol. The data frame includes a frame header, data length, velocity load bits, and checksum. The data frame is written into the speed control register of the underlying servo controller, overwriting the original constant speed parameter in the register. The feed rate variable value is then sent to the trajectory planning controller. The trajectory planning controller re-plans the displacement within the interpolation cycle based on the new velocity value, shortens the interpolation step size per unit time, and generates a rate adjustment instruction set.

[0025] The spatial interferometric measurement module includes: The collision force monitoring submodule drives the joint servo motor according to the multiplier control instruction set, extracts the torque feedback parameters of each node of the double arm body, calculates the force value of the double arm body, retrieves the preset collision force threshold constant, compares the force value with the collision setting value, and cuts off the motor drive signal when the force value exceeds the collision setting value, suspends the action command, and generates an over-limit force status indicator. The bounding box projection submodule performs a positive kinematic space position transformation based on the over-limit force state indicator, reads the angle values ​​of the joint servo motors, retrieves the three-dimensional geometric shell size parameters of the two arms, constructs the bounding box frame of the two arms body along the axis of each link, and projects the bounding box frame of the two arms body onto Cartesian space by combining the joint angle and link length parameters to generate a Cartesian projection coordinate system. The penetration measurement and positioning submodule performs iterative search of the spatial position distance of the vertices of the polygonal shell according to the Cartesian projection coordinate system, locates the three-dimensional coordinates of the two closest points between the two arm bounding box shells, calculates the minimum vertex spacing value in the projection coordinates, measures the overlap depth along the normal direction of the intersecting and overlapping area, calculates the coordinates of the deepest penetration direction, and generates a spatial interference vector set.

[0026] Specifically, based on the multiplier control instruction set, the speed and position information in the instruction set are parsed, and control pulses containing the target position and corrected speed are sent to each degree of freedom joint of the end effector to drive the joint servo motors. The working status data is collected through the current and torque sensors inside the motor driver, and the bus current parameters and rotor position parameters are read. The current value is converted into a torque value according to the motor torque constant. The torque feedback parameters of each node of the dual-arm body are extracted, and the compensation amount of gravity, Coriolis force and centrifugal force is calculated in combination with the robot's dynamic equations. The total torque is subtracted from the above-mentioned compensation torque to obtain the actual force on the dual-arm body when subjected to external compression or collision. The system retrieves a preset collision force threshold constant. This collision setting is achieved by collecting the average force of the robotic arm running along a predetermined trajectory for ten complete cycles in an obstacle-free space, and adding five times the standard deviation of the force fluctuation. For example, if the average force is measured to be 2 Newtons and the standard deviation of the fluctuation is 0.5 Newtons, then the collision setting is established as 4.5 Newtons. The system compares the force value with the collision setting. When the real-time force value, such as 5.2 Newtons, exceeds the collision setting of 4.5 Newtons, the system triggers an interrupt handler, cuts off the motor drive signal, blocks the inverter pulse output, suspends the action command, saves the position breakpoint at the moment the interrupt occurs, and generates an over-limit force status indicator.

[0027] Based on the over-limit force status indicator, the hardware status at the time of interruption is obtained. The absolute encoders at each joint are queried via the Ethernet bus to read the angle values ​​of the joint servo motors. The robot link kinematics (DH) parameter table, pre-stored in the configuration file, is retrieved, including link length, link torsion angle, link offset, and joint rotation angle parameters. A homogeneous transformation of the link coordinate system is performed based on the DH parameter matrix, resulting in a positive kinematic space position transformation. The three-dimensional spatial position and attitude matrix of each joint coordinate system relative to the base coordinate system are calculated. The three-dimensional geometric shell dimensions of the dual arms are retrieved, including the cylinder radius of each link segment. Based on the length data, a double-arm body bounding box frame is constructed along the axis of each link. A capsule geometric model is used to replace the irregular appearance for space occupation description. The capsule is composed of two hemispheres and a central cylinder. Its axis is the center line of the link. Its radius is set as the maximum cross-sectional radius of the link plus a one-centimeter anti-collision margin. Combining the joint angle and link length parameters, the coordinates of each vertex and feature point of the double-arm body bounding box frame are multiplied by a homogeneous transformation matrix. The double-arm body bounding box frame is projected onto Cartesian space to generate the specific position set of each capsule in the Cartesian projection coordinate system, thus generating the Cartesian projection coordinate system.

[0028] Based on the Cartesian projection coordinate system, all capsule feature data after projection of the left and right arms are extracted. Using the GJK convex set distance detection algorithm, an iterative search is performed on the spatial distance between the vertices of the polygonal shells of two convex polyhedra or capsule shells in space. A Minkowski difference space is constructed, and the simplex closest to the origin is found within this space. If the difference space contains the origin, it indicates physical interference; otherwise, the minimum projection distance from the origin to the simplex is calculated. The three-dimensional coordinates of the two closest points between the two arm-bound box shells are then located, i.e., the coordinates of points located on the shells of a link in the left arm and a link in the right arm are obtained respectively. The coordinates of the nearest feature points are calculated, and the Euclidean distance between the two nearest feature points is extracted as the minimum vertex spacing value in the projected coordinates. When physical interference is detected (i.e., the spacing value is less than zero), the EPA algorithm is used to expand along the surface boundary of the Minkowski difference set polygon to find the polygon surface closest to the origin. The overlap depth is calculated along the normal direction of the intersecting and overlapping area, and this normal direction is used as the adjustment direction to escape the collision. The vector from this surface to the origin is extracted, and the coordinates of the deepest penetration direction are calculated. The distance value, the coordinates of the nearest point, and the penetration direction vector are merged into a data structure array to generate a spatial interference vector set.

[0029] The obstacle avoidance path rewriting module includes: The constraint equation establishment submodule sets the boundary conditions for the optimization solution variables based on the spatial interference vector set, sets the target term for minimizing the pose deviation, establishes the constraint term for the constant coordinates of the end probe, extracts the vertex spacing and penetration direction within the spatial interference vector set, restricts the spacing between the two arms to be outside the safety threshold, establishes the constraint term for the spacing being greater than the set boundary, and generates the obstacle avoidance constraint equation set. The updated angle calculation submodule performs multivariate function iterative differentiation and calculation based on the obstacle avoidance constraint equation set. It combines the pose deviation minimization target term with the end probe coordinate constant equality constraint term and the spacing greater than the set boundary inequality constraint term, and calculates the derivative to obtain the safe avoidance pose parameters of each joint that meet the boundary conditions, i.e., the updated angle value, and generates the updated angle value. The obstacle avoidance trajectory reconstruction submodule performs a time-dimensional polynomial curve fitting operation based on the updated angle values, interpolates the updated angle values ​​to generate waypoint coordinates, advances the generated waypoint coordinates according to the time sequence, links the remaining action coordinate points in the original deferred queue, outputs a smooth, collision-free execution sequence, and places the waypoint coordinates at the front end of the action command to generate the obstacle avoidance reconstruction trajectory segment.

[0030] Specifically, based on the spatial interference vector set, a quadratic programming optimization model to resolve the interference state is constructed. Boundary conditions for the optimization solution variables are set, using joint angular velocity or joint angle increment as optimization variables. A pose deviation minimization objective is set, limiting the fluctuation range of the avoidance pose. The mathematical expression of this objective is set as the square of the L2 norm of the joint angle increment. An end-effector coordinate constant equation constraint is established, constraining the end-effector position to remain fixed. Its matrix form is defined as the result of multiplying the robot's Jacobian matrix by the joint angle increment matrix, which equals a zero vector. The vertex spacing and penetration within the spatial interference vector set are extracted. The direction, combined with the calculated deepest penetration direction coordinates, restricts the distance between the two arms to outside the safety threshold. This safety threshold is set by adding a 2 cm safety margin to the maximum braking distance calculated based on the robot's control cycle and maximum operating speed. For example, a maximum operating speed of 50 cm / s multiplied by a control cycle of 0.01 s plus 2 cm results in a 2.5 cm safety boundary. An inequality constraint term is established that the distance is greater than the set boundary, requiring the minimum distance between the surfaces of the two arm bounding boxes to be greater than 2.5 cm. The target term, the equality constraint term, and the inequality constraint term are combined and assembled into a standard matrix solution vector space to generate an obstacle avoidance constraint equation system.

[0031] Based on the obstacle avoidance constraint equations, a numerical optimization solver is used to solve the quadratic programming problem online at high speed. The interior-point method is used as the core iterative algorithm for multivariate function iterative differentiation. The pose deviation minimization objective term is combined with the end-probe coordinate constant equality constraint term and the spacing greater than the set boundary inequality constraint term to construct a Lagrangian function. Lagrange multipliers corresponding to the equality constraints and slack variables and dual variables corresponding to the inequality constraints are introduced into the Lagrangian function. Partial derivatives are calculated with respect to each variable of the Lagrangian function to obtain the KKT condition equations. The KKT conditions are then solved using the Newton-Raphson iteration method. The search direction of the equation system is updated along the calculated search direction. A convergence criterion for stopping the iteration is set. This criterion is set as the L2 norm of the difference between the joint angle increments of two consecutive iterations. When the L2 norm is less than 10 to the power of negative 4, i.e., 0.0001, the equation solution is considered to have converged and the loop process is exited. Alternatively, when the maximum set limit of 50 iterations is reached, the loop is forcibly exited. The derivative is used to obtain the safe avoidance pose parameters of each joint that meet the boundary conditions. The joint angle value at the moment of suspension is added to the optimal joint angle increment obtained by the solution to update the angle value, thus generating the updated angle value.

[0032] Based on the updated angle values, the target angle positions of each joint in the obstacle avoidance and escape state are obtained. A smooth transition path is constructed between the current suspension breakpoint position and the target updated position. A time-dimensional polynomial curve fitting operation is performed, selecting a fifth-order polynomial as the position interpolation basis function. Using the six boundary conditions of the starting and ending angle positions, angular velocity equal to zero, and angular acceleration equal to zero, the six unknown time coefficients of the fifth-order polynomial are calculated. Interpolation calculations are performed on the updated angle values. The position of the polynomial at the corresponding time point is calculated according to the interpolation clock cycle of the servo controller, for example, every two milliseconds. The function value generates discrete waypoint angle coordinates, i.e., waypoint coordinates. The generated waypoint coordinates are then arranged and combined into a new obstacle avoidance trajectory data packet set by prioritizing them according to the time sequence. The remaining motion coordinate points in the original deferred queue are linked together. The tail of the obstacle avoidance trajectory data packet is appended to the queue of the original acquisition program that was not completed before the suspension and interruption, covering the old trajectory segment that caused interference. A smooth, collision-free execution sequence in which the joint angles, speeds, and accelerations change continuously throughout the process is output and sent to the firmware of each joint servo driver at the underlying level. The waypoint coordinates are then placed at the front end of the motion command to generate the obstacle avoidance reconstructed trajectory segment.

Claims

1. A mobile dual-arm body-mounted intelligent data acquisition platform, characterized in that, The platform includes: The frequency domain signal conversion module acquires vibration time-series signals from the tactile skin array, performs time-domain to frequency domain discrete signal decomposition transformation, calculates the dominant spatial frequency value corresponding to the amplitude peak value in the frequency domain signal sequence, extracts the energy spectral density value of the corresponding frequency band range of the frequency domain signal sequence, and combines the dominant spatial frequency value and the energy spectral density value to generate a tactile frequency domain energy set. The feed rate control module performs inverse proportional mapping and correlation calculation based on the tactile frequency domain energy set, extracts the energy spectral density value within the tactile frequency domain energy set, constructs an inverse proportional correspondence between the energy spectral density value and the feed rate variable value, obtains the feed rate variable value, and sends the feed rate variable value to the trajectory planning controller to generate a rate control instruction set.

2. The mobile dual-arm body-equipped intelligent data acquisition platform according to claim 1, wherein, The platform also includes: The spatial interference calculation module drives the joint servo motor according to the magnification control instruction set, extracts the force values ​​of the two arms, compares the force values ​​with the collision setting value, suspends the action command, reads the angle value of the joint servo motor, projects the two arms' enclosing box frame onto the Cartesian space, calculates the minimum vertex spacing value and the deepest penetration direction coordinate of the projection coordinates, and generates a spatial interference vector set. The obstacle avoidance path rewriting module sets a target term for minimizing pose deviation based on the spatial interference vector set, establishes an equality constraint term for constant end probe coordinates and an inequality constraint term for spacing greater than a set boundary, obtains the updated angle value by differentiation, performs interpolation on the updated angle value to generate waypoint coordinates, and places the waypoint coordinates into the front end of the action command to generate the obstacle avoidance reconstructed trajectory segment. 3.The mobile dual-arm body-equipped intelligent data acquisition platform of claim 1, wherein, The frequency domain signal conversion module includes: The discrete signal decomposition submodule acquires vibration time-series signals from the tactile skin array, performs time-series waveform truncation and discrete time-domain slicing, establishes a window corresponding to the signal timestamp, separates high- and low-frequency fluctuation components, extracts high-frequency vibration amplitude coordinates, transforms the time-domain vibration amplitude coordinates to the frequency-domain coordinate system, outputs the amplitude of each frequency component, and generates a frequency-domain signal sequence. The dominant frequency calculation submodule performs amplitude scanning and comparison of each frequency point in the frequency coordinate system according to the frequency domain signal sequence, extracts the value of the horizontal axis frequency point corresponding to the maximum amplitude value as the dominant spatial frequency value corresponding to the amplitude peak value, defines the frequency band boundary around the frequency point, performs square integral accumulation on the amplitude of each frequency point in the frequency band, obtains the energy spectral density value of the corresponding frequency band range, and generates spectral feature value pairs. The frequency domain energy combination submodule performs multi-dimensional numerical association binding based on the spectral feature values, extracts the dominant spatial frequency value corresponding to the amplitude peak and the corresponding frequency band range energy spectral density value, packs and aligns the two according to the time stamp order, establishes a composite structure containing spatial frequency and energy density, combines the dominant spatial frequency value and energy spectral density value, and generates a tactile frequency domain energy set.

4. The mobile dual-arm body-equipped intelligent data acquisition platform according to claim 1, wherein, The feed rate control module includes: The inverse proportional mapping submodule performs inverse proportional mapping correlation calculation based on the tactile frequency domain energy set, extracts the energy spectral density value within the tactile frequency domain energy set, establishes a basic feed parameter constant, performs a division operation with the basic feed parameter constant as the numerator and the energy spectral density value as the denominator, constructs an inverse proportional correspondence between the energy spectral density value and the target feed variable, and generates a magnification mapping function term. The multiplier variable calculation submodule performs real-time parameter substitution and calculation based on the multiplier mapping function term, monitors the energy spectral density value increment under the current input state, substitutes the increased energy spectral density value into the denominator of the inverse proportional relationship, calculates the target feed ratio parameter after attenuation, and calculates the corresponding feed multiplier variable value. The control instruction generation submodule rewrites the underlying actuator parameters based on the feed rate variable value, reads the preset feed speed parameter, multiplies the preset feed speed parameter with the attenuated feed ratio parameter to obtain the corrected end-effector movement speed value, and sends the feed rate variable value to the trajectory planning controller to generate a rate control instruction set.

5. The mobile dual-arm body-equipped intelligent data acquisition platform according to claim 2, wherein, The spatial interferometry calculation module includes: The collision force monitoring submodule drives the joint servo motor according to the multiplier control instruction set, extracts the torque feedback parameters of each node of the double arm body, calculates the force value of the double arm body, retrieves the preset collision force threshold constant, compares the force value with the collision setting value, cuts off the motor drive signal when the force value exceeds the collision setting value, suspends the action command, and generates an over-limit force status indicator. The bounding box projection submodule performs a positive kinematic space position transformation based on the over-limit force state indicator, reads the angle values ​​of the joint servo motors, retrieves the three-dimensional geometric shell size parameters of the two arms, constructs the bounding box frame of the two arms body along the axis of each link, and projects the bounding box frame of the two arms body onto Cartesian space in combination with the joint angle and link length parameters to generate a Cartesian projection coordinate system.

6. The movable two-arm body intelligent data acquisition platform according to claim 5, characterized in that, The spatial interferometry calculation module also includes: The penetration measurement and positioning submodule performs iterative search of the spatial position distance of the vertices of the polygonal shell according to the Cartesian projection coordinate system, locates the three-dimensional coordinates of the two closest points between the two arm bounding box shells, calculates the minimum vertex spacing value of the projection coordinates, measures the overlap depth along the normal direction of the intersecting and overlapping area, calculates the coordinates of the deepest penetration direction, and generates a spatial interference vector set.

7. The movable two-arm body intelligent data acquisition platform according to claim 2, characterized in that, The obstacle avoidance path rewriting module includes: The constraint equation establishment submodule sets the boundary conditions for the optimization solution variables based on the spatial interference vector set, sets the target term for minimizing the pose deviation, establishes the constraint term for the constant coordinates of the end probe, extracts the vertex spacing and penetration direction within the spatial interference vector set, restricts the distance between the two arms to be outside the safety threshold, establishes the constraint term for the distance being greater than the set boundary, and generates the obstacle avoidance constraint equation set. The updated angle calculation submodule performs multivariate function iterative differentiation calculation based on the obstacle avoidance constraint equation set. It combines the pose deviation minimization target term with the end probe coordinate constant equality constraint term and the spacing greater than the set boundary inequality constraint term, and calculates the derivative to obtain the safe avoidance pose parameters of each joint that meet the boundary conditions, i.e., the updated angle value, and generates the updated angle value. 8.The movable dual-arm body-equipped intelligent data acquisition platform according to claim 7, characterized in that, The obstacle avoidance path rewriting module also includes: The obstacle avoidance trajectory reconstruction submodule performs a time-dimensional polynomial curve fitting operation based on the updated angle value, performs interpolation calculation on the updated angle value to generate waypoint coordinates, advances the generated waypoint coordinates according to the time sequence, links the remaining action coordinate points in the original deferred queue, outputs a smooth collision-free execution sequence, and places the waypoint coordinates into the front end of the action command to generate the obstacle avoidance reconstruction trajectory segment.

Images