从传感器数据到应用：手把手教你用ROS Noetic读取并处理UR5+FT300的力/力矩信息

从传感器数据到应用：手把手教你用ROS Noetic读取并处理UR5+FT300的力/力矩信息

在工业自动化和机器人研究领域，力/力矩传感器的应用正变得越来越广泛。UR5协作机器人搭配RobotIQ FT300传感器，为开发者提供了强大的力控能力。本文将带您深入探索如何从基础数据采集到高级应用开发，充分发挥这套系统的潜力。

想象一下，您的机器人能够感知接触力，实现精密的装配操作；或者在进行表面处理时，保持恒定的压力。这些场景的实现，都依赖于对力/力矩数据的准确获取和智能处理。本文将为您揭示从原始数据到实际应用的完整路径。

1. 理解FT300传感器的数据输出

FT300六维力/力矩传感器通过ROS发布两种主要话题：/robotiq_ft_sensor和/robotiq_ft_wrench。理解这些数据的含义是进行后续处理的基础。

/robotiq_ft_sensor话题发布的是原始传感器数据，包含以下关键信息：

• forces：x、y、z三个方向的力分量（单位：牛顿）
• torques：x、y、z三个方向的力矩分量（单位：牛顿米）
• header：包含时间戳和坐标系信息

而/robotiq_ft_wrench话题则提供了经过坐标系转换后的数据，更适合直接用于控制应用。两者的主要区别在于：

提示：在实际应用中，建议优先使用/robotiq_ft_wrench话题，除非您需要对原始数据进行特殊处理。

2. 实时数据监控与可视化

在开始编写处理代码前，掌握基本的监控和可视化方法至关重要。ROS提供了多种工具来实现这一目的。

2.1 使用命令行工具监控数据

最简单的监控方式是使用rostopic echo命令：

rostopic echo /robotiq_ft_wrench

这将实时显示传感器数据流。对于快速检查传感器是否正常工作非常有用。

2.2 使用rqt_plot进行数据可视化

rqt_plot是ROS内置的强大可视化工具，特别适合观察力/力矩数据随时间的变化。

启动方法：

rqt_plot /robotiq_ft_wrench/wrench/force/x /robotiq_ft_wrench/wrench/force/y /robotiq_ft_wrench/wrench/force/z

这将绘制x、y、z三个方向力的实时曲线。您可以添加更多分量或调整显示范围：

rqt_plot /robotiq_ft_wrench/wrench/torque/x:y:z /robotiq_ft_wrench/wrench/force/x:y:z

2.3 使用rqt_bag记录和分析数据

对于需要后期分析的应用场景，可以使用rosbag记录数据：

rosbag record -O force_data.bag /robotiq_ft_wrench

然后使用rqt_bag进行回放和分析：

rqt_bag force_data.bag

3. 编写数据处理节点

理解了基础数据后，让我们着手编写自定义的数据处理节点。这里提供Python和C++两种实现方式。

3.1 Python订阅者节点示例

创建一个Python脚本来订阅并处理力/力矩数据：

#!/usr/bin/env python import rospy from geometry_msgs.msg import WrenchStamped class ForceTorqueProcessor: def __init__(self): rospy.init_node('ft_data_processor') self.sub = rospy.Subscriber('/robotiq_ft_wrench', WrenchStamped, self.callback) self.force_threshold = 5.0 # 5N的力阈值 self.torque_threshold = 0.5 # 0.5Nm的力矩阈值 def callback(self, data): force = data.wrench.force torque = data.wrench.torque # 计算合力大小 total_force = (force.x**2 + force.y**2 + force.z**2)**0.5 # 简单的阈值检测 if total_force > self.force_threshold: rospy.loginfo("Force threshold exceeded! Total force: %.2f N", total_force) # 可以添加更多处理逻辑... if __name__ == '__main__': ft_processor = ForceTorqueProcessor() rospy.spin()

