前言

在前几期中，我们实现了PIPER机械臂的点位控制、轨迹录制、视觉识别、陀螺仪控制及手势识别。正向与逆向运动学，是连接“空间指令”与“关节动作”的桥梁。无论是点击屏幕、挥手控制，还是自动抓取，背后都离不开今天实现的这套FK/IK求解。

更重要的是——所有代码已开源，欢迎动手实践！

开源链接：https://github.com/agilexrobotics/Agilex-College.git

摘要

本章实现基于线性代数库Eigen实现松灵PIPER机械臂的正解，逆解的雅各比方法，自定义交互式标记interactive_marker_utils的实现

标签

运动学正解、运动学逆解的雅各比方法、RVIZ仿真、机械臂DH、交互式标记、松灵PIPER

功能演示

已关注 关注 重播 分享 赞 关闭观看更多更多退出全屏切换到竖屏全屏退出全屏AGV网已关注分享视频，时长00:48

0/0

00:00/00:48切换到横屏模式继续播放进度条，百分之0播放00:00/00:4800:48全屏倍速播放中 0.5倍 0.75倍 1.0倍 1.5倍 2.0倍 超清 流畅

继续观看

小白也能懂：用Eigen搞定机械臂数值法逆解

观看更多转载,小白也能懂：用Eigen搞定机械臂数值法逆解AGV网已关注分享点赞在看已同步到看一看写下你的评论

视频详情

已关注 关注 重播 分享 赞 关闭观看更多更多退出全屏切换到竖屏全屏退出全屏AGV网已关注分享视频，时长00:18

0/0

00:00/00:18切换到横屏模式继续播放进度条，百分之0播放00:00/00:1800:18全屏倍速播放中 0.5倍 0.75倍 1.0倍 1.5倍 2.0倍 超清 流畅

继续观看

小白也能懂：用Eigen搞定机械臂数值法逆解

观看更多转载,小白也能懂：用Eigen搞定机械臂数值法逆解AGV网已关注分享点赞在看已同步到看一看写下你的评论

视频详情

1.使用前准备

1.1.硬件准备

• • AgileX robotics Piper机械臂

1.2.软件环境配置

1. 1. PIPER机械臂驱动部署请参考：https://github.com/agilexrobotics/piper_sdk/blob/1_0_0_beta/README(ZH).MD

2. 2. PIPER机械臂ROS控制节点部署参考：https://github.com/agilexrobotics/piper_ros/blob/noetic/README.MD

3. 3. 安装Eigen线性代数库

sudo apt install libeigen3-dev

1.3.准备松灵PIPER的DH参数表以及关节限位

查阅松灵PIPER用户手册可以找到PIPER的改进DH参数表与关节限位：

• DH

• 机械臂关节运动范围

---

2.正向运动学计算FK

正向运动学FK的计算过程实际上是从每个关节的角度值----计算---->>机械臂某一关节在三维世界的位姿，本文以机械臂最后一个旋转关节joint6为例

2.1.准备DH参数

1. 1. 根据PIPER的DH参数表构建正向运动学计算程序，由1.3小结的松灵PIPER的改进DH参数表，可以得到

// 改进DH参数 [alpha, a, d, theta_offset]dh_params_ = {{0,         0,          0.123,      0},                     // Joint 1{-M_PI/2,   0,          0,          -172.22/180*M_PI},      // Joint 2 {0,         0.28503,    0,          -102.78/180*M_PI},      // Joint 3{M_PI/2,    -0.021984,  0.25075,    0},                     // Joint 4{-M_PI/2,   0,          0,          0},                     // Joint 5{M_PI/2,    0,          0.091,      0}                      // Joint 6};

转换为标准DH，可以参考以下的转换规则：

标准DH到改进DH

αᵢ₋₁(标准) = αᵢ(改进)

aᵢ₋₁(标准) = aᵢ(改进)

dᵢ(标准) = dᵢ(改进)

θᵢ(标准) = θᵢ(改进)

改进DH到标准DH

αᵢ(标准) = αᵢ₊₁(改进)

aᵢ(标准) = aᵢ₊₁(改进)

dᵢ(标准) = dᵢ(改进)

θᵢ(标准) = θᵢ(改进)

得到转换后的标准DH：

// 标准DH参数 [alpha, a, d, theta_offset]dh_params_ = {{-M_PI/2,   0,          0.123,      0},                     // Joint 1{0,         0.28503,    0,          -172.22/180*M_PI},      // Joint 2 {M_PI/2,    -0.021984,  0,          -102.78/180*M_PI},      // Joint 3{-M_PI/2,   0,          0.25075,    0},                     // Joint 4{M_PI/2,    0,          0,          0},                     // Joint 5{0,         0,          0.091,      0}                      // Joint 6};

1. 2. 准备DH变换矩阵

2. • 改进DH变换矩阵：

- 用Eigen改写为改进DH的变换矩阵：

T << cos(theta),            -sin(theta),            0,             a,sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -sin(alpha)*d,sin(theta)*sin(alpha), cos(theta)*sin(alpha),  cos(alpha),  cos(alpha)*d,0,                     0,                      0,             1;

