A control system for a drone equipped with a robotic arm

Abstract

This invention relates to the field of unmanned aerial vehicle (UAV) control technology and discloses a control system for a UAV equipped with a robotic arm. The system includes a flight control module, a robotic arm main control module, a communication bus, an electronic speed controller (ESC) module, and a joint servo module. The robotic arm main control module calculates and estimates the comprehensive reaction torque and the original feedforward speed correction vector based on the target joint state. The flight control module calculates the actual resultant external torque based on inertial measurement data, combines it with the historically estimated reaction torque with time alignment to obtain the residual vector of external environmental disturbance, and generates an adaptive mixing gain coefficient to scale the feedforward correction vector, which is then merged with the basic motor speed command. The communication bus broadcasts a message carrying a global absolute clock timestamp to trigger the execution module to synchronize its actions. This invention decouples internal robotic arm motion disturbances from external airflow disturbances, eliminates communication transmission time deviations, and improves the system's attitude stability and control accuracy in complex environments.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) control technology, specifically to a control system for a UAV equipped with a robotic arm. Background Technology

[0002] The robotic arm system can expand the drone's operational capabilities and is widely used in tasks such as aerial inspection and material handling. In actual operation, the stress state of the system is quite complex. When the robotic arm changes its configuration or performs actions in three-dimensional space, it will change the distribution of the drone's center of mass and generate a dynamic reaction torque on the drone's body. At the same time, the drone will also be subject to external environmental disturbances such as airflow when flying outdoors.

[0003] Existing control architectures typically treat internal physical disturbances generated by the robotic arm's movement and external aerodynamic disturbances caused by airflow as a unified, concentrated disturbance, relying primarily on the basic attitude closed-loop controller of the flight control system for passive adjustment. This approach makes it difficult to independently separate and decouple disturbance sources with different physical properties. Because it is impossible to accurately extract the amount of external environmental airflow disturbance, the system cannot dynamically adjust the proportion of the robotic arm's feedforward compensation based on the objective intensity of external disturbances when facing complex environments. This can lead to excessive feedforward commands being superimposed when external wind disturbances are large, resulting in overcompensation or system control divergence, causing flight attitude fluctuations.

[0004] On the other hand, existing UAV and robotic arm combination systems mostly adopt a distributed hardware bus structure. When control commands are cascaded and transmitted between the flight control module, the robotic arm main controller, the rotor ESC, and the joint servo mechanism, communication delays are inevitable. Due to the lack of a hardware-level global timing alignment mechanism, the control commands of the underlying actuators are often processed and executed sequentially according to the arrival order of network data packets. This results in a misalignment of microseconds to milliseconds between the moment when the rotor motor adjusts its speed to generate compensating force and the moment when the robotic arm joint actually rotates. This timing deviation at the physical execution level amplifies instantaneous dynamic errors and limits the overall control accuracy of the UAV-equipped robotic arm system in complex collaborative operations. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a control system for a drone equipped with a robotic arm, which solves the problems of existing systems being unable to decouple internal disturbances generated by the movement of the robotic arm from external airflow disturbances, and the timing misalignment in distributed hardware command transmission leading to control instability.

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

[0007] This invention provides a control system for a drone equipped with a robotic arm, including a flight frame, a flight control module, a robotic arm main control module, an inertial measurement module, an electronic speed controller module, a communication bus, and a joint servo module; The main control module of the robotic arm is used to calculate and estimate the comprehensive reaction torque and the original feedforward speed correction vector based on the target joint state and send them to the flight control module cache. The flight control module is used to back-calculate the actual resultant external torque based on the data from the inertial measurement module, combine the historically estimated reaction torque with time-aligned data to obtain the residual vector of external environmental disturbance, generate an adaptive mixing gain coefficient, and merge the original feedforward speed correction vector with the basic motor speed command vector to form a synthetic motor control command vector. The communication bus broadcasts a message carrying a global absolute clock timestamp, triggering the ESC module and the joint servo module to execute synchronously.

[0008] Therefore, this system combines the disturbance feedforward of the internal control of the robotic arm with the residual separation calculation of the external environment, and realizes the timing alignment of multiple actuators through the timestamp broadcasting mechanism.

[0009] Furthermore, the target joint state includes a target joint angle vector, a target joint angular velocity vector, and a target joint angular acceleration vector. The robotic arm main control module converts the target joint angle vector into a low-level memory address offset, extracts the static centroid position vector, the equivalent gravity scalar parameter, and the augmented dynamic coupling matrix. It uses the target joint angular velocity vector to generate a quadratic velocity term vector, and performs matrix multiplication and algebraic operations with the target joint angular acceleration vector and the augmented dynamic coupling matrix. It combines the actual static reaction force vector with vector cross product and addition to restore the estimated comprehensive reaction torque and estimated comprehensive reaction force vector for the next clock synchronization cycle. In this way, the dynamic disturbance physical quantities generated by the robotic arm movement are calculated at the control feedforward end.

[0010] Furthermore, the main control module of the robotic arm concatenates the transposed vector of the estimated comprehensive reaction force with the transposed vector of the estimated comprehensive reaction torque to construct a comprehensive disturbance feedforward vector; based on the rotor spatial position coordinate system, rotor thrust coefficient, and rotor torque coefficient of the flight frame, it constructs a pseudo-inverse of the generalized power distribution matrix with horizontal underactuated dimension rank reduction processing, and performs matrix multiplication mapping with the comprehensive disturbance feedforward vector to generate the original feedforward speed correction vector acting on each independent rotor motor node; thereby converting the disturbance torque parameters generated by the robotic arm in three-dimensional space into speed compensation values ​​of the underlying rotor motor.

[0011] In a preferred embodiment of the present invention, the flight control module has a first-in-first-out (FIFO) historical circular buffer in its internal random access memory. The memory depth threshold of this buffer is configured as the ratio of the maximum estimated group delay time constant to the single motion control cycle time, rounded down and supplemented with a preset safety redundancy margin. The flight control module extracts the target execution absolute clock timestamp and binds it with the estimated combined reaction torque, packaging it into the main stack area of ​​the historical circular buffer. This saves the estimated reaction torque data and provides a buffer basis for the data timing alignment of the control loop.

