从零搭建手势控制Stewart平台：Arduino实现并联机器人运动学

1. 项目概述与核心思路

如果你对机器人、机械臂或者飞行模拟器背后的技术感到好奇，那么Stewart平台绝对是一个绕不开的经典结构。它不像常见的串联机械臂那样，一个关节接一个关节，而是用六根可以独立伸缩的“腿”同时支撑一个平台，从而实现极其灵活和稳定的六自由度运动。听起来很酷，但一想到复杂的数学和控制，很多人可能就望而却步了。今天，我想分享一个我自己动手做的项目：用最“接地气”的材料——Arduino、几个舵机、一个加速度计，再加上一些硬纸板或3D打印件，来打造一个属于你自己的、能用手势控制的简易Stewart平台。

这个项目的核心目标很简单：让平台顶板能实时跟随你手部的倾斜角度，保持水平，甚至可以在上面尝试平衡一个小球。它麻雀虽小，五脏俱全，完整地涵盖了从机械结构搭建、电子系统联调，到最核心的运动学算法（逆解）在嵌入式系统上的实现。整个过程，我会把那些在学术论文里看起来高深莫测的矩阵运算，掰开揉碎了，用Arduino能听懂的方式讲出来。你会发现，所谓的“并联机构运动学”，其核心思想就是解决一个几何问题：为了让平台达到某个特定的位置和姿态，我这六条“腿”各自需要伸长或缩短多少？我们的工作，就是教会Arduino去实时计算这个答案。

整个项目非常适合有一定Arduino和编程基础，并希望向机器人控制领域迈出第一步的爱好者。你将亲手触摸到机械设计、传感器融合和实时控制这些机器人技术的核心模块。不用担心数学，我会带你一步步推导；也不用担心材料，我们的目标是“低成本验证原理”。下面，就让我们从理解这个平台的骨架开始。

2. 机械结构设计与搭建要点

一个Stewart平台的机械部分是其所有功能的基础，结构不稳，再精妙的算法也无用武之地。我们的目标是搭建一个稳定、顺滑且易于复现的物理原型。

2.1 平台设计与材料选择

平台的核心是上下两个承载板。根据经典的Stewart平台设计，下平台（基座）通常比上平台（动平台）大，并且两个平台上的铰链点（即舵机摆臂和连杆的连接点）的分布有特定的几何关系。

上平台与下平台的设计：最经典的设计是采用两个等边三角形，但其中一个旋转了60度。这样，六个连接点就均匀地分布在了平台的“有效工作空间”内。在我们的项目中，为了简化，可以直接采用两个大小不同的正六边形或圆形，并在其上对称地打孔作为铰链点。关键尺寸在于上下平台对应铰链点之间的距离（即连杆的初始长度）和平台本身的半径，这直接决定了平台的运动范围和舵机的力矩需求。

注意：铰链点（球头或方向节安装孔）的位置精度至关重要。如果六个点不是精确地两两对称，会导致运动学计算失效，平台运动卡顿甚至损坏舵机。建议使用CAD软件（如Fusion 360）进行设计，并确保导出用于3D打印或激光切割的图纸尺寸精确。

材料选择与加工：

• 低成本方案（推荐入门）：使用5-8毫米厚的航空层板或亚克力板，通过激光切割成型。这种方案精度高，强度足够，且加工快速。
• 3D打印方案（灵活性高）：使用PLA或PETG材料进行3D打印。你可以自由设计复杂的结构，比如集成舵机座、线缆通道等。但要注意，大尺寸平台的打印可能需要支撑，且长时间受力可能存在蠕变问题。
• “极客”快速原型方案：如原文所用，使用硬纸板切割。这非常适合快速验证概念和调整尺寸，但强度低，易变形，仅适用于最轻量级的演示。

