로봇의 기울여진 각도 확인

로봇의 기울여진 각도 확인

우리가 로봇을 구동하고 제어하는데 있어서 중요한 요소가 로봇의 기울여진 각도를 확인하여 넘어지지 않고 균형을 잡는 것이다.

그렇다면 기울여진 각도를 확인하기 위해서는 기울여진 각도를 측정해야하는데 사람이 로봇의 각도를 각도기로 지속적으로 측정하기위해선 제한적이다. 그렇다면 어떤 방법이 좋은것인가? 바로 로봇이 스스로 기울여진 각도를 계산하는 것이다. 이때 일반적으로 사용하는 센서가 있다. 바로 GYRO Sensor 와 ACCEL sensor이다.

GYRO Sensor는 각속도를 측정하는 센서이다.

→우리가 배운 수학과 물리(역학)로 접근하면 각속도를 적분하면 각도를 구할 수 있다.

Accel sensor는 가속도, 진동, 충격 등의 동적 힘을 감지하며 관성력, 전기변형, 자이로의 응용원리를 이용한 것이다.

→우리의 휴머노이드 로봇 kubot은 중력을 이용하여 X축, Y축, Z축에 작용하는 중력크기에 따른 차이나는 각도를 구하는 것이다.

→GYRO의 좌표계 →Accel의 좌표계

위에 사진들은 GYRO좌표계 Accel좌표계이다 축의 방향이 일치하지 않기 때문에 참고하는 것이 좋다.

→GYRO의 좌표계 →Accel의 좌표계

로보티즈 사에서 제공하는 DARwin-OP의 좌표계이다.

우리는 Sub controller CM730의 GYRO Sensor 와ACCEL sensor를 이용해서 받은 데이터를 프로그래밍을 통해 어떻게 기울여진 각도를 구하는지 확인하자.

우리가 볼 곳은 소스에서 Framework > src > motion > modules > walking.cpp 라는 곳이다. 굳이 walking.cpp에서 하는 이유는 walking.cpp는 로봇을 실제로 구동하는 main.cpp와 따로 지속적으로 실행되는 파일이고 0.008초가 보장되어 있는 실행 파일이다. 소스 해석으로 설명하겠다.

void Walking::Process()

{

rlGyroerr=MotionStatus::RL_GYRO;

fbGyroerr=MotionStatus::FB_GYRO;

zzGyroerr=MotionStatus::ZZ_GYRO;

rlAccelerr=MotionStatus::RL_ACCEL;

fbAccelerr=MotionStatus::FB_ACCEL;

zzAccelerr=MotionStatus::ZZ_ACCEL;

설명 : 각각의 축에 대한 자이로데이터와 가속도 데이터를 미리 선언한 변수에 대해 넣고 있다.

Gyro_Y=(int)((3.125)*fbGyroerr);

Gyro_Z=(int)((3.125)*zzGyroerr-1600.0);

설명 : 로보티즈 사에서 자이로 데이터의 단위dps(degree/sec)와 로봇이 미리 변화되서 받은 데이터를 비교한 그래프이다.

그렇기 때문에 맨처음 받은 GYRO Data는 dps 단위이가 아니기 때문에 dps 단위로 바꿀 필요가 있다.

Accel_X=(0.076640625)*rlAccelerr-39.24;

Accel_Y=(0.076640625)*fbAccelerr-39.24;

Accel_Z=(0.076640625)*zzAccelerr-39.24;

Accel_degree = (180/3.141592)*atan(Accel_Y/sqrt(pow(Accel_X,2)+pow(Accel_Z,2)));

설명 : 로보티즈 사에서 가속도 데이터의 단위 g(중력가속도)와 로봇이 미리 변화되서 받은 데이터를 비교한 그래프이다.

그렇기 때문에 맨처음 받은 Accel Data는 g 단위이가 아니기 때문에 g 단위로 바꿀 필요가 있다.

[Accel_degree = (180/3.141592)*atan(Accel_Y/sqrt(pow(Accel_X,2)+pow(Accel_Z,2)));]

축 방향에 대해 가속도를 각도로 계산하는 방법

Gyro_degree_Yaw += Gyro_Z*0.008;

Gyro_degree += Gyro_Y*0.008;

설명 : dps 단위로 받은 GYRO data를 0.008초를 곱한 다음 누적 적분을 하였다. 즉 각속도를 누적적분 함으로써 각도를 구해냈다.

Pre_degree=Real_degree;

printf("%d\t %d\t %d\t %d\t %d\t %f\t %f\t %f\n",time_count,Gyro_Y,Accel_X,Accel_Y,Accel_Z,Gyro_degree,Accel_degree,1.1*Real_degree);

printf("%d\t %f\n",Gyro_Y,1.1*Real_degree);

time_count++;

save[r][0]=0.008*time_count;

save[r][1]=Gyro_Y;

save[r][2]=Accel_X;

save[r][3]=Accel_Y;

save[r][4]=Accel_Z;

save[r][5]=Gyro_degree;

save[r][6]=Accel_degree;

save[r][7]=1.1*Real_degree;

save[r][8]=Gyro_degree_Yaw;

r++;

설명 : 각각 구하고 정리한 Data를 Save [ ][ ] 라는 배열에 저장한다.

Real_degree = (Pre_degree*K)+(Gyro_Y*0.008)+(1-K)*Accel_degree;

설명 : 위 Real_degree는 상보필터를 적용한 식이다. 우리가 자이로, 가속도 센서로 따로 구한 각도가 아니라 두 가지의 Data를 종합적으로 정리한 것으로 보다 정확하고 실질 적으로 우리 이용하는 Data이다.

[Real_degree = (Pre_degree*K)+(Gyro_Y*0.008)+(1-K)*Accel_degree;]

소스에서 Real_degree가 상보 필터를 적용한 거라고 알 수 있다. 그리고 K는 따로 구해 대입했다.

if(time_count>1000)

{

Flag_save=1;

}

walking.cpp에서 구한 Data가 어느 조건이 충족이 되면 main.cpp에서 그값들을 메모장에 저장하게 했다.

실험은 국민대학교 RClab의 박지현님의 세그웨이를 통해 실험했다.

회전하는 각도의 기준이 필요했기 때문에 보다 바퀴의 회전 각도를 보다 정확하게 구하는 세그웨이를 이용했다.

세그웨이는 엔코더를 통해 보다 회전하는 각도를 구하는데 있어 정확하다.

세그웨이 바퀴에다 실제 로봇을 축의 방향에 맞게 위치하여 단 모습이다.

실험결과 상보필터를 통해 나온 Real_degree가 세그웨이의 Encoder값과 거의 일치한다.

로봇의 기울여진 각도를 구하고 실험적으로 확인할 수 있었다.