Aerial interoperation system active disturbance rejection control method based on laser perception

Abstract

The invention discloses an active-disturbance-rejection control method for an air mutual-control system based on laser perception, and the method specifically comprises the steps: building a kinetic model and a kinematic model of a four-rotor unmanned plane, and taking a mechanical arm as the external disturbance of the unmanned plane; point cloud data around a flying mechanical arm are collected, through accurate segmentation and registration processing, graph optimization is conducted through the processed point cloud data, and therefore the position of the quadrotor unmanned aerial vehicle is accurately positioned; generating a torque instruction for controlling a motion axis of the quadrotor unmanned aerial vehicle according to the position of the quadrotor unmanned aerial vehicle through an integral type active disturbance rejection attitude control strategy improved by PID position control series connection; and according to the lift force and the torque instruction of the four-rotor unmanned aerial vehicle, calculating a throttle control signal corresponding to a propeller to complete control of the four-rotor unmanned aerial vehicle. Pose information is accurately transmitted through a graph optimization framework, the stability and operation precision of the system are remarkably improved in combination with an improved integral type active disturbance rejection control technology, and it is ensured that accurate control and stable maintenance of the unmanned aerial vehicle are achieved in a complex environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicles combined with mechanical arms, and in particular relates to a self-disturbance rejection control method of an aerial mutual manipulation system based on laser perception. Background Art

[0002] The aerial dexterous operating system is a new type of robot system that integrates the rapid spatial movement capability of drones and the precise manipulation capability of robotic arms. Through collaborative operation, this system has achieved the leap from a single observation function to an operational capability of drones, greatly expanding the application scenarios of drones. Compared with traditional drones, the aerial dexterous operating system has more precise manipulation and environmental interaction capabilities, which enables it to show unique advantages and research significance in key tasks such as live maintenance of long-distance high-voltage power lines, contact inspection and flaw detection of hazardous chemical pipelines in chemical plants, and rapid delivery of emergency materials with high timeliness.

[0003] With the rapid development of science and technology, especially the maturity of small multi-rotor drone technology and the improvement of the industrial chain, the application areas of aerial agile operating systems have expanded rapidly, from professional aerial photography to power line inspection, agricultural plant protection, emergency search and rescue, etc. This trend has been paid attention to and promoted at the national level.

[0004] However, despite the huge development potential and application prospects of aerial dexterous operation technology, there are still some key technical problems that have not been solved. The existing aerial dexterous operation platform is limited to application scenarios where it cannot physically contact the external environment. Due to this limitation, aerial dexterous operation systems often face problems of insufficient stability and accuracy when performing complex operations, especially in strong dynamic and uncertain environments. The control strategy and execution efficiency of the system need to be improved urgently.

[0005] Therefore, mastering the efficient control and environmental perception technology of aerial dexterous operation platforms is of great significance to promoting the development of this field. This can not only enhance the stability and accuracy of the UAV robotic arm when performing tasks, but also significantly improve the operating ability and autonomy of the UAV robotic arm in complex environments, and improve the intelligence level of the system and its ability to interact with complex environments. By achieving efficient collaborative control, the UAV robotic arm can be better integrated with the operator or other automated systems, improve operating efficiency, reduce labor costs, and increase the success rate of mission execution while ensuring personnel safety.

[0006] In addition, the intelligence and automation of aerial agile operation platforms are also hot topics in current research. With the rapid development of artificial intelligence technology, applying advanced algorithms and control strategies to aerial agile operation systems to achieve higher levels of autonomous control and intelligent decision-making is the key to improving system performance. At the same time, safety and reliability are also factors that cannot be ignored in the design of aerial agile operation systems, especially in application scenarios involving personnel safety and critical infrastructure.

[0007] The present invention conducts in-depth research on the mapping technology and control technology of aerial dexterous operation systems. It can not only provide precise positioning in the mapping of complex surrounding environments, but also provide new perspectives and methods for the development of control theory, which has important theoretical research value. It can also promote the implementation of aerial dexterous operation technology in practical applications, promote technological progress and industrial upgrading in related industries, and has broad practical value. Summary of the invention

[0008] In view of the above-mentioned deficiencies in the prior art, the laser sensing-based aerial interactive control system self-disturbance rejection control method provided by the present invention adopts advanced control strategies and algorithm optimization, aiming to improve the stability and accuracy of the aerial agile operation system in complex environments, enhance the autonomy and intelligence level of the system, improve control efficiency, and ensure the safety and reliability of operations.

[0009] In order to achieve the above-mentioned invention object, the technical solution adopted by the present invention is: an airborne mutual control system self-disturbance rejection control method based on laser sensing, comprising the following steps:

[0010] S1. Establish the dynamic model and kinematic model of the quadrotor drone and model the system;

[0011] S2. Based on the modeled system, collect point cloud data around the flight robot arm, segment and register the point cloud data, and use the processed point cloud data to perform graph optimization and accurately locate the position of the quadcopter;

[0012] S3, through the PID position control series improved integral anti-disturbance attitude control strategy, according to the position of the quadrotor drone, generate the torque command to control the motion axis of the quadrotor drone;

[0013] S4. Calculate the corresponding throttle control signal of the propeller according to the lift and torque instructions of the quadrotor drone to complete the control of the quadrotor drone.

[0014] Further: in said S1, the dynamic model includes a position kinematics model and a posture kinematics model;

[0015] Among them, the expression of the position kinematic model is specifically:

[0016]

[0017] In the formula, Indicates the position of the quadcopter in the world coordinate system, w x b , w y b and w z b They represent the quadcopter along o in the world coordinate system. w x w , o w y w and w z w The position of the axis, w υ b Represents the linear velocity of the quadcopter in the world coordinate system, and They represent the quadcopter along o in the world coordinate system. w x w , o w y w and w z w Linear speed of the axis;

[0018] The expression of the posture kinematics model is as follows:

[0019]

[0020] In the formula, represents the attitude change rate, and They represent the angular velocity of the three axes in the body coordinate system, φ b ,θ b and ψ b They represent the angles on the three axes in the body coordinate system. b ω represents the angular velocity of the body, b ω=[ b ω x , b ω y , b ω z ] T , b ω x , b ω y and b ω z They represent the quadcopter along o in the world coordinate system. b x b , o b yb and b z b Angular velocity of the axis;

[0021] The attitude kinematics model includes the position dynamics model and the attitude dynamics model;

[0022] Among them, the expression of the position dynamics model is specifically:

[0023]

[0024] In the formula, φ represents the roll angle of the aircraft, θ represents the pitch angle of the aircraft, ψ represents the yaw angle of the aircraft, and m represents the mass of the aircraft. b F represents the total amount of lift generated by the propeller in the body coordinate system, g represents the acceleration of gravity, and Respectively represent the acceleration of the three axes in the world coordinate system;

[0025] The expression of the posture dynamics model is as follows:

[0026]

[0027] Where J represents the total moment of inertia of the quadrotor drone, J = diag[J xx ,J yy ,J zz ], J xx represents the first moment of inertia, J yy represents the second moment of inertia, J zz represents the third moment of inertia, diag[·] represents the symbol of a diagonal matrix, which is used to represent a matrix with zero elements outside the main diagonal. b ω is the angular velocity of the quadrotor drone in the body coordinate system, represents the angular acceleration of the quadrotor drone in the body coordinate system, b τ represents the total torque vector in the quadrotor drone body coordinate system, Indicates along o b x b The torque vector about the axis, b τ y Indicates along o b y b The torque vector about the axis, b τ z Indicates along o b z b The moment vector about the axis. T represents the transpose sign.