舵机选型与安装：舵机是整个系统的“肌肉”。你需要六个完全相同的舵机。对于这个尺寸的平台（建议动平台直径在15-20厘米），推荐使用金属齿轮舵机，扭矩至少在15kg·cm以上，例如MG996R或DS3225。塑料齿轮舵机在反复受力下极易扫齿。

安装时，舵机需要牢固地固定在下平台上。3D打印一个统一的舵机安装座是最佳选择，它能确保所有舵机的输出轴高度一致且垂直于平台平面。如果使用激光切割板，可以设计卡槽结构，并用螺丝从背面紧固。绝对禁止只用热熔胶或双面胶固定，运动中的反复应力会很快导致脱落。

2.2 连杆与运动关节的实现

连杆连接着舵机的摆臂和上平台的铰链点，它将舵机的旋转运动转化为平台的平动和转动。

连杆设计与选择：

• 理想材料：碳纤维杆或铝合金杆。它们重量轻、刚度高，不易弯曲，能确保运动精度。
• 替代方案：如原文提到的，可以使用遥控车用的金属拉杆（转向拉杆）。这种拉杆两端通常有预制的螺纹，可以方便地连接球头关节，且长度可微调，这对后期校准平台水平非常有用。
• 长度计算：连杆的理论长度（两球头中心距）需要根据你的平台尺寸和期望的运动范围来估算。一个简单的起点是：当所有舵机处于90度中立位时，连杆应大致垂直于舵机摆臂和平台平面，形成一个稳定的初始状态。

关节类型——万向节还是球头？这是保证平台运动灵活性的关键。理论上，Stewart平台的上下连接点都需要球铰（球形关节），以实现多自由度转动。

• 专业方案：购买现成的微型球头轴承（如M3规格的杆端轴承）。这是性能最好、最可靠的选择。
• DIY低成本方案：可以使用“万向节”的思路。例如，在连杆两端使用两个互相垂直的螺丝-螺母组合，模拟出两个旋转自由度。或者，使用一小段橡胶管作为柔性连接，但这会引入无法计算的形变，只适用于对精度要求极低的演示。
• 原文的“橡胶筋”方案是一种极具创意的快速验证方法，它利用橡胶的柔性提供了多个自由度，但毫无精度和刚性可言，只能用于理解原理，无法进行精确控制。

一个实用的建议：在舵机的输出摆臂和连杆之间，建议增加一个球头连接。因为舵机摆臂的旋转平面是固定的，而连杆的运动方向是空间变化的，一个刚性连接会很快导致舵机堵转损坏。球头关节可以释放这个约束。

3. 电子系统搭建与传感器集成

机械部分是骨骼，电子系统则是神经和肌肉。我们需要一个大脑（Arduino）来协调所有信息，并驱动六个舵机协同工作。

3.1 主控与电源系统设计

主控选择：Arduino Uno的考量Arduino Uno是绝佳的入门选择，它有14个数字I/O口（其中6个支持PWM，正好驱动6个舵机）和6个模拟输入口，完全满足本项目需求。其简单的编程环境和丰富的库支持，让我们可以专注于算法逻辑而非底层驱动。

至关重要的电源方案这是新手最容易踩坑的地方。切勿尝试仅用Arduino Uno的USB口或板载5V稳压器来同时驱动6个舵机和一个传感器！

• 问题所在：一个舵机在堵转或启动瞬间，电流可能高达1-2A。六个舵机同时动作，峰值电流轻松超过10A。Arduino板载的5V线性稳压器最大输出电流通常只有1A左右，USB端口也只能提供500mA。强行驱动会导致电压骤降，Arduino重启，舵机抖动无力，甚至烧毁USB端口或稳压芯片。
• 正确方案：使用独立电源供电。
  1. 电源选择：选择一个输出为5-6V、持续电流不低于5A的直流稳压电源（比如旧的笔记本电脑电源适配器）。或者使用一块2S或3S锂电池（7.4V或11.1V）配合一个大电流5V BEC（电池消除器电路）。
  2. 接线方法：将外部电源的正极（V+）和负极（GND）分别接到一个独立的面包板电源轨或配电板上。然后，将所有6个舵机的VCC（红色线）和GND（棕色/黑色线）都并联连接到这个外部电源上。关键一步：必须将此外部电源的GND与Arduino的GND用一根导线连接起来，让它们拥有共同的参考地电位，否则信号无法通信。
  3. 信号线连接：每个舵机的信号线（橙色或黄色）分别连接到Arduino的一个PWM引脚（如引脚2, 3, 4, 5, 6, 7）。

