MoveIt2 逆向運動學與路徑規劃

問題定義

上一節送出的規劃請求只指定了 group_name，沒有告訴系統「要去哪裡」。這一節要處理兩件事：怎麼具體指定一個規劃目標，以及規劃器實際上是怎麼在關節空間裡找出一條可行路徑的。

核心概念說明

兩種指定目標的方式：關節空間 vs 笛卡爾空間

規劃目標可以用兩種方式描述：

• 關節空間目標（joint space goal）：直接指定每個關節該轉到多少角度，例如 shoulder_joint = 0.5, elbow_joint = -0.3。這是最直接的描述方式，因為規劃器最終要輸出的本來就是關節角度序列，用關節空間目標不需要額外的求解步驟。
• 笛卡爾空間目標（Cartesian / pose goal）：指定末端執行器（通常是夾爪或工具末端）在三維空間裡的位置與朝向，例如「移動到座標 (0.4, 0.1, 0.3)，夾爪朝下」。這是更符合實際任務描述習慣的方式（你在乎的是夾爪要碰到哪裡，不是每個關節個別轉多少），但需要額外一個步驟：**逆向運動學（Inverse Kinematics, IK）**求解——把末端姿態，反推回一組能達成這個姿態的關節角度。

逆向運動學：從末端姿態反推關節角度

第 4 章的正向運動學方向是「已知每個關節角度，算出末端執行器在哪裡」，這個方向的計算是唯一且直接的。逆向運動學是相反方向——已知末端執行器要到哪裡，反推關節角度該是多少——這個方向通常不是唯一解：同一個末端姿態，可能有好幾組不同的關節角度組合都能達成（想像手肘可以彎向不同方向，末端位置卻可以維持一樣），也可能完全無解（目標超出手臂實際可達範圍，對應上一節提到的 NO_IK_SOLUTION 錯誤）。MoveIt2 底層透過可替換的 IK 求解器插件（常見的有 KDL 提供的數值迭代解法，或針對特定機構的解析解法）處理這個反推過程，運作原理牽涉到第 4 章提過的雅可比矩陣，用來描述關節角度的微小變化如何映射到末端姿態的微小變化，數值迭代法本質上就是不斷用雅可比矩陣做局部線性近似，逐步逼近目標姿態。

OMPL：取樣式路徑規劃的基本邏輯

有了明確的目標關節角度之後，規劃器要在高維度關節空間裡，找出一條從目前姿態到目標姿態、且全程不發生碰撞的路徑。MoveIt2 預設整合的 OMPL 提供多種取樣式演算法（例如常見的 RRT，Rapidly-exploring Random Tree），核心邏輯是：從起點開始，不斷在關節空間裡隨機取樣新的候選姿態，檢查候選姿態本身是否無碰撞、以及從樹上最近的既有節點連過去的路徑是否無碰撞，逐步長出一棵覆蓋關節空間的樹，直到某個分支足夠接近目標為止。這種取樣式方法的特性是——不保證找到最優路徑，但在高維度空間裡通常能比窮舉搜尋更快找到「一條可行」路徑，這也是為什麼上面 FAQ 提到每次規劃結果可能略有不同、且路徑不一定是最短路線的根本原因。

實作範例：分別用兩種目標方式送出規劃請求

1. 關節空間目標

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import Constraints, JointConstraint


class JointGoalClient(Node):
    def __init__(self):
        super().__init__("joint_goal_client")
        self.client = ActionClient(self, MoveGroup, "move_action")

    def send_goal(self):
        self.client.wait_for_server()
        goal_msg = MoveGroup.Goal()
        goal_msg.request.group_name = "left_arm"
        goal_msg.request.allowed_planning_time = 5.0

        constraints = Constraints()
        for name, target in [("left_shoulder_joint", 0.5), ("left_elbow_joint", -0.8)]:
            jc = JointConstraint()
            jc.joint_name = name
            jc.position = target
            jc.tolerance_above = 0.01
            jc.tolerance_below = 0.01
            jc.weight = 1.0
            constraints.joint_constraints.append(jc)
        goal_msg.request.goal_constraints.append(constraints)

        return self.client.send_goal_async(goal_msg)