[0012] Furthermore, the flight control module reads the three-axis angular velocity data from the inertial measurement module in real time, performs discrete-time difference analysis, and obtains the three-axis fuselage angular acceleration vector after first-order low-pass filtering. The three-axis fuselage angular acceleration vector is then multiplied with the pre-fixed fuselage inertia tensor matrix, and the result is vector-superimposed with the cross product of the fuselage angular velocity vector and fuselage angular momentum vector caused by the undifferentiated original three-axis fuselage angular velocity data to accurately calculate the actual net external torque currently borne by the flight frame. This allows for the calculation of the actual physical force data currently experienced by the flight frame, including external airflow disturbances.

[0013] Furthermore, the flight control module obtains the current system absolute clock timestamp and subtracts the fixed group delay time constant calibrated by the digital filter frequency response characteristics, calculates the target matching timestamp, and extracts the historical estimated comprehensive reaction torque with the smallest absolute error to the target matching timestamp by traversing the queue of the historical circular buffer. The actual resultant external torque is then vector-subtracted from the historical estimated comprehensive reaction torque and the actual control torque obtained by forward mapping to solve for the pure external environment disturbance residual vector. This compensates for the digital filter delay in the time domain, separates the internal control torque of the robotic arm, and independently extracts the disturbance components of the external environment.

[0014] In a preferred embodiment of the present invention, the flight control module calculates the vector magnitude of the pure external environment disturbance residual vector. When the vector magnitude is less than the lower limit threshold of the safe residual, the adaptive mixing gain coefficient is configured as a full gain scalar value. When the vector magnitude is between the lower limit threshold of the safe residual and the upper limit threshold of the dangerous residual, the adaptive mixing gain coefficient performs linear decay according to a preset linear formula. When the vector magnitude is greater than the upper limit threshold of the dangerous residual, the adaptive mixing gain coefficient is forcibly configured to zero. In this way, the feedforward compensation weight is dynamically adjusted according to the disturbance residual magnitude of the external environment to avoid the superposition of feedforward commands when the set disturbance limit is exceeded, which would cause system control divergence.

[0015] Furthermore, the flight control module performs proportional scaling of the adaptive hybrid gain coefficient and the original feedforward speed correction vector using scalar-vector algebraic multiplication to generate the final feedforward injection vector. An independent data overlay channel is established at the underlying power distribution output node, and the final feedforward injection vector is combined with the basic motor speed command vector independently calculated by the system's basic attitude closed-loop control branch through vector addition to generate the synthetic motor control command vector. This bypasses the basic attitude closed loop, enabling independent overlay and feedforward merging of underlying power commands.

[0016] Furthermore, before outputting, the flight control module performs bidirectional saturation truncation verification on each dimension of the synthesized motor control command vector. When the synthesized command value exceeds the maximum supported rated speed threshold, it is forcibly limited to the maximum value. When the synthesized command value is lower than the set minimum idle speed physical threshold of the motor, it is forcibly limited to the minimum value. The feedforward independent injection channel is used to directly act on the final actuator control hardware level. This limits the synthesized command value from not exceeding the hardware configuration boundary and ensures the operational safety of the underlying power output module.

[0017] Furthermore, at the end of each control cycle, the communication bus broadcasts a message carrying a global absolute clock timestamp to the bus network; the ESC module triggers the underlying pulse width modulation electrical signal of the synthesized motor control command vector to flip and output based on the global absolute clock timestamp; when the joint servo module resolves the same global absolute clock timestamp, it synchronously drives the motor to change the stator coil current, eliminating the command arrival time deviation caused by the cascaded transmission of multiple communication nodes; thereby ensuring that the rotor motor power output and the joint servo action execution are synchronized at the physical timing level.

[0018] This invention provides a control system for a drone equipped with a robotic arm. It has the following advantages: 1. This invention pre-calculates the estimated comprehensive reaction torque generated by the robotic arm's movement through the main control module of the robotic arm, and performs a difference calculation by combining inertial measurement data and historical reaction torques with the flight control module to separate the residual vector of pure external environmental disturbance. This method decouples the dynamic disturbance of the robotic arm inside the system from the external airflow disturbance, avoids the calculation deviation of compensation amount caused by the coupling of internal and external disturbances in conventional control, and improves the flight attitude stability of the UAV during the robotic arm operation.

[0019] 2. Based on the calculated magnitude of the residual vector of the pure external environmental disturbance, this invention configures adaptive mixing gain coefficients in segments to dynamically scale the original feedforward rotation speed correction vector to generate the final feedforward injection vector. This design enables the system to automatically adjust the weight of the internal feedforward compensation according to the objective intensity of the external airflow disturbance, preventing the UAV from causing system control divergence due to excessive feedforward commands when encountering extreme external disturbances, and ensuring the safe operation of the underlying hardware in complex operating environments.

[0020] 3. This invention utilizes a communication bus to broadcast a synchronization trigger message carrying a global absolute clock timestamp at the end of the control cycle, enabling the underlying ESC module and joint servo module to synchronously trigger physical execution actions after parsing the same timestamp. This mechanism eliminates the instruction arrival time deviation caused by node cascading transmission in the distributed control architecture from the hardware level, ensuring that the timing of the rotor system's power output compensation and the robotic arm's spatial attitude adjustment actions are strictly aligned, reducing instantaneous control errors caused by timing misalignment. Attached Figure Description

[0021] Figure 1 This is a schematic diagram of the overall hardware architecture of the unmanned aerial vehicle (UAV) equipped with a robotic arm system of the present invention; Figure 2 This is a schematic diagram of the main flow of the overall system control method of the present invention; Figure 3 This is a schematic diagram of the dynamic parameter dimensionality reduction mapping and online interpolation reconstruction process of the present invention; Figure 4 This is a schematic diagram of the feedforward correction vector calculation and disturbance residual stripping process of the present invention; Figure 5 This is a schematic diagram of the feedforward power superposition and multi-actuator global synchronization triggering process of the present invention; Figure 6 This is a schematic diagram showing the comparison of fuselage pitch angle deviation under different control methods of the present invention; Figure 7 This is a schematic diagram of the adaptive mixing gain and the dynamic response of the external disturbance residual of the present invention (where a is a schematic diagram of the adaptive mixing gain coefficient changing with time, and b is a schematic diagram of the magnitude of the external environmental disturbance residual vector changing with time).