3.2 加速度计连接与数据解读

我们使用加速度计来感知手部（或手套）的姿态。MPU-6050是最常见的选择，它集成了三轴加速度计和三轴陀螺仪，性价比极高。

接线与通信：MPU-6050通过I2C总线与Arduino通信，这只需要两根数据线。

• VCC-> Arduino 5V（或外部电源的5V，但需共地）
• GND-> Arduino GND
• SDA-> Arduino Uno的A4引脚（这是固定的I2C数据线）
• SCL-> Arduino Uno的A5引脚（这是固定的I2C时钟线）

焊接建议：如原文所说，直接用杜邦线插在MPU-6050的排针上，在振动中容易接触不良。建议你花几分钟时间，用烙铁将四根导线牢固地焊接在模块的焊盘上，或者使用一个微型面包板进行转接，可靠性会大大提升。

从原始数据到姿态角：MPU-6050输出的原始数据是数字值，我们需要通过计算将其转换为有物理意义的姿态角。

1. 初始化与校准：上电后，需要配置MPU-6050的量程（例如，加速度计±2g）和滤波器参数。更关键的是静态校准：将传感器水平静止放置，读取数百个样本的平均值，这个平均值就是“零偏”，后续的读数需要减去这个零偏来消除误差。
2. 计算倾斜角：对于只需要平衡功能的平台，我们主要关心两个倾斜角：绕X轴的滚转角（Roll）和绕Y轴的俯仰角（Pitch）。在静止或低速运动状态下，可以忽略加速度计测量重力加速度以外的分量，用以下公式近似计算：Roll = atan2(accelY, accelZ) * 180 / PIPitch = atan2(-accelX, sqrt(accelY*accelY + accelZ*accelZ)) * 180 / PI这里accelX, accelY, accelZ是校准后的加速度值。atan2是四象限反正切函数，能给出-180°到180°的完整角度范围。

实操心得：单纯使用加速度计在动态情况下（比如快速移动手套）会产生严重的噪声和错误角度（因为测到的不仅是重力，还有运动加速度）。对于更高级的应用，需要结合陀螺仪的数据进行传感器融合（如互补滤波或卡尔曼滤波），但这会大幅增加代码复杂度。对于我们这个手势平衡 demo，动作缓慢一些，只用加速度计是可行的。

4. 运动学算法核心与代码实现

这是项目的“灵魂”所在。我们需要建立一个数学模型，描述平台姿态与六根连杆长度（对应舵机角度）之间的关系。

4.1 逆向运动学推导

Stewart平台的逆解，就是已知上平台的位置和姿态（共6个变量：X, Y, Z, Roll, Pitch, Yaw），求解六个连杆的当前长度L_i(i=1 to 6)。

1. 建立坐标系：

• 在下平台中心建立基坐标系{B}。
• 在上平台中心建立动坐标系{P}。
• 我们需要知道在{B}系中，每个上平台铰链点P_i和下平台铰链点B_i的坐标。这些坐标在你设计平台时就已经确定了，是固定的已知量。

2. 坐标变换：当上平台发生移动和转动后，上平台铰链点在基坐标系{B}中的新坐标P_i'可以通过旋转和平移变换得到：P_i' = R * P_i + T其中：

• R是一个3x3的旋转矩阵，由滚转(Roll φ)、俯仰(Pitch θ)、偏航(Yaw ψ)三个欧拉角计算得出。对于小角度变化，可以近似使用，但为了通用性，我们通常用以下公式计算：