3.2 C++订阅者节点示例

对于性能要求更高的应用，可以使用C++实现：

#include <ros/ros.h> #include <geometry_msgs/WrenchStamped.h> class ForceTorqueProcessor { public: ForceTorqueProcessor() { sub_ = nh_.subscribe("/robotiq_ft_wrench", 1, &ForceTorqueProcessor::callback, this); force_threshold_ = 5.0; // 5N torque_threshold_ = 0.5; // 0.5Nm } void callback(const geometry_msgs::WrenchStamped::ConstPtr& msg) { double fx = msg->wrench.force.x; double fy = msg->wrench.force.y; double fz = msg->wrench.force.z; double total_force = sqrt(fx*fx + fy*fy + fz*fz); if(total_force > force_threshold_) { ROS_INFO("Force threshold exceeded! Total force: %.2f N", total_force); } // 可以添加更多处理逻辑... } private: ros::NodeHandle nh_; ros::Subscriber sub_; double force_threshold_; double torque_threshold_; }; int main(int argc, char** argv) { ros::init(argc, argv, "ft_data_processor_cpp"); ForceTorqueProcessor processor; ros::spin(); return 0; }

4. 高级数据处理技术

简单的数据订阅只是开始，实际应用中需要更复杂的数据处理技术。

4.1 零漂校准

力传感器在无外力作用时可能会有小的输出（零漂）。进行零漂校准可以提高数据准确性：

def calibrate_zero_offset(self, sample_count=100): rospy.loginfo("Starting zero offset calibration...") force_sum = [0.0, 0.0, 0.0] torque_sum = [0.0, 0.0, 0.0] for i in range(sample_count): data = rospy.wait_for_message('/robotiq_ft_wrench', WrenchStamped) force_sum[0] += data.wrench.force.x force_sum[1] += data.wrench.force.y force_sum[2] += data.wrench.force.z torque_sum[0] += data.wrench.torque.x torque_sum[1] += data.wrench.torque.y torque_sum[2] += data.wrench.torque.z rospy.sleep(0.01) self.force_offset = [f/sample_count for f in force_sum] self.torque_offset = [t/sample_count for t in torque_sum] rospy.loginfo("Calibration complete. Offsets: Force %s, Torque %s", str(self.force_offset), str(self.torque_offset))

4.2 数据滤波

原始传感器数据可能包含噪声，适当的滤波可以提高数据质量。常用的滤波方法包括：

• 移动平均滤波
• 低通滤波
• 卡尔曼滤波

以下是移动平均滤波的Python实现：

class MovingAverageFilter: def __init__(self, window_size=5): self.window_size = window_size self.force_history = [] self.torque_history = [] def filter(self, wrench): # 处理力数据 self.force_history.append([ wrench.force.x, wrench.force.y, wrench.force.z ]) if len(self.force_history) > self.window_size: self.force_history.pop(0) avg_force = [ sum(f[i] for f in self.force_history)/len(self.force_history) for i in range(3) ] # 处理力矩数据（类似） # ... return avg_force, avg_torque

5. 力控应用开发

掌握了数据处理技术后，我们可以将这些数据应用于实际机器人控制中。

5.1 与MoveIt!集成

将力/力矩数据与MoveIt!运动规划框架结合，可以实现更智能的运动控制。基本集成步骤如下：

1. 创建一个监听力/力矩数据的节点
2. 根据传感器数据调整运动计划
3. 通过MoveIt! API发送调整后的命令

示例代码片段：

