Measurement and control method of four-rotor inverted pendulum system based on visual sensor

Abstract

The invention discloses a measurement and control method of a four-rotor inverted pendulum system based on a visual sensor, and relates to the field of pose measurement and control of space inverted pendulums. The problems that in an existing four-rotor inverted pendulum system, measurement of the space position of a swing rod depends on external optical motion capture equipment, and the cost of the four-rotor inverted pendulum system is extremely high due to the expensive optical motion capture equipment are solved. The method comprises the following steps: modeling a four-rotor inverted pendulum system, and respectively placing a monocular camera in an X-axis positive direction and a Y-axis positive direction of a four-rotor coordinate system; the tilt angle of the swing rod is calculated by using a swing rod motion image captured by a monocular camera and adopting Hough transform, and the tilt angle of the swing rod measured by a visual algorithm is compensated; the spatial position of the swing rod relative to a swing rod coordinate system is calculated, the four-rotor position and the swing rod position are subjected to dual control in a form of cascading a four-rotor position control loop and a swing rod position control loop, and a multi-point differential improved PID is used as a swing rod position loop controller.

Description

Technical Field

[0001] The present invention relates to the technical field of position and posture measurement and control of a space inverted pendulum, and in particular to a measurement and control method of a quadrotor inverted pendulum system based on a visual sensor. Background Art

[0002] As an inverted pendulum system is naturally unstable, researchers need to design an effective and complete control strategy to achieve stable control of the inverted pendulum. In real life, inverted pendulums are used in applications ranging from the attitude control of rockets to the balancing cars people use to commute to work. The quadcopter inverted pendulum system uses a quadcopter as a power device, with a pendulum placed on the quadcopter. The pendulum is balanced by controlling the movement of the quadcopter. It has broad application prospects in drone manned transportation and other aspects.

[0003] However, in a quadrotor inverted pendulum system, the spatial position measurement of the pendulum often relies on external optical motion capture equipment. The expensive optical motion capture equipment makes the research cost of the quadrotor inverted pendulum system extremely high. With the widespread use of visual sensors and the increasing richness of visual algorithms, the measurement of the spatial position of the inverted pendulum based on industrial-grade visual sensors can effectively lower the research threshold of the quadrotor inverted pendulum system, which is of great value and significance for the verification of control theory methods and education and teaching.

[0004] The Hough transform method has good stability and measurement accuracy in realizing the inclination detection of the pendulum. Hough transform is an image processing technology that is mainly used to detect geometric shapes in images, especially straight lines. In the Cartesian coordinate system, a straight line is usually determined by two points A = (X1, Y1) and B = (X2, Y2), or represented by a slope and an intercept (y = kx + b). However, in the Hough transform, the representation of the straight line is transformed. In the Hough space, a point is equivalent to a straight line in the Cartesian coordinate system, and vice versa. Specifically, a straight line in the Cartesian coordinate system corresponds to a point in the Hough space; and a point in the Cartesian coordinate system corresponds to a straight line in the Hough space. When multiple points in the Cartesian coordinate system are collinear, the straight lines corresponding to these points in the Hough space will intersect at one point. This feature enables the Hough transform to effectively detect straight lines in images. In order to deal with the situation where the slope of the straight line does not exist (i.e., a vertical line), the Hough transform is usually represented in polar coordinates. In the polar coordinate system, a point is determined by the distance ρ and the angle θ. In this way, no matter what the slope of the line is, the corresponding representation can be found in the Hough space. The algorithm steps of the Hough transform mainly include the following 5 steps. First, edge detection is performed on the image to extract the edge points in the image. Then the edge points are Hough transformed and represented in the Hough space. Then in the Hough space, an accumulator array is used to record the accumulated value of each point (that is, the representation of each line in the Hough space). The size of the accumulated value reflects the number of points in the Cartesian coordinate system corresponding to the line in the Hough space of the point. Then, by detecting the peak value in the accumulator array, the corresponding line in the Hough space can be determined. These peak points represent the straight lines in the image. Finally, according to the ρ and θ values ​​corresponding to the peak points, the corresponding straight lines can be drawn in the Cartesian coordinate system.

[0005] The first step of the Hough transform is to detect the edge of the image. In the field of image processing, edge detection is a crucial step that helps to identify shapes, objects, and features in the image. However, early edge detection algorithms, such as edge operators based on first-order derivatives or second-order derivatives, although simple and easy to use, are easily affected by noise and easily produce false edges. In order to overcome these defects, Canny proposed the Canny edge detection algorithm, which combines the gradient calculation method and non-maximum suppression technology to effectively suppress noise while maintaining edge information and detecting small edges. The core steps of the Canny edge detection operator include:

