1. RK3588平台ROS2 Humble环境搭建与机械臂驱动开发实战
在机器人开发领域,ROS2已成为事实上的标准框架。本文将详细介绍在RK3588开发板上搭建ROS2 Humble环境,并开发串口机械臂驱动的完整过程。这个项目涉及从系统配置到驱动开发的全流程,特别适合需要在嵌入式平台实现机器人控制的开发者参考。
1.1 硬件平台与基础环境
RK3588是一款高性能的ARM架构处理器,采用四核Cortex-A76和四核Cortex-A55设计,主频可达2.4GHz。我们使用的开发板运行Ubuntu 22.04 LTS操作系统,这是ROS2 Humble官方支持的平台。
在开始前,建议先检查基础环境:
bash复制lsb_release -a # 确认系统版本为Ubuntu 22.04
uname -m # 确认架构为aarch64
1.2 ROS2 Humble安装流程
由于国内网络环境特殊,我们需要先解决GitHub资源访问问题。编辑/etc/hosts文件,添加以下内容:
code复制185.199.108.133 raw.githubusercontent.com
185.199.109.133 raw.githubusercontent.com
185.199.110.133 raw.githubusercontent.com
185.199.111.133 raw.githubusercontent.com
这些IP地址可能会变化,如果后续安装失败,需要重新查询更新。
1.2.1 基础环境配置
首先设置locale,确保系统使用UTF-8编码:
bash复制sudo apt update
sudo apt install -y locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8
1.2.2 添加ROS2软件源
安装必要的工具并添加ROS2软件源:
bash复制sudo apt install -y software-properties-common curl gnupg lsb-release
sudo add-apt-repository universe
# 添加ROS2 GPG key
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-o /usr/share/keyrings/ros-archive-keyring.gpg
# 添加软件源
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | \
sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update
1.2.3 安装ROS2 Humble
ROS2 Humble提供两种安装选项:
- 完整桌面版(包含GUI工具):
bash复制sudo apt install -y ros-humble-desktop
- 轻量基础版(仅核心功能):
bash复制sudo apt install -y ros-humble-ros-base
对于机械臂控制开发,建议安装桌面版,以便使用RViz2等可视化工具。
1.2.4 环境配置与验证
设置环境变量,使其在每次打开终端时自动生效:
bash复制echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc
验证安装是否成功:
bash复制ros2 topic list # 应该能看到默认的主题列表
安装常用开发工具:
bash复制sudo apt install -y \
python3-colcon-common-extensions \
python3-rosdep \
python3-vcstool \
python3-argcomplete \
build-essential \
cmake \
git
# 初始化rosdep
sudo rosdep init
rosdep update
运行简单demo验证通信功能:
bash复制# 终端1
ros2 run demo_nodes_cpp talker
# 终端2
ros2 run demo_nodes_cpp listener
如果两个终端能正常通信,说明ROS2环境配置成功。
2. 机械臂串口驱动设计与实现
2.1 系统架构与数据流设计
机械臂控制系统需要实现ROS2与串口协议之间的稳定映射,主要考虑以下需求:
- 防止串口数据粘包/多帧导致的解析错误
- 避免运动控制命令重复发送刷爆控制器
- 处理键盘/手柄等非实时输入的平滑控制
- 查询状态时不干扰运动控制
系统数据流设计如下:
-
驱动节点(robot_serial_driver):
- ROS → 串口:
/robot/ee_target(PoseStamped) →@X,Y,Z,A,B,C\r\n/robot/joint_target(JointState) →&x,y,z,a,b,c\r\n/robot/start|stop|home|reset|disable(Trigger) →!COMMAND\r\n
- 串口 → ROS:
#GETLPOS返回 →/robot/ee_pose#GETJPOS返回 →/robot/joint_states
- 调试接口:
- 串口原始数据 →
/robot/driver_rx - 发送数据 →
/robot/driver_tx
- 串口原始数据 →
- ROS → 串口:
-
键盘节点(ee_keyboard_teleop_xyz):
- 订阅
/robot/ee_pose获取当前位置 - 根据按键输入生成连续目标
- 发布
/robot/ee_target
- 订阅
2.2 工程结构与编译运行
工程目录结构如下:
code复制ws_ROBOT/
src/robot_serial_driver/
src/robot_serial_driver_node.cpp
src/ee_keyboard_teleop_xyz.cpp
CMakeLists.txt
package.xml
build/
install/
log/
编译命令:
bash复制colcon build --packages-select robot_serial_driver
source install/setup.bash
查找串口设备:
bash复制ls -l /dev/ttyUSB* /dev/ttyACM* 2>/dev/null
启动驱动节点(需替换为实际串口设备):
bash复制ros2 run robot_serial_driver robot_serial_driver_node --ros-args \
-p port:=/dev/ttyACM1 -p baud:=115200
启动键盘控制节点:
bash复制ros2 run robot_serial_driver ee_keyboard_teleop_xyz_node
发送系统命令示例(禁用机械臂):
bash复制ros2 service call /robot/disable std_srvs/srv/Trigger "{}"
2.3 驱动节点核心实现
2.3.1 节点初始化
驱动节点启动时完成以下工作:
- 读取参数配置:
cpp复制cmd_timeout_s_ = declare_parameter<double>("cmd_timeout_s", 0.5);
port_ = declare_parameter<std::string>("port", "/dev/ttyACM0");
baud_ = declare_parameter<int>("baud", 115200);
poll_hz_ = declare_parameter<double>("poll_hz", 10.0);
pause_poll_while_sending_ = declare_parameter<bool>("pause_poll_while_sending", true);
send_check_hz_ = declare_parameter<double>("send_check_hz", 200.0);
min_send_interval_ms_ = declare_parameter<int>("min_send_interval_ms", 25);
min_free_size_to_send_ = declare_parameter<int>("min_free_size_to_send", 1);
suppress_poll_while_moving_ms_ = declare_parameter<int>("suppress_poll_while_moving_ms", 300);
frame_id_ = declare_parameter<std::string>("frame_id", "base_link");
- 创建ROS接口:
cpp复制// 发布器
rx_pub_ = create_publisher<std_msgs::msg::String>("/robot/driver_rx", 10);
tx_pub_ = create_publisher<std_msgs::msg::String>("/robot/driver_tx", 10);
auto pose_qos = rclcpp::QoS(rclcpp::KeepLast(5)).reliable();
ee_pose_pub_ = create_publisher<geometry_msgs::msg::PoseStamped>("/robot/ee_pose", pose_qos);
joint_pub_ = create_publisher<sensor_msgs::msg::JointState>("/robot/joint_states", 10);
// 订阅器
ee_target_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
"/robot/ee_target", rclcpp::QoS(10),
std::bind(&RobotSerialDriver::onEeTarget, this, _1));
joint_target_sub_ = create_subscription<sensor_msgs::msg::JointState>(
"/robot/joint_target", rclcpp::QoS(10),
std::bind(&RobotSerialDriver::onJointTarget, this, _1));
// 服务
srv_disable_ = makeTriggerService("/robot/disable", "!DISABLE");
- 设置定时器:
cpp复制poll_timer_ = create_wall_timer(1.0/poll_hz_, bind(&RobotSerialDriver::pollTick, this));
send_timer_ = create_wall_timer(1.0/send_check_hz_, bind(&RobotSerialDriver::sendLatestEeTargetIfDue, this));
- 打开串口并启动接收线程:
cpp复制openSerialOrThrow();
running_.store(true);
rx_thread_ = std::thread([this]{ rxLoop(); });
2.3.2 串口通信实现
串口接收采用异步IO模型,使用Boost.Asio库实现。关键接收逻辑如下:
cpp复制void rxLoop() {
asio::streambuf buf;
while (running_.load()) {
asio::read_until(serial_, buf, "\r\n", ec);
auto begin = asio::buffers_begin(buf.data());
auto end = asio::buffers_end(buf.data());
while (true) {
auto it = std::search(begin, end, "\r\n", "\r\n"+2);
if (it == end) break;
std::string line(begin, it);
buf.consume(std::distance(begin,it)+2);
line = trim_crlf(line);
if (line.empty()) continue;
rx_pub_->publish(line); // 发布原始接收数据
handleLine(line); // 处理协议数据
}
}
}
这种实现方式有效解决了串口数据粘包问题,确保每帧数据都能被正确解析。
2.3.3 状态查询状态机
为避免并发查询导致的数据混乱,采用状态机串行化查询请求:
cpp复制enum class PollState { WANT_LPOS, WAIT_LPOS, WANT_JPOS, WAIT_JPOS };
void pollTick() {
if (pause_poll_while_sending_ && sending_motion_) return;
if (shouldSuppressPoll()) return;
if (poll_state_ == WANT_LPOS) {
last_query_ = LPOS;
if (sendRaw("#GETLPOS\r\n")) poll_state_ = WAIT_LPOS;
return;
}
if (poll_state_ == WANT_JPOS) {
last_query_ = JPOS;
if (sendRaw("#GETJPOS\r\n")) poll_state_ = WAIT_JPOS;
return;
}
}
void onOkForLastQuery() {
if (poll_state_ == WAIT_LPOS) poll_state_ = WANT_JPOS;
else if (poll_state_ == WAIT_JPOS) poll_state_ = WANT_LPOS;
}
这种设计保证了同一时刻只有一个查询在传输中,避免了返回数据与查询不匹配的问题。
2.3.4 运动控制实现
运动控制采用缓存+闸门机制,避免命令刷屏:
cpp复制void onEeTarget(...) {
latest_ee_target_ = *msg;
last_target_time_ = now;
have_target_ = true;
target_dirty_ = true;
}
void sendLatestEeTargetIfDue() {
if (age_s > cmd_timeout_s_) {
have_target_ = false;
target_dirty_ = false;
return;
}
if (!target_dirty_) return;
if (elapsed_ms < min_send_interval_ms_) return;
if (fs >= 0 && fs < min_free_size_to_send_) return;
quatToRPY(q, roll, pitch, yaw);
A_deg = yaw*180/pi; B_deg = pitch*180/pi; C_deg = roll*180/pi;
sendRaw("@X,Y,Z,A,B,C\r\n");
target_dirty_ = false;
}
这种机制确保了:
- 超时无新目标自动停止发送
- 只发送新目标,不重复发送相同目标
- 发送频率受控,不超过设定值
- 串口缓冲区空间不足时暂停发送
2.4 键盘遥操作节点实现
键盘节点主要功能是将按键输入转换为平滑的运动控制。核心实现包括:
2.4.1 初始化与状态管理
节点启动后进入termios raw模式实现非阻塞读键,并等待接收到第一个位姿反馈:
cpp复制if (!have_pose_) {
have_pose_ = true;
start_ = msg;
locked_orientation_ = msg.pose.orientation;
have_locked_orientation_ = true;
off_x_mm_ = off_y_mm_ = off_z_mm_ = 0.0;
}
2.4.2 运动逻辑实现
运动控制基于"起始点+偏移量"模型:
cpp复制// 计算目标位置
target = start_ + off_/1000.0;
// 锁定姿态
if (have_locked_orientation_) {
target.pose.orientation = locked_orientation_;
}
2.4.3 防抖机制
为防止按键抖动导致运动不连续,实现了idle防抖机制:
cpp复制if (moving) {
last_moving_time_ = now;
if (!in_motion) { start_ = cur_; off=0; in_motion=true; }
} else {
idle_ms = ...;
if (in_motion && idle_ms < idle_reset_delay_ms_) {
// 保持start/off不变,维持运动连续性
} else {
start_ = cur_; off=0; in_motion=false;
if (!send_when_idle_) return;
}
}
这种设计有效解决了方向切换时的目标跳变问题。
3. 机械臂串口协议详解
3.1 协议命令格式
机械臂控制器使用基于文本的串口协议,主要命令包括:
| 命令前缀 | 功能描述 | 示例 |
|---|---|---|
! |
系统控制命令 | !START, !STOP, !HOME |
# |
查询与参数设置 | #GETJPOS, #GETLPOS |
@ |
笛卡尔空间运动 | @X,Y,Z,A,B,C |
& |
关节空间运动 | &j1,j2,j3,j4,j5,j6 |
3.2 数据格式与单位
- 位置单位:毫米(mm)
- 角度单位:度(deg)
- 欧拉角顺序:ZYX (Yaw-Pitch-Roll)
- A = yaw (绕Z轴)
- B = pitch (绕Y轴)
- C = roll (绕X轴)
- 行结束符:
\r\n
3.3 查询返回格式
-
关节位置查询返回:
code复制ok j1 j2 j3 j4 j5 j6其中j1-j6为各关节角度(deg)
-
末端位置查询返回:
code复制ok X Y Z A B C其中X/Y/Z为位置(mm),A/B/C为欧拉角(deg)
4. 开发中的问题与解决方案
4.1 串口数据粘包问题
现象:接收到的数据中偶尔出现多帧内容粘连在同一消息中。
原因:串口是字节流传输,一次读取可能包含多行数据,如果不正确处理缓冲区会导致数据粘连。
解决方案:
cpp复制asio::read_until(serial_, buf, "\r\n", ec);
while (true) {
auto it = std::search(begin, end, "\r\n", "\r\n"+2);
if (it == end) break;
std::string line(begin, it);
buf.consume(distance(begin,it)+2);
line = trim_crlf(line);
if (line.empty()) continue;
rx_pub_->publish(line);
handleLine(line);
}
这种实现确保即使一次读取包含多帧数据,也能正确分割处理每帧。
4.2 运动控制目标回弹问题
现象:连续运动时目标位置会突然跳回之前的值再继续。
原因:键盘遥操作节点的运动参考系(start_)在检测到无按键输入时会立即重置为当前位置(cur_),而当前位置反馈可能有延迟。
解决方案:引入防抖机制,短暂无输入时不立即重置参考系:
cpp复制if (in_motion && idle_ms < idle_reset_delay_ms_) {
// 保持start/off不变,维持连续性
} else {
start_ = cur_; off=0; in_motion=false;
}
4.3 状态查询干扰运动控制
现象:状态查询(#GETLPOS/#GETJPOS)会打断运动命令的连续发送,导致运动不流畅。
解决方案:
- 运动期间暂停状态查询:
cpp复制if (pause_poll_while_sending_ && sending_motion_.load()) return;
- 运动后短暂抑制查询:
cpp复制const double age_ms = (now - last_motion_send_time_).seconds() * 1000.0;
return age_ms < suppress_poll_while_moving_ms_;
4.4 流控信息丢失问题
现象:运动命令后的15ok响应未被正确解析,导致流控失效。
解决方案:增强协议解析器,支持多种响应格式:
cpp复制// 处理"15ok"格式的流控响应
if (auto sp = split_int_prefix(line)) {
if (is_integer_string(sp->first) && sp->second == "ok") {
last_free_size_.store(std::stoi(sp->first));
return;
}
}
5. 调试技巧与常用命令
开发过程中,以下命令对调试非常有帮助:
- 监控串口原始数据:
bash复制ros2 topic echo /robot/driver_tx # 发送数据
ros2 topic echo /robot/driver_rx # 接收数据
- 查看机械臂状态:
bash复制ros2 topic echo /robot/ee_pose # 末端位姿
ros2 topic echo /robot/joint_states # 关节状态
- 查询可用服务:
bash复制ros2 service list | grep /robot
- 调用服务示例(禁用机械臂):
bash复制ros2 service call /robot/disable std_srvs/srv/Trigger "{}"
- 查看节点列表:
bash复制ros2 node list
- 查看节点信息:
bash复制ros2 node info /robot_serial_driver
6. 项目改进方向
当前实现虽然功能完整,但仍有一些需要改进的地方:
6.1 消息语义重构
当前使用JointState消息承载&x,y,z,a,b,c命令不够直观,建议:
- 定义专用消息类型
CartesianTarget,明确表示笛卡尔空间目标 - 或者完全废弃
&命令,统一使用@命令
6.2 单位转换集中管理
当前单位转换分散在代码各处:
- LPOS: mm→m, deg→rad
- JPOS: 保持deg不变
建议在协议层统一管理单位转换,避免混淆。
6.3 代码结构优化
当前驱动节点功能集中,建议拆分为:
SerialTransport: 负责底层串口通信ProtocolCodec: 协议编解码PollScheduler: 状态查询调度RosBridge: ROS接口适配
这种分层设计将提高代码的可维护性和可测试性。
6.4 数据帧对齐优化
当前版本存在数据帧对齐问题,下个版本计划:
- 引入更健壮的帧同步机制
- 优化数据包解析算法
- 减少临时对象创建,提高性能
7. 实际开发经验分享
在开发过程中积累了一些有价值的经验:
-
串口通信稳定性:嵌入式系统中,串口通信容易受到干扰。除了本文提到的粘包处理,还应考虑:
- 增加CRC校验确保数据完整性
- 实现重传机制应对传输错误
- 添加心跳检测监控连接状态
-
实时性权衡:在资源受限的嵌入式平台上,需要平衡实时性和系统负载:
- 运动控制需要高优先级
- 状态查询可以适当降低频率
- 使用QoS配置管理数据流优先级
-
调试技巧:
- 使用
rqt_graph可视化节点拓扑 - 利用
ros2 bag记录和回放数据 - 开发模拟节点替代实际硬件加速调试
- 使用
-
性能优化:
- 减少动态内存分配
- 使用零拷贝机制传递大数据
- 合理设置缓冲区大小
-
安全考虑:
- 实现急停处理机制
- 添加运动范围限制
- 设计故障恢复流程
这个项目从环境搭建到功能实现涉及多个技术领域,包括嵌入式Linux、ROS2、串口通信、机器人运动控制等。通过不断迭代和问题解决,最终建立了一个稳定可靠的机械臂控制系统。希望这些经验能对类似项目的开发者有所帮助。