[0028] Further: S2 comprises the following sub-steps:

[0029] S21, collecting point cloud data around the flight robot arm, and segmenting the point cloud data using an optimized RANSAC algorithm;

[0030] S22, spatially registering the segmented point cloud data using a 3D-NDT algorithm to obtain point cloud registration information;

[0031] S23. Based on the point cloud registration information, nonlinear least squares is used in laser SLAM to implement graph optimization SLAM and accurately locate the position of the quadrotor drone.

[0032] Further: In S21, the method for segmenting the point cloud data by using the optimized RANSAC algorithm is specifically as follows:

[0033] Based on the set TIN segmentation strategy and flexibility consistency set threshold, the point cloud data is iteratively segmented to obtain segmented point cloud data;

[0034] Among them, the TIN segmentation strategy is as follows:

[0035] The triangle of TIN is used as the growth unit. According to the height difference between the structural units of obstacles in the corresponding environment, the corresponding constraint processing is performed in the case of occlusion. Then, the triangles with the same mark are grouped, and the area of ​​the point cloud data is segmented according to the grouped triangle area.

[0036] The specific expression of constraint processing is:

[0037]

[0038] In the formula, max(·) represents the maximum value function, min(·) represents the minimum value function, and t q1 Indicates the ground elevation at the first point in the triangle, t q2 Indicates the ground elevation at the second point in the triangle, t q3 Indicates the ground elevation at the third point in the triangle. Represents the height difference threshold, which is used to constrain the triangle;

[0039] The expression of the maximum number of iterations MaxIteration for setting the flexibility consistent set threshold is as follows:

[0040]

[0041] Where PN min Indicates the minimum number of points contained in the plane, PN max Indicates the maximum number of points contained in the plane. n is a positive integer used to ensure the integrity of the iteration effect. T factor is the shrinkage factor of the fixed consensus set threshold PN_S in the RANSAC algorithm, PNs ∈[PN min ,PN max ].

[0042] Further: In S22, the method for registering point cloud information using the 3D-NDT algorithm is specifically as follows:

[0043] The segmented point cloud data is divided into voxel cells of the same regular size, and their mean and covariance matrix are calculated. Normal distribution modeling is performed for each point in the voxel cell, and the point cloud space is represented as a set of normal distributions. The Hessian matrix method is used to optimize the registration of the probability values ​​of the normal distribution of the point cloud space to obtain the point cloud registration information.

[0044] Further: In S23, performing graph optimization SLAM by least squares includes the following sub-steps:

[0045] S231, construct the error function e of the linear system ij (x);

[0046]

[0047] Where, t i represents the coordinate value at the i node, t j represents the coordinate value at node j, t ij Represents the difference in coordinate values ​​between nodes i and j. represents the transpose of the rotation matrix of node i, describing the rotation of node i relative to the global coordinate system, represents the transpose of the rotation matrix from node i to node j, θ i represents the attitude angle at node i, θ j represents the attitude angle at node j, θ ij It represents the change of the attitude angle of node j relative to node i;

[0048] S232, using the error function to obtain partial derivatives of the horizontal axis coordinate values ​​of all nodes to obtain a Jacobian matrix, and using the Jacobian matrix to construct a sparse matrix and a dense vector;

[0049] Jacobian matrix J ij The specific expression is:

[0050]

[0051] In the formula, Represents the horizontal axis coordinate value x of the error function for node i i Find the partial derivative, Represents the horizontal axis coordinate value x of the error function for node j j Find partial derivatives;

[0052] The expressions of sparse matrix H and dense vector b are as follows:

[0053]

[0054]

[0055] Where C is C ij The block matrix composed of ij is the covariance matrix;

[0056] S233, solving the linear equation according to the sparse matrix and the dense vector and updating the position of the quadrotor drone, calculating the new sparse matrix and the dense vector according to the updated position of the quadrotor drone, iteratively solving the linear equation and updating the position of the quadrotor drone until the error between the current position and the updated position is less than the error threshold or the number of iterations reaches the maximum iteration value, thereby obtaining the optimized position of the quadrotor drone;

[0057] Among them, the updated quadrotor drone position P i The expression of the error ΔP between the current position and the updated position is as follows:

[0058] P i =P i ′ -H -1 b

[0059] ΔP=-H -1 b

[0060] Where P i ′ Indicates the position of the quadcopter before updating.

[0061] Further: S3 is specifically:

[0062] The position of the quadrotor drone is obtained, and the attitude angle is obtained through the PID position control strategy. The attitude angle is used as the target, and the total disturbance of the quadrotor drone is compensated through the improved integral anti-disturbance controller strategy to generate the torque command for controlling the motion axes of the quadrotor drone, including the roll axis, pitch axis and yaw axis.

[0063] Among them, the improved integral active disturbance rejection controller (SNLADRCI) strategy includes tracking differentiator (TD), improved extended state observer (SNLESO), integral error feedback control law (NLSEFI);

[0064] The expression of the tracking differentiator is specifically:

[0065]

[0066] In the formula, v1(k+1) represents the tracking input signal at time k+1, v1(k) represents the tracking input signal at time k, and v d (·) represents the expected signal of the tracking input, v2(k+1) represents the first-order derivative of v1(k+1), v2(k) represents the first-order derivative of v1(k), u1 represents the second-order derivative of v1(k), h represents the discrete sampling time, r represents the speed factor that determines the tracking speed, and fst(·) represents the fastest tracking function;

[0067] The expression of the improved extended state observer is as follows:

[0068]

[0069] Where y(k) represents the output of the quadrotor UAV system, z1(k) represents the estimated value of the system input reference value x1 by the improved extended state observer at time k, ε1 represents the estimated error of the improved extended state observer for the system input reference value x1, z1(k+1) represents the estimated value of the system input reference value x1 by the improved extended state observer at time k+1, z2(k) represents the estimated value of the derivative x2 of the input reference value by the improved extended state observer at time k, z2(k+1) represents the estimated value of the derivative x2 of the input reference value by the improved extended state observer at time k+1, z3(k) represents the estimated value of the system disturbance x3 by the improved extended state observer at time k, z3(k+1) represents the estimated value of the system disturbance x3 by the improved extended state observer at time k+1, β 01 represents the system input error feedback gain, α1 is the power of the nonlinear part of the sfal(·) function, β 02 represents the system input differential error feedback gain, α2 is the power of the nonlinear part of the sfal(·) function, sfal(·) represents the improved fal function, and its specific expression is sfal(e,α)=sign(e)|e| a , β 03 represents the system disturbance error feedback gain, h is the discrete sampling time, b is the control channel gain, and u(k) represents the control input at time k;

[0070] The expression of the integral error feedback control law is as follows:

[0071] u2=β0sfal(e0,α3)+β1sfal(e1,α4)+β2sat(c,d)

[0072] In the formula, u2 represents the control quantity, e0 represents the system input error, e1 represents the system input differential error, β0 represents the system input nonlinear feedback gain, β1 represents the system input differential term nonlinear feedback gain, β2 represents the system input integral term nonlinear feedback gain, sat(·) represents the saturation function, c represents the integral term, c=e1(k+1)+e1(k), e1(k+1) represents the system input error at time k+1, e1(k) represents the system input differential error at time k, and d represents the segmentation of the sat(·) function.

