실시간 영상처리를 이용한 로봇팔 제어 ( HODOO BOT )
Project HoDoo (3 dimentional Robot Arm Control By ComputerVision) - YouTube
영상처리를 이용한 로봇제어(PJ.HD) - YouTube
조장 : 나(brainKimDU)
조원 : HO , MIN
역할 분배
나 :
1. 로봇팔의 수학적 모델링 및 연산량을 줄이기 위한 방안 연구 및 아이디어 제시
2. 보고서 작성, 물품 구매, 졸업논문 작성, 발표 담당
HO :
1. 로봇팔 프로그래밍 담당
2. 로봇과의 블루투스 통신 담당
MIN
1. 영상처리 프로그래밍 담당
2. + 머신러닝
제작과정
『로봇팔 제어 시스템』 호두봇
만화 속 주인공들은 로봇을 제어하기 위해 자신의 몸을 움직여 거대한 로봇을 움직인다. 거대한 로봇은 마치 사람이 움직이는 것처럼, 주인공의 행동을 따라한다. 이와 같은 방법으로 사람의 움직임으로 로봇을 제어할 수 있을까? 이러한 고민을 바탕으로 사람 팔움직임을 그대로 따라하는 로봇을 제작하는 프로젝트를 시작하게 되었다.
이를 구현하기 위해 대략적인 설계도면을 그렸다. 구조는 간단하다.
자세한 동작과정은 블록선도를 확인하자.
사람의 손을 카메라가 찍는다. -> 컴퓨터가 영상처리를 하고, 좌표값을 구한다. -> 로봇팔을 움직인다
적절한 시간분배를 위해 목표를 나누었다.
1차 목표 : 2차원상 로봇팔 움직임 구현 (각자 연구한 내용을 하나로 합치기)
2차 목표 : 3차원상 로봇팔 움직임 구현
3차 목표 : 머신러닝을 통한 로봇팔 움직임 구현
1차 목표에 도달하기 위한 준비과정
1. 고성능의 로봇팔이 필요할 것이란 생각에 예산의 70%이상 (약 35만원)을 로봇팔 구매로 사용하였다.
2. 로봇팔이 하늘을 향해 있을때(원점)을 알아내기 위한 초기 과정을 가졌다.
3. 로봇팔의 움직임을 수학적으로 모델링하기
로봇공학에서 배웠던 역기구학을 사용하기로 하였다. 이를 알고리즘으로 구현하는 과정의 코드이다. C로 작성되었다.
#define _CRT_SECURE_NO_WARNINGS
#include <stdio.h>
#include <math.h>
#define PI 3.141592654
void createTMatrix(int jointNo, double param[3][4], double T[4][4]);
/*제목 : T메트릭스 생성
* 내용 : 트렌스폼 메트릭스 생성, 조인트넘버와 링크파라메터표를 주고 4X4 T matrix를 만들어 주는 것
*/
void multTransform(double A[4][4], double B[4][4], double C[4][4]);
/*제목: AXB 하는 연산
*/
void print4x4Mtx(char* str, double A[4][4]);
/* 4X4 행렬을 출력
*/
void print3x4Mtx(char* str, double A[3][4]);
/* 3X4 행렬을 출력
*/
void FileWrite(double T_Origin[4][2]);
/* 각각의 좌표계의 모든 원점들을 메모장으로 기록하는 함수
*/
int main(void)
{
double T_Origin[4][2] = { 0 }; // 각 좌표계의 원점을 나타내는 2차원 배열
double T_0_1[4][4]; // Homogeneous Transform Matrix from {0} to {1} 0->1로 가는 T
double T_1_2[4][4]; // Homogeneous Transform Matrix from {1} to {2} 1->2로 가는 T
double T_2_3[4][4]; // Homogeneous Transform Matrix from {2} to {3}
double T_0_3[4][4]; // 로봇 베이스 {0} 에서 {3} 을 표현하는 행렬
double T_0_2[4][4];
double param[3][4] = { 0, }; // 그림 3.8 의 링크인자 행렬. 알파i-1 ai-1 세타i di 를 한거
double L1 = 1.0; // L1 = 1 미터
double L2 = 1.0; // L2 = 1 미터
int i, jointNo;
double theta; // 사용자에게서 입력 받을 관절 각
// double A[4][4] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};
// double B[4][4] = {2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17};
// double C[4][4] = {0, };
//**** 파라미터 행렬 초기화.그림 3.8
param[1][1] = L1;
param[2][1] = L2;
print3x4Mtx("param Mtx", param); // 파라메터 메트릭스 출력
//**** 사용자에게서 관절 각 3개를 입력 받아서 param 행렬에 기록한다. theta 3개를 받는다.
for (i = 0; i < 3; i++) {
printf("관절 %d 의 각을 입력하세요(단위: 도) : ", i + 1);
scanf_s("%lf", &theta);
param[i][3] = theta * PI / 180.0; // degree 를 radian 으로 변환
}
//**** 변환 행렬 계산
jointNo = 0; // 1 번 관절
createTMatrix(jointNo, param, T_0_1); // 변환행렬 계산
// T_0_1 행렬 화면에 출력.
print4x4Mtx("T_0_1", T_0_1);
jointNo = 1; // 2 번 관절
createTMatrix(jointNo, param, T_1_2); // 변환행렬 계산
// T_1_2 행렬 화면에 출력.
print4x4Mtx("T_1_2", T_1_2);
jointNo = 2; // 3 번 관절
createTMatrix(jointNo, param, T_2_3); // 변환행렬 계산
// T_2_3 행렬 화면에 출력.
print4x4Mtx("T_2_3", T_2_3);
//**** 변환행렬 곱셈하여 T_0_3 계산
multTransform(T_0_1, T_1_2, T_0_2);
print4x4Mtx("T_0_2", T_0_2);
multTransform(T_0_2, T_2_3, T_0_3); // T_0_3 계산
print4x4Mtx("T_0_3", T_0_3);
// T_0_3 행렬 화면에 출력.
printf(" \n원점 %.1f , %.1f \n", 0.0 , 0.0); // 원점은 0이다.
printf("1번 %.1f , %.1f \n", T_0_1[0][3], T_0_1[1][3]); // 각 좌표계의 원점의 값을 표현한다.
printf("2번 %.1f , %.1f \n", T_0_2[0][3], T_0_2[1][3]);
printf("3번 %.1f , %.1f \n", T_0_3[0][3], T_0_3[1][3]);
T_Origin[0][0] = 0; // 원점
T_Origin[0][1] = 0;
T_Origin[1][0] = T_0_1[0][3]; // 1번좌표계 원점설정
T_Origin[1][1] = T_0_1[1][3];
T_Origin[2][0] = T_0_2[0][3]; // 2번 좌표계 원점설정
T_Origin[2][1] = T_0_2[1][3];
T_Origin[3][0] = T_0_3[0][3]; // 3번 좌표계 원점설정
T_Origin[3][1] = T_0_3[1][3];
FileWrite(T_Origin);
/* 각각의 좌표계의 모든 원점들을 메모장으로 기록하는 함수
*/
//[코딩 과제]
// 엑셀 파일을 이용해서 로봇의 링크 위치를 그림으로 그리려 한다.
// 원점 좌표 (0,0) 과 로봇 좌표계 {1}, {2}, {3} 의 원점 (x,y) 좌표를 파일에 기록한다.
// 이후에 엑셀로 열어서 로봇의 링크 위치를 그림으로 그려서 확인한다.
return 0;
}
void createTMatrix(int jointNo, double param[3][4], double T[4][4])
{
// 입력
// int jointNo : 관절 번호. 0,1,2
// double param[3][4] : 파라미터 행렬
// 반환
// double T[4][4] : 계산된 변환행렬. 식 (3.6) 을 계산하여 반환한다.
//
double alpha_i_1, a_i_1, d_i, theta_i;
alpha_i_1 = param[jointNo][0];
a_i_1 = param[jointNo][1];
d_i = param[jointNo][2];
theta_i = param[jointNo][3];
//[코딩] 식 (3.6)을 여기에 코딩.
T[0][0] = cos(theta_i);
T[0][1] = -sin(theta_i);
T[0][2] = 0.0;
T[0][3] = a_i_1;
T[1][0] = sin(theta_i) * cos(alpha_i_1);
T[1][1] = cos(theta_i) * cos(alpha_i_1);
T[1][2] = -sin(alpha_i_1);
T[1][3] = -sin(theta_i) * d_i;
T[2][0] = sin(theta_i) * sin(alpha_i_1);
T[2][1] = cos(theta_i) * sin(alpha_i_1);
T[2][2] = cos(alpha_i_1);
T[2][3] = cos(alpha_i_1) * d_i;
T[3][0] = 0;
T[3][1] = 0;
T[3][2] = 0;
T[3][3] = 1.0;
return;
}
void multTransform(double A[4][4], double B[4][4], double C[4][4])
{
// 입력 : 두개의 변환 행렬 A[4][4], B[4][4]
//
// 반환 : C 행렬 = A * B
int i, j, k;
for (i = 0; i < 4; i++) {
for (j = 0; j < 4; j++) {
C[i][j] = 0.0;
for (k = 0; k < 4; k++)
C[i][j] += A[i][k] * B[k][j];
}
}
}
void print4x4Mtx(char* str, double A[4][4])
{
int i, j;
puts(str);
for (i = 0; i < 4; i++) {
for (j = 0; j < 4; j++)
printf("\t%.2f ", A[i][j]);
printf("\n");
}
}
void print3x4Mtx(char* str, double A[3][4])
{
int i, j;
puts(str);
for (i = 0; i < 3; i++) {
for (j = 0; j < 4; j++)
printf("\t%.2f ", A[i][j]);
printf("\n");
}
}
// 2차원 배열을 받아오는 파일 입출력 담당 함수
void FileWrite(double T_Origin[4][2]) {
FILE* fp;
int i, err, dataIndex = 0;
err = fopen_s(&fp, "data.txt", "wt");
printf("data.txt를 여는 중입니다.......");
if (err == 0) {
puts("성공\n");
}
else {
puts("파일 입출력 실패.\n");
getchar();
return - 1;
}
printf("작성 중....");
fprintf(fp, "\tx좌표\ty좌표\n");
fprintf(fp, "원점\t%.1f\t%.1f\n", T_Origin[0][0], T_Origin[0][1]);
fprintf(fp, "{1}\t%.1f\t%.1f\n", T_Origin[1][0], T_Origin[1][1]);
fprintf(fp, "{2}\t%.1f\t%.1f\n", T_Origin[2][0], T_Origin[2][1]);
fprintf(fp, "{3}\t%.1f\t%.1f\n", T_Origin[3][0], T_Origin[3][1]); // 해당 순으로 원점에 관한 정보를 저장한다.
fclose(fp);
printf("성공 \n 프로그램을 종료합니다.");
}
4. 실시간 영상처리와 임베디드 시스템의 한계를 이해하고 앞으로의 방향성을 제시한다.
실시간 처리와 MCU를 이용한 시스템이기 때문에 연산량을 줄여야 하는 것에 모든 조원이 동의를 하고 있는 상황이였다.
역기구학을 통해 좌표값을 계산하는 것은 상당히 복잡한 과정의 연산을 거친다. 실시간 처리로 계획을 하고 있었던 상황에서 테이블 방식으로 연산을 처리하는 것으로 조원과 협의 하였다.
(테이블 방식 : 역기구학 알고리즘을 통해 좌표값을 미리 계산한다. 그리고 이를 모아놓은 데이터자료를 프로그램을 구동할때 불러온다. / 복잡한 연산이 필요없고 값을 입력하면 그에 맞는 값만 찾으면 되기 때문에 연산속도를 줄일 수 있다.)
#define _CRT_SECURE_NO_WARNINGS
/*
* 저자: 박호윤
* 목적: 데이터 세팅 프로그램
* last date: 2021.04.13
* Version: 1.0.0
*/
#include <stdio.h>
#include <math.h>
#define PI 3.141592654
void createTMatrix(int jointNo, double param[3][4], double T[4][4]);
void multTransform(double A[4][4], double B[4][4], double C[4][4]);
void print4x4Mtx(char* str, double A[4][4]);
void print3x4Mtx(char* str, double A[3][4]);
void mult_Tr_Vector(double A[4][4], double B[4], double C[4]);
void print4x1Vector(char* str, double A[4]);
int main(void)
{
FILE* Mfp;
FILE* Lfp;
FILE* Afp;
int err;
double tool[5][2] = { 500,0,600,100, 500,100, 500,-100, 600,-100 };
double apos[4] = { 0,0,0,1 }, bpos[4] = { 0,0,0,1 };
int jointNo;
double theta1 = 0, theta2 = 0;
int L1 = 1000; // L1
int L2 = 1000; // L2
//**** 사용자에게서 링크길이를 받는다
scanf("%d", &L1);
scanf("%d", &L2);
printf("L1: %d\n", L1);
printf("L2: %d\n", L2);
puts("");
//링크 길이(XY좌표에대한비율) 정보 저장
err = fopen_s(&Lfp, "Ldata.txt", "wt"); // 파일 오픈
printf("Ldata.txt Opening...");
if (err == 0) { // 성공
puts("Success");
}
else {
puts("Open Failure. Press Enter."); // 실패
getchar();
return -1;
}
fprintf(Lfp, "%d\n%d", L1, L2);
puts("Ldata saved.\n");
fclose(Lfp);
//좌표-모터각 정보 저장
err = fopen_s(&Mfp, "Mdata.txt", "wt"); // 파일 오픈
printf("Mdata.txt Opening...");
if (err == 0) { // 성공
puts("Success");
}
else {
puts("Open Failure. Press Enter."); // 실패
getchar();
return -1;
}
printf("File writing ... \n");
for (int x = 0; x <= L1 + L2; x++)
{
for (int y = 0; y <= L1 + L2; y++)
{
if ( (sqrt(pow(x, 2) + pow(y, 2)) >= sqrt(pow(L1, 2) + pow(L2, 2))) && (sqrt(pow(x, 2) + pow(y, 2)) <= (L1 + L2)) )
{
theta1 = atan2(x, y) - acos((pow(x, 2) + pow(y, 2) + pow(L1, 2) - pow(L2, 2)) / (2 * L1 * sqrt(pow(x, 2) + pow(y, 2))));
theta2 = 1 * acos((pow(x, 2) + pow(y, 2) - pow(L1, 2) - pow(L2, 2)) / (2 * L1 * L2));
fprintf(Mfp, "P(%d,%d)=[%.0f,%.0f]\n", x, y, theta1 * 180 / PI, theta2 * 180 / PI);
}
}
}
fclose(Mfp);
puts("Mdata saved \n");
//방위각-각도 정보 저장
printf("AzimuthData.txt Opening...");
Afp = fopen("AzimuthData.txt", "w");
if (Afp == NULL)
{
puts("Open Failure. Press Ente");
getchar();
return -1;
}
else
{
puts("Success");
}
printf("File writing ... \n");
float Azimuth;
for (int i = 0; i < L1 + L2; i++)
{
for (int j = 0; j < L1 + L2; j++)
{
Azimuth = atan2(j, i) * (180 / PI);
fprintf(Afp, "P(%d,%d)=[%d] \n", i, j, (int)Azimuth);
}
}
puts("Azinuth data saved.\n");
fclose(Afp);
puts("End of program. Press Enter");
return 0;
}
void createTMatrix(int jointNo, double param[3][4], double T[4][4])
{
// 입력
// int jointNo : 관절 번호. 0,1,2
// double param[3][4] : 파라미터 행렬
// 반환
// double T[4][4] : 계산된 변환행렬. 식 (3.6) 을 계산하여 반환한다.
//
double alpha_i_1, a_i_1, d_i, theta_i;
alpha_i_1 = param[jointNo][0];
a_i_1 = param[jointNo][1];
d_i = param[jointNo][2];
theta_i = param[jointNo][3];
//[코딩] 식 (3.6)을 여기에 코딩.
T[0][0] = cos(theta_i);
T[0][1] = -sin(theta_i);
T[0][2] = 0.0;
T[0][3] = a_i_1;
T[1][0] = sin(theta_i) * cos(alpha_i_1);
T[1][1] = cos(theta_i) * cos(alpha_i_1);
T[1][2] = -sin(alpha_i_1);
T[1][3] = -sin(alpha_i_1) * d_i;
T[2][0] = sin(theta_i) * sin(alpha_i_1);
T[2][1] = cos(theta_i) * sin(alpha_i_1);
T[2][2] = cos(alpha_i_1);
T[2][3] = cos(alpha_i_1) * d_i;
T[3][0] = T[3][1] = T[3][2] = 0.0;
T[3][3] = 1.0;
return;
}
void mult_Tr_Vector(double A[4][4], double B[4], double C[4])
{
int i, j;
for (i = 0; i < 4; i++) {
C[i] = 0.0;
for (j = 0; j < 4; j++) {
C[i] += A[i][j] * B[j];
}
}
}
void multTransform(double A[4][4], double B[4][4], double C[4][4])
{
// 입력 : 두개의 변환 행렬 A[4][4], B[4][4]
//
// 반환 : C 행렬 = A * B
int i, j, k;
for (i = 0; i < 4; i++) {
for (j = 0; j < 4; j++) {
C[i][j] = 0.0;
for (k = 0; k < 4; k++)
C[i][j] += A[i][k] * B[k][j];
}
}
}
void print4x4Mtx(char* str, double A[4][4])
{
int i, j;
puts(str);
for (i = 0; i < 4; i++) {
for (j = 0; j < 4; j++)
printf("\t%.1f ", A[i][j]);
printf("\n");
}
}
void print3x4Mtx(char* str, double A[3][4])
{
int i, j;
puts(str);
for (i = 0; i < 3; i++) {
for (j = 0; j < 4; j++)
printf("\t%.1f ", A[i][j]);
printf("\n");
}
}
void print4x1Vector(char* str, double A[4])
{
int i;
printf("%s : ", str);
for (i = 0; i < 4; i++)
printf("%.2f ", A[i]);
printf("\n");
}해당 코드를 통해 Ldata.txt(링크길이), Mdata.txt(좌표값), AzimuthData.txt(방위각) 파일에 데이터를 저장한다.
5. 조원 MIN이 영상처리를 배워오다.
영상처리의 기초라고 할 수 있는 구를 검출하는 알고리즘을 배워온 조원 MIN 그리고 HO는 블루투스를 통해 로봇팔과의 시리얼 통신을 배워와 서로의 알고리즘을 구현하여
로봇팔의 2차원 구현을 성공시켰다.
목표 2단계 : 3차원 로봇팔의 움직임 구현하기
6. 3차원 로봇팔 움직임 구현을 위한 고민
1안 : 카메라 두대를 이용해서 얻은 두 평면으로 좌표값을 구해내야 하는가?
-> x좌표가 100개 , y좌표가 100개 라면 10000개의 데이터량이 필요하지만 이를 3차원으로 생각할 경우 1000000 개의 데이터가 필요했다. 그 때문에 정밀한 움직임을 포기하거나, 많은 데이터량을 가지게하거나 둘 중 하나였으나 조원 모두 데이터량을 줄여야 한다는 것에 동의하였다.
또한 예산 문제로 카메라를 한 대 더 구매할 수 없었다.
2안 : 카메라 한 대를 이용하고, z축은 거리센서를 통해 바닥과의 거리(z)값을 얻어내다.
-> 복잡도가 높아짐은 물론, 데이터량을 줄이기 어려웠다.
3안 : 카메라 한 대를 이용하여, 3차원을 2차원으로 축약하는 방법 (방위각 + 평면)과 원의 크기를 비율로 하여 z축 움직임을 구현하는 방법
하나의 평면이 각도를 움직인다. 그렇게 된다면 평면과 각도는 서로 독립이 된다. 그러면 계산이 서로 분리되기 때문에 방위각에 대한 데이터와 한 평면에 대한 데이터만 있다면 된다.
데이터량을 획기적으로 줄이는 방법을 생각해내는 것에 성공했다.
또한 원을 검출하는 코드는 원의 반지름도 함께 구해지는 사실을 알게되어, 원의 크기를 비율로 하여 3차원 움직임을 구현할 수 있을 것이란 생각이 들었다.
조원 HO는 즉각 이를 반영하여 코드를 수정하였고 마침내 3차원 로봇팔 움직임을 구현할 수 있었다.
3단계 : 졸업작품을 완성하고 시간이 남았는데, 팀원 MIN이 머신러닝 수업을 통해 손을 인식하는 알고리즘을 배워와서 이를 적용시켰다.
결론
솔직히 다른 사람들의 프로젝트에 비하면 많이 부족한 것이 사실이다.
그러나 졸업작품을 진행하면서 더 높은 목표를 새우고 달성하기 위해 고뇌하던 과정이 당시에는 정말 괴로웠으나 지금 생각하면 정말 값진 경험이였다고 생각한다.
졸업작품을 진행하면서 겪은
데이터량을 줄이고 3차원 로봇팔의 움직임을 구현하자는 생각은 결국 졸업논문의 주제가 되었고
우리 팀은 해당 프로젝트를 통해 졸업작품 경진대회 우수상을 수상할 수 있었다.
brainKimDu/Real-Time-image-processing-Using-Robot-ARM- (github.com)
GitHub - brainKimDu/Real-Time-image-processing-Using-Robot-ARM-
Contribute to brainKimDu/Real-Time-image-processing-Using-Robot-ARM- development by creating an account on GitHub.
github.com
'ㅇ 프로젝트 > (Toy)_project' 카테고리의 다른 글
| 5. 네이버 API를 이용해서 그래픽카드 특가를 노려보자 (1/2) (0) | 2022.10.02 |
|---|---|
| 4. 세계 테러 데이터 분석하기 (0) | 2022.10.01 |
| crawling을 통해 그래픽카드의 특가를 노려보자. (0) | 2022.09.29 |
| 전기기사 자격증의 데이터를 분석해보자. (0) | 2022.09.27 |
| (JAVA) (대학교) 농산물 관리 프로그램 텀프로젝트 (1) | 2022.09.01 |