项目思维导图
该项目一共三个demo：
机械臂末端走直线
2. 变位机转台转动
3.机械臂末端多点样条运动
笔记：
基于等级的蚁群系统在3D网格地图中搜索路径的方法:
基于等级的蚁群系统(Hierarchical Ant Colony System,HACS)是一种改进的蚁群优化算法。它在传统的蚁群算法基础上,通过构建等级结构来优化搜索过程。
在3D网格地图中,我们可以将地图分为多个等级层次。最高层次是整张地图的概览,地图被等分为较大的网格区域。在较低层次,每个网格区域内又被等分为更小的子区域。最后,最底层是每个子区域内的详情网格。
在搜索路径时,蚁群按照层次顺序进行。在高层次,蚁群用更加宽泛的视野搜索整个地图,找到连接开始点和目标点的大致路径区域。然后在较低层次内搜索,逐步优化路径,找到更加详细和精确的路线。
与传统蚁群相比,这种分层搜索方法可以更快地锁定搜索区域,减少无效搜索,从而提高搜索效率。同时,低层次的详细搜索也可以找到更短和更优的路径。
总之,HACS利用地图的层次信息指导搜索,使蚁群系统既有高层次的宏观视野,也有低层次的局部优化能力。这种方法可有效提高在3D网格地图中搜索路径的性能。
使用蚁群系统解决广义旅行商问题：
广义旅行商问题(Generalized Traveling Salesman Problem, GTSP)是旅行商问题的推广,使用蚁群系统可以有效解决。
GTSP问题是将多个城市分组,要求旅行商访问每个组中的一个城市,并最小化总路程。蚁群算法解GTSP步骤如下:
构建解空间。将城市分组,每个组看作为一个虚拟城市。
蚁群搜索。蚁群按照传统TSP规则搜索路径,但每经过一个虚拟城市时,会随机选择该组内的一个真实城市访问。
信息更新。当蚁群完成一轮搜索后,更新信息素,包括每个城市内的信息素和连接两个虚拟城市的信息素。
重复搜索。反复迭代上述搜索和更新过程,逐步得到更优解。
结果输出。迭代终止后,输出当前最优解作为GTSP问题的近似最优解。
这种方法融合了蚁群算法的分布式搜索能力和 GTSP 问题的组内选择要求。相比暴力算法,可以大幅减少搜索空间,更快获得近似最优解。同时也比随机算法更有针对性。因此使用蚁群系统可以高效地求解 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_Hmain程序源码：
/* 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();
}
};
}结语：demo并不完美，尤其演示3机械臂末端走样条曲线。源码具有一定学术价值，实际应用效果并不看好，或许这些算法并不适合于连续焊接场景，蚁群算法类可借鉴。仿真场景Lua脚本（焊接火花模拟）可借鉴。
The End