[0073] Further: S4 includes the following sub-steps:

[0074] S41, using the control efficiency model to calculate the square of the propeller speed required by the quadrotor drone according to the torque command and the lift, where the torque command is specifically the total torque vector on the quadrotor drone body coordinate system b τ=[ b τ x , b τ y , b τ z ] T ;

[0075] The expression of the control efficiency model is as follows:

[0076]

[0077] In the formula, b F represents the lift of the quadrotor drone, C F is the lift coefficient, C M represents the moment coefficient, d represents half of the frame wheelbase length, represents the square of the speed of the i-th propeller, i=1,2,3,4;

[0078] S42, calculating the corresponding throttle control signal of the propeller according to the square of the propeller speed through the power unit model, thereby completing the control of the quadrotor drone;

[0079] The expression of the power unit model is as follows:

[0080]

[0081] Where, σ represents the throttle control signal, C R Indicates the proportional coefficient of the throttle control signal converted into the motor speed. Indicates the basic speed bias, T m represents the motor response time constant, Indicates propeller speed.

[0082] The beneficial effects of the present invention are:

[0083] (1) The present invention divides the aerial agile operating system into two key components, establishes the quadcopter as the core of the system, and regards the robotic arm as an external disturbance factor. To this end, the present invention uses a graph-optimized laser SLAM technology to perceive and map the surrounding complex environment, and matches and optimizes the high-similarity local graph with the precise drone posture information, and adopts a disturbance compensation control strategy. The core of this strategy is to strengthen the dominant position of the quadcopter, while incorporating the dynamic influence of the robotic arm into the category of disturbance synchronization compensation, aiming to ensure that the drone can achieve precise control and stability maintenance in a complex and changeable environment, thereby improving the response performance and operation accuracy of the overall system.

[0084] (2) Based on the simplification of the system model, the present invention designs an inner and outer loop separation control strategy. In the outer loop position control link, the classic PID control algorithm is used to achieve precise adjustment of the UAV position; and in the inner loop attitude control link, the SNLADRCI control technology is introduced to achieve efficient and stable control of the UAV attitude. In addition, the present invention also innovatively designs an improved integral self-disturbance rejection controller, which aims to stabilize the aerial agile operation platform by optimizing and efficiently suppressing the disturbances inside and outside the system.

[0085] (3) Aiming at the problem of discontinuity of the first-order derivative at the segmentation point ±δ of the traditional extended state observer, the present invention proposes an innovative simplified nonlinear function sfal(e,α), which ensures the continuity of the first-order derivative in the entire definition domain and effectively improves the performance of the observer. At the same time, the introduction of this function also simplifies the parameter setting process in nonlinear control and improves the flexibility and efficiency of control system design.

[0086] (4) In actual experimental tests, the traditional anti-disturbance control still has static errors in some cases. To this end, the present invention cleverly adds an integral term to the nonlinear error feedback control law to form an integral error feedback control law, which effectively eliminates the static error and further improves the control accuracy of the system.

[0087] (5) In response to the challenge of drone positioning in complex environments, the present invention uses graph-optimized laser SLAM technology to collect the position and attitude feedback data of the drone in real time to further enhance the drone's precise operation capability. Considering multiple factors such as system performance and safety, the present invention also conducts comprehensive multi-environment simulation test verification. By using the Gazebo environment for simulation testing, the present invention further ensures the stability and safety of the system to achieve an optimized control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] Figure 1 The present invention is a flow chart of the anti-disturbance control method of the aerial mutual control system based on laser sensing.

[0089] Figure 2 Build a map for the point cloud of the present invention.

[0090] Figure 3 Optimized laser SLAM structure diagram for the present invention

[0091] Figure 4 It is an improved integral type active disturbance rejection controller of the present invention.

[0092] Figure 5 This is a comparison chart between traditional nonlinear functions and new nonlinear functions.

[0093] Figure 6 This is a block diagram of the overall control structure of the system of the present invention.

[0094] Figure 7 This is a simulation model diagram for the Gazebo environment simulation model test of the system of the present invention.

[0095] Figure 8 This is the X-axis position control effect diagram of the cascade controller of SNLADRCI-PID of the present invention.

[0096] Fig. 9 This is a diagram showing the Y-axis position control effect of the cascade controller of the SNLADRCI-PID of the present invention.

[0097] Fig.10 This is a diagram showing the Z-axis position control effect of the cascade controller of the SNLADRCI-PID of the present invention. DETAILED DESCRIPTION

[0098] The specific implementation modes of the present invention are described below so that those skilled in the art can understand the present invention. However, it should be clear that the present invention is not limited to the scope of the specific implementation modes. For those of ordinary skill in the art, as long as various changes are within the spirit and scope of the present invention as defined and determined by the attached claims, these changes are obvious, and all inventions and creations utilizing the concept of the present invention are protected.

[0099] like Figure 1 As shown, in one embodiment of the present invention, the anti-disturbance control method of the airborne mutual manipulation system based on laser sensing includes the following steps:

[0100] S1. Establish the dynamic model and kinematic model of the quadrotor drone and model the system;

[0101] S2. Based on the modeled system, collect point cloud data around the flight robot arm, segment and register the point cloud data, and use the processed point cloud data to perform graph optimization and accurately locate the position of the quadcopter;

[0102] S3, through the PID position control series improved integral anti-disturbance attitude control strategy, according to the position of the quadrotor drone, generate the torque command to control the motion axis of the quadrotor drone;

[0103] S4. Calculate the corresponding throttle control signal of the propeller according to the lift and torque instructions of the quadrotor drone to complete the control of the quadrotor drone.

[0104] In this embodiment, taking into account the nonlinear nature of the quadrotor UAV, it is assumed when establishing the model that the geometric center of the aerial dexterous operating platform coincides with the center of gravity, the influence of the gyroscopic torque generated by the rotation of the motor and the propeller on the aerial dexterous operating platform is ignored, and the influence of air resistance on the aerial dexterous operating platform is ignored; under this approximation, since the self-anti-disturbance control algorithm used in the present invention does not rely on the precise mathematical model of the controlled object, the influence of the robotic arm is regarded as an external disturbance, thereby simplifying the model and completing the establishment of the dynamic model and kinematic model of the quadrotor UAV.

[0105] In S1, the dynamic model includes a position kinematics model and a posture kinematics model;

[0106] Among them, in the world coordinate system, the kinematic analysis of the quadrotor drone is carried out to obtain the position kinematic model of the quadrotor aircraft. The expression of the position kinematic model is as follows:

[0107]

[0108] In the formula, Indicates the position of the quadcopter in the world coordinate system, w x b , w y b and w z b They represent the quadcopter along o in the world coordinate system. w x w , o w y w and w z w The position of the axis, w υ b Represents the linear velocity of the quadcopter in the world coordinate system, and They represent the quadcopter along o in the world coordinate system.w x w , o w y w and w z w The linear velocity of the axis. The derivative of the position in the above formula is the linear velocity.

[0109] The quad-rotor drone is located along o in the body coordinate system b x b , o b y b , o b z b The angular velocity of the axis is b ω=[ b ω x , b ω y , b ω z ] T Project the rate of change of the Euler angle to the body coordinate system and combine it with the body angular velocity of the quadcopter b ω and attitude change rate Get the posture kinematics model;

[0110] The expression of the posture kinematics model is as follows:

[0111]

