网站营销代理,东莞网站建设模板报价,网站开发技术与开发环境,页面模板图片大小项目思维导图 该项目一共三个demo#xff1a; 机械臂末端走直线 2. 变位机转台转动 3.机械臂末端多点样条运动 笔记#xff1a; 基于等级的蚁群系统在3D网格地图中搜索路径的方法: 基于等级的蚁群系统(Hierarchical Ant Colony System,HACS)是一种改进的蚁群优化算法。它在传… 项目思维导图 该项目一共三个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 ctimeclass 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_castdouble(now.QuadPart - _startTime.QuadPart) / frequency.QuadPart*1.e9;}double getSeconds() { return (double)getNs() / 1.e9; }//struct timespec _startTime;
};#endif // PROJECT_TIMER_H main程序源码 /* 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;//使用蚁群系统解决广义旅行商问题//
BezierCurvefloat, 3 straight_line(2);//模板类BezierCurve,用于生成贝塞尔曲线//
BezierCurvefloat, 2 platform_angle(2);
BS_Basicfloat, 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::vectorfloat 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::vectorACS_Nodefloat * *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_Basicfloat, 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 pointsfloat 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 timestart_time timer.getMs();is_running true;}else if (demo_type 2)//转台//{//Set terminal pointsfloat 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 timestart_time timer.getMs();is_running true;}else if(demo_type 3)//演示//{/*读取工件模型//*/model.readFile(./files/cubic.stl);const std::vectorTrianglesfloat 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_Basicfloat, 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_Basicfloat, 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 Its 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 datastatic 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