R = [cosθ*cosψ, sinφ*sinθ*cosψ - cosφ*sinψ, cosφ*sinθ*cosψ + sinφ*sinψ] [cosθ*sinψ, sinφ*sinθ*sinψ + cosφ*cosψ, cosφ*sinθ*sinψ - sinφ*cosψ] [-sinθ, sinφ*cosθ, cosφ*cosθ]

• T = [X, Y, Z]^T是平移向量，表示上平台中心在基坐标系中的位置。在我们的平衡应用中，通常只希望平台绕中心点旋转，所以可以设定X=0, Y=0, Z为一个固定的初始高度。

3. 计算连杆长度：第i根连杆的长度就是向量(P_i' - B_i)的模（长度）：L_i = sqrt( (P_ix' - B_ix)^2 + (P_iy' - B_iy)^2 + (P_iz' - B_iz)^2 )

4. 连杆长度到舵机角度：舵机控制的是摆臂的旋转角度α_i，而不是直接控制长度。假设舵机旋转中心为S_i，摆臂长度为r，连杆与摆臂的连接点为J_i。那么J_i的坐标由舵机角度α_i决定。而J_i到上平台铰链点P_i'的距离就是固定的连杆长度L_i。 这构成了一个几何约束方程，可以解出α_i。在实际简化模型中，如果我们将舵机水平安装，且初始位置时摆臂水平，连杆竖直，那么连杆长度的变化量ΔL_i与舵机角度α_i近似满足：ΔL_i ≈ r * sin(α_i)。更精确的模型需要考虑空间几何关系。

4.2 Arduino代码结构解析

让我们抛开对大量数学代码的恐惧，从逻辑上理解程序是如何运行的。代码主要分为三个部分，正如原文所述。

第一部分：舵机中位校准 (resetToNinety.ino)这个程序的目的只有一个：让所有舵机旋转到90度的位置。这是我们的机械“零位”。

• 为什么是90度？对于180度行程的舵机，90度通常是其中点。在这个位置，根据你的机械安装方式，平台可能处于水平居中状态。
• 操作流程：上传代码后，舵机会转动。此时不要安装连杆。观察所有舵机摆臂是否指向同一个预设方向（例如，全部朝前）。如果不是，手动取下摆臂，调整到一致方向后再装回。确认无误后，再小心安装连杆，此时平台应大致水平且高度适中。如果平台倾斜，说明你的机械安装存在误差，需要微调连杆长度（如果使用可调拉杆）或重新校准舵机中位偏移量。

第二部分：平台联动测试 (moveUpAndDown.ino)这个程序不依赖传感器，它让所有舵机同步地往复运动，带动平台做简单的升降或倾斜。

• 目的：1.机械测试：检查六个舵机运动是否顺畅，有无卡滞、异响，结构是否牢固。2.电气测试：观察在运动时，电源电压是否稳定，有无因电流不足导致的Arduino重启现象。3.建立信心：看到自己搭建的平台“活”过来了，非常有成就感！
• 代码逻辑：在一个循环中，让所有舵机的角度从初始值（如80度）缓慢增加到100度，再减少回80度，如此反复。你可以通过修改角度范围和延迟时间来改变运动幅度和速度。

第三部分：主控制程序 (stewy.ino)这是核心，它实时读取加速度计数据，计算平台应有的姿态，并解算出每个舵机应有的角度。