[0022] Among them, 100 is the flight frame; 200 is the robotic arm; 300 is the flight control module; 400 is the robotic arm main control module; 500 is the inertial measurement module; 600 is the force sensing module; 700 is the electronic speed controller module; 800 is the joint servo module; and 900 is the communication bus. Detailed Implementation

[0023] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0024] Reference Figure 1 This invention provides a drone-mounted robotic arm system, which includes: a flight frame 100, a robotic arm 200, a flight control module 300, a robotic arm main control module 400, an inertial measurement module 500, a force sensing module 600, an electronic speed controller module 700, a joint servo module 800, and a communication bus 900.

[0025] Among them, the base of the robotic arm 200 is rigidly fixed to the central axis of the bottom of the flight frame 100, the flight control module 300 is installed in the internal center of the flight frame 100, and the main control module 400 of the robotic arm is installed inside the base of the robotic arm 200. The flight control module 300 and the robotic arm main control module 400 establish bidirectional data communication and are synchronized with the underlying clock via the communication bus 900. The inertial measurement module 500 is rigidly attached to the inside of the flight frame 100, and its data output terminal is electrically connected to the data input terminal of the flight control module 300. The force sensing module 600 is installed on the end effector flange of the robotic arm 200, and its data output terminal is electrically connected to the data input terminal of the robotic arm main control module 400. The communication control terminals of the ESC module 700 and the joint servo module 800 are both electrically connected to the communication bus 900. The flight control module 300 and the robotic arm main control module 400 send digital control commands to the corresponding execution hardware through the communication bus 900.

[0026] Reference Figure 2 The present invention also provides a flight operation cooperative control method based on the above system, the method comprising the following steps: S100: The non-volatile memory of the robotic arm main control module 400 contains a dimension-reduced static pose mapping table and a dynamic coupling mapping table. The dimension-reduced static pose mapping table and the dynamic coupling mapping table are extracted from the physical parameters and link dynamics model of the robotic arm 200. They are generated offline by stripping nonlinear pose parameters and linear motion parameters for multiple preset reference mass levels.

[0027] S200: After the gripping operation is completed, the robotic arm main control module 400 receives the static load torque increment signal collected by the force sensing module 600; and calculates the equivalent physical mass of the gripped object based on the current link posture and the static load torque increment, and locates the adjacent reference mass level corresponding to the equivalent physical mass; then performs linear interpolation operation on the corresponding dimension-reduced static posture mapping table and dynamic coupling mapping table in the storage area to generate a working mapping table adapted to the current physical load.

[0028] S300: At the beginning of a single motion control cycle, the robotic arm main control module 400 analyzes the target joint state of the joint servo module 800 in the next clock synchronization cycle, and uses the target joint angle as the input index to perform memory addressing on the working mapping table to extract the corresponding static parameter matrix and dynamic parameter matrix. Subsequently, the robotic arm main control module 400 performs algebraic multiplication and addition operations on the extracted parameter matrix and the target joint angular velocity and target joint angular acceleration to calculate and output the estimated reaction torque and estimated comprehensive reaction force vector for the next clock synchronization cycle, and converts them into the original feedforward speed correction vector. At the same time, the flight control module 300 synchronously receives the estimated reaction torque and pushes it into the internal historical circular buffer along with the corresponding target execution absolute clock timestamp.

[0029] S400: The flight control module 300 reads the fuselage angular acceleration data fed back by the inertial measurement module 500 in real time and calculates the actual torque borne by the flight frame 100. Based on the inherent digital filter group delay time constant of the inertial measurement module 500, the flight control module 300 extracts the estimated reaction torque with the same time constant timestamp lag from the historical circular buffer. Then, the actual torque is calculated by subtracting the extracted estimated reaction torque to obtain the error residual, and the adaptive mixing gain coefficient of the feedforward branch is generated based on the error residual.

[0030] S500: The control module 300 multiplies the original feedforward speed correction vector with the adaptive hybrid gain coefficient to generate the final feedforward injection vector; at the power distribution matrix output node at the bottom layer of the flight control module 300, the final feedforward injection vector is vector-superimposed and merged with the basic motor speed command generated by the system's basic attitude closed-loop control branch; relying on the global distributed clock pulse provided by the communication bus 900, the ESC module 700 and the joint servo module 800 execute the superimposed and merged speed command and joint motion command without time difference under the same hardware clock beat.

[0031] Reference Figure 3 Step S100 specifically includes: S110: The robotic arm 200 mounted on the flight frame 100 is considered as a rigid body linkage system with a moving base. Its dynamic coupling disturbance during spatial trajectory motion is influenced by both joint pose and kinematic parameters. A multibody dynamics model of the rigid body linkage system is constructed based on theoretical mechanics principles, and the nonlinear pose parameters containing trigonometric function operations and the linear motion parameters with derivative properties are algebraically separated. The robotic arm is defined to have N joint degrees of freedom, and its motion state is described by the joint angle vector, joint angular velocity vector, and joint angular acceleration vector. The formula for calculating the dynamic reaction torque of the robotic arm at the base of the flight frame 100 is: ; In the formula, This refers to the dynamic reaction torque of the robotic arm; The equivalent inertia matrix of the robotic arm linkage system projected onto the UAV base coordinate system; The joint angle vector; The joint angular acceleration vector. The Coriolis and centrifugal force coupling matrix is ​​projected onto the base coordinate system; It is a vector of the second-order velocity term; This is the joint angular velocity vector.

