diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c6114e1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,26 @@ +# Compiled binaries +*.exe +*.out +C++/emg_robotic_arm + +# Python cache +__pycache__/ +*.pyc +*.pyo +*.pyd + +# Build artifacts +*.o +*.obj +*.so +*.dll + +# IDE files +.vscode/ +.idea/ +*.swp +*.swo + +# OS files +.DS_Store +Thumbs.db \ No newline at end of file diff --git a/C++/Makefile b/C++/Makefile new file mode 100644 index 0000000..a3a51b5 --- /dev/null +++ b/C++/Makefile @@ -0,0 +1,43 @@ +# Makefile for EMG Robotic Arm System + +CXX = g++ +CXXFLAGS = -std=c++11 -Wall -Wextra -O2 +LDFLAGS = -pthread + +# Target executable +TARGET = emg_robotic_arm +SOURCE = emg_robotic_arm.cpp + +# Default target +all: $(TARGET) + +# Build the main executable +$(TARGET): $(SOURCE) + $(CXX) $(CXXFLAGS) $(SOURCE) -o $(TARGET) $(LDFLAGS) + +# Clean build artifacts +clean: + rm -f $(TARGET) *.o + +# Run the program +run: $(TARGET) + ./$(TARGET) + +# Debug build +debug: CXXFLAGS += -g -DDEBUG +debug: $(TARGET) + +# Install dependencies (if needed) +install-deps: + @echo "No additional dependencies required for this project" + +# Help target +help: + @echo "Available targets:" + @echo " all - Build the EMG robotic arm executable (default)" + @echo " clean - Remove build artifacts" + @echo " run - Build and run the program" + @echo " debug - Build with debug information" + @echo " help - Show this help message" + +.PHONY: all clean run debug install-deps help \ No newline at end of file diff --git a/C++/emg_robotic_arm b/C++/emg_robotic_arm new file mode 100755 index 0000000..69d3c3f Binary files /dev/null and b/C++/emg_robotic_arm differ diff --git a/C++/emg_robotic_arm.cpp b/C++/emg_robotic_arm.cpp new file mode 100644 index 0000000..b7c4c3c --- /dev/null +++ b/C++/emg_robotic_arm.cpp @@ -0,0 +1,431 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +// EMG信号数据结构 +struct EMGSignal { + double amplitude; // 信号幅度 (0.0 - 1.0) + double frequency; // 信号频率 (Hz) + chrono::steady_clock::time_point timestamp; + + EMGSignal(double amp = 0.0, double freq = 0.0) + : amplitude(amp), frequency(freq), timestamp(chrono::steady_clock::now()) {} +}; + +// 机械臂关节结构 +struct Joint { + int id; + double currentAngle; // 当前角度 (度) + double targetAngle; // 目标角度 (度) + double minAngle; // 最小角度限制 + double maxAngle; // 最大角度限制 + double speed; // 运动速度 (度/秒) + + Joint(int jointId, double minAng, double maxAng, double spd = 30.0) + : id(jointId), currentAngle(0.0), targetAngle(0.0), + minAngle(minAng), maxAngle(maxAng), speed(spd) {} +}; + +// 手势识别结果 +enum class Gesture { + REST, // 休息状态 + FIST, // 握拳 + OPEN_HAND, // 张开手掌 + POINT, // 指向 + GRASP, // 抓取 + WAVE // 挥手 +}; + +class EMGProcessor { +private: + queue signalBuffer; + mutex bufferMutex; + static const size_t BUFFER_SIZE = 100; + + // 信号滤波参数 + vector filterCoeffs = {0.1, 0.2, 0.4, 0.2, 0.1}; // 简单低通滤波器 + +public: + // 添加EMG信号到缓冲区 + void addSignal(const EMGSignal& signal) { + lock_guard lock(bufferMutex); + signalBuffer.push(signal); + + // 保持缓冲区大小 + if (signalBuffer.size() > BUFFER_SIZE) { + signalBuffer.pop(); + } + } + + // 信号滤波处理 + double filterSignal(const vector& rawData) { + if (rawData.size() < filterCoeffs.size()) { + return rawData.empty() ? 0.0 : rawData.back(); + } + + double filtered = 0.0; + for (size_t i = 0; i < filterCoeffs.size(); ++i) { + filtered += filterCoeffs[i] * rawData[rawData.size() - 1 - i]; + } + return filtered; + } + + // 手势识别 + Gesture recognizeGesture() { + lock_guard lock(bufferMutex); + + if (signalBuffer.empty()) { + return Gesture::REST; + } + + // 计算最近信号的平均幅度 + double avgAmplitude = 0.0; + double avgFrequency = 0.0; + int count = min(static_cast(signalBuffer.size()), 10); + + queue temp = signalBuffer; + vector recentSignals; + + while (!temp.empty() && recentSignals.size() < count) { + recentSignals.push_back(temp.front()); + temp.pop(); + } + + for (const auto& signal : recentSignals) { + avgAmplitude += signal.amplitude; + avgFrequency += signal.frequency; + } + + if (recentSignals.empty()) return Gesture::REST; + + avgAmplitude /= recentSignals.size(); + avgFrequency /= recentSignals.size(); + + // 基于幅度和频率的简单手势识别 + if (avgAmplitude < 0.1) { + return Gesture::REST; + } else if (avgAmplitude > 0.8 && avgFrequency > 50) { + return Gesture::FIST; + } else if (avgAmplitude > 0.6 && avgFrequency < 30) { + return Gesture::OPEN_HAND; + } else if (avgAmplitude > 0.4 && avgFrequency > 40) { + return Gesture::GRASP; + } else if (avgAmplitude > 0.3) { + return Gesture::POINT; + } else { + return Gesture::WAVE; + } + } +}; + +class RoboticArm { +private: + vector joints; + mutex armMutex; + atomic isMoving{false}; + +public: + RoboticArm() { + // 初始化6自由度机械臂关节 + joints.emplace_back(0, -180, 180, 45); // 基座旋转 + joints.emplace_back(1, -90, 90, 30); // 肩部 + joints.emplace_back(2, -120, 120, 35); // 肘部 + joints.emplace_back(3, -90, 90, 50); // 腕部俯仰 + joints.emplace_back(4, -180, 180, 60); // 腕部旋转 + joints.emplace_back(5, 0, 90, 40); // 夹爪 + } + + // 设置关节目标角度 + void setJointAngle(int jointId, double angle) { + lock_guard lock(armMutex); + if (jointId < 0 || jointId >= joints.size()) return; + + Joint& joint = joints[jointId]; + joint.targetAngle = max(joint.minAngle, min(joint.maxAngle, angle)); + } + + // 执行手势对应的动作 + void executeGesture(Gesture gesture) { + switch (gesture) { + case Gesture::REST: + // 回到初始位置 + setJointAngle(0, 0); // 基座 + setJointAngle(1, 0); // 肩部 + setJointAngle(2, 0); // 肘部 + setJointAngle(3, 0); // 腕部俯仰 + setJointAngle(4, 0); // 腕部旋转 + setJointAngle(5, 0); // 夹爪打开 + break; + + case Gesture::FIST: + // 紧握动作 + setJointAngle(1, -30); // 肩部下压 + setJointAngle(2, 45); // 肘部弯曲 + setJointAngle(5, 90); // 夹爪关闭 + break; + + case Gesture::OPEN_HAND: + // 张开手掌 + setJointAngle(1, 15); // 肩部上抬 + setJointAngle(2, -20); // 肘部伸展 + setJointAngle(5, 0); // 夹爪完全打开 + break; + + case Gesture::POINT: + // 指向动作 + setJointAngle(0, 30); // 基座旋转 + setJointAngle(1, 0); // 肩部水平 + setJointAngle(2, -45); // 肘部伸展 + setJointAngle(3, -15); // 腕部微调 + break; + + case Gesture::GRASP: + // 抓取动作 + setJointAngle(1, -15); // 肩部轻微下压 + setJointAngle(2, 30); // 肘部适度弯曲 + setJointAngle(5, 60); // 夹爪部分关闭 + break; + + case Gesture::WAVE: + // 挥手动作 + setJointAngle(0, -30); // 基座左转 + setJointAngle(1, 30); // 肩部上抬 + setJointAngle(4, 45); // 腕部旋转 + break; + } + } + + // 更新关节位置（运动控制） + void updateJoints() { + lock_guard lock(armMutex); + bool anyMoving = false; + + for (auto& joint : joints) { + if (abs(joint.currentAngle - joint.targetAngle) > 0.5) { + double direction = (joint.targetAngle > joint.currentAngle) ? 1.0 : -1.0; + double deltaTime = 0.1; // 100ms更新间隔 + double movement = joint.speed * deltaTime * direction; + + if (abs(movement) > abs(joint.targetAngle - joint.currentAngle)) { + joint.currentAngle = joint.targetAngle; + } else { + joint.currentAngle += movement; + } + anyMoving = true; + } + } + + isMoving = anyMoving; + } + + // 打印当前状态 + void printStatus() { + lock_guard lock(armMutex); + cout << "机械臂状态:" << endl; + const vector jointNames = {"基座", "肩部", "肘部", "腕俯仰", "腕旋转", "夹爪"}; + + for (size_t i = 0; i < joints.size(); ++i) { + cout << jointNames[i] << ": " + << fixed << setprecision(1) << joints[i].currentAngle << "° "; + if (abs(joints[i].currentAngle - joints[i].targetAngle) > 0.5) { + cout << "-> " << joints[i].targetAngle << "°"; + } + cout << endl; + } + cout << "运动状态: " << (isMoving ? "运动中" : "静止") << endl << endl; + } +}; + +// EMG信号模拟器 +class EMGSimulator { +private: + random_device rd; + mt19937 gen; + uniform_real_distribution<> ampDist; + uniform_real_distribution<> freqDist; + uniform_real_distribution<> noiseDist; + +public: + EMGSimulator() : gen(rd()), ampDist(0.0, 1.0), freqDist(20.0, 80.0), noiseDist(-0.05, 0.05) {} + + // 生成模拟EMG信号 + EMGSignal generateSignal(Gesture targetGesture = Gesture::REST) { + double baseAmplitude = 0.0; + double baseFrequency = 25.0; + + // 根据目标手势调整基础参数 + switch (targetGesture) { + case Gesture::REST: + baseAmplitude = 0.05; + baseFrequency = 25.0; + break; + case Gesture::FIST: + baseAmplitude = 0.9; + baseFrequency = 60.0; + break; + case Gesture::OPEN_HAND: + baseAmplitude = 0.7; + baseFrequency = 25.0; + break; + case Gesture::POINT: + baseAmplitude = 0.3; + baseFrequency = 35.0; + break; + case Gesture::GRASP: + baseAmplitude = 0.5; + baseFrequency = 45.0; + break; + case Gesture::WAVE: + baseAmplitude = 0.4; + baseFrequency = 30.0; + break; + } + + // 添加噪声和变化 + double amplitude = max(0.0, min(1.0, baseAmplitude + noiseDist(gen))); + double frequency = max(10.0, min(100.0, baseFrequency + noiseDist(gen) * 10)); + + return EMGSignal(amplitude, frequency); + } +}; + +// 主控制系统 +class EMGRoboticSystem { +private: + EMGProcessor processor; + RoboticArm arm; + EMGSimulator simulator; + atomic running{true}; + +public: + void run() { + cout << "EMG控制机械臂系统启动..." << endl; + cout << "系统将模拟EMG信号并控制机械臂运动" << endl; + cout << "按Ctrl+C退出程序" << endl << endl; + + // 创建线程 + thread emgThread(&EMGRoboticSystem::emgAcquisitionLoop, this); + thread controlThread(&EMGRoboticSystem::controlLoop, this); + thread motionThread(&EMGRoboticSystem::motionLoop, this); + + // 主循环 - 用户交互和状态显示 + vector gestureSequence = { + Gesture::REST, Gesture::FIST, Gesture::OPEN_HAND, + Gesture::POINT, Gesture::GRASP, Gesture::WAVE + }; + + size_t currentGesture = 0; + auto lastGestureChange = chrono::steady_clock::now(); + + while (running) { + // 每5秒切换一次手势进行演示 + auto now = chrono::steady_clock::now(); + if (chrono::duration_cast(now - lastGestureChange).count() >= 5) { + currentGesture = (currentGesture + 1) % gestureSequence.size(); + lastGestureChange = now; + cout << "切换到手势: " << gestureToString(gestureSequence[currentGesture]) << endl; + } + + // 生成对应手势的EMG信号 + EMGSignal signal = simulator.generateSignal(gestureSequence[currentGesture]); + processor.addSignal(signal); + + this_thread::sleep_for(chrono::milliseconds(100)); + } + + // 等待线程结束 + emgThread.join(); + controlThread.join(); + motionThread.join(); + } + + void stop() { + running = false; + } + +private: + void emgAcquisitionLoop() { + while (running) { + // 在实际应用中，这里会从EMG传感器获取真实信号 + // 现在使用模拟信号 + this_thread::sleep_for(chrono::milliseconds(50)); + } + } + + void controlLoop() { + Gesture lastGesture = Gesture::REST; + + while (running) { + // 识别当前手势 + Gesture currentGesture = processor.recognizeGesture(); + + // 如果手势改变，执行相应动作 + if (currentGesture != lastGesture) { + cout << "识别手势: " << gestureToString(currentGesture) << endl; + arm.executeGesture(currentGesture); + lastGesture = currentGesture; + } + + this_thread::sleep_for(chrono::milliseconds(200)); + } + } + + void motionLoop() { + while (running) { + arm.updateJoints(); + + // 每秒打印一次状态 + static auto lastPrint = chrono::steady_clock::now(); + auto now = chrono::steady_clock::now(); + if (chrono::duration_cast(now - lastPrint).count() >= 2) { + arm.printStatus(); + lastPrint = now; + } + + this_thread::sleep_for(chrono::milliseconds(100)); + } + } + + string gestureToString(Gesture gesture) { + switch (gesture) { + case Gesture::REST: return "休息"; + case Gesture::FIST: return "握拳"; + case Gesture::OPEN_HAND: return "张手"; + case Gesture::POINT: return "指向"; + case Gesture::GRASP: return "抓取"; + case Gesture::WAVE: return "挥手"; + default: return "未知"; + } + } +}; + +int main() { + EMGRoboticSystem system; + + // 设置信号处理，优雅退出 + signal(SIGINT, [](int) { + cout << "\n正在关闭系统..." << endl; + exit(0); + }); + + try { + system.run(); + } catch (const exception& e) { + cerr << "系统错误: " << e.what() << endl; + return 1; + } + + return 0; +} \ No newline at end of file diff --git a/EMG_ROBOTIC_ARM_README.md b/EMG_ROBOTIC_ARM_README.md new file mode 100644 index 0000000..01c5afc --- /dev/null +++ b/EMG_ROBOTIC_ARM_README.md @@ -0,0 +1,154 @@ +# EMG控制机械臂系统 + +本项目实现了一个基于肌电信号(EMG)控制的机械臂系统，可以通过肌肉活动信号来控制6自由度机械臂的运动。 + +## 项目特点 + +- **实时信号处理**: 实时采集和处理EMG信号 +- **手势识别**: 识别6种不同的手势动作 +- **机械臂控制**: 6自由度机械臂运动控制 +- **安全系统**: 关节角度限制和运动安全保护 +- **跨平台**: 提供C++和Python两种实现 + +## 系统架构 + +### 核心组件 + +1. **EMG信号处理器 (EMGProcessor)** + - 信号缓冲和滤波 + - 噪声消除 + - 特征提取 + +2. **手势识别系统** + - 基于幅度和频率的分类算法 + - 支持6种手势：休息、握拳、张手、指向、抓取、挥手 + +3. **机械臂控制器 (RoboticArm)** + - 6自由度关节控制 + - 平滑运动插值 + - 安全角度限制 + +4. **EMG信号模拟器 (EMGSimulator)** + - 生成不同手势对应的模拟EMG信号 + - 添加真实噪声模拟 + +## 支持的手势 + +| 手势 | 描述 | 机械臂动作 | +|------|------|------------| +| 休息 | 肌肉放松状态 | 回到初始位置 | +| 握拳 | 强力收缩 | 肩部下压，肘部弯曲，夹爪关闭 | +| 张手 | 手掌张开 | 肩部上抬，肘部伸展，夹爪打开 | +| 指向 | 指向动作 | 基座旋转，肘部伸展 | +| 抓取 | 抓取物体 | 适度弯曲，夹爪部分关闭 | +| 挥手 | 挥手动作 | 基座左转，肩部上抬，腕部旋转 | + +## 文件说明 + +### C++实现 +- `C++/emg_robotic_arm.cpp`: 完整的C++实现 +- 使用多线程进行并发处理 +- 需要支持C++11及以上标准 + +### Python实现 +- `Python/emg_robotic_arm.py`: Python实现版本 +- 使用threading模块实现并发 +- 无外部依赖，仅使用Python标准库 + +## 编译和运行 + +### C++版本 + +```bash +# 编译 +cd C++ +g++ -std=c++11 -pthread -o emg_robotic_arm emg_robotic_arm.cpp + +# 运行 +./emg_robotic_arm +``` + +### Python版本 + +```bash +# 直接运行 +cd Python +python3 emg_robotic_arm.py +``` + +## 系统运行流程 + +1. **系统初始化**: 创建EMG处理器、机械臂控制器和信号模拟器 +2. **多线程启动**: + - EMG信号采集线程 + - 手势识别和控制线程 + - 机械臂运动控制线程 +3. **实时处理**: + - 持续采集EMG信号 + - 实时识别手势 + - 控制机械臂执行对应动作 +4. **状态监控**: 定期输出机械臂状态和运动信息 + +## 技术细节 + +### EMG信号处理 +- 采样频率: 20Hz (可调整) +- 滤波器: 5点移动平均低通滤波器 +- 缓冲区大小: 100个样本 + +### 机械臂规格 +- 自由度: 6DOF (基座、肩部、肘部、腕俯仰、腕旋转、夹爪) +- 角度范围: 根据关节类型设定不同限制 +- 运动速度: 30-60度/秒 (可配置) + +### 手势识别算法 +基于EMG信号的幅度和频率特征进行分类： +- 幅度阈值分级 +- 频率特征分析 +- 时序稳定性检查 + +## 扩展功能 + +### 可能的改进方向 +1. **更复杂的手势**: 支持更多手势类型 +2. **机器学习**: 使用神经网络进行手势识别 +3. **硬件集成**: 连接真实的EMG传感器 +4. **视觉反馈**: 添加GUI界面显示 +5. **力反馈**: 集成力传感器和触觉反馈 + +### 实际应用场景 +- 康复医疗: 帮助残疾人士进行日常活动 +- 工业自动化: 人机协作系统 +- 科研教学: EMG信号处理和机器人控制学习 +- 娱乐游戏: 体感控制游戏 + +## 安全注意事项 + +1. **关节限制**: 所有关节都有安全角度限制 +2. **速度控制**: 运动速度限制防止突然动作 +3. **紧急停止**: 支持Ctrl+C快速停止系统 +4. **状态监控**: 实时显示运动状态 + +## 故障排除 + +### 常见问题 +1. **编译错误**: 确保使用C++11或更高版本，安装pthread库 +2. **Python模块错误**: 使用Python 3.6+版本 +3. **手势识别不准确**: 调整阈值参数或增加训练样本 + +### 调试技巧 +- 查看实时状态输出了解系统运行情况 +- 调整信号模拟器参数测试不同场景 +- 使用调试模式输出详细日志信息 + +## 贡献指南 + +欢迎提交改进建议和代码贡献： +1. Fork项目 +2. 创建特性分支 +3. 提交更改 +4. 创建Pull Request + +## 许可证 + +本项目采用MIT许可证，详见LICENSE文件。 \ No newline at end of file diff --git a/Python/__pycache__/emg_robotic_arm.cpython-312.pyc b/Python/__pycache__/emg_robotic_arm.cpython-312.pyc new file mode 100644 index 0000000..c7361cf Binary files /dev/null and b/Python/__pycache__/emg_robotic_arm.cpython-312.pyc differ diff --git a/Python/demo_emg_arm.py b/Python/demo_emg_arm.py new file mode 100644 index 0000000..b3f2bf8 --- /dev/null +++ b/Python/demo_emg_arm.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python3 +""" +EMG控制机械臂演示程序 +简化版本，用于快速演示系统功能 +""" + +import time +from emg_robotic_arm import EMGRoboticSystem, Gesture, EMGSimulator, EMGProcessor, RoboticArm + +def demo_gesture_sequence(): + """演示所有手势的机械臂动作""" + print("=== EMG控制机械臂演示程序 ===") + print("将演示所有6种手势对应的机械臂动作\n") + + # 创建系统组件 + arm = RoboticArm() + simulator = EMGSimulator() + + # 手势序列 + gestures = [ + (Gesture.REST, "休息状态 - 回到初始位置"), + (Gesture.FIST, "握拳动作 - 肩部下压，夹爪关闭"), + (Gesture.OPEN_HAND, "张手动作 - 肩部上抬，夹爪打开"), + (Gesture.POINT, "指向动作 - 基座旋转，肘部伸展"), + (Gesture.GRASP, "抓取动作 - 适度弯曲，夹爪部分关闭"), + (Gesture.WAVE, "挥手动作 - 基座左转，肩部上抬"), + ] + + print("开始演示，每个动作持续3秒...\n") + + for gesture, description in gestures: + print(f"执行手势: {description}") + + # 生成对应的EMG信号 + emg_signal = simulator.generate_signal(gesture) + print(f"EMG信号 - 幅度: {emg_signal.amplitude:.2f}, 频率: {emg_signal.frequency:.1f}Hz") + + # 执行手势动作 + arm.execute_gesture(gesture) + + # 模拟运动过程 + for _ in range(30): # 3秒，每次0.1秒 + arm.update_joints() + time.sleep(0.1) + + # 显示最终状态 + arm.print_status() + print("-" * 50) + + # 稍作停顿 + time.sleep(1) + + print("演示完成！") + +def interactive_demo(): + """交互式演示""" + print("=== 交互式EMG控制演示 ===") + print("输入手势编号来控制机械臂:") + print("0-休息, 1-握拳, 2-张手, 3-指向, 4-抓取, 5-挥手, q-退出\n") + + arm = RoboticArm() + simulator = EMGSimulator() + + gesture_map = { + '0': Gesture.REST, + '1': Gesture.FIST, + '2': Gesture.OPEN_HAND, + '3': Gesture.POINT, + '4': Gesture.GRASP, + '5': Gesture.WAVE, + } + + gesture_names = { + '0': '休息', + '1': '握拳', + '2': '张手', + '3': '指向', + '4': '抓取', + '5': '挥手', + } + + try: + while True: + user_input = input("请输入手势编号 (0-5) 或 'q' 退出: ").strip().lower() + + if user_input == 'q': + break + + if user_input in gesture_map: + gesture = gesture_map[user_input] + gesture_name = gesture_names[user_input] + + print(f"执行手势: {gesture_name}") + + # 生成EMG信号 + emg_signal = simulator.generate_signal(gesture) + print(f"模拟EMG信号 - 幅度: {emg_signal.amplitude:.2f}, 频率: {emg_signal.frequency:.1f}Hz") + + # 执行动作 + arm.execute_gesture(gesture) + + # 运动到目标位置 + print("机械臂运动中...") + for _ in range(30): + arm.update_joints() + time.sleep(0.1) + + # 显示状态 + arm.print_status() + else: + print("无效输入，请输入0-5或q") + + except KeyboardInterrupt: + print("\n演示结束") + +def signal_analysis_demo(): + """EMG信号分析演示""" + print("=== EMG信号分析演示 ===") + print("展示不同手势的EMG信号特征\n") + + simulator = EMGSimulator() + processor = EMGProcessor() + + gestures = [Gesture.REST, Gesture.FIST, Gesture.OPEN_HAND, + Gesture.POINT, Gesture.GRASP, Gesture.WAVE] + + for gesture in gestures: + print(f"手势: {gesture.value}") + + # 生成多个信号样本 + signals = [] + for _ in range(10): + signal = simulator.generate_signal(gesture) + signals.append(signal) + processor.add_signal(signal) + + # 计算统计信息 + avg_amp = sum(s.amplitude for s in signals) / len(signals) + avg_freq = sum(s.frequency for s in signals) / len(signals) + + print(f" 平均幅度: {avg_amp:.3f}") + print(f" 平均频率: {avg_freq:.1f}Hz") + + # 测试识别准确性 + recognized = processor.recognize_gesture() + print(f" 识别结果: {recognized.value}") + print(f" 识别正确: {'✓' if recognized == gesture else '✗'}") + print() + +if __name__ == "__main__": + print("EMG控制机械臂演示程序") + print("=" * 40) + print("1. 自动演示所有手势") + print("2. 交互式控制") + print("3. EMG信号分析") + print("4. 退出") + + try: + choice = input("\n请选择演示模式 (1-4): ").strip() + + if choice == '1': + demo_gesture_sequence() + elif choice == '2': + interactive_demo() + elif choice == '3': + signal_analysis_demo() + elif choice == '4': + print("退出程序") + else: + print("无效选择") + + except KeyboardInterrupt: + print("\n程序被中断") + except Exception as e: + print(f"程序错误: {e}") \ No newline at end of file diff --git a/Python/emg_robotic_arm.py b/Python/emg_robotic_arm.py new file mode 100644 index 0000000..57eb409 --- /dev/null +++ b/Python/emg_robotic_arm.py @@ -0,0 +1,371 @@ +#!/usr/bin/env python3 +""" +EMG控制机械臂系统 +使用肌电信号控制机械臂运动 +""" + +import time +import threading +import queue +import random +import math +import signal +import sys +from enum import Enum +from dataclasses import dataclass +from typing import List, Optional +# import numpy as np # Not needed for this implementation + +@dataclass +class EMGSignal: + """EMG信号数据结构""" + amplitude: float # 信号幅度 (0.0 - 1.0) + frequency: float # 信号频率 (Hz) + timestamp: float # 时间戳 + + def __init__(self, amplitude: float = 0.0, frequency: float = 0.0): + self.amplitude = amplitude + self.frequency = frequency + self.timestamp = time.time() + +@dataclass +class Joint: + """机械臂关节结构""" + id: int + current_angle: float = 0.0 # 当前角度 (度) + target_angle: float = 0.0 # 目标角度 (度) + min_angle: float = -180.0 # 最小角度限制 + max_angle: float = 180.0 # 最大角度限制 + speed: float = 30.0 # 运动速度 (度/秒) + +class Gesture(Enum): + """手势识别结果""" + REST = "休息" + FIST = "握拳" + OPEN_HAND = "张手" + POINT = "指向" + GRASP = "抓取" + WAVE = "挥手" + +class EMGProcessor: + """EMG信号处理器""" + + def __init__(self, buffer_size: int = 100): + self.signal_buffer = queue.Queue(maxsize=buffer_size) + self.buffer_lock = threading.Lock() + # 简单低通滤波器系数 + self.filter_coeffs = [0.1, 0.2, 0.4, 0.2, 0.1] + + def add_signal(self, signal: EMGSignal): + """添加EMG信号到缓冲区""" + with self.buffer_lock: + if self.signal_buffer.full(): + try: + self.signal_buffer.get_nowait() # 移除最老的信号 + except queue.Empty: + pass + self.signal_buffer.put(signal) + + def filter_signal(self, raw_data: List[float]) -> float: + """信号滤波处理""" + if len(raw_data) < len(self.filter_coeffs): + return raw_data[-1] if raw_data else 0.0 + + filtered = 0.0 + for i, coeff in enumerate(self.filter_coeffs): + filtered += coeff * raw_data[-(i+1)] + return filtered + + def recognize_gesture(self) -> Gesture: + """手势识别""" + with self.buffer_lock: + if self.signal_buffer.empty(): + return Gesture.REST + + # 获取最近的信号样本 + signals = [] + temp_queue = queue.Queue() + + # 从队列中取出信号进行分析 + while not self.signal_buffer.empty() and len(signals) < 10: + signal = self.signal_buffer.get() + signals.append(signal) + temp_queue.put(signal) + + # 将信号放回队列 + while not temp_queue.empty(): + self.signal_buffer.put(temp_queue.get()) + + if not signals: + return Gesture.REST + + # 计算平均幅度和频率 + avg_amplitude = sum(s.amplitude for s in signals) / len(signals) + avg_frequency = sum(s.frequency for s in signals) / len(signals) + + # 基于幅度和频率的手势识别 + if avg_amplitude < 0.1: + return Gesture.REST + elif avg_amplitude > 0.8 and avg_frequency > 50: + return Gesture.FIST + elif avg_amplitude > 0.6 and avg_frequency < 30: + return Gesture.OPEN_HAND + elif avg_amplitude > 0.4 and avg_frequency > 40: + return Gesture.GRASP + elif avg_amplitude > 0.3: + return Gesture.POINT + else: + return Gesture.WAVE + +class RoboticArm: + """机械臂控制系统""" + + def __init__(self): + self.joints = [ + Joint(0, 0.0, 0.0, -180, 180, 45), # 基座旋转 + Joint(1, 0.0, 0.0, -90, 90, 30), # 肩部 + Joint(2, 0.0, 0.0, -120, 120, 35), # 肘部 + Joint(3, 0.0, 0.0, -90, 90, 50), # 腕部俯仰 + Joint(4, 0.0, 0.0, -180, 180, 60), # 腕部旋转 + Joint(5, 0.0, 0.0, 0, 90, 40), # 夹爪 + ] + self.arm_lock = threading.Lock() + self.is_moving = False + + def set_joint_angle(self, joint_id: int, angle: float): + """设置关节目标角度""" + if not (0 <= joint_id < len(self.joints)): + return + + with self.arm_lock: + joint = self.joints[joint_id] + joint.target_angle = max(joint.min_angle, min(joint.max_angle, angle)) + + def execute_gesture(self, gesture: Gesture): + """执行手势对应的动作""" + gesture_actions = { + Gesture.REST: [ + (0, 0), # 基座 + (1, 0), # 肩部 + (2, 0), # 肘部 + (3, 0), # 腕部俯仰 + (4, 0), # 腕部旋转 + (5, 0), # 夹爪打开 + ], + Gesture.FIST: [ + (1, -30), # 肩部下压 + (2, 45), # 肘部弯曲 + (5, 90), # 夹爪关闭 + ], + Gesture.OPEN_HAND: [ + (1, 15), # 肩部上抬 + (2, -20), # 肘部伸展 + (5, 0), # 夹爪完全打开 + ], + Gesture.POINT: [ + (0, 30), # 基座旋转 + (1, 0), # 肩部水平 + (2, -45), # 肘部伸展 + (3, -15), # 腕部微调 + ], + Gesture.GRASP: [ + (1, -15), # 肩部轻微下压 + (2, 30), # 肘部适度弯曲 + (5, 60), # 夹爪部分关闭 + ], + Gesture.WAVE: [ + (0, -30), # 基座左转 + (1, 30), # 肩部上抬 + (4, 45), # 腕部旋转 + ], + } + + actions = gesture_actions.get(gesture, []) + for joint_id, angle in actions: + self.set_joint_angle(joint_id, angle) + + def update_joints(self): + """更新关节位置（运动控制）""" + with self.arm_lock: + any_moving = False + delta_time = 0.1 # 100ms更新间隔 + + for joint in self.joints: + angle_diff = joint.target_angle - joint.current_angle + if abs(angle_diff) > 0.5: + direction = 1.0 if angle_diff > 0 else -1.0 + movement = joint.speed * delta_time * direction + + if abs(movement) > abs(angle_diff): + joint.current_angle = joint.target_angle + else: + joint.current_angle += movement + any_moving = True + + self.is_moving = any_moving + + def print_status(self): + """打印当前状态""" + joint_names = ["基座", "肩部", "肘部", "腕俯仰", "腕旋转", "夹爪"] + + with self.arm_lock: + print("机械臂状态:") + for i, joint in enumerate(self.joints): + status = f"{joint_names[i]}: {joint.current_angle:.1f}°" + if abs(joint.current_angle - joint.target_angle) > 0.5: + status += f" -> {joint.target_angle:.1f}°" + print(status) + + print(f"运动状态: {'运动中' if self.is_moving else '静止'}") + print() + +class EMGSimulator: + """EMG信号模拟器""" + + def __init__(self): + random.seed() + + def generate_signal(self, target_gesture: Gesture = Gesture.REST) -> EMGSignal: + """生成模拟EMG信号""" + gesture_params = { + Gesture.REST: (0.05, 25.0), + Gesture.FIST: (0.9, 60.0), + Gesture.OPEN_HAND: (0.7, 25.0), + Gesture.POINT: (0.3, 35.0), + Gesture.GRASP: (0.5, 45.0), + Gesture.WAVE: (0.4, 30.0), + } + + base_amplitude, base_frequency = gesture_params[target_gesture] + + # 添加噪声和变化 + noise = random.uniform(-0.05, 0.05) + amplitude = max(0.0, min(1.0, base_amplitude + noise)) + frequency = max(10.0, min(100.0, base_frequency + noise * 10)) + + return EMGSignal(amplitude, frequency) + +class EMGRoboticSystem: + """EMG控制机械臂主系统""" + + def __init__(self): + self.processor = EMGProcessor() + self.arm = RoboticArm() + self.simulator = EMGSimulator() + self.running = False + + # 设置信号处理 + signal.signal(signal.SIGINT, self._signal_handler) + + def _signal_handler(self, signum, frame): + """信号处理器""" + print("\n正在关闭系统...") + self.stop() + sys.exit(0) + + def run(self): + """运行主系统""" + print("EMG控制机械臂系统启动...") + print("系统将模拟EMG信号并控制机械臂运动") + print("按Ctrl+C退出程序\n") + + self.running = True + + # 创建线程 + emg_thread = threading.Thread(target=self._emg_acquisition_loop) + control_thread = threading.Thread(target=self._control_loop) + motion_thread = threading.Thread(target=self._motion_loop) + + # 启动线程 + emg_thread.start() + control_thread.start() + motion_thread.start() + + # 主循环 - 演示不同手势 + gesture_sequence = [ + Gesture.REST, Gesture.FIST, Gesture.OPEN_HAND, + Gesture.POINT, Gesture.GRASP, Gesture.WAVE + ] + + current_gesture_idx = 0 + last_gesture_change = time.time() + + try: + while self.running: + # 每5秒切换一次手势进行演示 + now = time.time() + if now - last_gesture_change >= 5: + current_gesture_idx = (current_gesture_idx + 1) % len(gesture_sequence) + last_gesture_change = now + current_gesture = gesture_sequence[current_gesture_idx] + print(f"切换到手势: {current_gesture.value}") + + # 生成对应手势的EMG信号 + signal_data = self.simulator.generate_signal(gesture_sequence[current_gesture_idx]) + self.processor.add_signal(signal_data) + + time.sleep(0.1) + + except KeyboardInterrupt: + self.stop() + + # 等待线程结束 + emg_thread.join() + control_thread.join() + motion_thread.join() + + def stop(self): + """停止系统""" + self.running = False + + def _emg_acquisition_loop(self): + """EMG信号采集循环""" + while self.running: + # 在实际应用中，这里会从EMG传感器获取真实信号 + time.sleep(0.05) + + def _control_loop(self): + """控制循环""" + last_gesture = Gesture.REST + + while self.running: + # 识别当前手势 + current_gesture = self.processor.recognize_gesture() + + # 如果手势改变，执行相应动作 + if current_gesture != last_gesture: + print(f"识别手势: {current_gesture.value}") + self.arm.execute_gesture(current_gesture) + last_gesture = current_gesture + + time.sleep(0.2) + + def _motion_loop(self): + """运动控制循环""" + last_print = time.time() + + while self.running: + self.arm.update_joints() + + # 每2秒打印一次状态 + now = time.time() + if now - last_print >= 2: + self.arm.print_status() + last_print = now + + time.sleep(0.1) + +def main(): + """主函数""" + system = EMGRoboticSystem() + + try: + system.run() + except Exception as e: + print(f"系统错误: {e}") + return 1 + + return 0 + +if __name__ == "__main__": + sys.exit(main()) \ No newline at end of file diff --git a/config/emg_config.json b/config/emg_config.json new file mode 100644 index 0000000..ff5b7fe --- /dev/null +++ b/config/emg_config.json @@ -0,0 +1,107 @@ +{ + "emg_processing": { + "buffer_size": 100, + "sampling_rate": 20, + "filter_coefficients": [0.1, 0.2, 0.4, 0.2, 0.1], + "noise_threshold": 0.05 + }, + "gesture_recognition": { + "rest_threshold": 0.1, + "fist_amplitude_min": 0.8, + "fist_frequency_min": 50, + "open_hand_amplitude_min": 0.6, + "open_hand_frequency_max": 30, + "grasp_amplitude_min": 0.4, + "grasp_frequency_min": 40, + "point_amplitude_min": 0.3 + }, + "robotic_arm": { + "joints": [ + { + "id": 0, + "name": "base", + "min_angle": -180, + "max_angle": 180, + "speed": 45 + }, + { + "id": 1, + "name": "shoulder", + "min_angle": -90, + "max_angle": 90, + "speed": 30 + }, + { + "id": 2, + "name": "elbow", + "min_angle": -120, + "max_angle": 120, + "speed": 35 + }, + { + "id": 3, + "name": "wrist_pitch", + "min_angle": -90, + "max_angle": 90, + "speed": 50 + }, + { + "id": 4, + "name": "wrist_roll", + "min_angle": -180, + "max_angle": 180, + "speed": 60 + }, + { + "id": 5, + "name": "gripper", + "min_angle": 0, + "max_angle": 90, + "speed": 40 + } + ], + "update_interval": 0.1, + "safety_margin": 5.0 + }, + "gesture_actions": { + "REST": [ + {"joint_id": 0, "angle": 0}, + {"joint_id": 1, "angle": 0}, + {"joint_id": 2, "angle": 0}, + {"joint_id": 3, "angle": 0}, + {"joint_id": 4, "angle": 0}, + {"joint_id": 5, "angle": 0} + ], + "FIST": [ + {"joint_id": 1, "angle": -30}, + {"joint_id": 2, "angle": 45}, + {"joint_id": 5, "angle": 90} + ], + "OPEN_HAND": [ + {"joint_id": 1, "angle": 15}, + {"joint_id": 2, "angle": -20}, + {"joint_id": 5, "angle": 0} + ], + "POINT": [ + {"joint_id": 0, "angle": 30}, + {"joint_id": 1, "angle": 0}, + {"joint_id": 2, "angle": -45}, + {"joint_id": 3, "angle": -15} + ], + "GRASP": [ + {"joint_id": 1, "angle": -15}, + {"joint_id": 2, "angle": 30}, + {"joint_id": 5, "angle": 60} + ], + "WAVE": [ + {"joint_id": 0, "angle": -30}, + {"joint_id": 1, "angle": 30}, + {"joint_id": 4, "angle": 45} + ] + }, + "simulation": { + "noise_range": 0.05, + "demo_gesture_duration": 5.0, + "status_update_interval": 2.0 + } +} \ No newline at end of file diff --git a/tests/test_emg_system.py b/tests/test_emg_system.py new file mode 100644 index 0000000..af2fa9e --- /dev/null +++ b/tests/test_emg_system.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 +""" +Unit tests for EMG Robotic Arm System +""" + +import sys +import os +import unittest +import time + +# Add the parent directory to the path to import the modules +sys.path.append(os.path.join(os.path.dirname(__file__), '..', 'Python')) + +from emg_robotic_arm import EMGSignal, Joint, Gesture, EMGProcessor, RoboticArm, EMGSimulator + +class TestEMGSignal(unittest.TestCase): + """Test EMG Signal functionality""" + + def test_emg_signal_creation(self): + """Test EMG signal creation""" + signal = EMGSignal(0.5, 30.0) + self.assertEqual(signal.amplitude, 0.5) + self.assertEqual(signal.frequency, 30.0) + self.assertIsInstance(signal.timestamp, float) + +if __name__ == '__main__': + unittest.main()