前言

在前幾期中，我們實(shí)現(xiàn)了PIPER機(jī)械臂的點(diǎn)位控制、軌跡錄制、視覺識(shí)別、陀螺儀控制及手勢(shì)識(shí)別。正向與逆向運(yùn)動(dòng)學(xué)，是連接“空間指令”與“關(guān)節(jié)動(dòng)作”的橋梁。無論是點(diǎn)擊屏幕、揮手控制，還是自動(dòng)抓取，背后都離不開今天實(shí)現(xiàn)的這套FK/IK求解。

更重要的是——所有代碼已開源，歡迎動(dòng)手實(shí)踐！

開源鏈接：https://github.com/agilexrobotics/Agilex-College.git

摘要

本章實(shí)現(xiàn)基于線性代數(shù)庫(kù)Eigen實(shí)現(xiàn)松靈PIPER機(jī)械臂的正解，逆解的雅各比方法，自定義交互式標(biāo)記interactive_marker_utils的實(shí)現(xiàn)

標(biāo)簽

運(yùn)動(dòng)學(xué)正解、運(yùn)動(dòng)學(xué)逆解的雅各比方法、RVIZ仿真、機(jī)械臂DH、交互式標(biāo)記、松靈PIPER

功能演示

已關(guān)注 關(guān)注 重播 分享 贊 關(guān)閉觀看更多更多退出全屏切換到豎屏全屏退出全屏AGV網(wǎng)已關(guān)注分享視頻，時(shí)長(zhǎng)00:48

0/0

00:00/00:48切換到橫屏模式繼續(xù)播放進(jìn)度條，百分之0播放00:00/00:4800:48全屏倍速播放中 0.5倍 0.75倍 1.0倍 1.5倍 2.0倍 超清 流暢

繼續(xù)觀看

小白也能懂：用Eigen搞定機(jī)械臂數(shù)值法逆解

觀看更多轉(zhuǎn)載,小白也能懂：用Eigen搞定機(jī)械臂數(shù)值法逆解AGV網(wǎng)已關(guān)注分享點(diǎn)贊在看已同步到看一看寫下你的評(píng)論

視頻詳情

已關(guān)注 關(guān)注 重播 分享 贊 關(guān)閉觀看更多更多退出全屏切換到豎屏全屏退出全屏AGV網(wǎng)已關(guān)注分享視頻，時(shí)長(zhǎng)00:18

0/0

00:00/00:18切換到橫屏模式繼續(xù)播放進(jìn)度條，百分之0播放00:00/00:1800:18全屏倍速播放中 0.5倍 0.75倍 1.0倍 1.5倍 2.0倍 超清 流暢

繼續(xù)觀看

小白也能懂：用Eigen搞定機(jī)械臂數(shù)值法逆解

觀看更多轉(zhuǎn)載,小白也能懂：用Eigen搞定機(jī)械臂數(shù)值法逆解AGV網(wǎng)已關(guān)注分享點(diǎn)贊在看已同步到看一看寫下你的評(píng)論

視頻詳情

1.使用前準(zhǔn)備

1.1.硬件準(zhǔn)備

• ? AgileX robotics Piper機(jī)械臂

1.2.軟件環(huán)境配置

1. 1. PIPER機(jī)械臂驅(qū)動(dòng)部署請(qǐng)參考：https://github.com/agilexrobotics/piper_sdk/blob/1_0_0_beta/README(ZH).MD

2. 2. PIPER機(jī)械臂ROS控制節(jié)點(diǎn)部署參考：https://github.com/agilexrobotics/piper_ros/blob/noetic/README.MD

3. 3. 安裝Eigen線性代數(shù)庫(kù)

sudo apt install libeigen3-dev

1.3.準(zhǔn)備松靈PIPER的DH參數(shù)表以及關(guān)節(jié)限位

查閱松靈PIPER用戶手冊(cè)可以找到PIPER的改進(jìn)DH參數(shù)表與關(guān)節(jié)限位：

• DH

• 機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)范圍

---

2.正向運(yùn)動(dòng)學(xué)計(jì)算FK

正向運(yùn)動(dòng)學(xué)FK的計(jì)算過程實(shí)際上是從每個(gè)關(guān)節(jié)的角度值----計(jì)算---->>機(jī)械臂某一關(guān)節(jié)在三維世界的位姿，本文以機(jī)械臂最后一個(gè)旋轉(zhuǎn)關(guān)節(jié)joint6為例

2.1.準(zhǔn)備DH參數(shù)

