March 28, 2024

디바이스마트 미디어:

[66호] 원하는 색상으로 제어가 가능한 아두이노 IoT 스마트 무드등 키트 -

2021-06-25

★2021 ICT 융합 프로젝트 공모전 결과 발표! -

2021-05-12

디바이스마트 국내 온라인 유통사 유일 벨로다인 라이다 공급! -

2021-02-16

★총 상금 500만원 /2021 ICT 융합 프로젝트 공모전★ -

2021-01-18

디바이스마트 온라인 매거진 전자책(PDF)이 무료! -

2020-09-29

[61호]음성으로 제어하는 간접등 만들기 -

2020-08-26

디바이스마트 자체제작 코딩키트 ‘코딩 도담도담’ 출시 -

2020-08-10

GGM AC모터 대량등록! -

2020-07-10

[60호]초소형 레이더 MDR, 어떻게 제어하고 활용하나 -

2020-06-30

[60호]NANO 33 IoT보드를 활용한 블루투스 수평계 만들기 -

2020-06-30

라즈베리파이3가 드디어 출시!!! (Now Raspberry Pi 3 is Coming!!) -

2016-02-29

MoonWalker Actuator 판매개시!! -

2015-08-27

디바이스마트 레이저가공, 밀링, 선반, 라우터 등 커스텀서비스 견적요청 방법 설명동영상 입니다. -

2015-06-09

디바이스마트와 인텔®이 함께하는 IoT 경진대회! -

2015-05-19

드디어 adafruit도 디바이스마트에서 쉽고 저렴하게 !! -

2015-03-25

[29호] Intel Edison Review -

2015-03-10

Pololu 공식 Distributor 디바이스마트, Pololu 상품 판매 개시!! -

2015-03-09

[칩센]블루투스 전 제품 10%가격할인!! -

2015-02-02

[Arduino]Uno(R3) 구입시 37종 센서키트 할인이벤트!! -

2015-02-02

[M.A.I]Ahram_ISP_V1.5 60개 한정수량 할인이벤트!! -

2015-02-02

[68호]메카넘휠과 2D lidar를 이용한 자율주행 로봇

68 ICT_매카넘휠 (1)

ICT 융합 프로젝트 공모전 우수상

메카넘휠과 2D lidar를 이용한 자율주행 로봇

글 | 포항공과대학교 이도현

 

1. 심사평
칩센 자동차의 자율 주행을 위해서도 Lidar와 Radar를 이용한 기술들이 매우 많은 관심과 연구가 이루어지고 있는 것으로 알고 있습니다. 시연 동영상내에서도 경로를 잘 탐색한듯 하지만 마지막에 충돌이 발생하는 부분을 확인 가능한데, 이러한 부분을 개선 가능한 다른 솔루션도 함께 적용하면 시뮬레이션 레벨을 넘어서는 매우 훌륭한 작품이 만들어 질 것으로 보입니다.
펌테크 LIDAR를 활용한 자율주행 로봇 작품으로 전체 하드웨어 및 소프트웨어 시스템 연동을 위한 기획의도, 기술 구현 방안 및 작품의 하우징 구성 등이 인상적이었으며 기술 구현도, 완성도 등에서 상당히 우수한 작품으로 생각합니다.
위드로봇 메카넘 휠을 쓴 부분을 제외하곤 전형적인 이동로봇이라 창의성 부분이 아쉽습니다. 완성도는 높이 평가합니다.
뉴티씨 각종 센서로 장애물 등을 파악하여 회피할 수 있었는데, 비용만 충분하다면, 보다 많은 센서를 사용하여, 아무리 목적지가 벽 안에 있다고 해도 부딪히지 않았을 것 같아 개선의 여지가 보입니다. 만들다보면, 기계적인 부분에서도 많은 비용이 발생하고, 배선 부분이나 통신 부분에서도 어려움이 많은데, 조금 더 노력하면 더 좋은 작품이 탄생할 것 같습니다.

2. 작품 개요
본 프로젝트인 ‘메카넘휠과 lidar를 이용한 자율주행 로봇’ slam기술을 적용하여 자율주행을 구현하였다. slam이란 Simultaneous Localization and Mapping의 약자로, 동시에 로봇의 위치추정 및 주변 환경 지도 작성을 수행하는 기술이다. 본 프로젝트에서는 2d lidar를 통해 주변 환경을 pointcloud 데이터로 받아와, 메카넘휠의 odometry와 결합하여 map을 생성한다. 생성된 map을 바탕으로 출발점에서 목적지까지 최단 경로를 생성하여, 로봇이 그 경로를 따라 이동하는 방식으로 자율주행이 이루어진다.
단순히 로봇 주변에 장애물이 일정 이상 가까워지면 멈추고 다른 방향으로 가는 것이 아니라, lidar가 닿는 전 부분에서 주변의 모든 구조를 인식하고, 그 구조 위에서 목적지까지 장애물을 피해서 갈수 있는 최단 경로를 생성한 후에 그 경로를 따라 로봇이 움직이는 것이다. 이러한 작업이 실시간으로 진행된다.
또한 일반적인 자율주행 플랫폼과 달리, 만들어진 지도를 자율주행에만 사용하고 버리는 것이 아니라. 그 데이터를 TXT파일로 저장해 도출하여 주행이 끝나고 시스템이 종료된 뒤에도 만들어진 지도를 확인할 수 있는 기능을 가지고 있다.
해당 자율주행 플랫폼은 식당 서빙 로봇, 로봇청소기, 물류창고 로봇 등등 바퀴를 사용하는 플랫폼이라면 모두 적용할 수 있으므로, 활용성과 적용성이 매우 높다고 볼 수 있다.

3. 작품 설명
3.1. 주요 동작 및 특징
3.1.1. 전방향 이동 가능
메카넘 휠을 기반으로 플랫폼을 제작하였다. 메카넘휠의 특징 상, 플랫폼(차)이 회전하지 않고도 모든 방향으로 이동할 수 있다는 장점이 있다. 따라서 일반적인 조향장치를 가진 플랫폼이 가지 못하는 경로도 본 프로젝트의 메카넘휠 플랫폼은 따라 갈 수 있다는 장점이 있고, 이는 자율주행에 큰 이점이 될 수 있다.

