[드론] 표준 PID 제어 함수 구현하기

표준 PID 제어함수 구현

목표

• Roll, Pitch, Yaw 각도에 대한 PID 출력 값을 구하기

가장 먼저 해야할 일

자이로 센서를 이용해서 구한 각속도를 바탕으로 회전각을 구하는 부분을 삭제한 후 표준 PID 제어함수에서 수현을 해야한다.

---

소스코드

//표준 PID 제어 함수 구현(이중루프 PID 제어기 X)
//목표 : roll, pitch, yaw의 각도에 대한 pid 출력값 구하기
#include 
   
const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
   
void setup() {
  initMPU6050(); //가속도 자이로 센서 값을 읽음
  Serial.begin(115200);
  calibAccelGyro(); //센서 보정 루틴
  initDT();// 시간 간격에 대한 초기화
}
   
void loop() {
  readAccelGyro();
  calcDT(); //시간 간격 계산
  calcAccelYPR(); //가속도 센서 처리 루틴
  calcGyroYPR(); //자이로 센서 처리 루틴 --> 가속도 센서의 값을 해석하기 위해 Roll, Pitch, Yaw에 대한 각도 구하는 함수
  calcFilteredYPR(); //상보필터 적용
  calcYPRtoStdPID();
 
  static int cnt;
  cnt++;
  if(cnt%2==0)
    SendDataToProcessing(); //Roll, Pitch, Yaw에 대한 각도 정보를 보냄
}
  
//MPU-6050초기화 루틴
void initMPU6050()
{
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
}
  
 //가속도 자이로 센서를 읽는 루틴
void readAccelGyro()
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true);
  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();
  Tmp = Wire.read() << 8 | Wire.read();
  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  GyZ = Wire.read() << 8 | Wire.read();
}
  
float dt;
float accel_angle_x, accel_angle_y, accel_angle_z;
//float gyro_angle_x, gyro_angle_y, gyro_angle_z;
float filtered_angle_x, filtered_angle_y, filtered_angle_z;
float baseAcX, baseAcY, baseAcZ;
float baseGyX, baseGyY, baseGyZ;
extern float roll_output, pitch_output, yaw_output;
  
//센서 들의 기본값들의 평균을 내야하는 루틴(센서 보정 루틴)
void calibAccelGyro() 
{
  float sumAcX = 0, sumAcY = 0, sumAcZ = 0;
  float sumGyX = 0, sumGyY = 0, sumGyZ = 0;
  
  //가속도 자이로 센서를 읽어드림
  readAccelGyro();
 
  //읽어드렸으면 이제 읽어드린 값을 토대로 평균값을 구하면 됨
  for(int i=0; i<10; i++)
  {
    readAccelGyro();
    sumAcX += AcX; sumAcY += AcY; sumAcZ += AcZ;
    sumGyX += GyX; sumGyY += GyY; sumGyZ += GyZ;
    delay(100);//0.1초
  }
  //맨 처음 기본 센서 값들을 보여지고 그다음에 평균값을 구하는 함수
  baseAcX = sumAcX / 10;
  baseAcY = sumAcY / 10;
  baseAcZ = sumAcZ / 10;
  
  baseGyX = sumGyX / 10;
  baseGyY = sumGyY / 10;
  baseGyZ = sumGyZ / 10;
}
  
 //프로세싱 스케치로 각도 정보를 보내는 루틴
void SendDataToProcessing()
{
  Serial.print(F("DEL:"));
  Serial.print(dt,DEC);
  /*Serial.print(F("#ACC:"));
  Serial.print(accel_angle_y, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_y, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_z, 2);
  Serial.print(F("#GYR:"));
  Serial.print(gyro_angle_x, 2);
  Serial.print(F(","));
  Serial.print(gyro_angle_y, 2);
  Serial.print(F(","));
  Serial.print(gyro_angle_z, 2);
  Serial.print(F("#FIL:"));
  */
  Serial.print(filtered_angle_x, 2);
  Serial.print(F(","));
  Serial.print(filtered_angle_y, 2);
  Serial.print(F(","));
  Serial.print(filtered_angle_z, 2);
  Serial.print(F("#PID:"));
  Serial.print(roll_output, 2);
  Serial.print(F(","));
  Serial.print(pitch_output, 2);
  Serial.print(F(","));
  Serial.println(yaw_output, 2);
  delay(5);
}
 
