2019년 10월 3일 목요일

3차원 공간정보 스캔 마운트용 중량 6WD 로버 개발 방법

이 글은 중량 센서를 마운트할 수 있는 6WD 로버 제작 과정을 정리한 글이다.

이전에 개발한 SLAM장비를 직접 들고 스캔할 목적으로 스캔 백팩을 만들었으나 무거워 가지고 다니며 스캔하기 힘들다. 이런 이유로, 연구용 야외 공간정보 맵핑 및 탐색이 가능한 간단한 로버를 만들 계획이다. 공간맵핑용 장비는 앞서 개발한 벨로다인 LiDAR 기반 SLAM 장치, 로봇 암 등을 활용한다. 이를 마운트하려면 최소한 1,794 g (VLP16 830 g + TX2 144 g + 배터리 310 g x 2 + 기타 200 g) 페이로드가 필요하다. 일반 장난감 수준의 장비로는 움직이지도 않기 때문에, 별도 로버를 제작하기로 하였다.

이 글에서 소개하는 로버는 최소 5 kg 이상의 페이로드를 지원하며, 야외 울퉁불퉁한 환경에서 공간정보 스캔을 할 수 있는 고정밀 센서 장치를 마운트 할 수 있다.
3D geo scan rover (425 x 300 mm. 4 kg)

공간정보 맵핑용 SLAM 장비 개발은 다음 링크를 참고한다.

로버 임무
이 로버는 3차원 공간정보 스캔을 위한 탐색용 중량 로버(Exploration heavy weight rover for scanning 3D geo-information. 3D geo scan rover)이다. 프레임 상단에 벨로다인 VLP-16 및 RGBD 센서가 마운트될 수 있어야 하며, 통신 거리 100-200미터 범위에서 로버를 조정할 수 있어야 한다.

참고로, 로버를 개발할 때는 다음과 같은 사항을 고려해야 성공할 수 있다.
  • 로버의 페이로드(운반무게) 결정. 페이로드 고려한 모터, 모터드라이버 및 배터리 필요
  • 로버의 운영환경 결정. 만약, 습기가 있는 곳이라면 방수가 되는 패키징 필요. 야외라면 울퉁불퉁한 지면을 다닐 수 있는 큰 바퀴와 내구성있는 바디 프레임 필요
  • 로버의 목적에 따른 센서 결정. 수색용이면 카메라, 적외선, LiDAR센서 및 이를 처리할 수 있는 임베디드 컴퓨터 필요. 물체취득이 필요하면 그립퍼, 로봇암등이 필요함
  • 프레임 결정. 로버 목적에 따라 센서나 장비를 쉽게 마운트 할 수 있어야 함
  • 로봇 통신 방법 결정. 유선인지 무선인지 결정 필요. 무선이면 몇 미터 거리 범위까지 통신 가능한지에 따라 통신장치 개발해야 함. 예를 들어 200미터 정도 거리 통신이 필요하다면 2.4GHz 송수신기를 사용해야 함
로버 파워
스펙 상으로 15kg 정도 페이로드를 지원하는 HEAVY 6WD rover 개발을 고려해 모터와 모터드라이버는 다음과 같이 결정한다.
  • 모터: DC Gear Motor. DC 6-12V. 
  • 모터드라이버: BTS7960. 43A 고전력 모터 드라이버
  • 배터리: 11.1V LiPO 2200mAh