[0032] Through the above algebraic separation, the equivalent inertia matrix and Coriolis and centrifugal force coupling matrix, which contain trigonometric functions and matrix inversion operations in analytical form, are decoupled into a data array that is only affected by a single physical variable, namely the joint angle vector; the solution process of dynamic reaction torque is transformed into linear algebraic calculation of kinematic derivative parameters; optionally, in this embodiment, the Newton-Euler iterative algorithm is used to derive the equivalent inertia matrix and Coriolis matrix.

[0033] S120: The external loads faced by the robotic arm 200 in actual grasping operations exhibit a continuous distribution characteristic. Constrained by the limits of the underlying physical storage space of the control system, a set of discrete reference mass nodes is set within the expected working load range. In this embodiment, the number of discrete reference mass nodes is set to a range of 5 to 10. A set of reference mass levels is defined, where the lower bound node of the set corresponds to the unloaded physical state of the robotic arm 200, and the upper bound node corresponds to the maximum safe working load mass limited by the system based on the maximum torque of the actuator motor. In this embodiment, the maximum safe operating load mass ranges from 1.5kg to 5.0kg; the remaining intermediate nodes are divided and set between the two according to the equidistant distribution rule or the logarithmic distribution rule, and the above-mentioned finite node set provides boundary anchor points for subsequent online dynamic interpolation calculations.

[0034] S130: For any reference mass in the reference mass hierarchy set, the system performs offline calculations based on the link length and mass distribution parameters preset by the robotic arm 200. The system generates a dimension-reduced static pose mapping table stripped of the kinematic derivative dimension inside the robotic arm main control module 400. This table uses the joint angle vector as the unique addressing input index.

[0035] The reduced-dimensional static pose mapping table internally stores: the static centroid position vector of the overall center of gravity relative to the geometric center of the flight frame 100 under the current system pose, and the equivalent gravity scalar parameters of the linkage system acting on the base of the flight frame 100 under the influence of the gravitational field.

[0036] Simultaneously, a consistent offline solution logic is used to generate a dynamic coupling mapping table. This table also uses the joint angle vector as the unique addressing index to map and store the augmented dynamic coupling matrix under the current pose. The augmented dynamic coupling matrix is ​​formed by horizontally concatenating the aforementioned extracted equivalent inertia matrix with the Coriolis and centrifugal force coupling matrices. The calculation formula is as follows: ; In the formula: For augmented dynamic coupling matrix; The equivalent inertia matrix; The Coriolis and centrifugal force coupling matrix; This is the joint angle vector.

[0037] The dimensionality-reduced static pose mapping table and the dynamic coupling mapping table are written into the non-volatile storage area inside the robotic arm main control module 400 as array data structures. Through the above-mentioned dimensionality reduction physical stripping mechanism, the conventional multidimensional table lookup is reduced to a single joint angle addressing space, thereby converting the complex multibody differential equation solution into a low-level memory address offset reading process.

[0038] In this embodiment, step S200 specifically includes: S210: When the robotic arm 200 completes the grasping and closing action of the target object and the system is in a relatively static state, the robotic arm main control module 400 reads the end-effector triaxial force and torque data output by the force perception module 600; extracts the force vector from the measurement data, and obtains the current fuselage attitude angle of the flight frame 100 fed back by the flight control module 300; Combining the current joint pose parameters of the robotic arm 200 with the body attitude angle, the force vector is transformed to the global gravity coordinate system through forward kinematics. Subsequently, the constant gravity deviation component of the end gripper is eliminated by matrix transformation to obtain the static load force increment generated purely by the grasped object. The projection modulus of the static load force increment in the gravity direction is divided by the constant gravitational acceleration to calculate the actual equivalent physical mass of the grasped object.

[0039] S220: The robotic arm main control module 400 inputs the calculated equivalent physical mass into the preset benchmark mass level set for traversal comparison; if the equivalent physical mass exceeds the aforementioned maximum safe operating load mass, an overload alarm is triggered and the calculated input value is forcibly truncated to the maximum safe operating load mass; if the equivalent physical mass is less than the aforementioned no-load benchmark mass due to sensor zero bias noise, its calculated input value is forcibly truncated to the no-load benchmark mass to prevent the underlying memory from going out of bounds or interpolation divergence.

[0040] For the equivalent physical mass within the boundary range of the set, two adjacent reference mass nodes that enclose the equivalent physical mass are locked in the set and defined as the lower boundary reference mass and the upper boundary reference mass, respectively. The lower boundary reference mass is strictly smaller than the upper boundary reference mass, so that the equivalent physical mass satisfies the logical judgment condition between the two. When the equivalent physical mass is exactly equal to a certain reference mass node, the reference mapping table of that single node is directly extracted and subsequent interpolation operations are skipped. Otherwise, the robotic arm main control module 400 retrieves the corresponding reference mapping table data block from the internal non-volatile storage area based on the locked boundary node.

[0041] S230: When the linkage system grasps objects of different masses, the overall centroid offset and dynamic coupling matrix of the system are considered to follow a first-order linear mapping relationship within adjacent small mass intervals; the robotic arm main control module 400 calculates the interpolation weight coefficients using the relative offset of the equivalent physical mass within the addressing interval, performs linear interpolation on the extracted lower boundary dimensionality-reduced static pose mapping table and upper boundary dimensionality-reduced static pose mapping table, and generates a working static pose mapping table adapted to the current physical load. The calculation formula is as follows: ; In the formula: This is a static pose mapping table for the working position; A dimensionality-reduced static pose mapping table for the lower boundary; Equivalent physical mass; The lower boundary reference mass; The upper boundary reference mass; A dimension-reduced static pose mapping table for the upper boundary; This is the joint angle vector.

[0042] The fractional terms in the formula represent the linear bias weights of the current physical load within the addressing quality range; similarly, the robotic arm main control module 400 uses consistent interpolation weight coefficients to perform a linear hybrid operation on the extracted lower boundary dynamic coupling mapping table and upper boundary dynamic coupling mapping table to generate the working dynamic coupling mapping table, calculated as follows: ; In the formula: For dynamic coupling mapping table of work; This is the lower boundary dynamic coupling mapping table; Equivalent physical mass; The lower boundary reference mass; The upper boundary reference mass; This is the upper boundary dynamic coupling mapping table; This is the joint angle vector.

