A cricket system control method based on target detection and model predictive control

Abstract

The present invention discloses a control method of a cricket system based on target detection and model predictive control, which belongs to the field of computer vision processing and control. The present invention introduces color detection on the basis of Hough circle detection, and proposes an algorithm combining color detection with Hough circle detection algorithm to reduce the interference of environmental information on the Hough circle detection algorithm. A method for solving the tilt angle of the board according to the position of the ball using the MPC algorithm is also proposed.

Description

Technical Field

[0001] The invention relates to the fields of computer vision processing and control, and in particular to a cricket system control method based on target detection and model predictive control. Background Art

[0002] Object detection is one of the basic tasks in computer vision, which is to identify and locate the object of interest in a picture. It is the basis of many other computer vision tasks, such as instance segmentation, image captioning, object tracking, etc. Object detection algorithms are roughly divided into two stages: traditional object detection algorithms and deep learning-based object detection algorithms. With the improvement of hardware computing power, the rapid development of deep learning and the disclosure of high-quality data sets in recent years, researchers have proposed more and more object detection algorithms. At present, deep learning-based detection algorithms can generally be divided into two types: representative two-stage object detectors, such as the R-CNN series of algorithms and single-stage object detectors, such as the YOLO series of algorithms. Although deep learning algorithms are currently very applicable, for cricket systems, the detection target is relatively simple, and only the real-time position of the ball needs to be detected, so such a complex detection network is not required. For the detection of the ball, the Hough circle detection algorithm can be used, which is different from other traditional object detection algorithms. Other traditional object detection algorithms generally go through region proposal, feature extraction and classification regression, and the detection efficiency is relatively low and the detection speed is not fast enough. The Hough circle detection algorithm is only used for circle detection. It uses a voting method to first determine the center of the circle, and then determines the radius based on the center of the circle, thereby completing the circle detection. The detection process is simpler than other traditional detection algorithms, and the detection speed is faster than other traditional ones.

[0003] After the ball is detected in real time, the position of the ball should be controlled in order to make it roll to the center of the board as much as possible and prevent it from falling off the board. At present, the two types of control algorithms that are widely and successfully used in industrial practice are PID control algorithm and MPC control algorithm. PID control is one of the main controls in industrial control due to its simple structure, convenient operation and superior performance. In order to achieve the desired control effect, PID control can appear in the form of P, PI, PD, PID, and some other deformations. The MPC control algorithm has the function of displaying the future dynamic behavior of the system, so that the prediction model can be used to predict the output change of the controlled object and the deviation of the controlled variable from its given value at the future moment as the basis for determining the control action, so that it can adapt to the causal characteristics of the dynamic system and obtain better control effect than conventional control. However, for the cricket control system, the dynamic characteristics are relatively complex, and PID control may not necessarily achieve the ideal adjustment effect. At this time, the MPC algorithm will have a better effect.

[0004] The position of the ball is obtained through target detection and detected in real time. Due to environmental interference, some interference objects that are approximately circular are also identified as target objects, which has a great impact on the detection. Moreover, the position of the ball is in the camera coordinate system at this time. If the robot arm controls the position of the ball, the robot arm must first know where the ball is, so the position of the ball must be converted to the base coordinate system of the robot arm through camera calibration. The robot arm needs to adjust the angle of the board according to the real-time position of the ball to adjust the position of the ball, so that the ball can reach and stabilize in the center of the board as much as possible. The inclination angle of the board is a three-dimensional angle, and it is difficult to achieve the ideal control effect. Summary of the invention

[0005] In view of the problems existing in the above-mentioned technology, the purpose of the present invention is to propose a cricket system control method based on target detection and model predictive control. The method reduces the interference of environmental information on the Hough circle detection algorithm by introducing an algorithm that combines object color detection with the Hough circle detection algorithm, and proposes a method for solving the inclination angle of the board according to the position of the ball using the MPC algorithm.

[0006] In order to achieve the above object, the technical solution adopted by the present invention is as follows:

[0007] A cricket system control method based on target detection and model predictive control, characterized in that it comprises the following steps:

[0008] 1) In order to improve the detection accuracy, the color image collected by the camera is color processed, and only the image with the same color as the ball is collected. Then the image is subjected to a series of operations such as median filtering, opening operation, closing operation, etc., and then binarized to meet the input image requirements of the Hough circle detection algorithm;

[0009] 2) The pixel position of the ball detected by combining color detection with Hough circle detection is transferred to the robotic arm coordinate system through camera external parameter calibration.

