Pig farm track robot video anti-shake interpolation method based on pwm and foc

Abstract

The application discloses a pig farm track robot video anti-shake interpolation method based on PWM and FOC, relates to the technical field of track robot video anti-shake, and comprises the following steps: feedback control: according to the current attitude error, the motor adjustment amount is calculated, and the video frame is transformed and supplemented in combination with IMU data; feedforward control: according to track flaw data learned from history, the motor is adjusted in advance at a specific position, and the attitude change of the robot is compensated by controlling the reverse motion of the motor; in the feedforward control, a learning model is loaded to record the attitude error and the motor adjustment parameter of the robot at the corresponding position on the track through encoder positioning, and the motor and the video frame transformation data are adjusted when the same position is passed next time. The application realizes the improvement of the attitude stability of the track robot, improves the stability of the collected video image, reduces the image shaking amplitude, and has a fast response time.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of track robot video anti-shake technology, and particularly relates to a pig farm track robot video anti-shake interpolation method based on PWM and FOC. BACKGROUND

[0002] During the process of the pig farm track robot running on the I-shaped track, due to the inherent defects of the track unevenness such as the welding seam, deformation and installation error of the I-shaped steel track, the load dynamically changes: the robot carries detection equipment with different weights, the center of gravity changes constantly, long-term running wear: the wear between the track and the transmission wheel causes the gap to increase, environmental interference: the changes of temperature and humidity in the pig house, vibration interference and the like, which cause the posture of the pig farm track robot to change during the running under the special working condition, and affect the data acquisition stability of the video image.

[0003] The existing passive suspension system cannot compensate for low-frequency posture deviation, fixed parameter control: cannot adapt to the characteristic changes of different sections of the track, and lag response: the traditional PID control has phase lag, and cannot realize timely compensation. SUMMARY

[0004] The present application provides a pig farm track robot video anti-shake interpolation method based on PWM and FOC, which solves the problems of the prior art.

[0005] In the first aspect, the present application provides a pig farm track robot video anti-shake interpolation method based on PWM and FOC, records the posture error and the corresponding control parameter each time through each position of the track, including: collecting the posture data and acceleration of the track robot, measuring the rotation of the wheel, adjusting the torque and speed of each motor based on PWM vector control to compensate for the posture change, and specifically including the following control strategy:

[0006] Feedback control: calculate the motor adjustment amount according to the current posture error, and transform and compensate the video frame in combination with the IMU data; feedforward control: adjust the motor in advance at a specific position according to the learned track defect data, and compensate for the posture change of the robot by controlling the reverse motion of the motor;

[0007] In the feedforward control, the learning model is loaded to record the posture error and motor adjustment parameter of the robot at the corresponding position on the track through the encoder positioning, and when passing through the same position next time, the motor and the video frame transformation data are adjusted.

[0008] Further, including: initializing track robot running, reading IMU, encoder data, calculating current position on track according to encoder, calculating current attitude error, performing feedback control calculation, generating motor control PWM signal, outputting to motor, collecting video frame, combining IMU data to perform electronic anti-shake processing, recording current segment attitude error and control amount, updating learning system.

[0009] Further, including timestamp synchronization technology as follows:

[0010] Corresponding to the acquisition of IMU data timestamp, motor control timestamp, video frame timestamp and configuration synchronization tolerance respectively;

[0011] Align the timestamps of all sensors and control instructions.

[0012] Further, including: initializing synchronization frequency, synchronously collecting all sensor data, generating a synchronization pulse and then synchronously collecting each sensor data, timestamp alignment for each sensor, and applying hardware supplementary delay.

[0013] Further, for the current attitude error calculation, the attitude change detection and quantization steps are as follows:

[0014] The current robot attitude calculated using IMU and encoder data includes pitch, roll, yaw and attitude change rate;

[0015] Kalman filter fuses IMU and encoder data to obtain stable attitude estimation;

[0016] Calculate the deviation of the current attitude from the expected attitude, i.e. the horizontal attitude, as the attitude error;

[0017] Calculate the attitude change rate by gyroscope or attitude difference.

[0018] Further, for motor control calculation, including feedback calculation and feedforward calculation, specifically including:

[0019] Compensate for attitude changes by adjusting the torque of two drive wheels, the two drive wheels are arranged on both sides of the I-beam track, the feedback control calculates the compensation torque according to the current attitude error, the feedforward control outputs the compensation torque in advance according to the learned track characteristics, the specific calculation is:

[0020] Feedback control: using PID controller, calculating motor torque according to attitude error, torque feedback = Kp * attitude error + Kd * attitude change rate; Wherein, Kp is the proportional gain matrix, used to feedback control the response strength of the current attitude error, Kd is the differential gain matrix, used to feedback control the damping of the attitude change rate; Adjustment mechanism is to increase Kp can improve the response speed, but the excessive will lead to oscillation, increase Kd can enhance the damping, but the excessive will reduce the response speed;

[0021] Feedforward control: obtaining feedforward compensation torque of current track position from learning system;

[0022] Adding feedback and feedforward, getting the final torque instruction of each motor, output to motor driver through PWM;

[0023] Motor independently controls the torque of two wheels, when detecting that the track robot is forward inclined, i.e. the pitch angle is positive, at the same time, increasing the torque of two wheels to accelerate, so that the robot restores to horizontal; When detecting roll, i.e. there is roll angle, adjusting through differential torque of two wheels.

[0024] Further, the compensation of the attitude change of the robot by controlling the reverse motion of the motor is a part of the attitude change, and the remaining part of the attitude change is compensated by using video frame transformation compensation frame, and the calculation step comprises:

[0025] Using the attitude data obtained from the IMU and the motor control amount corresponding to the final torque instruction of each motor to estimate the motion model of the camera;

[0026] According to the motion model, inversely transforming the video frame;

[0027] Using frame interpolation technology to smooth the inter-frame transition.

[0028] Further, for the learning model, the learning model records the attitude error and the best feedforward compensation parameter corresponding to each position after the track robot runs on the same track multiple times, when passing the same position next time, applying feedforward compensation in advance, and predicting video compensation frame parameter, the specific calculation method is:

[0029] Dividing the I-shaped track into multiple intervals, recording a group of data in each interval: position, attitude error, successfully compensated motor feedforward parameter, video compensation frame parameter;

[0030] Based on neural network algorithm, mapping relationship from position to feedforward parameter and compensation frame parameter is established;

[0031] During running, querying the learning model according to the current position, getting the feedforward parameter and the predicted compensation frame parameter.

[0032] The pig farm track robot video anti-shake interpolation method based on PWM and FOC has improved attitude stability, improved video stability, reduced image jitter amplitude and fast response time. BRIEF DESCRIPTION OF DRAWINGS

[0033] The accompanying drawings, which are included to provide a further understanding of the embodiments of the application and are incorporated in and constitute a part of this application, illustrate embodiments of the application and together with the description serve to explain the principles of the application. In the drawings:

[0034] Figure 1 The pig farm track robot video anti-shake interpolation method based on PWM and FOC provided by an exemplary embodiment of the application is shown in the flowchart.

[0035] Figure 2 The track robot device diagram provided by an exemplary embodiment of the application is shown.

[0036] Figure 3 The track robot erection track I-beam track diagram provided by an exemplary embodiment of the application is shown.

[0037] In the figure: 1, brushless DC motor; 2, I-beam track; DETAILED DESCRIPTION

[0038] The exemplary embodiments will be described in detail herein with reference to the accompanying drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The implementations described in the following exemplary embodiments are not meant to represent all implementations consistent with the present application.

[0039] The pig farm track robot video anti-shake interpolation method based on PWM and FOC provided by the application aims to solve the above technical problems of the prior art.

[0040] The technical solutions of the application and how the technical solutions of the application solve the above technical problems will be described in detail below with specific embodiments. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described again in some embodiments. The embodiments of the application will be described below with reference to the drawings.

[0041] Embodiment 1: This embodiment is implemented according to the following steps, as shown in the figure: Figure 1

[0042] S1, system initialization and sensor data reading;

[0043] Initialize IMU, encoder, motor driver and camera.

[0044] Read IMU data (accelerometer and gyroscope) and encoder data (wheel rotation angle or speed). ​

[0045] S2, timestamp synchronization;

[0046] Using hardware trigger or software synchronization, ensure the timestamps of IMU, encoder and camera data are aligned.

[0047] S3, pose estimation and error calculation;

[0048] Using IMU and encoder data, estimate the robot's pose (pitch, roll, yaw) through Kalman filter.

[0049] Calculate the error between current pose and desired pose (level);