[0043] The robotic arm main control module 400 stores the reconstructed static pose mapping table and dynamic coupling mapping table into the high-frequency lookup table working area in the internal random access memory in the form of data block overwrite. In subsequent continuous motion control cycles, memory addressing and calling operations are only performed on this high-frequency lookup table working area, and the online state parameter synchronization configuration of the offline control model is completed by relying on the end physical feedback closed loop.

[0044] Reference Figure 4 Step S300 specifically includes: S310: At the start of the control cycle of the system operation, the trajectory planner inside the main control module 400 of the robotic arm parses and outputs the target joint state of the joint servo module 800 in the next clock synchronization cycle. The target joint state includes the target joint angle vector, the target joint angular velocity vector and the target joint angular acceleration vector. The robotic arm main control module 400 uses the value of the target joint angle vector as the input index and converts it into the low-level memory address offset of the high-frequency lookup table working area. Based on this memory address offset, it performs memory addressing operations on the working static pose mapping table and the working dynamic coupling mapping table, and simultaneously extracts the static centroid position vector, equivalent gravity scalar parameter and augmented dynamic coupling matrix corresponding to the target pose.

[0045] S320: The robotic arm main control module 400 uses the target joint angular velocity vector to calculate and generate the corresponding quadratic velocity term vector; it then performs matrix multiplication and algebraic operations with the extracted augmented dynamic coupling matrix, the target joint angular acceleration vector, and the quadratic velocity term vector.

[0046] Obtain the fuselage attitude rotation matrix of the current flight frame 100, use this matrix to reverse the global gravitational acceleration direction vector to the fuselage base coordinate system, and multiply it with the equivalent gravity scalar parameter to generate the actual static reaction force vector under the current fuselage attitude; Subsequently, the extracted static centroid position vector is multiplied by the actual static reaction force vector to obtain the actual static reaction torque after rigorous physical compensation of spatial attitude in real time. The above dynamic calculation result is then added to and combined with the static reaction torque and static reaction force vector to restore the estimated comprehensive reaction torque and estimated comprehensive reaction force vector for the next clock synchronization cycle. The calculation formula is as follows: ; In the formula: To estimate the overall reaction torque; It is a static reaction torque; For augmented dynamic coupling matrix; The target joint angle acceleration vector; The target joint angular velocity vector; This is a quadratic velocity term vector generated based on the target joint angular velocity vector; optionally, the microprocessor can call a hardware multiply-accumulate unit to perform the above calculations to achieve hardware-level acceleration.

[0047] S330: To effectively distribute the concentrated physical disturbances generated by the robotic arm in the base coordinate system to the independent actuators of the flight platform, the robotic arm main control module 400 transposes the estimated comprehensive reaction force vector extracted by addressing and concatenates it with the transpose of the estimated comprehensive reaction torque vector to construct the comprehensive disturbance feedforward vector: ; In the formula: This is the feedforward vector for the integrated disturbance; To estimate the transpose matrix of the combined reaction force vector; This is the transpose matrix for estimating the combined reaction torque.

[0048] Based on the inherent rotor spatial position coordinate system, rotor thrust coefficient, and rotor torque coefficient of the flight frame 100, a locally linearized generalized dynamic distribution matrix with rank reduction processing of the horizontal underactuated dimension is constructed, and the corresponding pseudo-inverse of the generalized dynamic distribution matrix is ​​obtained. Since the flight frame 100 is a horizontal underactuated system, the column vector elements corresponding to the horizontal and longitudinal reaction force dimensions in the pseudo-inverse of the generalized dynamic distribution matrix are pre-configured to zero.

[0049] The robotic arm main control module 400 performs matrix multiplication mapping between the pseudo-inverse of the generalized power distribution matrix and the comprehensive disturbance feedforward vector; through the above-mentioned zeroing mechanism, it automatically filters out horizontal disturbance components that cannot be directly offset by motor differential speed, and converts the spatial physical disturbance at the fuselage base into the original feedforward speed correction vector acting on each independent rotor motor node: ; In the formula: This is the original feedforward rotational speed correction vector; The pseudo-inverse of the generalized dynamical assignment matrix; This is the feedforward vector for the integrated disturbance.

[0050] The numerical dimension of the original feedforward rotational speed correction vector is consistent with the total number of rotors physically assembled on the flight frame 100.

[0051] S340: The robotic arm main control module 400 synchronously sends the calculated original feedforward speed correction vector and the estimated comprehensive reaction torque to the flight control module 300 via the communication bus 900; the flight control module 300 has a first-in-first-out historical circular buffer in its internal random access memory; the memory depth threshold of this buffer is configured as the ratio of the maximum estimated group delay time constant to the single motion control cycle time rounded down, with an additional preset safety redundancy margin; In this embodiment, the maximum estimated group delay time constant ranges from 15ms to 50ms, the single motion control cycle time ranges from 2ms to 10ms, and the additional preset safety redundancy margin ranges from 2 to 5 queue units.

[0052] After receiving the above data, the flight control module 300 extracts the corresponding target execution absolute clock timestamp, that is, the timestamp of the next clock synchronization cycle, binds the estimated comprehensive reaction torque with the absolute clock timestamp, packages it and pushes it into the main stack area of ​​the historical circular buffer; and establishes an automatic overwrite mechanism for historical data through a circular queue with limited memory depth to maintain the continuous disturbance prediction state sequence within the safe time window.

[0053] In this embodiment, step S400 specifically includes: S410: During the flight control and operation coordination process, the flight control module 300 reads the three-axis angular velocity data fed back by the inertial measurement module 500 in real time; by performing discrete time difference on the three-axis angular velocity data of adjacent control cycles and processing it through a first-order low-pass filter, the corresponding three-axis fuselage angular acceleration vector is calculated.