unsigned long t_now;
unsigned long t_prev;
 
void initDT(){
  t_prev = millis();
}
 
void calcDT(){
  t_now = millis();
  dt = (t_now - t_prev) / 1000.0;
  t_prev = t_now;
}
 
void calcAccelYPR() //가속도 센서 처리 루틴
{
  float accel_x, accel_y, accel_z; //x, y, z 축에 대한 각도 저장 변수
  float accel_xz, accel_yz;
  const float RADIANS_TO_DEGREES = 180 / 3.14159;
 
  accel_x = AcX - baseAcX;
  accel_y = AcY - baseAcY;
  accel_z = AcZ + (16384 - baseAcZ);
 
  accel_yz = sqrt(pow(accel_y, 2) + pow(accel_z, 2));
  accel_angle_y = atan(-accel_x / accel_yz)*RADIANS_TO_DEGREES;
 
  accel_xz = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
  accel_angle_y = atan(accel_y / accel_xz)*RADIANS_TO_DEGREES;
 
  accel_angle_z = 0;
}

float gyro_x, gyro_y, gyro_z;

void calcGyroYPR()
{
  const float GYROXYZ_TO_DEGREES_PER_SEC = 131; // 각속도를 저장하는 변수

  gyro_x = (GyX - baseGyX) / GYROXYZ_TO_DEGREES_PER_SEC;
  gyro_y = (GyY - baseGyY) / GYROXYZ_TO_DEGREES_PER_SEC;
  gyro_z = (GyZ - baseGyZ) / GYROXYZ_TO_DEGREES_PER_SEC;

 // gyro_angle_x += gyro_x * dt;
 // gyro_angle_y += gyro_y * dt;
 // gyro_angle_z += gyro_z * dt;
 /*주석을 하는 이유
  * 이 부분은 자이로 센서 자체적으로 회전각을 구하는 부분,
  * 이제는 PID 제어를 통해 회전각을 구할것이다.
  */
}

void calcFilteredYPR() //상보필터
{
   const float ALPHA = 0.96;
   float tmp_angle_x, tmp_angle_y, tmp_angle_z; //임시 각도

   tmp_angle_x = filtered_angle_x + gyro_x * dt; // 이전 보정 각도 + 현재 자이로 센서를 이용해 얻은 각도(gyro_x * dt)
   tmp_angle_y = filtered_angle_y + gyro_y * dt;
   tmp_angle_z = filtered_angle_z+ gyro_z * dt;

   filtered_angle_x = ALPHA * tmp_angle_x + (1.0 - ALPHA) * accel_angle_x;
   filtered_angle_y = ALPHA * tmp_angle_y + (1.0 - ALPHA) * accel_angle_y;
   filtered_angle_z = tmp_angle_z;
}

//표준 PID 제어기
void stdPID(float& setpoint, float& input, float& prev_input, float& kp, float& ki, float& kd, float& iterm, float& output)
{
  float error;
  float dInput;
  float pterm, dterm;

  error = setpoint - input; 
  dInput = input - prev_input;
  prev_input = input;

  pterm = kp * error;
  iterm += ki * error * dt;
  dterm = -kd * (dInput / dt);

  output = pterm + iterm + dterm;
}

float roll_target_angle = 0.0;
float roll_prev_angle = 0.0;
float roll_kp = 1;
float roll_ki = 0;
float roll_kd = 0;
float roll_iterm;
float roll_output;

float pitch_target_angle = 0.0;
float pitch_prev_angle = 0.0;
float pitch_kp = 1;
float pitch_ki = 0;
float pitch_kd = 0;
float pitch_iterm;
float pitch_output;