[0050] Using Kalman filter to fuse IMU and encoder data, state vector: x = [θ, β]^T, where θ is the attitude angle (pitch, roll, yaw), β is the bias of the gyroscope; pitch θ = atan2(ay, az), state transition equation for Kalman filter pose estimation:

[0051] x_k = F * x_{k-1} + B * u_k,

[0052] z_k = H * x_k,

[0053] k is the time;

[0054] where F = [1, -dt; 0, 1] (for each attitude angle, here is an example of a single angle, actually three angles), u_k = gyro_k (angular velocity), B = [dt; 0].

[0055] Motion model:

[0056] θ_{k} = θ_{k-1} + (gyro_{k-1} - β_{k-1}) * dt

[0057] β_{k} = β_{k-1}, dt: time step (seconds), used to control the period time, take 0.002 seconds corresponding to 500Hz control frequency;

[0058] Observation model: accelerometer measurement: accel = [ax, ay, az]^T, where ax, ay, az are accel_x, accel_y, accel_z respectively, representing three-axis accelerometer measurement (m / s²);

[0059] Assuming that the accelerometer measures the gravity, the acceleration of gravity, about 9.8 m / s² when stationary, so the attitude can be estimated with the accelerometer. Using the accelerometer measurements to observe the pitch and roll, pitch: pitch angle (radians), roll: roll angle (radians), yaw is observed with the encoder (when the encoder cannot provide yaw information, yaw is obtained by gyroscope integration);

[0060] Pitch angle calculation: θ = atan2(ay, az), pitch = math.atan2(accel_y, math.sqrt(accel_x**2 + accel_z**2));

[0061] Roll angle calculation: φ = atan2(-ax, sqrt(ay² + az²)), roll = math.atan2(-accel_x, math.sqrt(accel_y**2 + accel_z**2))

[0062] Standard formula for prediction and update steps of Kalman filter:

[0063] Attitude error: error = desired_attitude - current_attitude (current attitude), where the desired attitude desired_attitude is level, i.e. pitch and roll are 0;

[0064] S4, motor control (feedback + feedforward);

[0065] Feedback control: according to the attitude error and the rate of change, the feedback torque is calculated by PID;

[0066] torque_fb: feedback torque vector [torque_pitch, torque_roll, torque_yaw], torque_fb = kp * error + kd * error_rate, where kp: proportional gain vector [kp_pitch, kp_roll, kp_yaw], kd: differential gain vector [kd_pitch, kd_roll, kd_yaw], error: attitude error vector, error_rate: error rate vector;

[0067] Feedforward control: get the feedforward compensation torque of the current track position from the learning model;

[0068] Add feedback and feedforward to get the torque command of each motor, convert torque to PWM duty ratio through FOC algorithm, and output through PWM.

[0069] S5, video stabilization processing;

[0070] Estimate camera motion model using IMU data (and motor control), inverse transform video frames and smooth using inter-frame interpolation.

[0071] Estimate camera motion model, including translation and rotation, since the camera is fixed on a track robot, the pose change of the track robot will directly cause the camera motion, use IMU data (rotation) and encoder data (translation) to estimate the camera motion, for each frame of image, calculate an affine transformation matrix, which is the inverse transform of the camera motion, to offset the motion, then use the affine transformation to remap the image.

[0072] S6, learning model update and use;

[0073] Record the state of each track position (pose error, feedforward parameter, video frame interpolation parameter), use neural network or other machine learning algorithm to establish mapping, during running, query the learning model according to the current position to get the feedforward parameter and the predicted frame interpolation parameter.

[0074] The training process is as follows:

[0075] The input is the track position (encoder position), and the output is the feedforward torque and the video frame interpolation parameter, i.e. the parameter of the affine transformation;

[0076] In multiple runs, record the position, pose error, and motor feedforward parameter and video frame interpolation parameter after successful compensation, use gradient descent method until the loss function converges; the loss function is the training target: minimize the weighted sum of pose error and video jitter, when the feedforward parameter and frame interpolation parameter at the same position make the pose error and video jitter less than the threshold, and the multiple run parameters are stable, it is considered to be converged.

[0077] Embodiment 2: hardware configuration, sensors: MPU9250 (IMU), AS5048A (magnetic encoder); actuators: brushless DC motor × 2, FOC driver × 2;

[0078] As shown in Figure 2 , the brushless DC motor 1, as shown in Figure 3 , the I-shaped track 2;