[0112] In the formula, represents the attitude change rate, and They represent the angular velocity of the three axes in the body coordinate system, φ b ,θ b and ψ b They represent the angles on the three axes in the body coordinate system. b ω represents the angular velocity of the body, b ω=[ b ω x , b ω y , b ω z ] T , b ω x , b ω y and b ω z They represent the quadcopter along o in the world coordinate system. b x b , o b y b and b z bAngular velocity of the axis;

[0113] The attitude kinematics model includes the position dynamics model and the attitude dynamics model;

[0114] When analyzing the forces on the quadcopter, the present invention adopts a simplified approach, ignoring various minor forces including air resistance and friction, and assuming that the drone is mainly affected by gravity and the lift generated by the propeller. Under this approximation, the influence of the manipulator is regarded as an external disturbance, thereby simplifying the model. The expression of the position dynamics model is specifically:

[0115]

[0116] In the formula, φ represents the roll angle of the aircraft, θ represents the pitch angle of the aircraft, ψ represents the yaw angle of the aircraft, and m represents the mass of the aircraft. b F represents the total amount of lift generated by the propeller in the body coordinate system, g represents the acceleration of gravity, and Respectively represent the acceleration of the three axes in the world coordinate system;

[0117] The expression of the posture dynamics model is as follows:

[0118]

[0119] Where J represents the total moment of inertia of the quadrotor drone, J = diag[J xx ,J yy ,J zz ], J xx represents the first moment of inertia, J yy represents the second moment of inertia, J zz represents the third moment of inertia, diag[·] represents the symbol of a diagonal matrix, which is used to represent a matrix with zero elements outside the main diagonal. b ω is the angular velocity of the quadrotor drone in the body coordinate system, represents the angular acceleration of the quadrotor drone in the body coordinate system, b τ represents the total torque vector in the quadrotor drone body coordinate system, b τ=[ b τ x , b τ y , b τ z ] T , b τ x Indicates along o b x b The torque vector about the axis, b τ y Indicates along ob y b The torque vector about the axis, b τ z Indicates along o b z b The moment vector about the axis. T represents the transpose sign.

[0120] In this embodiment, after the system is modeled, the surrounding environment parameters are collected and the point cloud data obtained by scanning with a laser sensor is preprocessed. The preprocessing includes segmentation, detection and registration of the point cloud data, feature matching and pose estimation between adjacent frames of laser scanning, and high similarity matching between the established local maps. Figure 2 As shown in the figure, the flight manipulator establishes its related constraint equations, and optimizes and solves them to obtain more accurate UAV posture information and a high-precision and consistent three-dimensional environmental model. Figure 3 As shown, this embodiment uses a graph-optimized laser SLAM framework to accurately monitor the real-time posture of a quadrotor drone, ensuring the flexibility and accuracy of aerial operations.

[0121] The S2 comprises the following sub-steps:

[0122] S21, collecting point cloud data around the flight robot arm, and segmenting the point cloud data using an optimized RANSAC (random sampling consensus algorithm);

[0123] S22, spatially registering the segmented point cloud data using a 3D-NDT algorithm to obtain point cloud registration information;

[0124] S23. Based on the point cloud registration information, nonlinear least squares is used in laser SLAM to implement graph optimization SLAM and accurately locate the position of the quadrotor drone.

[0125] In this embodiment, three-dimensional point cloud data is collected at any observation point based on a three-dimensional laser sensor, and the point cloud data is segmented. Point cloud segmentation is based on feature points such as space, geometry, and texture. Point clouds in the same segmentation have similar features. The segmentation process further processes and analyzes each object to provide it with richer information. RANSAC is often used to iteratively estimate the parameters of the mathematical model from a set of observed data containing outliers.

[0126] In S21, the method for segmenting the point cloud data using the optimized RANSAC algorithm is specifically as follows:

[0127] Based on the set TIN segmentation strategy and flexibility consistency set threshold, the point cloud data is iteratively segmented to obtain segmented point cloud data;

[0128] Among them, the TIN (irregular triangulated network) segmentation strategy is as follows:

[0129] The triangle of TIN is used as the growth unit. According to the height difference between the structural units of obstacles in the corresponding environment, the corresponding constraint processing is performed in the case of occlusion. Then, the triangles with the same mark are grouped, and the area of ​​the point cloud data is segmented according to the grouped triangle area.

[0130] In this embodiment, the TIN segmentation strategy simulates the elevation change of the surface by connecting points scattered on the surface to form a triangular network when performing terrain analysis and 3D modeling. The slope of each triangle, that is, its angle with the horizontal plane, can reflect the slope change of the terrain. When processing TIN generated by obstacle point cloud, sudden elevation changes usually cause triangles located at the connection of structural primitives to have larger slopes.

[0131] Traverse all triangles in the TIN generated by the obstacle point cloud, determine whether their slopes exceed the limit, and mark them accordingly. This process can be described as:

[0132]

[0133] In the formula, Tri j Represents any triangle in the TIN generated by the obstacle point cloud, Tri j ∈D t , D t represents all triangles in the TIN generated by the obstacle point cloud, slope(·) represents the function for calculating the slope of the triangle, Slope max Indicates the slope threshold.

[0134] When using TIN to construct some special models in the terrain, you may encounter a situation where the height difference between some terrain structural primitives is large or the drone is at a low altitude. At this time, there may be occlusion between the structural primitives, resulting in the slope of some triangles generated in the TIN not accurately reflecting the slope changes of the actual terrain. The triangles in the occluded area may show a smaller slope value, so it is necessary to increase the height difference to constrain it. The specific expression for constraint processing is:

[0135]

[0136] In the formula, max(·) represents the maximum value function, min(·) represents the minimum value function, and t q1 Indicates the ground elevation at the first point in the triangle, t q2 Indicates the ground elevation at the second point in the triangle, t q3 Indicates the ground elevation at the third point in the triangle. Indicates the set height difference threshold, which is used to constrain the triangle;

[0137] After being represented by the TIN data structure, the surrounding area is represented by different segments. Each segment is composed of multiple groups of adjacent triangles in the original TIN. The segments are completely partitioned, and the boundary lines between the segments are also composed of the boundary lines of each triangle in the original TIN.

[0138] In this embodiment, based on the improvement of the fixed consistent set threshold in the traditional RANSAC algorithm, it is flexibly changed within a certain range according to the size of obstacles in the environment. A maximum threshold is first set to ensure that the fixed consistent set threshold after each iteration can correctly extract geometric primitives of all sizes under the threshold. Assuming that K iterations are performed, there are:

[0139] PN min =PN max (T factor ) K

[0140] In the formula, PN_S∈[PN min , PN max ], T factor ∈(0,1);

[0141] Iteration with PN S =PN max At the beginning, in each round of threshold update, a strict test is performed on the geometric primitives of the corresponding size. After a round of detection, a new threshold is iterated and the remaining sample environments are searched to detect the corresponding scale structure primitives. When the mth iteration reaches the maximum number of iterations, PN_S m =PN_S m-1 T factor The maximum number of iterations MaxIteration set by the flexibility consistent set threshold is expressed as follows:

[0142]

[0143] Where PN min Indicates the minimum number of points contained in the plane, PN max represents the maximum number of points contained in the plane, n is a positive integer, in this embodiment n = 1, used to ensure the integrity of the iteration effect, T factor is the shrinkage factor of the fixed consensus set threshold PN_S in the RANSAC algorithm, T factor ∈(0,1),PN S ∈[PN min ,PN max ].

