# 机械臂末端轨迹规划 **Repository Path**: duandijun/matlab_workspace ## Basic Information - **Project Name**: 机械臂末端轨迹规划 - **Description**: 目前比较粗糙，持续更新中 - **Primary Language**: Matlab - **License**: Apache-2.0 - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 0 - **Forks**: 2 - **Created**: 2023-09-14 - **Last Updated**: 2024-03-31 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README # 机械臂的仿真过程 ### 项目说明 * 该项目是进行机械臂末端执行器在笛卡尔空间的轨迹规划 * 研究目的：使得机械臂的关节角度、角速度、角加速度的变化平滑，同时还要使得时间最优 * 在时间最优规划当中，使用最小最大法则将轨迹规划问题转换为非线性时间优化问题，是一个凸优化问题，有且仅有一个解。 * 包含 * 直线轨迹规划 * 圆弧轨迹规划 * 多段直线轨迹规划 * 多段直线轨迹拐点处的圆弧插补 * B样条轨迹规划 * 时间最优规划 * ...... ### 一些文件的说明(后续增加的文件参见Change log) #### my_robot.m * 本项目机械臂的DH表 #### traj_planning.m * 使用**matlab**的**RTB**工具箱做的一个关节空间和笛卡尔空间的轨迹规划测试 #### traj_path_threeinterp.m 和 traj_path_fiveinterp.m * 关节空间的轨迹规划例子 * 重写了jtraj函数为Five_interp.m * 增加了三次多项式轨迹规划函数 #### functions文件夹 * 该项目中所用到的函数 #### robot_model文件夹 * 一些机械臂的cad文件 * coppeliasim的matlab和python的远程接口 #### line_traj_speed_planning.m * 包含笛卡尔空间直线轨迹的五种速度规划方式，分别是梯形规划，三次多项式规划，五次多项式规划，正弦函数规划，指数规划 * 速度规划方式是先加速，再匀速，再减速 * 使用逆雅可比矩阵求解角速度 * 使用逆雅可比矩阵和雅可比矩阵的导数求解角加速度 #### line_traj_derivat_kinematic.m * 同上述前三点 * 使用逆加速度级运动学求解关节角加速度（雅可比矩阵求导） * 弃用指数规划（指数规划只能逼近，不能相等，角加速度会有突变） * 在计算某段路径最短时间时，当时间较短时，指数规划在加速段的值不能逼近要求的速度 #### test_inverse_kinematic.m 测试机械臂的解析解（封闭解） *采用下表所示的DH数据可以求得八组解析解，当第3个关节的a有值时，只有前四组解满足要求 #### B_Spline_Traj.m 规划B样条曲线 *在B样条曲线规划器APP：plot_by_click，通过点击生成一条B样条曲线，使机械臂沿着该曲线进行运动 *同时关节角度是平滑变化的 #### line_traj_min_time(_Bspline).m 计算轨迹的最短时间 *采用梯度下降法，计算直线轨迹或B样条曲线轨迹的最短时间 ## 机械臂DH表 | j | theta | d | a | alpha | |:---:|:---------:|:---------:|:---------:|:---------:| | 1| q1| 1| 0| pi/2| | 2| q2| 0| 2| 0| | 3| q3| 0| 0| pi/2| | 4| q4| 2| 0| pi/2| | 5| q5| 0| 0| -pi/2| | 6| q6| 1| 0| 0| ## Change_Log ### 2023/5/7 * 增加了B样条曲线轨迹的仿真，规划和时间优化 * 增加了B样条曲线规划器APP plot_by_click.mlappinstall * 增加了B样条曲线从点集生成B样条曲线的功能，见B_spline文件夹，包括2D和3D的拟合 * 增加了一些文件的注释 ### 2022/5/3 * 添加牛顿欧拉机械臂动力学函数Newton_Euler.m 用于计算机械臂关节力矩 * 添加连杆集中质量和连杆分布质量的关节力矩比较图 ### 2022/4/28 * 添加模糊控制，梯度下降法，根据运动学约束求解时间最优轨迹规划 * 下一步添加力控制，求解时间最优轨迹规划所需的关节力矩 ### 2021/12/17 * 修改圆弧轨迹插补和贝塞尔轨迹插补函数的中的姿态插补模块，将其与末端的位置解耦 ### 2021/12/16 * 修改函数Cartesian_traj_bezier_curve.m * 为其增加了两种模式，分别可以指定时间和指定边界速度、 * 新增函数Cartesian_traj_free_curve_bezier.m * 用于规划笛卡尔空间自由曲线，使用分段贝塞尔曲线进行规划 * 新增脚本Free_curve.m * 用于测试函数Cartesian_traj_free_curve_bezier.m ### 2021/12/9 * 修改函数Cartesian_traj_bezier_curve.m * 现在可以指定贝塞尔曲线的阶数为1，2，3，4，5，6，7，8，9阶 * 新增贝塞尔曲线过渡轨迹动图和各关节角度，角速度，角加速度以及末端的速度和加速度曲线图 ### 2021/12/7 * 修改函数Cartesian_traj_bezier_curve.m * 现在可以指定贝塞尔曲线的阶数为2，4，6阶 ### 2021/12/6 * 新增函数Cartesian_traj_multi_points_bezier_curve_transiation.m * 在笛卡尔空间规划一条使用四阶贝塞尔曲线过渡的轨迹 * 修改函数Cartesian_traj_bezier_curve.m * 修改原2阶贝塞尔曲线，现根据拐点特点，规划5点4阶贝塞尔曲线及其一阶和二阶导数 * 修改函数bezier_curve.m * 将原函数改为采用递归法求解n阶贝塞尔曲线 * 新增贝塞尔曲线过渡轨迹动图和各关节角度，角速度，角加速度以及末端的速度和加速度曲线图 ### 2021/12/3 * 新建函数bezier_curve.m * 画一条二阶贝塞尔曲线 * 新建函数Cartesian_traj_bezier_curve.m * 在笛卡尔空间规划一条贝塞尔曲线 * 新建脚本bezier_curve_traj.m * 用于测试函数Cartesian_traj_bezier_curve.m的效果 * 新建脚本test_bezier_curve.m * 用于测试函数bezier_curve.m的效果 ### 2021/11/30 * 修改函数Cartesian_traj_multi_points_arc_transition_modify.m * 函数现在可以完成超过三个点的直线曲线路径插补 ### 2021/11/27 * 修改函数JointSpace2CartSpace.m * 增加功能选项 * mod： * dangle:五次多项式角速度规划 * angle:七次多项式角度规划 * 新增函数Seven_interp.m * 给定边界条件，可实现七次多项式的插补 * 修改函数Cartesian_traj_multi_points_arc_transition_modify.m * 不使用给定圆弧半径求曲线起点与终点的方法，采用给定等腰三角形底边长度r的方法确定曲线的起点与终点。当r值较小时，会产生轨迹的过拟合现象 ### 2021/11/26 * 修改函数 Cartesian_traj_poly_five.m * 增加速度规划设置 * mod:normal:常规五次多项式速度规划，此时boundary默认为[0 0] sp1:初速为0，末速为匀速时的速度 sp2:初速为匀速时的速度，末速为0 * 新增函数JointSpace2CartSpace.m * 实现关节空间到笛卡尔空间的映射 * 新增脚本test_JS2CS.m * 用于测试函数JointSpace2CartSpace.m * 修改函数Cartesian_traj_multi_points_arc_transition_modify.m * 在曲线段使用关节空间五次多项式轨迹规划 * 对于求最小时间，添加了不同比例系数kp对应的时间收敛曲线，在trajectory/picture文件夹下 ### 2021/11/25 * 修改函数 Cartesian_traj_poly_five.m和Cartesian_circle_traj_2p1o_poly.m * 笛卡尔空间五次多项式速度规划现在可以设定边界速度boundary_velo和边界角速度boundary_omiga * 修改函数 Cartesian_traj_multi_points_arc_transition_modify.m * 可以设置直线段与圆弧段连接点的边界速度 ### 2021/11/24 * 修改脚本 line_traj_min_time.m * 修改求给定路径最小时间的方法，采用类似PID的方法，当有合适的比例系数Kp时，可以很快收敛到最小时间 * 角加速度是否满足要求的判断条件，达到最大角加速度约束条件的99.8%+-0.2% * 增加脚本 test_line_min_time.m * 随机生成多条路径，测试上述最小时间求法 ### 2021/11/20 * 修改函数 Inverse_kinematic.m * 在求解关节角度时，考虑机械臂的关节角度限制 * 缺点是可用的解数量减少，机械臂经过奇异点时，关节角会发生突变。 ### 2021/11/12 * 修改函数 * Cartesian_traj_poly_five.m * Cartesian_traj_poly_three.m * Cartesian_traj_sin.m * Cartesian_circle_traj_poly.m * 增加功能：上述四个函数可以实现姿态插补（使用四元数），新增输出末端执行器的姿态变换速度和姿态变换加速度 * 前两个函数的姿态插补分别使用五次和三次多项式 * Cartesian_traj_sin.m使用三次多项式插补 * Cartesian_circle_traj_poly.m使用五次多项式插补 * 新增函数 Cartesian_circle_traj_2p1o_poly.m * 实现两点一圆心进行圆弧插补的功能 * 修改函数Cartesian_traj_multi_points_arc_transition_modify.m * 简化多余代码，实现姿态插补功能 * 新建文件夹pictures * 包含本项目中所包含的图片和一些GIF图 ### 2021/11/11 * 修改函数Inverse_kinematic.m * 使其输出方差最小的一组解，使其经过奇异点时角度不发生突变 * 缺点是机械臂的臂形不是期望的臂形，可能会超出设计的关节限制。 * 修改脚本中的逆运动学求解的机器人工具箱函数jixiebi.ikien为Inverse_kinematic ### 2021/11/9 * 新建函数Cartesian_traj_multi_points_arc_transition_modify.m * 采用直线段-圆弧段-直线段的多段多项式速度插补 * 优点：角加速度在直线与圆弧段的连接处平滑 * 缺点：运动所需时间变长，效率低下 * 改进 * 对减速段与加速段的边界速度进行调整，提高运行速度 * 问题在于，本项目中的多项式速度规划采用的是等时加减速插补 * 需要对多项式速度插补函数进行相应的改进 ### 2021/11/6 * 新增圆弧插补功能 * 新建函数Cartesian_traj_multi_points_arc_transition.m文件 * 可实现笛卡尔空间两段直线之间的圆弧插补 * 修改脚本mulit_traj_arc_transition.m * 用于测试该函数 * 存在的问题 * 当末端执行器由直线运动转为圆周运动时，会有一个瞬时的向心加速度 * 这就导致机械臂的末端执行器在进入和离开圆周运动的瞬间会有加速度的突变 * 由于采用加速度级逆运动学求解机械臂的关节角加速度 * 所以机械臂的关节角加速度产生了突变 ### 2021/11/5 * 修改函数Cartesian_circle_3p.m * 采用行列式求D1 * 新增可输出三点平面的法向量 * 新增函数Cartesian_circle_traj_poly.m * 对三点确定的圆进行角速度的多项式插补 * 输入三个点处的齐次矩阵，期望时间，加速时间占比，输出圆弧上轨迹点的齐次矩阵，速度，加速度等信息 * 新增脚本circle_traj.m * 测试末端执行器进行圆弧运动时，关节参数的变化 * 修改直线轨迹和多段轨迹测试脚本的figure显示布局 ### 2021/11/4 * 新增函数Cartesian_traj_multi_points_poly_five.m * 对多个路径点之间的速度进行插值，输出插值点的齐次矩阵，速度，加速度等信息 * 修改脚本multi_traj.m * 使用上述函数进行笛卡尔空间的多段轨迹规划 * 新增函数Cartesian_circle_3p.m * 输入空间三个点，输出圆心和半径 ### 2021/11/3 * 新增函数jacob_derivative.m * 用于求解本项目机械臂的雅可比矩阵的导数，采用数值法求导 * 新增脚本test_jacob_dot.m * 用于测试雅可比矩阵求导 * 修改脚本line_trajectory_cartesian_speed_planning_2.m的文件名为line_trajderivat_kinematic.m * 速度规划函数进行改动，可输出末端执行器的加速度（x,y,z方向），速度由标量变为矢量（x,y,z方向） * 重命名get_speed.m为line_traj_min_time.m * 用于计算末端执行器在给定路径上（直线）的最短时间 ### 2021/11/2 * 新增函数**jacob_matrix.m** * 用于计算本项目中机械臂的雅可比矩阵 * 新增脚本**test_jacobian_matrix.m** * 用于测试该函数和机器人工具箱所带的函数是否匹配 * 新增脚本**line_trajectory_cartesian_speed_planning_2.m** * 使用机器人加速度级运动学求解机械臂的关节角加速度 * 修改函数**Cartesian_traj_ladder.m** * 可以输出末端执行器的加速度