Control method of photovoltaic panel installation robot grasping system based on predictive control

Abstract

The present invention discloses a control method for a photovoltaic panel installation robot grasping system based on predictive control; it is divided into two stages, namely, an initialization stage and a predictive control stage. In the initialization stage, the robot arm is first modeled according to the DH parameters of the robot arm, and the characteristic point information and depth information obtained by the camera are used to calculate the parameter matrix to complete the initialization of the program; in the predictive control stage, an MPC algorithm with time delay compensation is added to solve the optimal solution, obtain the predicted joint speed value, and then send the speed value to the robot arm controller to control the robot arm to move. The present invention solves the problem of time delay caused by data acquisition conversion, data transmission, and data calculation, and causes the accumulated error of the robot arm joint, thereby reducing the time delay error caused by various parts of the system and improving the control accuracy and stability of the system.

Description

Technical Field

[0001] The present invention relates to the technical field of solar photovoltaic panel intelligent installation, and in particular to a grasping system and a control method of an outdoor solar photovoltaic panel intelligent installation robot. Background Art

[0002] Solar panels are mainly used to absorb solar energy. They are devices that convert solar energy into electrical energy for storage. They are mainly composed of silicon wafers and photovoltaic glass panels. The solar panel mounting bracket is arranged in multiple rows and columns. The traditional installation method is that a group of people lift the solar panels to the installation location, and then manually lift the solar panels to the mounting bracket. During the manual handling and installation process, the solar panels are easily damaged, and there are problems such as low installation efficiency, high labor costs, and many safety hazards. It is necessary to study the use of automated photovoltaic panel installation robots to replace manual installation to solve the above problems. Design a photovoltaic panel installation robot grasping system and control method based on predictive control. Summary of the invention

[0003] Aiming at the problems of high grasping accuracy, large cumulative error and poor adaptability of the existing photovoltaic panel installation robot based on visual servo mechanical arm to grasp photovoltaic panels, the present invention will design a robot visual servo method grasping system and control method based on predictive control. It solves the problem of time delay caused by data acquisition conversion, data transmission and data calculation, and the cumulative error of mechanical arm joints, thereby reducing the time delay error caused by various parts of the system and improving the control accuracy and stability of the system.

[0004] The technical solution of the present invention is as follows:

[0005] A photovoltaic panel installation robot grasping system based on predictive control includes a six-axis robot arm and a grasping device. In addition to a suction cup for sucking photovoltaic panels, the grasping device also has two visual cameras and a telescopic rod, which can extend and retract the visual cameras up and down to prevent the photovoltaic panels from blocking the camera's line of sight.

[0006] A control method for a photovoltaic panel installation robot grasping system based on predictive control. The specific predictive control method is as follows:

[0007] Step 1: Equation of State

[0008] The photovoltaic panel installation robot uses a six-axis robotic arm, and the state equation of the robotic arm is:

[0009] ;

[0010] in, For the The pixel coordinates of the four feature points of a period, For the The pixel coordinates of the four feature points of a period, is the input matrix, is the angular velocity of the six joint motors of the six-axis robot; the visual servo kinematic model of the robot is established by deducing the state equation.

[0011] Step 2: Design a visual servo prediction controller:

[0012] When using model predictive control, it is necessary to construct a cost function based on the state equation and actual requirements. When predicting in each cycle, the optimal control quantity is obtained by minimizing the cost function. The present invention defines the cost function as follows:

[0013] ;

[0014] The cost function can be divided into three parts: error cost, speed cost, and terminal error cost. is the prediction step length, Indicates The pixel coordinate error predicted for the next few steps in a cycle is Indicates The joint angular velocity predicted by the cycle for the next few steps is Indicates The future terminal pixel coordinate error predicted by cycles. , , It is a weight matrix. By adjusting the value in the weight matrix, the system can adjust the emphasis on the direction of the cost source.

[0015] Step 3: Field of View Constraints

[0016] In the control process, in order to prevent the field of view from being lost, it is necessary to set field of view constraints for the system. When predicting the speed in each cycle, it is necessary to ensure that the pixel coordinates of the next cycle obtained based on the predicted speed are within the image range, thereby obtaining the speed limit range. The output image size of the camera used in this invention is 960×720, so according to formula (1), it can be obtained

