从机械到智能：软件是大脑

机械是骨架，电子是神经，软件才是大脑。再好的舵机和传感器，没有软件控制就是一堆废铁。本章从最基础的坐标变换讲起，一直到ROS机器人操作系统，帮你建立完整的机器人软件知识体系。

软件控制的核心思想只有一条：感知—决策—执行的闭环。传感器告诉机器人"我在哪"，控制器告诉它"该往哪走"，执行器让机器人真正动起来。

坐标系与空间变换

机器人控制的第一步，是搞清楚"在哪"和"往哪去"。这就需要建立坐标系。

常用坐标系

• 世界坐标系：固定不动的参考系，机器人的绝对位置用它描述
• 基座坐标系：固定在机器人底座上，随机器人移动
• 工具坐标系：固定在末端执行器上，描述工具的姿态
• 目标坐标系：描述任务目标的位置（如抓取点）

坐标系的右手定则

机器人领域统一使用右手坐标系：

        Z (拇指向上)
        ↑
        |
        •————→ X (食指指向右)
       /
  Y (中指向外)

  握拳 + 伸出拇指 = Z轴正方向

齐次变换矩阵

刚体在三维空间的状态由位置和姿态共同描述。用4×4的齐次变换矩阵可以同时表达两者：

  T = | R  p |    =  | r11 r12 r13 px |
      | 0  1 |       | r21 r22 r23 py |
                       | r31 r32 r33 pz |
                       |  0   0   0  1 |

  R = 旋转矩阵(3×3)    描述姿态
  p = 位置向量(3×1)    描述位置

齐次变换的乘法可以实现坐标系的链式变换。例如：

// 末端执行器在世界坐标系中的位姿
// T_world_end = T_world_base × T_base_shoulder × T_shoulder_elbow × T_elbow_end

// 在Arduino中表示（简化3×3旋转矩阵）
float R[3][3];  // 旋转矩阵
float p[3];     // 位置

// 绕Z轴旋转θ角
void rotZ(float theta, float R[3][3]) {
    float c = cos(theta);
    float s = sin(theta);
    R[0][0] = c;  R[0][1] = -s; R[0][2] = 0;
    R[1][0] = s;  R[1][1] = c;  R[1][2] = 0;
    R[2][0] = 0;  R[2][1] = 0;  R[2][2] = 1;
}

坐标变换示例：机械臂正运动学

// 3自由度平面机械臂的正运动学
// 给定各关节角度，计算末端位置

struct Joint { float theta; float L; };

float forwardKinematics(Joint joints[3], float* x, float* y) {
    float cumulative_theta = 0;
    *x = 0; *y = 0;
    
    for (int i = 0; i < 3; i++) {
        cumulative_theta += joints[i].theta;  // 累计角度
        *x += joints[i].L * cos(cumulative_theta);  // 沿X方向投影
        *y += joints[i].L * sin(cumulative_theta);  // 沿Y方向投影
    }
    return atan2(*y, *x);  // 返回末端姿态角
}

// 使用示例
Joint arm[3] = {
    {PI/4, 80},   // 第一关节：45度，长度80mm
    {PI/3, 70},   // 第二关节：60度，长度70mm
    {PI/6, 50}    // 第三关节：30度，长度50mm
};
float x, y;
float end_angle = forwardKinematics(arm, &x, &y);
Serial.print("末端位置: (");
Serial.print(x, 1); Serial.print("mm, ");
Serial.print(y, 1); Serial.print("mm), 姿态角: ");
Serial.print(end_angle * 180 / PI, 1); Serial.println("度");

逆运动学

正运动学：已知关节角度 → 求末端位置（"我在哪"）

逆运动学：已知末端目标位置 → 求关节角度（"我该怎么动"）

逆运动学是机器人控制的核心——我们告诉机器人"去那里"，它自己算出各个关节该转多少。

解析法：2自由度平面臂

// 逆运动学：给定目标(x,y)，求关节角度
// 适用于2自由度平面机械臂

