1. ROS与达妙机械臂概述
达妙机械臂作为国产六轴协作机械臂的代表性产品,在工业分拣、实验室自动化等场景应用广泛。其ROS驱动包dm_ros提供了完整的MoveIt集成和硬件接口,其中DmHW.cpp作为核心硬件通信模块,承担着机械臂状态获取和指令下发的关键职能。
在实际部署中,我们发现该文件实现了以下核心功能:
- 通过自定义串口协议与机械臂控制器实时通信(波特率115200)
- 解析关节角度、力矩、错误码等实时状态数据
- 将MoveIt生成的轨迹点序列转换为机械臂识别的运动指令
- 实现急停、回零等安全控制功能
典型应用场景包括:
- 实验室环境下与MoveIt配合完成抓取演示
- 生产线上的视觉引导定位装配
- 配合力传感器实现自适应打磨
2. DmHW.cpp架构解析
2.1 类成员设计要点
该文件定义了DmHW类继承自hardware_interface::RobotHW,主要包含:
cpp复制class DmHW : public hardware_interface::RobotHW {
private:
serial::Serial port_; // 串口对象
vector<double> joint_position_; // 6个关节角度(弧度)
vector<double> joint_velocity_;
vector<double> joint_effort_;
vector<double> joint_position_command_; // 目标位置
hardware_interface::JointStateInterface jnt_state_interface_;
hardware_interface::PositionJointInterface jnt_pos_interface_;
};
关键设计特点:
- 采用双缓冲机制:实时状态与指令分离存储
- 接口注册方式符合ROS Control标准
- 串口超时设置为100ms(经验值)
2.2 通信协议逆向分析
通过抓包分析,机械臂通信协议主要特征:
code复制帧格式:0xAA 0x55 [长度] [命令字] [数据] [校验和]
典型指令:
- 0x01:读取关节状态(返回12字节×6关节)
- 0x02:下发目标位置(需转换为机械臂内部单位)
- 0xEE:急停指令
数据转换示例:
cpp复制// 弧度值转机械臂单位(0.001度)
uint16_t radToUnit(double rad) {
return static_cast<uint16_t>(rad * 180000 / M_PI);
}
3. 核心函数实现详解
3.1 init()函数启动流程
cpp复制bool DmHW::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) {
// 参数读取
std::string port;
robot_hw_nh.param("port_name", port, std::string("/dev/ttyUSB0"));
// 串口初始化
try {
port_.setPort(port);
port_.setBaudrate(115200);
serial::Timeout to = serial::Timeout::simpleTimeout(100);
port_.setTimeout(to);
port_.open();
} catch (...) {
ROS_ERROR("Port init failed!");
return false;
}
// 注册接口
for(int i=0; i<6; ++i) {
std::stringstream ss;
ss << "joint_" << i+1;
hardware_interface::JointStateHandle state_handle(
ss.str(), &joint_position_[i], &joint_velocity_[i], &joint_effort_[i]);
jnt_state_interface_.registerHandle(state_handle);
hardware_interface::JointHandle pos_handle(
jnt_state_interface_.getHandle(ss.str()), &joint_position_command_[i]);
jnt_pos_interface_.registerHandle(pos_handle);
}
registerInterface(&jnt_state_interface_);
registerInterface(&jnt_pos_interface_);
return true;
}
关键注意事项:
- 串口设备默认权限问题需通过udev规则解决
- 机械臂上电后需要500ms初始化时间
- 关节命名需与URDF严格一致
3.2 read()函数状态获取
cpp复制void DmHW::read(const ros::Time& time, const ros::Duration& period) {
uint8_t cmd[] = {0xAA, 0x55, 0x01, 0x01, 0x00, 0x00};
port_.write(cmd, 6);
uint8_t buf[128];
size_t len = port_.read(buf, sizeof(buf));
if(len == 38 && buf[0]==0xAA && buf[1]==0x55) { // 状态帧校验
for(int i=0; i<6; ++i) {
uint16_t val = (buf[4+i*2]<<8) | buf[5+i*2];
joint_position_[i] = static_cast<double>(val) * M_PI / 180000.0;
// 速度和力矩解析类似...
}
} else {
ROS_WARN_THROTTLE(1, "Invalid frame received!");
}
}
常见问题处理:
- 数据不完整:增加重试机制(最多3次)
- 校验失败:检查接地是否良好
- 数据跳变:添加低通滤波
3.3 write()函数指令下发
cpp复制void DmHW::write(const ros::Time& time, const ros::Duration& period) {
uint8_t cmd[20] = {0xAA, 0x55, 0x10, 0x02};
for(int i=0; i<6; ++i) {
uint16_t pos = radToUnit(joint_position_command_[i]);
cmd[4+i*2] = (pos >> 8) & 0xFF;
cmd[5+i*2] = pos & 0xFF;
}
// 计算校验和
uint8_t sum = 0;
for(int i=2; i<16; ++i) sum += cmd[i];
cmd[16] = sum;
port_.write(cmd, 17);
}
运动控制经验:
- 指令间隔建议≥20ms(对应50Hz控制频率)
- 大范围运动时需做梯形速度规划
- 遇到EStop时立即发送0xEE指令
4. 深度优化实践
4.1 实时性优化技巧
通过示波器测量发现原始实现存在约15ms延迟,优化措施包括:
- 使用ROS实时线程(需要打RT补丁)
cpp复制#include <ros/ros.h>
#include <sched.h>
void setRealtimePriority() {
struct sched_param param;
param.sched_priority = sched_get_priority_max(SCHED_FIFO);
pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m);
}
- 将串口读写分离到独立线程
- 禁用USB自动挂起功能
code复制echo -1 > /sys/module/usbcore/parameters/autosuspend
4.2 安全增强方案
实际部署中我们增加了:
- 软件限位保护
cpp复制bool DmHW::checkJointLimits() {
const static double limits[6] = {3.14, 2.09, 2.79, 3.14, 2.09, 3.14};
for(int i=0; i<6; ++i) {
if(fabs(joint_position_command_[i]) > limits[i]) {
ROS_ERROR("Joint %d over limit!", i+1);
sendEmergencyStop();
return false;
}
}
return true;
}
- 心跳包监测(3秒超时触发急停)
- 增加硬件看门狗电路
4.3 调试工具集
开发过程中使用的实用工具:
- 串口监控工具
bash复制sudo apt install cutecom
cutecom -device /dev/ttyUSB0 -baud 115200
- 实时曲线绘制
python复制#!/usr/bin/env python
import rospy
from sensor_msgs.msg import JointState
import matplotlib.pyplot as plt
def callback(data):
plt.clf()
plt.plot(data.position, 'r-')
plt.ylim(-3.14, 3.14)
plt.draw()
plt.pause(0.001)
rospy.init_node('joint_plotter')
rospy.Subscriber("/joint_states", JointState, callback)
plt.show()
5. 典型问题排查指南
5.1 通信故障排查流程
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 完全无响应 | 1. 电源未接通 2. 串口线序错误 |
1. 检查电源指示灯 2. 用万用表测量TX/RX电压 |
| 数据乱码 | 波特率不匹配 | 确认双方均为115200bps |
| 偶发丢包 | 电磁干扰 | 更换带屏蔽层的串口线 |
5.2 运动控制异常处理
案例1:机械臂剧烈抖动
- 检查步骤:
- 观察joint_states话题数据是否跳变
- 用rostopic hz检查控制频率
- 检查机械臂减速器是否损坏
- 根本原因:控制频率不稳定(有时低于30Hz)
- 修复方案:改用real_time_tools提供的定时器
案例2:末端偏移5mm
- 校准流程:
- 使用激光跟踪仪测量实际位置
- 修改URDF中的DH参数
- 重新标定工具坐标系
5.3 性能优化记录
通过以下改进将控制延迟从23ms降至8ms:
- 将串口读取改为非阻塞模式
cpp复制port_.setTimeout(serial::Timeout::max(), 5, 0, 5, 0);
- 预分配通信缓冲区
- 禁用ROS调试输出
cpp复制if(ros::console::set_logger_level(
ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn)) {
ros::console::notifyLoggerLevelsChanged();
}
6. 进阶开发方向
对于需要更高性能的场景,可以考虑:
- 改用EtherCAT通信(需硬件改造)
- 增加FPGA做硬件级轨迹插补
- 集成在线动力学补偿算法
在实验室环境中,我们成功实现了1kHz的控制频率,关键改动包括:
- 替换Linux内核为Xenomai实时系统
- 开发专用内核模块处理GPIO中断
- 使用内存映射方式直接操作串口控制器寄存器