[0017] ;

[0018] Rearranging formula (3), we can get:

[0019] ;

[0020] in, , ,make , , further combined, the field of view constraint equation is:

[0021] .

[0022] Step 4: Camera Backward Constraint

[0023] In the control process, in order to prevent the camera from retreating when the robot moves, it is necessary to set a camera retreat constraint for the system. At the initial moment, the depth from the feature point to the camera is recorded as the maximum depth (depth refers to the camera coordinate Zc value), and the state equation about the depth is calculated. When predicting the speed in each cycle, it is necessary to ensure that the depth of the next cycle obtained based on the predicted speed is less than the maximum depth, so as to obtain the speed limit range.

[0024] Define the Z of the feature point C Derivatives of coordinates:

[0025] ;

[0026] in, Representing coordinate system Relative to the coordinate system b The rotation transformation matrix of the world coordinate system w , the robot end coordinate system e , camera coordinate system C Use symbols express No. j If yes, then , , ; P w Refers to the coordinates of the feature points in the world coordinate system; the joint angles of the robot arm ;

[0027] make , then the state equation of the depth information under the discrete system can be obtained

[0028] ;

[0029] For a system with four feature points, the depth information in the camera coordinate system is , , , , then:

[0030] ;

[0031] make , , t Indicates time;

[0032] Then the depth information state equation of the four feature point system is obtained as:

[0033] ;

[0034] Construct constrained inequalities:

[0035] ;

[0036] make , , , Indicates the maximum value of Zc;

[0037] The camera retreat constraint equation is obtained as:

[0038] ;

[0039] Combining equations (5) and (11), let , The total constraint equation is obtained

[0040] ;

[0041] Since the prediction step length is , when solving, it is necessary to set the constraint equation of the same dimension. According to formula (12), the final constraint equation is:

[0042] ;

[0043] So far, the constraint equations of the system have been derived;

[0044] According to formula (1), the system k The status at this moment is:

[0045] ;

[0046] Indicates that the state quantity x is Always The predicted value at the moment, x represents the pixel coordinate, Indicates a mapping relationship;

[0047] Considering constraint (13), the controller of the system is:

[0048] ;

[0049] ;

[0050] in, is the initial pixel value of the feature point, is the initial pixel error value, is the lower limit of error, is the upper limit of error, is the lower limit of joint velocity, is the upper limit of joint speed, is the robot joint velocity sequence, is the pixel coordinate value of the target point;

[0051] In each calculation cycle, the velocity sequence is obtained according to formula (15): , and take the first set of data as the robot arm joint speed and send it to the controller.

[0052] Step 5: Time Delay

[0053] Set the delay to meet the cycle length. When the delay ends, it means that this cycle is about to end. Read the joint angle from the controller and enter the next cycle for calculation. Repeat this process until the error value is less than the set error value and the control task is completed.

[0054] A cycle includes T 1st moment - find the pixel error of feature points, Time-Joint speed is sent, Moment - read joint angles, Moment - Get feature point pixel coordinates and depth information, T 5 Moments - Calculation parameter matrix; where T 1 time has arrived Time is time period , T 1 time has arrived Time is time period , Time has come Time is time period , Time has come T 5 time periods ;

[0055] Get the robot arm joint angle at all times. After collecting the pixel coordinates and depth information of the feature points at all times, the time interval is calculated according to the current joint angular velocity. calculate The new joint angle at that moment, and then according to The joint angle at the moment, the pixel coordinates of the feature points and the depth information are used to calculate the estimated world coordinates of the feature points. The joint angle at the moment and the current joint angular velocity according to time Estimate the next cycle after the forecast calculation is completed The joint angle at the moment is The estimated joint angle at the moment is calculated with the previously calculated feature point world coordinates. The pixel coordinates and depth information of the feature points at each moment are used to calculate the corresponding parameter matrix, and finally the cost function is solved to obtain the final optimal value.

[0056] Design ideas:

[0057] The algorithm process of the present invention is divided into two stages: the initialization stage and the predictive control stage. In the initialization stage, the robot arm is first modeled according to the DH parameters of the robot arm, and the characteristic point information and depth information obtained by the camera are used to calculate the parameter matrix to complete the initialization of the program; in the predictive control stage, the MPC algorithm with delay compensation is added to solve the optimal solution, obtain the predicted joint speed value, and then send the speed value to the robot arm controller to control the robot arm to move. The present invention solves the problem of delay caused by data acquisition conversion, data transmission, and data calculation, and causes the cumulative error of the robot arm joint, thereby reducing the delay error caused by various parts of the system and improving the control accuracy and stability of the system.

[0058] The beneficial effects of the present invention are as follows:

[0059] 1) The present invention adopts a grasping control method of the mounting robot based on predictive control, which can effectively deal with the visual field constraint and the actuator speed constraint, ensuring that the robot arm can move within the field of view of the camera while ensuring that it does not exceed the actuator speed range.

[0060] 2) Based on the delay-compensated MPC algorithm of the present invention, the feature point trajectory and camera trajectory are smoother, the feature point error changes more steadily, and the robot arm joint speed changes more steadily. The pixel error curve and the robot arm speed change curve fluctuate less and change more steadily; therefore, the delay-compensated MPC algorithm has a more stable and precise control effect in the object grasping experiment.

[0061] 3) The present invention solves the problem of time delay caused by data acquisition conversion, data transmission and data calculation, which causes cumulative errors in the joints of the robotic arm, thereby reducing the time delay errors caused by various parts of the system and improving the control accuracy and stability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 A schematic diagram of the robot arm grasping of the present invention;

[0063] Figure 2 It is a schematic diagram of the overall flow of the predictive control algorithm of the present invention;

[0064] Figure 3 This is a flow chart of the classic MPC algorithm (prediction part);

[0065] Figure 4 It is a schematic diagram of the flow of the delay compensation MPC algorithm (prediction part) of the present invention;

[0066] Figure 5 This is a trajectory diagram of a camera in a world coordinate system according to an embodiment of the present invention. DETAILED DESCRIPTION

[0067] In order to make the technical problems, solutions and advantages to be solved by the present invention more clear, a detailed description will be given below with reference to the accompanying drawings and specific examples.

[0068] like Figure 1 The hardware platform of the present invention is shown in the figure, including a photovoltaic panel installation robot 1, a robot arm grasping device 2, a photovoltaic panel 3, and a bracket 4; the grasping device includes a suction cup 5, a visual camera 1 6, a visual camera 2 7, and a telescopic rod 8.

[0069] The action flow of the robotic arm: first, the robotic arm extends to the vicinity of the upper bracket, and obtains the coordinates and depth information of the four corner points of the bracket respectively, and the pixel coordinates of the corner point feature position are used as the target pixel coordinates; the target pixel coordinates are transformed to the pixel coordinates when the telescopic rod is unfolded; the robotic arm grabs the photovoltaic panel; the telescopic rod is unfolded upward and downward respectively to ensure that the camera can find the coordinates of the four feature points; when the pixel coordinates of the four feature points are exactly the same as the target pixel coordinates, the robotic arm stops, puts down the photovoltaic panel, and the telescopic rod returns to the initial state.

[0070] Algorithm flow: Figure 2 As shown in Figure 1, the algorithm is divided into two stages: initialization stage and predictive control stage.

[0071] In the initialization stage, the robot arm is first modeled according to the DH parameters of the robot arm, and then the initial joint angle of the robot arm is obtained and the rotation transformation matrix from the robot arm end coordinate system to the world coordinate system is calculated. And the rotation transformation matrix from the robot end coordinate system to the camera coordinate system , and obtain the camera's intrinsic matrix at the same time. Then use the camera to obtain the image, obtain the pixel coordinates and depth information of the four feature points of the object, and determine the target pixel coordinates of the feature points. After these are determined, define the system state equation matrix and the weight matrix of the cost function, and then calculate the parameter matrix needed in the calculation process. After the calculation is completed, the program initialization ends.