#include <Wire.h> #include <Servo.h> #include <MPU6050.h> // 假设使用一个封装好的MPU6050库 // 定义舵机对象 Servo servo[6]; // 定义舵机连接的引脚 int servoPins[6] = {2, 3, 4, 5, 6, 7}; // 定义MPU6050对象 MPU6050 mpu; // 定义平台几何参数（这些值需要根据你的实际设计测量和计算） float baseRadius = 100.0; // 下平台铰链点分布圆半径（单位：mm） float platformRadius = 80.0; // 上平台铰链点分布圆半径 float initialHeight = 150.0; // 平台初始高度 // ... 其他参数，如铰链点具体坐标数组 basePoints[6][3], platformPoints[6][3] // 变量声明 float roll, pitch; // 从MPU读取的姿态角 float servoAngles[6]; // 计算得到的6个舵机目标角度 void setup() { Serial.begin(9600); Wire.begin(); mpu.initialize(); // 初始化MPU6050 // 可选：进行传感器校准 // calibrateMPU(); // 初始化所有舵机 for(int i=0; i<6; i++) { servo[i].attach(servoPins[i]); servo[i].write(90); // 初始化为中位 delay(100); } } void loop() { // 1. 读取传感器数据 readMPUData(&roll, &pitch); // 自定义函数，获取滤波后的姿态角 // 2. 逆向运动学计算 calculateInverseKinematics(roll, pitch, servoAngles); // 核心计算函数 // 3. 驱动舵机 for(int i=0; i<6; i++) { // 添加限幅，防止舵机角度超出安全范围（如 30度 到 150度） servoAngles[i] = constrain(servoAngles[i], 30, 150); servo[i].write(servoAngles[i]); } // 加入一个小延迟，控制循环频率（如20-50Hz） delay(20); } // 以下是需要你填充的核心函数 void readMPUData(float *roll, float *pitch) { // 读取MPU原始数据 // 进行校准补偿 // 计算倾斜角（或进行传感器融合） // 将结果赋值给 *roll 和 *pitch } void calculateInverseKinematics(float roll, float pitch, float *angles) { // 将roll, pitch转换为弧度 float phi = roll * PI / 180.0; float theta = pitch * PI / 180.0; float psi = 0; // 假设偏航角为0 // 根据公式计算旋转矩阵 R float R[3][3] = { {cos(theta)*cos(psi), sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi), cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi)}, {cos(theta)*sin(psi), sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi), cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi)}, {-sin(theta), sin(phi)*cos(theta), cos(phi)*cos(theta)} }; // 平移向量 T， 假设平台只旋转，中心点位置不变 float T[3] = {0, 0, initialHeight}; // 对于每个支链 i (从0到5) for(int i=0; i<6; i++) { // 计算上平台铰链点变换后的坐标 P_i' float P_prime[3]; for(int j=0; j<3; j++) { P_prime[j] = 0; for(int k=0; k<3; k++) { P_prime[j] += R[j][k] * platformPoints[i][k]; } P_prime[j] += T[j]; } // 计算连杆向量 L_vec = P_i' - B_i float L_vec[3]; for(int j=0; j<3; j++) { L_vec[j] = P_prime[j] - basePoints[i][j]; } // 计算连杆长度 L_i float Li = sqrt(L_vec[0]*L_vec[0] + L_vec[1]*L_vec[1] + L_vec[2]*L_vec[2]); // 根据连杆长度 Li 和你的具体舵机-摆臂-连杆几何模型，反解出舵机角度 alpha_i // 这是一个简化示例，假设是简单的平面几何关系，实际模型更复杂 // float deltaL = Li - initialLinkLength[i]; // 连杆长度变化量 // float alpha_i = asin(deltaL / servoArmLength) * 180 / PI; // 转换为角度 // angles[i] = 90 + alpha_i; // 90度为舵机中位 // ！！！注意：上面两行是极度简化的示意，你需要根据实际的机械结构推导出正确的公式 ！！！ angles[i] = your_actual_kinematics_model(i, Li); // 替换成你的模型函数 } }

核心提示：代码中最关键也最困难的部分是your_actual_kinematics_model函数。你需要根据自己平台精确测量的basePoints和platformPoints坐标，以及舵机摆臂的安装方式，推导出从连杆长度Li到舵机角度angles[i]的准确数学关系。这可能需要一些解析几何或向量运算的知识。网上有很多开源的Stewart平台逆解库，你可以寻找并适配，但理解其原理对于调试至关重要。

