java - 树莓派四轴飞行器高速运转

Question

我正在尝试构建一个基于树莓派的四轴飞行器。到目前为止，我已经成功地与所有硬件连接，并且编写了一个在低 throttle 下相当稳定的 PID Controller 。问题是，在较高的 throttle 下，四轴飞行器开始颠簸和抖动。我什至还没能把它付诸实践，我所有的测试都是在测试台上完成的。这是我的代码有问题，还是电机坏了？非常感谢任何建议。

这是到目前为止我的代码:

QuadServer.java:

package com.zachary.quadserver;

import java.net.*;
import java.io.*;
import java.util.*;

import se.hirt.pi.adafruit.pwm.PWMDevice;
import se.hirt.pi.adafruit.pwm.PWMDevice.PWMChannel;

public class QuadServer {
    private static Sensor sensor = new Sensor();

    private final static int FREQUENCY = 490;

    private static double PX = 0;
    private static double PY = 0;

    private static double IX = 0;
    private static double IY = 0;

    private static double DX = 0;
    private static double DY = 0;

    private static double kP = 1.3;
    private static double kI = 2;
    private static double kD = 0;

    private static long time = System.currentTimeMillis();

    private static double last_errorX = 0;
    private static double last_errorY = 0;

    private static double outputX;
    private static double outputY;

    private static int val[] = new int[4];

    private static int throttle;

    static double setpointX = 0;
    static double setpointY = 0;

    static long receivedTime = System.currentTimeMillis();

    public static void main(String[] args) throws IOException, NullPointerException {

        PWMDevice device = new PWMDevice();
        device.setPWMFreqency(FREQUENCY);

        PWMChannel BR = device.getChannel(12);
        PWMChannel TR = device.getChannel(13);
        PWMChannel TL = device.getChannel(14);
        PWMChannel BL = device.getChannel(15);

        DatagramSocket serverSocket = new DatagramSocket(8080);


        Thread read = new Thread(){
                public void run(){
                    while(true) {
                    try {
                            byte receiveData[] = new byte[1024];
                            DatagramPacket receivePacket = new DatagramPacket(receiveData, receiveData.length);
                            serverSocket.receive(receivePacket);
                            String message = new String(receivePacket.getData());
                            throttle = (int)(Integer.parseInt((message.split("\\s+")[4]))*12.96)+733;
                            setpointX = Integer.parseInt((message.split("\\s+")[3]))-50;
                            setpointY = Integer.parseInt((message.split("\\s+")[3]))-50;

                        receivedTime = System.currentTimeMillis();

                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }
                }
        };
        read.start();

        while(true)
        {
            Arrays.fill(val, calculatePulseWidth((double)throttle/1000, FREQUENCY));

            double errorX = -sensor.readGyro(0)-setpointX;
            double errorY = sensor.readGyro(1)-setpointY;

            double dt = (double)(System.currentTimeMillis()-time)/1000;

            double accelX = sensor.readAccel(0);
            double accelY = sensor.readAccel(1);
            double accelZ = sensor.readAccel(2);

            double hypotX = Math.sqrt(Math.pow(accelX, 2)+Math.pow(accelZ, 2));
            double hypotY = Math.sqrt(Math.pow(accelY, 2)+Math.pow(accelZ, 2));


            double accelAngleX = Math.toDegrees(Math.asin(accelY/hypotY));
            double accelAngleY = Math.toDegrees(Math.asin(accelX/hypotX));

            if(dt > 0.01)
            {

                PX = errorX;
                PY = errorY;

                IX += errorX*dt;
                IY += errorY*dt;

                IX = 0.95*IX+0.05*accelAngleX;
                IY = 0.95*IY+0.05*accelAngleY;

                DX = (errorX - last_errorX)/dt;
                DY = (errorY - last_errorY)/dt;

                outputX = kP*PX+kI*IX+kD*DX;
                outputY = kP*PY+kI*IY+kD*DY;
                time = System.currentTimeMillis();
            }

            System.out.println(setpointX);

            add(-outputX+outputY, 0);
            add(-outputX-outputY, 1);
            add(outputX-outputY, 2);
            add(outputX+outputY, 3);

            //System.out.println(val[0]+", "+val[1]+", "+val[2]+", "+val[3]);
            if(System.currentTimeMillis()-receivedTime < 1000)
            {
                BR.setPWM(0, val[0]);
                TR.setPWM(0, val[1]);
                TL.setPWM(0, val[2]);
                BL.setPWM(0, val[3]);
            } else 
            {
                BR.setPWM(0, 1471);
                TR.setPWM(0, 1471);
                TL.setPWM(0, 1471);
                BL.setPWM(0, 1471);
            }

        }
    }

    private static void add(double value, int i)
    {
        value = calculatePulseWidth(value/1000, FREQUENCY);
        if(val[i]+value > 1471 && val[i]+value < 4071)
        {
            val[i] += value;
        }else if(val[i]+value < 1471)
        {
            //System.out.println("low");
            val[i] = 1471;
        }else if(val[i]+value > 4071)
        {
            //System.out.println("low");
            val[i] = 4071;
        }
    }

    private static int calculatePulseWidth(double millis, int frequency) {
        return (int) (Math.round(4096 * millis * frequency/1000));
    }
}

传感器.java:

package com.zachary.quadserver;

import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.PinState;
import com.pi4j.io.gpio.RaspiPin;
import com.pi4j.io.i2c.*;
import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.PinState;
import com.pi4j.io.gpio.RaspiPin;
import com.pi4j.io.i2c.*;

import java.net.*;
import java.io.*;

public class Sensor {
    static I2CDevice sensor;
    static I2CBus bus;
    static byte[] accelData, gyroData;
    static long accelCalib[] = new long[3];
    static long gyroCalib[] = new long[3];