[0006] Grayscale conversion: The Canny operator can only process single-channel grayscale images, so before edge detection, the original image needs to be converted into a grayscale image.

[0007] Filtering and noise reduction: Real images often contain a lot of noise information, such as salt and pepper noise and Gaussian noise, due to various reasons such as acquisition equipment and environmental interference. The Canny operator uses Gaussian filtering to remove these noises to avoid misidentifying erroneous noise information as edges during subsequent edge detection. Gaussian filtering filters out the noise part of the image through convolution operations while retaining the edge information of the image.

[0008] Calculate gradient: An edge is a place in an image where the brightness or color changes significantly, so the edge can be detected by calculating the image gradient. The Canny operator uses first-order finite differences to calculate the gradient, and the Sobel operator is usually used as the gradient operator. The Sobel operator can calculate the partial derivatives of the image in the x and y directions to obtain the gradient magnitude and direction.

[0009] Non-maximum suppression: After the gradient is calculated, each point in the image has a gradient magnitude and direction. The purpose of non-maximum suppression is to retain the local maximum in the gradient direction and suppress other non-maximum points, thereby refining the edge. In specific implementation, for each point, its gradient value is compared with the gradient values ​​on both sides of the gradient direction. If it is not a local maximum, it is set to 0.

[0010] Double threshold detection: After non-maximum suppression, there may still be some false edges caused by noise in the image. The Canny operator uses double thresholds to further screen edge points. The high threshold is used to determine strong edge points, which are more likely to be true edges; the low threshold is used to determine weak edge points, which may be true edges or caused by noise or color changes. If the gradient value of a point is greater than the high threshold, it is considered a strong edge point; if the gradient value is between the low threshold and the high threshold, it is considered a weak edge point; if the gradient value is less than the low threshold, it is considered a non-edge point.

[0011] Hysteresis connection: Hysteresis connection is an important step in the Canny operator, which is used to connect weak edge points to strong edge points, thereby reducing edge breaks. In specific implementation, for each weak edge point, a strong edge point is searched within its 8-neighborhood. If a strong edge point is found, the weak edge point is retained as an edge point; otherwise, it is suppressed as a non-edge point.

[0012] In terms of quadrotor pose measurement, equipping the quadrotor with a camera to collect visual information of the indoor environment can solve the problem of inaccurate indoor GPS information and realize the pose estimation of the quadrotor in the indoor environment. VINS-Fusion is the most highly ranked open source binocular VIO solution in the current KITTI Visual Odometry list. It is a binocular visual inertial navigation SLAM solution disclosed in the prior art after VINS-Mono and VINS-Mobile (monocular visual inertial navigation SLAM solution). VINS-Fusion is an optimized multi-sensor state estimator that can achieve accurate self-positioning of autonomous applications (drones, cars and AR / VR). VINS-Fusion is an extension of VINS-Mono and supports multiple visual inertial sensor types. The key point is that it supports static initialization of binocular cameras. In the gazebo simulation environment, controlling the movement of the quadrotor must be based on the normal operation of the sensor. Among many open source SLAM solutions, VINS-Fusion supports static initialization of camera parameters, which well meets the requirements of the gazebo simulation environment. At the same time, its measurement solution based on multi-sensor data fusion can more accurately obtain the quadrotor's posture data information. Therefore, the quadrotor's posture measurement adopts the VINS-Fusion open source SLAM solution.

[0013] In summary, in a quadrotor inverted pendulum system, there are problems such as the spatial position measurement of the pendulum often relies on external optical motion capture equipment. The expensive optical motion capture equipment makes the research cost of the quadrotor inverted pendulum system extremely high. Summary of the invention

[0014] The present invention aims to solve the problems in the prior art that in a quadrotor inverted pendulum system, the spatial position measurement of the pendulum rod often relies on an external optical motion capture device, and the expensive optical motion capture device makes the research cost of the quadrotor inverted pendulum system extremely high.

[0015] To solve the above technical problems, the present invention is achieved through the following technical solutions:

[0016] Solution 1: The present invention proposes a method for measuring and controlling a quadrotor inverted pendulum system based on a visual sensor, the method comprising the following steps:

[0017] Step 1: Model the quadrotor inverted pendulum system to obtain a dynamic model of the quadrotor inverted pendulum, and place a monocular camera in the positive direction of the X-axis and the positive direction of the Y-axis of the quadrotor coordinate system respectively;

[0018] Step 2: When the camera optical axis and the pendulum fulcrum are collinear, the pendulum motion image captured by the monocular camera is used to calculate the pendulum inclination angle using the Hough transform to restore the spatial position of the pendulum relative to the quadrotor coordinate system;