[0079] The motor control calculation is based on FOC+PWM implementation, motor pole pairs = 7, flux linkage (Wb) = 0.1, phase resistance (Ω) = 0.5, phase inductance (H) = 0.001, torque is converted into PWM signal; parameters: torque: desired torque (Nm), rotor_angle: rotor electrical angle (radians), dc_bus_voltage = 24, DC bus voltage (V), torque to q-axis current conversion, d-axis current reference (usually set to 0 to achieve maximum torque control), inverse Park transform (dq -> αβ), inverse Clark transform (αβ -> ABC), space vector modulation (SVM): calculate reference voltage vector angle and amplitude, perform sector judgment, calculate basic vector action time, calculate PWM duty cycle; return parameter pwm_duty: three-phase PWM duty cycle [A, B, C].

[0080] The modules described as separate components may or may not be physically separate, and the components shown as modules may or may not be physical modules, i.e., they may be located in one place or distributed to multiple network modules. Some or all of the modules can be selected according to actual needs to achieve the purpose of the embodiment of the present application.

[0081] In addition, each functional module in the embodiment of the present application can be integrated in one processing module, or each module can exist physically alone, or two or more modules can be integrated in one module. The integrated module can be realized in the form of hardware or in the form of hardware plus software function module.

[0082] Those skilled in the art will appreciate that embodiments of the application can be provided as a method or system. Therefore, the present application can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects.

[0083] It should also be noted that the terms "include", "contain" or any other variant thereof are intended to cover non-exclusive inclusion, so that the process, method, product or device including a series of elements not only includes those elements, but also includes other elements not explicitly listed or inherent to such process, method, product or device. Without more limitations, the element defined by the statement "including a" does not exclude the presence of another identical element in the process, method, product or device including the element.

[0084] The above merely provides the embodiment of the present application, but does not serve to limit the present application. For those skilled in the art, the present application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the scope of claims of the present application.

[0085] Other embodiments of the present application will be apparent to those skilled in the art from consideration of the specification and practice of the application disclosed here. For example, many of the components and / or methods described herein have multiple possible configurations, which have not been expressly shown or described herein to avoid obscuring the novelty of the present application. It will be appreciated by persons skilled in the art that within the scope of the application that various modifications and changes can be made. It is therefore intended that the present application not be limited to the illustrative embodiments set forth above but that the present application can be practiced with a wide and equivalent range of modifications and changes. The specification and examples given herein are primarily for that purpose and are not to be taken as limiting the application, as its scope is to be measured by the broadest permissible scope of the claims set forth below.

[0086] It should be understood that the application is not limited to the precise construction and compositions that have been described above and shown in the accompanying drawings, and that various modifications and changes can be made by those skilled in the art without departing from the scope of the application. The scope of the application is limited only by the claims that follow.

Claims

1. A pig farm track robot video anti-shake interpolation method based on PWM and FOC, records the attitude error and the corresponding control parameters at each time passing through each position of the track, characterized in that, Comprise: Collect the posture data and acceleration of the track robot, measure the rotation of the wheel, adjust the torque and speed of each motor based on PWM vector control to compensate for the change in posture, including the following control strategy: Feedback control: calculate the motor adjustment amount according to the current attitude error, and transform and fill in the video frame in combination with the IMU data, feedforward control: according to the track defect data learned in history, adjust the motor in advance at a specific position, compensate for the change in the posture of the robot by controlling the reverse motion of the motor; wherein, feedback control: according to the attitude error and the rate of change, the feedback torque is calculated by PID; torque_fb: feedback torque vector [torque_pitch, torque_roll, torque_yaw], torque_fb = kp * error + kd * error_rate, wherein, kp: proportional gain vector [kp_pitch, kp_roll, kp_yaw], kd: differential gain vector [kd_pitch, kd_roll, kd_yaw], error: attitude error vector, error_rate: error rate vector; Feedforward control: obtain the feedforward compensation torque of the current track position from the learning model; add the feedback and feedforward to obtain the torque instruction of each motor, convert the torque to PWM duty cycle through FOC algorithm, and output through PWM; Wherein, video anti-shake processing: estimate the camera motion model using IMU data and motor control amount, reverse transform the video frame, and use inter-frame interpolation to smooth; Estimate the motion model of the camera, including translation and rotation, since the camera is fixed on the track robot, the change in the posture of the track robot will directly cause the camera to move, use IMU data to estimate the camera motion corresponding to the rotation and encoder data to estimate the camera motion corresponding to the translation, for each frame of image, calculate an affine transformation matrix, which is the inverse transformation of the camera motion, to offset the motion, then use affine transformation to remap the image; including: initialize the track robot to run, read the IMU and encoder data, calculate the current position on the track according to the encoder, calculate the current attitude error, perform feedback control calculation, generate motor control PWM signal, output to the motor, collect video frame, combine IMU data for electronic anti-shake processing, record the current segment of the attitude error and control amount, and update the learning system; In the feedforward control, load the learning model to record the attitude error and motor adjustment parameters of the robot at the corresponding position on the track through the encoder positioning, and adjust the motor and video frame transformation data when passing through the same position next time, wherein, the compensation for the change in the posture of the robot by controlling the reverse motion of the motor is for part of the change in the posture, and the remaining part of the change in the posture is compensated by video frame transformation and filling, the calculation steps comprising: Estimate the motion model of the camera using the posture data obtained from the IMU and the motor control amount corresponding to the final torque instruction of each motor; Reverse transform the video frame according to the motion model; Inter-frame interpolation techniques are used to smooth inter-frame transitions.