68 ICT_매카넘휠 (2)

3.1.2. 주변 환경 인식 후 지도생성 가능

68 ICT_매카넘휠 (3)
본 프로젝트는 ydlidar X2L을 사용하여 주변 환경을 pointcloud로 받아온다. 받아온 pointcloud들을 메카넘휠의 wheel odometry와 합쳐 주변환경의 지도를 생성한다. 주변환경의 정보를 다음 페이지 테스트 환경과 지도로 수집할 수 있다는 그 자체로 장점이 될 수 있고, 저렇게 실시간으로 제작되는 지도는 자율주행을 위한 경로탐색 알고리즘을 담당하는 python코드로 전송되어 로봇이 목표지점까지 갈 수 있도록 하는 경로를 생성하는 데 이용된다.

3.1.3. 로봇이 갈 수 있는 최단경로 정보 생성 가능
본 프로젝트의 로봇은 얻은 지도 데이터를 바탕으로 로봇이 목표지점까지 갈 수 있는 최단 경로를 실시간으로 생성한다.
최단거리 생성을 우선으로 하되, 벽에 부딪힐 가능성을 줄이고자 벽과는 반드시 일정 거리를 두도록 프로그래밍 하였다. 경로탐색에 쓰인 알고리즘은 A* algorithm이다. 실시간으로 전송되어 오는 지도마다 경로탐색을 새로 실시하기 때문에, 주행 중 이전 지도에는 없던 장애물이나 벽을 감지하게 되면, 그에 따라 새로운 경로를 생성해낸다. 따라서 목표지점까지 장애물과 부딪히지 않으면서 최단 경로로 도달할 수 있다.

3.1.4. 생성된 최단경로를 바탕으로 로봇의 자율 주행 가능
A* 알고리즘을 통해 얻은 경로를 로봇이 그대로 따라갈 수 있는 능력을 가지고 있다. 자세한 원리는 후에 다른 목차에서 설명할 것이다. 현재 로봇의 목적은 자율주행이므로 A*알고리즘이 만든 경로만을 따라가지만, 설정을 바꾼다면 사람이 지도를 보고 그린 경로를 따라가게도 만들 수 있다.
본 프로젝트의 주요 동작은 자율주행이다. 라인트레이서나 트래커와 다르게 어떠한 표식이 없어도 스스로 주변환경을 인식하여 외부의 도움 없이 목적지까지 도달 할 수 있으며, 일반적인 자율주행과 다르게 메카넘휠을 사용하였기 때문에 모든 경로로 이동할 수 있는 장점이 있다. 또한, 주 부품이 스텝모터 4개, 저가형 2d lidar 하나, jetson nano로, 굉장히 저가로 자율주행을 구현할 수 있는 플랫폼을 제작하였다는 점에서 의미가 있다.

3.1.5. 로봇을 직접 조종하여 map data 얻고, 그 데이터를 txt파일로 저장하는 기능
자율주행 뿐 아니라, 사람이 직접 조종하여 환경의 지도 데이터를 얻고, 이를 txt파일로 저장하여 시스템이 꺼진 뒤에도 해당 공간의 지도 데이터를 저장해둘 수 있다. 해당 txt파일은 본 프로젝트에서 만든 파이썬 코드로 시각화할 수 있다.

3.2. 개발 환경
3.2.1. Linux (ubuntu 18.04)
모든 작업은 jetson nano의 ubuntu 18.04 LTS에서 이루어졌다.

3.2.2. arduino
로봇이 Vx, Vy, W만큼 이동하라는 명령을 받았을 때, 이를 메카넘휠 odometry 계산을 통해 각각의 스텝모터가 얼만큼의 각속도로 회전해야 하는지를 계산해주고, 그 계산값을 스텝모터에 적용해 실제로 돌게 하는 부분을 arduino IDE에서 작업하였다.

3.2.3. python
lidar센서에서 pointcloud데이터를 읽어오는 작업과, 로봇이 얼마나 이동했는지를 계산해 mm단위로 그 값을 반환하는 작업, 경로탐색 알고리즘인 A* 알고리즘을 python 코드로 구현하였다.

3.2.4. C++
python 코드에서 생성된 로봇의 Vx, Vy, W값을 받아와 아두이노로 그 값을 넘겨주는 부분을 담당하는 코드를 C++환경에서 구현하였다.

3.2.5. ROS melodic
위에서 언급한 모든 알고리즘들을 ROS를 통해 하나로 연결하여 자율주행이 가능하도록 만들었다. python, C++, arduino(serial) 코드들을 모두 ros node로 만들었으며, python코드에서 생성된 로봇의 Vx, Vy, W값을 ros topic을 통해 C++코드로 넘겨주었고, 마찬가지로 그 값들을 ros topic을 통해 아두이노로 전송하였다.

3.3. 전체 시스템 구성
3.3.1. 하드웨어
4개의 스텝모터에 각각 메카넘휠이 부착되어 있고, 각 스텝모터는 스텝모터 드라이버에 연결되어 있으며, 그 드라이버는 아두이노 우노에 연결되어있다. 아두이노 우노와 2d lidar는 jetson nano에 usb로 연결되어 있다. 그림으로 간단하게 나타내면 다음과 같다.

68 ICT_매카넘휠 (4)