BTS7960은 43A 모터 구동까지 가능하므로, 큰 힘이 필요한 상황에서 사용할 수 있다(예. #1, #2). 다음과 같이 배터리 팩 연결 단자를 남땜하고 모터드라이버와 연결한다. 참고로, 여기서는 지금 가지고 있는 전선을 이용했지만, 안전성을 고려해 30A 전류를 허용할 수 있는 직경을 가진 전선을 사용하는 것이 좋다(부록:AWG 참고). 

프레임
프레임은 가공업체에 맡겨 제작하거나 모터만 부착된 로봇 로버 프레임(#)을 구매해 사용한다. 센서를 마운트하기 편하게 HOLE이 있어야 한다. 다만, 범용 프레임은 빠르게 개발할 수 있는 장점은 있으나, 특수목적 및 환경에서 사용할 때는 별도 개발이 필요하다.

여기서 사용할 프레임은 링크와 유사하다. 다만, 자체가 낮고, 가공이 쉬우며, 배터리나 모터 드라이버를 내부에 장착할 수 있는 프레임으로 결정하였다. 프레임 크기는 425 x 300 mm 이며, 바퀴를 제외하면 380 x 120 mm 이다.
센서
센서는 LiDAR와 RGBD를 지원한다. 이 결과로 고정밀 포인트 클라우드와 이미지를 획득할 수 있다. 사용 센서는 다음과 같다.
  • LiDAR: VLP-16. 임베디드는 고성능 NVIDIA TX2 보드 사용
  • RGBD: ZED. 임베디드는 NVIDIA NANO 사용
통신
무선통신을 지원한다. 여기에서는 아래의 2.4GHz 송수신기를 사용한다. 송수신기는 다양한 종류가 있으니, 본인 개발 목적에 맞는 것을 사용한다.
송신기와 수신기 바인딩은 다음 링크를 참고한다.
송수신기를 서로 바인딩 한 후에는 다음과 같이 편리하게 서보모터 등을 제어할 수 있다. 
FS-TH9X, R9B binding

회로
회로는 다음과 같이 연결한다. 회로는 FS-R9B 수신기 CH2, CH3에서 DRIVE, STEER 신호를 각각 받아, 모터드라이버에 모터 구동신호를 전달한다. 배터리와 BTS7960은 2개를 사용해 모터에 전류를 충분히 공급한다. 참고로, L298N과 같이 2A미만 모터드라이버를 사용하면, 프레임 자체 자중도 견디기 어려울 것이다.
D22 - FS-R9B.CH2
D24 - FS-R9B.CH3
5V - FS-R9B.VCC
GND - FS-R9B.GND
D2 - RIGHT MOTOR BTS7960 RPWM
D3 - RIGHT MOTOR BTS7960 LPWM
D4 - RIGHT MOTOR BTS7960 L_EN
D5 - RIGHT MOTOR BTS7960 R_EN
D6 - LEFT MOTOR BTS7960 RPWM
D7 - LEFT MOTOR BTS7960 LPWM
D8 - LEFT MOTOR BTS7960 L_EN
D9 - LEFT MOTOR BTS7960 R_EN
VCC - BTS7960 VCC
GND - BTS6960 GND
11.1V BAT GND - BTS7960.GND
11.1V BAT VCC - BTS7960.VCC
RIGHT MOTOR BTS7960.M1 - RIGHT MOTOR +
RIGHT MOTOR BTS7960.M2 - RIGHT MOTOR -
LEFT MOTOR BTS7960.M1 - LEFT MOTOR +
LEFT MOTOR BTS7960.M2 - LEFT MOTOR -




회로 연결 과정

코딩
코딩은 다음과 같다(github 링크). 소스코드는 필드테스트 후에 다음과 같은 몇 가지 수정이 있었다.
  • 로버 전후좌우 이동 이외에도 후진 방식 추가
  • 송신기 전원 껴져 있는 경우, 최대속도 주행 오류 있어, 신호 없을 경우 멈춤기능 추가
  • 특정 모터 드라이버 구동 코드 및 반복된 소스코드 함수화
  • 모터 드라이버 회로 선 연결 착오로 핀 번호 수정
참고로, 아래 소스코드 사용할 경우, 수신기에서 얻은 채널별 디지털 신호의 범위는 테스트하여 구해야 하며, 이를 map, constrain 함수를 이용해 캘리브레이션해야 한다.

// 6WD rover control code. FS-R9B. BTS7960 driver
// by Taewook, Kang. laputa99999@gmail.com. 2019.10.8
// MIT license
//
#define chA 22
#define chB 24

#define R_RPWM 2
#define R_LPWM 3
#define R_LEN  4
#define R_REN  5
#define L_LPWM 6
#define L_RPWM 7
#define L_LEN  8
#define L_REN  9

void setup() {
  pinMode(chA, INPUT);
  pinMode(chB, INPUT);

  for(int i = R_RPWM; i <= L_REN; i++)
    pinMode(i, OUTPUT);
  for(int i = R_RPWM; i <= L_REN; i++)
    digitalWrite(i, LOW);

  delay(1000);
  Serial.begin(9600);
}

void runMotor(int leftDrive, int leftReverse, int rightDrive, int rightReverse)
{
  digitalWrite(R_REN, HIGH);
  digitalWrite(R_LEN, HIGH);
  digitalWrite(L_REN, HIGH);
  digitalWrite(L_LEN, HIGH);
  delay(100);

  analogWrite(L_LPWM, leftDrive);        
  analogWrite(L_RPWM, leftReverse);        
  analogWrite(R_LPWM, rightDrive);
  analogWrite(R_RPWM, rightReverse);    
}

void loop() {
  int drive = pulseIn (chA, HIGH);     // drive, back
  int steering = pulseIn (chB, HIGH);  // left, right

  String data = "D = " + String(drive);
  Serial.print(data);
  data =  ", S = " + String(steering);
  Serial.print(data);

  int motorDrive = 0;
  int motorSteer = 0;

  motorDrive = map(drive, 1070, 1880, 0, 255);
  motorDrive = constrain(motorDrive, 0, 255);
  motorSteer = map(steering, 1040, 1880, 0, 255);
  motorSteer = constrain(motorSteer, 0, 255);
  
  data =  ", MD = " + String(motorDrive);
  Serial.print(data);
  data =  ", MS = " + String(motorSteer);
  Serial.println(data);
  
  bool forward = true;
  if(90 <= motorDrive && motorDrive <= 110)
  {
    motorDrive = 0;
  }
  else if(motorDrive > 110)
  {
    motorDrive = map(motorDrive, 110, 255, 0, 255);
    motorDrive = constrain(motorDrive, 0, 255);
  }
  else 
  {
    motorDrive = map(motorDrive, 0, 90, 255, 0);
    motorDrive = constrain(motorDrive, 0, 255);   
    forward = false; 
  }

  // |_rover__>>
  // 
  if(motorSteer == 0)   // if transmitter power off the motorSteer value is 0
  {
    runMotor(0, 0, 0, 0);
  }
  else if(100 <= motorSteer && motorSteer <= 140)    // go straight
  {
    runMotor(forward ? motorDrive : 0, forward ? 0 : motorDrive, 
             forward ? motorDrive : 0, forward ? 0 : motorDrive);
  }
  else if(motorSteer < 100) // turn left
  {
    runMotor(forward ? motorDrive : 0, forward ? 0 : motorDrive, 
             forward ? 0 : motorDrive, forward ? motorDrive : 0);
  }
  else  // turn right
  {
    runMotor(forward ? 0 : motorDrive, forward ? motorDrive : 0, 
             forward ? motorDrive : 0, forward ? 0 : motorDrive);
  }
}

로버와 배터리를 연결하고, 송신기를 켠 후, 시리얼모니터를 확인하면, 다음과 같은 제어 신호 데이터를 확인할 수 있다.

송신기로 제어하면, 로버가 정상적으로 제어되는 것을 확인할 수 있다.

필드 테스트 
로버 성능은 어떻게 되는 지, 어느 정도 중량을 견디는 지 등을 확인하고, 이슈가 있는 지 확인하기 위해 필드 테스트를 한다. 중량 테스트를 위해, 다음과 같이 로버 등의 하중을 측정한다.
로버 자중 = 4.0 kg
배터리 2개 + 에나멜 통 2개 중량 = 4.8 kg
큰 에나멜 통 1개 중량 - 2.8 kg
작은 에나멜 통 1개 = 1.1 kg
배터리 한개 501 g
로버 위 에나멜 통 2개 + 배터리 3개 = 5.3 kg
에나멜 통 1개 + 배터리 3개 = 2.7 kg 

다양한 조건과 환경을 고려해 다음과 같이, 로버 테스트를 해 보았다.
1차 동작제어 및 주행속도 테스트 (자중 4 kg)
2차 중량 테스트 (하중 3.8 kg)
3차 장애물 테스트 (다양한 크기의 장애물 + 하중 2.8 kg)
4차 장애물 테스트 (다양한 크기의 장애물 + 하중 2.8 kg)
5차 중량 테스트 (하중 2.7 kg)
6차 중량+자유곡선 이동 테스트 (하중 2.7 kg)
7차 사면 등판테스트 (자중 4.0 kg)

테스트 해 본 결과 2.6kg 정도는 충분히 로버 프레임이 지지하며, 5kg은 로버의 서스펜션이 바닥에 닿는 문제를 제외하고는 큰 문제 없이 구동되었다. 서스펜션을 조정하면 5kg 페이로드도 가능하다. 경사 등판은 표면이 거친 경우, 40도 각도도 주행 가능하며, 무게중심이 낮다면 그 이상 각도도 등판 가능하다. 주먹만한 크기의 자갈길이나 작은 바위 위에서도 주행이 가능해 보인다.

마무리
앞의 개발 결과로 RF 제어가 되는 중량 센서 장치가 마운트될 수 있는 로버를 만들었다. 앞으로 로버 위에 개발된 LiDAR 및 RGBD 스캔 장치를 마운트하여, 공간정보 맵핑에 사용하려 한다. 여기에 비전, ROS, 딥러닝 등을 붙이면 장애물을 피해가며 지정된 구간을 스캔할 수 있는 장비를 개발할 수 있다.

부록: AWG 전선 규격표
AWG는 미국에서 만든 전산 사용 지침으로, 직경에 따라 저항 및 허용전류를 확인해 적절한 전선을 사용할 수 있도록 만든 가이드라인이다. 허용전류가 높은 데, 적은 직경의 전선을 사용하면, Joule 열이 발생하고, 전선 피복이 녹아 벗겨져 합선이나 화재가 발생할 수 있다. AWG는 이를 방지한다. 

레퍼런스

댓글 없음:

댓글 쓰기