[0054] The flight control module 300 has a pre-embedded fuselage inertia tensor matrix characterizing the overall mass distribution of the flight frame 100 and its mounted equipment. The flight control module 300 performs a preliminary matrix multiplication between the three-axis fuselage angular acceleration vector and the fuselage inertia tensor matrix, and then performs vector superposition of the result with the nonlinear gyroscopic torque term caused by the undifferentiated original three-axis fuselage angular velocity data, i.e., the cross product of the fuselage angular velocity vector and the fuselage angular momentum vector, to accurately calculate and obtain the actual net external torque currently borne by the flight frame 100. ; In the formula: This is the actual resultant external torque; Here is the fuselage inertia tensor matrix; This is the angular acceleration vector of the three-axis fuselage; This is the gyroscopic torque term.

[0055] Subsequently, the flight control module 300 calculates the actual control torque generated by the rotor of the flight frame 100 at that historical moment by forward mapping based on the actual output motor speed and the generalized power distribution matrix of the same group delay time constant in the system history.

[0056] S420: The internal hardware-level low-pass filter of the inertial measurement module 500 has the physical characteristic of eliminating high-frequency vibration noise, and is accompanied by the inherent signal phase lag phenomenon; the system calibrates a fixed group delay time constant by extracting the frequency response characteristic parameters of the digital filter.

[0057] In this embodiment, the fixed group delay time constant ranges from 10ms to 45ms. The flight control module 300 obtains the current system absolute clock timestamp and subtracts the group delay time constant to calculate the target matching timestamp. It then performs a traversal comparison in the queue of the historical circular buffer to extract the historical estimated comprehensive reaction torque with the smallest absolute error to the target matching timestamp.

[0058] S430: Internal state misalignment caused by sensor delay can lead to physical interference between feedforward and feedback signals at incorrect time points; the flight control module 300 performs vector subtraction calculations on the calculated actual net external torque, the extracted and aligned historical estimated combined reaction torque, and the actual control torque to obtain the pure external environment disturbance residual vector after removing the known dynamic coupling of the robotic arm 200 and the system's own control output: ; In the formula: The residual vector is the result of external environmental disturbances. This is the actual resultant external torque; This is the actual control torque; For historical estimation of comprehensive reaction torque; The disturbance residual vector represents the total amount of external uncontrollable airflow interference and unmodeled errors that the system currently experiences, which are not covered by the feedforward model. By forcibly pulling the actual sensing measurements and historical estimates into the same physical time base for calculation, false residual signal jumps caused by phase delay are eliminated.

[0059] S440: To avoid error coupling amplification when the feedforward model faces strong external nonlinear physical airflow interference, the flight control module 300 calculates the vector magnitude of the disturbance residual vector and inputs it into the preset confidence evaluation function. The system is pre-set with a lower limit threshold for safe residuals and an upper limit threshold for dangerous residuals based on the maximum attitude recovery torque determined by the wind tunnel calibration experiment of the 100-meter flight frame. In this embodiment, the lower limit threshold for safe residuals ranges from 0.5 N·m to 2.0 N·m, and the upper limit threshold for dangerous residuals ranges from 3.0 N·m to 8.0 N·m.

[0060] The evaluation function is configured as follows: When the vector magnitude is less than the lower limit of the safety residual threshold, the adaptive mixing gain coefficient is configured as a full-gain scalar value of 1.0; when the vector magnitude is between the lower limit of the safety residual threshold and the upper limit of the danger residual threshold, the adaptive mixing gain coefficient decays linearly according to the following formula: ; In the formula: For adaptive mixing gain coefficient; This is the upper limit threshold for dangerous residuals; Let be the vector magnitude of the residual vector caused by external environmental disturbance; The lower limit threshold for safety residuals; The residual vector is the result of external environmental disturbances. When the vector magnitude exceeds the upper limit threshold of the dangerous residual, the adaptive mixing gain coefficient is forcibly configured to a scalar value of 0; the above adaptive mapping mechanism limits the feedforward injection weight boundary when the system is in a severe airflow environment.

[0061] Reference Figure 5 Step S500 specifically includes: S510: The flight control module 300 extracts the original feedforward speed correction vector and adaptive mixing gain coefficients. It then performs proportional numerical scaling on each element of the original feedforward speed correction vector through algebraic multiplication of scalar and vector data to calculate and generate the final feedforward injection vector. ; In the formula: Inject vectors into the final feedforward; For adaptive mixing gain coefficient; This is the original feedforward rotational speed correction vector; it is used to generate the feedforward correction numerical physical output under different airflow disturbance confidence levels.

[0062] S520: The flight control module 300 acquires the basic motor speed command vector generated independently by the system's basic attitude closed-loop control branch; an independent data overlay channel is opened at the power distribution output node at the bottom layer of the flight control module 300, and the final feedforward injection vector is directly imported into this channel, and vector addition is performed with the basic motor speed command vector to generate a synthetic motor control command vector. ; In the formula, This is the synthetic motor control command vector; The basic motor speed command vector; This is the final feedforward injection vector; before output, the system performs bidirectional saturation truncation verification on each dimension of the synthesized motor control command vector. When the synthesized command value exceeds the maximum rated speed threshold supported by the ESC module 700, it is forcibly limited to the maximum value. In this embodiment, the maximum rated speed threshold ranges from 6000 RPM to 12000 RPM. When the synthesized command value is lower than the set minimum idle speed physical threshold of the motor, it is forcibly limited to the minimum value. In this embodiment, the minimum idle speed physical threshold ranges from 800 RPM to 1500 RPM. This hard-wired feedforward independent injection channel directly acts on the final actuator control hardware level.

[0063] S530: At the end of each control cycle, the communication bus 900 broadcasts a hardware-level synchronization trigger message carrying a global absolute clock timestamp to the bus network; after receiving the message, the microcontroller inside the ESC module 700 triggers the bottom-level pulse width modulation electrical signal of the synthesized motor control command vector to flip and output according to the timestamp. When the joint servo module 800 resolves the same global absolute clock timestamp, it drives the motor to change the stator coil current. Therefore, the timing of the action of the ESC module 700 driving the rotor motor to generate lift and the timing of the action of the joint servo module 800 driving the robotic arm link to rotate are set to be within the same microsecond time interval, eliminating the instruction arrival time deviation caused by the cascaded transmission of multiple communication nodes.