3.3.2. 소프트웨어
본 프로젝트의 자율주행 기능에는 총 4개 코드로 이루어져 있으며 talker.py, A_star.py, ladarbot_node.cpp, ladarbot_subnode.ino가 그것이다.
talker.py는 로봇이 현재위치를 실시간으로 계산하는 기능, lidar센서에서 pointcloud데이터를 얻어오는 기능, 얻어온 pointcloud데이터와 로봇의 현재위치 데이터를 기반으로 지도를 생성하는 기능, A_star.py내의 함수를 호출해 매개변수로 지도를 전달하여 A_star.py로부터 로봇이 이동해야 하는 경로를 받아오는 기능, 해당 경로를 주행하기 위해 로봇이 움직여야 하는 Vx, Vy, W를 생성하는 기능이 구현되어있다.
A_star.py는 talker.py에서 생성한 map을 받아와 로봇이 이동해야 하는 경로를 생성하여 그 경로를 다시 talker.py로 반환한다. A_star.py는 파이썬 모듈로써, talker.py에서 import하여 사용한다.
ladarbot_node.cpp는 talker.py에서 생성한 Vx, Vy, W를 ROS topic을 통해 받아와, ROS topic을 통해 ladarbot_subnode.ino로 그 값을 전송한다. 통신을 위한 매개코드라고 생각할 수 있다.
ladarbot_subnode.ino는 ladarbot_node.cpp에서 ros topic을 통해 전송해준 Vx,Vy,W를 받아와 로봇이 Vx,Vy,W만큼 움직이기 위해 각각의 스텝모터가 얼마만큼의 각속도로 회전해야 하는지를 계산한다. 그 값을 각각의 스텝모터에게 전달하여 로봇이 움직이도록 한다.
A_star.py와 talker.py간 연결을 제외한 모든 코드들간 연결은 ROS topic을 이용해 서로 통신한다.

68 ICT_매카넘휠 (5)
본 프로젝트의 지도 저장 기능을 구현하는 코드는 크게 5가지이다. ladarbot_node.cpp, ladarbot_subnode.ino, lidar.launch, lidar_range_node.cpp, slam_map.py 가 그것이다. 이중 ladarbot_node.cpp, ladarbot_subnode.ino는 자율주행에 사용한 코드와 동일하다. lidar.launch는 ydlidar를 제작한 회사에서 lidar의 데이터를 얻을 때 쓰라고 공유한 오픈소스이며, 지도데이터를 얻어와 txt파일로 만드는 데는 직접 짠 talker.py 대신 해당 오픈소스를 사용하였다. lidar_range_node.cpp는 lidar.launch에서 보내오는 map데이터를 읽어와 플랫폼의 odometry정보와 합쳐 지도정보를 만들고, 이를 txt파일로 내보내는 역할을 하는 코드이다. 마지막으로 slam_map.py는 txt파일을 읽어 지도를 시각화하는 코드이다.

4. 단계별 제작 과정
4.1. 하드웨어
하드웨어의 경우 본 공모전을 발견하기 전부터 만들어 오고 있었고, 때문에 만드는 과정에서의 사진이 남아 있지 않다. 본 프로젝트의 하드웨어는 메카넘휠 기반 모바일 로봇이며, 정확한 odometry(자기위치 추정)이 가능할 수 있도록 회전수와 회전속도를 정확히 컨트롤 할 수 있는 스텝모터를 사용하였다. 로봇에 들어가는 컴퓨터는 jetson nano를 사용하였다. 기본적인 하드웨어는 알루미늄 프레임을 기반으로 제작하였고, 배터리 고정하는 부품, lidar고정용 부품, 바퀴 보호개 등은 직접 설계하여 3d프린터로 출력하여 사용하였다. 하드웨어 제작의 난이도는 굉장히 낮고, 본 프로젝트의 핵심은 자율주행 알고리즘 및 ROS시스템 사용이므로 하드웨어는 간단하게 설명하였다.

4.2. 프로그래밍
4.2.1. 자율주행
하드웨어 제어
하드웨어 제어는 ladarbot_subnode.ino라는 arduino IDE코드가 담당한다. 해당 코드는 로봇 몸체가 움직여야할 속도와 방향을 알려주는 Vx, Vy, W값을 받아와서 그 움직임을 구현할 수 있게 각각의 스텝모터가 얼마만큼의 각속도로 회전해야 하는지를 구한다. 그 후 그 값들을 스텝모터들에게 입력하여 모터가 움직일 수 있도록 한다.
본 프로젝트의 로봇은 메카넘 휠을 이용해 이동한다. Vx, Vy, W만큼 로봇이 이동하였을 때, 그 로봇의 4개의 메카넘휠이 얼마만큼의 각속도로 회전하는지를 행렬 계산을 통해 변환 수식을 구해낼 수 있다. 「메카넘휠 기반의 전방향 이동로봇 슬립에 관한 해석 및 실험적 검증」 김설하, 이청화, 주백석, 「메카넘휠 기반의 전방향 이동로봇 슬립에 관한 해석 및 실험적 검증」, 한국정밀공학회지 제 37권 제 1호, 2020.01.35, pp.35-42. 논문을 참고하였다. 4개의 모터의 각속도를 각각 θ1, θ2, θ3, θ4라 하면, 로봇 자체의 속도 Vx, Vy, w와 θ1, θ2, θ3, θ4사이 관계는 다음과 같이 나타나진다.

68 ICT_매카넘휠 (6)

위 행렬식을 계산하여 아두이노 코드로 작성하면 다음과 같이 나타난다.

void control(unsigned long current, float Vx, float Vy, float W)
{
if((Vx == 0 && Vy == 0) && W == 0)
{
Stop();
}
else
{
w1 = (Vx – Vy – (l1 + l2) * W) * (1 / R);
w2 = (Vx + Vy + (l1 + l2) * W) * (1 / R);
w3 = (Vx – Vy + (l1 + l2) * W) * (1 / R);
w4 = (Vx + Vy – (l1 + l2) * W) * (1 / R);
motorRF(current, w2);
motorRB(current, w3);
motorLF(current, w1);
motorLB(current, w4);
}
}

motorRF, motorRB, motorLF, motorLB함수는 각각 오른쪽 앞, 오른쪽 뒤, 왼쪽 앞, 왼쪽 뒤 스텝모터를 제어하는 함수이며. w1, w2, w3, w4가 각각 θ1, θ2, θ3, θ4이다.
Stop함수는 스텝모터에 전류를 차단하는함수인데, 멈춰 있을 때에는 모터가 쉴 수 있도록 하기 위해 삽입하였다. motorRF 함수의 형태는 다음과 같다.

