达妙三轴机械臂控制代码解析与实现-嵌云网-嵌入式AI开发资源站

达妙三轴机械臂控制代码解析与实现

狸花实验室

1. 达妙三轴机械臂控制代码深度解析

作为一名长期从事机器人控制系统开发的工程师，今天我想分享一个工业级机械臂控制系统的核心实现——damiao.cpp。这个代码模块是达妙三轴机械臂的运动控制中枢，负责与底层电机驱动器的实时通信和控制。不同于教科书上的示例代码，这套系统已经在多个实际工业场景中验证过其稳定性和可靠性。

1.1 系统架构概览

damiao.cpp本质上是一个连接上层运动规划算法和底层电机硬件的桥梁。它主要实现三大功能：

电机参数管理和状态监控
CAN总线通信协议处理
多种控制模式实现（MIT模式、位置-速度模式等）

这个系统最巧妙的设计在于，它将所有电机参数和控制指令都封装在统一的数据结构中，通过哈希表实现快速访问，同时利用多线程实现实时控制和非阻塞通信。

2. 核心数据结构设计

2.1 电机参数限制结构体

cpp复制Limit_param limit_param[Num_Of_Motor]=
{
    {12.5, 30, 10 }, // DM4310
    {12.5, 50, 10 }, // DM4310_48V
    // ...其他电机型号
};

这个结构体数组定义了不同型号电机的物理极限参数，包含三个关键值：

位置极限(Q_MAX)：单位通常是弧度，表示电机转子的最大旋转角度
速度极限(DQ_MAX)：单位rad/s，防止电机超速运行
扭矩极限(TAU_MAX)：单位Nm，保护电机不过载

在实际应用中，这些参数需要根据具体机械结构进行调整。比如在机械臂末端负载较大时，需要适当降低扭矩限制以防止机械损伤。

2.2 电机类设计

cpp复制Motor::Motor(DM_Motor_Type Motor_Type, Motor_id Slave_id, Motor_id Master_id)
: Master_id(Master_id), Slave_id(Slave_id), Motor_Type(Motor_Type) 
{
   this->limit_param = damiao::limit_param[Motor_Type];
}

Motor类是系统的核心，每个电机对应一个实例。构造函数接收三个关键参数：

Motor_Type：电机型号，用于索引limit_param数组
Slave_id：电机的CAN总线ID
Master_id：主控板ID（在多主控系统中使用）

这个设计使得我们可以方便地管理多个电机实例，每个实例都保存着自己的状态和参数。

3. 通信协议实现

3.1 数据收发机制

cpp复制void Motor::receive_data(float q, float dq, float tau)
{
    this->state_q = q;
    this->state_dq = dq;
    this->state_tau = tau;
}

这个函数是电机状态更新的入口。当底层CAN通信解析出新的电机状态数据时，会调用此函数更新电机内部状态。三个参数分别代表：

q：当前位置（rad）
dq：当前速度（rad/s）
tau：当前扭矩（Nm）

在实际应用中，这个函数的调用频率决定了控制系统的带宽。通常我们会设置为1kHz以上。

3.2 参数存储设计

cpp复制union ValueUnion {
    float floatValue;
    uint32_t uint32Value;
};

struct ValueType {
    ValueUnion value;
    bool isFloat;
};

void Motor::set_param(int key, float value)
{
    ValueType v{};
    v.value.floatValue = value;
    v.isFloat = true;
    param_map[key] = v;
}

这个设计巧妙地利用了C++的联合体(union)特性，实现了既能存储浮点数又能存储整数的通用参数容器。关键点在于：

ValueUnion允许同一块内存以不同数据类型解释
isFloat标志位指示当前存储的数据类型
param_map使用哈希表实现快速参数存取

这种设计在机器人控制系统中很常见，因为我们需要处理各种类型的参数（有些是浮点数如PID参数，有些是整数如电机ID）。

4. 硬件驱动初始化

4.1 串口初始化