[0144] In S22, the method for registering point cloud information using the 3D-NDT algorithm is specifically as follows:

[0145] The segmented point cloud data is divided into voxel cells of the same regular size, and their mean and covariance matrix are calculated. Normal distribution modeling is performed for each point in the voxel cell, and the point cloud space is represented as a set of normal distributions. The Hessian matrix method is used to optimize the registration of the probability values ​​of the normal distribution of the point cloud space to obtain the point cloud registration information.

[0146] In this embodiment, the point cloud space is represented as a set of normal distribution p(x) expressions as follows:

[0147]

[0148] In the formula, q represents the mean vector in the point cloud x in the point cloud space, and C represents the covariance matrix in the voxel unit cell in the point cloud x. The specific expressions of q and C are:

[0149]

[0150]

[0151] The point cloud in the voxel cell is evaluated as the sum of the probability distribution as the fractional value of each coordinate transformation parameter, and the probability value s(p) of the normal distribution of the point cloud space is obtained:

[0152]

[0153] In the formula, x i The point cloud x is mapped to the coordinates of the laser scanning frame according to the transformation parameter p. The required rotation axis and angle information in 3D rotation are as follows T(p,X):

[0154]

[0155] Where p is the transformation vector, p = [t|r|θ], and the translation vector t = [t x |t y |t z ], including the translation components in the x, y, and z axes, and the rotation axis vector r = [r x |r y |r z ], including the rotation axis vectors in the x, y, and z axis directions, θ is the rotation angle, then s = sinθ, c = cosθ, e = 1-cosθ.

[0156] The probability value s(p) of the normal distribution in the point cloud space is optimized by the Hessian matrix method, and f=s(p)=∑p(x) is set, and Newton's iteration method is used to minimize s(p).

[0157] H ij Δp=-δ i

[0158] In the formula, δ i represents the transposed gradient of f, H ij represents the Hessian matrix of f,

[0159] In S23, performing graph optimization SLAM by least squares includes the following sub-steps:

[0160] S231, construct the error function e of the linear system ij (x);

[0161]

[0162] Where, t i represents the coordinate value at the i node, t j represents the coordinate value at node j, t ij Represents the difference in coordinate values ​​between nodes i and j. Represents the transpose of the rotation matrix of node i, which describes the rotation of node i relative to the global coordinate system or other reference coordinate system. Similarly, represents the transpose of the rotation matrix from node i to node j, θ i represents the attitude angle at node i, θ j represents the attitude angle at node j, θ ij It represents the change of the attitude angle of node j relative to node i;

[0163] S232, using the error function to obtain partial derivatives of the horizontal axis coordinate values ​​of all nodes to obtain a Jacobian matrix, and using the Jacobian matrix to construct a sparse matrix and a dense vector;

[0164] Jacobian matrix J ij The specific expression is:

[0165]

[0166] In the formula, Represents the horizontal axis coordinate value x of the error function for node i i Find the partial derivative, Represents the horizontal axis coordinate value x of the error function for node j j Find partial derivatives;

[0167] The expressions of sparse matrix H and dense vector b are as follows:

[0168]

[0169]

[0170] Where C is C ij The block matrix composed of ij is the covariance matrix;

[0171] S233, solving the linear equation according to the sparse matrix and the dense vector and updating the position of the quadrotor drone, calculating the new sparse matrix and the dense vector according to the updated position of the quadrotor drone, iteratively solving the linear equation and updating the position of the quadrotor drone until the error between the current position and the updated position is less than the error threshold or the number of iterations reaches the maximum iteration value, thereby obtaining the optimized position of the quadrotor drone;

[0172] Among them, the updated quadrotor drone position P i The expression of the error ΔP between the current position and the updated position is as follows:

[0173] P i =P i ′ -H -1 b

[0174] ΔP=-H -1 b

[0175] Where P i ′ Indicates the position of the quadcopter before updating.

[0176] In this embodiment, graph optimization is performed based on the point cloud registration information to locate the position of the quadrotor drone, so as to achieve high-precision monitoring of the real-time position and attitude of the drone. The obtained 3D environmental model and accurate drone posture information are used in the decision-making process of the subsequent closed-loop control algorithm. By dynamically adjusting the control parameters and optimizing the closed-loop control strategy, the accuracy and stability of the drone when performing tasks are ensured.

[0177] The S3 is specifically:

[0178] The position of the quadrotor drone is obtained, and the attitude angle is obtained through the PID position control strategy. The attitude angle is used as the target, and the total disturbance of the quadrotor drone is compensated through the improved integral anti-disturbance control strategy to generate torque instructions to control the motion axes of the quadrotor drone, including the roll axis, pitch axis and yaw axis; maintain a stable flight attitude and respond quickly to external disturbances. The PID position control strategy of the inner and outer loops and the improved integral anti-disturbance control strategy are closely coordinated to improve the flexibility and robustness of the system and ensure the stability and accuracy of the drone under various conditions.

[0179] The present invention develops an improved integral self-disturbance rejection control (SNLADRCI) strategy, which is applied to the inner loop control of a UAV to ensure stable control of the UAV attitude.

[0180] Quadcopter drones have the characteristics of under-actuation, strong coupling, nonlinear randomness, high uncertainty, and sensitivity to external disturbances. The advantage of the adopted ADRC is that it can operate without relying on the precise mathematical model of the controlled object, and can achieve effective management of the controlled object by real-time estimation and compensation of internal and external disturbances of the system model. Even in the face of uncertain disturbances or changes in system parameters, this controller can still maintain excellent control performance and show excellent robustness. In other words, the ADRC ensures the adaptability and stability of the system dynamics through its inherent disturbance observation and compensation mechanism, and can maintain efficient control effects even under variable operating conditions or changes in system characteristics. However, the traditional nonlinear ADRC (NLADRC) has the phenomenon of partial first-order derivative discontinuity, which will greatly affect the observation effect of the observer. At the same time, it has always been a huge challenge for the adjustment of nonlinear control parameters. In order to overcome these challenges, the present invention introduces a new type of nonlinear function to solve these problems. In addition, in response to the static error that may occur in the traditional nonlinear anti-disturbance controller in the experiment, the present invention innovatively adds an integral term to the nonlinear error feedback control law (NLSEF) to form an integral error feedback control law (NLSEFI), thereby effectively eliminating the static error and improving the accuracy of system control.

[0181] like Figure 4 As shown, in this embodiment, the improved integral active disturbance rejection controller strategy includes a tracking differentiator (TD), an improved extended state observer (SNLESO), and an integral error feedback control law (NLSEFI);

[0182] The expression of the tracking differentiator is specifically:

[0183]

[0184] In the formula, v1(k+1) represents the tracking input signal at time k+1, v1(k) represents the tracking input signal at time k, and v d (·) represents the expected signal of the tracking input, v2(k+1) represents the first-order derivative of v1(k+1), v2(k) represents the first-order derivative of v1(k), u1 represents the second-order derivative of v1(k), h represents the discrete sampling time, r represents the speed factor that determines the tracking speed, and fst(·) represents the fastest tracking function;

[0185] In this embodiment, the transition process is arranged to extract the target tracking signal differential, and the PID control may cause the system to overshoot due to the large initial deviation. In order to improve the tracking performance of the target signal, the present invention uses a tracking differentiator to smooth the transition and reduce this overshoot.