void motorRF(int current,float w)
{
if(w > 0)
digitalWrite(3,1);
else
{
digitalWrite(3,0);
w = -w;
}
float num = ((2*PI)/1600)*(1000000/w);
if(current – previousRF < num){
digitalWrite(2,LOW);
}
else
{
previousRF = current;
digitalWrite(2,HIGH);
}
}

motorRB, motorLF, motorLB함수 역시 유사한 형태이다.
float num = ((2*PI)/1600)*(1000000/w); 은 사용하는 스텝모터가 1600스텝일 때 한바퀴 회전하기 때문에, 그리고 스텝간 시간간격이 초단위가 아닌 microsecond 단위이므로 다음과 같이 나타내 주었다. 매개변수로 받아오는 current는 아두이노의 micros() 함수를 통해 측정되는 현재 시간이다.(microsecond단위로 반환한다)
다음은 ROS를 통한 통신 부분이다. ladarbot_node,cpp에서 Vx, Vy, w값을 geometry_msgs/Twist 메시지를 통해 ArduinoTopic이라는 Topic name으로 전달받는다.

#include<ros.h>
#include<std_msgs/Byte.h>
#include<geometry_msgs/Twist.h>

ros::NodeHandle nh;
void message(const geometry_msgs::Twist& msg);
void odometry(unsigned long current, float Vx, float Vy, float W, float *x, float *y, float *theta);
ros::Subscriber<geometry_msgs::Twist>sub(“ArduinoTopic”,message);

ArduinoTopic 토픽으로 값이 전송될 때마다 message함수를 호출한다. message함수의 구조는 다음과 같다.

void message(const geometry_msgs::Twist& msg)
{
Vx = msg.linear.x;
Vy = msg.linear.y;
W = msg.angular.z;
}

loop함수 내에서의 함수호출 및 실행은 다음과 같다.

void loop()
{
unsigned long current;
current = micros();
control(current, Vx, Vy, W);
nh.spinOnce();
}

lidar data reading & ploting & 로봇 현재위치 계산
lidar센서에서 데이터를 얻는 작업, 로봇의 현재위치를 계산하고 lidar데이터와 계산된 현재 위치를 바탕으로 지도를 만드는 작업, 그리고 만들어진 지도와 수신한 경로를 ploting 해 주는 작업을 talker.py에서 진행한다. 우선 lidar로부터 센서데이터(pointcloud) 값을 얻는 부분의 코드이다.

port = “/dev/ydlidar”;

laser = ydlidar.CYdLidar();
laser.setlidaropt(ydlidar.LidarPropSerialPort, port);
laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 115200)
laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TRIANGLE);
laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL);
laser.setlidaropt(ydlidar.LidarPropScanFrequency, 6.0);#10.0
laser.setlidaropt(ydlidar.LidarPropSampleRate, 9);
laser.setlidaropt(ydlidar.LidarPropSingleChannel, True);
scan = ydlidar.LaserScan()

사용하는 ydlidar_X2L의 스펙에 맞게, serial통신 baudrate를 115200으로 설정하였다.
다음은 로봇의 현재위치를 계산하고 lidar데이터와 계산된 현재 위치를 바탕으로 지도를 만드는 작업, 그리고 만들어진 지도와 수신한 경로를 ploting 해 주는 작업의 부분이다. 서로가 긴밀하게 연결되어 있어 따로따로 소개하기 힘들어 한번에 소개한다.
우선 로봇의 현재위치를 계산하기 위하여 함수가 돌아가는 시간을 한번의 loop마다 몇 초가 걸리는지를 계산한다. dt = (p.timer – p.current_time), p.current_time = time.time()과 함수 끝의 p.timer = time.time() 부분이다. 한 루프의 걸린 시간을 측정하는 이유는, 로봇이 Vx, Vy, w로 움직였을 때, 이동거리와 회전각도는 각각에 걸린 시간을 곱하는 형태로 구해지기 때문이다. 현재 위치를 계산하는 부분은 다음과 같다.

현재위치 계산

dt = (p.timer – p.current_time)
p.current_time = time.time()
p.theta = p.theta + p. W*dt
p.odo_x = p.odo_x + (p.Vx * math.cos(p.theta) – p.Vy * math.sin(p.theta)) * dt
p.odo_y = p.odo_y + (p.Vx * math.sin(p.theta) + p.Vy * math.cos(p.theta)) * dt


p.timer = time.time()

다음은 로봇 주변 공간을 지도로 만들 때, 그 공간을 격자화시키기 위한 코드이다. 로봇이 이동하는 공간이 격자화 되어 있어야 후에 로봇의 이동경로를 생성할 수 있다. 격자 하나를 10cm로 설정하였다.

지도 공간 격자화 코드

grid_x_max = 5*1000 # 10m
grid_y_max = 5*1000 # 10m
grid_x_min = -5*1000 # -10m
grid_y_min = -5*1000 # -10m
#maze = [] # stack
#grid_maze = [] # stack
grid_size = 100 # 0.1m
p.grid_size = grid_size
r = laser.doProcessSimple(scan);
k = 0
if r:
grid_x = [] grid_y = [] #for i in range(0, grid_x_max * 3):
# maze.append([0,0])
for i in range(grid_x_min, grid_x_max + grid_size, grid_size):
grid_x.append(i)
for i in range(grid_y_min, grid_y_max + grid_size, grid_size):
grid_y.append(i)
maze = [] # real-time delete
grid_maze = [] # real-time delete

다음으로 lidar에서 받아온 pointcloud데이터와 현재위치 데이터를 얻어와 지도를 만드는 코드이다. 이때, pointcloud의 점의 거리가 현재 설정되어있는 격자공간보다 더 큰 거리를 가진다면, 격자공간을 넓히는 작업을 시행한다. 해당 부분의 코드이다.

map 생성 및 격자공간 확장 코드

for point in scan.points:
dis = int(point.range * 1000)
if(dis > 100):
x = int(- p.odo_x * 1000 + dis * math.cos(point.angle))
y = int(- p.odo_y * 1000 + dis * math.sin(point.angle))
maze.append([-x,y])
else:
continue