void inverseKinematics2R(float x, float y, float L1, float L2,
                          float* theta1, float* theta2) {
    float d = sqrt(x*x + y*y);  // 到目标的距离
    
    // 判断是否在可达范围内
    if (d > L1 + L2) {
        Serial.println("目标超出工作空间！");
        d = L1 + L2 - 0.1;  // 限制到最近可达点
    }
    if (d < abs(L1 - L2)) {
        Serial.println("目标太近！");
        d = abs(L1 - L2) + 0.1;
    }
    
    // 肘部角度（余弦定理）
    float cos_theta2 = (L1*L1 + L2*L2 - d*d) / (2*L1*L2);
    cos_theta2 = constrain(cos_theta2, -1.0, 1.0);
    *theta2 = acos(cos_theta2);  // 肘关节外角
    
    // 肩部角度
    float k1 = L1 + L2 * cos(*theta2);
    float k2 = L2 * sin(*theta2);
    float phi = atan2(y, x);           // 总角度
    float alpha = atan2(k2, k1);       // 修正角
    *theta1 = phi - alpha;
    
    // 转换为度数（舵机常用0-180度）
    *theta1 = degrees(*theta1);
    *theta2 = degrees(*theta2);
}

// 调用示例：目标(100, 80)
float t1, t2;
inverseKinematics2R(100, 80, 100, 80, &t1, &t2);
Serial.print("肩关节: "); Serial.print(t1, 1);
Serial.print("度, 肘关节: "); Serial.print(t2, 1); Serial.println("度");

雅可比矩阵：速度与灵巧度

雅可比矩阵描述关节速度与末端速度的线性映射关系：

// 雅可比矩阵：vx = J(q) × q_dot
// 用于速度控制和奇异性检测

void computeJacobian2R(float theta1, float theta2, float L1, float L2, float J[2][2]) {
    // 雅可比矩阵（2R平面臂）
    J[0][0] = -L1*sin(theta1) - L2*sin(theta1+theta2);
    J[0][1] = -L2*sin(theta1+theta2);
    J[1][0] =  L1*cos(theta1) + L2*cos(theta1+theta2);
    J[1][1] =  L2*cos(theta1+theta2);
}

// 检测奇异性（关节接近伸展或折叠）
float determinant(float J[2][2]) {
    return J[0][0]*J[1][1] - J[0][1]*J[1][0];
}

void checkSingularity(float theta1, float theta2) {
    float J[2][2];
    computeJacobian2R(theta1, theta2, 100, 80, J);
    float det = determinant(J);
    
    if (abs(det) < 10) {  // 阈值
        Serial.println("警告：接近奇异构型，关节运动受限！");
    }
}

PID控制器：让机器人听话

历史连接：您五十年前学的比例控制，是PID的"简化版"。PID = 比例(P) + 积分(I) + 微分(D)，是工业控制中应用最广泛的算法，从导弹到机器人，无处不在。

PID三兄弟

• P（比例）：当前误差越大，输出越强。像拉弹簧——误差大时拉力大，误差小时拉力小。
• I（积分）：累积过去的误差，消除稳态误差。好比"总账"，不管多小的误差，积累多了也要处理。
• D（微分）：预判误差变化趋势，提前减速。像开车看前方——看到前方有弯道就提前减速。

PID公式

          ┌─────┐
 setpoint │     │   output    ┌───────┐
     ────→│ PID ├───┬───────→ │  plant │──→ measured
          │     │   │          └───────┘
          └─────┘   │              ↑
            ↑       │              │
            │    ┌───┘              │
            │    │    ┌────────┐   │
            └────┘    │error   │───┘
                      └────────┘
                      
output = Kp × e(t) + Ki × ∫e(t)dt + Kd × de(t)/dt
          比例项    积分项       微分项

Arduino位置式PID实现

// 位置式PID控制器（适用于舵机位置控制）
// 特点：输出是绝对位置值，响应直接

class PID {
public:
    float kp, ki, kd;
    float target, current, output;
    float integral = 0;
    float prev_error = 0;
    unsigned long lastTime;

    PID(float p, float i, float d) : kp(p), ki(i), kd(d) {
        lastTime = millis();
    }

    void setTarget(float t) { target = t; }
    float compute(float measured) {
        current = measured;
        unsigned long now = millis();
        float dt = (now - lastTime) / 1000.0;  // 秒
        lastTime = now;

        float error = target - current;

        // 积分项（带积分限幅，防止震荡）
        integral += error * dt;
        integral = constrain(integral, -100, 100);  // 积分限幅

        // 微分项（加低通滤波减少噪声）
        float dedt = (error - prev_error) / dt;
        if (dt > 0) dedt = dedt * 0.3 + prev_error * 0.7;  // 滤波
        prev_error = dedt;

        // PID公式
        output = kp * error + ki * integral + kd * dedt;
        output = constrain(output, -255, 255);  // 输出限幅

        return output;
    }