tolerance_above／tolerance_below 定義了「多接近算達成目標」的容忍範圍，設太小可能導致規劃器在數值精度限制下永遠無法精確命中，設太大則規劃結果的精準度會下降，這是實務上需要依任務精度需求權衡的參數。

2. 笛卡爾空間目標（透過 moveit_commander 風格的 Python 介面，實務上更常用）

from moveit.planning import MoveItPy
from geometry_msgs.msg import PoseStamped


def move_to_pose():
    moveit = MoveItPy(node_name="moveit_py_client")
    arm = moveit.get_planning_component("left_arm")

    target_pose = PoseStamped()
    target_pose.header.frame_id = "base_link"
    target_pose.pose.position.x = 0.35
    target_pose.pose.position.y = 0.1
    target_pose.pose.position.z = 0.25
    target_pose.pose.orientation.w = 1.0

    arm.set_start_state_to_current_state()
    arm.set_goal_state(pose_stamped_msg=target_pose, pose_link="left_gripper_link")

    plan_result = arm.plan()
    if plan_result:
        moveit.execute(plan_result.trajectory, controllers=[])

預期輸出

規劃成功時，move_group 終端機會印出：

[INFO] [moveit_move_group_capabilities_base.move_action_capability]: Solution found in 0.842 seconds
[INFO] [move_group]: Planning succeeded

規劃時間（此例 0.842 秒）是取樣式演算法特性的直接體現——場景越複雜、目標離起點越遠，取樣搜尋需要的時間通常越長，且每次執行的時間也可能有波動。

常見錯誤與除錯技巧

錯誤一：規劃耗時異常地長，甚至超過 allowed_planning_time 直接失敗

[ERROR] [move_group]: Timed out

原因：常見於場景障礙物過於複雜、或目標姿態附近的可行關節組合空間非常狹窄（例如需要手臂穿過一個窄縫才能到達目標），讓取樣式演算法需要嘗試非常多次取樣才能找到可行路徑。

排除方式：先確認 allowed_planning_time 是否設定了合理的上限（過短的時間限制在複雜場景下本來就容易失敗），如果確認場景本身合理但持續超時，檢查 Planning Scene 裡的障礙物設定是否過度保守（例如障礙物的碰撞幾何比實際物體大了不必要的安全邊界），必要時簡化場景或調整 OMPL 規劃器參數（例如換用針對窄通道問題優化的演算法變體）。

錯誤二：關節空間目標的 tolerance 設太嚴格，規劃persistently 失敗

原因：數值迭代或取樣式方法在浮點數精度下，很難「精確」命中某個關節角度的絕對值，如果 tolerance_above／tolerance_below 設得極小（例如 0.0001），演算法可能反覆嘗試但始終無法在容許誤差內收斂。

排除方式：給予合理但不過度嚴苛的容忍範圍（本文範例的 0.01 弳度，約 0.57 度，對多數應用已經足夠精確），如果任務真的需要極高精度定位，考慮改用專門的精細定位控制邏輯，而不是單純縮小規劃階段的容忍度。

小結

規劃目標可以用關節空間（直接、不需要 IK）或笛卡爾空間（更符合任務直覺、需要 IK 求解）兩種方式描述，逆向運動學求解本身可能有多解或無解，OMPL 的取樣式演算法特性決定了規劃結果的隨機性與非最優性。有了能規劃到目標姿態的能力後，下一節要進一步處理實際抓取任務——怎麼控制夾爪開合、以及規劃「抓取」這個複合動作時需要額外考慮的碰撞與姿態序列問題。

延伸閱讀

• MoveIt2 總覽：機械手臂的運動規劃框架
• MoveIt2 抓取與夾爪控制