ROS 2从入门到精通系列（十七）：机械臂控制 - 正逆运动学与轨迹规划

从运动学理论到实际控制，完整的机械臂控制系统实现。

---

引言

机械臂（robotic arm）是工业和服务机器人的重要组成部分。控制机械臂需要：

• 正运动学：已知关节角度，求末端位置
• 逆运动学：已知目标位置，求关节角度
• 轨迹规划：生成平滑的运动轨迹
• 控制执行：驱动关节到目标位置

---

一、机械臂建模和几何关系

1.1 D-H参数表示

1.2 正运动学方程

关节变换矩阵：

$$T_i^{i-1}=\left[\begin{array}{cccc}\cos {\theta }_i& -\sin {\theta }_i\cos {\alpha }_i& \sin {\theta }_i\sin {\alpha }_i& a_i\cos {\theta }_i\\ \sin {\theta }_i& \cos {\theta }_i\cos {\alpha }_i& -\cos {\theta }_i\sin {\alpha }_i& a_i\sin {\theta }_i\\ 0& \sin {\alpha }_i& \cos {\alpha }_i& d_i\\ 0& 0& 0& 1\end{array}\right]$$

末端变换：

$$T_n^0=T_1^0\cdot T_2^1\cdot ...\cdot T_n^{n-1}$$

1.3 2自由度平面臂的逆运动学

已知目标位置(x, y)，求θ1, θ2：

$${\theta }_2=\arccos \left(\frac{x^2+y^2-L_1^2-L_2^2}{2L_1{L}_2}\right)$$
$${\theta }_1=\arctan 2(y,x)-\arctan 2(L_2\sin {\theta }_2,L_1+L_2\cos {\theta }_2)$$

---

二、使用MoveIt进行运动规划

2.1 安装MoveIt

# 安装MoveIt2
sudo apt install ros-humble-moveit

# 安装运动规划库
sudo apt install ros-humble-moveit-planners

2.2 URDF机械臂描述

创建 robot.urdf：

<?xml version="1.0"?>
<robot name="simple_arm">
  <!-- 基座 -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 1 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>

  <!-- 第一个关节和连杆 -->
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.05" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="0" upper="3.14" effort="10" velocity="1"/>
  </joint>

  <link name="link1">
    <visual>
      <geometry>
        <cylinder length="0.3" radius="0.02"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
      <origin xyz="0.15 0 0" rpy="0 1.57 0"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.3" radius="0.02"/>
      </geometry>
      <origin xyz="0.15 0 0" rpy="0 1.57 0"/>
    </collision>
    <inertial>
      <mass value="1"/>
      <origin xyz="0.15 0 0"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>

  <!-- 第二个关节和连杆 -->
  <joint name="joint2" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0.3 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="0" upper="3.14" effort="10" velocity="1"/>
  </joint>

  <link name="link2">
    <visual>
      <geometry>
        <cylinder length="0.3" radius="0.02"/>
      </geometry>
      <material name="red">
        <color rgba="1 0 0 1"/>
      </material>
      <origin xyz="0.15 0 0" rpy="0 1.57 0"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.3" radius="0.02"/>
      </geometry>
      <origin xyz="0.15 0 0" rpy="0 1.57 0"/>
    </collision>
    <inertial>
      <mass value="1"/>
      <origin xyz="0.15 0 0"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>

  <!-- 末端执行器 -->
  <joint name="ee_joint" type="fixed">
    <parent link="link2"/>
    <child link="ee_link"/>
    <origin xyz="0.3 0 0" rpy="0 0 0"/>
  </joint>

  <link name="ee_link"/>
</robot>

---

三、正运动学计算

3.1 实现正运动学

#!/usr/bin/env python3
"""
机械臂正运动学计算
"""

import numpy as np
import math

