An air docking method and device based on a contact force-position neural network model

Abstract

The application provides an aerial docking method and device based on a contact force-position neural network model. The method provided by the application comprises the following steps: building a ground data acquisition device, collecting training data of contact force and corresponding position parameters in three dimensions of horizontal direction, vertical direction and relative angle through the ground data acquisition device; constructing a contact force-position neural network model, training the contact force-position neural network model by using the training data; acquiring relative position and attitude information between a work unmanned aerial vehicle and a target, and predicting contact force based on the contact force-position neural network model; combining the predicted contact force, position error of the work unmanned aerial vehicle and the target, and preset expected docking force to generate a preliminary reference trajectory; introducing a control allocation method to optimize and adjust the preliminary reference trajectory, and performing aerial docking between the work unmanned aerial vehicle and the target based on the optimized trajectory.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of air docking, and in particular to an air docking method and device based on a contact force-position neural network model. BACKGROUND

[0002] With the wide application of unmanned aerial vehicle technology in the fields of logistics transportation, power inspection, emergency rescue, etc., the operation radius, endurance and functional modules of a single unmanned aerial vehicle have been difficult to meet the needs of complex scenarios. Therefore, air docking technology has become a key support for breaking through the capacity bottleneck of a single unmanned aerial vehicle and expanding the application boundary of unmanned aerial vehicles, and its stability and safety directly determine the efficiency and feasibility of multi-unmanned aerial vehicle cooperative operation.

[0003] At present, the mainstream air docking method mainly relies on the technical path of "visual positioning + position control": the position and attitude information of the target is obtained through the visual sensor (such as binocular camera, laser radar) carried by the unmanned aerial vehicle, combined with the preset position control algorithm (such as PID control), the unmanned aerial vehicle flight platform or mechanical arm motion is driven, so that the execution end gradually approaches and contacts the target, and finally the docking is completed. Some solutions will add a buffer structure (such as an elastic pad) at the docking end, trying to reduce the impact force during docking through mechanical buffering, but the core control logic still takes "position accuracy" as the core target, and does not include contact force in the active control category. The existing air docking method has the following problems: on the one hand, visual positioning is easily disturbed by the environment (such as strong light, fog and haze leading to increased positioning error), and only relying on position control is easy to cause rigid collision between the docking execution end and the target, resulting in excessive contact force; on the other hand, even if the visual positioning is accurate, since the "position-contact force" correlation model is not established, the contact force during docking cannot be predicted in advance, and it is difficult for the passive buffer structure to effectively absorb the impact force, thereby causing the attitude of the operating unmanned aerial vehicle to change dramatically, the docking to fail, and even damaging the unmanned aerial vehicle or the target device.

[0004] Therefore, there is an urgent need for a method to actively predict and control the contact force, realize low-impact force stable docking, and improve the safety and success rate of air docking. SUMMARY

[0005] Therefore, the present application provides an air docking method and device based on a contact force-position neural network model, which actively predicts and controls the contact force to realize low-impact force stable docking and improve the safety and success rate of air docking.

[0006] Specifically, the present application is realized by the following technical solutions:

[0007] The first aspect of the present application provides an air docking method based on a contact force-position neural network model, the method comprising:

[0008] A ground data acquisition device is built, and the ground data acquisition device is used to acquire training data of contact force and corresponding position parameters in three dimensions of horizontal direction, vertical direction and relative angle;

[0009] A contact force-position neural network model is constructed, and the contact force-position neural network model is trained by using the training data;

[0010] Relative position and attitude information between the work unmanned aerial vehicle and the target is acquired, and contact force is predicted based on the contact force-position neural network model;

[0011] The predicted contact force, position error of the work unmanned aerial vehicle and the target, and a preset expected docking force are combined to generate a preliminary reference trajectory; the preliminary reference trajectory includes a flight platform reference trajectory and a mechanical arm end reference trajectory;

[0012] A control allocation method is introduced to optimize and adjust the preliminary reference trajectory, and the work unmanned aerial vehicle and the target are air docked based on the optimized trajectory.

[0013] The second aspect of the application provides an air docking device based on a contact force-position neural network model, the device comprising an acquisition module, a training module, a prediction module, a generation module and a docking module;

[0014] The acquisition module is configured to build a ground data acquisition device, and the ground data acquisition device is used to acquire training data of contact force and corresponding position parameters in three dimensions of horizontal direction, vertical direction and relative angle;

[0015] The training module is configured to construct a contact force-position neural network model, and the contact force-position neural network model is trained by using the training data;

[0016] The prediction module is configured to acquire relative position and attitude information between the work unmanned aerial vehicle and the target, and predict contact force based on the contact force-position neural network model;

[0017] The generation module is configured to combine the predicted contact force, position error of the work unmanned aerial vehicle and the target, and a preset expected docking force to generate a preliminary reference trajectory; the preliminary reference trajectory includes a flight platform reference trajectory and a mechanical arm end reference trajectory;

[0018] The docking module is configured to introduce a control allocation method to optimize and adjust the preliminary reference trajectory, and the work unmanned aerial vehicle and the target are air docked based on the optimized trajectory.

[0019] The aerial docking method and device based on the contact force-position neural network model provided in the application, through the ground data acquisition device, training data of contact force and position parameters in three dimensions of horizontal, vertical and relative angle are collected, basic data conforming to the actual docking scene is provided for subsequent model training, and it is ensured that the model can learn the real force-position mapping relationship; on this basis, the contact force-position neural network model is constructed and trained, the model has the ability to accurately predict the contact force according to the position parameter, and the problem that the contact force is difficult to directly measure in aerial docking is solved, and key force feedback basis is provided for trajectory planning; through obtaining relative position and attitude information and using the model to predict the contact force, the conversion from position information to contact force is realized, so that the trajectory planning not only considers the position accuracy, but also can adapt to the contact force demand in advance; in combination with the predicted contact force, the position error and the expected docking force, the preliminary reference trajectory including the flight platform and the mechanical arm end is generated, the preliminary balance of the trajectory in the aspects of position adjustment and force control is ensured, and the foundation for subsequent optimization is laid; finally, the control allocation method is introduced to optimize the trajectory, so that the trajectory meets the hardware constraints and realizes the cooperation of multiple systems, and finally the docking is executed based on the optimized trajectory, the whole process from data acquisition to model training, then to force prediction, trajectory generation and optimization forms a complete closed loop, which not only ensures the position accuracy of the docking, but also avoids impact damage through force control, and coordinates the movement of the flight platform and the mechanical arm, effectively improves the success rate, safety and stability of the aerial docking. BRIEF DESCRIPTION OF DRAWINGS

[0020] Figure 1 The flowchart of the aerial docking method based on the contact force-position neural network model provided for the first embodiment of the application;

[0021] Figure 2 The structure schematic diagram of the aerial docking device based on the contact force-position neural network model provided for the second embodiment of the application. DETAILED DESCRIPTION

[0022] Hereinafter, exemplary embodiments will be described in detail with reference to the accompanying drawings. In the following description, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments are not representative of all embodiments consistent with the present application.

[0023] The terms used in the present application are merely for the purpose of describing specific embodiments and are not intended to limit the present application. As used in the present application, the singular forms "a," "an," and "the" are intended to include the plural forms, unless the context clearly indicates otherwise. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0024] It should be understood that, although the terms first, second, third, etc. can be employed in this application to describe various information, the information should not be limited to these terms. These terms are only used to differentiate one piece of information from another piece of information. For example, without departing from the scope of the present application, the first information can also be referred to as the second information, and similarly, the second information can also be referred to as the first information. Depending on the context, the word "if" as used herein can be interpreted as "when" or "upon" or "in response to determining".

[0025] Specific embodiments are given below to detail the technical solutions of the present application.