cpp复制Motor_Control::Motor_Control(std::string serial_port, int seial_baud, 
    std::unordered_map<int, DmActData>* data_ptr)
    : data_ptr_(data_ptr) 
{
    // 串口配置
    serial_.setPort(serial_port);
    serial_.setBaudrate(seial_baud);
    serial_.setFlowcontrol(serial::flowcontrol_none);
    serial_.setParity(serial::parity_none);
    serial_.setStopbits(serial::stopbits_one);
    serial_.setBytesize(serial::eightbits);
    serial_.open();
    
    usleep(1000000); // 等待1秒确保稳定
    enable(); // 使能所有电机
}

串口初始化的几个关键点：

波特率通常设置为921600，这是达妙电机驱动器的推荐值
无流控和无校验位的配置简化了硬件连接
打开串口后的1秒延时很重要，给USB转CAN模块足够的启动时间
enable()会发送使能指令，让电机进入准备状态

在实际部署中，这个初始化过程需要加入重试机制，因为硬件可能需要更长时间准备。

4.2 多线程数据接收

cpp复制rec_thread = std::thread(boost::bind(&Motor_Control::get_motor_data_thread, this));

这里创建了一个专用线程来持续接收电机状态数据。这种设计的好处是：

避免阻塞主控制循环
确保状态更新及时性
提高系统响应速度

线程函数get_motor_data_thread内部是一个while循环，持续读取串口数据并解析。我们会在后面详细分析这个函数。

5. 控制模式实现

5.1 MIT控制模式

cpp复制void Motor_Control::control_mit(Motor &DM_Motor, float kp, float kd, 
    float q, float dq, float tau)
{
    // 参数压缩
    uint16_t q_uint = float_to_uint(q, -limit_param_cmd.Q_MAX, limit_param_cmd.Q_MAX, 16);
    uint16_t dq_uint = float_to_uint(dq, -limit_param_cmd.DQ_MAX, limit_param_cmd.DQ_MAX, 12);
    // ...其他参数压缩
    
    // 数据打包
    std::array<uint8_t, 8> data_buf{};
    data_buf[0] = (q_uint >> 8) & 0xff;
    data_buf[1] = q_uint & 0xff;
    // ...其他位操作
    
    // 发送数据
    send_data.modify(id, data_buf.data());
    serial_.write((uint8_t*)&send_data, sizeof(can_send_frame));
}

MIT控制模式是阻抗控制的一种实现，特点是将位置、速度、力矩指令以及刚度(kp)、阻尼(kd)参数一起发送给电机。这个函数的关键技术点：

参数压缩：将浮点数压缩为固定位宽的整数，充分利用CAN帧的8字节限制
位操作：巧妙利用移位和位或运算将多个参数打包到一个字节中
数据发送：通过修改send_data结构体并写入串口完成发送

这种控制模式特别适合需要柔顺控制的场景，比如机器人与环境交互时。

5.2 位置-速度模式

cpp复制void Motor_Control::control_pos_vel(Motor &DM_Motor, float pos, float vel)
{
    std::array<uint8_t, 8> data_buf{};
    memcpy(data_buf.data(), &pos, sizeof(float));
    memcpy(data_buf.data() + 4, &vel, sizeof(float));
    // ...发送数据
}

位置-速度模式相对简单，直接使用memcpy将浮点数拷贝到发送缓冲区。这种模式适用于：

点到点运动
轨迹跟踪
需要精确位置控制的场景

6. 参数管理与状态监控

6.1 参数读写机制

cpp复制float Motor_Control::read_motor_param(Motor &DM_Motor, uint8_t RID)
{
    // 发送读取请求
    std::array<uint8_t, 8> data_buf{can_low, can_high, 0x33, RID, 0x00, 0x00, 0x00, 0x00};
    send_data.modify(0x7FF, data_buf.data());
    serial_.write((uint8_t*)&send_data, sizeof(can_send_frame));
    
    // 等待并解析响应
    for(uint8_t i = 0; i < max_retries; i++) {
        usleep(retry_interval);
        receive_param();
        if (motors[DM_Motor.GetSlaveId()]->is_have_param(RID)) {
            return motors[DM_Motor.GetSlaveId()]->get_param_as_float(RID);
        }
    }
    return 0;
}