[0072] In the predictive control stage, the pixel errors of the four feature points are calculated in each cycle, and the constraint matrix is ​​calculated based on the current pixel coordinates and the joint angle of the robot arm. The constraint matrix is ​​brought into the quadratic programming problem of the cost function to solve the optimal solution and obtain the predicted joint velocity value. The velocity value is then sent to the robot arm controller to control the robot arm to move. At the same time, the delay is set to meet the cycle length. When the delay ends, it means that this cycle is about to end. At this time, the new robot arm joint angle, feature point pixel coordinates, depth information, and related parameter matrix are read from the controller, and the next cycle is entered for calculation. This cycle is repeated until the pixel error is small enough (less than the set error value) to complete the control task.

[0073] In the above-mentioned classic MPC algorithm, data collection and matrix operations need to be completed instantly, but when it is applied in practice, it faces many delay problems. Figure 3 As shown in the predictive control stage, the system Start reading the joint angle at Get the pixel coordinates and depth information of the feature points at all times. The speed value prediction operation is completed at the moment. When the operation is completed, the time since the last angle acquisition has elapsed. time, which is the time since the last feature point pixel coordinates and depth information collection During this period of time, the robot arm is still running at the speed of the previous cycle, so the joint angle at this time is different from the value when the angle was collected. The parameter matrix calculated by the angle information collected at the moment does not correspond to the actual parameters at this moment. The pixel coordinates of the feature points at this moment are also different from the values ​​when the coordinates were collected. The optimal value of the solved quadratic programming is also different from the optimal value in the actual situation. This will cause calculation errors and have an adverse effect on the control process of the robot arm. When the controller performance is insufficient, If the time is too long and there are other errors in the robot arm, the visual servoing task may even fail.

[0074] To address this situation, the present invention designs an MPC algorithm with delay compensation. The initialization process remains unchanged. The process in the prediction phase is as follows: Figure 4 As shown, still Get the robot arm joint angle at all times, but After collecting the pixel coordinates and depth information of the feature points at all times, the time interval is calculated according to the current joint angular velocity. calculate The new joint angle at that moment, and then according to The world coordinates of the estimated feature points are calculated based on the joint angle at the moment, the pixel coordinates of the feature points, and the depth information. The joint angle at the moment and the current joint angular velocity according to time Estimate the next cycle after the forecast calculation is completed The joint angle at the moment is The estimated joint angle at the moment is calculated with the previously calculated feature point world coordinates. The pixel coordinates and depth information of the feature points at the moment are used to calculate the corresponding parameter matrix, and finally the cost function is solved to obtain the final optimal value. Moment and The reading data at the moment predicts the next cycle Theoretical data at the moment, all parameters are adjusted uniformly to time, and then solve the quadratic programming according to the classic MPC algorithm, and obtain The optimal control value of the state at all times reduces the errors caused by delays in various aspects of the system and the inconsistency of various data at all times. , , , The data was collected by taking the average of multiple tests. The program was run 100 times, and then the average was calculated for each time to minimize data errors.

[0075] Initial parameter settings for the six-axis robotic arm of the photovoltaic panel installation robot. The camera output is set to 960×720 and the internal parameters are , , , , the six initial joint angles of the robot are set to 0, 0, -90°, 0, 90°, 0. According to formula (1), the state equation is established. According to the objective function, the weight matrix , is a diagonal matrix with diagonal element values ​​set to 10 and weight matrix It is also a diagonal matrix, with the diagonal elements set to 600 and the time domain set to The angular velocity of the robot joint is limited to between -0.2 rad / s and 0.2 rad / s, and the field of view and camera retreat constraints are set according to equations (5) and (12). The initial positions of the feature points are set to (309, 226), (398, 220), (399, 302), (309, 307), and the corresponding initial depths are 230 mm, 228 mm, 225 mm, and 227 mm. The target pixel coordinates are set to (431, 321), (573, 317), (578, 446), and (431, 449). The total simulation period is 110 ms. t 1 to t4 are 20ms, 30 ms, 30 ms, and 30 ms respectively. The overall algorithm is implemented using Matlab language, and the computer model used is Dell G15-5511, the CPU model is 16G Micron DDR4, and the GPU model is 6GB GeForce RTX3060.