1. 1. 根據(jù)PIPER的DH參數(shù)表構(gòu)建正向運(yùn)動(dòng)學(xué)計(jì)算程序，由1.3小結(jié)的松靈PIPER的改進(jìn)DH參數(shù)表，可以得到

// 改進(jìn)DH參數(shù) [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};

轉(zhuǎn)換為標(biāo)準(zhǔn)DH，可以參考以下的轉(zhuǎn)換規(guī)則：

標(biāo)準(zhǔn)DH到改進(jìn)DH

α???(標(biāo)準(zhǔn)) = α?(改進(jìn))

a???(標(biāo)準(zhǔn)) = a?(改進(jìn))

d?(標(biāo)準(zhǔn)) = d?(改進(jìn))

θ?(標(biāo)準(zhǔn)) = θ?(改進(jìn))

改進(jìn)DH到標(biāo)準(zhǔn)DH

α?(標(biāo)準(zhǔn)) = α???(改進(jìn))

a?(標(biāo)準(zhǔn)) = a???(改進(jìn))

d?(標(biāo)準(zhǔn)) = d?(改進(jìn))

θ?(標(biāo)準(zhǔn)) = θ?(改進(jìn))

得到轉(zhuǎn)換后的標(biāo)準(zhǔn)DH：

// 標(biāo)準(zhǔn)DH參數(shù) [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. 準(zhǔn)備DH變換矩陣

2. ? 改進(jìn)DH變換矩陣：

- 用Eigen改寫為改進(jìn)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;

- 標(biāo)準(zhǔn)DH變換矩陣：

- 用Eigen改寫為標(biāo)準(zhǔn)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. 實(shí)現(xiàn)正向運(yùn)動(dòng)學(xué)運(yùn)算的關(guān)鍵函數(shù)computeFK()，完整代碼見代碼倉(cāng)庫(kù)https://github.com/agilexrobotics/Agilex-College.git