[0010] 3) Using the pixel coordinates obtained in step 2) in the base coordinate system, fix the board to the end of the robot flange, and convert the pixel coordinates of the ball in the initial state and the expected position into coordinates in the base coordinate system of the robot.

[0011] 4) Based on the initial position and expected position obtained in step 3), a cricket system model can be established, and a three-dimensional angle position controller can be designed using the model predictive control algorithm. The specific controller design is as follows:

[0012] 4.1) Define the initial position and initial velocity of the ball, the expected position and expected velocity of the ball, and thus establish the cricket system model:

[0013]

[0014]

[0015]

[0016] Among them, P0 and V0 are the initial position and initial velocity of the ball respectively; P d 、V d are the expected position and expected velocity of the ball respectively; f(θ k ) is the acceleration of the ball at the current moment, T is the time difference between the previous moment and the current moment; Formula (1-3) uses the position, velocity and acceleration at the current moment to calculate the position and velocity at the next moment.

[0017] 4.2) The control problem of the cricket system can be transformed into an optimization problem of solving the optimal angle according to the position and speed of the ball at the current moment, and the optimal control quantity is calculated by using a single-step MPC prediction method. Too many constraints may compress the size of the optimal solution space, resulting in the inability to find the optimal solution for the planning problem at a certain moment, or may cause the computer program searching for the optimal solution to spend a lot of time, and the control calculation cannot be completed within the time interval between two samplings, resulting in loss of control. Therefore, for the cricket system, in addition to the necessary equality constraints, only the amplitude of the inclination angle variable of the board is constrained to prevent the board from tilting too much and the ball from rolling too fast and directly leaving the board.

[0018] Objective function:

[0019] Constraints:

[0020] Among them, the positive definite matrix Q p , Q v , R is customized as θ is the inclination angle of the board, θ x ,θ y are the inclination angles of the board in the x-axis and y-axis directions respectively.

[0021] 5) The acceleration f(θ in formula (1-5) k ) is a function of angles. The angle obtained is the inclination of the board. The board and flange are fixed together, so the inclination of the board is the inclination of the flange at the end of the robot. The flange is only translated relative to the robot base coordinate system, but not rotated. That is, the board is not rotated relative to the base coordinate system, so the three-dimensional acceleration in the world coordinate system is (0,0,9.8) T , but the z-axis of the robot is upward, so the three-dimensional acceleration in the robot base coordinate system is g = (0,0,-9.8) T.

[0022]

[0023]

[0024]

[0025] Because the Euler angle of the robot arm is converted into a rotation matrix in the form of ZYX, that is, the rotation order is to rotate around the z-axis for an angle of γ for the first time, around the y-axis for an angle of β for the second time, and around the x-axis for an angle of α for the third time, so the three-dimensional Euler angle is converted into the rotation matrix in the base coordinate system as follows:

[0026] R zyx =R z *R y *R x (1-9)

[0027] Therefore, the relationship between the acceleration and angle of the ball is a = R zyx *g, the acceleration in the z-axis direction does not affect the motion of the ball, so just take the acceleration in the x- and y-axis directions.

[0028] 6) Based on the initial position P0 and initial velocity V0 of the ball, the expected position P d and the desired speed V d , from the relationship between the Euler angle and the rotation matrix in step 5), the relationship between the Euler angle and the acceleration is obtained, and then the optimal three-dimensional Euler angle can be obtained from the objective function in step 4).

[0029] 7) Convert the three-dimensional Euler angles solved in step 6) into a rotation matrix. In order to allow the end of the robot to rotate at the initial position to adjust the inclination of the board and thus adjust the position of the ball, the translation matrix is ​​the translation matrix of the robot flange relative to the base coordinate system in the initial state, which is used as the desired homogeneous transformation matrix of the end of the robot.

[0030] 8) According to the deviation between the real-time position of the ball and the desired position, the controller is used to continuously obtain the Euler angle required to adjust the board, and the position of the ball on the board is adjusted by adjusting the inclination angle of the end flange through the robotic arm until the ball reaches the desired position.

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

[0032] 1) A method combining color detection and Hough circle detection is proposed to minimize the interference in the recognition of small balls in complex environments.

[0033] 2) The position of the ball is detected in real time by a camera, and a controller that controls the relationship between the tilt angle of the board and the position of the ball is designed using model prediction control. The robot arm adjusts the tilt angle of the board according to the real-time position of the ball, thereby adjusting the position of the ball. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 is a flow chart of a method implementation of an embodiment of the present invention;