[0076] Figure 5 The trajectory diagram of the camera in the world coordinate system under the two algorithms. It can be seen from the figure that before the object moves, the trajectory under the classic MPC algorithm has a protrusion and then turns back at the turning point. After the object moves, the camera moves with the object. In the process of approaching the object, the camera trajectory under the classic MPC algorithm has a protrusion and turns back again. The camera trajectory under the improved algorithm is smooth without protrusions, and the overall path is better.

[0077] The above comparison results show that although both algorithms can complete the task in the dynamic object grasping experiment, the feature point trajectory and camera trajectory under the control of the classic algorithm have the phenomenon of protrusion and then folding back. Under the control of the improved delay compensation MPC algorithm, the feature point trajectory and camera trajectory are smoother, the feature point error changes more steadily, and the robot arm joint speed changes more steadily. Therefore, the delay compensation MPC algorithm has a more stable and precise control effect in the dynamic object grasping experiment.

[0078] The above embodiments are only preferred embodiments of the present invention and are not limitations of the technical solutions of the present invention. Any technical solution that can be implemented on the basis of the above embodiments without creative work should be deemed to fall within the scope of protection of the patent of the present invention.

Claims

1. A control method for a photovoltaic panel installation robot grasping system based on predictive control, characterized in that: The grasping system includes a six-axis robot arm and a grasping device, wherein the grasping device includes a suction cup, a visual camera and a telescopic rod, wherein the suction cup is arranged on the six-axis robot arm, and the visual camera is arranged on the six-axis robot arm through the telescopic rod; The control method comprises the following steps: Step 1) State equation: Establish the state equation of the robotic arm of the photovoltaic panel installation robot, and establish the visual servo kinematic model of the robotic arm by deducing the state equation; Step 2) Design a visual servo prediction controller: construct a cost function based on the state equation and actual requirements, and obtain the optimal control amount by minimizing the cost function when making predictions in each cycle; Step 3) Field of view constraint: Set field of view constraint conditions for the system. When predicting the speed in each cycle, ensure that the pixel coordinates of the next cycle obtained based on the predicted speed are within the image range, thereby obtaining the speed limit range; Step 4) Camera retreat constraint: During the control process, in order to prevent the camera from retreating when the robot moves, a camera retreat constraint is set for the system; Step 5) Delay: Set the delay to meet the cycle length. When the delay ends, it means that this cycle is about to end. Read the joint angle from the controller and enter the next cycle for calculation. Repeat this process until the pixel error is less than the set error value and the control task is completed.

2. The control method of the photovoltaic panel installation robot grasping system based on predictive control according to claim 1 is characterized in that: Step 1) is as follows: The state equation of the robot arm is: x(k+1)=x(k)+B(k)×v(k) (1) Among them, x(k+1) is the pixel coordinates of the four feature points in the k+1th period, x(k) is the pixel coordinates of the four feature points in the kth period, B(k) is the input matrix, v(k) is the angular velocity of the six joint motors of the six-axis robot arm; the four feature points refer to the coordinates of the four corner points of the upper bracket.

3. The control method of the photovoltaic panel installation robot grasping system based on predictive control according to claim 2 is characterized in that: Step 2) is as follows: The cost function is defined as follows: Among them, N is the prediction step size, e(k+n|k) represents the pixel coordinate error of the next few steps predicted in the kth cycle, v(k+n|k) represents the joint angular velocity of the next few steps predicted in the kth cycle, and e(k+N|k) represents the future terminal pixel coordinate error predicted in the kth cycle; Q, R, and F are weight matrices.