float yaw_target_angle = 0.0;
float yaw_prev_angle = 0.0;
float yaw_kp = 1;
float yaw_ki = 0;
float yaw_kd = 0;
float yaw_iterm;
float yaw_output;

void calcYPRtoStdPID()
{
  stdPID(roll_target_angle, filtered_angle_y, roll_prev_angle, roll_kp, roll_ki, roll_kd, roll_iterm, roll_output);
  stdPID(pitch_target_angle, filtered_angle_x, pitch_target_angle, pitch_kp, pitch_ki, pitch_kd, pitch_iterm, pitch_output);
  stdPID(yaw_target_angle, filtered_angle_z, yaw_target_angle, yaw_kp, yaw_ki, yaw_kd, yaw_iterm, yaw_output);
}

다운로드 파일 : 

_15ypr2stdpid01.ino

---

설명 #1

참고 : [드론]자이로 센서 처리루틴 구현하기

이전에 작성한 내용에서 '#4 현재 각속도에 단위 시간을 곱해서 누적' 내용을 보시게 되면 자이로 센서만을 이용해서 회전각을 구하는 부분입니다. 근데 우리는 이제 PID 제어를 통해 Roll, Pitch, Yaw 각도에 대해 PID 출력값을 구할 것 이므로 이 부분은 앞으로 필요없는 내용입니다. 따라서 gyro_angel_x, gyro_angel_y, gyro_angel_z 변수와 #4 부분을 아래와 같이 주석처리 해주시면 됩니다.

---

설명 #2

표준 PID 알고리즘

참고 : [드론]PID 제어 알고리즘 구현하기

아두이노 소스코드를 작성할때 우선적으로 PID 제어 알고리즘을 기반으로 해서 표준 PID 제어기 구현 함수를 작성을 해야합니다.

첫번째 줄을 보시게 되면 'float& 어쩌구저쩌구' 이렇게 나와있습니다. 이 내용을 C언어의 주소 연산자의 내용이며 브릭봇 님의 블로그를 참고하시면 좋을거 같습니다. " C언어 연산자 우선순위 "

그래서 나중에 Roll, Pitch, Yaw의 각도에해 PID 값을 구하는 함수(calcYPRtoStdPID) 함수에서 보면 알겠지만 따로 변수를 선언을 해주어서 그 변수를 stdPID 함수로 보내주고 있습니다. 이 보내는 값들은 수시로 바뀌는 값들이고 또한 stdPID는 calcYPRtoStdPID함수에서 보낸 인자값을 그대로 같은 변수로 받지 않고 다른 변수로 받고 있는것을 확인할 수 있습니다.

따라서 이러한 경우는 주소값을 참조하여 값을 받는 것 입니다.

이 경우는 calcYPRtoStdPID함수를 선언한 후 최종적으로 다시 자세하게 설명을 드리도록 하겠습니다.

구현된 소스코드롤 보게 되면 PID 알고리즘에 맞게 코딩이 된것을 확인하실 수 있습니다.

error = setpoint - input; 

설정값(setpoint)에서 현재 입력 값(input)을 빼서 현재 오차(error)를 구함

dInput = input - prev_input;

현재 입력 값(input)에서 이전 입력 값(prev_input)을 빼서 입력 변화 값을 구함

prev_input = input;

다음 주기에 사용을 하기 위해서 dInput 구하는 과정이 끝나면 바로 이전 입력값(prev_input)에 현재 입력 값(input)을 저장해 줍니다.

pterm = kp * error;
iterm += ki * error * dt;

비례매개변수(kp)에 현재 오차(error)를 곱해서 비례항(pterm)을 구함.

적분매개변수(ki)에 현재 오차와 센서 입력 주기를 곱하고 누적한 후 적분항(iterm) 값을 구함.

iterm은 계속 누적된 값을 구하는 것 이므로 주소를 참조해서 값을 얻어오게 됩니다.

dterm = -kd * (dInput / dt);

이 부분은 현재 입력 변화 값(dInput)을 센서 입력 주기(dt)로 나눈 후 미분매개변수(kd)에 곱해 부호를 바꾸어 주어 미분항(dterm) 값을 구합니다.