[0026] Figure 1 A flowchart of an air docking method based on a contact force-position neural network model is provided for Embodiment One of the present application. Please refer to Figure 1 The method provided in the present embodiment can include:

[0027] S101, a ground data acquisition device is built, and training data of contact force and corresponding position parameters is acquired in three dimensions of horizontal direction, vertical direction and relative angle through the ground data acquisition device.

[0028] Specifically, the ground data acquisition device is a special experimental equipment for providing training samples for the contact force-position neural network model, which is composed of hardware components and a control module. The hardware includes: a force sensor (for acquiring contact force data), a signal conditioner (processing the original signal of the force sensor), a motion capture system (accurately recording position and attitude parameters), a three-dimensional moving platform (moving the object in multiple dimensions), and a control device (such as a computer or a controller, used to drive the platform movement and record data synchronously). The core function is to simulate the relative movement between the working unmanned aerial vehicle and the target in the air docking scene, and to obtain the corresponding relationship data of "position parameters-contact force" in a controllable environment. The contact force refers to the interaction force between the docking execution end (such as the end of the mechanical arm) of the working unmanned aerial vehicle and the target in the contact moment or contact process, including the horizontal, vertical and rotational components. The size and direction of the force directly affect the docking stability: too small may cause the docking to be unstable (such as failure to grasp), and too large may cause attitude disturbance and structural damage of the unmanned aerial vehicle or the target, which is the core control index of air docking safety.

[0029] The three dimensions refer to the key directions and attitudes of the relative motion between the operating unmanned aerial vehicle and the target during the simulation of the aerial docking. The horizontal direction refers to the relative translation distance (for example, the distance deviation in the left-right and front-back directions) of the two in the same horizontal plane (for example, the x-y plane of the unmanned aerial vehicle body coordinate system); the vertical direction refers to the relative height difference (for example, the distance deviation in the up-down direction) of the two in the vertical direction (for example, the z-axis of the unmanned aerial vehicle body coordinate system); and the relative angle refers to the rotation angle deviation (for example, the rotation angle deviation around the x, y and z axes) of the two in the attitude, reflecting the attitude alignment degree during the docking. Since the three dimensions directly determine the contact state and contact force during the aerial docking, they are the core factors affecting the docking safety: the distance deviation in the horizontal and vertical directions determines the spatial position relationship between the docking execution end (for example, the end of the mechanical arm) and the target, and too close or too far distance will cause too small (unable to dock) or too large (collision risk) contact force; the relative angle deviation determines the contact attitude during the docking, and too large angle deviation will cause “oblique collision”, generating unexpected impact force and damaging the attitude stability of the unmanned aerial vehicle. By collecting the data of the three dimensions, the key variables of “position + attitude” during the docking process can be comprehensively covered, and the neural network model can learn the complete “position-contact force” mapping relationship, providing a reliable basis for the contact force prediction during the actual docking.

[0030] In a specific implementation, a hardware component of a ground data collection device is configured; the hardware component includes a force sensor, a signal conditioner, a motion capture system, a three-dimensional movement platform, and a control device, the force sensor is used to collect contact force data, the signal conditioner is used to process the signal output by the force sensor, the motion capture system is used to collect position parameters, the three-dimensional movement platform is used to move the object in multiple dimensions, and the control device is used to control the movement of the three-dimensional movement platform; the ground data collection device is initialized, the force sensor is zeroed, and the relative positioning error of the two objects is calibrated by the motion capture system; the control device drives the three-dimensional movement platform to move the test object in the horizontal direction and the vertical direction at a set interval, and adjusts the attitude in the relative angle dimension at a set angle interval; during the movement of the test object, the force sensor collects contact force data in real time under different positions and attitudes, and the motion capture system synchronously collects corresponding position parameters; the collected contact force data and corresponding position parameters are aligned and recorded to form a training data set.

[0031] Specifically, a force sensor with appropriate range and accuracy is selected and installed at the docking contact part of the test object; the output end of the force sensor is connected with a signal conditioner, which amplifies, filters and linearizes the weak electrical signal output by the sensor, and converts it into a standard signal that can be recognized by subsequent devices; an action capture system is deployed in the experimental area, including multiple infrared cameras or optical sensors; a three-dimensional mobile platform is built, which has translational degrees of freedom in the horizontal (x, y axes) and vertical (z axis) directions and rotational degrees of freedom around each axis, and the test object is fixed at the execution end of the platform; a control device (such as an industrial computer or a PLC controller) is connected with the three-dimensional mobile platform through a data line, and the corresponding control software is installed. Further, the force sensor and the signal conditioner are started, and in the state of no external force, the output value of the force sensor is calibrated to zero through the zero setting function of the signal conditioner or the matching software, eliminating the zero drift error; the action capture system is started, the target object is fixed at the reference position in the experimental area, the three-dimensional mobile platform is controlled to drive the test object to move to the preset calibration point, the theoretical coordinates and actual detection coordinates of the two objects at this point are recorded by the action capture system, the positioning error compensation value is calculated and stored, and the calibration of the relative positioning error is completed. The motion parameters are set in the software of the control device: in the horizontal direction, move from the initial position to the target object at intervals of 0.5 cm, in the vertical direction, adjust the height at intervals of 0.5 cm, and in the relative angle, rotate around the central axis at intervals of 2°; the control device sends control instructions to the three-dimensional mobile platform according to the set parameters, drives the platform to move according to the preset trajectory, drives the test object to gradually approach or move away from the target object in the horizontal and vertical directions, and adjusts the posture of the test object at the set angle interval during the movement, simulating the relative motion state under different docking scenarios. During the movement of the test object, when the test object contacts the target object, the force sensor detects the size and direction of the contact force in real time, and transmits the force signal to the control device after processing by the signal conditioner; the action capture system synchronously tracks the positions of the test object and the target object, and collects the x, y coordinate difference in the horizontal direction, the z coordinate difference in the vertical direction, and the relative angle value of the test object in real time, and transmits the position parameters to the control device at a frequency of 10 ms / time; the control device matches the time stamps to ensure that the contact force data and position parameters collected at the same time correspond one by one, and arranges all the collected contact force data (including the horizontal, vertical and rotational components) and the corresponding position parameters (horizontal distance, vertical distance and relative angle) in chronological order to form a training data set for training the contact force-position neural network model.

[0032] S102, a contact force-position neural network model is constructed, and the contact force-position neural network model is trained using the training data.

[0033] The contact force-position neural network model is a three-layer fully connected neural network, including an input layer, two hidden layers and an output layer, the input layer is used to receive the normalized horizontal distance, vertical distance and relative angle parameters, each hidden layer contains a preset number of neurons and adopts an activation function to process data, and the output layer is used to output the contact force.

[0034] Specifically, the contact force-position neural network model is a prediction model based on a fully connected neural network, and its core function is to learn the mapping relationship between the relative position and attitude parameters of the work unmanned aerial vehicle and the target and the contact force generated when the two contact each other, so as to realize accurate prediction of the contact force in the air docking process. The model takes the horizontal distance, vertical distance and relative angle of the work unmanned aerial vehicle and the target as input, and outputs the contact force prediction value in the corresponding three directions (horizontal, vertical and rotational directions).

[0035] Further, the contact force-position neural network model adopts a three-layer fully connected neural network architecture, including an input layer, two hidden layers and an output layer, and the structures and functions of each layer are as follows: the input layer contains 3 neurons, which respectively receive the normalized “horizontal distance, vertical distance and relative angle” three position parameters, convert the original input parameters into a feature vector that can be processed by the neural network, and serve as the initial input signal of the model. The first hidden layer contains a preset number of neurons (such as 120), and adopts a ReLU activation function for non-linear mapping; after the first hidden layer receives the feature vector transmitted by the input layer, it processes the original position parameters through weight matrix operation and activation function, generates intermediate features reflecting the basic correlation between the position parameters and the contact force, and transmits them to the second hidden layer. The second hidden layer also contains a preset number of neurons (such as 120), and adopts a ReLU activation function; the second hidden layer receives the intermediate features output by the first hidden layer, extracts more abstract and comprehensive high-order features (such as “comprehensive influence law of three-parameter cooperation on contact force”) through further weight operation and non-linear conversion, forms comprehensive features that can directly support the final prediction, and transmits them to the output layer. The output layer contains 3 neurons without activation function (linear output is adopted); after the output layer receives the comprehensive features of the second hidden layer, it directly outputs the contact force prediction values in three directions (horizontal direction force, vertical direction force and rotational direction force) through linear superposition operation (weight + bias).