2. The PWM and FOC based pig farm rail robot video anti-shake interpolation method according to claim 1, characterized in that, The timestamp synchronization technique includes the following: Respectively correspond to the IMU data timestamp, motor control timestamp, video frame timestamp and configuration synchronization tolerance; Align the timestamps of all sensors and control instructions.

3. The PWM and FOC based pig farm rail robot video anti-shake interpolation method according to claim 2, characterized in that, It includes: Initialize the synchronization frequency, synchronize the collection of all sensor data, generate a synchronization pulse, and then synchronize the collection of each sensor data again, align the timestamps of each sensor, and apply hardware supplementary delay.

4. The PWM and FOC based pig farm rail robot video anti-shake interpolation method according to claim 3, characterized in that, For the current attitude error calculation, the attitude change detection and quantization steps are as follows: Calculate the current attitude of the robot using IMU and encoder data, including pitch, roll, yaw, and attitude change rate; Kalman filter fusion of IMU and encoder data to obtain stable attitude estimation; Calculate the deviation of the current attitude from the expected attitude, i.e., the horizontal attitude, as the attitude error; Calculate the attitude change rate through the gyroscope or attitude difference.

5. The PWM and FOC based pig farm rail robot video anti-shake interpolation method according to claim 1, characterized in that, The motor control calculation includes feedback calculation and feedforward calculation, which specifically includes: Compensate for attitude changes by adjusting the torque of two drive wheels, which are respectively arranged on both sides of the I-beam track, the feedback control calculates the compensation torque according to the current attitude error, and the feedforward control outputs the compensation torque in advance according to the learned track characteristics, the specific calculation is as follows: Feedback control: use a PID controller to calculate motor torque based on attitude error, torque feedback = Kp * attitude error + Kd * attitude change rate; where Kp is the proportional gain matrix, used to control the response strength of the current attitude error, Kd is the differential gain matrix, used to control the damping effect of the attitude change rate; Feedforward control: obtain the feedforward compensation torque of the current track position from the learning system; Add feedback and feedforward to get the final torque command of each motor, and output it to the motor driver through PWM; The motor independently controls the torque of the two wheels, when it is detected that the track robot is tilted forward, i.e., the pitch angle is positive, the torque of the two wheels is increased to accelerate, so that the robot restores to horizontal; when it is detected that the track robot is tilted sideways, i.e., there is a roll angle, the differential torque of the two wheels is adjusted.

6. The PWM and FOC based pig farm rail robot video anti-shake interpolation method according to claim 5, characterized in that, For the learning model, the learning model records the attitude error and optimal feedforward compensation parameter corresponding to each position after the track robot runs on the same track multiple times, and applies feedforward compensation and predicts video frame interpolation parameters when passing through the same position next time, the specific calculation method is as follows: Divide the I-beam track into multiple intervals, and record a set of data for each interval: position, attitude error, successful motor feedforward compensation parameter, and video frame interpolation parameter; Based on neural network algorithm, establish the mapping relationship from position to feedforward parameter and frame interpolation parameter; During operation, query the learning model according to the current position to obtain the feedforward parameter and the predicted frame interpolation parameter.

Images