왜 부호를 바꾸는가?

원래 기존 PID 제어 적분 알고리즘 dError = Error - prevError DTerm = kd * (dError / dt) 미분항의 오차변화율에서 setpoint를 없애주어야 드론이 안정적으로 비행을 할 수 있습니다. 따라서 오차변화율(Error = setpoint - input)에서 setpoint값이 변화는 상황을 제거하게 되면 Error의 미분은 부호가 바뀐 Input 미분과 동일하게 됩니다. 따라서 kd*derror를 더해주는 대신에 -kd*dInput을 빼주면 setpoint의 변화가 output의 결과에 영향을 주지못합니다.

최종 미분항 값 알고리즘 dinput = input - prev_input dterm = -kd * (dinput / dt)  이 알고리즘을 적용시키면 아무리 setpoint을 변경을 시켜도 output에 영향을 주지 않는다.

---

설명 #3

float roll_target_angle = 0.0;
float roll_prev_angle = 0.0;
float roll_kp = 1;
float roll_ki = 0;
float roll_kd = 0;
float roll_iterm;
float roll_output;

float pitch_target_angle = 0.0;
float pitch_prev_angle = 0.0;
float pitch_kp = 1;
float pitch_ki = 0;
float pitch_kd = 0;
float pitch_iterm;
float pitch_output;

float yaw_target_angle = 0.0;
float yaw_prev_angle = 0.0;
float yaw_kp = 1;
float yaw_ki = 0;
float yaw_kd = 0;
float yaw_iterm;
float yaw_output;

void calcYPRtoStdPID()
{
  stdPID(roll_target_angle, filtered_angle_y, roll_prev_angle, roll_kp, roll_ki, roll_kd, roll_iterm, roll_output);
  stdPID(pitch_target_angle, filtered_angle_x, pitch_target_angle, pitch_kp, pitch_ki, pitch_kd, pitch_iterm, pitch_output);
  stdPID(yaw_target_angle, filtered_angle_z, yaw_target_angle, yaw_kp, yaw_ki, yaw_kd, yaw_iterm, yaw_output);
}

Roll, Pitch, Yaw의 출력 값을 내기 위해서 filtered_angel_y, x, z, 의 값을 사용하고 있습니다.

참고 : [드론]상보필터 근사공식 이용하기

제가 stdPID 함수 루틴을 구현할때 "왜 float& 를 이용해서 주소를 참조하는가"에 대해 설명을 여기서 한다고 하였습니다.

여기서 보면 아시겠지만 현재 따로 변수를 선언을 해주어서 상시로 변화된 값을 넘겨주기 위해서 주소참조를 사용한 것 같습니다.

////////////////////////////////////////////틀린 부분이 있다면 댓글로 피드백 부탁드려요~/////////////////////////////////////

61번째 줄에서 extern을 사용해서 변수 선언을 해줍니다.그래서 Roll, Pitch, Yaw에 대한 출력값을 저장해 줍니다.

extern은 다른 파일에서도 변수로 사용할 수 있도록 전역변수를 설정하는 놈 입니다. 즉 다음 포스팅에서 보면 아 시겠지만 프로세싱 프로그램에서 extern으로 설정해준 전연변수를 사용하게 되는데 그게 가능하도록 하기 위해서 extern을 사용한 것 입니다.

[선언해준 변수가 어딘가에 존재하고 있다는 것을 컴파일러에게 알릴대 사용하는 변수, 여기서는 SendDataToProcessing 함수에 알려주는 역할]

참고 : extern에 대한 참고 블로그

그리고 마지막으로 Roll,Pitch,Yaw에 대한 보정 각도 출력과 Roll,pitch,Yaw의 출력값을 보여줍니다.

나중에 roll_output, pitch_output, yaw_output변수는 출력값을 모터에 입력 해주는 값 역할을 하게 됩니다.

---

결과

현재 기울어진 각도에 따라 같은 크기의 값이 부호가 바뀌어서 출력을 하게 되는데 이 출력은 모터로 연결이 됩니다.