[0064] Application Examples: To better understand the technical solution of this invention, the following scenario uses a six-rotor aircraft frame with a four-degree-of-freedom lightweight robotic arm mounted on its bottom. Its main task is to fly to a high-altitude high-voltage power line tower, grab and replace damaged insulators as an application example for illustration: For the four physical states of the robotic arm—unloaded, suspended with 1.5kg, 3.0kg, and 5.0kg loads—the system separates nonlinear physical parameters through offline calculation and stores four sets of basic three-dimensional mapping tables in the main control module of the robotic arm.

[0065] The flight frame flies to the target position and hovers. The robotic arm extends its end effector to close and grab the insulator. The system enters a relatively static state. The force sensing module measures the static load at the end. After the system removes the end effector's self-weight parameter, it calculates that the equivalent physical mass of the insulator is 2.6 kg. The robotic arm's main control module determines that this mass value is between the preset reference mass levels of 1.5 kg and 3.0 kg. It extracts the corresponding lower boundary mapping table and upper boundary mapping table, performs linear interpolation, generates a working dynamic coupling mapping table and a working static pose mapping table adapted to the 2.6 kg load, and overwrites and stores them in the high-frequency buffer.

[0066] The robotic arm moves backward and upward according to the trajectory plan to remove the insulator. At the beginning of a single control cycle, the robotic arm's main control module analyzes the joint target angle for the next cycle and performs memory addressing and table lookup in the high-frequency buffer. It calculates the estimated reaction torque by combining the target angular velocity parameter and the target angular acceleration parameter. The estimated reaction torque is converted into a six-dimensional rotor speed compensation command through the pseudo-inverse of the generalized power distribution matrix and sent to the flight control module with an additional hardware absolute clock timestamp to be pushed into the historical circular buffer.

[0067] During the spatial displacement motion of the robotic arm, the system is subjected to external lateral wind disturbance input. The inertial measurement module collects the three-axis angular acceleration data of the fuselage, the flight control module extracts the actual resultant external torque at the current moment and subtracts the actual control torque, and simultaneously extracts the historical estimated comprehensive reaction torque from the historical circular buffer and compares it with the strictly aligned timestamps to obtain the pure external environmental disturbance residual vector. The system determines that the external environmental disturbance residual vector exceeds the internally preset safety residual lower limit threshold, triggers the adaptive threshold evaluation mechanism, and reduces the mixed gain coefficient from scalar 1.0 to scalar 0.4 according to the linear decay formula.

[0068] The flight control module multiplies the extracted original speed compensation command vector by an adaptive mixing gain coefficient of 0.4 to generate the final feedforward injection vector. At the bottom power node, it adds and merges the vector with the basic motor speed command vector generated by the attitude closed loop. The electronic speed controller (ESC) module receives the superimposed command and drives the six rotor motors to output rotor thrust. The joint servo module synchronously drives the robotic arm linkage movement. Both complete the consistent execution of hardware layer control commands under the trigger of the same microsecond-level absolute clock pulse.

[0069] The comparison benchmark settings include Method A, which uses traditional attitude feedback control without robotic arm motion feedforward compensation; Method B, which uses conventional robotic arm motion feedforward control without digital filter group delay timing alignment mechanism and adaptive mixed gain mechanism; and Method C, which uses the present invention's solution with timing alignment residual stripping mechanism and adaptive mixed gain collaborative control.

[0070] The displacement experiment in an environment without external wind disturbance was conducted indoors in a windless state. The system was in a hovering state, and the end of the robotic arm flange was equipped with a 3.0kg load to perform a full-stroke horizontal extension action.

[0071]

[0072] Under indoor windless parameter boundaries, Method A measured a maximum pitch angle deviation of 7.85 degrees and a maximum height drop deviation of 48.5 cm. Method B measured a maximum pitch angle deviation of 1.62 degrees and a maximum height drop deviation of 12.3 cm. Method C used a historical circular buffer to perform strict timing alignment to eliminate the delay misalignment of the underlying sensors, and measured a maximum pitch angle deviation of 1.24 degrees and a maximum height drop deviation of 9.1 cm.

[0073] The strong external wind disturbance environment experiment was set up in an outdoor wind tunnel test site with an average flow velocity of 6 m / s and a maximum flow velocity of 8 m / s lateral wind. The system controlled the robotic arm to carry a 3.0 kg load and perform a full-stroke flat extension action while in a wind-resistant hovering state.

[0074]

[0075] Under the operational boundary with wind deflection disturbance at a maximum flow velocity of 8 m / s, the maximum fuselage attitude deviation measured by method A was 12.45 degrees. Method B, by superimposing the ambient wind moment and the conventional feedforward moment, triggered the system saturation shutdown. Method C, based on the time-series alignment mechanism, extracted the residual vector of the ambient wind disturbance, triggered the adaptive evaluation mechanism to lower the mixing gain parameter, and limited the non-target dynamic high-frequency incremental injection into the system branch. The maximum fuselage attitude deviation measured by method C was 4.35 degrees.

[0076] See attached document Figure 6 The two-dimensional coordinate system of the figure has a time variable parameter in seconds on the horizontal axis and a fuselage pitch angle deviation variable parameter in degrees on the vertical axis. The figure shows the multi-damped oscillation curve corresponding to the test data of method A, the high-frequency noise jump curve corresponding to the test data of method B, and the single peak smooth response curve corresponding to the test data of method C, which converges smoothly at zero position without overshoot.

[0077] See attached document Figure 7 This includes an upper and lower chart with a shared horizontal time axis. The vertical axis of the lower chart is a value calibrated by the magnitude of the residual vector of external environmental disturbances, while the vertical axis of the upper chart is a value calibrated by the adaptive mixing gain coefficient. When the residual fluctuation curve in the lower chart crosses the lower limit threshold coordinate point, the corresponding gain coefficient value curve in the upper chart is drawn downward from the scalar value of 1.0. When the residual fluctuation curve approaches the upper limit threshold coordinate point of the dangerous residual, the corresponding gain coefficient value curve extends to the value of 0.2.