[0035] Figure 2 This is a diagram of the position change of a ball simulated by the method of an embodiment of the present invention. It can be seen that the ball can finally converge to the desired position;

[0036] Figure 3 This is a diagram of the ball speed change in simulation using the method of an embodiment of the present invention. It can be seen that the ball speed starts from 0 and is almost 0 at the end. The possible reason is that the ball oscillates at the desired position. DETAILED DESCRIPTION

[0037] The present invention will be further described in detail below in conjunction with specific embodiments.

[0038] The purpose of the present invention is to provide a cricket system control method based on target detection and model predictive control. The method reduces the interference of environmental information on the Hough circle detection algorithm by introducing an algorithm that combines object color detection with the Hough circle detection algorithm, and proposes a method for solving the inclination angle of the board according to the position of the ball using the MPC algorithm.

[0039] In this embodiment, the position of the ball is detected in real time by a camera and a target detection algorithm, and a single-step model predictive control algorithm is used to solve the inclination angle of the board according to the real-time position of the ball. A robotic arm is then used to control the board so that the ball reaches the desired position and stabilizes as much as possible.

[0040] A cricket system control method based on target detection and model predictive control, the specific steps are as follows:

[0041] Step 1: In order to improve the detection accuracy, the color image captured by the camera is color processed, and only the image with the same color as the ball is collected. The image is then subjected to a series of operations such as median filtering, opening and closing operations, and then binarized to meet the input image requirements of the Hough circle detection algorithm.

[0042] Step 2: The pixel position of the ball detected by combining color detection with Hough circle detection is transferred to the robot arm coordinate system through camera external parameter calibration. The specific steps of camera calibration are as follows:

[0043] 1) Convert the world coordinate system to the camera coordinate system:

[0044] The Aruco code is used to calibrate the camera and obtain the conversion relationship between the world coordinate system and the camera coordinate system.

[0045]

[0046] Among them, R is a 3*3 rotation matrix, t is a 3*1 translation vector, (x c ,y c ,z c ,1) T is the homogeneous coordinate of the spatial point P in the camera coordinate system, (x w ,y w ,z w ,1) T is the homogeneous coordinate of the spatial point P in the world coordinate system.

[0047] 2) Convert the camera coordinate system to the image coordinate system:

[0048]

[0049] Among them, (x,y,1) T are the homogeneous coordinates of the image point p in the image coordinate system.

[0050] 3) Convert image coordinate system to pixel coordinate system:

[0051] The origin of the image coordinate system is recorded as o(u0,v0) in the pixel coordinate system, and the coordinate of the image point p in the pixel coordinate system is recorded as p(u,v). Let dx and dy be the physical size of each pixel on the x-axis and y-axis respectively, then we have the relationship of formula (2-3).

[0052]

[0053] Right now

[0054] 4) Convert the world coordinate system to the pixel coordinate system:

[0055]

[0056] Among them, M1 is the camera intrinsic parameter matrix, which can be directly read from the Intel D415 camera, and M2 is the camera extrinsic parameter matrix, which is obtained through calibration.

[0057] 5) The robot base coordinate system is converted to the world coordinate system:

[0058] The world coordinate system is specified to have an x-axis offset of 0.3m and y-axis and z-axis offsets of 0 with respect to the robot base coordinate system. At this time, the relationship between the robot base coordinate system and the world coordinate system is expressed as formula (2-6).

[0059]

[0060] Among them, (x b ,y b ,z b ) T is the secondary coordinate of the robot base coordinate system.

[0061] 6) The robot base coordinate system is converted to the pixel coordinate system:

[0062]

[0063] Step 3: Use the pixel coordinates obtained in step 2 in the base coordinate system to fix the board to the end of the robot flange, and convert the pixel coordinates of the ball in the initial state and the expected position into coordinates in the base coordinate system of the robot.

[0064] Step 4: Based on the initial position and expected position obtained in step 3, a cricket system model can be established, and a three-dimensional angle position controller can be designed using the model predictive control algorithm.

[0065] 1) Define the initial position and initial velocity of the ball, the expected position and expected velocity of the ball, and thus establish the cricket system model:

[0066]

[0067]

[0068]

