프로젝트 마인드맵
이 프로젝트에는 세 가지 데모가 있습니다 .
팔의 끝은 일직선으로 걷는다.
2. 포지셔너의 턴테이블이 회전합니다.
3. 로봇 팔 끝의 다점 스플라인 운동
노트:
레벨 기반 개미 군체 시스템을 기반으로 한 3차원 그리드 맵에서 경로 탐색 방법:
HACS(Hierarchical Ant Colony System)는 개선된 개미 군집 최적화 알고리즘입니다. 전통적인 개미군집 알고리즘을 기반으로 계층 구조를 구성하여 검색 프로세스를 최적화합니다.
3D 그리드 맵에서는 맵을 여러 수준으로 나눌 수 있습니다. 가장 높은 수준은 전체 맵의 개요이며 더 큰 그리드 영역으로 균등하게 나뉩니다. 더 낮은 수준에서 각 그리드 영역은 더 작은 하위 영역으로 나뉩니다. 마지막으로 맨 아래 레이어는 각 하위 영역 내의 세부 그리드입니다.
경로를 검색할 때 개미 식민지는 계층적 순서로 진행됩니다. 높은 수준에서 개미 군집은 시작 지점과 목표 지점을 연결하는 대략적인 경로 영역을 찾기 위해 전체 맵을 검색하기 위해 더 넓은 시야를 사용합니다. 그런 다음 하위 수준에서 검색하고 단계별로 경로를 최적화하고 더 상세하고 정확한 경로를 찾으십시오.
기존의 개미 식민지와 비교할 때 이 계층적 검색 방법은 검색 영역을 더 빨리 잠그고 잘못된 검색을 줄여 검색 효율성을 향상시킬 수 있습니다. 동시에 낮은 수준의 상세 검색도 더 짧고 더 나은 경로를 찾을 수 있습니다.
한마디로 HACS는 지도의 계층 정보를 사용하여 검색을 안내하므로 개미 군집 시스템은 높은 수준의 매크로 비전과 낮은 수준의 로컬 최적화 기능을 모두 갖습니다. 이 방법은 3D 그리드 맵에서 경로 검색 성능을 효과적으로 향상시킬 수 있습니다.
개미 식민지 시스템을 사용하여 일반화된 순회 외판원 문제를 해결합니다.
일반화된 여행하는 세일즈맨 문제(GTSP)는 여행하는 세일즈맨 문제를 일반화한 것으로 개미 식민지 시스템을 사용하여 효과적으로 해결할 수 있습니다.
GTSP 문제는 도시를 그룹화하고 여행하는 세일즈맨에게 각 그룹에서 한 도시를 방문하도록 요청하고 총 이동 거리를 최소화하는 것입니다. GTSP를 해결하기 위한 Ant colony 알고리즘의 단계는 다음과 같습니다.
솔루션 공간을 구축하십시오. 도시는 그룹화되며 각 그룹은 가상 도시로 간주됩니다.
개미 식민지 검색. 개미 군집은 전통적인 TSP 규칙에 따라 경로를 검색하지만, 가상 도시를 지날 때마다 무작위로 그룹 내 실제 도시를 선택하여 방문합니다.
정보 업데이트. 개미 군집이 탐색 라운드를 완료하면 각 도시의 페로몬과 두 가상 도시를 연결하는 페로몬을 포함하여 페로몬이 업데이트됩니다.
검색을 반복하십시오. 위의 검색 및 업데이트 프로세스를 반복적으로 반복하고 점차 더 나은 솔루션을 얻으십시오.
결과 출력. 반복이 종료되면 GTSP 문제의 근사 최적해로 현재 최적해가 출력된다.
이 방법은 개미 식민지 알고리즘의 분산 검색 기능과 GTSP 문제의 그룹 선택 요구 사항을 결합합니다. Brute Force 알고리즘에 비해 검색 공간을 크게 줄일 수 있으며 근사 최적 솔루션을 더 빨리 얻을 수 있습니다. 또한 무작위 알고리즘보다 더 표적화됩니다. 따라서 개미 군체 시스템을 사용하면 GTSP 문제를 효율적으로 해결할 수 있습니다.
Windows 시스템에서 테스트하려면 Timer.h를 수정해야 합니다.
#ifndef PROJECT_TIMER_H
#define PROJECT_TIMER_H
#include <assert.h>
#include <stdint.h>
#include <time.h>
#include <windows.h>
//#include <ctime>
class Timer
{
public:
LARGE_INTEGER frequency;
LARGE_INTEGER _startTime;
explicit Timer() { start(); }
// void start() { clock_gettime(CLOCK_REALTIME, &_startTime); }// clock_gettime(CLOCK_MONOTONIC, &_startTime);
void start() {
QueryPerformanceFrequency(&frequency);
QueryPerformanceCounter(&_startTime);
}
double getMs() { return (double)getNs() / 1.e6; }
int64_t getNs() {
//struct timespec now;
LARGE_INTEGER now;
QueryPerformanceCounter(&now);//clock_gettime(CLOCK_MONOTONIC, &now);
//return (int64_t)(now.tv_nsec - _startTime.tv_nsec) +
//1000000000 * (now.tv_sec - _startTime.tv_sec);
return static_cast<double>(now.QuadPart - _startTime.QuadPart) / frequency.QuadPart*1.e9;
}
double getSeconds() { return (double)getNs() / 1.e9; }
//struct timespec _startTime;
};
#endif // PROJECT_TIMER_H주요 프로그램 소스 코드:
/* Includes ------------------------------------------------------------------*/
#include "matplotlibcpp.h"
#include <iostream>
#include "CoppeliaSim.h"
#include "sys_log.h"
#include "core/BSplineBasic.h"
#include "core/BezierCurve.h"
#include "core/Timer.h"
#include "core/ACSRank_3D.hpp"
#include "core/read_STL.hpp"
#include "core/ACS_GTSP.hpp"
/* Usr defines ---------------------------------------------------------------*/
using namespace std;
namespace plt = matplotlibcpp;
enum Pose_t
{
x,
y,
z,
alpha,
beta,
_gamma
};
#ifndef M_PI
#define M_PI 3.14159265358979323846
#define M_PI_2 M_PI/2
#endif
_simObjectHandle_Type *Tip_target;
_simObjectHandle_Type *Tip_op;
_simObjectHandle_Type *Joint[6];
_simObjectHandle_Type *platform[2];
_simSignalHandle_Type *weld_cmd;
/*Test*/
STLReader model;
ACS_Rank mySearchPath;//使用基于等级的蚁群系统在基于网格的 3D 地图中搜索路径，ACSRnk_3D 被优化为自动选择搜索参数。//
ACS_GTSP GlobalRoute;//使用蚁群系统解决广义旅行商问题//
BezierCurve<float, 3> straight_line(2);//模板类BezierCurve,用于生成贝塞尔曲线//
BezierCurve<float, 2> platform_angle(2);
BS_Basic<float, 3, 0, 0, 0> *smooth_curve1;/*B样条曲线*/
Timer timer;//计时器//
int demo_type;//演示类型//
bool is_running = false;
float start_time = 0;
float total_time = 0;
float current_pt[6];//当前点//
float target_pt[6];//目标点//
std::vector<float> smooth_x, smooth_y, smooth_z;
/* Founctions ----------------------------------------------------------------*/
// float[6], float[3]
bool go_next_point(float *next, float *res)
{
static float last[6] = {0};
bool state = false;
for (int i(0); i < 6; i++)
{
state |= (next[i] != last[i]) ? 1 : 0;
}
if(state){
float start_pt[3] = {current_pt[0], current_pt[1], current_pt[2]};
float next_pt[3] = {next[0], next[1], next[2]};
float **ctrl_pt = new float *[3];
ctrl_pt[0] = start_pt;
ctrl_pt[1] = next_pt;
straight_line.SetParam(ctrl_pt, total_time);
start_time = timer.getMs();
for (int i(0); i < 6; i++)
{
last[i] = next[i];
}
}
float now_time = (float)timer.getMs() - start_time;
if (now_time >= total_time)
return false;
else
{
straight_line.getCurvePoint(now_time, res);
printf("This point: %.3f, %.3f, %.3f \n", res[0], res[1], res[2]);
return true;
}
}
void manual_input()
{
if (is_running == true)//运行中//
{
if (demo_type == 1)//演示类型1 机械手//
{
// exit: current time > move time ?
float now_time = (float)timer.getMs() - start_time;
if (now_time >= total_time)
is_running = false;
float res[3] = {};
straight_line.getCurvePoint(now_time, res);//获取下一点//
target_pt[0] = res[0];
target_pt[1] = res[1];
target_pt[2] = res[2];
std::cout << "Target(x,y,z):" << target_pt[0] << ", " << target_pt[1] << ", " << target_pt[2] << endl;
}
else if (demo_type == 2)//演示类型2 转台//
{
// exit: current time > move time ?
float now_time = (float)timer.getMs() - start_time;
if (now_time >= total_time)
is_running = false;
float res[2];
platform_angle.getCurvePoint(now_time, res);//获取转台的下一点：两个转角//
platform[0]->obj_Target.angle_f = res[0];
platform[1]->obj_Target.angle_f = res[1];
std::cout << "Target(pitch, yaw):" << res[0] << ", " << res[1] << endl;
}
else//其他类型//
{
static int i = 0;
if(i < smooth_x.size())
{
static clock_t lastTime = clock();
if (clock() - lastTime >= 10)
{
lastTime = clock();
if (smooth_x[i] - target_pt[0] < 0.3 && smooth_y[i] - target_pt[1] < 0.3
&& smooth_z[i] - target_pt[2] < 0.3)
{
target_pt[0] = smooth_x[i];
target_pt[1] = smooth_y[i];
target_pt[2] = smooth_z[i];
std::cout << "Target(x,y,z):" << target_pt[0] << ", " << target_pt[1] << ", " << target_pt[2] << endl;
i++;
}
}
}
else
{
is_running = false;
}
// // 论文和答辩中简单的演示,简单的状态机//
//float* res;
// static int stage = 0;
// switch(stage)
// {
// case 0:
// {
// //到第一个点
// total_time = 3000;
// float next[3] = {mySearchPath.route_points[0].x, mySearchPath.route_points[0].y, mySearchPath.route_points[0].z};
// if(go_next_point(next,res))
// {
// target_pt[0] = res[0];
// target_pt[1] = res[1];
// target_pt[2] = res[2];
// // target_pt[0] = res[0];
// // target_pt[1] = res[1];
// // target_pt[2] = res[2];
// }
// else
// {
// start_time = timer.getMs();
// stage = 1;
// }
// }
// break;
// case 1:
// {//
// //开始焊接，到第二个点//
// weld_cmd->target = 1;
// float next[3] = {mySearchPath.route_points[1].x, mySearchPath.route_points[1].y, mySearchPath.route_points[1].z};
// if(go_next_point(next,res))
// {
// /* target_pt[0] = res[0];
// target_pt[1] = res[1];
// target_pt[2] = res[2];*/
// target_pt[0] = res[0];
// target_pt[1] = res[1];
// target_pt[2] = res[2];
// }
// else{
// const std::vector<ACS_Node<float> *> *path = mySearchPath.best_matrix[1][2].getPath();
// int pt_num = (*path).size();
// float start_pt[3] = {mySearchPath.route_points[1].x, mySearchPath.route_points[1].y, mySearchPath.route_points[1].z};
// float end_pt[3] = {mySearchPath.route_points[2].x, mySearchPath.route_points[2].y, mySearchPath.route_points[2].z};
// float **ctrl_pt = new float *[pt_num];
// for (int i = 0; i < pt_num; ++i)
// {
// ctrl_pt[i] = new float[3];
// ctrl_pt[i][0] = (*path)[i]->pt.x;
// ctrl_pt[i][1] = (*path)[i]->pt.y;
// ctrl_pt[i][2] = (*path)[i]->pt.z;
// }
// smooth_curve1 = new BS_Basic<float, 3, 0, 0, 0>(pt_num);
// smooth_curve1->SetParam(start_pt, end_pt, ctrl_pt, total_time);
// start_time = timer.getMs();
// weld_cmd->target = 0;
// stage = 2;
// }
// }
// break;
// case 2:
// {
// //停止焊接，到下面焊路//
// float now_time = (float)timer.getMs() - start_time;
// if(now_time < total_time + 500)
// {
// smooth_curve1->getCurvePoint(now_time, res);
// }
// else
// {
// weld_cmd->target = 1;
// stage = 3;
// }
// }
// break;
// case 3:
// {
// // 第二段焊路//
// float next[3] = {mySearchPath.route_points[3].x, mySearchPath.route_points[3].y, mySearchPath.route_points[3].z};
// if(go_next_point(next,res))
// {
// }
// else
// {
// while(1){}
// }
// }
// break;
// default:
// break;
// }
// target_pt[0] = res[0];
// target_pt[1] = res[1];
// target_pt[2] = res[2];
//}
}
}
else//未运行//
{
//Select type 选择类型//
cout << "Please choose control type: 1) Manipulator 2) Platform 3) Demo : ";
cin >> demo_type;
if (demo_type == 1)//机械手//
{
//Set terminal points
float start_pt[3] = {current_pt[0], current_pt[1], current_pt[2]};
float next_pt[3];
float **ctrl_pt = new float *[3];
ctrl_pt[0] = start_pt;
ctrl_pt[1] = next_pt;
cout << "Current point:(" << current_pt[0] << ", " << current_pt[1] << ", " << current_pt[2] << ")" << endl;
cout << "Next point(x, y, z) and Time(t): ";
cin >> next_pt[0] >> next_pt[1] >> next_pt[2] >> total_time;
straight_line.SetParam(ctrl_pt, total_time);
//Set time
start_time = timer.getMs();
is_running = true;
}
else if (demo_type == 2)//转台//
{
//Set terminal points
float start_pt[2] = {platform[0]->obj_Data.angle_f, platform[1]->obj_Data.angle_f};
float next_pt[2];
float **ctrl_pt = new float *[2];
ctrl_pt[0] = start_pt;
ctrl_pt[1] = next_pt;
cout << "Current point:(" << platform[0]->obj_Data.angle_f << ", " << platform[1]->obj_Data.angle_f << ")" << endl;
cout << "Target angle(pitch, yaw) and Time(t): ";
cin >> next_pt[0] >> next_pt[1] >> total_time;
platform_angle.SetParam(ctrl_pt, total_time);
//Set time
start_time = timer.getMs();
is_running = true;
}
else if(demo_type == 3)//演示//
{
/*
读取工件模型//
*/
model.readFile("./files/cubic.stl");
const std::vector<Triangles<float>> meshes = model.TriangleList();
/*
搜索路径//
*/
mySearchPath.creatGridMap(meshes, 0.005, 10,"./files/cubic_grid_map.in");
mySearchPath.searchBestPathOfPoints(0.5, "./files/cubic_weld_points.in", "./files/graph.in");//没有文件，//
GlobalRoute.readFromGraphFile("./files/graph.in");
GlobalRoute.computeSolution();
GlobalRoute.read_all_segments(mySearchPath.best_matrix);
/*
曲线平滑//
*/
int pt_num = GlobalRoute.g_path_x.size();
float start_pt[3] = {GlobalRoute.g_path_x[0], GlobalRoute.g_path_y[0], GlobalRoute.g_path_z[0]};
float end_pt[3] = {GlobalRoute.g_path_x[pt_num - 1], GlobalRoute.g_path_y[pt_num - 1], GlobalRoute.g_path_z[pt_num - 1]};
float **ctrl_pt = new float *[pt_num];
for (int i = 0; i < pt_num; ++i)
{
ctrl_pt[i] = new float[3];
ctrl_pt[i][0] = GlobalRoute.g_path_x[i];
ctrl_pt[i][1] = GlobalRoute.g_path_y[i];
ctrl_pt[i][2] = GlobalRoute.g_path_z[i];
}
BS_Basic<float, 3, 0, 0, 0> smooth_curve(pt_num);
smooth_curve.SetParam(start_pt, end_pt, ctrl_pt, 150);
clock_t base_t = clock();
clock_t now_t = clock()-base_t;
float res[3];
do
{
if(clock() - base_t - now_t >= 10)
{
now_t = clock() - base_t;
smooth_curve.getCurvePoint(now_t, res);
smooth_x.push_back(res[0]);
smooth_y.push_back(res[1]);
smooth_z.push_back(res[2]);
//printf("Curve point: %f, %f, %f, time:%d \n", res[0], res[1], res[2], now_t);
}
} while (now_t <= 150);
//二次平滑//
const float constrain = 0.05;
pt_num = smooth_y.size();
float second_start_pt[9] = {GlobalRoute.g_path_x[0], GlobalRoute.g_path_y[0], GlobalRoute.g_path_z[0],0,0,0,0,0,0};
float second_end_pt[9] = {GlobalRoute.g_path_x[pt_num - 1], GlobalRoute.g_path_y[pt_num - 1], GlobalRoute.g_path_z[pt_num - 1],0,0,0,0,0,0};
float **second_pt = new float*[pt_num];
for (int i = 0; i < pt_num; ++i)
{
second_pt[i] = new float[9];
second_pt[i][0] = smooth_x[i];
second_pt[i][1] = smooth_y[i];
second_pt[i][2] = smooth_z[i];
for (int j(3); j < 9; j++)
second_pt[i][j] = constrain;
}
smooth_x.clear();
smooth_y.clear();
smooth_z.clear();
BS_Basic<float, 3, 2, 2, 2> second_curve(pt_num);
second_curve.SetParam(second_start_pt,second_end_pt,second_pt, 6000);
base_t = clock();
now_t = clock()-base_t;
do
{
if(clock() - base_t - now_t >= 50)
{
now_t = clock() - base_t;
second_curve.getCurvePoint(now_t, res);
smooth_x.push_back(res[0]);
smooth_y.push_back(res[1]);
smooth_z.push_back(res[2]);
//printf("Second point: %f, %f, %f, time:%d \n", res[0], res[1], res[2], now_t);
}
} while (now_t <= 6000);
start_time = timer.getMs();
is_running = true;
}
else
{
cout << "Unidentified type, please select again." << endl;
}
}
}
/**
* @brief This is the main function for user.
*/
void Usr_Main()
{
//这里是主循环，可以运行我们的各部分算法//
manual_input();
}
/**
* @brief User can config simulation client in this function.
* @note It will be called before entering the main loop.
*/
void Usr_ConfigSimulation() //读取句柄//
{
//添加关节对象到Joint_list，每个关节可以读写位置和速度，不用单独控制每个关节可以注释下面这段//
Joint[0] = CoppeliaSim->Add_Object("IRB4600_joint1", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});
Joint[1] = CoppeliaSim->Add_Object("IRB4600_joint2", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});
Joint[2] = CoppeliaSim->Add_Object("IRB4600_joint3", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});
Joint[3] = CoppeliaSim->Add_Object("IRB4600_joint4", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});
Joint[4] = CoppeliaSim->Add_Object("IRB4600_joint5", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});
Joint[5] = CoppeliaSim->Add_Object("IRB4600_joint6", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});
//读写执行末端相对于器件坐标系的位姿//
Tip_target = CoppeliaSim->Add_Object("IRB4600_IkTarget", OTHER_OBJECT, {SIM_POSITION | CLIENT_WO, SIM_ORIENTATION | CLIENT_WO});
Tip_op = CoppeliaSim->Add_Object("IRB4600_IkTip", OTHER_OBJECT, {SIM_POSITION | CLIENT_RO, SIM_ORIENTATION | CLIENT_RO});
platform[0] = CoppeliaSim->Add_Object("platform_yaw", JOINT, {SIM_POSITION | CLIENT_RW});
platform[1] = CoppeliaSim->Add_Object("platform_pitch", JOINT, {SIM_POSITION | CLIENT_RW});
weld_cmd = CoppeliaSim->Add_Object("weld_cmd", SIM_INTEGER_SIGNAL, {SIM_SIGNAL_OP | CLIENT_WO});
/*Init value*/
target_pt[x] = 1.76; //-0.2;
target_pt[y] = 0.09;
target_pt[z] = 1.42;
target_pt[alpha] = 0;
target_pt[beta] = M_PI_2 + M_PI_2/2;
target_pt[_gamma] = -M_PI_2;
Tip_target->obj_Target.position_3f[0] = target_pt[x] + 0;//1.7;
Tip_target->obj_Target.position_3f[1] = target_pt[y] + 0;
Tip_target->obj_Target.position_3f[2] = target_pt[z] + 0;
Tip_target->obj_Target.orientation_3f[0] = target_pt[alpha];
Tip_target->obj_Target.orientation_3f[1] = target_pt[beta];
Tip_target->obj_Target.orientation_3f[2] = target_pt[_gamma];
}
/**
* @brief These two function will be called for each loop.
* User can set their message to send or read from sim enviroment.
*/
void Usr_SendToSimulation()//设置目标位姿//
{
//这里可以设置关节指令//
Tip_target->obj_Target.position_3f[0] = target_pt[x] + 0; //1.7;
Tip_target->obj_Target.position_3f[1] = target_pt[y] + 0;
Tip_target->obj_Target.position_3f[2] = target_pt[z] + 0;
Tip_target->obj_Target.orientation_3f[0] = target_pt[alpha];
Tip_target->obj_Target.orientation_3f[1] = target_pt[beta];
Tip_target->obj_Target.orientation_3f[2] = target_pt[_gamma];
}
//读取tip当前位姿参数//
void Usr_ReadFromSimulation()
{
//这里可以读取反馈//
current_pt[x] = Tip_op->obj_Data.position_3f[0] - 0; //1.7;
current_pt[y] = Tip_op->obj_Data.position_3f[1] - 0;
current_pt[z] = Tip_op->obj_Data.position_3f[2] - 0;
current_pt[alpha] = Tip_op->obj_Data.orientation_3f[0];
current_pt[beta] = Tip_op->obj_Data.orientation_3f[1];
current_pt[_gamma] = Tip_op->obj_Data.orientation_3f[2];
}
/**
* @brief It's NOT recommended that user modefies this function.
* Plz programm the functions with the prefix "Usr_".
*/
int main(int argc, char *argv[])
{
/*
System Logger tool init.
*/
std::cout << "[System Logger] Configuring... \n";
std::cout << "[System Logger] Logger is ready ! \n";
/*
Simulation connection init.
*/
CoppeliaSim_Client *hClient = &CoppeliaSim_Client::getInstance();
std::cout << "[CoppeliaSim Client] Connecting to server.. \n";
while (!hClient->Start("127.0.0.1", 5000, 5, false))
{
};
std::cout << "[CoppeliaSim Client] Successfully connected to server, configuring...\n";
Usr_ConfigSimulation();
std::cout << "[CoppeliaSim Client] Configure done, simulation is ready ! \n";
while (1)
{
// Abandon top 5 data
static int init_num = 5;
if (hClient->Is_Connected())
{
hClient->ComWithServer();
}
if (init_num > 0)
init_num--;
else
{
Usr_ReadFromSimulation();
Usr_Main();
Usr_SendToSimulation();
}
};
}결론: 데모, 특히 3개의 로봇 팔 끝에 있는 스플라인 곡선의 데모는 완벽하지 않습니다. 소스 코드는 일정한 학술적 가치가 있지만 실제 적용 효과는 좋지 않으며 아마도 이러한 알고리즘은 연속 용접 시나리오에 적합하지 않으며 개미 식민지 알고리즘을 참조로 사용할 수 있습니다. 시뮬레이션 장면 Lua 스크립트(용접 스파크 시뮬레이션)를 참조할 수 있습니다.
끝