5. 系统调试、优化与问题排查

将代码上传，给系统通电，并不意味着马上就能成功。调试是让项目从“能动”到“好用”的关键过程。

5.1 分步调试策略

1. 传感器单独调试：先不连接舵机，只运行读取MPU-6050并打印Roll和Pitch值的程序。通过串口监视器观察输出。平稳旋转传感器，看角度变化是否平滑、符合预期。如果数据跳动剧烈，检查接线，并优化你的滤波算法（例如，对原始加速度数据做滑动平均滤波）。
2. 单舵机开环测试：将平台拆下，或断开其他五根连杆。编写一个简单程序，让一个舵机根据你手动输入的数值运动。检查其运动范围是否顺畅，有无异响，并记录其能安全运动的角度极限（例如，物理结构限制在30°到150°之间）。
3. 平台静态平衡调试：这是最繁琐但最重要的一步。将所有硬件连接好，上传主程序，但注释掉驱动舵机的那行代码（servo[i].write(...)），改为通过串口打印计算出的6个目标角度。将加速度计水平静止放置，此时roll和pitch应为0。观察打印出的6个角度值，它们应该非常接近（不一定完全相等，因为机械安装有误差）。记录下这6个值作为“零位偏移量”。在后续驱动舵机时，将计算出的角度减去各自的零位偏移量，再进行输出。
4. 闭环试运行：引入反馈控制。在驱动舵机的代码后，加入一个非常缓慢的响应（增大delay）。用手轻轻推动平台，观察它是否试图“抵抗”你的推动回到水平位置。如果平台剧烈抖动或向错误方向运动，立即断电检查。常见原因是：正反馈（应将传感器数据取反）、舵机映射关系错误（例如1号舵机的信号接到了2号舵机上）、或运动学计算中的正负号错误。

5.2 常见问题与解决方案速查表

5.3 性能优化与扩展思路

当基本功能实现后，你可以考虑以下优化，让平台表现更专业：

1. 引入PID控制：目前是开环的“角度映射”，平台受到外力干扰后恢复能力弱。可以引入PID控制器。将传感器测得的平台实际姿态角与目标姿态角（来自手套）做比较，得到误差，然后用PID算法计算出更平滑、抗干扰能力更强的舵机控制量。
2. 传感器融合：如前所述，加入MPU-6050的陀螺仪数据，与加速度计进行互补滤波，得到更准确、反应更快的姿态角，尤其是在动态手势下。
3. 增加上位机调试界面：使用Processing或Python（配合pySerial）编写一个简单的PC端界面，实时显示传感器数据、计算出的角度、甚至以3D模型显示平台姿态，这将极大方便调试。
4. 更换执行机构：舵机有行程和速度限制。如果想实现更大、更快、更精确的平台，可以考虑使用步进电机+滚珠丝杠组成的电动缸，但这需要更复杂的驱动电路（如步进电机驱动器）和闭环控制（如加装编码器）。
5. 正向运动学与自校准：更高级的玩法是实现正向运动学（通过测量连杆长度反推平台姿态），这样你可以在平台上加装另一个传感器来验证控制精度，甚至实现无需外部测量的闭环控制。

搭建这个Stewart平台的过程，就像完成一次微型的机器人系统工程。它强迫你去思考机械、电子、软件和算法如何协同工作，去亲手解决那些在仿真中不会遇到的、真实的噪声、误差和不确定性。当平台最终能颤颤巍巍但又坚定地跟随你的手势保持平衡时，那种跨越虚拟与现实、将数学方程转化为物理运动的成就感，是无与伦比的。希望这份详细的指南能帮你少走弯路，顺利开启你的并联机器人探索之旅。如果在调试中遇到具体问题，不妨回到最基本的环节——检查电源、确认机械顺滑、分模块验证代码，往往能发现那些隐藏在复杂表象下的简单错误。