before_gridx_max = grid_x_max
before_gridy_max = grid_y_max
before_gridx_min = grid_x_min
before_gridy_min = grid_y_min
if grid_x_max < maze[k][0]:
grid_x_max = maze[k][0] + (maze[k][0] % grid_size)
if grid_y_max < maze[k][1]:
grid_y_max = maze[k][1] + (maze[k][1] % grid_size)
if grid_x_min > maze[k][0]:
grid_x_min = maze[k][0] + (-maze[k][0] % grid_size)
if grid_y_min > maze[k][1]:
grid_y_min = maze[k][1] + (-maze[k][1] % grid_size)

for i in range(before_gridx_min – grid_size, grid_x_min – 2 * grid_size, -grid_size):
grid_x.insert(0, i)
for i in range(before_gridy_min – grid_size, grid_y_min – 2 * grid_size, -grid_size):
grid_y.insert(0, i)
for i in range(before_gridx_max + grid_size, grid_x_max + grid_size, grid_size):
grid_x.append(i)
for i in range(before_gridy_max + grid_size, grid_y_max + grid_size, grid_size):
grid_y.append(i)

다음은, 지도를 형성하는 점들이 하나의 격자에 여러 개가 찍힐 때, 그 격자만 벽으로 인식하게 하는 코드이다. 이 부분의 코드를 거치기 전에는 pointcloud데이터에 어떠한 가공도 하지 않은 지도였지만, 이 작업 후에는 격자에 맞게 지도가 그려진다.

지도의 격자화 코드

px = 0
py = 0
for i in range (0, len(grid_x)-1):
if(grid_x[i] < maze[k][0] and grid_x[i+1] >= maze[k][0]):
px = grid_x[i] + grid_size / 2
break
for i in range (0, len(grid_y)-1):
if(grid_y[i] < maze[k][1] and grid_y[i+1] >= maze[k][1]):
py = grid_y[i] + grid_size / 2
break
for i in range (0, len(grid_x)-1):
if(grid_x[i] < p.odo_x*1000 and grid_x[i+1] >= p.odo_x*1000):
p.gridodo_x = grid_x[i] + grid_size / 2
break
for i in range (0, len(grid_y)-1):
if(grid_y[i] < p.odo_y*1000 and grid_y[i+1] >= p.odo_y*1000):
p.gridodo_y = grid_y[i] + grid_size / 2
break

if [px,py] in grid_maze:
grid_maze.remove([px,py])
grid_maze.append([px,py])
k = k + 1

다음으로는 A*알고리즘을 구현한 A_star.py 파이썬 모듈에 격자화된 map데이터를 넘겨주어 로봇이 움직여야 할 경로를 받아오고, 그 경로와 격자화된 map을 실시간으로 plot하는 부분이다.

경로생성 및 ploting

data1 = Astar.a_star(grid_maze, grid_size)
p.astar = data1
map1 = np.array(maze)
map2 = np.array(data1)

lidar_polar.clear()
lidar_polar.set_ylim(-5000,5000)
lidar_polar.set_xlim(-5000,5000)
lidar_polar.grid(True)

lidar_polar.scatter(map1[:, 0],map1[:, 1], s=1, color=’blue’)
lidar_polar.scatter(map2[:, 0],map2[:, 1], s=1, color=’red’)
now = [p.gridodo_x , p.gridodo_y]

다음으로는 생성된 경로에 맞게 로봇이 움직여야 할 Vx, Vy를 계산하는 부분이다. 경로는 계속해서 업데이트 되기 때문에, 현재 위치의 바로 다음 경로만을 파악하여 그 방향으로 속도를 결정한다. 현재 공간은 모두 격자화 되어있기 때문에, 현재 위치에서 로봇이 이동할 수 있는 방향은 총 8가지(위, 오른쪽, 왼쪽, 아래 오른쪽 위, 오른쪽 아래, 왼쪽 위, 왼쪽 아래)이다. 생성된 경로에서 현재위치와 가장 가까운 경로의 위치를 파악하고, 그 방향으로 속도를 결정한다. ex: 다음 경로가 오른쪽 위라면 Vx = 0.04, Vy = 0.04로 설정(0.04m/s로 로봇이 움직이게 만들어 놓음)
해당 코드이다.

Vx, Vy 생성코드

xvel = 0.04ㅍv
yvel = 0.04
if now in p.astar:
for i in p.astar:
if(abs(now[0] – i[0]) == p.grid_size or abs(now[1] – i[1]) == p.grid_size):
vx = i[0] – now[0] vy = i[1] – now[1] if(vx > 0):
p.Vx = xvel
if(vx < 0):
p.Vx = -xvel
if(vy > 0):
p.Vy = yvel
if(vy < 0):
p.Vy = -yvel
if(vx == 0):
p.Vx = 0
if(vy == 0):
p.Vy = 0

다음으로, 생성된 Vx, Vy, w (메카넘휠이므로 로봇 몸체의 회전 없이도 전 방향으로 이동할 수 있어 w를 따로 계산 하지는 않음)를 ros topic을 통해 ladarbot_node.cpp로 전송하는 코드이다.

Vx, Vy, w 전송 코드

sab = geometry_msgs.msg.Twist()
sab.linear.x = p.Vx
sab.linear.y = p.Vy
sab.angular.z = p.W
pub.publish(sab)

이 외 다른 ros 송.수신 관련 코드는 talker.py의 전체 코드에서 확인할 수 있다.