    static double gyroX = 0;
    static double gyroY = 0;
    static double gyroZ = 0;

    static double accelX;
    static double accelY;
    static double accelZ;

    static double angleX;
    static double angleY;
    static double angleZ;

    public Sensor() {
        //System.out.println("Hello, Raspberry Pi!");
        try {
            bus = I2CFactory.getInstance(I2CBus.BUS_1);

            sensor = bus.getDevice(0x68);

            sensor.write(0x6B, (byte) 0x0);
            sensor.write(0x6C, (byte) 0x0);
            System.out.println("Calibrating...");

            calibrate();

            Thread sensors = new Thread(){
                    public void run(){
                        try {
                            readSensors();
                        } catch (IOException e) {
                        System.out.println(e.getMessage());
                    }
                    }
            };
            sensors.start();
        } catch (IOException e) {
            System.out.println(e.getMessage());
        }
    }

    private static void readSensors() throws IOException {
        long time = System.currentTimeMillis();
        long sendTime = System.currentTimeMillis();

        while (true) {
            accelData = new byte[6];
            gyroData = new byte[6];
            int r = sensor.read(0x3B, accelData, 0, 6);
            accelX = (((accelData[0] << 8)+accelData[1]-accelCalib[0])/16384.0)*9.8;
            accelY = (((accelData[2] << 8)+accelData[3]-accelCalib[1])/16384.0)*9.8;
            accelZ = ((((accelData[4] << 8)+accelData[5]-accelCalib[2])/16384.0)*9.8)+9.8;
            accelZ = 9.8-Math.abs(accelZ-9.8);

            double hypotX = Math.sqrt(Math.pow(accelX, 2)+Math.pow(accelZ, 2));
            double hypotY = Math.sqrt(Math.pow(accelY, 2)+Math.pow(accelZ, 2));


            double accelAngleX = Math.toDegrees(Math.asin(accelY/hypotY));
            double accelAngleY = Math.toDegrees(Math.asin(accelX/hypotX));

            //System.out.println((int)gyroX+", "+(int)gyroY);

            //System.out.println("accelX: " + accelX+" accelY: " + accelY+" accelZ: " + accelZ);

            r = sensor.read(0x43, gyroData, 0, 6);
            if(System.currentTimeMillis()-time >= 5)
            {
                gyroX = (((gyroData[0] << 8)+gyroData[1]-gyroCalib[0])/131.0);
                gyroY = (((gyroData[2] << 8)+gyroData[3]-gyroCalib[1])/131.0);
                gyroZ = (((gyroData[4] << 8)+gyroData[5]-gyroCalib[2])/131.0);

                angleX += gyroX*(System.currentTimeMillis()-time)/1000;
                angleY += gyroY*(System.currentTimeMillis()-time)/1000;
                angleZ += gyroZ;

                angleX = 0.95*angleX + 0.05*accelAngleX;
                angleY = 0.95*angleY + 0.05*accelAngleY;

                time = System.currentTimeMillis();
            }
            //System.out.println((int)angleX+", "+(int)angleY);
            //System.out.println((int)accelAngleX+", "+(int)accelAngleY);
        }
    }

    public static void calibrate() throws IOException {
        int i;
        for(i = 0; i < 3000; i++)
        {
            accelData = new byte[6];
            gyroData = new byte[6];
            int r = sensor.read(0x3B, accelData, 0, 6);
            accelCalib[0] += (accelData[0] << 8)+accelData[1];
            accelCalib[1] += (accelData[2] << 8)+accelData[3];
            accelCalib[2] += (accelData[4] << 8)+accelData[5];

            r = sensor.read(0x43, gyroData, 0, 6);
            gyroCalib[0] += (gyroData[0] << 8)+gyroData[1];
            gyroCalib[1] += (gyroData[2] << 8)+gyroData[3];
            gyroCalib[2] += (gyroData[4] << 8)+gyroData[5];
            try {
                Thread.sleep(1);
            } catch (Exception e){
                e.printStackTrace();
            }
        }
        gyroCalib[0] /= i;
        gyroCalib[1] /= i;
        gyroCalib[2] /= i;

        accelCalib[0] /= i;
        accelCalib[1] /= i;
        accelCalib[2] /= i;
        System.out.println(gyroCalib[0]+", "+gyroCalib[1]+", "+gyroCalib[2]);
    }

    public double readAngle(int axis)
    {
        switch (axis)
        {
            case 0:
                return angleX;
            case 1:
                return angleY;
            case 2:
                return angleZ;
        }

        return 0;
    }

    public double readGyro(int axis)
    {
        switch (axis)
        {
            case 0:
                return gyroX;
            case 1:
                return gyroY;
            case 2:
                return gyroZ;
        }

        return 0;
    }

    public double readAccel(int axis)
    {
        switch (axis)
        {
            case 0:
                return accelX;
            case 1:
                return accelY;
            case 2:
                return accelZ;
        }

        return 0;
    }
}

Answer 1

您可以根据不同的工况尝试为 Controller 的增益设置不同的值。然后，您应该只能识别每种操作条件，相应地更改 PID 增益，并验证设计。在您的情况下，您可以尝试使用 throttle 或从可用传感器读取的其他相关变量来安排 PID 的增益。

搜索增益调度实现:

https://www.mathworks.com/help/control/ug/gain-scheduled-control-systems.html

这是一种非常有用的技术，它将线性控制设计应用于非线性系统，并取得了非常令人满意的结果。