[0186] The expression of the improved extended state observer is as follows:

[0187]

[0188] Where y(k) represents the output of the quadrotor UAV system, z1(k) represents the estimated value of the system input reference value x1 by the improved extended state observer at time k, ε1 represents the estimated error of the improved extended state observer for the system input reference value x1, z1(k+1) represents the estimated value of the system input reference value x1 by the improved extended state observer at time k+1, z2(k) represents the estimated value of the derivative x2 of the input reference value by the improved extended state observer at time k, z2(k+1) represents the estimated value of the derivative x2 of the input reference value by the improved extended state observer at time k+1, z3(k) represents the estimated value of the system disturbance x3 by the improved extended state observer at time k, z3(k+1) represents the estimated value of the system disturbance x3 by the improved extended state observer at time k+1, β 01 represents the system input error feedback gain, α1 is the power of the nonlinear part of the sfal(·) function, β 02 represents the system input differential error feedback gain, α2 is the power of the nonlinear part of the sfal(·) function, sfal(·) represents the improved fal function, and its specific expression is sfal(e,α)=sign(e)|e| a , β 03 represents the system disturbance error feedback gain, h is the discrete sampling time, b is the control channel gain, and u(k) represents the function fal(e, α, δ) used in the design of the traditional extended state observer (ESO) at time k. The first-order derivative of the function fal(e, α, δ) is discontinuous at the segmentation point ±δ, which will weaken the observation ability of the ESO to a certain extent. In this embodiment, the present invention proposes an improved fal(·) function sfal(e, a) to address this problem, as shown in Figure 5The figure shows the comparison between the function sfal(e,a) and the function fal(e,α,δ) at δ=0.05 and α=0.5. The function sfal(e,a) removes the segmentation points and ensures the continuity of the first-order derivative in the entire domain. The sfal(·) function not only follows the natural trend of the function change, but also has certain filtering characteristics. In addition, the parameter tuning problem in nonlinear control has always been a technical challenge. By reducing one parameter, the introduction of the sfal(·) function not only reduces the complexity of parameter adjustment, but also improves the flexibility and efficiency of control system design.

[0189] The expression of the integral error feedback control law is as follows:

[0190] u2=β0sfal(e0,α3)+β1sfal(e1,α4)+β2sat(c,d)

[0191] In the formula, u2 represents the control quantity, e0 represents the system input error, e1 represents the system input differential error, β0 represents the system input nonlinear feedback gain, β1 represents the system input differential term nonlinear feedback gain, β2 represents the system input integral term nonlinear feedback gain, sat(·) represents the saturation function, c represents the integral term, c=e1(k+1)+e1(k), e1(k+1) represents the system input error at time k+1, e1(k) represents the system input differential error at time k, and d represents the segmentation of the sat(·) function. The specific expression of sat(c,d) is:

[0192]

[0193] In this embodiment, the method for calculating the moment command of the roll axis, pitch axis and yaw axis of the present invention adopts the improved integral type active disturbance rejection controller strategy to realize the control of the aerial dexterous maneuvering system along the body coordinate system. b x b , o b y b , o b z b The angle of the axis is controlled, and the force F of the robot arm is estimated and compensated in real time. ′ The lower edge o of the UAV body coordinate system b x b , o b y b , o b z b The disturbance caused by the axis.

[0194] In this embodiment, the pitch angle θ is taken as an example, and the process of calculating the torque command of the pitch angle by using the improved integral active disturbance rejection controller strategy is as follows:

[0195] (1) Tracking Differentiator

[0196] In the outer loop position control system, the tracking differentiator uses the pitch angle as the tracking input signal and first calculates the desired pitch angle value θ d Then, this expected value is passed to the pitch angle tracking differentiator in the inner loop attitude control system. Inside the tracking differentiator, the expected pitch angle θ is d After processing, two outputs are generated: one is the tracking signal θ1 of the desired pitch angle, and the other is the derivative of the tracking signal, that is, the differential signal θ2. The mathematical expression of the tracking differentiator describes this conversion process in detail:

[0197]

[0198] Wherein, θ1(k+1) and θ1(k) represent the tracked pitch angles at time k+1 and time k, θ2(k+1) and θ2(k) represent the first-order derivatives of θ1(k+1) and θ1(k), and u1 represents the second-order derivative.

[0199] (2) Improved Extended State Observer

[0200] First, determine the deviation between the expected tracking value and the actual value of the pitch angle θ, that is, the error ε θ Next, construct the estimated value of the state quantity z θ1 To monitor the pitch angle error, the estimated value of the state quantity z θ2 Used to track the rate of change of the error; in addition, the extended state quantity estimate z is introduced θ3 To estimate the overall disturbance of the system. Based on the observation of these state quantities, an improved extended state observer is designed. By establishing a set of state quantities to continuously monitor and estimate the tracking error of the pitch angle and its dynamic changes, as well as the disturbance in the system, an extended state observer equation for improving the observation capability of nonlinear systems is formed as follows:

[0201]

[0202] Among them, β θ01 ,β θ02 ,β θ03 is the pitch angle error feedback gain, which affects the convergence speed of ESO. α1 and α2 are the powers of the nonlinear part of the improved SFAL function. Generally, α1=0.5 and α2=0.25 are taken. b is the control channel gain, which determines the range of variation of the total disturbance estimate.

[0203] (3) Integral error feedback control law

[0204] In the ADRC, the nonlinear error feedback law acts as an independent nonlinear control unit. Based on the tracking signal θ1 and its differential signal θ2 output by the tracking differentiator, combined with the state quantity estimate z provided by the extended state observer, θ1 、z θ2 , and calculate the corresponding error signal. Based on these error signals, the control module dynamically controls the system and compensates for the total disturbance of the system in real time. In order to solve the static errors that may occur in the actual measurement process, the controller specifically introduces a nonlinear integral term. Finally, a nonlinear error feedback control law with an integral function is formed, and its discretized form defines the implementation method of the controller in detail. The discretized form of the nonlinear error feedback control law with an integral function is as follows:

[0205]

[0206] Among them, β θ1 , β θ2 are the nonlinear feedback proportional factor and differential gain respectively. The improved sfal(·) function is the nonlinear function of the simplified fal(·). θ0 (k), e θ1 (k) are the system input error of the pitch angle and its differential error, α3 and α4 are the powers of the nonlinear part of the improved sfal(·) function, generally α3 = 0.5 and α4 = 0.25, sat is a saturation function, c is the integral term, c = e1(k+1)+e1(k); d is the segmentation of the sat function.

[0207] The calculation algorithm of the torque command of the roll angle and yaw angle is the same as the calculation principle of the torque command of the pitch angle mentioned above. Therefore, for the calculation design of the torque command of the roll angle and yaw angle, the design principles and steps of the torque command of the pitch angle can be referred to, including error calculation, state quantity observation, disturbance compensation and application of nonlinear control law, so as to achieve the coordination and efficient operation of the entire UAV attitude control system. Since these design processes follow similar control logic, the controller design of each channel will not be described repeatedly.

[0208] The S4 comprises the following sub-steps:

[0209] S41, using the control efficiency model to calculate the square of the propeller speed required by the quadrotor drone according to the torque command and the lift, where the torque command is specifically the total torque vector on the quadrotor drone body coordinate system b τ=[ b τ x , b τ y , b τ z ] T ;

[0210] The expression of the control efficiency model is as follows:

[0211]