[0019] Step 3, respectively using the visual SLAM algorithm and the Hough transform algorithm to obtain the attitude angle and the pendulum inclination angle of the dynamic model of the quadrotor inverted pendulum, and compensate the pendulum inclination angle measured by the visual algorithm;

[0020] Step 4, calculate the spatial position of the pendulum relative to the pendulum coordinate system, that is, complete the measurement of the spatial pendulum position in the quadrotor inverted pendulum system;

[0021] Step 5: Dual control of the quadrotor position and the pendulum position is achieved by cascading the quadrotor position control loop and the pendulum position control loop, and an improved PID based on multi-point differential is used as the pendulum position loop controller to complete the measurement and control of the quadrotor inverted pendulum system.

[0022] Further, a preferred embodiment is provided, in which the dynamic model of the quadrotor inverted pendulum described in step 1 is:

[0023]

[0024] Among them, f r 、f s 、f x 、f y is the disturbance, and the quadrotor is defined to rotate around X w , Y w , Z w The axis rotation angles are the roll angle φ, the pitch angle θ, and the yaw angle ψ. The coordinates of the center of mass of the pendulum in the pendulum coordinate system are (r, s, ζ), and g is the gravitational acceleration.

[0025] Furthermore, a preferred embodiment is provided, which includes the steps of establishing a model from the swing bar position input to the quadrotor position output and from the quadrotor attitude input to the swing bar position output based on the dynamic model of the quadrotor inverted pendulum obtained in step 1.

[0026] Further, a preferred implementation is provided, in step 3, the method of using the visual SLAM algorithm and the Hough transform algorithm to obtain the attitude angle and the inclination angle of the swing arm of the dynamic model of the quadrotor inverted pendulum is:

[0027] The monocular camera collects images and determines the time synchronization of the images collected by the two cameras;

[0028] Convert two frames of images that meet the time synchronization requirements into grayscale images;

[0029] The Canny edge detection method is used to process the grayscale image and extract the edge of the pendulum;

[0030] Hough transform is used to detect the straight line at the edge of the pendulum and obtain the pendulum inclination angle in the quadrotor coordinate system;

[0031] The VINS-Fusion visual measurement algorithm is used to obtain the attitude angle of the quadrotor.

[0032] Furthermore, a preferred embodiment is provided, in which the method for compensating the pendulum arm inclination angle α measured by the visual algorithm in step 3 is to complete error-free pendulum arm inclination angle compensation by rotating around a fixed coordinate system as a rotation axis.

[0033] Further, a preferred embodiment is provided, in which the method for calculating the spatial position of the pendulum relative to the pendulum coordinate system in step 4 is:

[0034] The quadrotor attitude angle measured by the VINS-Fusion vision algorithm is used to compensate the pendulum inclination angle calculated by the Hough transform algorithm, and finally the pendulum position r and s under the established dynamic model are solved.

[0035] Further, a preferred implementation is provided, in which the method of using the improved PID based on multi-point differential as the swing arm position loop controller in step 5 is:

[0036]

[0037] Wherein, u(k) represents the differential term of the control quantity in the improved PID controller at the kth moment, e(ki) represents the error value input to the controller at the kith moment, N ≥ 2, and N is an even number.

[0038] Solution 2: A computer device includes a memory and a processor, wherein the memory stores a computer program. When the processor runs the computer program stored in the memory, the processor executes any one of the methods described in Solution 1.

[0039] Solution three: a computer-readable storage medium storing a computer program, wherein the computer program, when executed by a processor, implements the steps of the method described in any one of Solution one.

[0040] The present invention is beneficial in that:

[0041] The present invention provides a method for measuring and controlling a quadrotor inverted pendulum system based on a visual sensor.

[0042] In terms of the position measurement of a spatial pendulum, the present invention proposes a spatial pendulum position measurement scheme based on a monocular camera. A monocular camera is placed respectively in the positive direction of the X-axis and the positive direction of the Y-axis of the quadrotor coordinate system. While ensuring that the optical axis of the camera is collinear with the pendulum fulcrum, the pendulum motion image captured by the camera is used to solve the inclination angle of the pendulum using the Hough transform, thereby restoring the spatial position of the pendulum relative to the quadrotor coordinate system, and then the attitude angle of the quadrotor is obtained to compensate for the inclination angle of the pendulum measured by the visual algorithm, and finally the spatial position of the pendulum relative to the pendulum coordinate system is solved, thereby realizing the spatial pendulum position measurement in the quadrotor inverted pendulum system with low cost.