    void reset() { integral = 0; prev_error = 0; }
};

// 使用示例：控制舵机跟踪目标角度
#include <Servo.h>
Servo myServo;
PID pid(2.0, 0.5, 0.1);  // Kp=2, Ki=0.5, Kd=0.1
const int SERVO_PIN = 9;
const int POT_PIN = A0;

void setup() {
    myServo.attach(SERVO_PIN);
    pid.setTarget(90);  // 目标角度90度
}

void loop() {
    int pot = analogRead(POT_PIN);
    float angle = map(pot, 0, 1023, 0, 180);

    float control = pid.compute(angle);
    int pwm = map(90 + control, 0, 180, 1000, 2000);
    myServo.writeMicroseconds(pwm);
    delay(20);
}

增量式PID实现

// 增量式PID（更适合步进电机和气缸）
// 特点：输出是控制量的变化值，不是绝对值

class IncrementalPID {
public:
    float kp, ki, kd;
    float target, current;
    float prev_error = 0, prev2_error = 0;
    float output = 0;

    IncrementalPID(float p, float i, float d)
        : kp(p), ki(i), kd(d) {}

    float compute(float measured, float tgt) {
        current = measured;
        target = tgt;

        float error = target - current;

        // 增量计算
        float delta_u = kp * (error - prev_error)
                      + ki * error
                      + kd * (error - 2*prev_error + prev2_error);

        output += delta_u;  // 累积输出
        output = constrain(output, 0, 255);

        prev2_error = prev_error;
        prev_error = error;
        return output;
    }
};

PID调参实战

串级PID：双环控制

// 串级PID：外环位置环 + 内环速度环
// 适合需要精确轨迹跟踪的场景

class CascadePID {
public:
    // 外环：位置PID
    PID posPID;
    // 内环：速度PID
    PID velPID;

    float prevVelocity = 0;

    CascadePID(float Kp_pos, float Ki_pos, float Kd_pos,
              float Kp_vel, float Ki_vel, float Kd_vel)
        : posPID(Kp_pos, Ki_pos, Kd_pos),
          velPID(Kp_vel, Ki_vel, Kd_vel) {}

    float compute(float targetPos, float currentPos, float currentVel) {
        // 外环：位置误差 → 目标速度
        posPID.setTarget(targetPos);
        float targetVel = posPID.compute(currentPos);

        // 内环：速度误差 → 控制输出
        return velPID.compute(currentVel, targetVel);
    }
};

步态规划基础

让双足机器人走路，是机器人学中最具挑战性的问题之一。核心挑战是如何在只有两个支撑点的情况下保持平衡。

双足步行的周期

一个完整的步行周期分为两个阶段：

• 摆动相（SWING）：脚抬起，在空中移动，占周期约40%
• 支撑相（STANCE）：脚着地，支撑体重，推动身体前进，占周期约60%

步行周期时序图：

  左腿   ████████████████▌▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒
  右腿   ▒▒▒▒▒▒▒▒▒▒▒███████████████▌▒▒▒▒▒▒

  图例：██ 支撑相  ▒▒ 摆动相

  周期 = 支撑相(60%) + 摆动相(40%)

简化步态生成

// 双足机器人简化步态生成
// 生成周期性步态的髋关节和膝关节角度

#define PI 3.1415926535

// 步态参数
const float STEP_HEIGHT = 25.0;    // 抬脚高度(mm)
const float STEP_LENGTH = 50.0;     // 步长(mm)
const float GAIT_PERIOD = 1.0;      // 周期(秒)
const float LEG_LENGTH = 80.0;       // 腿长(mm)

// 正弦插值（平滑过渡）
float sineEase(float t) {
    return 0.5 - 0.5 * cos(t * PI);
}

// 摆动相轨迹（摆线插值）
float swingPhase(float t, float L) {
    // 摆线：x = L*(t - sin(2πt)/2π)
    return L * (t - sin(2*PI*t) / (2*PI));
}