4.2.2. 최단 경로 찾기 알고리즘 – A star algorithm
A* 알고리즘이란 시작점과 도착점이 주어져 있을 때, 그 사이 벽, 장애물 등을 피해 최단 경로를 만들어주는 알고리즘이다. 원리를 간단하게 설명하자면, 격자화되어있는 map이 제공되었을 때 우선 어느 부분이 이동할 수 없는 지역이고 어느 부분이 이동가능한 지역인지를 파악한다. 본 프로젝트는 talker.py에서 격자화된 지도를 만들며 벽이 있는 부분의 격자에는 1을, 그렇지 않는 부분에는 0을 저장하여 구분하였다. A*알고리즘을 통해 어느 사각형(격자)들을 거쳐야 하는지를 파악함으로서 길을 찾는다. 경로가 발견되면 목표 도달때까지 사각형에서 다음 사각형의 중심으로 이동한다. 이 중심점들을 노드라고 부른다.
시작점을 열린 목록이라는 일종의 배열에 저장한다. 시작점에 인접한 이동불가지역을 무시하고, 이동가능한 사각형들을 열린 목록에 저장한다. 그 후 열린 목적에서 시작점을 제거하고, 다시 볼 필요 없는 닫힌 목록에 넣는다. 열린목록에 저장되어있는 사각형들은 부모 노드로 시작점을 가리키도록 한다. 현재 열린 목록에 존재하는 사각형들 중에 하나를 선택하여 위 과정을 반복하는데, 이때 사각형을 선택하는데 기준이 되는 것이 비용함수이다. 비용함수는 다음과 같이 이루어져 있다.

F = G + H

G는 시작점으로부터 현재 사각형까지의 경로를 따라 이동하는데 소요되는 비용이다.
H는 현재 사각형에서 목적지 B까지의 예상 이동 비용이다. 사이에 벽등으로 실제 거리는 알지 못하고, 그들을 무시하고 예상 거리를 산출한다.
F는 현재까지 이동하는데 걸린 비용과 예상 비용을 합친 것이다.
이렇게 구해진 비용함수를 통해 가장 작은 F를 가진 사각형을 선택한다. 이렇게 선택된 사각형을 열린 목록에서 빼고 닫힌 목록에 저장한다. 그 후 현재 사각형에서 인접한 사각형을 확인한다. 벽 등 이동불가사각형은 무시하고, 열린목록에 주변 사각형이 없다면 열린 목록에 추가한다. 이때 열린 목록에 추가된 사각형들은 현재 사각형을 부모로 설정한다. 인접한 사각형이 이미 열린 목록에 존재한다면 그 사각형과 선택된 사각형 중 어느 것의 G비용이 더 낮은지 확인한다. 기존 선택된 사각형의 G비용이 더 낮다면 그대로 두지만, 인접한 사각형의 G비용이 더 낮다면 인접 사각형들의 부모를 새로운 사각형으로 바꾼다. 마지막으로 그 사각형의 F와 G를 다시 계산한다. 이러한 과정을 반복한다. 이렇게 길을 찾는 도중 목표 사각형을 열린 사각형에 추가하였을 때, 또는 열린 목록이 비어있게 될 경우(길을 못찾은 경우) 탐색을 중단하고 목표 사각형으로부터 각각의 사각형의 부모사각형을 향하여 시작사각형에 도달할 때까지 거슬러 올라가 경로를 저장한다.

다음은 이를 파이썬으로 구현한 코드이다. 우선 node를 만드는 클래스를 소개하겠다. 각 사각형(노드)의 F,G,H값, 부모노드를 저장할 수 있는 공간을 만들어 두었다.

class Node:
def __init__(self, parent=None, position=None):
self.parent = parent
self.position = position

self.g = 0
self.h = 0
self.f = 0

def __eq__(self, other):
return self.position == other.position

다음으로 A*알고리즘의 구현 코드이다.

def aStar(maze, start, end, grid_size_r):
n = 1
grid_size = grid_size_r / n
startNode = Node(None, start)
endNode = Node(None, end)

openList = [] closedList = []

openList.append(startNode)

while openList:
currentNode = openList[0] currentIdx = 0
for index, item in enumerate(openList):
if item.f < currentNode.f:
currentNode = item
currentIdx = index

openList.pop(currentIdx)
closedList.append(currentNode)

if currentNode == endNode:
path = [] current = currentNode
while current is not None:

path.append(current.position)
current = current.parent
return path[::-1]

children = []

checking_list = [(0, -grid_size), (0, grid_size), (-grid_size, 0), (grid_size, 0), (-grid_size, -grid_size), (-grid_size, grid_size), (grid_size, -grid_size), (grid_size, grid_size)]

for newPosition in checking_list:

nodePosition = (
currentNode.position[0] + newPosition[0], # X
currentNode.position[1] + newPosition[1]) # Y

state = 0
if [nodePosition[0],nodePosition[1]] in maze:
continue

for i in maze:
dis_x = abs(nodePosition[0] – i[0])
dis_y = abs(nodePosition[1] – i[1])

dis = dis_x**2 + dis_y**2
if dis < (4*grid_size)**2:
state = 1
break

if state == 1:
continue
new_node = Node(currentNode, nodePosition)
children.append(new_node)

for child in children:
if child in closedList:
continue

child.g = currentNode.g + 1
child.h = ((child.position[0] – endNode.position[0]) **
2) + ((child.position[1] – endNode.position[1]) ** 2)
child.f = child.g + child.h

if len([openNode for openNode in openList
if child == openNode and child.g > openNode.g]) > 0:
continue

openList.append(child)

코드 중간에

for i in maze:
dis_x = abs(nodePosition[0] – i[0])
dis_y = abs(nodePosition[1] – i[1])

dis = dis_x**2 + dis_y**2
if dis < (4*grid_size)**2:
state = 1
break

if state == 1:
continue

이러한 부분이 존재한다. 위 부분은 A*알고리즘과 직접적인 연관은 없지만, 벽과 경로 사이 반드시 일정 거리를 유지하도록 한 코드이다. 최단경로가 아니더라도 벽과 부딪히지 않는 것이 더 중요하다고 판단하여 추가하였다.

Vx, Vy, w 값을 받아와 아두이노로 넘겨주는 코드
원래 Vx,Vy,w를 생성하는 talker.py에서 아두이노로 바로 값을 전달해주는 것이 가장 간편하지만, 그 과정에서 계속해서 오류가 발생해 C++파일을 매개체로 하여 ROS로 연결하였다. 해당 역할을 하는 코드는 ladarbot_node.cpp이며, talker.py에서 Vx, Vy, w값을 ros topic을 통해 받아와 아두이노와 연결을 해주는 serial노드로 값을 전달한다. 다음은 해당 코드이다.

#include<sys/time.h>
#include<chrono>

#include<ctime> //time function
#include<cmath>
#include<iostream> //time function