[0043] In the control scheme design of the quadrotor inverted pendulum system, the quadrotor position control loop and the pendulum position control loop are cascaded to achieve dual control of the quadrotor position and the pendulum position. In view of the measurement noise introduced by the visual measurement end, an improved PID based on multi-point differential is adopted as the pendulum position loop controller, which effectively suppresses the influence of the measurement noise at the visual measurement end on the system, and finally achieves dual control of the quadrotor position and the pendulum position.

[0044] The present invention also relates to the field of a spatial inverted pendulum posture measurement and control method based on a visual sensor when a quadrotor is used as an actuator. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 Schematic diagram of the coordinate system defined by the quadrotor inverted pendulum system described in embodiment eleven.

[0046] Figure 2 This is a geometric diagram of the quadrotor rotation attitude angle described in embodiment eleven.

[0047] Figure 2 In the figure, (a) is a schematic diagram of the roll angle of the quadrotor, (b) is a schematic diagram of the pitch angle of the quadrotor, and (c) is a schematic diagram of the yaw angle of the quadrotor.

[0048] Figure 3 This is a geometric diagram of the spatial pendulum angle measurement principle described in the eleventh embodiment.

[0049] Figure 4 This is a schematic diagram of the visual angle measurement algorithm flow described in Implementation Example 11.

[0050] Figure 5 This is a block diagram of the cascade control of the quadrotor inverted pendulum as described in embodiment eleven.

[0051] Figure 6 This is a schematic diagram of the quadrotor position response curve described in embodiment eleven.

[0052] Figure 6In the figure, (a) is a schematic diagram of the position response curve of the quadrotor rotating around the x-axis, (b) is a schematic diagram of the position response curve of the quadrotor rotating around the y-axis, and (c) is a schematic diagram of the position response curve of the quadrotor rotating around the z-axis.

[0053] Figure 7 Schematic diagram of the pendulum rod position response curve described in the eleventh embodiment.

[0054] Figure 7 In the figure, (a) is a schematic diagram of the position response curve of the pendulum r, and (b) is a schematic diagram of the position response curve of the pendulum s. DETAILED DESCRIPTION

[0055] In order to make the purpose, technical solutions and advantages of the implementation methods of the present application clearer, the technical solutions in the implementation methods of the present application will be clearly and completely described below in conjunction with the drawings in the implementation methods of the present application. Obviously, the described implementation methods are only part of the implementation methods of the present application, not all of the implementation methods.

[0056] Embodiment 1: This embodiment provides a method for measuring and controlling a quadrotor inverted pendulum system based on a visual sensor, the method comprising the following steps:

[0057] Step 1: Model the quadrotor inverted pendulum system to obtain a dynamic model of the quadrotor inverted pendulum, and place a monocular camera in the positive direction of the X-axis and the positive direction of the Y-axis of the quadrotor coordinate system respectively;

[0058] Step 2: Based on the premise that the fulcrum of the pendulum falls on the optical axis of the camera, the pendulum motion image captured by the monocular camera is used to calculate the pendulum inclination angle using the Hough transform to restore the spatial position of the pendulum relative to the quadrotor coordinate system;

[0059] Step 3, respectively using the visual SLAM algorithm and the Hough transform algorithm to obtain the attitude angle and the pendulum inclination angle of the dynamic model of the quadrotor inverted pendulum, and compensate the pendulum inclination angle measured by the visual algorithm;

[0060] Step 4, calculate the spatial position of the pendulum relative to the pendulum coordinate system, that is, complete the measurement of the spatial pendulum position in the quadrotor inverted pendulum system;

[0061] Step 5: Dual control of the quadrotor position and the pendulum position is achieved by cascading the quadrotor position control loop and the pendulum position control loop, and an improved PID based on multi-point differential is used as the pendulum position loop controller to complete the measurement and control of the quadrotor inverted pendulum system.

[0062] Embodiment 2: This embodiment further limits the measurement and control method of a quadrotor inverted pendulum system based on a visual sensor described in embodiment 1. The dynamic model of the quadrotor inverted pendulum described in step 1 is:

[0063]

[0064] Among them, f r 、f s 、f x 、f y is the disturbance, and the quadrotor is defined to rotate around X w , Y w , Z w The axis rotation angles are the roll angle φ, the pitch angle θ, and the yaw angle ψ. The coordinates of the center of mass of the pendulum in the pendulum coordinate system are (r, s, ζ), and g represents the acceleration of gravity.

[0065] Implementation method three. This implementation method is a further limitation of the measurement and control method of a quadrotor inverted pendulum system based on a visual sensor described in implementation method one. Based on the dynamic model of the quadrotor inverted pendulum obtained in step 1, it also includes the step of establishing a model from the swing arm position input to the quadrotor position output and from the quadrotor attitude input to the swing arm position output.