参数读取的完整流程：

发送读取请求（0x33表示读操作）
等待电机响应（通常需要几毫秒）
检查参数是否已更新
返回参数值或超时处理

这个机制确保了参数读写的可靠性，在实际应用中，retry_interval通常设置为5ms，max_retries设置为5-10次。

6.2 状态监控线程

cpp复制void Motor_Control::get_motor_data_thread() 
{      
    while (!stop_thread_) {  
        size_t bytes_read = serial_.read((uint8_t*)&receive_data, sizeof(CAN_Receive_Frame));
        if(bytes_read > 0 && receive_data.CMD == 0x11 && receive_data.frameEnd == 0x55) {
            // 解析数据
            uint16_t q_uint = (uint16_t(data[1]) << 8) | data[2];
            // ...其他参数解析
            
            // 更新电机状态
            float receive_q = uint_to_float(q_uint, -limit_param_receive.Q_MAX, limit_param_receive.Q_MAX, 16);
            m->receive_data(receive_q, receive_dq, receive_tau);      
        }
    }
}

状态监控线程的核心工作：

持续读取串口数据
校验数据完整性（CMD和frameEnd）
解析原始数据（位操作逆过程）
将整数转换回浮点数
更新电机状态

这个线程的运行频率决定了状态更新的实时性，通常能达到1kHz以上。

7. 实际应用中的经验分享

7.1 控制时序优化

在实际部署中，我们发现控制时序对系统性能影响很大。以下是几个关键经验：

控制周期要稳定：使用高精度定时器确保控制周期抖动小于10us
指令间隔要合理：CAN指令之间需要350us间隔，防止总线拥堵
状态更新要及时：确保状态反馈延迟小于1个控制周期

cpp复制void Motor_Control::write()
{
    for(const auto& m : *data_ptr_) {
        control_mit(*it, m.second.kp, m.second.kd, m.second.cmd_pos, m.second.cmd_vel, m.second.cmd_effort);
        usleep(350); // 关键延时
    }
}

7.2 异常处理机制

工业环境中，通信异常很常见。我们实现了多级保护：

串口异常捕获：通过try-catch块处理硬件异常
超时重试机制：重要操作都有重试次数限制
状态校验：写入参数后主动读取验证

cpp复制try {
    size_t bytes_read = serial_.read((uint8_t*)&receive_data, sizeof(CAN_Receive_Frame));
} catch (const serial::SerialException& e) {
    std::cerr << "Serial exception: " << e.what() << std::endl;
    usleep(100000); // 出错后等待100ms再重试
}

7.3 性能优化技巧

经过多次优化，我们发现以下技巧能显著提升性能：

使用unordered_map代替map：参数查询速度提升3倍
预分配内存：避免实时控制中的内存分配
减少系统调用：批量处理CAN指令
使用内存拷贝代替逐字节赋值

cpp复制// 好的做法：使用memcpy批量处理
std::array<uint8_t, 8> data_buf{};
memcpy(data_buf.data(), &pos, sizeof(float));
memcpy(data_buf.data() + 4, &vel, sizeof(float));

// 不好的做法：逐字节赋值
data_buf[0] = ((uint8_t*)&pos)[0];
data_buf[1] = ((uint8_t*)&pos)[1];
// ...

这套达妙机械臂控制系统已经在多个工业场景中得到验证，包括装配、焊接和搬运等应用。它的核心价值在于将复杂的机器人控制抽象为简洁的API，同时保持了足够的灵活性和可靠性。对于想要深入理解机器人底层控制的开发者来说，这个代码库是一个很好的学习资源。