[0036] In a specific implementation, the training of the contact force-position neural network model using the training data comprises configuring contact force-position neural network model training parameters; inputting the training data set into the constructed contact force-position neural network model in batches according to a preset batch size, and updating the model parameters based on the error calculated by the loss function through an optimizer; introducing an early stopping mechanism in the training process, and stopping the training when the performance of the contact force-position neural network model on the validation set tends to be stable, to obtain the trained contact force-position neural network model.

[0037] Specifically, the training parameters are configured as follows: the related parameters of model training are set, including the batch size (such as 32 groups of data per batch), the learning rate (such as an initial value of 0.001), the total number of training rounds (such as a maximum of 500 rounds), the loss function (such as mean squared error (MSE) for calculating the deviation of the predicted contact force from the actual contact force) and the optimizer (such as Adam optimizer for updating the model parameters), and the validation set is divided from the training data set (such as dividing the training set and the validation set in a ratio of 7:3). The training data set is divided into batches according to a preset batch size, and each time a batch of training data (including horizontal distance, vertical distance, relative angle parameters and corresponding actual contact force labels) is input into the model. The model performs forward calculation on the input data and outputs the predicted contact force. The error between the predicted value and the actual label is calculated through the loss function, and the optimizer updates the weight and bias parameters of each layer of the model based on the error through the back propagation algorithm. The process is repeated until all batch data in the current round are trained. After each round of training is completed, the performance of the model is evaluated using the validation set data (calculating the loss value on the validation set). The trend of the validation set loss value is recorded, and if the validation set loss value does not decrease and tends to be stable for a continuous preset number of rounds (such as 10 rounds), the early stopping mechanism is triggered and the model training is stopped. The model parameters at this time are saved, and the trained contact force-position neural network model is obtained.

[0038] S103, obtaining the relative position and attitude information between the task unmanned aerial vehicle and the target, and predicting the contact force based on the contact force-position neural network model.

[0039] Specifically, relative position refers to the relative difference in spatial location between the operational drone and the target (such as equipment or materials to be docked) in three-dimensional space, with a reference object (usually the target as a reference point, or a unified coordinate system) as the benchmark. It primarily reflects the positional relationship between "where the drone is" and "where the target is," and is quantified using horizontal and vertical distance parameters. Attitude information refers to the drone's own attitude angle state in three-dimensional space, that is, the drone's tilt and rotation attitude relative to the target (or reference coordinate system). It primarily reflects "what posture the drone is facing the target," and is quantified using relative angle parameters. In other words, relative position and attitude information are macroscopic descriptions of the spatial relationship between the drone and the target, while horizontal distance, vertical distance, and relative angle are specific quantitative parameters obtained by breaking down this macroscopic description and can be directly input into the model for calculation.

[0040] In specific implementation, obtaining the relative position and attitude information between the UAV and the target includes: obtaining the position coordinates of the UAV's robotic arm end effector in the UAV's body coordinate system and the target's position coordinates in the UAV's body coordinate system; calculating the relative horizontal distance based on the horizontal coordinates of the robotic arm end effector and the target in the UAV's body coordinate system using the horizontal distance formula between the two points; obtaining the relative vertical distance based on the vertical coordinates of the robotic arm end effector and the target in the UAV's body coordinate system by calculating the absolute difference between the two coordinate values; obtaining the attitude quaternion of the target in the UAV's body coordinate system and the attitude quaternion of the robotic arm end effector in the UAV's body coordinate system; and converting the attitude information into angle information using the quaternion attitude conversion formula based on the attitude quaternions of the target and the robotic arm end effector, and calculating the relative angle.