[0066] Implementation 4. This implementation is a further limitation of the measurement and control method of a quadrotor inverted pendulum system based on a visual sensor described in Implementation 1. In step 3, the method of using the visual SLAM algorithm and the Hough transform algorithm to obtain the attitude angle and the inclination angle of the pendulum of the dynamic model of the quadrotor inverted pendulum is:

[0067] The monocular camera collects images and determines the time synchronization of the images collected by the two cameras;

[0068] Convert two frames of images that meet the time synchronization requirements into grayscale images;

[0069] The Canny edge detection method is used to process the grayscale image and extract the edge of the pendulum;

[0070] Hough transform is used to detect the straight line at the edge of the pendulum and obtain the pendulum inclination angle in the quadrotor coordinate system;

[0071] The VINS-Fusion visual measurement algorithm is used to obtain the attitude angle of the quadrotor.

[0072] Implementation method five: This implementation method is a further limitation of the measurement and control method of a quadrotor inverted pendulum system based on a visual sensor described in implementation method one. The method for compensating the pendulum inclination angle measured by the visual algorithm in step 3 is: error-free pendulum inclination angle compensation is completed by rotating around a fixed coordinate system as the rotation axis.

[0073] Embodiment 6: This embodiment further limits the measurement and control method of a quadrotor inverted pendulum system based on a visual sensor described in Embodiment 1. The method for calculating the spatial position of the pendulum relative to the pendulum coordinate system in step 4 is:

[0074] The quadrotor attitude angle measured by the VINS-Fusion vision algorithm is used to compensate the pendulum inclination angle calculated by the Hough transform algorithm, and finally the pendulum position r and s under the established dynamic model are solved.

[0075] Implementation method seven. This implementation method is a further limitation of the measurement and control method of a quadrotor inverted pendulum system based on a visual sensor described in implementation method one. In step 5, the quadrotor position control loop and the rocker position control loop are cascaded in a series structure, in which the rocker controller is the inner loop and the quadrotor position controller is the outer loop.

[0076] Implementation method seven. This implementation method is a further limitation of the measurement and control method of a quadrotor inverted pendulum system based on a visual sensor described in implementation method one. In step 5, the quadrotor position control loop and the rocker position control loop are cascaded in a series structure, in which the rocker controller is the inner loop and the quadrotor position controller is the outer loop.

[0077] Embodiment 8: This embodiment further limits the measurement and control method of a quadrotor inverted pendulum system based on a visual sensor described in Embodiment 1. The method of using an improved PID based on multi-point differential as a pendulum position loop controller in step 5 is:

[0078]

[0079] Wherein, u(k) represents the differential term of the control quantity in the improved PID controller at the kth moment, e(ki) represents the error value input to the controller at the kith moment, N ≥ 2, and N is an even number.

[0080] Embodiment 9: This embodiment proposes a computer device, including a memory and a processor, wherein the memory stores a computer program, and when the processor runs the computer program stored in the memory, the processor executes the method described in any one of embodiments 1 to 8.

[0081] Embodiment 10: This embodiment proposes a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the steps of the method described in any one of embodiments 1 to 8 are implemented.

[0082] Implementation eleven: This implementation provides an example, which is used to explain the above implementations one to eight. The specific example is as follows:

[0083] See also Figures 1 to 7 To illustrate this embodiment, the method described in this embodiment includes the following steps:

[0084] Step 1: Modeling the quadrotor inverted pendulum system. In the inertial coordinate system, let the coordinate of the quadrotor be P m =[x,y,z] T , in the pendulum coordinate system, let the center of mass of the pendulum be P p =[r,s,ζ], assuming the length of the pendulum is 2L, we can get Therefore, the coordinates of the pendulum's center of mass in the inertial coordinate system are [x+r,y+s,z+ζ].

[0085] Assume that the rotational kinetic energy of the pendulum about the fulcrum is T p1 , the translational kinetic energy of the pendulum is T p2 , the total kinetic energy of the pendulum is T p , we can get:

[0086] T p =T p1 +T p2 (1-1)

[0087] Since the pendulum is a light and thin rod, during the control process of the quadrotor inverted pendulum, the moment of inertia of the pendulum around the Z axis can be considered to be zero, and its rotational kinetic energy T p1 for:

[0088]

[0089] Among them, Ω p，xy is the angular velocity vector of the pendulum rod around the X-axis and Y-axis of the inverted pendulum, which can be expressed as To calculate, J P is the moment of inertia of the pendulum rod around the X-axis or Y-axis of the inverted pendulum coordinate system,