[0069] Among them, P0 and V0 are the initial position and initial velocity of the ball respectively; P d 、V d are the expected position and expected velocity of the ball respectively; f(θ k ) is the acceleration of the ball at the current moment, T is the time difference between the previous moment and the current moment; P k 、V k are the current position and velocity, P k+1 、V k+1 are the position and velocity at the next moment respectively; formula (2-10) uses the position, velocity and acceleration at the current moment to calculate the position and velocity at the next moment.

[0070] 2) The control problem of the cricket system can be transformed into an optimization problem of solving the optimal angle according to the position and speed of the ball at the current moment, and the optimal control quantity is calculated by using a single-step MPC prediction method. Too many constraints may compress the size of the optimal solution space, resulting in the inability to find the optimal solution for the planning problem at a certain moment. It may also cause the computer program searching for the optimal solution to spend a lot of time, and the control calculation cannot be completed within the time interval between two samplings, resulting in loss of control. Therefore, for the cricket system, in addition to the necessary equality constraints, only the amplitude of the inclination angle variable of the board is constrained to prevent the board from tilting too much and the ball from rolling too fast and directly leaving the board.

[0071] Objective function:

[0072] Constraints:

[0073] Among them, the positive definite matrix Q p , Q v , R is customized as θ is the inclination angle of the board, θ x ,θ y are the inclination angles of the board in the x-axis and y-axis directions respectively.

[0074] Step 5. The acceleration f(θ k ) is a function of angles. The angle obtained is the inclination of the board. The board and flange are fixed together, so the inclination of the board is the inclination of the flange at the end of the robot. The flange is only translated relative to the robot base coordinate system, but not rotated. That is, the board is not rotated relative to the base coordinate system, so the three-dimensional acceleration in the world coordinate system is (0,0,9.8) T , but the z-axis of the robot is upward, so the three-dimensional acceleration in the robot base coordinate system is g = (0,0,-9.8) T .

[0075]

[0076]

[0077]

[0078] Because the Euler angle of the robot arm is converted into a rotation matrix in the form of ZYX, that is, the rotation order is to rotate around the z-axis for an angle of γ for the first time, around the y-axis for an angle of β for the second time, and around the x-axis for an angle of α for the third time, so the three-dimensional Euler angle is converted into the rotation matrix in the base coordinate system as follows:

[0079] R zyx =R z*R y *R x (2-16)

[0080] Therefore, the relationship between the acceleration and angle of the ball is a = R zyx *g, the acceleration in the z-axis direction does not affect the motion of the ball, so the acceleration in the x- and y-axis directions can be used;

[0081] Step 6. Based on the initial position P0 and initial velocity V0 of the ball, the expected position P d and the desired speed V d , from the relationship between the Euler angle and the rotation matrix in step 5, the relationship between the Euler angle and the acceleration is obtained, and then the optimal three-dimensional Euler angle can be obtained from the objective function in step 4.

[0082] Step 7. Convert the three-dimensional Euler angles solved in step 6 into a rotation matrix. In order to allow the end of the robot to rotate at the initial position to adjust the inclination of the board and thereby adjust the position of the ball, the translation matrix is ​​the translation matrix of the robot flange relative to the base coordinate system in the initial state, which is used as the desired homogeneous transformation matrix of the end of the robot.

[0083] Step 8. Based on the deviation between the real-time position of the ball and the desired position, the controller is used to continuously obtain the Euler angle required to adjust the board. The position of the ball on the board is adjusted by adjusting the inclination angle of the end flange through the robotic arm until the ball reaches the desired position.

[0084] The above are preferred embodiments of the present invention. Any changes made according to the technical solution of the present invention, as long as the resulting functions do not exceed the scope of the technical solution of the present invention, belong to the protection scope of the present invention.

Claims