[0041] Specifically, the position coordinates of the robotic arm's end effector in the drone's coordinate system are collected using visual sensors (such as binocular cameras or LiDAR) or positioning modules mounted on the drone (denoted as ( )). , , Simultaneously, the target's position coordinates in the body coordinate system are collected (denoted as ( , , The two sets of coordinate data are then transmitted to the UAV's control unit. The horizontal coordinates (from the robotic arm's end effector and the target's position coordinates) are extracted. , ,Target , Substitute the values ​​into the formula for the horizontal distance between two points (the formula is: relative horizontal distance). ), the relative horizontal distance between the two is calculated by the control unit. The vertical direction coordinate (the position coordinate of the end of the mechanical arm , the target ) is extracted from the position coordinates of the end of the mechanical arm and the target, and the absolute value difference between the two coordinate values (formula: relative vertical distance ) is calculated by the control unit to obtain the relative vertical distance between the two. The attitude quaternion (denoted as =(w1, , , ) of the end of the mechanical arm in the body coordinate system of the working UAV is collected by the attitude sensor (such as an IMU inertial measurement unit) carried by the UAV, and the attitude quaternion (denoted as =(w2, , , ) of the target in the body coordinate system is collected, and the two sets of quaternion data are transmitted to the control unit. In the control unit, the quaternion attitude conversion formula is called to convert the attitude quaternions (w1, , ) of the target and the end of the mechanical arm into corresponding Euler angles (such as roll angle, pitch angle, and yaw angle), and then calculate the difference between the corresponding angles of the two sets of Euler angles, which is the relative angle between the end of the mechanical arm and the target (formula: relative angle ).

[0042] Optionally, the method for predicting the contact force based on the contact force-position neural network model comprises: performing normalization processing on the relative position and attitude information as input data of the contact force-position neural network model; transmitting the normalized input data to the input layer of the contact force-position neural network model, and transmitting the input data from the input layer to the first hidden layer; the first hidden layer receives the input data transmitted from the input layer, extracts and converts the input data through neurons, and performs nonlinear mapping on the extracted intermediate features through an activation function, and transmits the mapping result to the second hidden layer; the second hidden layer receives the mapping result transmitted from the first hidden layer, extracts and converts the comprehensive features through neurons, and performs nonlinear mapping through an activation function, and transmits the mapping result to the output layer; the output layer receives the mapping result transmitted from the second hidden layer, generates and outputs the corresponding contact force prediction value through linear superposition processing.

[0043] In a specific implementation, three parameters, i.e., relative horizontal distance, relative vertical distance, and relative angle, are extracted from the obtained relative position and attitude information, and the Min-Max normalization method (mapping the parameter value to the interval [0, 1], the formula is: normalized value = (original value - parameter minimum value) / (parameter maximum value - parameter minimum value)) is used to normalize the three parameters respectively to obtain input data meeting the input requirements of the model. The normalized input data (including normalized horizontal distance, vertical distance, and relative angle) are transmitted to the input layer of the contact force-position neural network model, and the three neurons of the input layer receive the corresponding parameter data and transmit the input data directly to the first hidden layer. After receiving the input data transmitted by the input layer, the first hidden layer extracts and converts the data through a preset number of neurons (such as 120) in the layer (the neurons perform multiplication operation on the input data through a weight matrix and add a bias term); a ReLU activation function is called to perform nonlinear mapping on the extracted intermediate features, and the mapped feature data are transmitted to the second hidden layer. After receiving the mapping results transmitted by the first hidden layer, the second hidden layer further extracts comprehensive features and completes conversion through a preset number of neurons (such as 120) in the layer (also through weight matrix operation and bias term addition); a ReLU activation function is called again to perform nonlinear mapping on the comprehensive features, and the mapped final feature data are transmitted to the output layer. The output layer receives the mapping results transmitted by the second hidden layer, performs linear superposition processing on the final feature data through three neurons in the layer (without using an activation function, directly calculating the product of the weight and the feature data and adding a bias term), and generates and outputs the contact force prediction values corresponding to the horizontal direction, the vertical direction, and the rotation direction, respectively.

[0044] In S104, a preliminary reference trajectory is generated in combination with the predicted contact force, the position error of the work unmanned aerial vehicle and the target, and the preset expected docking force.

[0045] The preliminary reference trajectory includes a flight platform reference trajectory and a mechanical arm end reference trajectory.

[0046] Specifically, the preliminary reference trajectory refers to an initial motion path scheme for guiding the subsequent action of the unmanned aerial vehicle, which is generated by a trajectory planning algorithm in combination with three types of key information, i.e., the contact force predicted by the contact force-position neural network model, the actual existing position error of the unmanned aerial vehicle and the target, and the preset expected docking force, and is used to guide the subsequent action of the unmanned aerial vehicle when the unmanned aerial vehicle performs a docking task (such as grabbing, assembling, etc.) with the target. It is the core action basis for the unmanned aerial vehicle to transition from the current state to the target docking state.

[0047] Further, the preliminary reference trajectory includes a flight platform reference trajectory and a mechanical arm end reference trajectory. The flight platform reference trajectory is a motion path scheme generated for the main body of the unmanned aerial vehicle, i.e., the flight platform, to adjust the spatial position and attitude of the unmanned aerial vehicle body to eliminate the macroscopic position error between the unmanned aerial vehicle as a whole and the target, and to create conditions for subsequent accurate docking of the mechanical arm. For example, if the unmanned aerial vehicle body currently has a position deviation of 3 meters in the horizontal direction and 1 meter in the vertical direction from the target, and the body attitude has a 5° yaw angle error from the target, the flight platform reference trajectory will specify the specific motion path (including motion speed, acceleration, coordinates and attitude parameters at each time point, etc.) that the body needs to move 3 meters in the horizontal direction, 1 meter in the vertical direction, and adjust the yaw angle by 5°. The mechanical arm end reference trajectory is a motion path scheme generated for the end effector (such as a gripper, a suction cup, etc.) of the mechanical arm carried by the working unmanned aerial vehicle, which is used to further finely adjust the position and attitude of the mechanical arm end after the flight platform is adjusted to the approximate docking range, so as to ensure that the end effector can achieve accurate contact and docking with the target at a preset expected docking force. For example, after the flight platform completes the macroscopic position adjustment, if the mechanical arm end still has a horizontal deviation of 5 cm and a vertical deviation of 2 cm from the target, and the end attitude has a 2° pitch angle error from the target, the mechanical arm end reference trajectory will specify how each joint of the mechanical arm needs to rotate to drive the end effector to move 5 cm (horizontally) and 2 cm (vertically) along a specific path, and correct the 2° pitch angle, so that the end finally contacts the target at the expected docking force (such as 5N pressure).

[0048] In a specific implementation, the preliminary reference trajectory is generated in combination with the predicted contact force, the position error between the working unmanned aerial vehicle and the target, and the preset expected docking force, including:

[0049] (1) determining an expected position of the flight platform of the working unmanned aerial vehicle; the expected position is calculated based on the position of the target in the body coordinate system of the working unmanned aerial vehicle, the working space parameters of the mechanical arm, and the normal vector of the target.

[0050] Specifically, the expected position of the flight platform of the working unmanned aerial vehicle refers to the optimal spatial position of the flight platform (main body) when the working unmanned aerial vehicle performs the docking task with the target, i.e., the target position that the flight platform should reach, so as to support the subsequent accurate docking of the mechanical arm.

[0051] In a specific implementation, the position coordinates of the target in the body coordinate system of the work unmanned aerial vehicle are collected by a sensor; preset mechanical arm workspace parameters, including the maximum extension length, the minimum contraction length and the range of motion of each joint of the mechanical arm, are called; and the normal vector of the surface of the target is obtained through visual recognition or sensor detection. Based on the mechanical arm workspace parameters, the offset of the end of the mechanical arm from the flight platform in the workspace is determined. The offset is directionally corrected according to the normal vector of the target, and the corrected offset is consistent with the direction of the normal vector of the target. The position coordinates of the target and the corrected offset are subjected to vector operation, and the expected position coordinates of the flight platform of the work unmanned aerial vehicle are calculated.

[0052] For example, in an embodiment, the expected position of the flight platform of the work unmanned aerial vehicle can be expressed as:

[0053] ;

[0054] Wherein, the expected position of the flight platform of the work unmanned aerial vehicle is P; the position of the target in the body coordinate system of the work unmanned aerial vehicle is Ptarget; the distance from the center point of the workspace of the mechanical arm to the plane of the body coordinate system of the flight platform is D; and the normal vector of the target is N.

[0055] (2) Calculate the position error of the flight platform of the work unmanned aerial vehicle, generate a flight platform reference trajectory in combination with the expected position and a preset first control parameter, and obtain the flight platform reference trajectory by subtracting the product of the position error and the first control parameter from the expected position.

[0056] Specifically, the position error refers to the difference between the actual position of the flight platform of the work unmanned aerial vehicle and the determined expected position of the flight platform, and reflects the degree of deviation between the current position of the flight platform and the optimal position of the target.

[0057] In a specific implementation, the actual position coordinates of the flight platform of the work unmanned aerial vehicle are obtained by a sensor, the determined expected position coordinates of the flight platform are called, the difference between the two in each axis of the body coordinate system is calculated, and the position error of the flight platform is obtained. The position error is multiplied by a preset first control parameter to obtain the correction amount of the flight platform trajectory. The flight platform reference trajectory of the flight platform moving from the current position to the expected position is calculated by subtracting the correction amount of the flight platform trajectory from the expected position coordinates.

[0058] For example, in an embodiment, the position error of the flight platform of the work unmanned aerial vehicle can be expressed as:

[0059] ;​​​​​

[0060] wherein the is a position error of the work unmanned aerial vehicle flight platform; the is a current actual position of the work unmanned aerial vehicle flight platform; the is a desired position of the work unmanned aerial vehicle flight platform.

[0061] The flight platform reference trajectory can be expressed as:

[0062] ;

[0063] wherein the is a flight platform reference trajectory; the is a desired position of the work unmanned aerial vehicle flight platform; the is a position error of the work unmanned aerial vehicle flight platform; the is a first control parameter greater than 0.

[0064] (3) determining a position error of the work unmanned aerial vehicle mechanical arm end and the target in the body coordinate system.

[0065] Specifically, the position error of the work unmanned aerial vehicle mechanical arm end and the target in the body coordinate system refers to the difference between the current actual position of the end effector (such as a gripper, a suction cup, etc.) of the mechanical arm carried by the work unmanned aerial vehicle in the body coordinate system of the work unmanned aerial vehicle and the target position of the target in the same body coordinate system, which quantifies the spatial deviation degree between the current position of the mechanical arm end and the target position.

[0066] In specific implementation, the current actual position coordinates of the mechanical arm end in the body coordinate system of the work unmanned aerial vehicle are collected through a motion capture system or a sensor, and the position coordinates of the target in the body coordinate system are called. In the body coordinate system, the coordinate difference values of the mechanical arm end and the target in the X-axis, Y-axis and Z-axis directions are calculated respectively to obtain the position error in the form of a three-dimensional vector, i.e. the position error of the mechanical arm end and the target in the body coordinate system.

[0067] For example, in an embodiment, the position error of the work unmanned aerial vehicle mechanical arm end and the target in the body coordinate system can be expressed as:

[0068] ;

[0069] wherein the is a position error of the work unmanned aerial vehicle mechanical arm end and the target in the body coordinate system; the is a position of the work unmanned aerial vehicle mechanical arm end in the body coordinate system; the is a position of the target in the body coordinate system.

[0070] (4) generating a reference trajectory of the end of the mechanical arm based on the position of the target in the body coordinate system of the work unmanned aerial vehicle, the position error of the end of the mechanical arm and the target, the reference trajectory of the flight platform, the deviation of the predicted contact force from the preset expected docking force, and the preset expected docking force.

[0071] Specifically, the position of the target in the body coordinate system of the work unmanned aerial vehicle is taken as a reference point; the position error of the end of the mechanical arm and the target is calculated, the position error is multiplied by a preset second control parameter to obtain a position error correction term; based on the reference trajectory of the flight platform and the rotation matrix conversion relationship between the body coordinate system of the work unmanned aerial vehicle and the world coordinate system, a flight platform motion coordination term is generated, the flight platform motion coordination term is multiplied by a preset third control parameter to obtain a platform coordination correction term; the deviation of the predicted contact force from the preset expected docking force is calculated, the deviation is multiplied by a preset fourth control parameter to obtain a contact force deviation correction term; the reference trajectory of the end of the mechanical arm is obtained by sequentially subtracting the position error correction term, the platform coordination correction term and the contact force deviation correction term from the reference point.

[0072] Specifically, the position of the target in the body coordinate system of the work unmanned aerial vehicle is taken as a reference point. The determined position error of the end of the mechanical arm and the target is called, and the preset second control parameter is called. The position error is multiplied by the second control parameter to obtain a position error correction term. Based on the reference trajectory of the flight platform, the rotation matrix conversion relationship between the body coordinate system of the work unmanned aerial vehicle and the world coordinate system is combined to obtain a flight platform motion coordination term by multiplication; the preset third control parameter is called, and the flight platform motion coordination term is multiplied by the third control parameter to obtain a platform coordination correction term. The deviation of the predicted contact force from the preset expected docking force is calculated, the preset fourth control parameter is called, and the deviation is multiplied by the fourth control parameter to obtain a contact force deviation correction term. Based on the reference point, the position error correction term, the platform coordination correction term and the contact force deviation correction term are sequentially subtracted to obtain the reference trajectory of the end of the mechanical arm.

[0073] For example, in an embodiment, the reference trajectory of the end of the mechanical arm can be expressed as:

[0074] ;

[0075] Wherein, the reference trajectory of the end of the mechanical arm is represented as x; the position of the target in the body coordinate system of the work unmanned aerial vehicle is represented as x0; the second control parameter greater than 0 is represented as k1; the position error of the end of the mechanical arm and the target is represented as e; the third control parameter greater than 0 is represented as k2; the rotation matrix of the body coordinate system of the work unmanned aerial vehicle and the world coordinate system is represented as R; and the reference trajectory of the flight platform is represented as xref. ​​​​​​is a fourth control parameter greater than 0; the fourth control parameter is used to quantify the deviation between the predicted contact force and the preset expected docking force, and convert it into a trajectory correction amount; the trajectory correction amount is subtracted from the reference point to further match the docking force demand on the basis of the position adjustment, so as to avoid physical damage to the unmanned aerial platform or the target caused by excessive contact force, damage to the docking attitude stability, and docking failure or loosening after docking caused by insufficient contact force; the trajectory correction amount is used to compensate for the defects of the pure position control that cannot cope with the dynamic force changes in the aerial docking, so as to adapt the trajectory of the end of the mechanical arm to the three core demands of "position accuracy", "platform motion coordination" and "docking force stability", and finally ensure that the end of the mechanical arm can stably and accurately dock with the target under the action of appropriate contact force in the aerial docking process, reduce the influence of dynamic changes in the aerial environment (such as platform micro-shift caused by air flow disturbance and force impact in the docking moment) on the docking effect, and improve the success rate and safety of the aerial docking. is a fourth control parameter greater than 0; the fourth control parameter is used to quantify the deviation between the predicted contact force and the preset expected docking force, and convert it into a trajectory correction amount; the trajectory correction amount is subtracted from the reference point to further match the docking force demand on the basis of the position adjustment, so as to avoid physical damage to the unmanned aerial platform or the target caused by excessive contact force, damage to the docking attitude stability, and docking failure or loosening after docking caused by insufficient contact force; the trajectory correction amount is used to compensate for the defects of the pure position control that cannot cope with the dynamic force changes in the aerial docking, so as to adapt the trajectory of the end of the mechanical arm to the three core demands of "position accuracy", "platform motion coordination" and "docking force stability", and finally ensure that the end of the mechanical arm can stably and accurately dock with the target under the action of appropriate contact force in the aerial docking process, reduce the influence of dynamic changes in the aerial environment (such as platform micro-shift caused by air flow disturbance and force impact in the docking moment) on the docking effect, and improve the success rate and safety of the aerial docking. is a fourth control parameter greater than 0; the fourth control parameter is used to quantify the deviation between the predicted contact force and the preset expected docking force, and convert it into a trajectory correction amount; the trajectory correction amount is subtracted from the reference point to further match the docking force demand on the basis of the position adjustment, so as to avoid physical damage to the unmanned aerial platform or the target caused by excessive contact force, damage to the docking attitude stability, and docking failure or loosening after docking caused by insufficient contact force; the trajectory correction amount is used to compensate for the defects of the pure position control that cannot cope with the dynamic force changes in the aerial docking, so as to adapt the trajectory of the end of the mechanical arm to the three core demands of "position accuracy", "platform motion coordination" and "docking force stability", and finally ensure that the end of the mechanical arm can stably and accurately dock with the target under the action of appropriate contact force in the aerial docking process, reduce the influence of dynamic changes in the aerial environment (such as platform micro-shift caused by air flow disturbance and force impact in the docking moment) on the docking effect, and improve the success rate and safety of the aerial docking. is a fourth control parameter greater than 0; the fourth control parameter is used to quantify the deviation between the predicted contact force and the preset expected docking force, and convert it into a trajectory correction amount; the trajectory correction amount is subtracted from the reference point to further match the docking force demand on the basis of the position adjustment, so as to avoid physical damage to the unmanned aerial platform or the target caused by excessive contact force, damage to the docking attitude stability, and docking failure or loosening after docking caused by insufficient contact force; the trajectory correction amount is used to compensate for the defects of the pure position control that cannot cope with the dynamic force changes in the aerial docking, so as to adapt the trajectory of the end of the mechanical arm to the three core demands of "position accuracy", "platform motion coordination" and "docking force stability", and finally ensure that the end of the mechanical arm can stably and accurately dock with the target under the action of appropriate contact force in the aerial docking process, reduce the influence of dynamic changes in the aerial environment (such as platform micro-shift caused by air flow disturbance and force impact in the docking moment) on the docking effect, and improve the success rate and safety of the aerial docking.

[0076] The method provided by the embodiment introduces the contact force related parameter when generating the reference trajectory of the end of the mechanical arm (the contact force deviation correction term is obtained by calculating the deviation between the predicted contact force and the preset expected docking force, combined with the fourth control parameter, and is used for trajectory correction), and integrates the "force feedback" in the aerial docking process into the trajectory planning, instead of relying only on the position information. Ultimately, the aerial docking can bring many key benefits: the trajectory of the end of the mechanical arm is dynamically adjusted through the contact force deviation correction term, when the predicted contact force and the expected docking force deviate (such as the target may be deviated due to excessive contact force between the end of the mechanical arm and the target in the initial docking, and the docking may fail due to insufficient contact force), the fourth control parameter quantitatively adjusts the deviation and converts it into a trajectory correction amount, and the trajectory of the end of the mechanical arm is further matched with the docking force demand on the basis of the position adjustment after the correction amount is subtracted from the reference point. This can avoid physical damage to the unmanned aerial platform or the target caused by excessive contact force, damage to the docking attitude stability, and docking failure or loosening after docking caused by insufficient contact force. At the same time, combined with the flight platform motion coordination term (related to the flight platform reference trajectory and coordinate system conversion) and the position error correction term (correcting the position deviation between the end of the mechanical arm and the target), the contact force deviation correction term can compensate for the defects of the pure position control that cannot cope with the dynamic force changes in the aerial docking, so as to adapt the trajectory of the end of the mechanical arm to the three core demands of "position accuracy", "platform motion coordination" and "docking force stability". Finally, the end of the mechanical arm can stably and accurately dock with the target under the action of appropriate contact force in the aerial docking process, reduce the influence of dynamic changes in the aerial environment (such as platform micro-shift caused by air flow disturbance and force impact in the docking moment) on the docking effect, and improve the success rate and safety of the aerial docking.

[0077] S105, introducing a control allocation method to optimize and adjust the preliminary reference trajectory, and performing aerial docking between the work unmanned aerial vehicle and the target based on the optimized trajectory.

[0078] Specifically, since the preliminary reference trajectory is usually a theoretical trajectory generated based on the core parameters such as position error, contact force deviation, flight platform coordination, and the actual hardware constraints and dynamic limitations of the aerial docking scene are not fully considered, direct use may cause docking failure or hardware risks, so the preliminary reference trajectory needs to be adjusted. The optimized trajectory refers to the final executable trajectory obtained by adjusting the preliminary reference trajectory through the control allocation method, which meets the hardware constraints of the work unmanned aerial vehicle, adapts to the dynamic environment in the air, and can realize the coordinated motion of the flight platform and the mechanical arm.

[0079] In a specific implementation, a control variable is defined; the control variable is a combination of a control quantity corresponding to a reference trajectory of the end of the mechanical arm and a control quantity corresponding to a reference trajectory of the flight platform; a state variable is defined; the state variable is a combination of position information of the end of the mechanical arm and position information of the flight platform; based on the reference trajectory of the end of the mechanical arm, a corresponding first ideal control quantity is generated as a nominal control component of the end of the mechanical arm; based on the reference trajectory of the flight platform, a corresponding second ideal control quantity is generated as a nominal control component of the flight platform; the nominal control component of the end of the mechanical arm and the nominal control component of the flight platform are integrated to form a nominal control law; the nominal control law is an ideal target value of the control variable; a constraint range of the control variable is set; an optimization problem aiming to minimize the deviation between the actual control variable and the nominal control law is constructed, a quadratic programming method is used to solve the optimization problem, and an optimized control variable is obtained under the condition of meeting the constraint range of the control variable; the reference trajectory of the end of the mechanical arm and the reference trajectory of the flight platform are synchronously adjusted according to the optimized control variable to obtain an optimized trajectory.

[0080] Specifically, the control quantity corresponding to the end-of-arm reference trajectory (such as the driving torque of each joint, the motion speed instruction, etc.) is combined with the control quantity corresponding to the flight platform reference trajectory (such as the thrust of each propeller, the flight speed instruction, etc.) to form a vector containing two types of control quantities as the control variable of the entire system. The real-time position coordinates of the end of the arm in the body coordinate system (such as the x, y, and z axis coordinates) are combined with the real-time position coordinates of the flight platform in the body coordinate system (such as the x, y, and z axis coordinates) to form a vector containing two types of position information as the state variable of the system. For the end-of-arm reference trajectory, according to the position, speed, etc. parameters contained in the trajectory, combined with the dynamics model of the arm (such as the relationship between joint motion and driving torque), the first ideal control quantity (such as the target torque required by each joint) that can make the end of the arm move according to the reference trajectory is calculated as the nominal control component of the end of the arm. For the flight platform reference trajectory, the same is true according to the trajectory parameters combined with the dynamics model of the flight platform (such as the relationship between position change and propeller thrust), the second ideal control quantity (such as the target thrust of each propeller) that can make the flight platform move according to the reference trajectory is calculated as the nominal control component of the flight platform. The nominal control components of the end of the arm and the nominal control components of the flight platform are integrated into a vector in a predetermined order, and the vector is the nominal control law. According to the hardware physical limitations of the work unmanned aerial vehicle, the constraint ranges of the arm control quantity (such as the maximum torque of the joint, the maximum motion speed) and the constraint ranges of the flight platform control quantity (such as the maximum thrust of the propeller, the minimum thrust) are determined, and these constraints are integrated into the overall constraint range of the control variable. Half of the sum of the deviations of the actual control variable and the nominal control law is taken as the objective function, and an optimization problem is constructed; the quadratic programming algorithm is used to solve the problem, and the control variable value that minimizes the objective function is determined on the premise that the solution satisfies the constraint range of the control variable, that is, the optimized control variable. The optimized control variable is decomposed into the end-of-arm control component and the flight platform control component, which are substituted into the kinematics model of the arm and the flight platform respectively to recalculate the position, speed, etc. parameters of the end of the arm and the flight platform at each time, and the original end-of-arm reference trajectory and flight platform reference trajectory are adjusted synchronously to finally obtain the optimized trajectory that meets the hardware constraints and has the minimum deviation. The specific implementation process of calculating the ideal control quantity can be referred to the description in the related art, which will not be repeated here.

[0081] For example, in an embodiment, the control variable can be represented as:

[0082] ;

[0083] wherein the control variable is represented as u; the control quantity corresponding to the end-of-arm reference trajectory is represented as u arm; and the control quantity corresponding to the flight platform reference trajectory is represented as u flight. ​​The control quantity corresponding to the reference trajectory of the flight platform.

[0084] The state variable can be expressed as:

[0085] ;

[0086] The control variable is obtained by solving the following QP problem: The state variable; the position information of the end of the mechanical arm is the position information of the flight platform. The state variable; the position information of the end of the mechanical arm is the position information of the flight platform. The state variable; the position information of the end of the mechanical arm is the position information of the flight platform.

[0087] The state equation is obtained The nominal control law is obtained, and the control allocation is obtained by solving the following QP problem:

[0088] ;

[0089] ;

[0090] The control variable is obtained by solving the following QP problem: The control variable is obtained by solving the following QP problem: The control variable is obtained by solving the following QP problem: The lower limit and upper limit of the control variable are respectively.

[0091] Further, from the generated optimized trajectory, the optimized flight platform reference trajectory (including the target position coordinates, attitude parameters, motion speed and acceleration of the flight platform at each time) and the optimized mechanical arm end reference trajectory (including the target position coordinates, attitude parameters, joint motion angle and speed of the mechanical arm end at each time) are extracted respectively, and the two types of trajectory data are transmitted to the central control unit of the working unmanned aerial vehicle. The central control unit generates corresponding flight control instructions (such as the target thrust of each propeller, the steering angle of the rudder, etc.) according to the optimized flight platform reference trajectory, and sends it to the actuator (such as the propeller, the rudder) of the flight platform; at the same time, according to the optimized mechanical arm end reference trajectory, the corresponding mechanical arm control instruction (such as the target speed of each joint motor, the driving torque, etc.) is generated, and is sent to the joint actuator (such as the joint motor, the speed reducer) of the mechanical arm. The flight platform actuator adjusts the propeller thrust and rudder state according to the flight control instruction to drive the flight platform to move according to the optimized trajectory; the mechanical arm joint actuator adjusts the joint motion according to the mechanical arm control instruction to drive the end effector to move according to the optimized trajectory.

[0092] ​The method provided by the embodiment can guarantee aerial docking in multiple aspects. First, by regarding the mechanical arm and the flight platform as a collaborative system rather than independent individuals, the synchronization optimization of the movements of the two is achieved by integrating the control variables, thereby avoiding movement conflicts (such as position deviation caused by the fact that the mechanical arm trajectory is not adapted when the flight platform is adjusted) that may be caused by the optimization of a single system, and ensuring that the actions of the two are coordinated and consistent. At the same time, the setting of the control variable constraint range (such as the joint movement limit of the mechanical arm and the thrust limit of the flight platform) can make the optimized control variables strictly comply with the hardware performance of the operation unmanned aerial vehicle, and prevent mechanical failure or movement failure caused by control instructions exceeding the hardware capability. The quadratic programming method is used to solve the optimization problem under the goal of minimizing the deviation, which not only retains the reasonable components planned based on the position error and contact force and the like in the preliminary reference trajectory, but also corrects the trajectory defects (such as path redundancy and force impact risk) under the constraint condition, so that the optimized trajectory can not only ensure the precise docking of the mechanical arm end with the target, but also enable the flight platform to stably cooperate and effectively cope with the aerial dynamic interference, thereby improving the safety, accuracy and efficiency of the docking process and ensuring the reliable completion of the aerial docking task.

[0093] The method provided by the application can collect training data of contact force and position parameters in three dimensions of horizontal, vertical and relative angle by building a ground data acquisition device, provide basic samples for the contact force-position neural network model, and ensure that the model can learn the real “position-contact force” mapping relationship. On this basis, a three-layer fully connected contact force-position neural network model is constructed and trained, so that it has the ability to accurately predict the contact force according to the relative position and attitude information, and solves the pain point that the contact force is difficult to measure directly in aerial docking. Then, the relative position and attitude information of the operation unmanned aerial vehicle and the target are obtained by the sensor, and the trained model is used to predict the contact force, so as to realize the conversion from position information to force information. Then, the preliminary reference trajectory including the flight platform and the mechanical arm end is generated by combining the predicted contact force, the position error and the expected docking force, so as to provide the initial movement basis for docking. Finally, a control allocation method is introduced, the control variables of the mechanical arm and the flight platform are combined, and the constraints are set, and the optimization problem is solved by quadratic programming to obtain the optimized trajectory, and the aerial docking is performed based on the optimized trajectory. The whole process forms a closed loop of “data-model-prediction-planning-optimization-execution”, which not only ensures the accurate docking position, but also avoids impact damage through force control, coordinates the movement of multiple systems, and improves the success rate and safety of the docking.

[0094] In the second aspect, when the preliminary reference trajectory is generated, the expected position of the flight platform is determined based on the target position, the mechanical arm workspace parameters and the target normal vector, the flight platform reference trajectory is generated by combining the position error and the first control parameter, the position error is calculated by the end of the mechanical arm and the target, and the position error correction term, the platform coordination correction term and the contact force deviation correction term are generated by combining the flight platform reference trajectory and the contact force deviation, and the three correction terms are subtracted from the target position reference point to obtain the end of the mechanical arm reference trajectory. This setting considers position alignment, platform coordination and force stability in trajectory planning, avoids excessive or insufficient docking force caused by pure position control, and lays a reasonable foundation for subsequent optimization.

[0095] In the third aspect, when the optimized trajectory is generated, the control variables and state variables are combined based on the control variables and position information of the mechanical arm and the flight platform, the nominal control law is generated based on the preliminary trajectory, the optimization problem is constructed by combining the hardware constraints and minimizing the deviation of the control variables and the nominal control law, and the quadratic programming is used for solving. This method can make the optimized control variables strictly comply with the hardware performance limitations of the operation unmanned aerial vehicle, avoid hardware overload or motion conflict, and at the same time, retain the reasonable components of the preliminary trajectory to the greatest extent, correct the defects such as path redundancy and force impact, so that the optimized trajectory can adapt to the dynamic interference in the air, realize the coordinated motion of the mechanical arm and the flight platform, and ensure the stability and efficiency of the docking process.

[0096] Corresponding to the foregoing embodiment of the air docking method based on the contact force-position neural network model, the present application also provides an embodiment of an air docking device based on the contact force-position neural network model.

[0097] Figure 2 The structure schematic diagram of the air docking device based on the contact force-position neural network model provided in Embodiment Two of the present application is shown in FIG. 2. Please refer to FIG. 2, Figure 2 The device provided in the present embodiment includes a collection module 210, a training module 220, a prediction module 230, a generation module 240 and a docking module 250.

[0098] The collection module 210 is configured to build a ground data collection device, and collect training data of contact force and corresponding position parameters in three dimensions of horizontal direction, vertical direction and relative angle by the ground data collection device.

[0099] The training module 220 is configured to construct a contact force-position neural network model, and train the contact force-position neural network model by using the training data.

[0100] The prediction module 230 is configured to obtain the relative position and attitude information between the operation unmanned aerial vehicle and the target, and predict the contact force based on the contact force-position neural network model.

[0101] The generation module 240 is configured to generate a preliminary reference trajectory in combination with the predicted contact force, the position error of the work unmanned aerial vehicle and the target, and a preset expected docking force; the preliminary reference trajectory includes a flight platform reference trajectory and a mechanical arm end reference trajectory.

[0102] The docking module 250 is configured to introduce a control allocation method, to optimize and adjust the preliminary reference trajectory, and to perform aerial docking of the work unmanned aerial vehicle and the target based on the optimized trajectory.

[0103] The device of the embodiment can be used to execute Figure 1 The steps of the method embodiment are similar to the specific implementation principle and implementation process, and will not be described here.

[0104] The implementation process of the functions and roles of each unit in the device is specifically described in the implementation process of the corresponding steps in the above method, and will not be described here.

[0105] For the device embodiment, since it basically corresponds to the method embodiment, the related parts can be referred to the part of the method embodiment. The device embodiments described above are only schematic, and the units described as separate components can or can not be physically separate, and the components displayed as units can or can not be physical units, that is, they can be located in one place, or distributed on multiple network units. According to actual needs, part or all of the modules can be selected to achieve the purpose of the scheme of the present application. Those skilled in the art can understand and implement without creative labor.

[0106] The above is only the preferred embodiment of the present application, and is not used to limit the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the scope of protection of the present application.

Claims

1. An aerial docking method based on a contact force-position neural network model, characterized in that, The method includes: A ground data acquisition device is set up to collect training data on contact force and corresponding position parameters in three dimensions: horizontal, vertical and relative angle. Construct a contact force-position neural network model, and train the contact force-position neural network model using the training data; The relative position and attitude information between the operation drone and the target are obtained, and the contact force is predicted based on the contact force-position neural network model. By combining the predicted contact force, the positional error between the drone and the target, and the preset expected docking force, a preliminary reference trajectory is generated; the preliminary reference trajectory includes the flight platform reference trajectory and the robotic arm end effector reference trajectory. A control allocation method is introduced to optimize and adjust the preliminary reference trajectory, and the aerial docking between the operation UAV and the target is carried out based on the optimized trajectory; The introduced control allocation method optimizes and adjusts the preliminary reference trajectory, including: Define control variables; the control variables are the combination of the control quantities corresponding to the reference trajectory of the robotic arm end effector and the control quantities corresponding to the reference trajectory of the flight platform. Define state variables; the state variables are a combination of the position information of the robotic arm's end effector and the position information of the flight platform. Based on the reference trajectory of the robotic arm end effector, the corresponding first ideal control quantity is generated as the nominal control component of the robotic arm end effector. Based on the flight platform reference trajectory, a corresponding second ideal control quantity is generated as the nominal control component of the flight platform. The nominal control component at the end of the robotic arm is integrated with the nominal control component of the flight platform to form a nominal control law; the nominal control law is the ideal target value of the control variable. Set the constraint range for the control variables; An optimization problem is constructed with the objective of minimizing the deviation between the actual control variables and the nominal control law. The optimization problem is solved using a quadratic programming method. Under the condition of satisfying the constraints of the control variables, the optimized control variables are obtained. Based on the optimized control variables, the reference trajectory of the robotic arm end effector and the reference trajectory of the flight platform are adjusted synchronously to obtain the optimized trajectory.

2. The method according to claim 1, characterized in that, The process of generating a preliminary reference trajectory by combining the predicted contact force, the positional error between the UAV and the target, and the preset expected contact force includes: Determine the desired position of the operation drone flight platform; the desired position is calculated based on the target's position in the operation drone's body coordinate system, the robotic arm's workspace parameters, and the target's normal vector; The position error of the UAV flight platform is calculated, and a reference trajectory for the flight platform is generated by combining the desired position and the preset first control parameters. The reference trajectory for the flight platform is obtained by subtracting the product of the position error and the first control parameters from the desired position. Determine the positional error between the end effector of the drone's robotic arm and the target in the drone's body coordinate system; By combining the target's position in the body coordinate system, the positional error between the robotic arm's end effector and the target, the flight platform's reference trajectory, and the deviation between the predicted contact force and the preset expected contact force, a reference trajectory for the robotic arm's end effector is generated.

3. The method according to claim 2, characterized in that, The process of generating a reference trajectory for the robotic arm's end effector by combining the target's position in the body coordinate system, the positional error between the robotic arm's end effector and the target, the flight platform's reference trajectory, and the deviation between the predicted contact force and the preset expected contact force includes: The position of the target in the coordinate system of the drone is used as the reference point; Calculate the position error between the robotic arm's end effector and the target, and multiply the position error by a preset second control parameter to obtain a position error correction term; Based on the flight platform reference trajectory and the rotation matrix transformation relationship between the UAV body coordinate system and the world coordinate system, a flight platform motion coordination term is generated. The flight platform motion coordination term is then multiplied by a preset third control parameter to obtain a platform coordination correction term. The deviation between the predicted contact force and the preset expected contact force is calculated, and the deviation is multiplied by the preset fourth control parameter to obtain the contact force deviation correction term. The position error correction term, platform coordination correction term, and contact force deviation correction term are sequentially subtracted from the reference point to obtain the reference trajectory of the robotic arm end effector.

4. The method according to claim 1, characterized in that, The acquisition of the relative position and attitude information between the operational UAV and the target includes: Obtain the position coordinates of the end effector of the robotic arm of the operational drone in the coordinate system of the operational drone, and the position coordinates of the target in the coordinate system of the operational drone; Based on the horizontal coordinates of the robotic arm end effector and the target in the coordinate system of the drone body, the relative horizontal distance is calculated using the formula for the horizontal distance between the two points. Based on the vertical coordinates of the robotic arm end effector and the target in the coordinate system of the drone body, the relative vertical distance is obtained by calculating the absolute difference between the two coordinate values. Obtain the attitude quaternion of the target in the coordinate system of the operating UAV, and the attitude quaternion of the robotic arm end effector in the coordinate system of the UAV. Based on the quaternions of the target and the end effector of the robotic arm, the attitude information is converted into angle information through the quaternion attitude conversion formula, and the relative angle is calculated.

5. The method according to claim 1, characterized in that, The prediction of contact force based on the contact force-bit neural network model includes: The relative position and attitude information is normalized and used as input data for the contact force-position neural network model; The normalized input data is fed into the input layer of the contact force-position neural network model, and the input layer transmits the input data to the first hidden layer. The first hidden layer receives input data transmitted from the input layer, extracts and transforms features from the input data through neurons, and uses an activation function to perform non-linear mapping on the extracted intermediate features, transmitting the mapping result to the second hidden layer. The second hidden layer receives the mapping result transmitted from the first hidden layer, extracts and transforms the comprehensive features through neurons, performs non-linear mapping using an activation function, and transmits the mapping result to the output layer. The output layer receives the mapping results transmitted from the second hidden layer, generates and outputs the corresponding contact force prediction value through linear superposition processing.

6. The method according to claim 1, characterized in that, The aforementioned ground data acquisition device collects training data on contact force and corresponding position parameters in three dimensions: horizontal, vertical, and relative angle. This includes: The hardware components of the ground data acquisition device are configured; the hardware components include a force sensor, a signal conditioner, a motion capture system, a three-dimensional moving platform, and a control device. The force sensor is used to collect contact force data, the signal conditioner is used to process the signal output by the force sensor, the motion capture system is used to collect position parameters, the three-dimensional moving platform is used to drive the object to perform multi-dimensional motion, and the control device is used to control the motion of the three-dimensional moving platform. The ground data acquisition device is initialized, the force sensor is zeroed, and the relative positioning error of the two objects is calibrated through the motion capture system. The control device drives the three-dimensional moving platform to move the test object in the horizontal and vertical directions at set intervals, and adjusts its posture in the relative angular dimension at set angular intervals. During the test object's motion, the force sensor collects contact force data in real time at different positions and postures, and the motion capture system simultaneously collects the corresponding position parameters. The collected contact force data is aligned with and recorded with the corresponding position parameters to form a training dataset.

7. The method according to claim 1, characterized in that, The contact force-position neural network model is a three-layer fully connected neural network, including an input layer, two hidden layers and an output layer. The input layer is used to receive normalized horizontal distance, vertical distance and relative angle parameters. Each hidden layer contains a preset number of neurons and uses an activation function to process the data. The output layer is used to output the contact force.

8. The method according to claim 1, characterized in that, The step of training the contact force-position neural network model using the training data includes: Configure the training parameters for the contact force-position neural network model; The training dataset is input into the constructed force contact-position neural network model in a batch processing manner according to a preset batch, and the model parameters are updated in reverse by the error calculated by the optimizer based on the loss function. An early stopping mechanism is introduced during training. Training is stopped when the performance of the contact force-position neural network model on the validation set tends to stabilize, thus obtaining the trained contact force-position neural network model.

9. An aerial docking device based on a contact force-position neural network model, characterized in that, The device includes an acquisition module, a training module, a prediction module, a generation module, and a docking module; The acquisition module is used to build a ground data acquisition device, which collects training data on contact force and corresponding position parameters in three dimensions: horizontal, vertical and relative angle. The training module is used to construct a contact force-position neural network model and train the contact force-position neural network model using the training data; The prediction module is used to obtain the relative position and attitude information between the operation drone and the target, and predict the contact force based on the contact force-position neural network model. The generation module is used to generate a preliminary reference trajectory by combining the predicted contact force, the positional error between the UAV and the target, and the preset expected docking force; the preliminary reference trajectory includes the flight platform reference trajectory and the robotic arm end effector reference trajectory. The docking module is used to introduce a control allocation method to optimize and adjust the preliminary reference trajectory, and to perform aerial docking between the operational UAV and the target based on the optimized trajectory. The introduced control allocation method optimizes and adjusts the preliminary reference trajectory, including: Define control variables; the control variables are the combination of the control quantities corresponding to the reference trajectory of the robotic arm end effector and the control quantities corresponding to the reference trajectory of the flight platform. Define state variables; the state variables are a combination of the position information of the robotic arm's end effector and the position information of the flight platform. Based on the reference trajectory of the robotic arm end effector, the corresponding first ideal control quantity is generated as the nominal control component of the robotic arm end effector. Based on the flight platform reference trajectory, a corresponding second ideal control quantity is generated as the nominal control component of the flight platform. The nominal control component at the end of the robotic arm is integrated with the nominal control component of the flight platform to form a nominal control law; the nominal control law is the ideal target value of the control variable. Set the constraint range for the control variables; An optimization problem is constructed with the objective of minimizing the deviation between the actual control variables and the nominal control law. The optimization problem is solved using a quadratic programming method. Under the condition of satisfying the constraints of the control variables, the optimized control variables are obtained. Based on the optimized control variables, the reference trajectory of the robotic arm end effector and the reference trajectory of the flight platform are adjusted synchronously to obtain the optimized trajectory.

Images