[0090] Because the pendulum rotates symmetrically around the X-axis and around the Y-axis, it is:

[0091]

[0092] m p is the mass of the pendulum, L is the length from the center of mass of the pendulum to the fulcrum, we can get:

[0093]

[0094] Let the velocity of the pendulum in the inertial coordinate system be v p , then

[0095]

[0096] Right now

[0097]

[0098] According to the formula for calculating the kinetic energy of the pendulum We can get:

[0099]

[0100] Let the total potential energy of the system be V p , from formula (1-1), formula (1-4), formula (1-7) we get

[0101]

[0102] According to the Lagrangian mechanics method, the generalized coordinates are taken as r, s, then

[0103]

[0104] Where L p =T p -V p , can be obtained

[0105]

[0106] The quadrotor modeling process does not consider the impact of the earth's rotation and its own curvature on the quadrotor, and assumes that the quadrotor is a homogeneous rigid body, and its posture changes slightly during the quadrotor pole erection process. The quadrotor Euler angle is defined according to the external rotation definition, and the rotation transformation matrix R from the body coordinate system to the world coordinate system is

[0107]

[0108] In order to facilitate the derivation and calculation of subsequent models, the following assumptions can be made for the modeling process of the quadrotor:

[0109] a) The effect of the earth’s rotation and its own curvature on the quadrotor is not considered.

[0110] b) The quadrotor is a homogeneous rigid body with its center of mass at the geometric center.

[0111] c) During the implementation of the quadrotor inverted pendulum, the quadrotor has a small movement amplitude and a small attitude change.

[0112] Assume the speed of the i-th motor of the quadrotor is w i , the lift coefficient is K p , the torque coefficient is K d , the lift and torque generated by the i-th propeller are T i 、M i , according to the propeller blade element theory:

[0113]

[0114] Therefore, the total lift T generated by the quadrotor propeller is:

[0115]

[0116] The force generated by the quadrotor propeller in the inertial coordinate system is:

[0117]

[0118] Assuming the mass of the quadrotor is m, the kinematic equation of the quadrotor is obtained from Newton's second law:

[0119]

[0120] From formula (1-14) and formula (1-15), we get

[0121]

[0122] In the quadrotor inverted pendulum system, the quadrotor acts as an actuator and realizes stable control of the pendulum by controlling its own attitude. During the stable control of the quadrotor inverted pendulum, the height of the quadrotor remains constant and the yaw angle is maintained near zero. On the basis of establishing the pendulum dynamics model and the quadrotor dynamics model, the dynamics model of the quadrotor inverted pendulum can be obtained from equations (1-10) and (1-16):

[0123]

[0124] Among them, f r 、f s 、f x 、f y is the disturbance amount.

[0125] Since the pendulum is always near the zero position during the stable control of the inverted pendulum, the attitude change of the quadrotor is small. The equation (1-17) is linearized with a small deviation at the working point, and the dynamic model of the quadrotor inverted pendulum is further simplified. The model of the pendulum position input to the quadrotor position output and the quadrotor attitude input to the pendulum position output is established as follows:

[0126]

[0127] Step 2: The pose estimation of the quadrotor adopts the visual VINS-Fusion algorithm. As shown in formula (1-18), the positions r and s of the pendulum need to be measured by sensors. In order to realize the visual measurement scheme of the pendulum, the following analysis and description are now carried out.

[0128] First, assume that the quadrotor is stationary and describe the spatial swing arm position scheme geometrically. Figure 2As shown, assuming that the quadrotor is stationary, the spatial coordinate relationship of the pendulum in the pendulum coordinate system is established, the viewing plane of camera 1 is the projection plane of the front view of the pendulum, the viewing plane of camera 2 is the projection plane of the side view of the pendulum, point A is the center of mass of the pendulum, line segment FH and line segment GI are the projection lines of the pendulum in the viewing plane of camera 1 and the viewing plane of camera 2 respectively, and through the binocular synchronous angle measurement scheme, it can be measured that the angle between line segment FH and the vertical direction of the Z axis is α, the angle between line segment GI and the vertical direction of the Z axis is β, the projection of pendulum OA in the XOY plane is line segment OB, the projection line segment OB in the X-axis direction of the pendulum coordinate system is OD, and the projection line segment in the Y-axis direction of the pendulum coordinate system is OC, and line segment OE is the projection of line segment OA in the XOZ plane of the pendulum coordinate system, so line segment GE is perpendicular to OE, and line segment ED is perpendicular to the X-axis.

