【文献代码研究】工具方向、运动冗余和路径点时序协同优化用于机器人辅助制造...
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
这段代码实现了一些用于矩阵运算的工具函数，特别是与李代数和李群相关的运算。以下是代码的主要功能总结：
矩阵对数运算（logm）：
计算 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 函数用于将节点类型和求解器类型转换为可读的字符串，主要用于调试或输出信息。
该代码结构简洁，主要用于辅助碰撞检测相关的计算与调试。