[0212] In the formula, b F represents the lift of the quadrotor drone, C F is the lift coefficient, C M represents the moment coefficient, d represents half of the frame wheelbase length, represents the square of the speed of the i-th propeller, i=1,2,3,4;

[0213] In this embodiment, the power distribution of the quad-rotor UAV can be achieved by inverting the square of the rotation speed required by the four propellers of the aerial dexterous maneuvering system according to the lift and torque expected by the quad-rotor UAV.

[0214] S42, calculating the corresponding throttle control signal of the propeller according to the square of the propeller speed through the power unit model, thereby completing the control of the quadrotor drone;

[0215] The expression of the power unit model is as follows:

[0216]

[0217] Where, σ represents the throttle control signal, C R Indicates the proportional coefficient of the throttle control signal converted into the motor speed. Indicates the base speed offset in radians per second, T m represents the motor response time constant, Indicates the propeller speed. Finally, the throttle command is generated by the torque and lift, and the final result is as follows Figure 6 The overall control structure block diagram of the system is shown.

[0218] In this embodiment, the precise positioning and attitude monitoring of the aerial dexterous operation platform is achieved by adopting a series of sensor fusion technologies, including laser radar, IMU, etc. The surrounding environment model and the real-time position and attitude data of the drone are provided, which are used to guide the decision-making of the control system and improve the control accuracy and flight stability of the drone by continuously optimizing the control parameters.

[0219] During flight, the drone continuously provides real-time feedback of its position and attitude to the control system. This feedback information is an integral part of the closed-loop control system, and is used to adjust and optimize the control input to ensure that the drone can accurately perform its intended mission. Through this real-time feedback and adjustment mechanism, the drone's control system can achieve a high degree of automation and intelligence, thereby maintaining optimal performance under various flight conditions.

[0220] The present invention also conducts safety and stability tests, which comprehensively tests the system in the Gazebo simulation environment to verify the safety and stability of the drone in a simulated variable operating environment. Figure 7 The figure shows the system Gazebo environment simulation model test. Based on the basic theoretical analysis of S1-S4, the overall system test includes not only conventional flight operations and aerial operations, but also extreme situations and emergency responses to ensure that the system can maintain stable operation in the face of unexpected situations. In this stage, the performance of the control algorithm is evaluated. The test results will be used to fine-tune the algorithm parameters to optimize the system response and improve the control accuracy. In addition, the tuning process also includes enhancing the robustness of the algorithm, using PID to control the movement of the robotic arm, and simulating the external interference of the robotic arm on the drone to adapt to possible model uncertainties and external disturbances.

[0221] In the presence of external interference, especially when the robot arm performs aerial operations, the SNLADRCI controller can provide a more stable and reliable flight state, significantly enhancing the system's anti-interference performance. Through detailed flight log data evaluation, such as Figures 8-9 As shown in the figure, the design of the improved SNLADRCI controller takes into account that the actual application is different from the theory, and there is a certain static error. A nonlinear integral term is added to eliminate this static error. At the same time, the improved smooth nonlinear function sfal(e,a) is used to make the system control smoother, ensuring the observation performance of SNLESO and reducing the difficulty of parameter adjustment.

[0222] In the description of the present invention, it should be understood that the orientation or positional relationship indicated by the terms "center", "upper", "lower", "horizontal", "top", "bottom", "inner", "outer", "radial", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention. In addition, the terms "first", "second", and "third" are used for descriptive purposes only, and cannot be understood as indicating or implying the relative importance or the number of implicitly specified technical features. Therefore, the features defined by "first", "second", and "third" may explicitly or implicitly include one or more of the features.

Claims

1. The anti-disturbance control method of the airborne mutual control system based on laser sensing is characterized by: The following steps are involved: S1. Establish the dynamic model and kinematic model of the quadrotor drone and model the system; S2. Based on the modeled system, collect point cloud data around the flight robot arm, segment and register the point cloud data, and use the processed point cloud data to perform graph optimization and accurately locate the position of the quadcopter; S3, through the PID position control series improved integral anti-disturbance attitude control strategy, according to the position of the quadrotor drone, generate the torque command to control the motion axis of the quadrotor drone; S4. Calculate the corresponding throttle control signal of the propeller according to the lift and torque instructions of the quadrotor drone to complete the control of the quadrotor drone.

2. The method for anti-disturbance control of an airborne interactive control system based on laser sensing according to claim 1, characterized in that: In S1, the dynamic model includes a position kinematics model and a posture kinematics model; Among them, the expression of the position kinematic model is specifically: In the formula, Indicates the position of the quadcopter in the world coordinate system, w x b , w y b and w z b They represent the quadcopter along o in the world coordinate system. w x w , o w y w and w z w The position of the axis, w v b Represents the linear velocity of the quadcopter in the world coordinate system, and They represent the quadcopter along o in the world coordinate system. w x w , o w y w and w z w Linear speed of the axis; The expression of the posture kinematics model is as follows: In the formula, represents the attitude change rate, and They represent the angular velocity of the three axes in the body coordinate system, φ b ,θ b and ψ b They represent the angles on the three axes in the body coordinate system. b ω represents the angular velocity of the body, b ω=[ b ω x , b ω y , b ω z ] T , b ω x , b ω y and b ω z They represent the quadcopter along o in the world coordinate system. b x b , o b y b and b z b Angular velocity of the axis; The attitude kinematics model includes the position dynamics model and the attitude dynamics model; Among them, the expression of the position dynamics model is specifically: In the formula, φ represents the roll angle of the aircraft, θ represents the pitch angle of the aircraft, ψ represents the yaw angle of the aircraft, and m represents the mass of the aircraft. b F represents the total amount of lift generated by the propeller in the body coordinate system, g represents the acceleration due to gravity, and Respectively represent the acceleration of the three axes in the world coordinate system; The expression of the posture dynamics model is as follows: Where J represents the total moment of inertia of the quadrotor drone, J = diag[J xx ,J yy ,J zz ], J xx represents the first moment of inertia, J yy represents the second moment of inertia, J zz represents the third moment of inertia, diag[·] represents the symbol of a diagonal matrix, which is used to represent a matrix with zero elements outside the main diagonal. b ω is the angular velocity of the quadrotor drone in the body coordinate system, represents the angular acceleration of the quadrotor drone in the body coordinate system, b τ represents the total torque vector on the quadrotor drone body coordinate system, b τ=[ b τ x , b τ y , b τ z ] T , b τ x Indicates along o b x b The torque vector about the axis, b τ y Indicates along o b y b The torque vector about the axis, b τ z Indicates along o b z b The moment vector about the axis. T represents the transpose sign.

3. The method for anti-disturbance control of an airborne mutual manipulation system based on laser sensing according to claim 1, characterized in that: The S2 comprises the following sub-steps: S21, collecting point cloud data around the flight robot arm, and segmenting the point cloud data using an optimized RANSAC algorithm; S22, spatially registering the segmented point cloud data using a 3D-NDT algorithm to obtain point cloud registration information; S23. Based on the point cloud registration information, nonlinear least squares is used in laser SLAM to implement graph optimization SLAM and accurately locate the position of the quadrotor drone.