// 生成左腿角度
void leftLegGait(float phase, float* hip, float* knee) {
    if (phase < 0.5) {
        // 摆动相：抬腿前进
        float swing_t = phase * 2;  // 0~1
        float x = swingPhase(swing_t, STEP_LENGTH);
        float y = STEP_HEIGHT * sin(swing_t * PI);  // 抛物线

        // 简化的逆运动学（只考虑高度）
        *hip = map(y, 0, STEP_HEIGHT, 0, 45);  // 髋关节
        *knee = map(y, 0, STEP_HEIGHT, 0, 60); // 膝关节
    } else {
        // 支撑相：着地
        *hip = 0;
        *knee = 0;
    }
}

// 生成右腿角度（相位相差0.5）
void rightLegGait(float phase, float* hip, float* knee) {
    leftLegGait(fmod(phase + 0.5, 1.0), hip, knee);
}

ZMP：零力矩点

ZMP（Zero Moment Point）是双足步行稳定性分析的核心概念：

定义：地面反作用力的合力矩在水平方向为零的点。
意义：只要ZMP在支撑多边形内，机器人就不会倒。

// 简化ZMP计算
// ZMP在支撑脚组成的凸多边形内 = 稳定

struct Vec2 { float x, y; };

// 检查ZMP是否在支撑多边形内（凸多边形顶点数）
bool isZMPStable(Vec2 zmp, Vec2 supportPoly[], int n) {
    for (int i = 0; i < n; i++) {
        Vec2 v1 = supportPoly[i];
        Vec2 v2 = supportPoly[(i+1) % n];

        // 边的外向法向量
        Vec2 edge = {v2.y - v1.y, -(v2.x - v1.x)};

        // ZMP到边的投影
        Vec2 toZMP = {zmp.x - v1.x, zmp.y - v1.y};
        float dot = edge.x * toZMP.x + edge.y * toZMP.y;

        if (dot < 0) return false;  // ZMP在多边形外
    }
    return true;  // ZMP在多边形内
}

ROS机器人操作系统入门

ROS不是操作系统，而是一套机器人软件"中间件"框架。它提供了节点通信、工具库、仿真环境等，让不同功能的机器人软件可以方便地组合在一起工作。

ROS 2核心概念

• 节点（Node）：独立运行的程序模块，如"雷达节点"、"控制节点"
• 话题（Topic）：单向数据流，如/scan（雷达数据）、/cmd_vel（速度命令）
• 服务（Service）：请求-应答模式，如/spawn（生成地图点）
• 动作（Action）：长时间任务，如/navigation（导航任务）
• 参数（Parameter）：配置数据，如最大速度、传感器参数

ROS 2安装（Ubuntu）

# Ubuntu 22.04 + ROS 2 Humble
sudo apt update
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt install curl
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key | sudo apt-key add -
sudo sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2.list'

sudo apt update
sudo apt install ros-humble-ros-base
sudo apt install ros-humble-ros2-control ros-humble-ros2-controllers
sudo apt install ros-humble-xacro  # 机器人模型工具

# 环境变量（每次开终端都要source）
source /opt/ros/humble/setup.bash

# 创建工作空间
mkdir -p ~/robot_ws/src
cd ~/robot_ws
colcon build
source install/setup.bash

第一个ROS 2节点（Python）

# robot_node.py - 发布关节角度话题

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
import math

class RobotJointPublisher(Node):
    def __init__(self):
        super().__init__('robot_joint_publisher')

        # 发布关节状态话题
        self.joint_pub = self.create_publisher(
            JointState,
            '/joint_states',
            10)

        # 定时器：50Hz
        self.timer = self.create_timer(0.02, self.timer_callback)
        self.t = 0.0

        self.get_logger().info('关节状态发布节点已启动')

    def timer_callback(self):
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.name = ['joint1', 'joint2', 'joint3', 'joint4']
        msg.position = [
            90.0 + 10.0 * math.sin(self.t),     # 关节1
            90.0 + 20.0 * math.sin(self.t*1.3), # 关节2
            90.0 + 15.0 * math.sin(self.t*0.7), # 关节3
            30.0 + 30.0 * math.sin(self.t*1.1)   # 夹爪
        ]
        self.joint_pub.publish(msg)
        self.t += 0.02

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

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

订阅关节命令（Arduino）

// ROS 2串口接收节点（Arduino端）
// 订阅/joint_command话题，通过串口发送给下位机

#include <Arduino.h>
#include <SoftwareSerial.h>