[0129] Assume ∠AOE = α', the angle between the line segment FH and the vertical direction of the Z axis measured by the two cameras synchronous angle measurement scheme is α (assuming α is negative at this time, if α is on the other side of the vertical line, it is positive), and the angle between the line segment GI and the vertical direction of the Z axis is β (assuming β is positive at this time, if β is on the other side of the vertical line, it is negative), so ∠ADE = α, ∠OED = β. Assume line segment OA = L, line segment OE = L 1 , line segment AE = L 2 , line segment ED = L 3 , we can get:

[0130]

[0131] In ΔAED, Right now Solved

[0132] α'=arctan(cosβtanα) (1-20)

[0133] Assume that the coordinates of point B in the pendulum coordinate system are (r, s), we can get:

[0134]

[0135] Step 3: Use visual algorithm to obtain angles α and β. The main steps of the visual algorithm are:

[0136] a) The camera collects images and determines the time synchronization of the images collected by the two cameras

[0137] b) Convert the two frames of images that meet the time synchronization requirements into grayscale images

[0138] c) Use the Canny edge detection operator to process the grayscale image and extract the edge of the pendulum.

[0139] d) Use Hough transform to detect the straight line at the edge of the pendulum to solve the angles α and β.

[0140] The visual algorithm runs on a LENOVO Y7000P laptop equipped with an Intel Core i5 9th generation CPU and an NVIDIA GeForce RTX 1650 GPU, running on the Ubuntu 18.04 operating system. The images collected by the two cameras can be processed at a maximum speed of 30 frames per second.

[0141] Step 4: Since the camera is fixed on the plane of the quadrotor, it is necessary to consider the impact of the quadrotor plane on the measurement of the swing arm position.

[0142] Because the attitude angle of the quadrotor needs to be used for the tilt angle compensation of the pendulum, the rotation description around the fixed coordinate system as the rotation axis can achieve error-free attitude angle compensation. Figure 2 As shown, the quadrotor rotates around the X-axis at an angle of φ, and around the Y-axis at an angle of θ. Therefore, in view of the influence of the quadrotor plane on the measurement of the spatial swing position, it is necessary to compensate for the angles α and β. The angles obtained after compensation are α+φ and β+θ, and finally we can get

[0143]

[0144] This enables contactless spatial pendulum position measurement based on visual sensors in the plane of the quadrotor.

[0145] Step 5: In a limited indoor space, the rocker control needs to ensure the convergence of the quadrotor position. The rocker controller is the inner loop, and the quadrotor position controller is the outer loop. The control block diagram is as follows: Figure 5 As shown, in terms of parameter debugging, the series structure follows the principle of inside first and outside later, which is more convenient and quicker when using engineering methods for adjustment.

[0146] In the actual parameter setting process, when the PID controller system is used directly, the pendulum control loop cannot be stabilized. The reason is that in order to ensure good dynamic performance, the pendulum controller uses a larger differential term coefficient. However, there is noise in the position measurement of the pendulum, and a larger differential term coefficient will amplify this noise, causing the system to diverge. In order to prevent the system from diverging, this paper implements the differential term in the PID in a multi-point differential manner to perform noise filtering, and uses an improved PID algorithm based on multi-point differential as the controller of the pendulum position control loop to achieve stable control of the pendulum loop. The multi-point differential formula is as follows:

[0147]

[0148] Here, u(k) represents the derivative term at step k, and e(ki) represents the error value input to the controller at step ki, N ≥ 2, and N is an even number. A larger N will enhance the noise filtering effect, but may reduce the dynamic response of the system. On the contrary, a smaller N will weaken the noise filtering effect and have less impact on the dynamic response of the system. The specific value of N needs to be tested according to the actual system conditions. In this experiment, N is set to 6 to take into account both noise suppression and the dynamic response of the system.

[0149] The final control effect is as follows Figure 6 and Figure 7 As shown in the figure, during the experiment, the quadrotor aircraft initially hovers at the position (0, 0, 1.5). At about 12 seconds, a pendulum is placed on the quadrotor to put the quadrotor into the upright mode. It can be seen that the position of the quadrotor and the pendulum are stable at zero. At 26 seconds, a disturbance force is applied, and the position of the quadrotor and the pendulum are still stable at zero, which fully verifies the effectiveness of the measurement scheme and the robustness of the control scheme.

[0150] Those skilled in the art will appreciate that the above are only preferred embodiments of the present invention, and the various embodiments of the present disclosure and / or the features described in the claims may be combined or combined in various ways, even if such combinations or combinations are not explicitly described in the present disclosure. It is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art may still modify the technical solutions described in the aforementioned embodiments, or perform equivalent substitutions on some of the technical features therein. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included in the protection scope of the present invention.

