Raspberry Pi quadcopter trrashes на высоких скоростях

Question

Я пытаюсь построить квадроцикл на основе малины. До сих пор мне удалось взаимодействовать со всем оборудованием, и я написал ПИД-регулятор, который достаточно стабилен при низком дросселе. Проблема в том, что при более высокой дроссельной заслонке квадроцикл начинает биться и дергаться. Я даже не смог его снять с земли, все мои испытания были проведены на стенде. Это проблема с моим кодом или, может быть, с плохим двигателем? любые предложения приветствуются.

вот мой код до сих пор:

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)); 
    } 
} 

Sensor.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

Вы можете попробовать настройки различных значений коэффициентов усиления контроллера основанные на различных условиях эксплуатации. Тогда вы должны быть в состоянии идентифицировать каждое условие операции, соответственно изменить коэффициент усиления вашего PID и проверить дизайн. В вашем случае вы можете попытаться запланировать усиление PID с дросселем или другой связанной переменной, считанной с доступных датчиков.

Поиск реализаций Scheduling Gain:

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

Это очень полезный метод, который применяется линейный дизайн управления для нелинейных систем с очень удовлетворительными результатами.