using std::cout; using std::endl;
using std::chrono::duration_cast;
using std::chrono::milliseconds;
using std::chrono::seconds;
using std::chrono::system_clock;
#include<ros/ros.h>
#include<std_msgs/Byte.h> //std_msgs have geometry_msgs
#include<geometry_msgs/Twist.h>
//keyboard use: rosrun teleop_twist_keyboard teleop_twist_keyboard.py

float arr[3] = {0,0,0};
void Callback(const geometry_msgs::Twist::ConstPtr& msg)
{
arr[0] = msg->linear.x;
arr[1] = msg->angular.z;
arr[2] = msg->linear.y;
ROS_INFO(“linear.x : %f”, msg->linear.x);
ROS_INFO(“linear.y : %f”, msg->linear.y);
ROS_INFO(“linear.z : %f”, msg->linear.z);
ROS_INFO(“angular.x : %f”, msg->angular.x);
ROS_INFO(“angular.y : %f”, msg->angular.y);
ROS_INFO(“angular.z : %f”, msg->angular.z);
}

double x = 0;
double y = 0;
double theta = 0;
auto timer = 0;
auto current = 0;
double tot_time = 0;
int main(int argc, char **argv)
{
struct timeval time_now{};
gettimeofday(&time_now, nullptr);

double dt = 0;
//timer = clock();
ros::init(argc, argv, “ladarbot_node”);
ros::NodeHandle nh;
ros::Subscriber ArduinoRos_sub = nh.subscribe(“cmd_vel2″,1000,Callback);

//ros::Publisher ArduinoRos_pub = nh.advertise<std_msgs::Byte>(“ArduinoTopic”, 1000);//
ros::Publisher ArduinoRos_pub = nh.advertise<geometry_msgs::Twist>(“ArduinoTopic”, 1000);
ros::Publisher ArduinoRos_pub2 = nh.advertise<geometry_msgs::Twist>(“/odometry”, 1000);
ros::Rate rate(10);
//std_msgs::Byte msg1;//
geometry_msgs::Twist msg1;
geometry_msgs::Twist msg2;
ros::spinOnce();

while(ros::ok())
{
current = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
msg1.linear.x = arr[0]; //Vx
msg1.angular.z = arr[1]; //W
msg1.linear.y = arr[2]; //Vy
dt = (double)(current-timer) * 0.001;
ROS_INFO(“%f”, dt);
theta = theta + (arr[1]) * dt;
x = x + (arr[0] * cos(theta) – arr[2] * sin(theta)) * dt;
y = y + (arr[0] * sin(theta) + arr[2] * cos(theta)) * dt;
tot_time = tot_time + dt;
//ROS_INFO(“%f”, tot_time);

ROS_INFO(“x : %f”, x);
ROS_INFO(“y : %f”, y);
ROS_INFO(“theta : %f”, theta * (180/3.14));
msg2.linear.x = x;
msg2.linear.y = y;
msg2.angular.z = theta;

ArduinoRos_pub.publish(msg1);
ArduinoRos_pub2.publish(msg2);
ros::spinOnce();
rate.sleep();
timer = current;//1
}

return 0;
}

ROS 통한 연결 및 실행
앞서 소개했듯, A_star.py와 talker.py간 연결을 제외하면 모든 코드의 연결이 ROS로 연결되어 있다. talker.py는 ros_python package안에 존재하고, ladarbot_node.cpp는 ladarbot package안에 존재한다. 아두이노와 cpp파일 간 rosserial 연결을 도와주는 serial_node.py는 rosserial_python package안에 존재한다.
실행하는 방법은 다음과 같다. 해당 명령어를 각각의 터미널 창에 입력하면 된다.

· sudo chmod a+rw /dev/ttyACM0 ->포트 사용을 위해 유저에게 읽고 쓰는 권한 제공
· roscore
· rosrun ladarbot ladarbot_node
· rosrun ros_python talker.py
· rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=57600

4.2.3.사용자 조종을 통해 얻는 지도 데이터 txt저장 및 시각화
로봇 현재위치 계산
자율주행을 구현하는데에도 통신 매개체로 쓰인 ladarbot_node.cpp가 여기에서도 사용된다. 이전에는 설명 안 했지만 ladarbot_node.cpp에도 로봇의 odometry(현재위치)를 계산하는 코드 부분이 있다. 코드 실행시간을 받아와 Vx, Vy, w와 곱하여 실시간으로 로봇의 현재 위치를 계산해 ros topic으로 lidar_range_node.cpp로 odometry를 전송한다. ladar_node.cpp의 해당 부분의 코드이다.

while(ros::ok())
{
current = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
msg1.linear.x = arr[0]; //Vx
msg1.angular.z = arr[1]; //W
msg1.linear.y = arr[2]; //Vy
dt = (double)(current-timer) * 0.001;
ROS_INFO(“%f”, dt);
theta = theta + (arr[1]) * dt;
x = x + (arr[0] * cos(theta) – arr[2] * sin(theta)) * dt;
y = y + (arr[0] * sin(theta) + arr[2] * cos(theta)) * dt;
tot_time = tot_time + dt;
//ROS_INFO(“%f”, tot_time);

ROS_INFO(“x : %f”, x);
ROS_INFO(“y : %f”, y);
ROS_INFO(“theta : %f”, theta * (180/3.14));
msg2.linear.x = x;
msg2.linear.y = y;
msg2.angular.z = theta;

ArduinoRos_pub.publish(msg1);
ArduinoRos_pub2.publish(msg2);
ros::spinOnce();
rate.sleep();
timer = current;//1
}

mapdata를 저장한 txt파일 도출
ladar_node.cpp와 ydlidar 오픈소스인 lidar.launch에서 각각 전송해준 odometry와 lidar센서값(pointcloud)를 이용해 합쳐 map을 생성하고, 그 맵을 txt파일로 만들어 저장해주는 코드이다. 해당 작업은 lidar_range_node.cpp에서 이루어졌으며, 아래는 우선 odometry를 받아오는 코드이다.