4. The control method of the photovoltaic panel installation robot grasping system based on predictive control according to claim 3 is characterized in that: Step 3) is as follows: The output image size of the camera is 960×720, so according to formula (1): Rearranging formula (3), we get: Among them, x max =[960 720 960 720 960 720 960 720 960 720] T , x min =[0 0 0 0 0 0 0 0] T ,make Further merging, the field of view constraint equation is: A C1 ×v(k)<B C1 (5)。 5. The control method of the photovoltaic panel installation robot grasping system based on predictive control according to claim 4 is characterized in that: Step 4) is as follows: At the initial moment, the depth from the feature point to the camera is recorded as the maximum depth, and the state equation about the depth is obtained. When predicting the speed in each cycle, it is necessary to ensure that the depth of the next cycle obtained based on the predicted speed is less than the maximum depth, so as to obtain the speed limit range; Define the Z of the feature point C Derivatives of coordinates: in, Represents the rotation transformation matrix of coordinate system a relative to coordinate system b; the world coordinate system w, the robot end coordinate system e, and the camera coordinate system C are represented by the symbol T c (j) indicates T c The jth row of T c (1), T c (2), T c (3); P w Refers to the coordinates of the feature points in the world coordinate system; the joint angle θ(t) of the robot arm; make Then the state equation of depth information in discrete system is obtained: With c (k+1)=z c (k)+Ω z (k)×t×v(k) (7) For a system with four feature points, the depth information in the camera coordinate system is z c1 、z c2 、z c3 、z c4 , then: Let z c (k) = [z c1 (k)z c2 (k)z c3 (k)z c4 (k)] T , B z (k) = [Ω z1 (k)Ω z2 (k)Ω z3 (k)Ω z4 (k)] T ×t, the depth information state equation of the four feature point system is obtained as: z c (k+1)=z c (k)+B z (k)×v(k) (9) Construct constrained inequalities: With c (k)+B z (k)×v(k)<[z c1max With c2max With c3max With c4max ] T (10) Let z cmax =[z c1max z c2max z c3max z c4max ] T ,A C2 =B z (k),B C2 =z cmax -z c (k), the camera retreat constraint equation is obtained as: A C2 ×v(k)<B C2 (11) Combining equations (5) and (11), let The total constraint equation is obtained: A C ×v(k)<B C (12) Since the prediction step is N, it is necessary to set the constraint equation of the same dimension when solving. According to formula (12), the final constraint equation is: So far, the constraint equations of the system have been derived; According to formula (1), the state of the system at time k is: x(k+n+1|k)=f(x(k+n|k),v(k+n|k)),n∈[0,N-1] (14) x(k+n+1|k) represents the predicted value of the state quantity x at time k for time k+n+1, x represents the pixel coordinates, and f represents the mapping relationship; Considering constraint (13), the controller of the system is: V k =arg min(J) (15) Among them, x(k|k) is the initial pixel value of the feature point, e(k|k) is the initial pixel error value, and e min is the lower limit of error, e max is the upper limit of error, v min is the lower limit of joint velocity, v max is the upper limit of joint speed, V k is the robot joint velocity sequence, x t is the pixel coordinate value of the target point; In each cycle, the velocity sequence V is obtained according to formula (15): k , and take the first set of data as the robot arm joint speed and send it to the controller.

6. The control method of the photovoltaic panel installation robot grasping system based on predictive control according to claim 1, characterized in that: Step 5) is as follows: One cycle includes T1 moment - calculating the pixel error of the feature point, T2 moment - sending the joint speed, T3 moment - reading the joint angle, T4 moment - obtaining the pixel coordinates and depth information of the feature point, T5 moment - calculating the parameter matrix; among which T1 moment to T2 moment is the time period t1, T1 moment to T3 moment is the time period t2, T3 moment to T4 moment is the time period t3, T4 moment to T5 moment is the time period t4; The joint angle of the robot arm is obtained at time T3. After the pixel coordinates and depth information of the feature points are collected at time T4, the new joint angle at time T4 is calculated according to the current joint angular velocity according to the time period t3. Then, the world coordinates of the estimated feature points are calculated according to the joint angle at time T4 and the pixel coordinates and depth information of the feature points. Then, according to the joint angle at time T4 and the current joint angular velocity according to the time t4+t1, the joint angle at the next cycle T2 after the predicted calculation is completed is estimated. According to the estimated joint angle at time T2 and the previously calculated feature point world coordinates, the pixel coordinates and depth information of the feature points at time T2 are calculated, and the corresponding parameter matrix is ​​calculated. Finally, the cost function is solved to obtain the final optimal value.

Images