4. The method for anti-disturbance control of an airborne interactive control system based on laser sensing according to claim 3 is characterized in that: In S21, the method for segmenting the point cloud data using the optimized RANSAC algorithm is specifically as follows: Based on the set TIN segmentation strategy and flexibility consistency set threshold, the point cloud data is iteratively segmented to obtain segmented point cloud data; Among them, the TIN segmentation strategy is as follows: The triangle of TIN is used as the growth unit. According to the height difference between the structural units of obstacles in the corresponding environment, the corresponding constraint processing is performed in the case of occlusion. Then, the triangles with the same mark are grouped, and the area of ​​the point cloud data is segmented according to the grouped triangle area. The specific expression of constraint processing is: In the formula, max(·) represents the maximum value function, min(·) represents the minimum value function, and t q1 Indicates the ground elevation at the first point in the triangle, t q2 Indicates the ground elevation at the second point in the triangle, t q3 Indicates the ground elevation at the third point in the triangle. Represents the height difference threshold, which is used to constrain the triangle; The expression of the maximum number of iterations MaxIteration for setting the flexibility consistent set threshold is as follows: Where PN min Indicates the minimum number of points contained in the plane, PN max Indicates the maximum number of points contained in the plane. n is a positive integer used to ensure the integrity of the iteration effect. T factor is the shrinkage factor of the fixed consensus set threshold PN_S in the RANSAC algorithm, PN S ∈[PN min ,PN max ].

5. The method for anti-disturbance control of an airborne mutual manipulation system based on laser sensing according to claim 4 is characterized in that: In S22, the method for registering point cloud information using the 3D-NDT algorithm is specifically as follows: The segmented point cloud data is divided into voxel cells of the same regular size, and their mean and covariance matrix are calculated. Normal distribution modeling is performed for each point in the voxel cell, and the point cloud space is represented as a set of normal distributions. The Hessian matrix method is used to optimize the registration of the probability values ​​of the normal distribution of the point cloud space to obtain the point cloud registration information.

6. The method for anti-disturbance control of an airborne mutual manipulation system based on laser sensing according to claim 5 is characterized in that: In S23, performing graph optimization SLAM by least squares includes the following sub-steps: S231, construct the error function e of the linear system ij (x); Where, t i represents the coordinate value at the i node, t j represents the coordinate value at node j, t ij Represents the difference in coordinate values ​​between nodes i and j. represents the transpose of the rotation matrix of node i, describing the rotation of node i relative to the global coordinate system, represents the transpose of the rotation matrix from node i to node j, θ i represents the attitude angle at node i, θ j represents the attitude angle at node j, θ ij It represents the change of the attitude angle of node j relative to node i; S232, calculating partial derivatives of the horizontal axis coordinate values ​​of all nodes through the error function to obtain the Jacobian matrix, and constructing a sparse matrix and a dense vector through the Jacobian matrix; Jacobian matrix J ij The specific expression is: In the formula, Represents the horizontal coordinate value x of the error function for node i i Find the partial derivative, Represents the horizontal axis coordinate value x of the error function for node j j Find partial derivatives; The expressions of sparse matrix H and dense vector b are as follows: Where C is C ij The block matrix composed of ij is the covariance matrix; S233, solving the linear equation according to the sparse matrix and the dense vector and updating the position of the quadrotor drone, calculating the new sparse matrix and the dense vector according to the updated position of the quadrotor drone, iteratively solving the linear equation and updating the position of the quadrotor drone until the error between the current position and the updated position is less than the error threshold or the number of iterations reaches the maximum iteration value, thereby obtaining the optimized position of the quadrotor drone; Among them, the updated quadrotor drone position P i The expression of the error ΔP between the current position and the updated position is as follows: P i =P i ′ -H -1 b ΔP=-H -1 b Where P i ′ Indicates the position of the quadcopter before updating.

7. The method for anti-disturbance control of an airborne mutual manipulation system based on laser sensing according to claim 6 is characterized in that: The S3 is specifically: The position of the quadrotor drone is obtained, and the attitude angle is obtained through the PID position control strategy. The attitude angle is used as the target, and the total disturbance of the quadrotor drone is compensated through the improved integral anti-disturbance controller strategy to generate the torque command for controlling the motion axes of the quadrotor drone, including the roll axis, pitch axis and yaw axis. Among them, the improved integral ADRC strategy includes tracking differentiator, improved extended state observer, and integral error feedback control law; The expression of the tracking differentiator is specifically: In the formula, v1(k+1) represents the tracking input signal at time k+1, v1(k) represents the tracking input signal at time k, and v d (·) represents the expected signal of the tracking input, v2(k+1) represents the first-order derivative of v1(k+1), v2(k) represents the first-order derivative of v1(k), u1 represents the second-order derivative of v1(k), h represents the discrete sampling time, r represents the speed factor that determines the tracking speed, and fst(·) represents the fastest tracking function; The expression of the improved extended state observer is as follows: Where y(k) represents the output of the quadrotor UAV system, z1(k) represents the estimated value of the system input reference value x1 by the improved extended state observer at time k, ε1 represents the estimated error of the improved extended state observer for the system input reference value x1, z1(k+1) represents the estimated value of the system input reference value x1 by the improved extended state observer at time k+1, z2(k) represents the estimated value of the derivative x2 of the input reference value by the improved extended state observer at time k, z2(k+1) represents the estimated value of the derivative x2 of the input reference value by the improved extended state observer at time k+1, z3(k) represents the estimated value of the system disturbance x3 by the improved extended state observer at time k, z3(k+1) represents the estimated value of the system disturbance x3 by the improved extended state observer at time k+1, β 01 represents the system input error feedback gain, α1 is the power of the nonlinear part of the sfal(·) function, β 02 represents the system input differential error feedback gain, α2 is the power of the nonlinear part of the sfal(·) function, sfal(·) represents the improved fal function, and its specific expression is sfal(e, α) = sign(e)|e| a , β 03 represents the system disturbance error feedback gain, h is the discrete sampling time, b is the control channel gain, and u(k) represents the control input at time k; The expression of the integral error feedback control law is as follows: u2=β0sfal(e0,α3)+β1sfal(e1,α4)+β2sat(c,d) In the formula, u2 represents the control quantity, e0 represents the system input error, e1 represents the system input differential error, β0 represents the system input nonlinear feedback gain, β1 represents the system input differential term nonlinear feedback gain, β2 represents the system input integral term nonlinear feedback gain, sat(·) represents the saturation function, c represents the integral term, c=e1(k+1)+e1(k), e1(k+1) represents the system input error at time k+1, e1(k) represents the system input differential error at time k, and d represents the segmentation of the sat(·) function.

8. The method for anti-disturbance control of an airborne mutual manipulation system based on laser sensing according to claim 7 is characterized in that: The S4 comprises the following sub-steps: S41, using the control efficiency model to calculate the square of the propeller speed required by the quadrotor drone according to the torque command and the lift, where the torque command is specifically the total torque vector on the quadrotor drone body coordinate system b τ=[ b τ x , b τ y , b τ z ] T ; The expression of the control efficiency model is as follows: In the formula, b F represents the lift of the quadrotor drone, C F is the lift coefficient, C M represents the moment coefficient, d represents half of the frame wheelbase length, represents the square of the speed of the i-th propeller, i = 1, 2, 3, 4; S42, calculating the corresponding throttle control signal of the propeller according to the square of the propeller speed through the power unit model, thereby completing the control of the quadrotor drone; The expression of the power unit model is as follows: Where, σ represents the throttle control signal, C R Indicates the proportional coefficient of the throttle control signal converted into the motor speed. Indicates the basic speed bias, T m represents the motor response time constant, Indicates propeller speed.

Images