from moveit_commander import MoveGroupCommander class ForceControlledMotion: def __init__(self): self.group = MoveGroupCommander("manipulator") self.ft_sub = rospy.Subscriber('/robotiq_ft_wrench', WrenchStamped, self.ft_callback) self.current_force = None def ft_callback(self, data): self.current_force = data.wrench.force def compliant_move(self, direction, max_force=10.0, step_size=0.01): rate = rospy.Rate(10) # 10Hz while not rospy.is_shutdown(): if self.current_force is None: rospy.logwarn("No force data received yet") rate.sleep() continue # 检查是否超过最大允许力 if abs(self.current_force.z) > max_force: rospy.loginfo("Force limit reached, stopping motion") break # 获取当前位置 current_pose = self.group.get_current_pose().pose # 在指定方向上进行微小移动 if direction == 'z': current_pose.position.z += step_size # 其他方向... # 执行运动 self.group.set_pose_target(current_pose) self.group.go(wait=True) rate.sleep()

5.2 实现恒力控制

恒力控制是许多工业应用（如打磨、抛光）的关键需求。下面是一个简单的恒力控制实现思路：

1. 设定目标力值
2. 持续监测实际力值
3. 根据误差调整机器人位置
4. 使用PID控制器提高控制精度

class ConstantForceController: def __init__(self, target_force): self.target_force = target_force self.kp = 0.001 # 比例增益 self.ki = 0.0001 # 积分增益 self.kd = 0.0005 # 微分增益 self.integral = 0 self.last_error = 0 def update(self, current_force): error = self.target_force - current_force self.integral += error derivative = error - self.last_error self.last_error = error # 计算控制输出（位置调整量） output = self.kp * error + self.ki * self.integral + self.kd * derivative return output

6. 实际应用案例

让我们看几个具体的应用场景，了解如何将上述技术组合使用。

6.1 顺应性装配

在精密装配任务中，零件之间的配合往往需要柔顺的操作。使用力控可以实现：

1. 在接近阶段使用位置控制
2. 检测到接触后切换为力控模式
3. 保持恒定的接触力进行装配操作

关键实现代码：

def assembly_routine(self): # 第一阶段：位置控制接近目标 approach_pose = self.group.get_current_pose().pose approach_pose.position.z -= 0.1 # 向下移动10cm self.group.set_pose_target(approach_pose) self.group.go(wait=True) # 第二阶段：力控下的精细运动 self.compliant_move('z', max_force=15.0, step_size=0.001) # 第三阶段：完成装配后的回退 retreat_pose = self.group.get_current_pose().pose retreat_pose.position.z += 0.05 self.group.set_pose_target(retreat_pose) self.group.go(wait=True)

6.2 表面打磨应用

表面打磨需要保持恒定的接触压力。实现方案：

1. 使用路径规划生成表面轨迹
2. 在轨迹跟踪过程中叠加力控调整
3. 实时监测力/力矩数据确保质量

def surface_grinding(self, path_points, target_force): force_controller = ConstantForceController(target_force) for point in path_points: # 设置目标位置 self.group.set_pose_target(point) # 获取当前力值 wrench = rospy.wait_for_message('/robotiq_ft_wrench', WrenchStamped) current_force = wrench.wrench.force.z # 计算力控调整 adjustment = force_controller.update(current_force) # 调整目标位置 adjusted_point = copy.deepcopy(point) adjusted_point.position.z += adjustment # 执行调整后的运动 self.group.set_pose_target(adjusted_point) self.group.go(wait=True)

7. 调试技巧与最佳实践

在实际开发中，以下几个技巧可以帮助您更高效地工作：

• 数据记录与分析：始终记录重要的测试数据，便于后期分析
• 增量开发：从简单功能开始，逐步增加复杂性
• 安全第一：设置合理的力/力矩限制，避免损坏设备
• 可视化调试：充分利用rqt工具进行实时监控

一个实用的调试工作流程：

1. 使用rostopic echo确认基础数据流
2. 用rqt_plot观察数据特征
3. 编写简单的处理节点验证基本逻辑
4. 逐步增加功能复杂性
5. 最终集成到完整应用中

注意：在进行力控实验时，务必设置紧急停止机制，当检测到异常大的力/力矩时能够立即停止机器人运动。