[0078] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A control system for a drone equipped with a robotic arm, characterized in that, It includes a flight frame, flight control module, robotic arm main control module, inertial measurement module, electronic speed controller module, communication bus, and joint servo module; The main control module of the robotic arm is used to calculate and estimate the comprehensive reaction torque and the original feedforward speed correction vector based on the target joint state and send them to the flight control module cache. The flight control module is used to back-calculate the actual resultant external torque based on the data from the inertial measurement module, combine the historically estimated reaction torque with time-aligned data to obtain the residual vector of external environmental disturbance, generate an adaptive mixing gain coefficient, and merge the original feedforward speed correction vector with the basic motor speed command vector to form a synthetic motor control command vector. The communication bus broadcasts a message carrying a global absolute clock timestamp, triggering the ESC module and the joint servo module to execute synchronously.

2. The control system for a drone equipped with a robotic arm according to claim 1, characterized in that, The target joint state includes the target joint angle vector, the target joint angular velocity vector, and the target joint angular acceleration vector. The robotic arm main control module converts the target joint angle vector into a low-level memory address offset, extracts the static centroid position vector, the equivalent gravity scalar parameter, and the augmented dynamic coupling matrix. It uses the target joint angular velocity vector to generate a quadratic velocity term vector, and performs matrix multiplication and algebraic operations with the target joint angular acceleration vector and the augmented dynamic coupling matrix. It combines the actual static reaction force vector to perform vector cross product and addition to restore the estimated comprehensive reaction torque and estimated comprehensive reaction force vector for the next clock synchronization cycle.

3. The control system for a drone equipped with a robotic arm according to claim 2, characterized in that, The robotic arm main control module constructs a comprehensive disturbance feedforward vector by transposing the estimated comprehensive reaction force vector and concatenating it with the transposed vector of the estimated comprehensive reaction torque. Based on the rotor spatial position coordinate system, rotor thrust coefficient, and rotor torque coefficient of the flight frame, a pseudo-inverse of the generalized power distribution matrix with horizontal underactuated dimension rank reduction processing is constructed, and matrix multiplication is performed on it with the comprehensive disturbance feedforward vector to generate the original feedforward speed correction vector acting on each independent rotor motor node.

4. The control system for a drone equipped with a robotic arm according to claim 1, characterized in that, The flight control module has a first-in-first-out (FIFO) historical circular buffer in its internal random access memory. The memory depth threshold of this buffer is configured as the ratio of the maximum estimated group delay time constant to the single motion control cycle time, rounded down and supplemented with a preset safety redundancy margin. The flight control module extracts the target execution absolute clock timestamp and binds it with the estimated comprehensive reaction torque, packing it into the main stack area of ​​the historical circular buffer.

5. A control system for a drone equipped with a robotic arm according to claim 4, characterized in that, The flight control module reads the three-axis angular velocity data from the inertial measurement module in real time, performs discrete-time difference analysis, and obtains the three-axis fuselage angular acceleration vector after first-order low-pass filtering. The three-axis fuselage angular acceleration vector is then multiplied with the pre-fixed fuselage inertia tensor matrix, and the result is vector-superimposed with the cross product of the fuselage angular velocity vector and the fuselage angular momentum vector caused by the undifferentiated original three-axis fuselage angular velocity data to accurately calculate the actual net external torque currently borne by the flight frame.

6. A control system for a drone equipped with a robotic arm according to claim 5, characterized in that, The flight control module obtains the current system absolute clock timestamp and subtracts the fixed group delay time constant calibrated by the frequency response characteristics of the digital filter, calculates the target matching timestamp, and extracts the historical estimated comprehensive reaction torque with the smallest absolute error to the target matching timestamp from the queue of the historical circular buffer. The actual resultant external torque is then vector-subtracted from the historical estimated comprehensive reaction torque and the actual control torque obtained by forward mapping to solve for the pure external environment disturbance residual vector.

7. A control system for a drone equipped with a robotic arm according to claim 6, characterized in that, The flight control module calculates the vector magnitude of the pure external environment disturbance residual vector. When the vector magnitude is less than the safety residual lower limit threshold, the adaptive mixing gain coefficient is configured as a full gain scalar value. When the vector magnitude is between the lower limit threshold of the safe residual and the upper limit threshold of the dangerous residual, the adaptive mixing gain coefficient is linearly decayed according to a preset linear formula; when the vector magnitude is greater than the upper limit threshold of the dangerous residual, the adaptive mixing gain coefficient is forcibly configured to zero.

8. A control system for a drone equipped with a robotic arm according to claim 7, characterized in that, The flight control module performs algebraic multiplication of scalar and vector to scale the adaptive hybrid gain coefficient and the original feedforward speed correction vector proportionally to generate the final feedforward injection vector. At the underlying power distribution output node, an independent data superposition channel is opened to perform vector addition and merging calculation on the final feedforward injection vector and the basic motor speed command vector generated independently by the system's basic attitude closed-loop control branch to generate the synthetic motor control command vector.

9. A control system for a drone equipped with a robotic arm according to claim 8, characterized in that, Before outputting, the flight control module performs bidirectional saturation truncation verification on each dimension of the synthetic motor control command vector. When the synthetic command value exceeds the maximum supported rated speed threshold, it is forcibly limited to the maximum value. When the synthetic command value is lower than the set minimum idle speed physical threshold of the motor, it is forcibly limited to the minimum value. The feedforward independent injection channel is used to directly act on the final actuator control hardware level.

10. A control system for a drone equipped with a robotic arm according to claim 1, characterized in that, The communication bus broadcasts the message carrying the global absolute clock timestamp to the bus network at the end of each control cycle. The ESC module triggers the bottom-level pulse width modulation electrical signal of the synthesized motor control command vector to flip and output based on the global absolute clock timestamp; When the joint servo module resolves to the same global absolute clock timestamp, it synchronously drives the motor to change the stator coil current, thereby eliminating the instruction arrival time deviation caused by the cascaded transmission of multiple communication nodes.

Images