[0151] Although preferred embodiments of the present invention have been described, additional changes and modifications may be made to these embodiments by those skilled in the art once the basic inventive concepts are known. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention. Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A method for measuring and controlling a quadrotor inverted pendulum system based on a visual sensor, characterized in that: The method comprises the following steps: Step 1: Model the quadrotor inverted pendulum system to obtain a dynamic model of the quadrotor inverted pendulum, and place a monocular camera in the positive direction of the X-axis and the positive direction of the Y-axis of the quadrotor coordinate system respectively; Step 2: Based on the premise that the fulcrum of the pendulum falls on the optical axis of the camera, the pendulum motion image captured by the monocular camera is used to calculate the pendulum inclination angle using Hough transform to restore the spatial position of the pendulum relative to the quadrotor coordinate system; Step 3, respectively using the visual SLAM algorithm and the Hough transform algorithm to obtain the attitude angle and the pendulum inclination angle of the quadrotor inverted pendulum dynamics model, and compensate the pendulum inclination angle measured by the visual algorithm; Step 4, calculate the spatial position of the pendulum relative to the pendulum coordinate system, and complete the spatial pendulum position measurement in the quadrotor inverted pendulum system; Step 5: Dual control of the quadrotor position and the pendulum position is achieved by cascading the quadrotor position control loop and the pendulum position control loop, and an improved PID based on multi-point differential is used as the pendulum position loop controller to complete the measurement and control of the quadrotor inverted pendulum system.

2. The method for measuring and controlling a quadrotor inverted pendulum system based on a visual sensor according to claim 1, characterized in that: The dynamic model of the quadrotor inverted pendulum described in step 1 is: Among them, f r 、f s 、f x 、f y is the disturbance, and the quadrotor is defined to rotate around X w , Y w , Z w The axis rotation angles are the roll angle φ, the pitch angle θ, and the yaw angle ψ. The coordinates of the center of mass of the pendulum in the pendulum coordinate system are (r, s, ζ), and g is the gravitational acceleration.

3. The method for measuring and controlling a quadrotor inverted pendulum system based on a visual sensor according to claim 1, characterized in that: The dynamic model of the quadrotor inverted pendulum obtained in step 1 also includes the steps of establishing a model from the swing bar position input to the quadrotor position output and from the quadrotor attitude input to the swing bar position output.

4. The method for measuring and controlling a quadrotor inverted pendulum system based on a visual sensor according to claim 1, characterized in that: In step 3, the method of using the visual SLAM algorithm and the Hough transform algorithm to obtain the attitude angle and the inclination angle of the pendulum of the dynamic model of the quadrotor inverted pendulum is: The monocular camera collects images and determines the time synchronization of the images collected by the two cameras; Convert two frames of images that meet the time synchronization requirements into grayscale images; The Canny edge detection method is used to process the grayscale image and extract the edge of the pendulum; Hough transform is used to detect the straight line at the edge of the pendulum and obtain the pendulum inclination angle in the quadrotor coordinate system; The VINS-Fusion visual measurement algorithm is used to obtain the attitude angle of the quadrotor.

5. The method for measuring and controlling a quadrotor inverted pendulum system based on a visual sensor according to claim 1, characterized in that: The method for compensating the pendulum inclination angle measured by the visual algorithm in step 3 is to complete error-free pendulum inclination angle compensation by rotating around a fixed coordinate system as a rotation axis.

6. The method for measuring and controlling a quadrotor inverted pendulum system based on a visual sensor according to claim 1, characterized in that: The method for calculating the spatial position of the pendulum relative to the pendulum coordinate system in step 4 is: The quadrotor attitude angle measured by the VINS-Fusion vision algorithm is used to compensate the pendulum inclination angle calculated by the Hough transform algorithm, and finally the pendulum position r and s under the established dynamic model are solved.

7. The method for measuring and controlling a quadrotor inverted pendulum system based on a visual sensor according to claim 1, characterized in that: In step 5, the quadrotor position control loop and the rocker position control loop are cascaded in a series structure, wherein the rocker controller is the inner loop and the quadrotor position controller is the outer loop.

8. The method for measuring and controlling a quadrotor inverted pendulum system based on a visual sensor according to claim 1, characterized in that: The method of using the improved PID based on multi-point differential as the swing arm position loop controller in step 5 is: Wherein, u(k) represents the differential term of the control quantity in the improved PID controller at the kth moment, e(ki) represents the error value input to the controller at the kith moment, N ≥ 2, and N is an even number.

9. A computer device comprising a memory and a processor, characterized in that A computer program is stored in the memory. When the processor runs the computer program stored in the memory, the processor executes the method according to any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 8 are implemented.

Images