- 标准DH变换矩阵：

- 用Eigen改写为标准DH的变换矩阵：

T << cos(theta), -sin(theta)*cos(alpha),  sin(theta)*sin(alpha), a*cos(theta),sin(theta),  cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta),0,           sin(alpha),             cos(alpha),            d,0,           0,                      0,                     1;

1. 3. 实现正向运动学运算的关键函数computeFK()，完整代码见代码仓库https://github.com/agilexrobotics/Agilex-College.git

Eigen::Matrix4d computeFK(conststd::vector& joint_values){//检查输入关节值数量是否足够（至少6个）if(joint_values.size() < 6) {throwstd::runtime_error("Piper arm requires at least 6 joint values for FK");}//初始化单位矩阵作为初始变换Eigen::Matrix4d T = Eigen::Matrix4d::Identity();//对每个关节：//    计算实际关节角度 = 输入值 + 偏移量//    获取固定参数d值//    计算当前关节的变换矩阵并累积到总变换for(size_ti = 0; i < 6; ++i) {doubletheta = joint_values[i] + dh_params_[i][3];  // θ = joint_value + θ_offsetdoubled = dh_params_[i][2];                       // d = d_fixed (如果是旋转关节)T *= computeTransform(dh_params_[i][0],  // alphadh_params_[i][1],  // ad,                 // dtheta              // theta);}//返回最终变换矩阵returnT;}

2.2.验证FK计算准确性

1. 1. 启动正向运动学验证程序

ros2 launch piper_kinematics test_fk.launch.py

1. 2. 启动RVIZ仿真程序，开启显示TF树，观察计算的FK所得的机械臂末端link6_from_fk的姿态是否和机器人原本的link6（由joint_state_publisher计算）是否重合

ros2 launch  piper_description display_piper_with_joint_state_pub_gui..launch.py

可以看见重合度很高，且从link6_from_fk和link6的姿态可以看出误差基本在小数点后四位。

3.逆向运动学解算IK

逆向运动学IK的计算过程实际上是给定一个目标点----计算---->>机械臂每个关节要在什么位置才能使机械臂末端到达目标点

3.1.确认关节限位

• • 需要确定PIPER机械臂每个关节的限位，确保IK解算出来的路径不会超过机械臂的限位，从而导致机械臂损坏或者其他危险情况

• • 由1.3节可以得知PIPER机械臂每个关节的限位为：

• 示意截图

• • 从而得到机械臂关节限位的矩阵

std::vector> limits = {{-154/180*M_PI, 154/180*M_PI},        // Joint 1{0,             195/180*M_PI},      // Joint 2{-175/180*M_PI, 0},                 // Joint 3{-102/180*M_PI, 102/180*M_PI},      // Joint 4{-75/180*M_PI,  75/180*M_PI},       // Joint 5{-120/180*M_PI, 120/180*M_PI}       // Joint 6};

3.2.IK的雅各比矩阵方法实现的简要步骤

求解过程

1. 1. 计算误差e：当前位姿与目标位姿的差异(6维向量：3位置+3姿态)

2. 2. 误差e是否小于阈值？

3. • 是→ 返回当前θ作为解

4. • 否→ 进入迭代优化步骤

5. 3. 计算雅可比矩阵J：6×6矩阵

6. 4. 计算阻尼伪逆：

⁺ᵀᵀ²⁻¹

λ是阻尼系数，避免奇异位形时数值不稳定

1. 5. 计算关节角度增量：

⁺

通过误差e和伪逆计算关节角度的调整量

1. 6. 更新关节角度：

应用调整量到当前关节角度

1. 7. 应用关节限制：

2. 8. 角度归一化

3. 9. 达到最大迭代?

4. • 否→ 回到步骤2继续迭代

5. • 是→ 抛出未收敛错误

关键函数computeIK()

std::vector computeIK(conststd::vector& initial_guess, constEigen::Matrix4d& target_pose,boolverbose = false,Eigen::VectorXd* final_error = nullptr){//初始化一个猜测姿态（初始姿态）if(initial_guess.size() < 6) {throwstd::runtime_error("Initial guess must have at least 6 joint values");}std::vector joint_values = initial_guess;Eigen::Matrix4d current_pose;Eigen::VectorXd error(6);boolsuccess = false;//开始迭代计算for(intiter = 0; iter < max_iterations_; ++iter) {//先计算初始状态的FK获取初始状态的位置与姿态current_pose = fk_.computeFK(joint_values);//计算初始状态的位置与姿态相较于目标点的误差error = computePoseError(current_pose, target_pose);if(verbose) {std::cout << "Iteration "<< iter << ": error norm = "<< error.norm() << " (pos: "<< error.head<3>().norm() << ", orient: "<< error.tail<3>().norm() << ")n";}//检查误差是否小于阈值，分为位置误差阈值与姿态误差阈值if(error.head<3>().norm() < position_tolerance_ && error.tail<3>().norm() < orientation_tolerance_) {success = true;break;}//计算雅各比矩阵（默认使用解析雅各比）Eigen::MatrixXd J = use_analytical_jacobian_ ? computeAnalyticalJacobian(joint_values, current_pose) :computeNumericalJacobian(joint_values);//采用阻尼最小二乘法(Levenberg-Marquardt)//Δθ = Jᵀ(JJᵀ + λ²I)⁻¹e//θ_new = θ + ΔθEigen::MatrixXd Jt = J.transpose();Eigen::MatrixXd JJt = J * Jt;//lambda_: 阻尼系数(默认0.1)，避免奇异位形时数值不稳定JJt.diagonal().array() += lambda_ * lambda_;Eigen::VectorXd delta_theta = Jt * JJt.ldlt().solve(error);//更新for(inti = 0; i < 6; ++i) {//应用调整量到当前关节角度doublenew_value = joint_values[i] + delta_theta(i);//确保更新后的θ在机械臂的物理限制范围内（关节角度限位）joint_values[i] = std::clamp(new_value, joint_limits_[i].first, joint_limits_[i].second);}//将关节角度规范到[-π,π]等标准范围内（避免不必要的多圈旋转）normalizeJointAngles(joint_values);}//如果超过最大迭代次数(100)还未求解出结果，抛出异常if(!success) {throwstd::runtime_error("IK did not converge within maximum iterations");}//计算误差if(final_error != nullptr) {current_pose = fk_.computeFK(joint_values);*final_error = computePoseError(current_pose, target_pose);}returnjoint_values;}

3.3.使用interactive_marker以实现发布机械臂三维空间目标点

1. 1. 安装ROS2依赖包

sudo apt install ros-${ROS_DISTRO}-interactive-markers ros-${ROS_DISTRO}-tf2-ros

1. 2. 启动interactive_marker_utils实现三维空间目标点发布

ros2 launch interactive_marker_utils marker.launch.py

1. 3. 启动RVIZ2观察Marker

1. 4. 拖动Marker，并用ros2 topic echo 观察Marker发布的目标点是否有变化

3.4.在RVIZ中通过interactive_marker验证IK是否正确

1. 1. 启动松灵PIPER的RVIZ仿真demo，因为此时没有joint_state_publisher，所以模型没有正确显示

ros2 launch piper_description display_piper.launch.py

1. 2. 接下来启动IK节点和interactive_marker节点（在同一个launch文件里）,启动成功后可以看到机械臂正常显示

ros2 launch piper_kinematics piper_ik.launch.py

1. 3. 使用interactive_marker控制机械臂进行IK解算

1. 4. 拖动interactive_marker可以看见IK成功解算出每个关节的角度

1. 5. 如果拖动interactive_marker到达无法解算的地方则会抛出异常

4.在真实PIPER机械臂上验证IK

1. 1. 首先启动连接PIPER的CAN通信的脚本

cd piper_ros./find_all_can_port.sh ./can_activate.sh

1. 2. 启动PIPER真机控制节点

ros2 launch piper my_start_single_piper_rviz.launch.py

1. 3. 接下来启动IK节点和interactive_marker节点（在同一个launch文件里），可以看到机械臂运动到HOME点

ros2 launch piper_kinematics piper_ik.launch.py

1. 4. 拖动interactive_marker并观察PIPER机械臂运动情况

结语

最后，不知道大家学‘废’了吗，所以说，逆运动学“解不出来”并不可怕——只要搞懂了原理、用对了方法，哪怕是机械臂的“死胡同”位置，我们也能知道它为什么“够不着”。从理论到代码，再到真机运行，每一步都是让机器人真正“听话”的关键。现在，你的PIPER也能做到指哪打哪了！

往期内容

客户开源：玩转PiPER|PiPER移植lerobot运动控制优化

第五弹：玩转PiPER|挥手，用手势“隔空操控”机械臂

第四弹：玩转PiPER| 用一部手机就能做到！不用遥控器也能控机械臂

第三弹：玩转PiPER| 用颜色说话，机械臂如何“看懂”方块与曲线并自动执行？

第二弹：玩转PiPER|机械臂连续轨迹录制与播放功能实现

第一弹：玩转PiPER | 机械臂固定点位录制与播放功能实现

松灵机器人成立于2016年，是全球领先的机器人底盘制造商和移动机器人系统解决方案服务商。目前，松灵机器人已经拥有多款适用于不同地形的室内外移动机器人底盘，在载重、续航、速度、运动模式等不同需求场景下实现全矩阵覆盖。同时，松灵机器人还推出了自动驾驶解决方案，平行驾驶解决方案，机器人科研教育套件等移动机器人底盘配套产品，帮助客户在自动驾驶、机械控制、计算机、车辆等领域完成实验验证。

凭借领先的研发技术，松灵机器人已经与包括阿里巴巴、华为、本田、中建三局在内的30多家行业领军企业，以及中科院、清华大学、南方科技大学、北京理工大学、新加坡国立大学、纽约大学等国内外50多所顶尖学府开展了深度合作。