SoftwareSerial rosSerial(10, 11);  // RX, TX

void setup() {
    Serial.begin(115200);      // 调试串口
    rosSerial.begin(115200);   // ROS串口
}

void loop() {
    // 接收ROS命令
    if (rosSerial.available()) {
        String cmd = rosSerial.readStringUntil('\n');
        cmd.trim();

        if (cmd.startsWith("JOINTS:")) {
            // 解析: JOINTS:j1:90.0,j2:45.0,j3:120.0,j4:30.0
            parseJoints(cmd);
        } else if (cmd.startsWith("HOME")) {
            // 回中位
            goHome();
        }
    }
}

void parseJoints(String cmd) {
    // 解析各关节角度
    // 实现舵机控制
}

手机控制ROS机器人（Web界面）

# 手机控制网页服务（Flask + ROS 2）
# 监听手机滑块和按键，通过ROS发送命令

from flask import Flask, render_template
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import threading

app = Flask(__name__)
rclpy.init()
node = Node('web_controller')
pub = node.create_publisher(String, '/robot_command', 10)

@app.route('/')
def index():
    return render_template('control.html')

@app.route('/cmd')
def cmd():
    direction = request.args.get('dir', 'stop')
    msg = String()
    msg.data = direction
    pub.publish(msg)
    return {'status': 'ok', 'cmd': direction}

# 在后台线程运行ROS
def spin_ros():
    rclpy.spin(node)

threading.Thread(target=spin_ros, daemon=True).start()
app.run(host='0.0.0.0', port=5000)  # 手机浏览器访问 http://机器人IP:5000

<!-- control.html 滑块控制界面 -->
<!DOCTYPE html>
<html>
<head>
    <meta name="viewport" content="width=device-width">
    <title>机器人控制</title>
    <style>
        body { background: #1a1a2e; color: #fff; font-family: sans-serif; text-align: center; padding: 20px; }
        .slider-group { margin: 20px auto; max-width: 300px; }
        label { display: block; margin: 10px 0 5px; color: #00d4ff; }
        input[type=range] { width: 100%; height: 10px; accent-color: #7b2cbf; }
        .btn-grid { display: grid; grid-template-columns: repeat(3, 1fr); gap: 10px; max-width: 250px; margin: 20px auto; }
        .btn { background: #7b2cbf; color: #fff; border: none; padding: 15px; border-radius: 8px; font-size: 1.2rem; cursor: pointer; }
        .btn:active { background: #00d4ff; }
        .btn-stop { background: #f44336; }
        .status { margin-top: 20px; color: #888; }
    </style>
</head>
<body>
    <h2>🤖 机器人控制台</h2>

    <div class="slider-group">
        <label>关节1 (基座): <span id="v1">90</span>°</label>
        <input type="range" min="0" max="180" value="90" id="s1" oninput="sendJoints()">
    </div>
    <div class="slider-group">
        <label>关节2 (肩部): <span id="v2">90</span>°</label>
        <input type="range" min="0" max="180" value="90" id="s2" oninput="sendJoints()">
    </div>

    <div class="btn-grid">
        <button class="btn" onclick="cmd('forward')">↑前</button>
        <button class="btn" onclick="cmd('home')">⌂中</button>
        <button class="btn" onclick="cmd('back')">↓后</button>
    </div>

    <script>
        function sendJoints() {
            const v1 = document.getElementById('s1').value;
            const v2 = document.getElementById('s2').value;
            document.getElementById('v1').textContent = v1;
            document.getElementById('v2').textContent = v2;
            fetch(`/cmd?dir=J1:${v1},J2:${v2}`);
        }
        function cmd(c) { fetch(`/cmd?dir=${c}`); }
    </script>
    <p class="status">手机浏览器即可控制，无需安装App</p>
</body>
</html>

本章小结

• ✅ 掌握坐标系和齐次变换，理解机器人"在哪"的问题
• ✅ 学会正运动学和逆运动学，能根据角度算位置，也能根据位置算角度
• ✅ 理解PID控制器的原理和调参方法，融入您五十年前的比例控制经验
• ✅ 了解步态规划基础和ZMP稳定性判据
• ✅ 入门ROS 2，能编写发布/订阅节点，实现手机Web控制

"控制是机器人技术的灵魂，PID是控制技术的基石。"