void scanCallback2(const geometry_msgs::Twist::ConstPtr& msg)
{
printf(“%f\t%f\t%f\n”, msg->linear.x, msg->linear.y, msg->angular.z); //degree, distance, x_pos, y_pos, theta
arr[0] = msg->linear.x;
arr[1] = msg->linear.y;
arr[2] = msg->angular.z;
}

다음은 lidar센서값을 lidar.launch에서 받아오는 부분의 코드이다.

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
int count = scan->scan_time / scan->time_increment;
//printf(“[YDLIDAR INFO]: I heard a laser scan %s[%d]:\n”, scan->header.frame_id.c_str(), count);
//printf(“[YDLIDAR INFO]: angle_range : [%f, %f]\n”, RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));

for(int i = 0; i < count; i++) {
float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
//printf(“[YDLIDAR INFO]: angle-distance : [%f, %f, %i]\n”, degree, scan->ranges[i], i);
if (scan->ranges[i] > 0.001){
printf(“%f\t%f\n”, degree, scan->ranges[i]); //degree, distance, x_pos, y_pos, theta
lidar[0][i] = degree;
lidar[1][i] = scan->ranges[i];
}
/*if(i == count – 2)
{
lidar[1][i] = -1.0;
}*/
}
}

다음은 두 정보를 합쳐 지도로 만든 뒤 txt파일로 내보내는 코드이다.

int main(int argc, char **argv)
{
ofstream ofile;
ofile.open(“slam.txt”);
//string str = “haha”;
//ofile.write(str.c_str(), str.size());

ros::init(argc, argv, “lidar_range_node”);
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>(“/scan”, 1000, scanCallback);
ros::Subscriber sub2 = n.subscribe<geometry_msgs::Twist>(“/odometry”,1000, scanCallback2);
ros::Rate rate(10);
ros::spinOnce();
while(ros::ok())
{
for(int i = 0;i < 10000 ; i++)
{
if(lidar[1][i] != 0)
ofile<<lidar[0][i]<<”\t”<<lidar[1][i]<<”\n”;
}

ofile<<arr[0]<<”\t”<<arr[1]<<”\t”<<arr[2]<<”\n”;
ros::spinOnce();
rate.sleep();
}
ofile<<endl;
ofile.close();
return 0;
}

txt파일을 읽어 지도를 시각화하기
저장된 txt파일을 읽어 파이썬을 통해 시각화해주는 코드이다. 해당 작업을 한 코드는 slam_map.py이며, 설명할 부분이 거의 없는 간단한 코드이기 때문에 후에 전체 코드만 공개하겠다. slam_map.py를 실행하기전, txt파일을 열어 파일의 끝에 end를 추가해주어야 한다.

실행하기
로봇을 조종하고 지도를 만들어 txt파일로 저장하기 위해서는 터미널 창에 다음과 같은 명령어를 입력해야 한다.

roslaunch ladarbot ladarbot.launch

ladarbot.launch는 ladarbot_node.cpp 및 아두이노 코드에 해당하는 ladarbot_subnode.ino, 그리고 로봇을 조종할 키보드 입력을 받아오는 코드를 한번에 실행하는 launch파일이다. 키보드 입력 받는 코드는 ros를 설치할 때 기본적으로 제공되는 파일이기에 따로 코드 설명을 하지는 않는다.
그 다음 다른 터미널 창을 열어 해당 명령어를 입력한다.

roslaunch ydlidar_ros lidar.launch

lidar.launch파일을 실행하는 명령이다.
다음으로 다른 터미널창을 열어 다음 명령어를 입력한다.

rosrun lidar_range lidar_range_node

해당 명령은 lidar_range_node.cpp파일을 실행하는 명령이다. 이 모든 명령을 실행하면 키보드의 I j l , u o m . 키로 로봇을 8방향으로 조종할 수 있게되며, 동시에 slam.txt파일이 생기며 그 파일안에 mapdata가 기록된다.
txt파일을 시각화하고 싶다면 slam_map.py가 있는 경로에 slam.txt 파일을 복사한 다음, 터미널창에 python slam_map.py를 입력한다. 그럼 시각화된 지도를 확인할 수 있다.

5. 실행 결과
5.1. 자율주행
목적지를 출발지로부터 x축으로 2.5m, y축으로 2.5m로 했을 때 동작 사진이다.

68 ICT_매카넘휠 (7)

5.2. 생성된 지도 txt파일로 저장 및 시각화

68 ICT_매카넘휠 (8)

5.3. 실행영상
실행영상을 추가 제출파일로 제출하였다. 해당 영상에서는 5.1에서처럼 로봇에 디스플레이를 연결한 것이 아닌, 영상 송수신기(브이젯 HDMI무선송수신기)를 통해 모니터로 화면을 띄웠다. 영상 마지막에 벽에 부딪히는 이유는 목적지를 출발점으로부터 x방향 2.5m, y방향 2.5m로 설정해 두었는데, 그 위치가 벽 속이여서 그러한 일이 발생한 것이다. 자율주행 자체에는 문제가 없다. 다만, 영상에서 보이는 정도의 거리는 문제없이 자율주행이 되지만, 주행시간을 늘리게 되면, 주행 중 zetson nano의 cpu가 다운되며 로봇이 정지하는 경우가 발생한다. A star 알고리즘 및 시각화알고리즘이 무거워 cpu다운이 일어나는 걸로 예상되며, 컴퓨터를 성능이 더 좋은 것으로 교체하면 해결될 문제로 보고 있다. 화면에 뜨는 map과 생성된 경로와, 로봇의 움직임을 함께 확인하면 된다.

6. 사용한 제품 리스트

제품명 디바 상품 번호
스텝모터 드라이버 TB6600 10894384
lidar ydlidar X2L -
jetson nano 12513656
arduino 및 케이블 [KEYES] 아두이노 우노 (R3) DIY 베이직 키트 [KT0030] -
배터리 tattu 12000mah -
display SZH-RP001 1382391
로봇 프레임(스텝모터, 메카넘휠 포함)
wheel universal intelligent car chassis omni directional robot
-
통신장치 브이젯 HDMI무선송수신기 -

 

 

 

 

Leave A Comment

*