class ArmKinematics:
    """机械臂运动学求解器"""

    def __init__(self, link_lengths):
        """
        初始化
        link_lengths: 各连杆长度列表
        """
        self.link_lengths = link_lengths
        self.n_joints = len(link_lengths)

    def forward_kinematics_2dof(self, theta1, theta2):
        """
        2自由度平面臂的正运动学

        theta1: 第一关节角（弧度）
        theta2: 第二关节角（弧度）

        返回: (x, y) 末端位置
        """
        L1, L2 = self.link_lengths

        # 末端位置
        x = L1 * math.cos(theta1) + L2 * math.cos(theta1 + theta2)
        y = L1 * math.sin(theta1) + L2 * math.sin(theta1 + theta2)

        # 末端角度
        phi = theta1 + theta2

        return x, y, phi

    def forward_kinematics_3dof(self, theta1, theta2, theta3):
        """3自由度臂的正运动学"""
        L1, L2, L3 = self.link_lengths

        # 在xy平面上的投影
        xy_dist = L1 * math.cos(theta1) + \
                  L2 * math.cos(theta1 + theta2) + \
                  L3 * math.cos(theta1 + theta2 + theta3)

        x = xy_dist
        y = L1 * math.sin(theta1) + \
            L2 * math.sin(theta1 + theta2) + \
            L3 * math.sin(theta1 + theta2 + theta3)

        return x, y

    def jacobian_2dof(self, theta1, theta2):
        """
        2自由度臂的雅可比矩阵
        用于速度变换和逆运动学
        """
        L1, L2 = self.link_lengths

        J = np.array([
            [-L1 * math.sin(theta1) - L2 * math.sin(theta1 + theta2),
             -L2 * math.sin(theta1 + theta2)],
            [L1 * math.cos(theta1) + L2 * math.cos(theta1 + theta2),
             L2 * math.cos(theta1 + theta2)]
        ])

        return J

    def velocity_kinematics(self, theta_dot, jacobian):
        """
        速度运动学
        dX = J * dθ
        """
        x_dot = np.dot(jacobian, theta_dot)
        return x_dot


# 使用示例
if __name__ == '__main__':
    # 创建2自由度臂
    arm = ArmKinematics([0.3, 0.3])

    # 计算末端位置
    theta1, theta2 = math.pi / 4, math.pi / 4
    x, y, phi = arm.forward_kinematics_2dof(theta1, theta2)

    print(f'关节角: θ1={theta1:.2f}rad, θ2={theta2:.2f}rad')
    print(f'末端位置: x={x:.3f}m, y={y:.3f}m, φ={phi:.2f}rad')

    # 计算雅可比矩阵
    J = arm.jacobian_2dof(theta1, theta2)
    print(f'雅可比矩阵:\n{J}')

---

四、逆运动学求解

4.1 解析逆运动学（2自由度）

class ArmKinematics:
    """...(前面的代码)..."""

    def inverse_kinematics_2dof(self, x, y, elbow_up=True):
        """
        2自由度臂的解析逆运动学

        x, y: 目标末端位置
        elbow_up: 肘部朝向（True=上, False=下）

        返回: (theta1, theta2)
        """
        L1, L2 = self.link_lengths

        # 计算目标距离
        d = math.sqrt(x**2 + y**2)

        # 检查可达性
        if d > L1 + L2 or d < abs(L1 - L2):
            return None  # 目标不可达

        # 使用余弦定理计算theta2
        cos_theta2 = (d**2 - L1**2 - L2**2) / (2 * L1 * L2)
        cos_theta2 = np.clip(cos_theta2, -1, 1)  # 防止数值误差

        if elbow_up:
            theta2 = math.acos(cos_theta2)
        else:
            theta2 = -math.acos(cos_theta2)

        # 计算theta1
        alpha = math.atan2(y, x)
        beta = math.atan2(L2 * math.sin(theta2),
                         L1 + L2 * math.cos(theta2))

        theta1 = alpha - beta

        return theta1, theta2

    def inverse_kinematics_numerical(self, target_x, target_y,
                                     initial_guess=None,
                                     max_iterations=100,
                                     tolerance=1e-4):
        """
        数值逆运动学（牛顿-拉夫逊法）

        适用于复杂的多自由度机械臂
        """
        if initial_guess is None:
            theta = np.zeros(self.n_joints)
        else:
            theta = np.array(initial_guess)

        target = np.array([target_x, target_y])

        for i in range(max_iterations):
            # 当前正运动学结果
            x, y = self.forward_kinematics_2dof(theta[0], theta[1])[:2]
            current = np.array([x, y])

            # 误差
            error = target - current
            error_norm = np.linalg.norm(error)

            if error_norm < tolerance:
                return theta

            # 计算雅可比矩阵
            J = self.jacobian_2dof(theta[0], theta[1])

            # 伪逆
            J_pinv = np.linalg.pinv(J)

            # 更新关节角
            delta_theta = np.dot(J_pinv, error)
            theta = theta + delta_theta * 0.5

        if error_norm < tolerance:
            return theta
        else:
            return None  # 未能收敛