1. A cricket system control method based on target detection and model predictive control, characterized in that: The following steps are involved: 1) In order to improve the detection accuracy, the color image collected by the camera is color processed, and only the image with the same color as the ball is collected. Then the image is subjected to a series of operations such as median filtering, opening and closing operations, and then binarization processing is performed to make it meet the input image requirements of the Hough circle detection algorithm; 2) The pixel position of the ball detected by combining color detection with Hough circle detection is transferred to the robot arm coordinate system through camera external parameter calibration; 3) Using the pixel coordinates obtained in step 2) in the base coordinate system, fix the board to the end of the robot flange, and convert the pixel coordinates of the ball in the initial state and the expected position into the coordinates in the base coordinate system of the robot; 4) Based on the initial position and the expected position obtained in step 3), a cricket system model can be established, and a three-dimensional angle position controller can be designed using a model predictive control algorithm; The specific design of the controller in step 4) is as follows: 4.1) Define the initial position and initial velocity of the ball, the expected position and expected velocity of the ball, and thus establish the cricket system model: Among them, P0 and V0 are the initial position and initial velocity of the ball respectively; P d 、V d are the expected position and expected velocity of the ball respectively; f(θ k ) is the acceleration of the ball at the current moment, T is the time difference between the previous moment and the current moment; P k 、V k are the current position and velocity, P k+1 、V k+1 are the position and velocity at the next moment respectively; formula (2-3) uses the position, velocity and acceleration at the current moment to calculate the position and velocity at the next moment; 4.2) The control problem of the cricket system is transformed into an optimization problem of solving the optimal angle according to the position and speed of the ball at the current moment. The single-step MPC prediction method is used to calculate the optimal control quantity. Too many constraints may compress the size of the optimal solution space, resulting in the inability to find the optimal solution for the planning problem at a certain moment. It may also cause the computer program searching for the optimal solution to spend a lot of time and fail to complete the control calculation within the time interval between two samplings, resulting in loss of control. Therefore, for the cricket system, in addition to the necessary equality constraints, only the amplitude of the inclination angle variable of the board is constrained to prevent the board from tilting too much and the ball from rolling too fast and directly leaving the board. Objective function: Constraints: Among them, the positive definite matrix Q p , Q v , R is customized as θ is the inclination angle of the board, θ x ,θ y are the inclination angles of the board in the x-axis and y-axis directions respectively; 5) The angle obtained in step 4) is the inclination angle of the board. The board and the flange are fixed together, so the inclination angle of the board is the inclination angle of the flange at the end of the robot arm. The flange is only translated relative to the robot arm base coordinate system, and there is no rotational transformation, that is, the board is not rotated relative to the base coordinate system. The three-dimensional acceleration in the world coordinate system is (0,0,9.8) T , but the z-axis of the robot is upward, so the three-dimensional acceleration in the robot base coordinate system is g = (0,0,9.8) T ; Because the Euler angle of the robot arm is converted into a rotation matrix in the form of ZYX, that is, the rotation order is to rotate around the z-axis for an angle of γ for the first time, around the y-axis for an angle of β for the second time, and around the x-axis for an angle of α for the third time, so the three-dimensional Euler angle is converted into the rotation matrix in the base coordinate system as follows: R zyx =R z *R y *R x (1-4) Therefore, the relationship between the acceleration and angle of the ball is a = R zyx *g, the acceleration in the z-axis direction does not affect the motion of the ball, so the acceleration in the x- and y-axis directions can be used; 6) Based on the initial position P0 and initial velocity V0 of the ball, the expected position P d and the desired speed V d , the relationship between the Euler angle and the rotation matrix in step 5) is used to obtain the relationship between the Euler angle and the acceleration, and then the optimal three-dimensional Euler angle can be obtained from the objective function in step 4); 7) Convert the three-dimensional Euler angles solved in step 6) into a rotation matrix. In order to allow the end of the robot to rotate at the initial position to adjust the inclination of the board, so as to adjust the position of the ball, the translation matrix is ​​the translation matrix of the robot flange relative to the base coordinate system in the initial state, which is used as the desired homogeneous transformation matrix of the end of the robot; 8) According to the deviation between the real-time position of the ball and the desired position, the controller is used to continuously obtain the Euler angle required to adjust the board, and the position of the ball on the board is adjusted by adjusting the inclination angle of the end flange through the robotic arm until the ball reaches the desired position.

2. A cricket system control method based on target detection and model predictive control according to claim 1, characterized in that: In the step 7), the three-dimensional Euler angle solved in step 6) is converted into a rotation matrix. In order to allow the end of the robot to rotate at the initial position to adjust the inclination angle of the board, so as to adjust the position of the ball, the translation matrix is ​​the translation matrix of the robot flange relative to the base coordinate system in the initial state. The rotation matrix and the translation matrix are combined to serve as the desired homogeneous transformation matrix of the end flange of the robot. The purpose is to allow the homogeneous transformation matrix of the current end flange of the robot to change with the desired end flange homogeneous transformation matrix. In step 8), the Euler angle required for adjusting the board is continuously obtained through the controller according to the deviation between the real-time position of the ball and the desired position, so that the inclination angle of the board can be adjusted, and then the position of the ball can be adjusted.

Images