简介基于Leap Motion与Arduino的机器人手臂控制系统源码包面向机器人控制、嵌入式与人机交互方向的学习者提供一套完整的手势控制机械臂实现方案。项目通过Leap Motion捕获用户手势信息并转换为控制信号配合Arduino的GPIO接口驱动伺服马达用户凭简单手势即可指示机械臂移动方向与位置适合课程设计或入门实战参考。压缩包共43个文件总大小约483KB主体为C/Arduino源码及其头文件辅以Python脚本完成逆运动学解算、串口指令解析与运动逻辑另含Leap Motion捕获脚本、启动Shell脚本以及Markdown格式的连接说明与进度笔记便于按模块阅读和二次开发。目前已有90人浏览学习借助这套资源可快速搭建手势控制机械臂原型理解从手部坐标到关节角度的解算流程、Arduino底层控制方法以及Leap Motion与Pico/Arduino之间的通信方式为扩展多自由度机械臂或优化交互体验提供清晰基础。1. 手势到机械臂LeapMotionArduino控制系统的完整源码链路打开这个源码包你不会看到现成的可执行文件而是一组分工明确的Python脚本和一个下位机固件工程。Leap Motion把手指坐标抓下来inverse_kin_3.py负责把三维坐标换算成肩关节和肘关节的角度servo.py和pio_servo.py再把角度变成舵机PWM脉冲最终让机械臂跟随人手动作。这套系统最值得拆的不是某个算法多高深而是它把手势识别、逆运动学、串口通信和舵机驱动完整串成了一条数据链路。适合手里有Leap Motion设备、想复现手势控制机械臂的开发者也适合正在做机器人控制课程设计、需要一套能跑通并且能讲清楚原理的人。源码结构不复杂但每一层的职责切得很干净后续换舵机、换机械臂结构都不用推倒重来。2. 手势数据管道parser.py如何把Leap Motion三维坐标变成可用指令2.1 Leap Motion坐标系与数据输出格式Leap Motion输出的是右手直角坐标系x轴指向设备右侧y轴竖直向上z轴指向设备外侧也就是朝向操作者。它不像普通摄像头给一张图像而是直接给骨骼点数据每帧包含手ID、掌心位置、每根手指每个关节的三维坐标以及抓握强度grab_strength这个关键量。数据刷新率最高能到120fps但对机械臂控制来说这个频率不是越高越好后面章节会专门讲节流和插值。解析坐标之前先明确一个容易踩坑的点Leap单位是毫米而且坐标系原点是传感器中心。直接拿这个坐标去驱动机械臂你会发现手的活动范围只有几厘米到几十厘米直接映射到舵机角度会非常跳。所以parser.py里的第一步不是解析JSON而是定坐标基准。数据字段含义单位/范围典型用途hand.id手实例ID整型区分左右手、过滤杂讯palm_position掌心三维坐标毫米机械臂末端目标点fingers[j].tip_position第j根手指指尖坐标毫米指尖跟随模式grab_strength抓握强度0.0 - 1.0安全锁、模式切换timestamp帧时间戳微秒轨迹平滑与帧率控制2.2 parser.py的解析逻辑与数据包结构parser.py在项目里承担的是卫兵角色从Leap Motion拉原始数据判断这帧数据要不要用、用哪些字段然后打包成自定义格式交给上层。我一般不走官方SDK的Python绑定而是通过Leap内置的WebSocket服务默认ws://127.0.0.1:6437读JSON帧。原因很简单官方SDK绑定版本和Python版本经常对不上换一次环境就要重编一次而WebSocket接口跨版本稳定拿到什么数据结构一眼就能看清。import json import websocket class LeapParser: def __init__(self, ws_urlws://127.0.0.1:6437): self.ws websocket.create_connection(ws_url, timeout5) self.base_pos None def get_hand_data(self): # 读取一帧Leap Motion数据 raw self.ws.recv() frame json.loads(raw) if hands not in frame or len(frame[hands]) 0: return None hand frame[hands][0] palm hand[palmPosition] # grab_strength 作为机械臂动作的使能开关 return { x: palm[0], y: palm[1], z: palm[2], grab: hand[grabStrength] }这段代码里做了两个关键决定。timeout5是WebSocket连接超时Leap服务没启动时5秒内报错退出不至于让主程序挂死。palmPosition取掌心而不是指尖是因为掌心运动更平稳指尖在高频抖动下会放大机械臂末端抖动新手最容易在这里翻车。grab_strength在0到1之间0是五指张开1是握拳后面会用它做安全锁。2.3 手势到目标点的映射策略拿到掌心坐标后不能直接丢给逆运动学。Leap坐标系里手随便一挪就是几百毫米而桌面机械臂的臂展往往只有二三百毫米。常见做法是做归一化映射把Leap可识别的空间范围映射到机械臂的工作空间。这个映射在parser.py里体现为两组边界值的比值# Leap工作空间到机械臂工作空间的线性映射 ARM_XY_RANGE (80, 200) # 机械臂末端有效半径范围单位mm LEAP_XY_RANGE (50, 250) # Leap传感器上方手的有效活动范围单位mm def map_to_arm_space(coord): # coord_low 和 coord_high 是Leap坐标系中的边界 ratio (coord - coord_low) / (coord_high - coord_low) # 映射到机械臂空间范围 return ARM_XY_RANGE[0] ratio * (ARM_XY_RANGE[1] - ARM_XY_RANGE[0])coord_low和coord_high要按你的Leap安装高度实测标定不要照抄网上的默认值。我习惯是把手掌放在机械臂正上方约200mm处读一次坐标、再升高到300mm读一次用这两点差值做映射系数。另外grab_strength大于0.8时直接丢弃当前帧防止握拳状态下机械臂突然甩动——这是手势控制系统里最基本的安全措施。3. 逆运动学解算inverse_kin_3.py从指尖坐标到舵机角度3.1 二连杆机械臂的几何逆解公式推导机械臂控制的正向逻辑是给你肩关节角度和肘关节角度算出末端位置这叫正运动学。反过来知道末端位置要求关节角度就是逆运动学。这个项目用的是平面二连杆结构肩关节一个舵机、肘关节一个舵机末端只能在二维平面内运动。虽然Leap给的是三维坐标但实际项目里通常锁定z轴只取x和y两个维度做解算因为大多数课设机械臂的底座不能旋转。设大臂长度L1、小臂长度L2、末端目标点(x, y)先用勾股定理算出目标点到肩关节的距离d sqrt(x^2 y^2)。如果d L1 L2目标点超出机械臂可达范围需要截断。肘关节角度用余弦定理import math def inverse_kinematics(x, y, L195, L285): # d 是目标点到肩关节的直线距离 d math.sqrt(x*x y*y) d min(d, L1 L2) # 超出可达范围时截断到极限距离 # 余弦定理求解肘关节角度 cos_theta2 (L1*L1 L2*L2 - d*d) / (2 * L1 * L2) cos_theta2 max(-1.0, min(1.0, cos_theta2)) # 防止float越界 theta2 math.acos(cos_theta2) # 肩关节角度目标点方向的atan2 减去 小臂偏移角 theta1 math.atan2(y, x) - math.atan2(L2 * math.sin(theta2), L1 L2 * math.cos(theta2)) return math.degrees(theta1), math.degrees(theta2)L195和L285是这套源码对应的机械臂臂长单位毫米你手里的硬件不一样必须用卡尺实测后替换否则算出来的角度全是错的。acos函数的入参要夹在[-1, 1]之间因为浮点运算误差可能让cos_theta2算出1.0000001这种值直接传给acos会得到nan。atan2(y, x)和atan2(L2*sinθ2, L1L2*cosθ2)的组合是几何法求肩关节角度的标准写法理解不了可以画个三角形目标点方向角减去小臂引起的偏移角剩下的就是大臂倾角。3.2 奇异点处理与角度范围约束代码跑通之后第一个要面对的就是奇异点。当x和y都接近0时atan2(y, x)失去意义肩关节角度会在正负90度之间跳变。这个位置对应机械臂竖直指向肩关节正下方物理上舵机顶多转到极限位置卡住不会真的转360度但姿态会突然失控。处理方式分两层。Python侧在解算前加一个最小半径判断目标点离肩关节小于30mm时直接沿用上一帧的角度不做更新。下位机侧再做一次关节限位这个在Arduino或Pico代码里更可靠串口发过来的角度先constrain()再写舵机。肩关节范围一般给-90到90度肘关节是0到150度超出就丢弃这帧数据而不是截断因为截断会让机械臂在一个方向持续偏移丢弃则最多损失一帧的跟随精度。3.3 坐标系统一与初始姿态基准还有一个坑在坐标基准。Leap Motion的y轴是竖直向上但机械臂基座装在桌面上它的x/y平面和Leap的x/y平面之间可能差一个旋转角度。源码里解决这个问题的方法是在启动时记录一次校准姿态把手臂水平向前伸平掌心朝下记录这个状态下的掌心坐标作为基准点。后续每一帧输入的坐标都要先减去基准点再做坐标旋转把Leap坐标系对齐到机械臂基座坐标系。这个旋转矩阵用校准姿态时手掌的法向量和机械臂基座法向量之间的夹角来构造但大多数课设场景下机械臂和Leap平行摆放旋转角度为0直接用偏移量就够了。参数含义校准方式offset_xLeap原点到机械臂基座的x偏移手伸平在机械臂正上方时记录offset_yLeap原点到机械臂基座的y偏移同上base_theta两坐标系间的旋转角手掌法向量与机械臂基座法向量夹角4. 舵机驱动与下位机通信servo.py、pio_servo.py和Arduino端PWM控制4.1 servo.py角度到PWM脉宽映射逆运动学解算出来的角度是理论值舵机不认识角度它只认PWM脉冲宽度。标准舵机的控制信号是50Hz方波周期20ms脉宽500us对应0度2500us对应180度。servo.py就是做这个线性换算的。MIN_PULSE 500 # 0度对应的脉宽单位us MAX_PULSE 2500 # 180度对应的脉宽单位us PULSE_RANGE MAX_PULSE - MIN_PULSE SERVO_ANGLE_RANGE 180 def angle_to_pulse(angle_deg): # 角度限制在0-180防止越界写坏舵机 angle_deg max(0, min(180, angle_deg)) # 线性插值计算目标脉宽 pulse_us MIN_PULSE (angle_deg / SERVO_ANGLE_RANGE) * PULSE_RANGE return int(round(pulse_us))注意这里有个细节角度要限制在0到180之间而不是解算出来的理论角度直接传进来。inverse_kinematics()返回的theta1可能是负90度对某些180度舵机来说负角度直接报错。int(round(...))这一步很关键因为后面串口要拆成两个字节传脉宽浮点数会丢掉精度导致舵机实际停的位置和计算值差出好几度。这个函数跑在PC端它算出的pulse_us会通过串口发给下位机。4.2 pio_servo.pyPico端PWM驱动实现源码包里同时有servo.py和pio_servo.py刚开始看会觉得冗余。实际上这两个文件跑在不同设备上servo.py在PC上做角度到脉宽的换算pio_servo.py跑在树莓派Pico上也就是Notes on Connecting Leap to Pico.md提到的方案直接驱动GPIO输出PWM波形。Pico上的MicroPython用machine.PWM它的占空比寄存器是16位的换算方式和Arduino的Servo.write()不一样这也是很多人移植代码时最容易出错的地方。from machine import Pin, PWM class PioServo: def __init__(self, pin, min_pulse500, max_pulse2500, freq50): self.pwm PWM(Pin(pin)) self.pwm.freq(freq) self.min_pulse min_pulse self.max_pulse max_pulse def set_angle(self, angle_deg): # 限制角度范围 angle_deg max(0, min(180, angle_deg)) # 线性映射到脉宽us pulse_us self.min_pulse (angle_deg / 180.0) * (self.max_pulse - self.min_pulse) # MicroPython的duty_u16范围是0-65535对应0%-100%占空比 duty int(pulse_us * 65535 / 20000) self.pwm.duty_u16(duty)freq50对应20ms周期这个不能省部分舵机支持更高频率但标准舵机必须锁50Hz。duty_u16换算的逻辑是脉宽占整个周期的比例乘上65535公式里/20000是因为周期是20000us。如果你用Arduino IDE而不是PlatformIO同样的逻辑要写成Servo.write(angle)但注意Servo库内部自带500到2500的换算不需要你自己算脉宽两个环境的API差异很大别混着用。4.3 串口协议与帧格式设计PC端和下位机之间的通信协议不复杂但格式必须定死。源码里采用的是一帧五字节的固定结构两个字节包头、一个字节舵机ID、两个字节脉宽值高字节在前最后加一个校验字节。为什么不用明文文本因为角度和脉宽都可能出现换行符或空格字符纯文本解析在数据边界上容易出错二进制帧格式更稳。// PlatformIO / Arduino框架下串口接收解析 #define FRAME_HEAD1 0xFF #define FRAME_HEAD2 0xAA #define FRAME_SIZE 6 // 2字节包头 1字节ID 2字节脉宽 1字节校验 uint8_t buf[FRAME_SIZE]; int buf_idx 0; void loop() { if (Serial.available() 0) { uint8_t b Serial.read(); if (buf_idx 0 b ! FRAME_HEAD1) return; if (buf_idx 1 b ! FRAME_HEAD2) { buf_idx 0; return; } buf[buf_idx] b; if (buf_idx FRAME_SIZE) { uint8_t checksum buf[2] ^ buf[3] ^ buf[4]; if (checksum buf[FRAME_SIZE - 1]) { int servo_id buf[2]; int pulse (buf[3] 8) | buf[4]; // 校验通过才写舵机 my_servos[servo_id].writeMicroseconds(pulse); } buf_idx 0; } } }这个接收逻辑有几个值得注意的点。包头匹配用的是状态机方式buf_idx 0时只认0xFFbuf_idx 1时只认0xAA任何一个字节对不上就丢弃重新等包头避免串口噪声导致帧错位。校验用异或而不是累加和因为异或计算快、不需要进位单片机上几行代码就能算完。writeMicroseconds()直接写脉宽值跳过了Servo.write(angle)的二次换算精度更高这也是PC端传脉宽而不是传角度过来的原因。字节偏移内容取值范围说明0包头10xFF固定帧头1包头20xAA防误触发2舵机ID0 - 5对应PWM通道3脉宽高字节0 - 255500-2500us拆成两字节4脉宽低字节0 - 255低位5校验和0 - 2552-4字节异或结果5. 系统联调start_leap.sh、mover.py与实测校准技巧5.1 启动链路与脚本参数整套系统的启动顺序有讲究先启动Leap Motion服务再跑Python主程序最后通过串口与Pico握手。start_leap.sh这个脚本把启动顺序固化下来避免每次手动敲命令。脚本核心逻辑是检查Leap服务端口存活再带参数启动__main__.py串口号和波特率通过环境变量传入。#!/bin/bash # 等待Leap Daemon就绪最多等5秒 for i in $(seq 1 5); do if nc -z 127.0.0.1 6437; then break fi sleep 1 done # 启动主程序/dev/ttyACM0是Pico的USB串口 python3 __main__.py --port /dev/ttyACM0 --baud 115200--port参数在Linux上要确认是/dev/ttyACM0还是/dev/ttyUSB0Pico通常走ACM设备而Arduino Nano的USB转串口芯片通常是/dev/ttyUSB0用错了会报serial.serialutil.SerialException。--baud 115200是Pico端pyb.UART或Serial.begin()里配好的波特率两端必须一致这个是老生常谈但每次联调都会有人卡在9600和115200对不上。5.2 mover.py的平滑插值策略Leap Motion以120fps吐数据但机械臂物理上根本追不上这个速度直接每一帧都驱动舵机结果是舵机疯狂抖动、电流飙升。mover.py在这里的作用是节流和插值。节流是指只从120fps里取30fps的帧来算逆运动学插值是指目标角度和当前角度之间做线性过渡让机械臂运动看起来平滑而不是一卡一卡。class TrajectorySmoother: def __init__(self, step3.0): self.current_angle None self.step step # 每周期最大变化角度单位度 def update(self, target_angle): if self.current_angle is None: self.current_angle target_angle return self.current_angle # 限制每帧角度增量防止舵机猛甩 diff target_angle - self.current_angle if abs(diff) self.step: diff self.step if diff 0 else -self.step self.current_angle diff return self.current_anglestep参数是每帧允许的最大角度变化量调参时先设3.0如果觉得机械臂动作太慢就加到5.0但超过8.0之后舵机噪音会明显变大说明步进太快。这个限制实质上是个低通滤波器它牺牲了响应速度换来了稳定性经验值是跟随延迟控制在100ms内人眼基本无感。5.3 调参经验与仿真验证最后一套比较实用的验证流程是先把串口协议逻辑跑在Wokwi仿真平台上。Wokwi支持Arduino和ESP32仿真你可以先用它验证帧头解析、校验和异或、PWM输出通道这几个独立模块把上位机的串口数据丢进去看舵机角度日志是否正确判断逻辑问题还是硬件问题。同一个固件代码实物上跑不通时拿仿真平台对比是最快的排错手段。机械零位校准这块建议把舵机装舵盘之前先发一次1500us脉宽等舵机转到中位再手动把机械臂调整到水平姿态最后才锁紧舵盘螺丝。否则你会发现逆运动学算出来的角度和实际姿态永远差一个固定偏置。调好的标准是张开手掌机械臂平滑跟随掌心移动慢慢握拳机械臂停在当前位置不抖不漂。Leap Motion传感器建议放在操作者正前方略低于手肘的高度保证抬手时手不会超出传感器可视范围最后把start_leap.sh里的串口号和inverse_kin_3.py里的臂长参数确认一遍重新启动就能完整跑起来。本文还有配套的精品资源点击获取
LeapMotion手势控制机械臂：逆运动学与舵机驱动的完整实现
简介基于Leap Motion与Arduino的机器人手臂控制系统源码包面向机器人控制、嵌入式与人机交互方向的学习者提供一套完整的手势控制机械臂实现方案。项目通过Leap Motion捕获用户手势信息并转换为控制信号配合Arduino的GPIO接口驱动伺服马达用户凭简单手势即可指示机械臂移动方向与位置适合课程设计或入门实战参考。压缩包共43个文件总大小约483KB主体为C/Arduino源码及其头文件辅以Python脚本完成逆运动学解算、串口指令解析与运动逻辑另含Leap Motion捕获脚本、启动Shell脚本以及Markdown格式的连接说明与进度笔记便于按模块阅读和二次开发。目前已有90人浏览学习借助这套资源可快速搭建手势控制机械臂原型理解从手部坐标到关节角度的解算流程、Arduino底层控制方法以及Leap Motion与Pico/Arduino之间的通信方式为扩展多自由度机械臂或优化交互体验提供清晰基础。1. 手势到机械臂LeapMotionArduino控制系统的完整源码链路打开这个源码包你不会看到现成的可执行文件而是一组分工明确的Python脚本和一个下位机固件工程。Leap Motion把手指坐标抓下来inverse_kin_3.py负责把三维坐标换算成肩关节和肘关节的角度servo.py和pio_servo.py再把角度变成舵机PWM脉冲最终让机械臂跟随人手动作。这套系统最值得拆的不是某个算法多高深而是它把手势识别、逆运动学、串口通信和舵机驱动完整串成了一条数据链路。适合手里有Leap Motion设备、想复现手势控制机械臂的开发者也适合正在做机器人控制课程设计、需要一套能跑通并且能讲清楚原理的人。源码结构不复杂但每一层的职责切得很干净后续换舵机、换机械臂结构都不用推倒重来。2. 手势数据管道parser.py如何把Leap Motion三维坐标变成可用指令2.1 Leap Motion坐标系与数据输出格式Leap Motion输出的是右手直角坐标系x轴指向设备右侧y轴竖直向上z轴指向设备外侧也就是朝向操作者。它不像普通摄像头给一张图像而是直接给骨骼点数据每帧包含手ID、掌心位置、每根手指每个关节的三维坐标以及抓握强度grab_strength这个关键量。数据刷新率最高能到120fps但对机械臂控制来说这个频率不是越高越好后面章节会专门讲节流和插值。解析坐标之前先明确一个容易踩坑的点Leap单位是毫米而且坐标系原点是传感器中心。直接拿这个坐标去驱动机械臂你会发现手的活动范围只有几厘米到几十厘米直接映射到舵机角度会非常跳。所以parser.py里的第一步不是解析JSON而是定坐标基准。数据字段含义单位/范围典型用途hand.id手实例ID整型区分左右手、过滤杂讯palm_position掌心三维坐标毫米机械臂末端目标点fingers[j].tip_position第j根手指指尖坐标毫米指尖跟随模式grab_strength抓握强度0.0 - 1.0安全锁、模式切换timestamp帧时间戳微秒轨迹平滑与帧率控制2.2 parser.py的解析逻辑与数据包结构parser.py在项目里承担的是卫兵角色从Leap Motion拉原始数据判断这帧数据要不要用、用哪些字段然后打包成自定义格式交给上层。我一般不走官方SDK的Python绑定而是通过Leap内置的WebSocket服务默认ws://127.0.0.1:6437读JSON帧。原因很简单官方SDK绑定版本和Python版本经常对不上换一次环境就要重编一次而WebSocket接口跨版本稳定拿到什么数据结构一眼就能看清。import json import websocket class LeapParser: def __init__(self, ws_urlws://127.0.0.1:6437): self.ws websocket.create_connection(ws_url, timeout5) self.base_pos None def get_hand_data(self): # 读取一帧Leap Motion数据 raw self.ws.recv() frame json.loads(raw) if hands not in frame or len(frame[hands]) 0: return None hand frame[hands][0] palm hand[palmPosition] # grab_strength 作为机械臂动作的使能开关 return { x: palm[0], y: palm[1], z: palm[2], grab: hand[grabStrength] }这段代码里做了两个关键决定。timeout5是WebSocket连接超时Leap服务没启动时5秒内报错退出不至于让主程序挂死。palmPosition取掌心而不是指尖是因为掌心运动更平稳指尖在高频抖动下会放大机械臂末端抖动新手最容易在这里翻车。grab_strength在0到1之间0是五指张开1是握拳后面会用它做安全锁。2.3 手势到目标点的映射策略拿到掌心坐标后不能直接丢给逆运动学。Leap坐标系里手随便一挪就是几百毫米而桌面机械臂的臂展往往只有二三百毫米。常见做法是做归一化映射把Leap可识别的空间范围映射到机械臂的工作空间。这个映射在parser.py里体现为两组边界值的比值# Leap工作空间到机械臂工作空间的线性映射 ARM_XY_RANGE (80, 200) # 机械臂末端有效半径范围单位mm LEAP_XY_RANGE (50, 250) # Leap传感器上方手的有效活动范围单位mm def map_to_arm_space(coord): # coord_low 和 coord_high 是Leap坐标系中的边界 ratio (coord - coord_low) / (coord_high - coord_low) # 映射到机械臂空间范围 return ARM_XY_RANGE[0] ratio * (ARM_XY_RANGE[1] - ARM_XY_RANGE[0])coord_low和coord_high要按你的Leap安装高度实测标定不要照抄网上的默认值。我习惯是把手掌放在机械臂正上方约200mm处读一次坐标、再升高到300mm读一次用这两点差值做映射系数。另外grab_strength大于0.8时直接丢弃当前帧防止握拳状态下机械臂突然甩动——这是手势控制系统里最基本的安全措施。3. 逆运动学解算inverse_kin_3.py从指尖坐标到舵机角度3.1 二连杆机械臂的几何逆解公式推导机械臂控制的正向逻辑是给你肩关节角度和肘关节角度算出末端位置这叫正运动学。反过来知道末端位置要求关节角度就是逆运动学。这个项目用的是平面二连杆结构肩关节一个舵机、肘关节一个舵机末端只能在二维平面内运动。虽然Leap给的是三维坐标但实际项目里通常锁定z轴只取x和y两个维度做解算因为大多数课设机械臂的底座不能旋转。设大臂长度L1、小臂长度L2、末端目标点(x, y)先用勾股定理算出目标点到肩关节的距离d sqrt(x^2 y^2)。如果d L1 L2目标点超出机械臂可达范围需要截断。肘关节角度用余弦定理import math def inverse_kinematics(x, y, L195, L285): # d 是目标点到肩关节的直线距离 d math.sqrt(x*x y*y) d min(d, L1 L2) # 超出可达范围时截断到极限距离 # 余弦定理求解肘关节角度 cos_theta2 (L1*L1 L2*L2 - d*d) / (2 * L1 * L2) cos_theta2 max(-1.0, min(1.0, cos_theta2)) # 防止float越界 theta2 math.acos(cos_theta2) # 肩关节角度目标点方向的atan2 减去 小臂偏移角 theta1 math.atan2(y, x) - math.atan2(L2 * math.sin(theta2), L1 L2 * math.cos(theta2)) return math.degrees(theta1), math.degrees(theta2)L195和L285是这套源码对应的机械臂臂长单位毫米你手里的硬件不一样必须用卡尺实测后替换否则算出来的角度全是错的。acos函数的入参要夹在[-1, 1]之间因为浮点运算误差可能让cos_theta2算出1.0000001这种值直接传给acos会得到nan。atan2(y, x)和atan2(L2*sinθ2, L1L2*cosθ2)的组合是几何法求肩关节角度的标准写法理解不了可以画个三角形目标点方向角减去小臂引起的偏移角剩下的就是大臂倾角。3.2 奇异点处理与角度范围约束代码跑通之后第一个要面对的就是奇异点。当x和y都接近0时atan2(y, x)失去意义肩关节角度会在正负90度之间跳变。这个位置对应机械臂竖直指向肩关节正下方物理上舵机顶多转到极限位置卡住不会真的转360度但姿态会突然失控。处理方式分两层。Python侧在解算前加一个最小半径判断目标点离肩关节小于30mm时直接沿用上一帧的角度不做更新。下位机侧再做一次关节限位这个在Arduino或Pico代码里更可靠串口发过来的角度先constrain()再写舵机。肩关节范围一般给-90到90度肘关节是0到150度超出就丢弃这帧数据而不是截断因为截断会让机械臂在一个方向持续偏移丢弃则最多损失一帧的跟随精度。3.3 坐标系统一与初始姿态基准还有一个坑在坐标基准。Leap Motion的y轴是竖直向上但机械臂基座装在桌面上它的x/y平面和Leap的x/y平面之间可能差一个旋转角度。源码里解决这个问题的方法是在启动时记录一次校准姿态把手臂水平向前伸平掌心朝下记录这个状态下的掌心坐标作为基准点。后续每一帧输入的坐标都要先减去基准点再做坐标旋转把Leap坐标系对齐到机械臂基座坐标系。这个旋转矩阵用校准姿态时手掌的法向量和机械臂基座法向量之间的夹角来构造但大多数课设场景下机械臂和Leap平行摆放旋转角度为0直接用偏移量就够了。参数含义校准方式offset_xLeap原点到机械臂基座的x偏移手伸平在机械臂正上方时记录offset_yLeap原点到机械臂基座的y偏移同上base_theta两坐标系间的旋转角手掌法向量与机械臂基座法向量夹角4. 舵机驱动与下位机通信servo.py、pio_servo.py和Arduino端PWM控制4.1 servo.py角度到PWM脉宽映射逆运动学解算出来的角度是理论值舵机不认识角度它只认PWM脉冲宽度。标准舵机的控制信号是50Hz方波周期20ms脉宽500us对应0度2500us对应180度。servo.py就是做这个线性换算的。MIN_PULSE 500 # 0度对应的脉宽单位us MAX_PULSE 2500 # 180度对应的脉宽单位us PULSE_RANGE MAX_PULSE - MIN_PULSE SERVO_ANGLE_RANGE 180 def angle_to_pulse(angle_deg): # 角度限制在0-180防止越界写坏舵机 angle_deg max(0, min(180, angle_deg)) # 线性插值计算目标脉宽 pulse_us MIN_PULSE (angle_deg / SERVO_ANGLE_RANGE) * PULSE_RANGE return int(round(pulse_us))注意这里有个细节角度要限制在0到180之间而不是解算出来的理论角度直接传进来。inverse_kinematics()返回的theta1可能是负90度对某些180度舵机来说负角度直接报错。int(round(...))这一步很关键因为后面串口要拆成两个字节传脉宽浮点数会丢掉精度导致舵机实际停的位置和计算值差出好几度。这个函数跑在PC端它算出的pulse_us会通过串口发给下位机。4.2 pio_servo.pyPico端PWM驱动实现源码包里同时有servo.py和pio_servo.py刚开始看会觉得冗余。实际上这两个文件跑在不同设备上servo.py在PC上做角度到脉宽的换算pio_servo.py跑在树莓派Pico上也就是Notes on Connecting Leap to Pico.md提到的方案直接驱动GPIO输出PWM波形。Pico上的MicroPython用machine.PWM它的占空比寄存器是16位的换算方式和Arduino的Servo.write()不一样这也是很多人移植代码时最容易出错的地方。from machine import Pin, PWM class PioServo: def __init__(self, pin, min_pulse500, max_pulse2500, freq50): self.pwm PWM(Pin(pin)) self.pwm.freq(freq) self.min_pulse min_pulse self.max_pulse max_pulse def set_angle(self, angle_deg): # 限制角度范围 angle_deg max(0, min(180, angle_deg)) # 线性映射到脉宽us pulse_us self.min_pulse (angle_deg / 180.0) * (self.max_pulse - self.min_pulse) # MicroPython的duty_u16范围是0-65535对应0%-100%占空比 duty int(pulse_us * 65535 / 20000) self.pwm.duty_u16(duty)freq50对应20ms周期这个不能省部分舵机支持更高频率但标准舵机必须锁50Hz。duty_u16换算的逻辑是脉宽占整个周期的比例乘上65535公式里/20000是因为周期是20000us。如果你用Arduino IDE而不是PlatformIO同样的逻辑要写成Servo.write(angle)但注意Servo库内部自带500到2500的换算不需要你自己算脉宽两个环境的API差异很大别混着用。4.3 串口协议与帧格式设计PC端和下位机之间的通信协议不复杂但格式必须定死。源码里采用的是一帧五字节的固定结构两个字节包头、一个字节舵机ID、两个字节脉宽值高字节在前最后加一个校验字节。为什么不用明文文本因为角度和脉宽都可能出现换行符或空格字符纯文本解析在数据边界上容易出错二进制帧格式更稳。// PlatformIO / Arduino框架下串口接收解析 #define FRAME_HEAD1 0xFF #define FRAME_HEAD2 0xAA #define FRAME_SIZE 6 // 2字节包头 1字节ID 2字节脉宽 1字节校验 uint8_t buf[FRAME_SIZE]; int buf_idx 0; void loop() { if (Serial.available() 0) { uint8_t b Serial.read(); if (buf_idx 0 b ! FRAME_HEAD1) return; if (buf_idx 1 b ! FRAME_HEAD2) { buf_idx 0; return; } buf[buf_idx] b; if (buf_idx FRAME_SIZE) { uint8_t checksum buf[2] ^ buf[3] ^ buf[4]; if (checksum buf[FRAME_SIZE - 1]) { int servo_id buf[2]; int pulse (buf[3] 8) | buf[4]; // 校验通过才写舵机 my_servos[servo_id].writeMicroseconds(pulse); } buf_idx 0; } } }这个接收逻辑有几个值得注意的点。包头匹配用的是状态机方式buf_idx 0时只认0xFFbuf_idx 1时只认0xAA任何一个字节对不上就丢弃重新等包头避免串口噪声导致帧错位。校验用异或而不是累加和因为异或计算快、不需要进位单片机上几行代码就能算完。writeMicroseconds()直接写脉宽值跳过了Servo.write(angle)的二次换算精度更高这也是PC端传脉宽而不是传角度过来的原因。字节偏移内容取值范围说明0包头10xFF固定帧头1包头20xAA防误触发2舵机ID0 - 5对应PWM通道3脉宽高字节0 - 255500-2500us拆成两字节4脉宽低字节0 - 255低位5校验和0 - 2552-4字节异或结果5. 系统联调start_leap.sh、mover.py与实测校准技巧5.1 启动链路与脚本参数整套系统的启动顺序有讲究先启动Leap Motion服务再跑Python主程序最后通过串口与Pico握手。start_leap.sh这个脚本把启动顺序固化下来避免每次手动敲命令。脚本核心逻辑是检查Leap服务端口存活再带参数启动__main__.py串口号和波特率通过环境变量传入。#!/bin/bash # 等待Leap Daemon就绪最多等5秒 for i in $(seq 1 5); do if nc -z 127.0.0.1 6437; then break fi sleep 1 done # 启动主程序/dev/ttyACM0是Pico的USB串口 python3 __main__.py --port /dev/ttyACM0 --baud 115200--port参数在Linux上要确认是/dev/ttyACM0还是/dev/ttyUSB0Pico通常走ACM设备而Arduino Nano的USB转串口芯片通常是/dev/ttyUSB0用错了会报serial.serialutil.SerialException。--baud 115200是Pico端pyb.UART或Serial.begin()里配好的波特率两端必须一致这个是老生常谈但每次联调都会有人卡在9600和115200对不上。5.2 mover.py的平滑插值策略Leap Motion以120fps吐数据但机械臂物理上根本追不上这个速度直接每一帧都驱动舵机结果是舵机疯狂抖动、电流飙升。mover.py在这里的作用是节流和插值。节流是指只从120fps里取30fps的帧来算逆运动学插值是指目标角度和当前角度之间做线性过渡让机械臂运动看起来平滑而不是一卡一卡。class TrajectorySmoother: def __init__(self, step3.0): self.current_angle None self.step step # 每周期最大变化角度单位度 def update(self, target_angle): if self.current_angle is None: self.current_angle target_angle return self.current_angle # 限制每帧角度增量防止舵机猛甩 diff target_angle - self.current_angle if abs(diff) self.step: diff self.step if diff 0 else -self.step self.current_angle diff return self.current_anglestep参数是每帧允许的最大角度变化量调参时先设3.0如果觉得机械臂动作太慢就加到5.0但超过8.0之后舵机噪音会明显变大说明步进太快。这个限制实质上是个低通滤波器它牺牲了响应速度换来了稳定性经验值是跟随延迟控制在100ms内人眼基本无感。5.3 调参经验与仿真验证最后一套比较实用的验证流程是先把串口协议逻辑跑在Wokwi仿真平台上。Wokwi支持Arduino和ESP32仿真你可以先用它验证帧头解析、校验和异或、PWM输出通道这几个独立模块把上位机的串口数据丢进去看舵机角度日志是否正确判断逻辑问题还是硬件问题。同一个固件代码实物上跑不通时拿仿真平台对比是最快的排错手段。机械零位校准这块建议把舵机装舵盘之前先发一次1500us脉宽等舵机转到中位再手动把机械臂调整到水平姿态最后才锁紧舵盘螺丝。否则你会发现逆运动学算出来的角度和实际姿态永远差一个固定偏置。调好的标准是张开手掌机械臂平滑跟随掌心移动慢慢握拳机械臂停在当前位置不抖不漂。Leap Motion传感器建议放在操作者正前方略低于手肘的高度保证抬手时手不会超出传感器可视范围最后把start_leap.sh里的串口号和inverse_kin_3.py里的臂长参数确认一遍重新启动就能完整跑起来。本文还有配套的精品资源点击获取