---

五、轨迹规划

5.1 使用MoveIt进行轨迹规划

#!/usr/bin/env python3
"""
使用MoveIt进行轨迹规划和控制
"""

import rclpy
from rclpy.node import Node
from moveit_msgs.action import MoveGroup
from moveit_msgs.srv import GetPositionIK
from geometry_msgs.msg import PoseStamped
from rclpy.action import ActionClient
import math

class ArmController(Node):
    def __init__(self):
        super().__init__('arm_controller')

        # 创建MoveGroup动作客户端
        self.move_group_client = ActionClient(self, MoveGroup, '/move_action_server')

        # 创建IK服务客户端
        self.ik_client = self.create_client(
            GetPositionIK, '/compute_ik')

        self.get_logger().info('机械臂控制器已启动')

    def move_to_pose(self, x, y, z):
        """
        移动机械臂到目标位置
        """
        # 创建目标位姿
        target_pose = PoseStamped()
        target_pose.header.frame_id = 'base_link'
        target_pose.pose.position.x = x
        target_pose.pose.position.y = y
        target_pose.pose.position.z = z
        target_pose.pose.orientation.w = 1.0

        # 使用逆运动学求解
        joint_values = self.solve_ik(target_pose)

        if joint_values is None:
            self.get_logger().error('逆运动学无解')
            return False

        # 规划轨迹
        return self.plan_and_execute(joint_values)

    def solve_ik(self, target_pose):
        """求逆运动学"""
        try:
            # 创建IK请求
            request = GetPositionIK.Request()
            request.ik_request.group_name = 'manipulator'
            request.ik_request.pose_stamped = target_pose
            request.ik_request.timeout = rclpy.duration.Duration(seconds=5)

            # 发送请求
            future = self.ik_client.call_async(request)
            rclpy.spin_until_future_complete(self, future, timeout_sec=5)

            if future.result() is not None:
                return future.result().solution.joint_state.position
            else:
                return None
        except Exception as e:
            self.get_logger().error(f'IK求解失败: {str(e)}')
            return None

    def plan_and_execute(self, joint_values):
        """规划并执行轨迹"""
        # 创建目标
        goal = MoveGroup.Goal()
        goal.request.group_name = 'manipulator'
        goal.request.goal_constraints[0].joint_constraints[0].joint_name = 'joint1'
        goal.request.goal_constraints[0].joint_constraints[0].position = joint_values[0]

        # 发送目标
        send_goal_future = self.move_group_client.send_goal_async(goal)
        rclpy.spin_until_future_complete(self, send_goal_future)

        goal_handle = send_goal_future.result()
        if not goal_handle.accepted:
            self.get_logger().error('目标被拒绝')
            return False

        # 等待执行完成
        result_future = goal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, result_future)

        result = result_future.result().result
        return result.error_code.val == 1  # MoveItErrorCodes::SUCCESS = 1

    def plan_cartesian_path(self, waypoints):
        """
        笛卡尔路径规划（直线轨迹）
        """
        # 创建动作请求
        goal = MoveGroup.Goal()
        goal.request.group_name = 'manipulator'
        goal.request.workspace_parameters.header.frame_id = 'base_link'
        goal.request.workspace_parameters.min_corner.x = -1.0
        goal.request.workspace_parameters.max_corner.x = 1.0

        # 发送目标
        send_goal_future = self.move_group_client.send_goal_async(goal)
        rclpy.spin_until_future_complete(self, send_goal_future)

        return send_goal_future.result()

def main(args=None):
    rclpy.init(args=args)
    node = ArmController()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

六、关节控制和力反馈

6.1 直接关节控制