Eigen::Matrix4d computeFK(conststd::vector& joint_values){//檢查輸入關(guān)節(jié)值數(shù)量是否足夠（至少6個(gè)）if(joint_values.size() < 6) {throwstd::runtime_error("Piper arm requires at least 6 joint values for FK");}//初始化單位矩陣作為初始變換Eigen::Matrix4d T = Eigen::Matrix4d::Identity();//對(duì)每個(gè)關(guān)節(jié)：//    計(jì)算實(shí)際關(guān)節(jié)角度 = 輸入值 + 偏移量//    獲取固定參數(shù)d值//    計(jì)算當(dāng)前關(guān)節(jié)的變換矩陣并累積到總變換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 (如果是旋轉(zhuǎn)關(guān)節(jié))T *= computeTransform(dh_params_[i][0],  // alphadh_params_[i][1],  // ad,                 // dtheta              // theta);}//返回最終變換矩陣returnT;}

2.2.驗(yàn)證FK計(jì)算準(zhǔn)確性

1. 1. 啟動(dòng)正向運(yùn)動(dòng)學(xué)驗(yàn)證程序

ros2 launch piper_kinematics test_fk.launch.py

1. 2. 啟動(dòng)RVIZ仿真程序，開啟顯示TF樹，觀察計(jì)算的FK所得的機(jī)械臂末端link6_from_fk的姿態(tài)是否和機(jī)器人原本的link6（由joint_state_publisher計(jì)算）是否重合

ros2 launch  piper_description display_piper_with_joint_state_pub_gui..launch.py

可以看見重合度很高，且從link6_from_fk和link6的姿態(tài)可以看出誤差基本在小數(shù)點(diǎn)后四位。

3.逆向運(yùn)動(dòng)學(xué)解算IK

逆向運(yùn)動(dòng)學(xué)IK的計(jì)算過程實(shí)際上是給定一個(gè)目標(biāo)點(diǎn)----計(jì)算---->>機(jī)械臂每個(gè)關(guān)節(jié)要在什么位置才能使機(jī)械臂末端到達(dá)目標(biāo)點(diǎn)

3.1.確認(rèn)關(guān)節(jié)限位

• ? 需要確定PIPER機(jī)械臂每個(gè)關(guān)節(jié)的限位，確保IK解算出來的路徑不會(huì)超過機(jī)械臂的限位，從而導(dǎo)致機(jī)械臂損壞或者其他危險(xiǎn)情況

• ? 由1.3節(jié)可以得知PIPER機(jī)械臂每個(gè)關(guān)節(jié)的限位為：

• 示意截圖

• ? 從而得到機(jī)械臂關(guān)節(jié)限位的矩陣

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的雅各比矩陣方法實(shí)現(xiàn)的簡(jiǎn)要步驟

求解過程

1. 1. 計(jì)算誤差e：當(dāng)前位姿與目標(biāo)位姿的差異(6維向量：3位置+3姿態(tài))

2. 2. 誤差e是否小于閾值？

3. ? 是→ 返回當(dāng)前θ作為解

4. ? 否→ 進(jìn)入迭代優(yōu)化步驟

5. 3. 計(jì)算雅可比矩陣J：6×6矩陣

6. 4. 計(jì)算阻尼偽逆：

???2?1

λ是阻尼系數(shù)，避免奇異位形時(shí)數(shù)值不穩(wěn)定

1. 5. 計(jì)算關(guān)節(jié)角度增量：

?

通過誤差e和偽逆計(jì)算關(guān)節(jié)角度的調(diào)整量

1. 6. 更新關(guān)節(jié)角度：

應(yīng)用調(diào)整量到當(dāng)前關(guān)節(jié)角度

1. 7. 應(yīng)用關(guān)節(jié)限制：

2. 8. 角度歸一化

3. 9. 達(dá)到最大迭代?

4. ? 否→ 回到步驟2繼續(xù)迭代

5. ? 是→ 拋出未收斂錯(cuò)誤

關(guān)鍵函數(shù)computeIK()

std::vector computeIK(conststd::vector& initial_guess, constEigen::Matrix4d& target_pose,boolverbose = false,Eigen::VectorXd* final_error = nullptr){//初始化一個(gè)猜測(cè)姿態(tài)（初始姿態(tài)）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;//開始迭代計(jì)算for(intiter = 0; iter < max_iterations_; ++iter) {//先計(jì)算初始狀態(tài)的FK獲取初始狀態(tài)的位置與姿態(tài)current_pose = fk_.computeFK(joint_values);//計(jì)算初始狀態(tài)的位置與姿態(tài)相較于目標(biāo)點(diǎn)的誤差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";}//檢查誤差是否小于閾值，分為位置誤差閾值與姿態(tài)誤差閾值if(error.head<3>().norm() < position_tolerance_ && error.tail<3>().norm() < orientation_tolerance_) {success = true;break;}//計(jì)算雅各比矩陣（默認(rèn)使用解析雅各比）Eigen::MatrixXd J = use_analytical_jacobian_ ? computeAnalyticalJacobian(joint_values, current_pose) :computeNumericalJacobian(joint_values);//采用阻尼最小二乘法(Levenberg-Marquardt)//Δθ = J?(JJ? + λ2I)?1e//θ_new = θ + ΔθEigen::MatrixXd Jt = J.transpose();Eigen::MatrixXd JJt = J * Jt;//lambda_: 阻尼系數(shù)(默認(rèn)0.1)，避免奇異位形時(shí)數(shù)值不穩(wěn)定JJt.diagonal().array() += lambda_ * lambda_;Eigen::VectorXd delta_theta = Jt * JJt.ldlt().solve(error);//更新for(inti = 0; i < 6; ++i) {//應(yīng)用調(diào)整量到當(dāng)前關(guān)節(jié)角度doublenew_value = joint_values[i] + delta_theta(i);//確保更新后的θ在機(jī)械臂的物理限制范圍內(nèi)（關(guān)節(jié)角度限位）joint_values[i] = std::clamp(new_value, joint_limits_[i].first, joint_limits_[i].second);}//將關(guān)節(jié)角度規(guī)范到[-π,π]等標(biāo)準(zhǔn)范圍內(nèi)（避免不必要的多圈旋轉(zhuǎn)）normalizeJointAngles(joint_values);}//如果超過最大迭代次數(shù)(100)還未求解出結(jié)果，拋出異常if(!success) {throwstd::runtime_error("IK did not converge within maximum iterations");}//計(jì)算誤差if(final_error != nullptr) {current_pose = fk_.computeFK(joint_values);*final_error = computePoseError(current_pose, target_pose);}returnjoint_values;}

3.3.使用interactive_marker以實(shí)現(xiàn)發(fā)布機(jī)械臂三維空間目標(biāo)點(diǎn)

1. 1. 安裝ROS2依賴包

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

1. 2. 啟動(dòng)interactive_marker_utils實(shí)現(xiàn)三維空間目標(biāo)點(diǎn)發(fā)布

ros2 launch interactive_marker_utils marker.launch.py

1. 3. 啟動(dòng)RVIZ2觀察Marker

1. 4. 拖動(dòng)Marker，并用ros2 topic echo 觀察Marker發(fā)布的目標(biāo)點(diǎn)是否有變化

3.4.在RVIZ中通過interactive_marker驗(yàn)證IK是否正確

1. 1. 啟動(dòng)松靈PIPER的RVIZ仿真demo，因?yàn)榇藭r(shí)沒有joint_state_publisher，所以模型沒有正確顯示

ros2 launch piper_description display_piper.launch.py

1. 2. 接下來啟動(dòng)IK節(jié)點(diǎn)和interactive_marker節(jié)點(diǎn)（在同一個(gè)launch文件里）,啟動(dòng)成功后可以看到機(jī)械臂正常顯示

ros2 launch piper_kinematics piper_ik.launch.py

1. 3. 使用interactive_marker控制機(jī)械臂進(jìn)行IK解算

1. 4. 拖動(dòng)interactive_marker可以看見IK成功解算出每個(gè)關(guān)節(jié)的角度

1. 5. 如果拖動(dòng)interactive_marker到達(dá)無法解算的地方則會(huì)拋出異常

4.在真實(shí)PIPER機(jī)械臂上驗(yàn)證IK

1. 1. 首先啟動(dòng)連接PIPER的CAN通信的腳本

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

1. 2. 啟動(dòng)PIPER真機(jī)控制節(jié)點(diǎn)

ros2 launch piper my_start_single_piper_rviz.launch.py

1. 3. 接下來啟動(dòng)IK節(jié)點(diǎn)和interactive_marker節(jié)點(diǎn)（在同一個(gè)launch文件里），可以看到機(jī)械臂運(yùn)動(dòng)到HOME點(diǎn)

ros2 launch piper_kinematics piper_ik.launch.py

1. 4. 拖動(dòng)interactive_marker并觀察PIPER機(jī)械臂運(yùn)動(dòng)情況

結(jié)語

最后，不知道大家學(xué)‘廢’了嗎，所以說，逆運(yùn)動(dòng)學(xué)“解不出來”并不可怕——只要搞懂了原理、用對(duì)了方法，哪怕是機(jī)械臂的“死胡同”位置，我們也能知道它為什么“夠不著”。從理論到代碼，再到真機(jī)運(yùn)行，每一步都是讓機(jī)器人真正“聽話”的關(guān)鍵?，F(xiàn)在，你的PIPER也能做到指哪打哪了！

往期內(nèi)容

客戶開源：玩轉(zhuǎn)PiPER|PiPER移植lerobot運(yùn)動(dòng)控制優(yōu)化

第五彈：玩轉(zhuǎn)PiPER|揮手，用手勢(shì)“隔空操控”機(jī)械臂

第四彈：玩轉(zhuǎn)PiPER| 用一部手機(jī)就能做到！不用遙控器也能控機(jī)械臂

第三彈：玩轉(zhuǎn)PiPER| 用顏色說話，機(jī)械臂如何“看懂”方塊與曲線并自動(dòng)執(zhí)行？

第二彈：玩轉(zhuǎn)PiPER|機(jī)械臂連續(xù)軌跡錄制與播放功能實(shí)現(xiàn)

第一彈：玩轉(zhuǎn)PiPER | 機(jī)械臂固定點(diǎn)位錄制與播放功能實(shí)現(xiàn)

松靈機(jī)器人成立于2016年，是全球領(lǐng)先的機(jī)器人底盤制造商和移動(dòng)機(jī)器人系統(tǒng)解決方案服務(wù)商。目前，松靈機(jī)器人已經(jīng)擁有多款適用于不同地形的室內(nèi)外移動(dòng)機(jī)器人底盤，在載重、續(xù)航、速度、運(yùn)動(dòng)模式等不同需求場(chǎng)景下實(shí)現(xiàn)全矩陣覆蓋。同時(shí)，松靈機(jī)器人還推出了自動(dòng)駕駛解決方案，平行駕駛解決方案，機(jī)器人科研教育套件等移動(dòng)機(jī)器人底盤配套產(chǎn)品，幫助客戶在自動(dòng)駕駛、機(jī)械控制、計(jì)算機(jī)、車輛等領(lǐng)域完成實(shí)驗(yàn)驗(yàn)證。

憑借領(lǐng)先的研發(fā)技術(shù)，松靈機(jī)器人已經(jīng)與包括阿里巴巴、華為、本田、中建三局在內(nèi)的30多家行業(yè)領(lǐng)軍企業(yè)，以及中科院、清華大學(xué)、南方科技大學(xué)、北京理工大學(xué)、新加坡國(guó)立大學(xué)、紐約大學(xué)等國(guó)內(nèi)外50多所頂尖學(xué)府開展了深度合作。