|
Project 介绍
工具方向、活动冗余和路径点时序协同优化用于呆板人辅助制造
作者：Yongxue Chen, Tianyu Zhang, Yuming Huang, Tao Liu, Charlie C.L. Wang
在本文中，作者提出了一种并发和可扩展的轨迹优化方法，以提高呆板人辅助制造的质量。作者的方法在输入的包罗大量路径点的工具路径上同时优化工具方向、活动冗余和路径点时序，以提高活动的平滑性，同时考虑制造约束。差别的是，现有方法总是以解耦的方式确定这些参数。为了处理工具路径上的大量路径点，我们提出了一种基于分解的数值方案，以外核（out-of-core）方式优化轨迹，该方案也可以并行运行以提高效率。通过仿真和物理实验，作者展示了他们的方法在呆板人辅助增材制造示例中的性能。
“工具方向、活动冗余和路径点时序协同优化用于呆板人辅助制造”，有条件继承，2024年12月。
Arxiv 论文 https://arxiv.org/abs/2409.13448
截图来源：https://www.youtube.com/watch?v=vILrYwFufUk&ab_channel=CharlieC.L.Wang
代码获取与编译
代码： https://github.com/Yongxue-Chen/ConcurrentTrajOpt
安装
请使用 Qmake 文件 "ShapeLab.pro" 编译代码。
平台：Windows + Visual Studio + QT 插件（测试版本：VS2022 + QT5.14.2 + msvc2017_64）
安装步调：
- 安装 Visual Studio 扩展插件 (QT VS Tool) 以打开 *.pro 文件并生成项目
- 将 'ShapeLab' 设置为启动项目
- 更改三个项目 (GLKLib、QMeshLib 和 ShapeLab) 的平台工具集（如果不匹配）：属性页 -> 配置属性 -> 常规 -> 常规属性 -> 平台工具集 -> 选择正确的平台工具集。
- 在 ShapeLab 项目中安装所需的软件包（1）fcl，（2）abseil，（3）armadillo，（4）gsl 和（5）gtest。使用 “vcpkg” 安装这些软件包是一个简朴的方法。注意：使用 vcpkg 安装 fcl 可能会在解决方案配置为“调试”时导致编译错误。因此，最好使用“发布”配置。
- 在以下位置添加额外的库字典：ShapeLab 属性页 -> 配置属性 -> 链接器 -> 常规 -> 额外的库字典 -> 添加两个字典 "..\ThirdPartyDependence\libsvm" 和 "..\ThirdPartyDependence\osqp"
- 启用 OpenMP 以得到最佳性能：ShapeLab 属性页 -> 配置属性 -> C/C++ -> 语言 -> Open MP 支持 -> 选择 'Yes (/openmp)'
- 打开控制台：ShapeLab 属性页 -> 配置属性 -> 链接器 -> 系统 -> 子系统 -> 选择 'Console (/SUBSYSTEM:CONSOLE)'
使用
步调 0：加载呆板人模子：点击 UI 右侧的 Load Robots 按钮。
步调 1：加载工具路径和层：点击 inputPathAndLayer 按钮。\DataSet\models 中提供了一些测试示例。
步调 2：初始化优化：点击 Initialize Optimization 按钮。
步调 3：优化：点击 Optimization 按钮。呆板人关节角度以及基坐标系中工具的位置和方向将保存到 \DataSet\output 中。
CC BY 4.0许可协议
运行程序
主界面
终端输出：
- hPos=12.6pTool=151.762 -88.265 188.612delete robSystemhPos=12.6pTool= 151.585 -88.7572 189.022Finish input objFinish input objFinish input objFinish input objFinish input objFinish input objFinish input objFinish input objFinish input objFinish input objFinish input objFinish input objrobs loaded!
- Working on layer: 49Finish input objcontruct fclLayer: 0length of path: 7497Length of the part: 7497number of paths: 1
- initial optimizationvectorXd read!7497vTTPMax: 20 aTTPMax_T: 150 aTTPMax_N: 150alpha: 0.94 beta: 0.99 gamma: 0.98jJointMax: 20 20 20 20 20 20 20 20vJointMax: 0.5 0.5 0.5 0.5 0.5 0.5 1 1
- Initialize tool path...........................................................................check collision..............................................................................Initialize tool path done
- Initialize optimizationset k1 = 0, k2 = 0.5, k3 = 1initial smoothness index: 4.4961initial time: 822.721kTime: 0.00277458set kTime: 0.00277458Initialize optimization done
- initial optimization
- Block Coordinate Descentinitial smoothness index: 4.4961initial time: 822.721................................................................................................................................................Optimization time: 765.81smoothness index: 0.0945976printing time: 763.462obj: 4.26685................................................................................................................................................Optimization time: 1015.88smoothness index: 0.0890096printing time: 754.259obj: 4.21097................................................................................................................................................Optimization time: 1117.82smoothness index: 0.0873863printing time: 752.031obj: 4.19717................................................................................................................................................Optimization time: 1202.11smoothness index: 0.0875931printing time: 750.544obj: 4.18925check collision..............................................................................After collision correction: 0.0875931BCD done
- Fail to open filecannot open fileend of Opt
焦点代码
一. trajOpt.cpp
该代码实现了一个路径优化类 trajOpt，用于处理路径规划中的轨迹分割、初始化、优化以及干系的动态与多少参数计算。以下是重要功能的概述：
- 设置了关节的最大/最小角度范围。
- 初始化矩阵 M 的某些参数。
路径分割 (splitPath)：
- 将工具路径 layerPath 分割为多个路径片断，根据点之间的间隔是否高出指定的 maxDist。
路径初始化 (initializePath)：
- 计算路径点的工具位姿和法向量。
- 初始化路径点在工件坐标系中的位置。
- 通过逆活动学解算得到关节角度，并考虑路径的一连性和碰撞检测。
优化初始化 (initializeOpt)：
- 计算路径点的速度、加速度、跃度，并初始化优化干系的动态参数（如曲率、时间点等）。
- 界说目标函数的权重（例如平滑度与时间优化的衡量）。
块坐标下降优化 (bcd 和 bcd_time)：
- 使用块坐标下降法优化路径，分为两个窗口（indexSave1 和 indexSave2）交替进行。
- 每轮优化后，根据目标函数值和碰撞检测效果更新关节配置与路径时间。
局部优化 (local_optimal)：
- 针对路径片断，调解路径点的关节配置以优化局部路径的目标函数值。
动态特性计算：
- 函数如 velNum、accNum 和 jerkNum 分别计算速度、加速度和跃度。
- 伴随计算对应的导数矩阵（如 getdkVel）。
代码特点：
- 多线程并行计算：在多个地方使用 #pragma omp parallel for 加速路径点的处理。
- 碰撞检测：通过调用外部模块 colliTrain 进行快速检测与优化。
- 平滑度优化：通过速度、加速度、跃度以及时间优化相联合，最终实现路径优化。
该代码实用于呆板人路径规划中的复杂使命，尤其是涉及多维优化和高精度活动的场景。
这段代码的重要内容和功能:
这是一个基于QT框架的呆板人轨迹优化系统的主窗口(MainWindow)实现代码,重要功能包罗:
- 实现了呆板人轨迹的优化和碰撞检测
- 支持导入和管理多个3D模子(包罗呆板人模子、工件模子等)
- 提供了轨迹规划的可视化界面
- 文件管理：支持导入/导出OBJ和TET格式的3D模子文件
- 可视化控制：包罗视角控制、表现模式切换、缩放等功能
- 轨迹优化：
- 可以加载呆板人系统(loadRob)
- 获取底子碰撞数据(getBaseColi)
- 练习碰撞检测模子(trainBaseColi)
- 执行轨迹优化(trajOptimization)
- 使用TreeView管理和表现模子层级
- 提供工具栏进行导航和选择操纵
- 包罗多个参数设置的控件(如呆板人速度、加速度限定等)
- 使用OpenGL进行3D渲染
- 集成了SVM用于碰撞检测练习
- 使用Eigen库进行矩阵运算
- 实现了分层的轨迹优化算法
- 支持设置工具头位置参数
- 可调节活动学约束参数
- 提供碰撞检测干系参数配置
- 轨迹优化的各种权重系数设置
这个系统重要用于工业呆板人的轨迹规划和优化,特殊关注了碰撞克制和活动平滑性的题目。它提供了一个完整的图形界面来配置和监视整个优化过程。
二. MainWindow.cpp
MainWindow.cpp 是一个基于 Qt 框架开辟的 GUI 程序，重要用于呆板人路径优化和碰撞检测的图形化界面实现。以下是代码功能的总结：
重要功能
- 使用 Qt Designer 创建的 MainWindow 界面，初始化了工具栏、信号映射器、树视图等。
- 配置了一些默认参数（例如模子路径、优化参数）。
文件操纵：
- 支持导入和保存 .obj 和 .tet 文件，用于加载和存储三维模子数据。
- 提供节点和面选择数据的读写功能，以支持用户对模子部分进行选择和标注。
模子表现和管理：
- 使用 GLKLib 库处理三维模子的加载、表现、以及交互。
- 提供模子树视图，用于管理加载的多种类型模子（如呆板人模子、路径模子、分层模子等）。
- 支持通过拖放方式导入模子文件。
路径优化和碰撞检测：
- 集成了路径优化模块 trajOpt，提供路径初始化、分割和优化功能。
- 支持多种优化策略，包罗块坐标下降法（Block Coordinate Descent）。
- 集成了碰撞检测模块 colliTrain，通过支持向量机（SVM）和快速碰撞检测算法对模子进行练习和测试。
- 实现了基于采样的方法，生成并优化呆板人基座的碰撞数据。
呆板人和变位机加载：
- 支持 ABB 呆板人的模子加载，包罗关节角度和末端执行器位姿的转换。
- 支持变位机的加载和初始位姿设置。
用户交互：
- 提供多个按钮和菜单项触发操纵，如加载路径层、初始化优化、执行优化等。
- 支持调解三维视图导航（如放大、缩小、等距视图）和表现模式（如网格、节点、法向量等）。
路径层处理：
- 支持特定路径层的加载、处理和优化。
- 构造碰撞检测代理模子以加速路径优化。
代码特点
- 模块化设计：代码通过类和模块实现了较好的分离，例如文件操纵、路径优化、碰撞检测平分别由专用类处理。
- 并行化支持：部分计算使用 OpenMP 并行处理加速。
- 高度可配置：界面允许用户调解优化和碰撞检测的参数，支持差别模子和路径的加载与处理。
- 输出多样性：能够输出关节角度、末端执行器位姿、时间等多种优化效果。
应用场景
该程序重要用于呆板人路径规划与优化使命，实用于复杂多少环境下的路径计算，特殊是在需要高精度的碰撞检测和动态约束的场景中，例如工业主动化和制造。
三. svm_grad
这段代码的重要功能：
这是一个实现支持向量机(SVM)梯度计算的C++类SVMGrad。重要功能包罗：
- 通过文件加载SVM模子参数
- 可以加载支持向量(SVs)、权重(alphas)、偏置项(b)和核函数参数(sigma)
- 使用RBF(径向基)核函数进行计算
- 可以计算样本的分类效果(calculateClass)
- 可以计算决策函数值(calculateGamma)
- 可以计算决策函数的梯度(calculateGammaDerivative)
- 支持同时计算决策函数值和梯度(calculateGammaAndDerivative)
- 提供核函数值预计算功能(preComputeKernel)
- 可以存储中间计算效果以提高效率
- 支持Armadillo和Eigen两种矩阵运算库的数据类型转换
- 使用RBF核函数：K(x,x') = exp(-λ||x-x'||²)
- λ 参数通过sigma计算得出：λ = 1/(2σ²)
- 决策函数的计算接纳典型的SVM形式：f(x) = Σ(αᵢyᵢK(x,xᵢ)) + b
这段代码重要用于需要计算SVM梯度的场景，比如在优化题目大概需要分析决策界限时。代码结构清晰，并且考虑了计算效率的优化。
四. fileIO
该代码界说了一个名为 fileIO 的类，重要用于读取和写入与呆板人系统、路径和层数据干系的文件。代码中实现了多个功能模块，以下是重要功能的总结：
1. 路径数据输入（inputLayerPath）
从文本文件中读取路径点和法向量，生成颠末调解的路径数据并存储在 toolpath 对象中。
- 读取路径文件内容，解析为点矩阵。
- 对路径点进行坐标变换（包罗Z轴调解）后去除冗余点。
- 设置路径点和法向量到 toolpath 对象。
2. 呆板人模子数据读取（readRobotData）
从指定目次加载呆板人模子文件并构建多边形网格（PolygenMesh），同时加载干系的碰撞检测数据。
- 支持按部件（例如 BASE、LINK1 等）逐个读取 CAD 文件。
- 将读取的数据添加到 PolygenMesh 和 robSystem 中。
3. CNC 机床数据读取（readPosData）
从指定目次加载 CNC 机床的多少模子文件。
- 与呆板人数据类似，逐个加载各部件文件（如 B1、B2 等）。
- 构建并存储多边形网格和干系的碰撞检测数据。
4. 层数据读取（readLayerData）
读取并处理层数据文件（如 layerIndex.obj）。
- 将层数据加载到 QMeshPatch 中。
- 调解层坐标系并将其导入到碰撞检测模子中。
- 构建用于碰撞检测的 PQP_Model。
5. 数据写入功能
提供多种格式的数据写入方法：
- 路径点数据：写入 8 维向量数组到文件。
- 关节数据和碰撞数据：写入关节配置与对应的碰撞值。
- 两组向量：写入两组标量向量到文件。
- 刚体变换矩阵（Transform3d）：写入 4x4 变换矩阵数组。
6. 数据读取功能
从文件中读取差别类型的数据：
- 关节与标签数据：从 SVM 格式的文件中读取关节配置和标签。
- 碰撞数据：从文件中读取关节配置、碰撞数据以及位置关节数据。
- 任意向量（VectorXd）：从文件中读取一维向量数据。
7. 错误处理
在文件读取和写入过程中均加入了错误处理，如果文件操纵失败会输出相应的错误信息。
应用场景
该代码重要用于呆板人路径规划与仿真系统，功能包罗：
- 路径规划与优化。
- 多呆板人多少模子的加载与处理。
- 呆板人碰撞检测与配置空间分析。
- CNC 加工模拟中的模子管理。
五. mathTool.cpp
这段代码实现了一些用于矩阵运算的工具函数，特殊是与李代数和李群干系的运算。以下是代码的重要功能总结：
- 计算 3x3 或 4x4 矩阵的对数映射，将李群中的元素转换为对应的李代数。
- 包罗特殊情况处理，如单元矩阵和迹值为 -1 的情况。
- 对于 4x4 矩阵，分解旋转部分和位移部分，并联合对数映射的性子进行计算。
矩阵指数运算（expm）：
- 计算 3x3 或 4x4 矩阵的指数映射，将李代数中的元素映射回李群。
- 使用 Rodrigues 公式处理旋转矩阵的指数计算。
- 对于 4x4 矩阵，计算旋转和位移部分的指数映射。
导数计算（dexpm 和 diffRotm）：
- dexpm：计算矩阵指数运算的导数。
- diffRotm：计算旋转矩阵的导数，考虑角速度和角加速度的影响。
辅助函数：
- skew：将向量转换为反对称矩阵（skew-symmetric matrix）。
- vex：从反对称矩阵提取向量。
- xi2xihat：将李代数中的 6x1 向量表现转换为 4x4 反对称矩阵形式。
重要应用场景：
- 实用于呆板人学、计算机视觉中的刚体活动建模。
- 支持 SE(3) 和 SO(3) 群的对数映射、指数映射及其导数计算。
代码的焦点思想是使用李代数和李群的数学性子来简化刚体变换和旋转矩阵的计算。
六、colliTrain.cpp
这段代码界说了一个名为 colliTrain 的类，用于支持向量机(SVM)的练习、推测和干系碰撞检测算法的实现。代码重要包罗以下功能模块：
- colliTrain 构造函数初始化干系变量，包罗SVM的参数和模子文件。
- 提供了默认的参数设置和命令行解析功能(parse_command_line)，以便用户可以灵活配置SVM模子。
SVM练习与验证：
- svmtrain 方法实现了SVM模子的练习，支持多种核函数（如线性核、多项式核、径向基函数等），并支持交织验证。
- read_problem 方法读取练习数据，并解析数据文件中的特征与标签。
- do_cross_validation 方法执行交织验证以评估模子性能。
SVM推测：
- svmpredict 方法对测试数据进行推测，支持概率估计输出。
- predict 方法实现推测细节，包罗计算精度、均方偏差等。
采样与碰撞检测：
- samFirstLayer 方法对路径点进行采样，生成随机点并计算其逆解以检测碰撞。
- 通过多种随机生成和优化方法提高采样点的密度并减少碰撞点。
- 支持对采样点和支持点的碰撞情况进行渐渐优化。
支持向量机模子更新与主动学习：
- FastronModelUpdate 更新SVM模子，优化支持向量。
- FastronActiveLearning 使用主动学习策略，通过支持点生成更多采样点并进行碰撞检测。
核函数与矩阵操纵：
- 提供了多种核函数，如 kernelRQ 和 kernelPH，用于计算点与点之间的相似度。
- 通过 computeGramMatrixColumn 计算核矩阵列，用于支持向量更新。
- 包罗矩阵和向量的动态调解功能，如删除指定行和列(removeRowAndCol 和 removeElement)。
总结
该代码集成了SVM练习、路径点采样、碰撞检测和主动学习等功能，是一个面向呆板人路径规划和优化的完整实现，适合复杂场景下的碰撞规避使命。
七. osqp++.cc
这段代码实现了一个 C++ 的优化求解器接口 OSQP++（基于 OSQP），重要功能如下：
- 提供了一个高效的二次规划题目（QP）求解接口，支持希罕矩阵和向量格式。
- 提供配置选项，可以调解求解器参数（如精度、最大迭代次数、热启动选项等）。
求解功能：
- 通过设置目标函数的二次项（Hessian 矩阵）、线性项（目标系数）以及约束（等式与不等式），构造优化题目。
- 利用 OSQP 求解库进行题目求解，返回优化变量的效果。
输入/输出处理：
- 支持用户友好的 API 接口，用于快速设置输入数据。
- 输出求解的效果，包罗变量值、目标值及求解状态。
性能优化：
- 利用希罕矩阵表现和数值优化算法，提高计算效率，适合大规模优化题目。
- 提供热启动选项以减少多次求解相似题目的计算成本。
应用场景
该代码可用于工业优化、路径规划、资源分配等范畴，适合需要高效求解二次优化题目的场景。
八. robSystem.cpp
这段代码实现了一个用于呆板人系统控制的 robSystem 类，涵盖了正向活动学、逆向活动学、碰撞检测以及模子变换等多方面的功能。以下是代码的中文摘要：
1. 构造与初始化
- 初始化呆板人系统的参数，包罗末端执行器工具位置、变位机基准位置、姿态矩阵等。
- 通过工具坐标和旋转矩阵界说刀具姿态。
- 初始化呆板人的关节变换、初始位置、正向活动学及模子。
2. 正向活动学（FK）
- fkABB2600：计算 ABB 2600 呆板人各个关节的正向活动学，获取各关节的姿态矩阵。
- _FK_Pos：根据给定的参数计算呆板人位置变位机的活动学。
- _FK_eachlink：计算指定关节的正向活动学，返回对应的变换矩阵。
3. 逆向活动学（IK）
- ikABB2600：使用迭代法计算关节角度，确保末端执行器到达指定位置。
- ikPosA：基于法向矢量的逆向活动学计算，求解末端执行器的最佳姿态。
4. 模子变换
- Trans_mesh：将三维网格模子根据指定的变换矩阵进行旋转宁静移操纵。
- updateRobPos 和 updateLayerPos：
更新呆板人各关节和层模子的位置。
5. 碰撞检测
- checkBaseColi：检测呆板人与基座之间的碰撞。
- checkLayerColi：检测呆板人与某一层的碰撞。
- checkAllColi：综合检测呆板人与所有层和基座的碰撞情况。
6. 呆板人活动学分析
- JaS_ABB 和 JaB_ABB：计算雅可比矩阵，用于分析呆板人关节活动对末端执行器的影响。
- Joints2Postions：将关节角度映射为末端执行器位置。
- dfkPos：计算位置活动学的导数，用于分析位置变革率。
7. 文件和模子读取
- fclReadRobot 和 fclReadPOS：从文件加载呆板人模子和位姿信息。
- fclReadLayer：读取层模子文件。
应用场景
该代码实用于呆板人路径规划、活动控制、模子仿真以及及时碰撞检测。它提供了一套完整的工具集，用于控制呆板人活动并确保其运行安全。
九、pathStruct.cpp
这段代码实现了一个工具路径（toolpath）管理类，其功能重要围绕路径点和路径法向量的管理与计算。焦点功能可以总结如下：
十. test_fcl_unitlity.cpp
这段代码实现了一个简朴的 定时器类 和一些工具函数，用于计算碰撞检测中的多少对象类型和求解器类型。
重要功能模块：
- Timer 类：
根据平台的差别，使用 QueryPerformanceCounter（Windows）大概 gettimeofday（Unix/Linux）进行高精度计时。
- 用于计时，支持高精度的微秒、毫秒和秒级别的计时。
- start：启动计时器。
- stop：克制计时器。
- getElapsedTimeInMicroSec：获取计时器的时间隔断（微秒）。
- getElapsedTimeInMilliSec：获取计时器的时间隔断（毫秒）。
- getElapsedTimeInSec：获取计时器的时间隔断（秒）。
- getElapsedTime：返回毫秒级的时间隔断。
getNodeTypeName 函数：
- 根据传入的节点类型（NODE_TYPE）返回对应的字符串名称。
- 支持多种多少类型，如 AABB、OBB、KDOP、Box、Sphere 等。
getGJKSolverName 函数：
- 根据传入的求解器类型（GJKSolverType）返回对应的字符串名称。
- 目前支持的求解器类型有 libccd 和 内置求解器。
总结：
- Timer 类提供了用于计时的基本工具，精度较高，实用于需要精确时间丈量的场景。
- getNodeTypeName 和 getGJKSolverName 函数用于将节点类型和求解器类型转换为可读的字符串，重要用于调试或输出信息。
该代码结构简洁，重要用于辅助碰撞检测干系的计算与调试。
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。 | 