#!/usr/bin/env python3
"""
直接关节控制 - 关节级命令
"""

import rclpy
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
import math

class JointController(Node):
    def __init__(self):
        super().__init__('joint_controller')

        # 发布关节轨迹
        self.traj_pub = self.create_publisher(
            JointTrajectory, '/joint_trajectory_controller/command', 10)

    def move_joint_trajectory(self, joint_names, positions, duration):
        """
        执行关节轨迹

        joint_names: 关节名称列表
        positions: 目标位置列表
        duration: 执行时间（秒）
        """
        trajectory = JointTrajectory()
        trajectory.joint_names = joint_names

        # 创建轨迹点
        point = JointTrajectoryPoint()
        point.positions = positions
        point.time_from_start.sec = int(duration)
        point.time_from_start.nanosec = int((duration % 1) * 1e9)

        trajectory.points.append(point)

        # 发布轨迹
        self.traj_pub.publish(trajectory)

        self.get_logger().info(f'执行轨迹: {duration}秒')

    def interpolate_trajectory(self, start_pos, end_pos, num_points, duration):
        """
        生成插值轨迹
        """
        trajectory = JointTrajectory()
        trajectory.joint_names = ['joint1', 'joint2']

        for i in range(num_points):
            # 线性插值
            t = i / (num_points - 1)
            positions = [
                start_pos[0] + t * (end_pos[0] - start_pos[0]),
                start_pos[1] + t * (end_pos[1] - start_pos[1])
            ]

            point = JointTrajectoryPoint()
            point.positions = positions
            point.time_from_start.sec = int(t * duration)
            point.time_from_start.nanosec = int(
                ((t * duration) % 1) * 1e9)

            trajectory.points.append(point)

        self.traj_pub.publish(trajectory)

def main(args=None):
    rclpy.init(args=args)
    node = JointController()

    # 示例：移动关节
    start = [0.0, 0.0]
    end = [1.57, 1.57]  # π/2 rad
    node.interpolate_trajectory(start, end, 10, 5.0)

    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

七、碰撞检测和避障

class ArmPlanner:
    """包含碰撞检测的规划器"""

    def __init__(self, robot_model):
        self.robot_model = robot_model
        self.collision_checker = CollisionChecker(robot_model)

    def plan_with_collision_avoidance(self, start, goal):
        """
        带碰撞检测的路径规划
        """
        # RRT*算法
        path = self.rrt_star(start, goal)

        # 检查碰撞
        if self.collision_checker.is_path_valid(path):
            return path
        else:
            # 尝试重新规划
            return self.plan_with_collision_avoidance(start, goal)

    def rrt_star(self, start, goal, max_iterations=1000):
        """RRT*运动规划算法"""
        # 初始化树
        nodes = [start]
        edges = []

        for _ in range(max_iterations):
            # 随机采样
            random_node = self.random_sample()

            # 找最近邻点
            nearest_idx = self.nearest_neighbor(nodes, random_node)
            nearest_node = nodes[nearest_idx]

            # 向目标扩展
            new_node = self.steer(nearest_node, random_node)

            # 检查碰撞
            if self.collision_checker.is_edge_valid(nearest_node, new_node):
                nodes.append(new_node)
                edges.append((nearest_idx, len(nodes) - 1))

                # 检查是否到达目标
                if self.distance(new_node, goal) < 0.1:
                    return self.extract_path(edges, len(nodes) - 1)

        return None

---

八、本项目要点总结

✅ 正运动学：

• D-H参数表示
• 变换矩阵
• 末端位置计算

✅ 逆运动学：

• 解析求解（2-DOF）
• 数值求解（牛顿-拉夫逊）
• 可达性检查

✅ 轨迹规划：

• MoveIt框架
• 关节空间规划
• 笛卡尔空间规划

✅ 控制执行：

• 关节轨迹控制
• 插值运动
• 力/力矩反馈

✅ 安全性：

• 碰撞检测
• 避障规划
• 速度限制

---

下一篇预告：《ROS2从入门到精通系列（十八）：视觉识别系统 - 目标检测与跟踪》

料 机械臂是精密控制的典范。掌握这套系统，你就掌握了工业机器人的核心！