ros2_control 多模控制系列之控制器生命周期
·
第一部分 ros2_control 架构概览与生命周期管理
1.1 ros2_control 核心架构与设计哲学
ros2_control 是 ROS2 中用于机器人控制的标准框架，它将硬件抽象层（Hardware Interface）、控制器（Controller）和控制器管理器（Controller Manager）解耦，提供了模块化、可复用的控制系统开发范式。
1.1.1 核心组件与交互关系
[ROS2 节点/应用] [Controller Manager (生命周期节点)] ↓ ↓ 1. 通过 ROS2 服务加载/卸载控制器 1. 管理控制器生命周期（加载/卸载/激活/去激活） 2. 通过 ROS2 参数设置控制参数 2. 管理硬件接口资源（读取/写入） 3. 发布关节命令/状态 3. 实时控制循环（由 `control_period` 触发） ↓ [Hardware Interface (生命周期组件)] ↓ 1. 读取传感器数据（位置、速度、力矩） 2. 执行控制命令（位置/速度/力矩指令） 3. 与底层通信协议交互（CANopen/EtherCAT/Modbus） ↓ [实际物理硬件 (电机/驱动器/传感器)]
1.1.2 生命周期状态机（Lifecycle State Machine）
+-------------+ | Unconfigured| <-- on_init() 失败 +------+------+ | | on_init() 成功 v +-------------+ | Inactive | <-- on_configure() 失败 +------+------+ | | on_configure() 成功 v +-------------+ | Active | <-- on_activate() 失败 +------+------+ | | on_activate() 成功 v +-------------+ | Running | <-- 正常运行时 +------+------+ | | on_deactivate() 调用 v +-------------+ | Inactive | <-- 回到 Inactive +-------------+
1.2 核心数据结构
1.2.1 hardware_interface::HardwareInfo：URDF 解析后的硬件描述
/**
* @struct hardware_interface::HardwareInfo
* @brief 从 URDF 的 <ros2_control> 标签中解析出的硬件配置信息。
* 每个硬件组件（如关节、传感器）都对应一份 HardwareInfo。
*/
struct HardwareInfo {
std::string name; /**< 硬件组件名称，如 "joint1" */
std::string class_type; /**< 硬件接口类型，如 "my_robot/SystemInterface" */
std::map<std::string, std::string> hardware_parameters; /**< 硬件参数键值对 */
std::vector<JointInfo> joints; /**< 关节信息列表 */
std::vector<SensorInfo> sensors; /**< 传感器信息列表 */
std::vector<TransmissionInfo> transmissions; /**< 传动信息列表 */
};
/**
* @struct JointInfo
* @brief 关节信息，包含状态接口和命令接口的名称。
*/
struct JointInfo {
std::string name; /**< 关节名称 */
std::vector<StateInterfaceInfo> state_interfaces; /**< 状态接口列表（position, velocity, effort） */
std::vector<CommandInterfaceInfo> command_interfaces; /**< 命令接口列表（position, velocity, effort） */
};
/**
* @struct StateInterfaceInfo
* @brief 状态接口描述。
*/
struct StateInterfaceInfo {
std::string name; /**< 接口名称，如 "position" */
std::string data_type; /**< 数据类型，如 "double" */
};
/**
* @struct CommandInterfaceInfo
* @brief 命令接口描述。
*/
struct CommandInterfaceInfo {
std::string name; /**< 接口名称，如 "position" */
std::string data_type; /**< 数据类型，如 "double" */
};
1.2.2 hardware_interface::SystemInterface：系统级硬件接口抽象
/**
* @class hardware_interface::SystemInterface
* @brief 系统级硬件接口抽象类，适用于多关节系统（如机械臂）。
* 用户需继承此类并实现虚函数。
*/
class SystemInterface : public hardware_interface::InterfaceBase {
public:
/**
* @brief 初始化硬件接口，从 HardwareInfo 读取配置。
* @param info 硬件信息
* @return 返回 call_return 状态
*/
virtual hardware_interface::return_type on_init(const HardwareInfo &info) = 0;
/**
* @brief 配置硬件（打开设备、建立通信、设置参数等）。
* @param previous_state 之前的状态
* @return 返回 call_return 状态
*/
virtual hardware_interface::return_type on_configure(const rclcpp_lifecycle::State &previous_state) = 0;
/**
* @brief 激活硬件，开始周期读写。
* @param previous_state 之前的状态
* @return 返回 call_return 状态
*/
virtual hardware_interface::return_type on_activate(const rclcpp_lifecycle::State &previous_state) = 0;
/**
* @brief 去激活，停止硬件操作。
* @param previous_state 之前的状态
* @return 返回 call_return 状态
*/
virtual hardware_interface::return_type on_deactivate(const rclcpp_lifecycle::State &previous_state) = 0;
/**
* @brief 清理硬件资源。
* @param previous_state 之前的状态
* @return 返回 call_return 状态
*/
virtual hardware_interface::return_type on_cleanup(const rclcpp_lifecycle::State &previous_state) = 0;
/**
* @brief 关闭硬件接口。
* @param previous_state 之前的状态
* @return 返回 call_return 状态
*/
virtual hardware_interface::return_type on_shutdown(const rclcpp_lifecycle::State &previous_state) = 0;
/**
* @brief 周期性读取硬件状态（位置、速度、力矩等）。
* @param state 当前生命周期状态
* @param data 只读数据容器
* @return 返回 call_return 状态
*/
virtual hardware_interface::return_type read(const rclcpp_lifecycle::State &state,
hardware_interface::ReadOnlyData &data) = 0;
/**
* @brief 周期性向硬件写入命令（位置/速度/力矩指令）。
* @param state 当前生命周期状态
* @param data 只写数据容器
* @return 返回 call_return 状态
*/
virtual hardware_interface::return_type write(const rclcpp_lifecycle::State &state,
hardware_interface::WriteOnlyData &data) = 0;
/**
* @brief 导出硬件状态接口（用于发布关节状态）。
* @return 返回状态接口列表
*/
virtual std::vector<hardware_interface::StateInterface> export_state_interfaces() = 0;
/**
* @brief 导出硬件命令接口（用于接收控制器指令）。
* @return 返回命令接口列表
*/
virtual std::vector<hardware_interface::CommandInterface> export_command_interfaces() = 0;
};
1.2.3 controller_manager 内部状态结构
/**
* @struct controller_manager::ControllerManagerState
* @brief Controller Manager 内部状态，管理所有控制器和硬件接口。
*/
struct ControllerManagerState {
std::map<std::string, std::shared_ptr<controller_interface::ControllerInterface>> controllers; /**< 控制器映射 */
std::vector<hardware_interface::HardwareInfo> hardware_infos; /**< 硬件信息列表 */
std::vector<std::shared_ptr<hardware_interface::SystemInterface>> hardware_interfaces; /**< 硬件接口实例 */
std::shared_ptr<hardware_interface::ResourceManager> resource_manager; /**< 资源管理器 */
std::shared_ptr<rclcpp::executor::Executor> executor; /**< 执行器（默认 MultiThreadedExecutor） */
rclcpp::TimerBase::SharedPtr control_loop_timer; /**< 控制循环定时器 */
std::chrono::nanoseconds control_period; /**< 控制周期（如 1ms） */
bool is_activated; /**< 是否已激活 */
std::map<std::string, std::vector<hardware_interface::CommandInterface>> command_maps; /**< 命令接口映射 */
std::map<std::string, std::vector<hardware_interface::StateInterface>> state_maps; /**< 状态接口映射 */
};
1.3 完整代码框架（以 EtherCAT 硬件接口为例）
1.3.1 URDF 配置示例
<!-- robot.urdf.xacro --> <robot name="my_robot"> <ros2_control name="my_robot_hardware" type="system"> <hardware> <plugin>my_robot_hardware/SystemInterface</plugin> <param name="ethercat_master_iface">eth0</param> <param name="ethercat_cycle_time_us">1000</param> <param name="max_joint_velocity">3.14</param> </hardware> <!-- 关节 1 --> <joint name="joint1"> <command_interface name="position"> <param name="min">-3.14</param> <param name="max">3.14</param> </command_interface> <command_interface name="velocity"/> <command_interface name="effort"/> <state_interface name="position"/> <state_interface name="velocity"/> <state_interface name="effort"/> </joint> <!-- 更多关节... --> </ros2_control> </robot>
1.3.2 硬件接口完整实现（SystemInterface）
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <ethercat.h> // 假设使用 SOEM 或开源的 EtherCAT 库
#include <memory>
#include <vector>
namespace my_robot_hardware
{
/**
* @class MyEthercatHardware
* @brief EtherCAT 硬件接口实现。
*/
class MyEthercatHardware : public hardware_interface::SystemInterface
{
public:
/**
* @brief 初始化硬件接口，解析参数。
*/
hardware_interface::return_type on_init(const hardware_interface::HardwareInfo &info) override
{
// 1. 保存硬件信息
info_ = info;
// 2. 解析硬件参数
for (const auto ¶m : info.hardware_parameters) {
if (param.first == "ethercat_master_iface") {
ethercat_iface_ = param.second;
} else if (param.first == "ethercat_cycle_time_us") {
cycle_time_us_ = std::stoi(param.second);
} else if (param.first == "max_joint_velocity") {
max_joint_velocity_ = std::stod(param.second);
}
}
// 3. 根据关节数量预分配缓冲区
joint_positions_.resize(info.joints.size(), 0.0);
joint_velocities_.resize(info.joints.size(), 0.0);
joint_efforts_.resize(info.joints.size(), 0.0);
joint_cmd_positions_.resize(info.joints.size(), 0.0);
joint_cmd_velocities_.resize(info.joints.size(), 0.0);
joint_cmd_efforts_.resize(info.joints.size(), 0.0);
RCLCPP_INFO(rclcpp::get_logger("MyEthercatHardware"), "Initialized with %zu joints", info.joints.size());
return hardware_interface::return_type::OK;
}
/**
* @brief 配置硬件，打开 EtherCAT 主站。
*/
hardware_interface::return_type on_configure(const rclcpp_lifecycle::State &previous_state) override
{
// 1. 初始化 EtherCAT 主站
if (!ethercat_init(ethercat_iface_.c_str())) {
RCLCPP_ERROR(rclcpp::get_logger("MyEthercatHardware"), "Failed to initialize EtherCAT master on %s", ethercat_iface_.c_str());
return hardware_interface::return_type::ERROR;
}
// 2. 扫描从站
if (ethercat_scan_slaves() <= 0) {
RCLCPP_ERROR(rclcpp::get_logger("MyEthercatHardware"), "No EtherCAT slaves found");
ethercat_close();
return hardware_interface::return_type::ERROR;
}
// 3. 配置 PDO 映射（假设使用预定义映射）
ethercat_config_pdo();
// 4. 配置周期时间
ethercat_set_cycle_time(cycle_time_us_);
RCLCPP_INFO(rclcpp::get_logger("MyEthercatHardware"), "EtherCAT configured successfully");
return hardware_interface::return_type::OK;
}
/**
* @brief 激活硬件，开始周期控制。
*/
hardware_interface::return_type on_activate(const rclcpp_lifecycle::State &previous_state) override
{
// 1. 启动 EtherCAT 周期数据交换
if (!ethercat_start_cyclic()) {
RCLCPP_ERROR(rclcpp::get_logger("MyEthercatHardware"), "Failed to start EtherCAT cyclic exchange");
return hardware_interface::return_type::ERROR;
}
RCLCPP_INFO(rclcpp::get_logger("MyEthercatHardware"), "EtherCAT cyclic exchange started");
return hardware_interface::return_type::OK;
}
/**
* @brief 去激活硬件，停止 EtherCAT。
*/
hardware_interface::return_type on_deactivate(const rclcpp_lifecycle::State &previous_state) override
{
ethercat_stop_cyclic();
RCLCPP_INFO(rclcpp::get_logger("MyEthercatHardware"), "EtherCAT cyclic exchange stopped");
return hardware_interface::return_type::OK;
}
/**
* @brief 清理硬件，关闭 EtherCAT 主站。
*/
hardware_interface::return_type on_cleanup(const rclcpp_lifecycle::State &previous_state) override
{
ethercat_close();
RCLCPP_INFO(rclcpp::get_logger("MyEthercatHardware"), "EtherCAT closed");
return hardware_interface::return_type::OK;
}
/**
* @brief 从硬件读取状态（通过 EtherCAT PDO）。
*/
hardware_interface::return_type read(const rclcpp_lifecycle::State &state,
hardware_interface::ReadOnlyData &data) override
{
// 1. 等待 EtherCAT 数据更新
if (!ethercat_wait_for_cycle()) {
RCLCPP_WARN(rclcpp::get_logger("MyEthercatHardware"), "EtherCAT cycle missed");
return hardware_interface::return_type::ERROR;
}
// 2. 从 EtherCAT 从站读取每个关节的位置、速度、力矩
for (size_t i = 0; i < info_.joints.size(); ++i) {
joint_positions_[i] = ethercat_get_position(i);
joint_velocities_[i] = ethercat_get_velocity(i);
joint_efforts_[i] = ethercat_get_effort(i);
}
// 3. 将数据写入 data 容器（由 controller_manager 管理）
data.position.assign(joint_positions_.begin(), joint_positions_.end());
data.velocity.assign(joint_velocities_.begin(), joint_velocities_.end());
data.effort.assign(joint_efforts_.begin(), joint_efforts_.end());
return hardware_interface::return_type::OK;
}
/**
* @brief 向硬件写入命令（通过 EtherCAT PDO）。
*/
hardware_interface::return_type write(const rclcpp_lifecycle::State &state,
hardware_interface::WriteOnlyData &data) override
{
// 1. 从 data 容器读取命令
for (size_t i = 0; i < info_.joints.size(); ++i) {
// 检查命令接口是否存在
if (i < data.position_command.size()) {
joint_cmd_positions_[i] = data.position_command[i];
}
if (i < data.velocity_command.size()) {
joint_cmd_velocities_[i] = data.velocity_command[i];
}
if (i < data.effort_command.size()) {
joint_cmd_efforts_[i] = data.effort_command[i];
}
}
// 2. 将命令写入 EtherCAT 从站
for (size_t i = 0; i < info_.joints.size(); ++i) {
// 根据控制器类型（位置/速度/力矩）选择写入模式
if (data.position_command.size() > i) {
ethercat_set_position(i, joint_cmd_positions_[i]);
} else if (data.velocity_command.size() > i) {
ethercat_set_velocity(i, joint_cmd_velocities_[i]);
} else if (data.effort_command.size() > i) {
ethercat_set_effort(i, joint_cmd_efforts_[i]);
}
}
return hardware_interface::return_type::OK;
}
/**
* @brief 导出状态接口（用于发布 joint_states）。
*/
std::vector<hardware_interface::StateInterface> export_state_interfaces() override
{
std::vector<hardware_interface::StateInterface> state_interfaces;
for (size_t i = 0; i < info_.joints.size(); ++i) {
const auto &joint_name = info_.joints[i].name;
state_interfaces.emplace_back(joint_name, "position", &joint_positions_[i]);
state_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_[i]);
state_interfaces.emplace_back(joint_name, "effort", &joint_efforts_[i]);
}
return state_interfaces;
}
/**
* @brief 导出命令接口（用于接收控制器指令）。
*/
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
for (size_t i = 0; i < info_.joints.size(); ++i) {
const auto &joint_name = info_.joints[i].name;
command_interfaces.emplace_back(joint_name, "position", &joint_cmd_positions_[i]);
command_interfaces.emplace_back(joint_name, "velocity", &joint_cmd_velocities_[i]);
command_interfaces.emplace_back(joint_name, "effort", &joint_cmd_efforts_[i]);
}
return command_interfaces;
}
private:
hardware_interface::HardwareInfo info_;
std::string ethercat_iface_;
int cycle_time_us_ = 1000;
double max_joint_velocity_ = 3.14;
// 关节状态缓冲区
std::vector<double> joint_positions_;
std::vector<double> joint_velocities_;
std::vector<double> joint_efforts_;
// 关节命令缓冲区
std::vector<double> joint_cmd_positions_;
std::vector<double> joint_cmd_velocities_;
std::vector<double> joint_cmd_efforts_;
// EtherCAT 相关函数（使用 SOEM）
bool ethercat_init(const char* iface);
int ethercat_scan_slaves();
void ethercat_config_pdo();
void ethercat_set_cycle_time(int us);
bool ethercat_start_cyclic();
void ethercat_stop_cyclic();
bool ethercat_wait_for_cycle();
double ethercat_get_position(int slave_idx);
double ethercat_get_velocity(int slave_idx);
double ethercat_get_effort(int slave_idx);
void ethercat_set_position(int slave_idx, double pos);
void ethercat_set_velocity(int slave_idx, double vel);
void ethercat_set_effort(int slave_idx, double effort);
void ethercat_close();
};
} // namespace my_robot_hardware
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(my_robot_hardware::MyEthercatHardware, hardware_interface::SystemInterface)
1.3.3 controller_manager 的 Launch 配置
# launch/robot_control.launch.py from launch import LaunchDescription from launch_ros.actions import Node from launch.actions import ExecuteProcess def generate_launch_description(): return LaunchDescription([ # 1. 启动 controller_manager Node( package='controller_manager', executable='ros2_control_node', parameters=['config/robot_controllers.yaml'], output='screen', ), # 2. 加载关节状态广播器 ExecuteProcess( cmd=['ros2', 'control', 'load_controller', 'joint_state_broadcaster'], output='screen', ), # 3. 加载位置控制器 ExecuteProcess( cmd=['ros2', 'control', 'load_controller', 'joint_trajectory_controller'], output='screen', ), # 4. 激活控制器 ExecuteProcess( cmd=['ros2', 'control', 'set_controller_state', 'joint_trajectory_controller', 'active'], output='screen', ), ])
1.3.4 controller_manager 参数配置
# config/robot_controllers.yaml controller_manager: ros__parameters: update_rate: 1000 # 1 kHz 控制频率 use_sim_time: false joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController joints: - joint1 - joint2 - joint3 - joint4 - joint5 - joint6 command_interfaces: - position state_interfaces: - position - velocity action_name: joint_trajectory_action allow_partial_joints_goal: true allow_non_trajectory_goal: true open_loop_control: true allow_partial_joints_goal: true action_monitor_rate: 20.0
1.4 软件模式分析
ros2_control 核心子系统 ├── Controller Manager (ControllerManager) │ ├── 生命周期节点 (LifecycleNode) │ │ ├── on_configure(): 加载配置、检查依赖、创建资源管理器 │ │ ├── on_activate(): 启动控制循环定时器、更新控制器状态 │ │ ├── on_deactivate(): 停止控制循环、停用控制器 │ │ └── on_cleanup(): 清理控制器、释放资源 │ ├── 控制器加载器 (ControllerLoader) [工厂] │ │ ├── 通过 pluginlib 加载控制器插件 │ │ ├── 创建控制器实例 │ │ ├── 调用 on_configure() 设置控制器 │ │ └── 将控制器添加到映射 │ ├── 资源管理器 (ResourceManager) [单例] │ │ ├── 解析 URDF 中的 <ros2_control> 标签 │ │ ├── 加载硬件接口插件 (SystemInterface/ActuatorInterface) │ │ ├── 管理硬件接口生命周期 │ │ ├── 提供命令接口和状态接口的访问 │ │ └── 实现 read()/write() 的分发 │ ├── 控制循环 (ControlLoop) [定时器模式] │ │ ├── 由 rclcpp::Timer 驱动 │ │ ├── 每个循环调用 step() 方法 │ │ └── step() 执行顺序：read() → update() → write() │ └── 执行器 (Executor) │ ├── 默认使用 MultiThreadedExecutor │ ├── 允许并行处理控制器回调 │ └── 支持互斥锁保护共享资源 ├── Hardware Interface (HardwareInterface) [策略模式] │ ├── SystemInterface (多关节系统) │ │ ├── 通过 pluginlib 导出 │ │ ├── 实现生命周期 (Lifecycle) │ │ ├── 管理底层通信 (CANopen/EtherCAT/Modbus) │ │ ├── 提供状态接口 (state_interfaces) │ │ └── 接收命令接口 (command_interfaces) │ ├── ActuatorInterface (单执行器) │ │ ├── 继承自 SystemInterface │ │ ├── 适用于单个电机/驱动 │ │ └── 简化了资源管理 │ └── SensorInterface (传感器) │ ├── 仅读取数据，不支持命令 │ ├── 适用于 IMU、力矩传感器等 │ └── 通过 state_interfaces 导出数据 └── Controller (ControllerInterface) [策略模式] ├── 实现 lifecycle (继承自 LifecycleNode) ├── 命令接口映射 (command_interfaces) ├── 状态接口映射 (state_interfaces) ├── 更新方法 (update())：计算控制输出 ├── 从硬件读取状态 (read()) ├── 向硬件写入命令 (write()) ├── 发布控制状态 (如 joint_states) ├── 支持参数配置 (on_parameters_set) └── 典型控制器： ├── JointTrajectoryController ├── JointStateBroadcaster ├── PositionController ├── ForceController ├── ImpedanceController └── GravityCompensationController
1.5 ros2_control 调试核心难点与陷阱
1.5.1 硬件接口加载失败
现象：启动 controller_manager 后，日志显示 Failed to load plugin 'my_robot_hardware/SystemInterface'。
原因：
-
硬件接口插件未正确注册到
pluginlib。 -
插件库路径未正确导出。
-
硬件接口类继承自错误的基类（如
SystemInterface与ActuatorInterface混淆）。
解决方法：
-
检查
PLUGINLIB_EXPORT_CLASS宏是否正确使用。 -
确保在
package.xml中声明了pluginlib导出。 -
验证硬件接口基类是否与 URDF 中指定的
type一致。
1.5.2 控制周期抖动（Jitter）
现象：使用 Preempt-RT 内核和 EtherCAT 驱动后，控制周期仍有 10-20ms 的抖动。
原因：
-
未正确配置
controller_manager的update_rate。 -
硬件接口的
read()/write()中执行了阻塞操作（如std::this_thread::sleep）。 -
数据共享未使用锁保护，导致竞争条件。
解决方法：
-
将
update_rate设置为与硬件采样率匹配（如 1000 Hz）。 -
在
read()和write()中使用非阻塞 I/O 或异步 I/O。 -
使用
std::atomic保护共享缓冲区，或使用std::lock_guard。
1.5.3 多控制器切换时的资源竞争
现象：从一个控制器切换到另一个时，出现 Command interface 'position' already claimed。
原因：
-
两个控制器同时声明了相同的命令接口。
-
切换时未先去激活当前控制器。
-
resource_manager中接口的独占访问控制未正确实现。
解决方法：
-
在切换前调用
controller_manager::set_controller_state(controller_name, "inactive")。 -
实现
ResourceManager::claim_command_interfaces()的独占检查。 -
使用
on_activate和on_deactivate周期分配和释放接口。
1.5.4 实时性保障与 Linux Preempt-RT
现象：在使用 EtherCAT 的 1kHz 控制循环中，超过 100 us 的延迟偶尔出现。
原因：
-
未配置 CPU 隔离和中断亲和性。
-
控制循环线程未分配实时优先级。
-
硬件接口中的
write()或read()使用了动态内存分配。
解决方法：
-
在 Grub 启动参数中添加
isolcpus=1,2，将控制线程绑定到隔离 CPU。 -
使用
sched_setscheduler将controller_manager的线程提升到SCHED_FIFO或SCHED_RR。 -
避免在
read()/write()中使用new/malloc，使用预分配的内存池。
第二部分 多模控制器设计
2.1 多模控制器的核心概念
在现代机器人控制中，单一的控制模式往往无法满足复杂任务需求。例如，机器人在自由空间需要精确的位置控制，而在接触环境时需要柔顺的力控制，在搬运物体时需要调节末端阻抗。因此，多模控制器设计成为机器人控制的核心技能。
2.1.1 五种控制模式的对比
| 控制模式 | 输入变量 | 输出变量 | 适用场景 |
|---|---|---|---|
| 位置控制 (Position Control) | 目标位置/轨迹 | 关节位置指令 | 自由运动、轨迹跟踪 |
| 力控制 (Force Control) | 目标力/力矩 | 关节力矩指令 | 接触任务、抛光、装配 |
| 阻抗控制 (Impedance Control) | 目标位置 + 刚度/阻尼 | 关节力矩指令 | 接触力控制、人机交互 |
| 导纳控制 (Admittance Control) | 检测到的力 + 导纳模型 | 位置修正量 | 外力响应、协作机器人 |
| 混合力位控制 (Hybrid Force-Position) | 位置约束 + 力约束 | 位置指令 + 力矩指令 | 曲面跟踪、复杂装配 |
2.1.2 多模控制器架构流程
[任务管理器] → [控制器选择器] → [控制器切换] → [硬件接口] ↓ ↓ [控制器配置] ← [控制模式切换] ← [状态反馈]
详细流程：
-
任务管理器：根据当前任务阶段（自由运动、接触、搬运）选择对应的控制模式。
-
控制器选择器：加载对应的控制器实例（如
position_controller、force_controller、impedance_controller）。 -
控制器切换：通过
controller_manager去激活当前控制器，激活新控制器，确保平滑切换（如位置和力控制切换时避免冲击）。 -
硬件接口：将控制器指令（位置、速度、力矩）写入硬件。
2.2 核心数据结构
2.2.1 ControlMode 枚举与切换状态
/**
* @enum ControlMode
* @brief 机器人控制模式枚举。
*/
enum class ControlMode {
POSITION = 0, /**< 纯位置控制 */
FORCE = 1, /**< 纯力控制 */
IMPEDANCE = 2, /**< 阻抗控制 */
ADMITTANCE = 3, /**< 导纳控制 */
HYBRID = 4 /**< 混合力位控制 */
};
/**
* @struct ControllerSwitchState
* @brief 控制器切换状态结构。
*/
struct ControllerSwitchState {
ControlMode current_mode; /**< 当前控制模式 */
ControlMode target_mode; /**< 目标控制模式 */
bool is_switching; /**< 是否正在切换 */
double transition_time; /**< 切换过渡时间 (秒) */
double transition_progress; /**< 过渡进度 (0-1) */
std::vector<double> position_start; /**< 切换起始位置 */
std::vector<double> force_start; /**< 切换起始力矩 */
std::vector<double> target_position; /**< 目标位置 */
std::vector<double> target_force; /**< 目标力矩 */
};
2.2.2 阻抗控制参数结构
/**
* @struct ImpedanceParams
* @brief 阻抗控制参数结构。
*/
struct ImpedanceParams {
std::vector<double> stiffness; /**< 刚度矩阵 (N/m) */
std::vector<double> damping; /**< 阻尼矩阵 (Ns/m) */
std::vector<double> mass; /**< 惯性矩阵 (kg) */
std::vector<double> inertia; /**< 惯性矩阵 (用于笛卡尔空间) */
std::vector<double> target_position; /**< 目标位置 */
std::vector<double> target_velocity; /**< 目标速度 */
bool enable_gravity_compensation; /**< 是否启用重力补偿 */
bool enable_friction_compensation; /**< 是否启用摩擦力补偿 */
};
2.2.3 导纳控制参数结构
/**
* @struct AdmittanceParams
* @brief 导纳控制参数结构。
*/
struct AdmittanceParams {
std::vector<double> admittance; /**< 导纳矩阵 (m/N) */
std::vector<double> damping; /**< 阻尼矩阵 (Ns/m) */
std::vector<double> mass; /**< 惯性矩阵 (kg) */
std::vector<double> force_threshold; /**< 力阈值 (超过阈值才响应) */
double max_position_correction; /**< 最大位置修正量 (m) */
double max_velocity_correction; /**< 最大速度修正量 (m/s) */
double filter_coefficient; /**< 低通滤波器系数 */
};
2.2.4 混合力位控制参数结构
/**
* @struct HybridForcePositionParams
* @brief 混合力位控制参数结构。
*/
struct HybridForcePositionParams {
std::vector<int> position_dof; /**< 位置控制自由度掩码 (1=位置控制, 0=力控制) */
std::vector<int> force_dof; /**< 力控制自由度掩码 (1=力控制, 0=位置控制) */
std::vector<double> position_gain; /**< 位置控制增益 (P) */
std::vector<double> force_gain; /**< 力控制增益 (P) */
std::vector<double> position_limits; /**< 位置限位 */
std::vector<double> force_limits; /**< 力限位 */
std::vector<double> target_position; /**< 目标位置 */
std::vector<double> target_force; /**< 目标力 */
};
2.3 控制器实现框架（基于 ControllerInterface）
2.3.1 位置控制器基类实现
#include <controller_interface/controller_interface.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <rclcpp_lifecycle/state.hpp>
namespace my_robot_controllers
{
/**
* @class PositionController
* @brief 位置控制器实现。
*/
class PositionController : public controller_interface::ControllerInterface
{
public:
/**
* @brief 初始化控制器，获取命令和状态接口。
*/
controller_interface::InterfaceConfiguration command_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
// 声明需要接收位置命令
for (const auto &joint_name : joint_names_) {
config.names.push_back(joint_name + "/position");
}
return config;
}
/**
* @brief 声明需要的状态接口（位置、速度、力矩）。
*/
controller_interface::InterfaceConfiguration state_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto &joint_name : joint_names_) {
config.names.push_back(joint_name + "/position");
config.names.push_back(joint_name + "/velocity");
config.names.push_back(joint_name + "/effort");
}
return config;
}
/**
* @brief 控制器更新函数。
* @param time 当前时间
* @param period 控制周期
* @return 返回 call_return 状态
*/
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
{
// 1. 读取目标位置（从参数或订阅消息读取）
// 这里假设通过订阅 /position_controller/commands 获取
if (!target_positions_.empty()) {
// 2. 计算位置误差
for (size_t i = 0; i < joint_names_.size(); ++i) {
double current_pos = state_interfaces_[i].get_value();
double error = target_positions_[i] - current_pos;
// 3. 计算命令（使用 PID 控制器）
double velocity_cmd = pid_controllers_[i].update(error, period);
// 4. 写入命令接口
command_interfaces_[i].set_value(velocity_cmd);
}
}
return controller_interface::return_type::OK;
}
private:
std::vector<std::string> joint_names_;
std::vector<hardware_interface::CommandInterface> command_interfaces_;
std::vector<hardware_interface::StateInterface> state_interfaces_;
std::vector<PIDController> pid_controllers_;
std::vector<double> target_positions_;
};
2.3.2 阻抗控制器实现 (核心部分)
/**
* @class ImpedanceController
* @brief 阻抗控制器实现（在关节空间或笛卡尔空间）。
*/
class ImpedanceController : public controller_interface::ControllerInterface
{
public:
controller_interface::InterfaceConfiguration command_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
// 阻抗控制输出力矩指令
for (const auto &joint_name : joint_names_) {
config.names.push_back(joint_name + "/effort");
}
return config;
}
controller_interface::InterfaceConfiguration state_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
// 需要位置、速度、力矩状态
for (const auto &joint_name : joint_names_) {
config.names.push_back(joint_name + "/position");
config.names.push_back(joint_name + "/velocity");
config.names.push_back(joint_name + "/effort");
}
return config;
}
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
{
// 1. 计算笛卡尔阻抗控制（假设使用雅可比）
// f_ext = K*(x_des - x) + D*(v_des - v) + M*(a_des - a) + G
// 2. 转换为关节力矩：tau = J^T * f_ext
// 3. 写入硬件接口
for (size_t i = 0; i < joint_names_.size(); ++i) {
double current_pos = state_interfaces_[i].get_value();
double current_vel = state_interfaces_[i+joint_names_.size()].get_value();
double desired_pos = target_positions_[i];
double desired_vel = target_velocities_[i];
double stiffness = stiffness_[i];
double damping = damping_[i];
double mass = mass_[i];
// 阻抗方程：tau = K*(x_des - x) + D*(v_des - v) + M*(a_des)
double acceleration = 0.0; // 目标加速度 (通常为0)
double effort = stiffness * (desired_pos - current_pos)
+ damping * (desired_vel - current_vel)
+ mass * acceleration;
command_interfaces_[i].set_value(effort);
}
return controller_interface::return_type::OK;
}
private:
std::vector<std::string> joint_names_;
std::vector<double> stiffness_;
std::vector<double> damping_;
std::vector<double> mass_;
std::vector<double> target_positions_;
std::vector<double> target_velocities_;
std::vector<hardware_interface::CommandInterface> command_interfaces_;
std::vector<hardware_interface::StateInterface> state_interfaces_;
};
2.3.3 混合力位控制器实现 (Hybrid Force-Position)
/**
* @class HybridForcePositionController
* @brief 混合力位控制器实现。
*/
class HybridForcePositionController : public controller_interface::ControllerInterface
{
public:
controller_interface::InterfaceConfiguration command_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
// 混合控制需要位置和力矩双重接口
for (const auto &joint_name : joint_names_) {
config.names.push_back(joint_name + "/position");
config.names.push_back(joint_name + "/effort");
}
return config;
}
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
{
// 1. 根据自由度掩码分配控制模式
for (size_t i = 0; i < joint_names_.size(); ++i) {
if (position_dof_mask_[i] == 1) {
// 位置控制模式
double current_pos = state_interfaces_pos_[i].get_value();
double error = target_positions_[i] - current_pos;
double pos_cmd = position_pid_[i].update(error, period);
// 写入位置命令接口
command_interfaces_pos_[i].set_value(pos_cmd);
// 力矩命令接口设为0 (避免冲突)
command_interfaces_eff_[i].set_value(0.0);
} else if (force_dof_mask_[i] == 1) {
// 力控制模式
double current_eff = state_interfaces_eff_[i].get_value();
double error = target_forces_[i] - current_eff;
double force_cmd = force_pid_[i].update(error, period);
// 写入力矩命令接口
command_interfaces_eff_[i].set_value(force_cmd);
// 位置命令接口设为当前值 (避免位置跃变)
double current_pos = state_interfaces_pos_[i].get_value();
command_interfaces_pos_[i].set_value(current_pos);
}
}
return controller_interface::return_type::OK;
}
private:
std::vector<std::string> joint_names_;
std::vector<int> position_dof_mask_; // 1=位置控制, 0=不用位置控制
std::vector<int> force_dof_mask_; // 1=力控制, 0=不用力控制
std::vector<double> target_positions_;
std::vector<double> target_forces_;
std::vector<PIDController> position_pid_;
std::vector<PIDController> force_pid_;
// 接口向量
std::vector<hardware_interface::StateInterface> state_interfaces_pos_;
std::vector<hardware_interface::StateInterface> state_interfaces_eff_;
std::vector<hardware_interface::CommandInterface> command_interfaces_pos_;
std::vector<hardware_interface::CommandInterface> command_interfaces_eff_;
};
2.4 控制器切换与多模式管理
2.4.1 控制器管理器中的切换实现
/**
* @brief 在 ControllerManager 中实现控制器切换。
*/
class ControllerManager : public rclcpp_lifecycle::LifecycleNode
{
public:
/**
* @brief 切换到指定控制模式。
* @param mode 目标控制模式
* @return 0 成功，-1 失败
*/
int switchToControlMode(ControlMode mode)
{
// 1. 检查当前激活的控制器
std::vector<std::string> active_controllers = getActiveControllers();
// 2. 根据目标模式选择对应的控制器
std::string target_controller_name = getControllerNameForMode(mode);
// 3. 去激活所有当前控制器
for (auto &ctrl : active_controllers) {
if (ctrl != target_controller_name) {
deactivateController(ctrl);
}
}
// 4. 激活目标控制器
activateController(target_controller_name);
return 0;
}
private:
std::vector<std::string> getActiveControllers()
{
// 遍历控制器映射，返回状态为 ACTIVE 的控制器名称列表
std::vector<std::string> active;
for (auto &[name, ctrl] : controllers_) {
if (ctrl->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
active.push_back(name);
}
}
return active;
}
std::string getControllerNameForMode(ControlMode mode)
{
switch (mode) {
case ControlMode::POSITION: return "joint_trajectory_controller";
case ControlMode::FORCE: return "force_controller";
case ControlMode::IMPEDANCE: return "impedance_controller";
case ControlMode::ADMITTANCE: return "admittance_controller";
case ControlMode::HYBRID: return "hybrid_force_position_controller";
default: return "";
}
}
std::map<std::string, std::shared_ptr<controller_interface::ControllerInterface>> controllers_;
};
2.4.2 切换平滑过渡（位置-力控制切换）
/**
* @brief 位置控制到力控制的平滑切换（使用切换状态机）。
*/
class ControllerSwitcher
{
public:
/**
* @brief 执行平滑切换。
* @param from 当前控制模式
* @param to 目标控制模式
* @param duration 切换持续时间 (秒)
*/
void smoothSwitch(ControlMode from, ControlMode to, double duration)
{
// 1. 记录当前位置和力矩
std::vector<double> current_positions = read_current_positions();
std::vector<double> current_efforts = read_current_efforts();
// 2. 设置切换状态
switch_state_.current_mode = from;
switch_state_.target_mode = to;
switch_state_.is_switching = true;
switch_state_.transition_time = duration;
switch_state_.transition_progress = 0.0;
switch_state_.position_start = current_positions;
switch_state_.force_start = current_efforts;
// 3. 启动定时器更新切换进度
update_timer_ = rclcpp::create_timer(0.01, [this]() {
this->updateTransition();
});
}
private:
void updateTransition()
{
switch_state_.transition_progress += 0.01 / switch_state_.transition_time;
if (switch_state_.transition_progress >= 1.0) {
switch_state_.is_switching = false;
// 完成切换
applyTargetController();
return;
}
// 混合控制：根据进度在位置控制和力控制间线性过渡
double alpha = switch_state_.transition_progress;
for (size_t i = 0; i < joint_names_.size(); ++i) {
double pos_cmd = (1 - alpha) * switch_state_.position_start[i]
+ alpha * target_positions_[i];
double force_cmd = (1 - alpha) * switch_state_.force_start[i]
+ alpha * target_forces_[i];
// 写入硬件接口
command_interfaces_pos_[i].set_value(pos_cmd);
command_interfaces_eff_[i].set_value(force_cmd);
}
}
std::vector<double> read_current_positions()
{
// 从状态接口读取当前位置
std::vector<double> pos;
for (auto &state : state_interfaces_pos_) {
pos.push_back(state.get_value());
}
return pos;
}
std::vector<double> read_current_efforts()
{
// 从状态接口读取当前力矩
std::vector<double> eff;
for (auto &state : state_interfaces_eff_) {
eff.push_back(state.get_value());
}
return eff;
}
};
2.5 软件模式分析（多模控制器）
多模控制器子系统 ├── 控制器管理器 (ControllerManager) │ ├── 控制器注册表 (ControllerRegistry) │ │ ├── position_controller │ │ ├── force_controller │ │ ├── impedance_controller │ │ ├── admittance_controller │ │ └── hybrid_force_position_controller │ ├── 切换状态机 (SwitchStateMachine) │ │ ├── IDLE: 无切换 │ │ ├── SWITCHING: 切换进行中 │ │ ├── TRANSITION: 过渡期 │ │ └── COMPLETED: 切换完成 │ └── 平滑过渡策略 (SmoothTransition) [策略模式] │ ├── 线性插值 (LinearInterpolation) │ ├── 三次样条插值 (CubicSplineInterpolation) │ └── 指数衰减 (ExponentialDecay) ├── 位置控制器 (PositionController) │ ├── 参考轨迹生成器 (TrajectoryGenerator) │ ├── 前馈补偿 (FeedforwardCompensation) │ ├── PID 控制器 │ └── 抗饱和 (AntiWindup) ├── 力控制器 (ForceController) │ ├── 力传感器处理 (ForceSensorProcessing) │ ├── 力跟踪控制器 (ForceTracking) │ ├── 极限保护 (ForceLimitProtection) │ └── 接触检测 (ContactDetection) ├── 阻抗控制器 (ImpedanceController) │ ├── 刚度和阻尼矩阵计算 │ ├── 惯性效应补偿 │ ├── 雅可比矩阵计算 (JacobianCalculation) │ └── 外部力估计 (ExternalForceEstimation) ├── 导纳控制器 (AdmittanceController) │ ├── 导纳模型求解 │ ├── 位置修正生成 │ ├── 速度限制保护 │ └── 力阈值处理 (ForceThreshold) └── 混合力位控制器 (HybridForcePositionController) ├── 自由度掩码管理 (DOFMaskManager) ├── 模式切换矩阵 ├── 关节级 PID 控制器 ├── 任务空间映射 (TaskSpaceMapping) └── 安全保护 (SafeSwitch)
2.6 多模控制器调试与优化
2.6.1 力控制稳定性问题
现象：启用力控制时系统震荡，产生尖叫和抖动。
原因：
-
力传感器噪声过大，未经过滤处理。
-
力控制器增益过高，系统不稳定。
-
接触刚度未知，导致力控制环路不稳定。
调试方法：
-
添加低通滤波：
filtered_force = alpha * raw_force + (1 - alpha) * filtered_force;
-
降低力控制器增益，增加阻尼项。
-
使用自适应增益：根据估计的接触刚度动态调整 Kp。
2.6.2 阻抗控制中的接触震荡
现象：当末端接触刚性环境时，阻抗控制会产生震荡。
原因：
-
刚度矩阵过大，导致刚性碰撞。
-
阻尼不足，无法快速抑制震荡。
-
未考虑运动学奇异点。
调试方法：
-
减小刚度矩阵值（如从 1000 N/m 降至 100 N/m）。
-
增加阻尼系数（如从 20 Ns/m 增加至 50 Ns/m）。
-
禁用奇异点附近的阻抗（使用零空间投影）。
2.6.3 导纳控制响应延迟
现象：操作员施加力后，机器人响应滞后明显。
原因：
-
导纳矩阵中的惯性项过大。
-
位置修正速度受最大速度限幅影响。
-
低通滤波器滤波系数过小。
调试方法：
-
降低惯性参数（例如从 10 kg 降至 2 kg）。
-
提高最大速度修正限幅（从 0.1 m/s 增加到 0.3 m/s）。
-
减小滤波系数（alpha = 0.1 → 0.3）。
2.6.4 混合力位控制的模式切换冲击
现象：从位置控制切换到力控制时，末端出现冲击位移或冲击力。
原因：
-
切换时没有平滑过渡。
-
目标位置 / 目标力未与当前状态对齐。
-
两个控制通道的增益差异过大。
调试方法：
-
使用上文实现的
ControllerSwitcher进行平滑切换。 -
切换时将目标位置设置为当前实际位置，避免突变。
-
在切换过程中逐渐过渡增益（从位置控制增益过渡到力控制增益）。
2.6.5 性能优化建议
| 场景 | 推荐控制器 | 调试关键点 |
|---|---|---|
| 自由运动 | 位置控制 | 轨迹平滑、前馈补偿 |
| 表面打磨 | 力控制 | 力传感器滤波、接触刚度估计 |
| 人机交互 | 阻抗控制 | 刚度/阻尼选择、安全限制 |
| 被动外力响应 | 导纳控制 | 惯性参数、速度限幅 |
| 复杂装配 | 混合力位控制 | 自由度掩码、切换时机 |
第三部分 动力学补偿：重力补偿、摩擦力补偿与动力学参数辨识
3.1 动力学补偿的核心意义
在机器人控制中，动力学补偿是提升控制精度和透明度（Transparency）的关键。真实机器人受到重力、摩擦力、科里奥利力、离心力等非线性因素的影响，这些力如果不加以补偿，会导致跟踪误差、稳态误差和动态响应滞后。
3.1.1 需要补偿的动力学因素
| 因素 | 来源 | 影响 | 补偿方式 |
|---|---|---|---|
| 重力 | 机器人自重 | 静态误差、下垂 | 重力模型计算 |
| 摩擦力 | 减速器、轴承 | 稳态误差、爬行现象 | Stribeck 模型 + 库仑摩擦 |
| 科里奥利力/离心力 | 高速运动时惯性耦合 | 轨迹跟踪误差 | 完整动力学模型 |
| 惯性参数偏差 | 负载变化、辨识误差 | 动态响应失真 | 参数辨识 + 自适应控制 |
3.1.2 动力学补偿在控制环路中的位置
[目标位置/速度] → [控制器 (PID/阻抗)] → [动力学补偿] → [硬件接口] ↑ ↓ [状态反馈] [重力/摩擦力模型]
3.2 核心数据结构
3.2.1 机器人动力学参数结构
/**
* @struct RobotDynamicsParams
* @brief 机器人动力学参数，包含重力、摩擦力、惯性参数。
*/
struct RobotDynamicsParams {
// 关节惯性参数
std::vector<double> link_masses; /**< 连杆质量 (kg) */
std::vector<std::vector<double>> link_com; /**< 连杆质心位置 (m) */
std::vector<std::vector<double>> link_inertia; /**< 连杆惯性矩阵 (kg·m²) */
// 摩擦力参数 (Stribeck 模型)
std::vector<double> friction_coulomb; /**< 库仑摩擦力系数 (Nm) */
std::vector<double> friction_viscous; /**< 粘性摩擦力系数 (Nm·s/rad) */
std::vector<double> friction_stribeck; /**< Stribeck 摩擦力系数 (Nm) */
std::vector<double> friction_breakaway; /**< 静摩擦系数 (Nm) */
std::vector<double> friction_stribeck_velocity; /**< Stribeck 速度 (rad/s) */
// 运动学参数
std::vector<double> link_lengths; /**< 连杆长度 (m) */
std::vector<double> link_offsets; /**< 连杆偏移 (m) */
std::vector<double> joint_damping; /**< 关节阻尼 (Nm·s/rad) */
// 重力参数
std::array<double, 3> gravity; /**< 重力向量 (m/s²) */
bool gravity_compensation_enabled; /**< 是否启用重力补偿 */
};
3.2.2 摩擦力模型 (Stribeck 模型)
/**
* @brief Stribeck 摩擦力模型：描述摩擦力与速度的关系。
* @param velocity 当前关节速度 (rad/s)
* @param params 摩擦力参数
* @return 摩擦力矩 (Nm)
*/
double stribeck_friction_model(double velocity, const FrictionParams ¶ms)
{
// F = F_c * sign(v) + (F_s - F_c) * exp(-|v/v_s|^2) + F_v * v
double sign = (velocity > 0) ? 1.0 : ((velocity < 0) ? -1.0 : 0.0);
double abs_v = std::abs(velocity);
double coulomb = params.coulomb * sign;
double stribeck = (params.breakaway - params.coulomb) *
std::exp(-std::pow(abs_v / params.stribeck_velocity, 2)) * sign;
double viscous = params.viscous * velocity;
return coulomb + stribeck + viscous;
}
3.3 核心代码实现
3.3.1 重力补偿控制器实现
#include <controller_interface/controller_interface.hpp>
#include <kdl/chain.hpp>
#include <kdl/chainjnttojacdotsolver.hpp>
#include <kdl/chainidsolver.hpp>
#include <kdl/frames.hpp>
/**
* @class GravityCompensationController
* @brief 重力补偿控制器，在关节力矩上叠加重力补偿项。
*/
class GravityCompensationController : public controller_interface::ControllerInterface
{
public:
controller_interface::InterfaceConfiguration command_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
// 输出力矩指令 (effort)
for (const auto &joint_name : joint_names_) {
config.names.push_back(joint_name + "/effort");
}
return config;
}
controller_interface::InterfaceConfiguration state_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
// 需要位置状态
for (const auto &joint_name : joint_names_) {
config.names.push_back(joint_name + "/position");
}
return config;
}
controller_interface::return_type init(const std::string &controller_name) override
{
// 1. 加载 URDF 构建 KDL 链
if (!loadURDF()) {
RCLCPP_ERROR(get_node()->get_logger(), "Failed to load URDF");
return controller_interface::return_type::ERROR;
}
// 2. 构建动力学求解器 (使用 KDL 或 RBDL)
id_solver_ = std::make_unique<KDL::ChainIdSolver_RNE>(chain_, gravity_);
return controller_interface::return_type::OK;
}
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
{
if (state_interfaces_.empty()) {
return controller_interface::return_type::OK;
}
// 1. 读取当前关节位置
KDL::JntArray q(joint_names_.size());
KDL::JntArray qdot(joint_names_.size());
KDL::JntArray qddot(joint_names_.size());
for (size_t i = 0; i < joint_names_.size(); ++i) {
q(i) = state_interfaces_[i].get_value();
// 如果支持速度接口，读取速度
if (state_interfaces_.size() > joint_names_.size() + i) {
qdot(i) = state_interfaces_[joint_names_.size() + i].get_value();
}
}
// 2. 计算重力补偿力矩 (使用 KDL 的反向动力学求解器)
KDL::JntArray gravity_torques(joint_names_.size());
id_solver_->CartToJnt(q, qdot, qddot, gravity_torques);
// 3. 将补偿力矩写入命令接口
for (size_t i = 0; i < joint_names_.size(); ++i) {
command_interfaces_[i].set_value(gravity_torques(i));
}
return controller_interface::return_type::OK;
}
private:
bool loadURDF()
{
// 从参数或共享库加载 URDF 描述
// 使用 urdf_parser 或 robot_state_publisher
return true;
}
std::vector<std::string> joint_names_;
std::vector<hardware_interface::CommandInterface> command_interfaces_;
std::vector<hardware_interface::StateInterface> state_interfaces_;
KDL::Chain chain_;
KDL::Vector gravity_;
std::unique_ptr<KDL::ChainIdSolver_RNE> id_solver_;
};
3.3.2 摩擦力补偿控制器实现
/**
* @class FrictionCompensationController
* @brief 摩擦力补偿控制器，采用 Stribeck 模型补偿关节摩擦力。
*/
class FrictionCompensationController : public controller_interface::ControllerInterface
{
public:
controller_interface::InterfaceConfiguration command_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto &joint_name : joint_names_) {
config.names.push_back(joint_name + "/effort");
}
return config;
}
controller_interface::InterfaceConfiguration state_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto &joint_name : joint_names_) {
config.names.push_back(joint_name + "/position");
config.names.push_back(joint_name + "/velocity");
}
return config;
}
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
{
for (size_t i = 0; i < joint_names_.size(); ++i) {
double velocity = state_interfaces_[joint_names_.size() + i].get_value();
// 计算摩擦力矩
double friction_torque = compute_friction(i, velocity);
command_interfaces_[i].set_value(friction_torque);
}
return controller_interface::return_type::OK;
}
private:
double compute_friction(int joint_idx, double velocity)
{
// Stribeck 模型
const auto &p = friction_params_[joint_idx];
double sign = (velocity > 0) ? 1.0 : ((velocity < 0) ? -1.0 : 0.0);
double abs_v = std::abs(velocity);
double coulomb = p.coulomb * sign;
double stribeck = (p.breakaway - p.coulomb) *
std::exp(-std::pow(abs_v / p.stribeck_velocity, 2)) * sign;
double viscous = p.viscous * velocity;
return coulomb + stribeck + viscous;
}
struct FrictionParams {
double coulomb;
double viscous;
double breakaway;
double stribeck_velocity;
};
std::vector<FrictionParams> friction_params_;
std::vector<std::string> joint_names_;
std::vector<hardware_interface::CommandInterface> command_interfaces_;
std::vector<hardware_interface::StateInterface> state_interfaces_;
};
3.3.3 动力学参数辨识方法 (基于最小二乘法)
/**
* @class DynamicsParameterIdentification
* @brief 基于最小二乘法的动力学参数辨识。
*/
class DynamicsParameterIdentification
{
public:
/**
* @brief 收集训练数据（位置、速度、力矩）。
*/
void collectData(double position, double velocity, double torque)
{
// 构造回归向量 (Regressor)
// y = τ = Φ * θ
// 其中 Φ 是回归矩阵，θ 是待辨识参数
if (data_size_ < max_data_size_) {
regression_matrix_[data_size_] = {1, velocity, 0.5 * velocity * std::abs(velocity)};
torque_vector_[data_size_] = torque;
data_size_++;
}
}
/**
* @brief 使用最小二乘法辨识参数。
* @return 辨识出的参数 (θ)
*/
std::vector<double> identify()
{
if (data_size_ < 10) {
return {}; // 数据不足
}
// 使用 Eigen 求解 (Φ^T * Φ) * θ = Φ^T * y
Eigen::MatrixXd Phi(data_size_, 3);
Eigen::VectorXd y(data_size_);
for (int i = 0; i < data_size_; ++i) {
Phi.row(i) = Eigen::Vector3d(regression_matrix_[i][0],
regression_matrix_[i][1],
regression_matrix_[i][2]);
y(i) = torque_vector_[i];
}
Eigen::VectorXd theta = (Phi.transpose() * Phi).inverse() * Phi.transpose() * y;
return {theta(0), theta(1), theta(2)};
}
private:
static const int max_data_size_ = 1000;
int data_size_ = 0;
std::array<std::array<double, 3>, max_data_size_> regression_matrix_;
std::array<double, max_data_size_> torque_vector_;
};
3.4 软件模式分析
动力学补偿子系统 ├── 重力补偿 (GravityCompensation) │ ├── 前向动力学 (ForwardDynamics) │ ├── 反向动力学 (InverseDynamics) │ ├── 雅可比矩阵 (Jacobian) │ ├── 惯性矩阵 (InertiaMatrix) │ ├── 科里奥利/离心力项 (Coriolis/Centrifugal) │ └── 重力项 (GravityTerm) ├── 摩擦力补偿 (FrictionCompensation) │ ├── 库仑摩擦模型 (CoulombFriction) │ ├── 粘性摩擦模型 (ViscousFriction) │ ├── Stribeck 模型 (StribeckModel) │ ├── LuGre 模型 (LuGreModel) │ ├── 自适应摩擦补偿 (AdaptiveFriction) │ └── 摩擦参数估计 (FrictionEstimation) ├── 动力学参数辨识 (ParameterIdentification) │ ├── 最小二乘法 (LeastSquares) │ │ ├── 批量最小二乘 (BatchLS) │ │ ├── 递归最小二乘 (RLS) │ │ └── 加权最小二乘 (WLS) │ ├── 数值优化方法 (OptimizationBased) │ │ ├── 梯度下降 (GradientDescent) │ │ ├── Levenberg-Marquardt │ │ └── 遗传算法 (GeneticAlgorithm) │ └── 激励轨迹设计 (ExcitationTrajectory) │ ├── 傅里叶级数 (FourierSeries) │ ├── 有限傅里叶 (FiniteFourier) │ └── 优化轨迹 (OptimizedTrajectory) └── 模型验证 (ModelValidation) ├── 残差分析 (ResidualAnalysis) ├── 交叉验证 (CrossValidation) ├── 预测误差 (PredictionError) └── 控制性能评估 (PerformanceEvaluation)
3.5 动力学补偿调试核心难点
3.5.1 重力补偿不准确
现象：机器人保持固定姿势时仍有下垂或漂移。
原因：
-
连杆质量或质心参数不准确。
-
未考虑末端负载或工具重量。
-
重力方向向量与安装方向不匹配。
解决方法：
-
使用负载传感器：通过末端力传感器实时检测负载。
-
进行负载辨识：在机器人空载和负载下分别运行，辨识质量。
-
校准重力方向：使用 IMU 或加速度计校准安装方向。
3.5.2 摩擦补偿引起震荡
现象：加入摩擦补偿后，低速运动时出现震荡或抖动。
原因：
-
Stribeck 模型参数未正确辨识。
-
速度测量噪声导致摩擦项突变。
-
静摩擦项在零速附近不连续。
解决方法：
-
添加速度低通滤波：
velocity_filtered = 0.9 * velocity_filtered + 0.1 * velocity_raw;
-
在零速附近使用平滑过渡：
if (std::abs(velocity) < 0.001) { friction = 0; // 小范围不补偿 } -
使用 LuGre 模型替代 Stribeck：提供更好的低速连续性。
3.5.3 参数辨识收敛慢
现象：辨识算法收敛缓慢，需要很长时间才能得到稳定参数。
原因：
-
激励轨迹未充分激发所有动力学模式。
-
初始参数估计偏离较大。
-
数据量过大，在线更新缓慢。
解决方法：
-
设计最优激励轨迹：使用傅里叶级数组合，覆盖所有频率。
-
使用多阶段辨识：
-
第一阶段：辨识惯性和重力项
-
第二阶段：辨识摩擦项
-
第三阶段：验证和修正
-
-
采用递归最小二乘（RLS）：支持在线增量更新。
3.5.4 前馈补偿与反馈控制的配合
现象：补偿后系统仍然出现稳态误差。
原因：
-
反馈增益过低，无法消除残余误差。
-
前馈模型不完整，遗漏了高阶项。
-
反馈和前馈互相冲突。
解决方法：
-
调整反馈增益：增加比例增益以消除稳态误差。
-
复合控制：
total_command = feedforward_cmd + feedback_cmd;
-
使用自适应前馈：根据误差实时调整补偿系数。
3.6 与其他模块的协同
| 模块 | 协同方式 | 调试关键点 |
|---|---|---|
| 硬件接口 | 提供关节位置/速度状态，接收力矩命令 | 状态噪声、命令饱和度 |
| 多模控制器 | 在位置/力/阻抗控制基础上叠加补偿 | 补偿项与控制器输出叠加策略 |
| 控制器管理器 | 作为复合控制器的一部分加载 | 控制器加载顺序、切换平滑 |
| 参数辨识 | 提供离线/在线辨识的模型参数 | 激励轨迹设计、数据收集策略 |
| 实时通信 | 在 CANopen/EtherCAT 周期中运行补偿计算 | 计算时间、周期同步 |
| 生命周期 | 确保补偿控制器与硬件状态同步 | on_activate 时加载参数 |
| 监控系统 | 记录补偿效果和参数变化 | 跟踪误差、补偿力矩监控 |
第四部分 电机 FOC 控制与三环参数调试
4.1 电机 FOC 控制的核心概念
FOC（Field Oriented Control，磁场定向控制）是永磁同步电机（PMSM）和直流无刷电机（BLDC）的高性能控制方法。它通过将三相电流变换到旋转坐标系下，实现磁链和转矩的解耦控制，达到类似直流电机的控制性能。
4.1.1 FOC 控制的基本原理
FOC 的核心思想是将三相交流电机的定子电流分解为两个正交分量：
-
直轴电流 (Id)：控制磁链，通常设定为 0（或弱磁控制时设定为负值）。
-
交轴电流 (Iq)：控制转矩，是产生输出力矩的主要成分。
4.1.2 FOC 控制环路架构
[目标位置] → [位置环 P控制器] → [目标速度] → [速度环 PI控制器] → [目标Iq] → [电流环 PI控制器] → [SVPWM] → [逆变器] → [电机] ↑ ↑ [速度反馈] [Iq反馈]
4.1.3 坐标变换流程
[三相电流 Ia,Ib,Ic] → [Clarke变换] → [Iα, Iβ] → [Park变换] → [Id, Iq] ↓ [目标Id, 目标Iq] → [逆Park变换] → [Vα, Vβ] → [逆Clarke变换] → [PWM]
4.2 核心数据结构
4.2.1 FOC 控制器参数结构
/**
* @struct FOCParams
* @brief FOC 控制器参数结构。
*/
struct FOCParams {
// 电机参数
double motor_resistance; /**< 定子电阻 (Ω) */
double motor_inductance; /**< 定子电感 (H) */
double motor_flux_linkage; /**< 磁链 (Wb) */
int motor_pole_pairs; /**< 极对数 */
double motor_max_current; /**< 最大电流 (A) */
double motor_max_velocity; /**< 最大速度 (rad/s) */
// 控制参数
double current_loop_kp; /**< 电流环比例增益 */
double current_loop_ki; /**< 电流环积分增益 */
double velocity_loop_kp; /**< 速度环比例增益 */
double velocity_loop_ki; /**< 速度环积分增益 */
double position_loop_kp; /**< 位置环比例增益 */
// 滤波器参数
double current_filter_alpha; /**< 电流低通滤波系数 */
double velocity_filter_alpha; /**< 速度低通滤波系数 */
// 限制参数
double max_current_limit; /**< 电流限幅 (A) */
double max_velocity_limit; /**< 速度限幅 (rad/s) */
double max_position_limit; /**< 位置限幅 (rad) */
bool enable_anti_windup; /**< 是否启用抗积分饱和 */
};
/**
* @struct FOCState
* @brief FOC 运行时状态。
*/
struct FOCState {
double Ia; /**< A相电流 (A) */
double Ib; /**< B相电流 (A) */
double Ic; /**< C相电流 (A) */
double Ialpha; /**< α轴电流 (A) */
double Ibeta; /**< β轴电流 (A) */
double Id; /**< d轴电流 (A) */
double Iq; /**< q轴电流 (A) */
double Vd; /**< d轴电压 (V) */
double Vq; /**< q轴电压 (V) */
double theta_e; /**< 电角度 (rad) */
double theta_m; /**< 机械角度 (rad) */
double velocity_e; /**< 电角速度 (rad/s) */
double velocity_m; /**< 机械角速度 (rad/s) */
double torque; /**< 输出转矩 (Nm) */
double position; /**< 位置 (rad) */
double target_position; /**< 目标位置 (rad) */
double target_velocity; /**< 目标速度 (rad/s) */
double target_Iq; /**< 目标q轴电流 (A) */
double target_Id; /**< 目标d轴电流 (A) */
};
/**
* @struct PIDParams
* @brief PID 控制器参数。
*/
struct PIDParams {
double kp; /**< 比例增益 */
double ki; /**< 积分增益 */
double kd; /**< 微分增益 */
double integral_max; /**< 积分上限 */
double output_max; /**< 输出上限 */
double output_min; /**< 输出下限 */
double dead_zone; /**< 死区范围 */
double feedforward; /**< 前馈增益 */
};
4.2.2 FOC 控制器的状态结构
/**
* @struct PIDController
* @brief PID 控制器实现。
*/
struct PIDController {
PIDParams params; /**< PID 参数 */
double integral; /**< 积分累计 */
double prev_error; /**< 上次误差 */
double output; /**< 当前输出 */
/**
* @brief 更新 PID 控制器。
* @param error 当前误差
* @param dt 时间步长 (秒)
* @return 控制器输出
*/
double update(double error, double dt)
{
// 比例项
double p = params.kp * error;
// 积分项 (带抗饱和)
integral += error * dt;
if (params.enable_anti_windup) {
// 根据输出是否饱和限制积分
if (output >= params.output_max || output <= params.output_min) {
integral = std::clamp(integral, -params.integral_max, params.integral_max);
}
}
double i = params.ki * integral;
// 微分项
double derivative = (error - prev_error) / dt;
double d = params.kd * derivative;
// 前馈项
double ff = params.feedforward * error;
// 计算输出
output = p + i + d + ff;
// 限幅
output = std::clamp(output, params.output_min, params.output_max);
// 死区处理
if (std::abs(output) < params.dead_zone) {
output = 0;
}
prev_error = error;
return output;
}
/**
* @brief 重置 PID 控制器。
*/
void reset()
{
integral = 0;
prev_error = 0;
output = 0;
}
};
4.3 核心代码实现
4.3.1 坐标变换实现
/**
* @brief Clarke 变换：将三相电流转换为两相静止坐标系。
* @param Ia A相电流
* @param Ib B相电流
* @param Ic C相电流
* @param Ialpha 输出 α 轴电流
* @param Ibeta 输出 β 轴电流
*/
void clarke_transform(double Ia, double Ib, double Ic, double &Ialpha, double &Ibeta)
{
Ialpha = Ia;
Ibeta = (Ia + 2 * Ib) / sqrt(3);
}
/**
* @brief Park 变换：将两相静止坐标系转换为旋转坐标系。
* @param Ialpha α 轴电流
* @param Ibeta β 轴电流
* @param theta 电角度 (rad)
* @param Id 输出 d 轴电流
* @param Iq 输出 q 轴电流
*/
void park_transform(double Ialpha, double Ibeta, double theta, double &Id, double &Iq)
{
double cos_theta = cos(theta);
double sin_theta = sin(theta);
Id = Ialpha * cos_theta + Ibeta * sin_theta;
Iq = -Ialpha * sin_theta + Ibeta * cos_theta;
}
/**
* @brief 逆 Park 变换：将旋转坐标系转换为两相静止坐标系。
* @param Vd d 轴电压
* @param Vq q 轴电压
* @param theta 电角度 (rad)
* @param Vα 输出 α 轴电压
* @param Vβ 输出 β 轴电压
*/
void inverse_park_transform(double Vd, double Vq, double theta, double &Vα, double &Vβ)
{
double cos_theta = cos(theta);
double sin_theta = sin(theta);
Vα = Vd * cos_theta - Vq * sin_theta;
Vβ = Vd * sin_theta + Vq * cos_theta;
}
/**
* @brief 逆 Clarke 变换：将两相静止坐标系转换为三相电压。
* @param Vα α 轴电压
* @param Vβ β 轴电压
* @param Va 输出 A 相电压
* @param Vb 输出 B 相电压
* @param Vc 输出 C 相电压
*/
void inverse_clarke_transform(double Vα, double Vβ, double &Va, double &Vb, double &Vc)
{
Va = Vα;
Vb = (-Vα + sqrt(3) * Vβ) / 2;
Vc = (-Vα - sqrt(3) * Vβ) / 2;
}
4.3.2 FOC 控制器主循环实现
/**
* @class FOCController
* @brief FOC 控制器主类，包含三环控制。
*/
class FOCController
{
public:
/**
* @brief 初始化 FOC 控制器。
* @param params FOC 参数
*/
void init(const FOCParams ¶ms)
{
params_ = params;
state_ = FOCState{};
// 初始化三个 PID 控制器
current_pid_.params = PIDParams{
.kp = params.current_loop_kp,
.ki = params.current_loop_ki,
.kd = 0.0,
.integral_max = params.max_current_limit,
.output_max = params.max_current_limit,
.output_min = -params.max_current_limit,
.dead_zone = 0.001,
.feedforward = 0.0
};
velocity_pid_.params = PIDParams{
.kp = params.velocity_loop_kp,
.ki = params.velocity_loop_ki,
.kd = 0.0,
.integral_max = params.max_current_limit,
.output_max = params.max_current_limit,
.output_min = -params.max_current_limit,
.dead_zone = 0.001,
.feedforward = 0.0
};
position_pid_.params = PIDParams{
.kp = params.position_loop_kp,
.ki = 0.0,
.kd = 0.0,
.integral_max = params.max_velocity_limit,
.output_max = params.max_velocity_limit,
.output_min = -params.max_velocity_limit,
.dead_zone = 0.001,
.feedforward = 0.0
};
}
/**
* @brief 执行 FOC 控制循环。
* @param Ia A相电流
* @param Ib B相电流
* @param Ic C相电流
* @param theta_e 电角度
* @param dt 时间步长 (秒)
*/
void update(double Ia, double Ib, double Ic, double theta_e, double dt)
{
// 1. 保存当前状态
state_.Ia = Ia;
state_.Ib = Ib;
state_.Ic = Ic;
state_.theta_e = theta_e;
// 2. Clarke 变换
clarke_transform(Ia, Ib, Ic, state_.Ialpha, state_.Ibeta);
// 3. Park 变换
park_transform(state_.Ialpha, state_.Ibeta, theta_e, state_.Id, state_.Iq);
// 4. 电流环控制 (生成 Vd, Vq)
double error_Iq = state_.target_Iq - state_.Iq;
double error_Id = state_.target_Id - state_.Id;
state_.Vq = current_pid_.update(error_Iq, dt);
state_.Vd = current_pid_.update(error_Id, dt);
// 5. 前馈补偿 (反电动势)
double EMF = params_.motor_flux_linkage * state_.velocity_e;
state_.Vq += EMF;
// 6. 逆 Park 变换
inverse_park_transform(state_.Vd, state_.Vq, theta_e, state_.Vα, state_.Vβ);
// 7. 逆 Clarke 变换 (生成 PWM 占空比)
double Va, Vb, Vc;
inverse_clarke_transform(state_.Vα, state_.Vβ, Va, Vb, Vc);
// 8. 转换为 PWM 占空比 (0-1)
double Vdc = 24.0; // 直流母线电压
state_.pwm_a = (Va / Vdc) * 0.5 + 0.5;
state_.pwm_b = (Vb / Vdc) * 0.5 + 0.5;
state_.pwm_c = (Vc / Vdc) * 0.5 + 0.5;
}
/**
* @brief 设置位置环目标。
* @param target_pos 目标位置 (rad)
*/
void set_position_target(double target_pos)
{
state_.target_position = target_pos;
}
/**
* @brief 设置速度环目标。
* @param target_vel 目标速度 (rad/s)
*/
void set_velocity_target(double target_vel)
{
state_.target_velocity = target_vel;
}
/**
* @brief 获取 PWM 占空比。
*/
double get_pwm_a() const { return state_.pwm_a; }
double get_pwm_b() const { return state_.pwm_b; }
double get_pwm_c() const { return state_.pwm_c; }
private:
FOCParams params_;
FOCState state_;
PIDController current_pid_;
PIDController velocity_pid_;
PIDController position_pid_;
};
4.3.3 位置环、速度环、电流环参数整定流程
/**
* @brief 三环参数整定流程 (从内到外)。
*/
class PIDTuningProcedure
{
public:
/**
* @brief 执行三环参数整定。
* @param foc FOC 控制器实例
*/
void tune(FOCController &foc)
{
// 步骤 1: 电流环整定 (Iq控制)
tune_current_loop(foc);
// 步骤 2: 速度环整定
tune_velocity_loop(foc);
// 步骤 3: 位置环整定
tune_position_loop(foc);
// 步骤 4: 综合调优
final_tuning(foc);
}
private:
void tune_current_loop(FOCController &foc)
{
// 1.1 设置电机参数
// 1.2 设置 Iq 目标为阶跃信号
// 1.3 调整 Kp, Ki 直到响应无超调且快速
// 1.4 测量电流环带宽
}
void tune_velocity_loop(FOCController &foc)
{
// 2.1 在电流环整定完成后进行
// 2.2 设定速度目标为阶跃信号 (10 rad/s)
// 2.3 调整 Kp, Ki 直到速度响应无超调
// 2.4 调整前馈增益
}
void tune_position_loop(FOCController &foc)
{
// 3.1 在速度环整定完成后进行
// 3.2 设定位置目标为阶跃信号 (1 rad)
// 3.3 调整 Kp (通常不需要积分)
// 3.4 测量定位精度和稳态误差
}
void final_tuning(FOCController &foc)
{
// 4.1 综合测试 (正弦轨迹、S形轨迹)
// 4.2 调整抗积分饱和参数
// 4.3 调整死区参数
// 4.4 验证极限条件下稳定性
}
};
4.4 软件模式分析
FOC 控制子系统 ├── 坐标变换 (CoordinateTransform) │ ├── Clarke 变换 │ ├── Park 变换 │ ├── 逆 Park 变换 │ └── 逆 Clarke 变换 ├── 三环控制器 (ThreeLoopController) │ ├── 位置环 (PositionLoop) │ │ ├── P 控制器 (PController) │ │ ├── 前馈生成 (FeedforwardGenerator) │ │ └── 轨迹规划器 (TrajectoryPlanner) │ ├── 速度环 (VelocityLoop) │ │ ├── PI 控制器 (PIController) │ │ ├── 速度观测器 (VelocityObserver) │ │ └── 限幅器 (VelocityLimiter) │ └── 电流环 (CurrentLoop) │ ├── PI 控制器 (PIController) │ ├── 反电动势补偿 (EMFCompensation) │ ├── 抗饱和 (AntiWindup) │ └── 死区补偿 (DeadZoneCompensation) ├── PWM 生成 (PWMGeneration) │ ├── SVPWM (SpaceVectorPWM) │ ├── SPWM (SinusoidalPWM) │ └── 死区补偿 (DeadZoneCompensation) ├── 参数整定 (PIDTuning) │ ├── 手动整定 (ManualTuning) │ │ ├── Ziegler-Nichols │ │ └── 试错法 (TrialAndError) │ ├── 自动整定 (AutoTuning) │ │ ├── 继电反馈法 (RelayFeedback) │ │ ├── 频率响应法 (FrequencyResponse) │ │ └── 基于模型的方法 (ModelBased) │ └── 参数辨识 (ParameterIdentification) │ ├── 离线辨识 (OfflineIdentification) │ └── 在线辨识 (OnlineIdentification) └── 电机模型 (MotorModel) ├── 电气模型 (ElectricalModel) ├── 机械模型 (MechanicalModel) ├── 损耗模型 (LossModel) └── 热模型 (ThermalModel)
4.5 FOC 调试核心难点
4.5.1 电流环震荡
现象：在电流环控制下，电流波形出现高频振荡。
原因：
-
Kp 增益过大，引起稳定性问题。
-
逆变器死区效应未补偿。
-
电流采样噪声过大。
解决方法：
-
降低 Kp 增益，增加 Ki 以维持稳态精度。
-
添加死区补偿：
if (fabs(pwm_duty) < 0.05) { pwm_duty = 0.0; // 死区范围内不输出 } -
增加电流低通滤波：
I_filtered = alpha * I_raw + (1 - alpha) * I_filtered;
4.5.2 速度环超调
现象：速度达到目标后产生超调，需要长时间才能稳定。
原因：
-
Kp 增益过大。
-
积分项积累过快。
-
未加前馈补偿。
解决方法：
-
降低 Kp，增加 Kd 以抑制超调。
-
限制积分项：设置积分上限，使用抗饱和机制。
-
添加前馈项：使用加速度命令作为前馈。
4.5.3 位置环稳态误差
现象：位置控制无法到达目标，存在稳态误差。
原因：
-
未加积分项，无法消除稳态误差。
-
负载力矩干扰未补偿。
-
电机死区导致小信号无响应。
解决方法：
-
添加积分项（注意可能引起超调）。
-
叠加力矩前馈：根据负载估计值补偿。
-
处理死区：小误差范围内使用非线性控制。
4.5.4 三环参数整定策略
| 环节 | 整定策略 | 调试关键点 |
|---|---|---|
| 电流环 | 先调整 Kp，再加入 Ki | 响应速度、电流纹波 |
| 速度环 | 先调整 Kp，再加入 Ki | 超调量、稳态精度 |
| 位置环 | 通常只使用 Kp | 定位精度、稳定时间 |
| 前馈 | 根据模型计算 | 减少跟踪误差 |
4.6 与其他模块的协同
| 模块 | 协同方式 | 调试关键点 |
|---|---|---|
| 硬件接口 | 通过硬件接口读取电流/位置/速度，写入PWM占空比 | PWM精度、电流采样噪声 |
| 多模控制器 | FOC作为底层电机控制，上层位置/力/阻抗控制通过FOC实现 | 接口适配、命令传递 |
| 动力学补偿 | FOC速度环的输出叠加动力学补偿力矩 | 补偿叠加时机、防冲突 |
| 控制器管理器 | 将FOC控制器作为底层驱动控制器加载 | 控制器加载顺序、切换平滑 |
| 实时通信 | 在CANopen/EtherCAT周期中执行FOC计算 | 计算时间、周期同步 |
| 生命周期 | 确保FOC控制器与硬件状态同步 | on_activate 时加载参数、初始化电机 |
| 监控系统 | 记录FOC状态、电流/速度/位置轨迹 | 采样率、数据存储 |
第五部分 关节控制参数 PID 前馈适配与整定
5.1 关节控制参数整定的核心意义
在机器人控制中，PID 参数整定是决定控制性能的关键环节。不恰当的 PID 参数会导致超调、震荡、稳态误差或响应迟缓。前馈补偿可以显著提高跟踪精度，但需要准确的模型参数。本部分将系统介绍从理论整定到工程实践的全流程方法。
5.1.1 PID 控制的局限性
| 问题 | 表现 | 原因 |
|---|---|---|
| 超调 | 响应超过目标值 | 比例增益过高或积分项累积 |
| 震荡 | 系统在目标值附近振荡 | 增益过高或系统存在死区 |
| 稳态误差 | 无法到达目标值 | 无积分项或负载力矩干扰 |
| 响应缓慢 | 跟踪速度慢 | 增益过低或惯性过大 |
| 积分饱和 | 积分项持续累积导致过冲 | 长时间处于饱和状态 |
5.1.2 前馈控制的优势
前馈控制通过预先补偿已知的扰动（如重力、摩擦力、惯性力），可以显著提高跟踪精度，同时减少反馈增益的需求，从而降低超调和震荡风险。
5.2 核心数据结构
5.2.1 PID 参数结构（扩展版）
/**
* @struct PIDParams
* @brief PID 控制器参数，包含完整的整定参数和限幅。
*/
struct PIDParams {
// 增益参数
double kp; /**< 比例增益 */
double ki; /**< 积分增益 */
double kd; /**< 微分增益 */
double kf; /**< 前馈增益 (速度前馈) */
double ka; /**< 加速度前馈增益 */
double kg; /**< 重力补偿增益 */
// 限幅参数
double integral_max; /**< 积分上限 */
double output_max; /**< 输出上限 */
double output_min; /**< 输出下限 */
double velocity_max; /**< 速度限幅 */
double effort_max; /**< 力矩限幅 */
// 滤波参数
double d_filter_alpha; /**< 微分项滤波系数 (0-1) */
double velocity_filter_alpha; /**< 速度反馈滤波系数 (0-1) */
// 死区和阈值
double dead_zone; /**< 死区范围 */
double error_threshold; /**< 误差阈值 (用于静差消除) */
// 非线性补偿
double friction_coulomb; /**< 库仑摩擦力补偿系数 */
double friction_viscous; /**< 粘性摩擦力补偿系数 */
double gravity_compensation; /**< 重力补偿系数 */
// 抗饱和
bool enable_anti_windup; /**< 是否启用抗积分饱和 */
bool enable_clamping; /**< 是否启用积分钳位 */
double back_calculation_coeff; /**< 反算系数 (抗饱和) */
};
/**
* @struct PIDState
* @brief PID 控制器运行时状态。
*/
struct PIDState {
double integral; /**< 积分累计值 */
double prev_error; /**< 上次误差 */
double prev_output; /**< 上次输出 */
double filtered_d; /**< 滤波后的微分项 */
double filtered_velocity; /**< 滤波后的速度反馈 */
double output; /**< 当前输出 */
double error; /**< 当前误差 */
double p_term; /**< 比例项 */
double i_term; /**< 积分项 */
double d_term; /**< 微分项 */
double f_term; /**< 前馈项 */
double saturation_count; /**< 饱和计数 */
double windup_level; /**< 积分饱和水平 */
};
5.3 核心代码实现
5.3.1 增强型 PID 控制器
/**
* @class AdvancedPIDController
* @brief 增强型 PID 控制器，支持前馈、滤波、抗饱和和非线性补偿。
*/
class AdvancedPIDController
{
public:
/**
* @brief 初始化 PID 控制器。
* @param params PID 参数
*/
void init(const PIDParams ¶ms)
{
params_ = params;
state_ = PIDState{};
state_.integral = 0.0;
state_.prev_error = 0.0;
state_.prev_output = 0.0;
state_.filtered_d = 0.0;
state_.filtered_velocity = 0.0;
}
/**
* @brief 更新 PID 控制器。
* @param setpoint 目标值
* @param feedback 反馈值
* @param feedback_velocity 反馈速度 (用于微分和速度前馈)
* @param dt 时间步长 (秒)
* @return 控制器输出
*/
double update(double setpoint, double feedback, double feedback_velocity, double dt)
{
// 1. 计算误差
state_.error = setpoint - feedback;
// 2. 死区处理
if (std::abs(state_.error) < params_.dead_zone) {
state_.error = 0.0;
}
// 3. 比例项
state_.p_term = params_.kp * state_.error;
// 4. 积分项 (带抗饱和)
state_.integral += state_.error * dt;
if (params_.enable_anti_windup) {
// 根据输出是否饱和限制积分
if (state_.output >= params_.output_max || state_.output <= params_.output_min) {
state_.integral = std::clamp(state_.integral, -params_.integral_max, params_.integral_max);
}
}
state_.i_term = params_.ki * state_.integral;
// 5. 微分项 (带滤波)
double derivative = (state_.error - state_.prev_error) / dt;
state_.filtered_d = params_.d_filter_alpha * derivative +
(1.0 - params_.d_filter_alpha) * state_.filtered_d;
state_.d_term = params_.kd * state_.filtered_d;
// 6. 速度前馈
double target_velocity = (setpoint - state_.prev_setpoint) / dt;
state_.f_term = params_.kf * target_velocity;
// 7. 加速度前馈
double target_acceleration = (target_velocity - state_.prev_velocity) / dt;
state_.f_term += params_.ka * target_acceleration;
// 8. 摩擦力补偿
double friction_comp = 0.0;
if (std::abs(feedback_velocity) > 0.001) {
friction_comp = params_.friction_coulomb * (feedback_velocity > 0 ? 1.0 : -1.0) +
params_.friction_viscous * feedback_velocity;
}
state_.f_term += friction_comp;
// 9. 重力补偿
double gravity_comp = params_.gravity_compensation * sin(feedback);
state_.f_term += gravity_comp;
// 10. 计算输出
state_.output = state_.p_term + state_.i_term + state_.d_term + state_.f_term;
// 11. 输出限幅
state_.output = std::clamp(state_.output, params_.output_min, params_.output_max);
// 12. 更新状态
state_.prev_error = state_.error;
state_.prev_output = state_.output;
state_.prev_setpoint = setpoint;
state_.prev_velocity = target_velocity;
return state_.output;
}
/**
* @brief 重置 PID 控制器。
*/
void reset()
{
state_.integral = 0.0;
state_.prev_error = 0.0;
state_.prev_output = 0.0;
state_.filtered_d = 0.0;
state_.filtered_velocity = 0.0;
}
/**
* @brief 获取当前状态。
*/
PIDState get_state() const { return state_; }
private:
PIDParams params_;
PIDState state_;
};
5.3.2 参数整定方法实现
/**
* @class PIDTuner
* @brief PID 参数整定器，提供多种整定方法。
*/
class PIDTuner
{
public:
/**
* @brief Ziegler-Nichols 整定法 (基于阶跃响应)。
* @param Kp_initial 初始比例增益
* @param Tu 振荡周期
* @param Ku 临界增益
* @return 整定后的 PID 参数
*/
PIDParams ziegler_nichols_tuning(double Kp_initial, double Tu, double Ku)
{
PIDParams params;
params.kp = 0.6 * Ku;
params.ki = 2.0 * params.kp / Tu;
params.kd = params.kp * Tu / 8.0;
return params;
}
/**
* @brief 阶跃响应法 (反应曲线法)。
* @param K 过程增益
* @param T 时间常数
* @param L 延迟时间
* @return 整定后的 PID 参数
*/
PIDParams step_response_tuning(double K, double T, double L)
{
PIDParams params;
params.kp = 1.2 * T / (K * L);
params.ki = 0.6 * T / (K * L * L);
params.kd = 0.6 * T / K;
return params;
}
/**
* @brief 基于模型的整定法 (IMC 方法)。
* @param plant 过程模型 (假设为二阶系统)
* @param lambda 闭环时间常数
* @return 整定后的 PID 参数
*/
PIDParams model_based_tuning(const SecondOrderModel &plant, double lambda)
{
PIDParams params;
// IMC 整定公式
params.kp = plant.get_time_constant() / (lambda + plant.get_time_delay());
params.ki = params.kp / plant.get_time_constant();
params.kd = params.kp * plant.get_time_constant() / 4.0;
return params;
}
/**
* @brief 自动整定 (继电反馈法)。
* @param system 系统接口
* @param timeout 整定超时 (秒)
* @return 整定后的 PID 参数
*/
PIDParams relay_feedback_auto_tuning(SystemInterface &system, double timeout)
{
// 1. 启动继电反馈
double relay_amplitude = 0.1;
double hysteresis = 0.01;
double Ku, Tu;
bool tuned = false;
// 2. 记录振荡周期和临界增益
double start_time = get_current_time();
std::vector<double> zero_crossings;
while (get_current_time() - start_time < timeout) {
double feedback = system.read_position();
double error = system.get_target() - feedback;
// 继电控制
double output;
if (error > hysteresis) {
output = relay_amplitude;
} else if (error < -hysteresis) {
output = -relay_amplitude;
} else {
output = 0.0;
}
system.write_position(output);
// 检测过零点
if (error * prev_error < 0) {
zero_crossings.push_back(get_current_time());
if (zero_crossings.size() >= 6) {
// 计算振荡周期
double period = (zero_crossings[5] - zero_crossings[0]) / 5.0;
Tu = period;
Ku = 4.0 * relay_amplitude / (M_PI * 0.5 * std::abs(error));
tuned = true;
break;
}
}
prev_error = error;
}
if (!tuned) {
return PIDParams(); // 整定失败
}
// 3. 使用 Ziegler-Nichols 公式
return ziegler_nichols_tuning(0.0, Tu, Ku);
}
};
5.3.3 前馈补偿适配
/**
* @class FeedforwardAdapter
* @brief 前馈补偿适配器，根据模型和在线估计调整前馈参数。
*/
class FeedforwardAdapter
{
public:
/**
* @brief 初始化前馈适配器。
* @param dt 控制周期 (秒)
*/
void init(double dt)
{
dt_ = dt;
prev_position_ = 0.0;
prev_velocity_ = 0.0;
}
/**
* @brief 更新前馈项 (基于模型)。
* @param target_position 目标位置
* @param current_position 当前位置
* @param current_velocity 当前速度
* @param current_acceleration 当前加速度
* @param gravity_angle 重力角度
* @return 前馈补偿力矩
*/
double update_feedforward(double target_position,
double current_position,
double current_velocity,
double current_acceleration,
double gravity_angle)
{
// 1. 计算期望速度和加速度
double target_velocity = (target_position - prev_target_position_) / dt_;
double target_acceleration = (target_velocity - prev_target_velocity_) / dt_;
// 2. 重力补偿
double gravity_comp = gravity_compensation_coeff_ * sin(gravity_angle);
// 3. 摩擦力补偿
double friction_comp = 0.0;
if (std::abs(current_velocity) > 0.001) {
friction_comp = coulomb_friction_ * (current_velocity > 0 ? 1.0 : -1.0) +
viscous_friction_ * current_velocity;
}
// 4. 惯性补偿
double inertia_comp = inertia_ * target_acceleration;
// 5. 科里奥利力补偿 (如果有多关节)
double coriolis_comp = 0.0;
if (coriolis_compensation_enabled_) {
coriolis_comp = coriolis_matrix_ * current_velocity * current_velocity;
}
// 6. 总前馈
double feedforward = gravity_comp + friction_comp + inertia_comp + coriolis_comp;
// 7. 更新状态
prev_target_position_ = target_position;
prev_target_velocity_ = target_velocity;
return feedforward;
}
/**
* @brief 在线调整前馈参数 (基于误差分析)。
* @param error 跟踪误差
* @param dt 时间步长
*/
void adapt_parameters(double error, double dt)
{
// 1. 根据误差趋势调整参数
if (std::abs(error) > error_threshold_) {
// 积分误差以调整参数
integral_error_ += error * dt;
if (std::abs(integral_error_) > integral_error_max_) {
integral_error_ = integral_error_max_ * (integral_error_ > 0 ? 1.0 : -1.0);
}
// 2. 调整摩擦补偿
if (std::abs(error) > 0.1) {
coulomb_friction_ += 0.001 * error;
coulomb_friction_ = std::clamp(coulomb_friction_, 0.0, 1.0);
}
// 3. 调整惯性补偿
if (std::abs(error) > 0.05) {
inertia_ += 0.01 * error * dt;
inertia_ = std::clamp(inertia_, 0.0, 10.0);
}
} else {
// 误差小于阈值，开始收敛
integral_error_ = 0.0;
}
}
private:
double dt_;
double prev_target_position_;
double prev_target_velocity_;
double integral_error_ = 0.0;
double integral_error_max_ = 10.0;
double error_threshold_ = 0.01;
// 前馈参数
double gravity_compensation_coeff_ = 0.0;
double coulomb_friction_ = 0.0;
double viscous_friction_ = 0.0;
double inertia_ = 0.0;
double coriolis_matrix_ = 0.0;
bool coriolis_compensation_enabled_ = false;
};
5.3.4 关节控制器封装
/**
* @class JointController
* @brief 关节控制器封装，集成 PID 和前馈补偿。
*/
class JointController
{
public:
/**
* @brief 初始化关节控制器。
* @param joint_name 关节名称
* @param pid_params PID 参数
*/
void init(const std::string &joint_name, const PIDParams &pid_params)
{
joint_name_ = joint_name;
pid_.init(pid_params);
feedforward_.init(0.001); // 1ms 控制周期
}
/**
* @brief 更新关节控制。
* @param target_position 目标位置
* @param target_velocity 目标速度 (可选)
* @param current_position 当前位置
* @param current_velocity 当前速度
* @param current_acceleration 当前加速度
* @param gravity_angle 重力角度
* @param dt 时间步长
* @return 控制输出 (力矩或速度命令)
*/
double update(double target_position,
double target_velocity,
double current_position,
double current_velocity,
double current_acceleration,
double gravity_angle,
double dt)
{
// 1. 计算 PID 输出
double pid_output = pid_.update(target_position, current_position, current_velocity, dt);
// 2. 计算前馈输出
double feedforward_output = feedforward_.update_feedforward(target_position,
current_position,
current_velocity,
current_acceleration,
gravity_angle);
// 3. 合成总输出
double total_output = pid_output + feedforward_output;
// 4. 输出限幅
total_output = std::clamp(total_output, -effort_limit_, effort_limit_);
// 5. 在线自适应 (如果启用)
if (enable_adaptation_) {
double error = target_position - current_position;
feedforward_.adapt_parameters(error, dt);
}
return total_output;
}
/**
* @brief 重置控制器。
*/
void reset()
{
pid_.reset();
feedforward_ = FeedforwardAdapter();
}
/**
* @brief 启用参数自适应。
*/
void enable_adaptation(bool enable) { enable_adaptation_ = enable; }
/**
* @brief 设置输出限幅。
*/
void set_effort_limit(double limit) { effort_limit_ = limit; }
private:
std::string joint_name_;
AdvancedPIDController pid_;
FeedforwardAdapter feedforward_;
double effort_limit_ = 100.0;
bool enable_adaptation_ = false;
};
5.4 软件模式分析
关节控制参数整定子系统 ├── PID 控制器 (PIDController) │ ├── 经典 PID (StandardPID) │ │ ├── 位置式 PID │ │ ├── 增量式 PID │ │ └── 速度式 PID │ ├── 增强型 PID (AdvancedPID) │ │ ├── 抗积分饱和 (AntiWindup) │ │ ├── 微分先行 (DerivativeOnMeasurement) │ │ ├── 二自由度 PID (TwoDegreeOfFreedomPID) │ │ └── 非线性 PID (NonlinearPID) │ └── 自适应 PID (AdaptivePID) │ ├── 增益调度 (GainScheduling) │ ├── 模型参考自适应 (MRAC) │ └── 自整定 (AutoTuning) ├── 前馈补偿 (FeedforwardCompensation) │ ├── 重力补偿 (GravityCompensation) │ ├── 摩擦力补偿 (FrictionCompensation) │ │ ├── 库仑摩擦 (Coulomb) │ │ ├── 粘性摩擦 (Viscous) │ │ └── Stribeck 模型 │ ├── 惯性补偿 (InertiaCompensation) │ ├── 科里奥利力补偿 (CoriolisCompensation) │ └── 在线参数调整 (OnlineAdaptation) │ ├── 梯度下降 (GradientDescent) │ ├── 递归最小二乘 (RLS) │ └── 扩展卡尔曼滤波 (EKF) ├── 整定方法 (TuningMethod) │ ├── 手动整定 (ManualTuning) │ │ ├── Ziegler-Nichols │ │ ├── Cohen-Coon │ │ ├── Chien-Hrones-Reswick │ │ └── 试错法 (TrialAndError) │ ├── 自动整定 (AutoTuning) │ │ ├── 继电反馈法 (RelayFeedback) │ │ ├── 频率响应法 (FrequencyResponse) │ │ ├── 基于模型的方法 (ModelBased) │ │ └── 优化方法 (OptimizationBased) │ └── 在线整定 (OnlineTuning) │ ├── 递归参数估计 (RecursiveEstimation) │ ├── 自校正调节器 (STR) │ └── 模型预测控制 (MPC) └── 关节控制器封装 (JointController) ├── 关节状态管理 (StateManagement) ├── 命令接口 (CommandInterface) ├── 状态接口 (StateInterface) ├── 参数配置 (ParameterConfiguration) └── 安全保护 (SafetyProtection)
5.5 PID 参数整定核心难点
5.5.1 积分饱和导致超调
现象：系统在响应大阶跃信号时产生较大超调，且难以稳定。
原因：
-
积分项在大误差下持续累积。
-
输出达到限幅后积分继续累积。
-
限幅解除后积分项仍保持较大值。
解决方法：
-
启用抗积分饱和：
if (output >= output_max || output <= output_min) { integral = std::clamp(integral, -integral_max, integral_max); } -
使用积分钳位：
if (output >= output_max) { integral = integral_max; } else if (output <= output_min) { integral = -integral_max; } -
使用反算方法：
double saturated = saturate(output); double feedback = (saturated - output) * back_calculation_coeff; integral += feedback * dt;
5.5.2 微分项引入噪声
现象：系统在小信号下频繁调整，造成抖动和磨损。
原因：
-
反馈信号中的高频噪声被微分放大。
-
微分增益过高。
-
采样周期过短。
解决方法：
-
添加低通滤波器：
derivative_filtered = alpha * derivative + (1 - alpha) * derivative_filtered;
-
使用微分先行：
// 微分只作用在反馈信号上 derivative = (feedback - prev_feedback) / dt;
-
降低微分增益，增加比例增益。
5.5.3 负载变化导致参数失效
现象：空载时控制正常，负载后出现跟踪误差或振荡。
原因：
-
负载增加了系统的惯性和摩擦。
-
PID 参数未随负载变化调整。
-
未进行负载辨识和补偿。
解决方法：
-
在线参数估计：
// 使用 RLS 估计负载惯性 double theta = rls_update(velocity, torque);
-
增益调度：
if (load_weight > threshold) { kp = kp_heavy * 1.5; } -
力矩前馈：
torque_feedforward = load_weight * gravity_comp;
5.5.4 多关节耦合影响
现象：单关节测试正常，多关节运动时跟踪精度下降。
原因：
-
科里奥利力和离心力未补偿。
-
关节间动力学耦合。
-
整体运动时重力分布变化。
解决方法：
-
完整动力学补偿：
// 使用 KDL/RBDL 计算完整动力学 torque = inverse_dynamics(q, qdot, qddot);
-
解耦控制：
// 计算耦合矩阵并解耦 tau_decoupled = inv_jacobian * tau;
-
分布式协调：
// 在控制器管理器层面协调 controller_manager->coordinate_multi_joint();
5.5.5 参数整定最佳实践
| 调试场景 | 推荐方法 | 调试关键点 |
|---|---|---|
| 首次整定 | 先进行阶跃响应测试，获得过程参数 | 选择合适阶跃幅度，避免过度振荡 |
| 提高响应速度 | 增加 Kp，调整 Kd | 响应速度与超调量的平衡 |
| 消除稳态误差 | 增加 Ki，加入积分项 | 积分限幅与抗饱和 |
| 减少超调 | 降低 Kp，增加 Kd，调整 Ki | 微分先行、微分滤波 |
| 提高跟踪精度 | 加入前馈补偿 | 重力补偿、摩擦补偿、惯性补偿 |
| 适应负载变化 | 启用自适应控制 | 在线参数估计、增益调度 |
| 处理非线性 | 分段 PID 或增益调度 | 不同区域使用不同参数 |
| 高速高精度 | 模型预测控制 (MPC) | 预测周期、模型精度 |
5.6 与其他模块的协同
| 模块 | 协同方式 | 调试关键点 |
|---|---|---|
| 硬件接口 | 通过硬件接口读取关节状态，写入命令 | 状态噪声、命令饱和度 |
| 多模控制器 | 作为位置/力/阻抗控制的基础层 | 命令转换、模式切换 |
| 动力学补偿 | PID 输出叠加动力学补偿力矩 | 补偿叠加时机、防冲突 |
| FOC 控制 | 将 PID 输出转换为电流指令 | 电流限幅、响应时间 |
| 控制器管理器 | 作为独立控制器加载或与其它控制器组合 | 加载顺序、切换平滑 |
| 实时通信 | 在 CANopen/EtherCAT 周期中执行计算 | 计算时间、周期同步 |
| 生命周期 | 确保控制器与硬件状态同步 | on_activate 时加载参数 |
| 监控系统 | 记录 PID 参数和跟踪误差 | 参数变化趋势、误差分析 |
第六部分 关节设计标零方案与软硬件限位保护逻辑
6.1 关节标零与限位保护的核心意义
在机器人控制中，绝对位置的确定是闭环控制的基础。关节标零（Hom/Calibration）决定了机器人的零点，直接影响运动学计算的准确性和安全性。同时，软硬件限位保护是防止机器人超出行程、避免碰撞损坏的最后防线。
6.1.1 标零方案分类对比
| 标零方式 | 原理 | 适用场景 | 优缺点 |
|---|---|---|---|
| 增量编码器 + 零位开关 | 上电后运动到零位开关触发位置 | 通用工业机器人 | 需运动到特定位置，耗时 |
| 绝对编码器 | 上电直接读取绝对位置 | 高端伺服系统 | 无需标零，但成本高 |
| 磁编码器 + 上电标零 | 使用磁编码器 + 上电后自动完成标零 | 协作机器人 | 快速，但需要额外电路 |
| 无传感器标零 | 通过电流检测或反电动势估计 | 低成本系统 | 精度低，仅适用于简易场景 |
| 光学标零 | 使用视觉识别标定点 | 移动机器人 | 灵活性高，但依赖环境 |
6.1.2 限位保护层级
[硬件限位] → [软件限位] → [运动学限位] → [控制器限位] ↓ ↓ ↓ ↓ 物理挡块 软件行程限位 关节活动范围 PID输出限幅
6.2 核心数据结构
6.2.1 关节标零参数结构
/**
* @struct JointHomingParams
* @brief 关节标零参数结构。
*/
struct JointHomingParams {
// 标零配置
enum HomingMethod {
HOMING_METHOD_HARDWARE = 0, /**< 硬件零位开关 */
HOMING_METHOD_ABSOLUTE, /**< 绝对编码器 */
HOMING_METHOD_MAGNETIC, /**< 磁编码器上电标零 */
HOMING_METHOD_SENSORLESS, /**< 无传感器标零 */
HOMING_METHOD_OPTICAL /**< 光学标零 */
} method; /**< 标零方法 */
// 硬件零位开关参数
int homing_switch_gpio; /**< 零位开关 GPIO 号 */
double homing_velocity; /**< 标零搜索速度 (rad/s) */
double homing_offset; /**< 零位偏移 (rad) */
double homing_distance; /**< 标零搜索距离 (rad) */
double homing_search_direction; /**< 标零搜索方向 (1=正向, -1=负向) */
// 绝对编码器参数
int encoder_turns; /**< 编码器圈数 */
double encoder_resolution; /**< 编码器分辨率 (脉冲/圈) */
double encoder_offset; /**< 编码器零点偏移 (rad) */
// 磁编码器参数
double magnetic_pole_pairs; /**< 磁极对数 */
double magnetic_calibration_sequence[8]; /**< 磁校准序列 */
// 光学标零参数
double optical_landmark_position; /**< 视觉标定点位置 (rad) */
double optical_landmark_size; /**< 标定点尺寸 (mm) */
double optical_search_region; /**< 搜索区域范围 (rad) */
// 通用参数
double homing_torque_limit; /**< 标零力矩限幅 (Nm) */
double homing_timeout; /**< 标零超时 (秒) */
int homing_retry_count; /**< 标零重试次数 */
bool homing_auto_start; /**< 是否上电自动标零 */
};
/**
* @struct JointLimitParams
* @brief 关节限位参数结构。
*/
struct JointLimitParams {
// 硬件限位参数
double hardware_limit_min; /**< 硬件限位最小位置 (rad) */
double hardware_limit_max; /**< 硬件限位最大位置 (rad) */
int hardware_limit_gpio_min; /**< 硬件限位最小 GPIO */
int hardware_limit_gpio_max; /**< 硬件限位最大 GPIO */
bool hardware_limit_enabled; /**< 硬件限位是否启用 */
// 软件限位参数
double software_limit_min; /**< 软件限位最小位置 (rad) */
double software_limit_max; /**< 软件限位最大位置 (rad) */
double software_limit_safety_margin; /**< 软件限位安全余量 (rad) */
bool software_limit_enabled; /**< 软件限位是否启用 */
// 运动学限位参数
double velocity_limit; /**< 速度限幅 (rad/s) */
double acceleration_limit; /**< 加速度限幅 (rad/s²) */
double jerk_limit; /**< 加加速度限幅 (rad/s³) */
double effort_limit; /**< 力矩限幅 (Nm) */
// 限位保护策略
enum LimitAction {
LIMIT_ACTION_STOP = 0, /**< 立即停止 */
LIMIT_ACTION_DECELERATE, /**< 减速停止 */
LIMIT_ACTION_HOLD, /**< 保持当前状态 */
LIMIT_ACTION_EMERGENCY_STOP /**< 紧急停止 */
} action_on_limit; /**< 触发限位时的动作 */
// 限位恢复
bool limit_auto_reset; /**< 限位后是否自动复位 */
double limit_recovery_velocity; /**< 限位恢复速度 (rad/s) */
double limit_recovery_distance; /**< 限位恢复距离 (rad) */
double limit_recovery_timeout; /**< 限位恢复超时 (秒) */
};
6.3 核心代码实现
6.3.1 硬件零位标零实现
/**
* @class HardwareHoming
* @brief 硬件零位开关标零实现。
*/
class HardwareHoming
{
public:
/**
* @brief 初始化硬件标零。
* @param params 标零参数
* @param joint_interface 关节接口
*/
void init(const JointHomingParams ¶ms, JointInterface &joint_interface)
{
params_ = params;
joint_interface_ = joint_interface;
homing_state_ = HomingState::IDLE;
}
/**
* @brief 执行标零流程。
* @return 0 成功，-1 失败
*/
int perform_homing()
{
// 1. 检查硬件零位开关是否已触发
if (is_homing_switch_triggered()) {
// 如果已经在零位，直接设置零点
set_absolute_zero();
return 0;
}
// 2. 设置标零速度和方向
double velocity = params_.homing_velocity * params_.homing_search_direction;
joint_interface_.set_velocity_command(velocity);
// 3. 搜索零位开关
double start_position = joint_interface_.get_position();
double timeout_timer = 0.0;
bool switch_found = false;
while (timeout_timer < params_.homing_timeout) {
// 检查零位开关状态
if (is_homing_switch_triggered()) {
switch_found = true;
break;
}
// 检查是否超出搜索距离
double current_position = joint_interface_.get_position();
if (std::abs(current_position - start_position) > params_.homing_distance) {
break;
}
// 检查力矩限幅
if (joint_interface_.get_effort() > params_.homing_torque_limit) {
break;
}
usleep(10000); // 10ms
timeout_timer += 0.01;
}
// 4. 停止运动
joint_interface_.set_velocity_command(0.0);
if (!switch_found) {
// 标零失败
return -1;
}
// 5. 设置绝对零点
set_absolute_zero();
return 0;
}
private:
bool is_homing_switch_triggered()
{
// 读取 GPIO 状态
return gpio_read(params_.homing_switch_gpio) == 1;
}
void set_absolute_zero()
{
// 设置当前位置为绝对零点
double current_position = joint_interface_.get_position();
double zero_offset = current_position + params_.homing_offset;
joint_interface_.set_absolute_zero(zero_offset);
}
JointHomingParams params_;
JointInterface &joint_interface_;
enum HomingState {
IDLE,
SEARCHING,
FOUND,
ZERO_SET,
FAILED
} homing_state_;
};
6.3.2 绝对编码器标零实现
/**
* @class AbsoluteEncoderHoming
* @brief 绝对编码器标零实现。
*/
class AbsoluteEncoderHoming
{
public:
/**
* @brief 初始化绝对编码器标零。
* @param params 标零参数
*/
void init(const JointHomingParams ¶ms)
{
params_ = params;
}
/**
* @brief 从绝对编码器读取位置。
* @return 绝对位置 (rad)
*/
double get_absolute_position()
{
// 1. 读取编码器原始值
int32_t raw_value = read_encoder_raw();
// 2. 计算绝对位置
double absolute_position = (raw_value / params_.encoder_resolution) * 2 * M_PI;
// 3. 应用零点偏移
absolute_position += params_.encoder_offset;
return absolute_position;
}
private:
int32_t read_encoder_raw()
{
// 通过 SPI/I2C 读取绝对编码器
return encoder_read_reg(ENCODER_POSITION_REG);
}
JointHomingParams params_;
};
6.3.3 软件限位保护实现
/**
* @class SoftwareLimitProtection
* @brief 软件限位保护实现。
*/
class SoftwareLimitProtection
{
public:
/**
* @brief 初始化软件限位保护。
* @param limit_params 限位参数
*/
void init(const JointLimitParams &limit_params)
{
limit_params_ = limit_params;
last_position_ = 0.0;
last_velocity_ = 0.0;
}
/**
* @brief 检查位置是否在限位范围内。
* @param position 当前位置
* @param velocity 当前速度
* @return true 在限位范围内，false 超限
*/
bool check_limits(double position, double velocity)
{
// 1. 检查软件限位
if (limit_params_.software_limit_enabled) {
if (position < limit_params_.software_limit_min ||
position > limit_params_.software_limit_max) {
handle_limit_violation("Software position limit");
return false;
}
}
// 2. 检查速度限幅
if (std::abs(velocity) > limit_params_.velocity_limit) {
handle_limit_violation("Velocity limit");
return false;
}
// 3. 检查加速度限幅
double acceleration = (velocity - last_velocity_) / 0.001; // 假设 1ms 周期
if (std::abs(acceleration) > limit_params_.acceleration_limit) {
handle_limit_violation("Acceleration limit");
return false;
}
last_position_ = position;
last_velocity_ = velocity;
return true;
}
/**
* @brief 生成受限时的控制输出。
* @param desired_output 期望输出
* @param position 当前位置
* @param velocity 当前速度
* @return 受限后的输出
*/
double apply_limit_output(double desired_output, double position, double velocity)
{
// 1. 计算到限位的距离和方向
double distance_to_min = position - limit_params_.software_limit_min;
double distance_to_max = limit_params_.software_limit_max - position;
// 2. 根据距离动态调整输出
if (distance_to_min < 0.1 && desired_output < 0) {
// 接近下限且输出为负方向，减速
double reduction = std::min(1.0, distance_to_min / 0.1);
return desired_output * reduction;
}
if (distance_to_max < 0.1 && desired_output > 0) {
// 接近上限且输出为正方向，减速
double reduction = std::min(1.0, distance_to_max / 0.1);
return desired_output * reduction;
}
return desired_output;
}
private:
void handle_limit_violation(const std::string &reason)
{
// 1. 记录日志
printf("Limit violation: %s\n", reason.c_str());
// 2. 执行限位动作
switch (limit_params_.action_on_limit) {
case JointLimitParams::LIMIT_ACTION_STOP:
// 立即停止
joint_interface_.set_velocity_command(0.0);
break;
case JointLimitParams::LIMIT_ACTION_DECELERATE:
// 减速停止
joint_interface_.set_velocity_command(joint_interface_.get_velocity() * 0.5);
break;
case JointLimitParams::LIMIT_ACTION_HOLD:
// 保持当前状态
break;
case JointLimitParams::LIMIT_ACTION_EMERGENCY_STOP:
// 紧急停止
joint_interface_.set_effort_command(0.0);
break;
}
}
JointLimitParams limit_params_;
double last_position_;
double last_velocity_;
JointInterface &joint_interface_;
};
6.3.4 硬件限位保护实现
/**
* @class HardwareLimitProtection
* @brief 硬件限位保护实现（使用 GPIO 读取限位开关状态）。
*/
class HardwareLimitProtection
{
public:
/**
* @brief 初始化硬件限位保护。
* @param limit_params 限位参数
*/
void init(const JointLimitParams &limit_params)
{
limit_params_ = limit_params;
// 配置限位开关 GPIO 为输入
if (limit_params_.hardware_limit_enabled) {
gpio_set_direction(limit_params_.hardware_limit_gpio_min, GPIO_INPUT);
gpio_set_direction(limit_params_.hardware_limit_gpio_max, GPIO_INPUT);
}
}
/**
* @brief 检查硬件限位是否触发。
* @return true 触发，false 未触发
*/
bool check_hardware_limits()
{
if (!limit_params_.hardware_limit_enabled) {
return false;
}
// 1. 读取最小限位开关
if (gpio_read(limit_params_.hardware_limit_gpio_min) == 1) {
handle_hardware_limit("Minimum hardware limit");
return true;
}
// 2. 读取最大限位开关
if (gpio_read(limit_params_.hardware_limit_gpio_max) == 1) {
handle_hardware_limit("Maximum hardware limit");
return true;
}
return false;
}
private:
void handle_hardware_limit(const std::string &reason)
{
printf("Hardware limit triggered: %s\n", reason.c_str());
// 1. 立即停止
joint_interface_.set_effort_command(0.0);
joint_interface_.set_velocity_command(0.0);
// 2. 执行紧急停止
joint_interface_.emergency_stop();
}
JointLimitParams limit_params_;
JointInterface &joint_interface_;
};
6.3.5 限位恢复机制
/**
* @class LimitRecovery
* @brief 限位恢复机制实现。
*/
class LimitRecovery
{
public:
/**
* @brief 初始化限位恢复。
* @param limit_params 限位参数
*/
void init(const JointLimitParams &limit_params)
{
limit_params_ = limit_params;
}
/**
* @brief 执行限位恢复。
* @param position 当前位置
* @param limit_min 最小限位
* @param limit_max 最大限位
* @return 0 成功，-1 失败
*/
int recover_from_limit(double position, double limit_min, double limit_max)
{
if (!limit_params_.limit_auto_reset) {
return -1;
}
// 1. 确定恢复方向
double direction;
if (position < limit_min) {
// 下限位，向正向运动
direction = 1.0;
} else if (position > limit_max) {
// 上限位，向负向运动
direction = -1.0;
} else {
return 0; // 已在限位内
}
// 2. 恢复运动
double recovery_velocity = direction * limit_params_.limit_recovery_velocity;
joint_interface_.set_velocity_command(recovery_velocity);
// 3. 等待恢复完成
double timeout = 0.0;
while (timeout < limit_params_.limit_recovery_timeout) {
double current_pos = joint_interface_.get_position();
if (current_pos > limit_min + 0.05 && current_pos < limit_max - 0.05) {
// 已恢复安全区域
joint_interface_.set_velocity_command(0.0);
return 0;
}
usleep(10000);
timeout += 0.01;
}
// 恢复超时
joint_interface_.set_velocity_command(0.0);
return -1;
}
private:
JointLimitParams limit_params_;
JointInterface &joint_interface_;
};
6.4 软件模式分析
关节标零与限位保护子系统 ├── 关节标零 (JointHoming) │ ├── 硬件标零 (HardwareHoming) │ │ ├── 零位开关搜索 │ │ ├── 自旋搜索 │ │ ├── 双向搜索 │ │ └── 接触检测标零 │ ├── 绝对编码器标零 (AbsoluteEncoderHoming) │ │ ├── 单圈绝对位置读取 │ │ ├── 多圈绝对位置读取 │ │ └── 零点偏移校准 │ ├── 磁编码器标零 (MagneticHoming) │ │ ├── 磁极检测 │ │ ├── 磁极序列校准 │ │ └── 上电自动标零 │ ├── 光学标零 (OpticalHoming) │ │ ├── 视觉识别标定点 │ │ └── 搜索区域扫描 │ └── 无传感器标零 (SensorlessHoming) │ ├── 电流检测法 │ ├── 反电动势检测法 │ └── 冲击检测法 ├── 软件限位保护 (SoftwareLimitProtection) │ ├── 位置限位 (PositionLimit) │ ├── 速度限位 (VelocityLimit) │ ├── 加速度限位 (AccelerationLimit) │ ├── 力矩限位 (EffortLimit) │ └── 柔性限位 (SoftLimit) │ ├── 预减速区 │ ├── 动态限位 │ └── 路径规划限位 ├── 硬件限位保护 (HardwareLimitProtection) │ ├── 限位开关 (LimitSwitch) │ │ ├── 机械限位开关 │ │ ├── 光电限位开关 │ │ └── 磁感应限位开关 │ ├── 限位传感器 (LimitSensor) │ ├── 紧急停止按钮 (EmergencyStop) │ └── 硬件看门狗 (HardwareWatchdog) └── 限位恢复 (LimitRecovery) ├── 自动恢复 (AutoRecovery) │ ├── 反向运动恢复 │ └── 慢速恢复 ├── 手动恢复 (ManualRecovery) │ ├── 手动反向运动 │ └── 手动释放限位 └── 远程恢复 (RemoteRecovery) ├── 远程指令恢复 └── 远程重置限位
6.5 标零与限位调试核心难点
6.5.1 标零重复性差
现象：多次标零后，零点位置偏差较大。
原因：
-
零位开关触发一致性差。
-
标零速度不稳定。
-
编码器分辨率不足。
解决方法：
-
采用双方向标零：
// 先正向搜索到零位，再反向搜索 velocity = homing_velocity; while (!switch_triggered) { ... } velocity = -homing_velocity * 0.2; while (switch_triggered) { ... } set_zero(); -
增加标零速度滤波。
-
使用更高分辨率的编码器。
6.5.2 限位开关误触发
现象：正常运动时频繁触发硬件限位。
原因：
-
限位开关安装位置偏差。
-
振动导致误触发。
-
限位开关灵敏度设置不当。
解决方法：
-
增加软件滤波：
if (gpio_read(limit_pin) == 1 && prev_gpio == 1) { // 确认是真实触发 } -
设置安全余量：
software_limit_min = hardware_limit_min + 0.1; software_limit_max = hardware_limit_max - 0.1;
-
使用冗余限位开关：两个开关串联，同时触发才有效。
6.5.3 限位后无法恢复
现象：触发限位后，无论怎么做都无法解除限位状态。
原因：
-
恢复速度设置过慢。
-
恢复距离不足。
-
限位恢复被限位逻辑阻止。
解决方法：
-
提高恢复速度。
-
增加恢复距离。
-
限位恢复时临时禁用限位检查：
if (in_recovery_mode) { limit_check_enabled = false; }
6.5.4 限位逻辑死锁
现象：限位触发后系统进入死循环，无法正常响应。
原因：
-
限位处理函数中调用了会重新触发限位的操作。
-
恢复逻辑不完善，无法退出限位状态。
-
限位恢复时再次触发限位。
解决方法：
-
使用状态机管理限位状态：
enum LimitState { NORMAL, LIMIT_MIN_TRIGGERED, LIMIT_MAX_TRIGGERED, RECOVERING }; -
恢复时确保不会再次触发限位。
-
设置超时机制：限位恢复超时后进入错误状态。
6.5.5 标零与限位协同最佳实践
| 调试场景 | 推荐方法 | 调试关键点 |
|---|---|---|
| 首次上电调试 | 先手动慢速运动到零位，进行人工标零 | 确认零位开关位置 |
| 标零精度优化 | 降低标零速度，增加标零方向切换次数 | 速度、等待时间 |
| 限位调试 | 用慢速逐个触发限位，验证动作 | 限位触发位置、响应时间 |
| 限位恢复测试 | 触发限位后测试恢复逻辑 | 恢复速度、恢复后状态 |
| 长期运行稳定性 | 定期重新标零，消除累积误差 | 标零间隔、漂移检测 |
6.6 与其他模块的协同
| 模块 | 协同方式 | 调试关键点 |
|---|---|---|
| 硬件接口 | 提供关节位置、限位 GPIO、编码器接口 | 状态噪声、GPIO 抖动 |
| 多模控制器 | 限位触发后控制器输出限制 | 限位输出处理、恢复后 PID 重初始化 |
| FOC 控制 | 限位时关闭电流环或限流 | 电流限幅、响应时间 |
| PID 控制 | 限位时重置积分项 | 积分复位、防突变 |
| 控制器管理器 | 限位时通知管理器切换至安全模式 | 切换时机、模式恢复 |
| 实时通信 | 限位事件通过 CANopen/EtherCAT 上报 | 事件上报延迟、响应时间 |
| 生命周期 | 限位严重时进入错误状态 | 状态切换、错误恢复 |
| 监控系统 | 记录限位事件和恢复过程 | 日志分析、趋势预测 |
第七部分：灵巧手控模式设计与触力反馈
7.1 灵巧手控制的核心概念
灵巧手是多指仿生机械手，相比传统二指夹爪，它能实现更丰富的抓取模式和更精细的操作能力。灵巧手控制的核心挑战在于：多关节协调控制、触力感知与反馈、抓取策略选择和末端执行器精准调控。
7.1.1 灵巧手控制模式分类
| 控制模式 | 适用场景 | 控制策略 | 传感器需求 |
|---|---|---|---|
| 捏合控制 (Pinch Control) | 抓取细小物体 | 指尖位置 + 力控制 | 指尖压力传感器 |
| 握持控制 (Power Grasp) | 抓取大物体 | 手掌闭合 + 恒力控制 | 掌心压力传感器 |
| 精确控制 (Precision Control) | 精密操作 | 多指协调位置 + 力反馈 | 关节位置 + 指尖力 |
| 挤压控制 (Squeeze Control) | 软体操作 | 力控制 + 顺应性 | 触觉传感器 |
| 自适应性抓取 (Adaptive Grasp) | 未知形状物体 | 触觉反馈 + 闭环调节 | 分布式触觉传感器 |
7.1.2 灵巧手控制架构
[目标物体] → [抓取规划器] → [抓取策略选择] → [多指协调控制] → [灵巧手硬件] ↑ ↓ ↑ | [抓取模式切换] [触力反馈处理] | ↓ ↑ └────────────────[触觉传感器]─────────────────────┘
7.2 核心数据结构
7.2.1 灵巧手关节配置结构
/**
* @struct DexterousHandJointConfig
* @brief 灵巧手关节配置结构。
*/
struct DexterousHandJointConfig {
struct Joint {
std::string name; /**< 关节名称 */
int joint_id; /**< 关节 ID */
double min_position; /**< 最小位置 (rad) */
double max_position; /**< 最大位置 (rad) */
double max_velocity; /**< 最大速度 (rad/s) */
double max_effort; /**< 最大力矩 (Nm) */
double home_position; /**< 回零位置 (rad) */
bool has_force_sensor; /**< 是否有力传感器 */
int force_sensor_id; /**< 力传感器 ID */
};
std::vector<Joint> finger_joints; /**< 手指关节列表 */
std::vector<Joint> thumb_joints; /**< 拇指关节列表 */
std::vector<Joint> wrist_joints; /**< 手腕关节列表 */
int finger_count; /**< 手指数量 */
int thumb_joint_count; /**< 拇指关节数量 */
int total_joints; /**< 总关节数 */
std::vector<std::vector<int>> finger_joint_map; /**< 手指关节映射表 */
};
/**
* @struct GraspConfig
* @brief 抓取配置结构。
*/
struct GraspConfig {
enum GraspType {
GRASP_TYPE_PINCH = 0, /**< 捏合抓取 */
GRASP_TYPE_POWER, /**< 握持抓取 */
GRASP_TYPE_PRECISION, /**< 精确抓取 */
GRASP_TYPE_SQUEEZE, /**< 挤压抓取 */
GRASP_TYPE_ADAPTIVE /**< 自适应抓取 */
} type; /**< 抓取类型 */
std::vector<double> target_positions; /**< 目标关节位置 */
std::vector<double> target_forces; /**< 目标指尖力 */
std::vector<double> current_forces; /**< 当前指尖力 */
std::vector<double> force_gains; /**< 力控制增益 */
double target_force_total; /**< 总目标力 (N) */
double max_force_total; /**< 最大总力 (N) */
double min_force_total; /**< 最小总力 (N) */
double force_tolerance; /**< 力容差 (N) */
double position_tolerance; /**< 位置容差 (rad) */
double grasp_velocity; /**< 抓取速度 (rad/s) */
double grasp_duration; /**< 抓取持续时间 (秒) */
double release_duration; /**< 释放持续时间 (秒) */
bool force_closed_loop; /**< 是否力闭环控制 */
bool position_prioritized; /**< 位置优先还是力优先 */
std::vector<int> active_fingers; /**< 参与抓取的手指索引 */
};
/**
* @struct TactileSensorData
* @brief 触觉传感器数据结构。
*/
struct TactileSensorData {
struct SensorPoint {
int sensor_id; /**< 传感器 ID */
double pressure; /**< 压力值 (kPa) */
double temperature; /**< 温度 (℃) */
double x; /**< X 坐标 */
double y; /**< Y 坐标 */
double z; /**< Z 坐标 */
uint8_t status; /**< 状态位 */
};
std::vector<SensorPoint> points; /**< 传感器点阵数据 */
int frame_count; /**< 帧计数 */
double timestamp; /**< 时间戳 (秒) */
double avg_pressure; /**< 平均压力 */
double max_pressure; /**< 最大压力 */
double min_pressure; /**< 最小压力 */
std::vector<double> pressure_distribution; /**< 压力分布矩阵 */
};
7.3 核心代码实现
7.3.1 灵巧手控制器基类
/**
* @class DexterousHandController
* @brief 灵巧手控制器基类，实现通用的多指协调控制。
*/
class DexterousHandController : public controller_interface::ControllerInterface
{
public:
DexterousHandController() = default;
controller_interface::InterfaceConfiguration command_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto &joint : joint_config_.total_joints) {
config.names.push_back(joint.name + "/position");
config.names.push_back(joint.name + "/effort");
}
return config;
}
controller_interface::InterfaceConfiguration state_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto &joint : joint_config_.total_joints) {
config.names.push_back(joint.name + "/position");
config.names.push_back(joint.name + "/velocity");
config.names.push_back(joint.name + "/effort");
}
return config;
}
controller_interface::return_type init(const std::string &controller_name) override
{
// 1. 加载关节配置
if (!load_joint_config()) {
return controller_interface::return_type::ERROR;
}
// 2. 初始化抓取策略
grasp_strategy_ = std::make_unique<GraspStrategyManager>();
// 3. 初始化触觉传感器
tactile_sensor_ = std::make_unique<TactileSensorManager>();
return controller_interface::return_type::OK;
}
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
{
// 1. 读取当前关节状态
update_joint_states();
// 2. 读取触觉传感器数据
tactile_sensor_->update();
// 3. 根据当前抓取模式执行控制
switch (current_grasp_config_.type) {
case GraspConfig::GRASP_TYPE_PINCH:
execute_pinch_control();
break;
case GraspConfig::GRASP_TYPE_POWER:
execute_power_grasp();
break;
case GraspConfig::GRASP_TYPE_PRECISION:
execute_precision_control();
break;
case GraspConfig::GRASP_TYPE_SQUEEZE:
execute_squeeze_control();
break;
case GraspConfig::GRASP_TYPE_ADAPTIVE:
execute_adaptive_grasp();
break;
}
// 4. 更新命令接口
update_command_interfaces();
return controller_interface::return_type::OK;
}
protected:
/**
* @brief 执行捏合控制。
*/
virtual void execute_pinch_control() = 0;
/**
* @brief 执行握持控制。
*/
virtual void execute_power_grasp() = 0;
/**
* @brief 执行精确控制。
*/
virtual void execute_precision_control() = 0;
/**
* @brief 执行挤压控制。
*/
virtual void execute_squeeze_control() = 0;
/**
* @brief 执行自适应抓取。
*/
virtual void execute_adaptive_grasp() = 0;
DexterousHandJointConfig joint_config_;
GraspConfig current_grasp_config_;
std::unique_ptr<GraspStrategyManager> grasp_strategy_;
std::unique_ptr<TactileSensorManager> tactile_sensor_;
std::vector<hardware_interface::CommandInterface> command_interfaces_;
std::vector<hardware_interface::StateInterface> state_interfaces_;
std::vector<double> joint_positions_;
std::vector<double> joint_velocities_;
std::vector<double> joint_efforts_;
};
7.3.2 抓取策略管理器
/**
* @class GraspStrategyManager
* @brief 抓取策略管理器，管理不同抓取模式的切换和参数。
*/
class GraspStrategyManager
{
public:
/**
* @brief 初始化抓取策略管理器。
*/
void init(const DexterousHandJointConfig &config)
{
joint_config_ = config;
current_grasp_type_ = GraspConfig::GRASP_TYPE_PINCH;
}
/**
* @brief 切换抓取模式。
* @param type 抓取类型
* @param params 抓取参数
*/
void switch_grasp_type(GraspConfig::GraspType type, const GraspConfig ¶ms)
{
// 1. 检查是否合法切换
if (!is_valid_switch(current_grasp_type_, type)) {
return;
}
// 2. 执行切换前处理
pre_switch_actions(current_grasp_type_, type);
// 3. 更新抓取配置
current_grasp_config_ = params;
current_grasp_type_ = type;
// 4. 执行切换后处理
post_switch_actions(current_grasp_type_);
}
/**
* @brief 获取当前抓取配置。
*/
const GraspConfig& get_current_grasp_config() const
{
return current_grasp_config_;
}
/**
* @brief 根据物体信息选择最佳抓取模式。
* @param object_info 物体信息
* @return 推荐的抓取类型
*/
GraspConfig::GraspType select_grasp_type(const ObjectInfo &object_info)
{
// 1. 根据物体大小和形状选择
if (object_info.size < 0.02) {
return GraspConfig::GRASP_TYPE_PINCH;
} else if (object_info.size > 0.1) {
return GraspConfig::GRASP_TYPE_POWER;
} else if (object_info.material == "soft") {
return GraspConfig::GRASP_TYPE_SQUEEZE;
} else if (object_info.shape == "irregular") {
return GraspConfig::GRASP_TYPE_ADAPTIVE;
} else {
return GraspConfig::GRASP_TYPE_PRECISION;
}
}
private:
bool is_valid_switch(GraspConfig::GraspType from, GraspConfig::GraspType to)
{
// 检查切换是否合法
if (from == to) return true;
if (from == GraspConfig::GRASP_TYPE_POWER && to == GraspConfig::GRASP_TYPE_PINCH) {
return false; // 不能直接从握持切换到捏合
}
return true;
}
void pre_switch_actions(GraspConfig::GraspType from, GraspConfig::GraspType to)
{
// 切换前处理：释放当前抓取
release_current_grasp();
}
void post_switch_actions(GraspConfig::GraspType to)
{
// 切换后处理：重新初始化力控制
reset_force_control();
}
DexterousHandJointConfig joint_config_;
GraspConfig current_grasp_config_;
GraspConfig::GraspType current_grasp_type_;
};
7.3.3 触觉传感器管理器
/**
* @class TactileSensorManager
* @brief 触觉传感器管理器，处理传感器数据并提供反馈。
*/
class TactileSensorManager
{
public:
/**
* @brief 初始化触觉传感器。
* @param sensor_count 传感器数量
* @param sample_rate 采样率 (Hz)
*/
void init(int sensor_count, int sample_rate)
{
sensor_count_ = sensor_count;
sample_rate_ = sample_rate;
sensor_data_ = std::make_unique<TactileSensorData>();
// 初始化传感器点阵
sensor_data_->points.resize(sensor_count * 16); // 每个传感器16个点
}
/**
* @brief 更新传感器数据。
*/
void update()
{
// 1. 读取传感器硬件数据
std::vector<uint8_t> raw_data = read_sensor_hardware();
// 2. 解析传感器数据
parse_sensor_data(raw_data);
// 3. 计算统计信息
compute_sensor_statistics();
// 4. 检测接触状态
detect_contact();
}
/**
* @brief 获取压力分布矩阵。
*/
std::vector<double> get_pressure_distribution() const
{
return sensor_data_->pressure_distribution;
}
/**
* @brief 检测接触事件。
* @return true 有接触，false 无接触
*/
bool detect_contact()
{
double avg_pressure = sensor_data_->avg_pressure;
double max_pressure = sensor_data_->max_pressure;
// 如果平均压力超过阈值，认为有接触
if (avg_pressure > contact_threshold_) {
contact_detected_ = true;
contact_timestamp_ = get_current_time();
} else {
contact_detected_ = false;
}
return contact_detected_;
}
/**
* @brief 获取接触位置。
*/
std::pair<double, double> get_contact_position() const
{
// 计算接触点的质心
if (!contact_detected_) {
return {0, 0};
}
double total_pressure = 0.0;
double cx = 0.0, cy = 0.0;
for (const auto &point : sensor_data_->points) {
if (point.pressure > contact_threshold_) {
total_pressure += point.pressure;
cx += point.x * point.pressure;
cy += point.y * point.pressure;
}
}
if (total_pressure > 0) {
return {cx / total_pressure, cy / total_pressure};
}
return {0, 0};
}
private:
std::vector<uint8_t> read_sensor_hardware()
{
// 通过 I2C/SPI 读取传感器硬件
std::vector<uint8_t> raw_data;
// ...
return raw_data;
}
void parse_sensor_data(const std::vector<uint8_t> &raw_data)
{
// 解析原始数据到传感器点阵
// ...
}
void compute_sensor_statistics()
{
double sum = 0.0;
double max_val = 0.0;
double min_val = 1000.0;
for (const auto &point : sensor_data_->points) {
sum += point.pressure;
max_val = std::max(max_val, point.pressure);
min_val = std::min(min_val, point.pressure);
}
sensor_data_->avg_pressure = sum / sensor_data_->points.size();
sensor_data_->max_pressure = max_val;
sensor_data_->min_pressure = min_val;
}
int sensor_count_;
int sample_rate_;
std::unique_ptr<TactileSensorData> sensor_data_;
bool contact_detected_ = false;
double contact_threshold_ = 10.0; // kPa
double contact_timestamp_ = 0.0;
};
7.3.4 捏合控制实现
/**
* @class PinchController
* @brief 捏合控制器实现。
*/
class PinchController : public DexterousHandController
{
protected:
void execute_pinch_control() override
{
// 1. 获取目标位置和力
std::vector<double> target_positions = current_grasp_config_.target_positions;
std::vector<double> target_forces = current_grasp_config_.target_forces;
// 2. 获取当前触觉反馈
auto pressure = tactile_sensor_->get_pressure_distribution();
auto contact_pos = tactile_sensor_->get_contact_position();
// 3. 计算位置误差
for (size_t i = 0; i < joint_config_.total_joints.size(); ++i) {
double error = target_positions[i] - joint_positions_[i];
// 4. 计算力误差 (如果有力传感器)
double force_error = 0.0;
if (i < target_forces.size()) {
double current_force = joint_efforts_[i];
force_error = target_forces[i] - current_force;
}
// 5. 位置控制与力控制混合 (位置优先)
double command = kp_ * error;
if (current_grasp_config_.force_closed_loop) {
// 叠加力控制
command += kf_ * force_error;
}
// 6. 限幅
command = std::clamp(command, -joint_config_.total_joints[i].max_effort,
joint_config_.total_joints[i].max_effort);
command_interfaces_[i].set_value(command);
}
}
void execute_power_grasp() override
{
// 握持控制实现
// ...
}
void execute_precision_control() override
{
// 精确控制实现
// ...
}
void execute_squeeze_control() override
{
// 挤压控制实现
// ...
}
void execute_adaptive_grasp() override
{
// 自适应抓取实现
// ...
}
private:
double kp_ = 10.0;
double kf_ = 0.5;
};
7.3.5 自适应抓取控制
/**
* @class AdaptiveGraspController
* @brief 自适应抓取控制器，根据触觉反馈实时调整抓取力。
*/
class AdaptiveGraspController : public DexterousHandController
{
protected:
void execute_adaptive_grasp() override
{
// 1. 读取触觉传感器数据
auto pressure_dist = tactile_sensor_->get_pressure_distribution();
double max_pressure = tactile_sensor_->get_max_pressure();
double avg_pressure = tactile_sensor_->get_avg_pressure();
// 2. 检测滑动 (通过压力分布变化)
bool sliding = detect_sliding(pressure_dist);
// 3. 根据滑动检测调整抓取力
if (sliding) {
// 滑动检测，增加抓取力
current_grasp_config_.target_forces[0] *= 1.1;
// 限制最大力
if (current_grasp_config_.target_forces[0] > current_grasp_config_.max_force_total) {
current_grasp_config_.target_forces[0] = current_grasp_config_.max_force_total;
}
} else {
// 稳定状态，可以逐渐减小抓取力 (降低功耗)
if (avg_pressure > current_grasp_config_.target_force_total * 1.5) {
current_grasp_config_.target_forces[0] *= 0.99;
if (current_grasp_config_.target_forces[0] < current_grasp_config_.min_force_total) {
current_grasp_config_.target_forces[0] = current_grasp_config_.min_force_total;
}
}
}
// 4. 执行力控制
for (size_t i = 0; i < joint_config_.total_joints.size(); ++i) {
double force_error = current_grasp_config_.target_forces[i] - joint_efforts_[i];
double command = force_pid_[i].update(force_error, 0.001);
command_interfaces_[i].set_value(command);
}
}
private:
bool detect_sliding(const std::vector<double> &pressure_dist)
{
// 计算压力分布的方差变化
static double prev_variance = 0.0;
double current_variance = compute_variance(pressure_dist);
double variance_change = std::abs(current_variance - prev_variance);
prev_variance = current_variance;
// 如果方差变化超过阈值，认为发生滑动
return variance_change > variance_threshold_;
}
double compute_variance(const std::vector<double> &data)
{
double mean = 0.0;
for (double v : data) mean += v;
mean /= data.size();
double variance = 0.0;
for (double v : data) {
variance += (v - mean) * (v - mean);
}
variance /= data.size();
return variance;
}
std::vector<PIDController> force_pid_;
double variance_threshold_ = 0.1;
};
7.4 软件模式树形分析
灵巧手控制子系统 ├── 灵巧手控制器 (DexterousHandController) │ ├── 捏合控制器 (PinchController) │ ├── 握持控制器 (PowerGraspController) │ ├── 精确控制器 (PrecisionController) │ ├── 挤压控制器 (SqueezeController) │ └── 自适应控制器 (AdaptiveGraspController) ├── 抓取策略 (GraspStrategy) │ ├── 抓取规划 (GraspPlanning) │ │ ├── 物体识别 │ │ ├── 接触点计算 │ │ └── 力分配优化 │ ├── 抓取模式选择 (GraspModeSelection) │ ├── 抓取切换 (GraspTransition) │ └── 抓取释放 (GraspRelease) ├── 触觉反馈 (TactileFeedback) │ ├── 传感器采集 (SensorAcquisition) │ │ ├── 数据解析 │ │ ├── 信号滤波 │ │ └── 压力映射 │ ├── 接触检测 (ContactDetection) │ │ ├── 阈值检测 │ │ ├── 滑动检测 │ │ └── 接触位置估计 │ └── 力反馈 (ForceFeedback) │ ├── 力映射 (ForceMapping) │ ├── 力限制 (ForceLimiting) │ └── 自适应增益 (AdaptiveGain) └── 末端执行器调控 (EndEffectorControl) ├── 位置控制 (PositionControl) ├── 力控制 (ForceControl) ├── 阻抗控制 (ImpedanceControl) ├── 导纳控制 (AdmittanceControl) └── 混合控制 (HybridControl)
7.5 灵巧手调试核心难点
7.5.1 接触检测不准确
现象：灵巧手接触物体时，传感器反馈不准确或延迟。
原因：
-
传感器采样率不足。
-
压力传感器校准不准确。
-
环境噪声干扰大。
解决方法：
-
提高传感器采样率 (从 100Hz 提高到 500Hz)。
-
定期校准传感器零点：
void calibrate_sensors() { // 空载状态下校准零点 zero_offset_ = read_sensor_raw(); } -
使用低通滤波：
filtered_pressure = alpha * raw_pressure + (1 - alpha) * filtered_pressure;
7.5.2 抓取过程中物体滑动
现象：抓取物体后，物体在手中滑动或脱落。
原因：
-
抓取力不足。
-
接触点位置不合理。
-
物体表面摩擦系数低。
解决方法：
-
逐步增加抓取力，直到滑动停止。
-
多指协同：增加参与抓取的手指数量。
-
使用自适应抓取策略，实时调整抓取力。
7.5.3 力控制震荡
现象：力控制模式下，力反馈出现振荡或抖动。
原因：
-
力控制增益过高。
-
传感器噪声放大。
-
机械系统柔性导致控制不稳定。
解决方法：
-
降低力控制增益，增加阻尼项。
-
增加传感器滤波。
-
使用力-位置混合控制，限制位置变化。
7.5.4 多指协调困难
现象：多指同时运动时，手指之间出现干扰或不协调。
原因：
-
各手指运动学耦合。
-
控制周期不同步。
-
缺乏协调控制策略。
解决方法：
-
使用主-从控制：选择一个手指为主，其他跟随。
-
同步控制周期。
-
使用任务空间控制：在笛卡尔空间规划手指运动。
7.5.5 灵巧手调试最佳实践
| 调试场景 | 推荐方法 | 调试关键点 |
|---|---|---|
| 首次校准 | 执行空载校准，确认传感器零点 | 零点偏移、温度漂移 |
| 简单抓取测试 | 从捏合控制开始，逐步增加复杂度 | 抓取稳定性、力控制精度 |
| 滑动检测测试 | 使用不同摩擦力物体测试滑动检测 | 检测灵敏度、响应时间 |
| 力控制优化 | 使用力阶跃响应测试，调整增益 | 响应速度、超调量 |
| 多指协同调试 | 使用慢速运动测试手指协调性 | 同步精度、干涉检测 |
7.6 与其他模块的协同
| 模块 | 协同方式 | 调试关键点 |
|---|---|---|
| 硬件接口 | 提供灵巧手关节状态，写入位置/力矩命令 | 关节噪声、命令饱和度 |
| 多模控制器 | 作为灵巧手的底层控制框架 | 模式切换、力-位置切换 |
| 动力学补偿 | 灵巧手轻量化，重力补偿影响大 | 重力模型、末端负载 |
| FOC 控制 | 灵巧手手指使用微型电机驱动 | 电流限幅、响应时间 |
| PID 控制 | 关节级 PID 控制，力控制叠加 | PID 参数整定、力反馈滤波 |
| 控制器管理器 | 作为高级控制器加载 | 控制器加载顺序、切换平滑 |
| 实时通信 | 灵巧手控制周期高 (1-2ms) | 周期抖动、数据延迟 |
| 监控系统 | 记录触觉数据和抓取状态 | 数据可视化、趋势分析 |
第八部分：抓取力控制调节与末端精准执行器调控
8.1 抓取力控制的核心概念
在灵巧手操作中，力控制是决定抓取成功与否的关键。与位置控制不同，力控制需要处理接触环境的不确定性、物体的多样性以及操作过程中的动态变化。精确的力控制可以防止物体损坏、避免滑动，并确保操作的可靠性。
8.1.1 力控制策略对比
| 策略 | 原理 | 适用场景 | 优缺点 |
|---|---|---|---|
| 恒力控制 | 保持恒定的目标力 | 抓取易碎物体 | 简单稳定，但无法适应动态变化 |
| 力-位置混合控制 | 在力控制方向使用力控制，其他方向用位置控制 | 曲面跟踪、装配 | 灵活性高，但参数调校复杂 |
| 力-速度混合控制 | 在力控制方向使用速度控制 | 抛光、打磨 | 响应快，但精度较低 |
| 阻抗控制 | 控制末端刚度/阻尼，间接控制力 | 人机交互、柔性操作 | 安全性高，但实现复杂 |
| 自适应力控制 | 根据接触状态实时调整力目标 | 未知物体抓取 | 鲁棒性强，但计算量大 |
8.1.2 力控制架构
[力传感器] → [力信号处理] → [力控制器] → [力矩分配] → [关节驱动] → [灵巧手] ↑ ↓ ↑ | [力-位置切换] [抓取状态] | ↓ | └───────────────────────[接触状态估计]─────────────────────────┘
8.2 核心数据结构
8.2.1 力控制参数结构
/**
* @struct ForceControlParams
* @brief 力控制参数结构。
*/
struct ForceControlParams {
// 力控制器参数
double kp_force; /**< 力控制比例增益 */
double ki_force; /**< 力控制积分增益 */
double kd_force; /**< 力控制微分增益 */
double kf_force_feedforward; /**< 力前馈增益 */
double kp_position; /**< 位置控制比例增益 */
double kp_velocity; /**< 速度控制比例增益 */
// 力限制
double max_force; /**< 最大力 (N) */
double min_force; /**< 最小力 (N) */
double max_force_rate; /**< 最大力变化率 (N/s) */
double force_tolerance; /**< 力容差 (N) */
// 滤波参数
double force_filter_alpha; /**< 力信号滤波系数 (0-1) */
double position_filter_alpha; /**< 位置滤波系数 (0-1) */
// 切换参数
double force_position_threshold; /**< 力-位置切换阈值 */
bool force_priority; /**< 力优先还是位置优先 */
double transition_time; /**< 切换过渡时间 (秒) */
// 安全参数
double max_force_emergency; /**< 紧急力限幅 (N) */
double emergency_stop_time; /**< 紧急停止响应时间 (秒) */
};
/**
* @struct ForceControlState
* @brief 力控制运行时状态。
*/
struct ForceControlState {
double force_error; /**< 力误差 */
double force_integral; /**< 力积分累计 */
double prev_force_error; /**< 上次力误差 */
double force_output; /**< 力控制输出 */
double position_error; /**< 位置误差 */
double position_output; /**< 位置控制输出 */
double total_output; /**< 总控制输出 */
double current_force; /**< 当前力 (N) */
double target_force; /**< 目标力 (N) */
double current_position; /**< 当前位置 (m) */
double target_position; /**< 目标位置 (m) */
double contact_force; /**< 接触力 (N) */
bool in_contact; /**< 是否已接触 */
double contact_time; /**< 接触持续时间 (秒) */
enum ForceControlMode {
MODE_FORCE_CONTROL = 0,
MODE_POSITION_CONTROL,
MODE_HYBRID_CONTROL,
MODE_TRANSITION
} current_mode; /**< 当前控制模式 */
};
8.2.2 末端执行器参数结构
/**
* @struct EndEffectorConfig
* @brief 末端执行器配置参数。
*/
struct EndEffectorConfig {
struct Joint {
std::string name; /**< 关节名称 */
int joint_id; /**< 关节 ID */
double stroke_min; /**< 最小行程 (m) */
double stroke_max; /**< 最大行程 (m) */
double max_velocity; /**< 最大速度 (m/s) */
double max_force; /**< 最大力 (N) */
double resolution; /**< 分辨率 (m) */
bool has_force_sensor; /**< 是否力传感器 */
bool has_position_sensor; /**< 是否位置传感器 */
};
std::vector<Joint> joints; /**< 末端执行器关节列表 */
double max_opening; /**< 最大开口 (m) */
double min_closing; /**< 最小闭合 (m) */
double max_grasp_force; /**< 最大抓取力 (N) */
double grasp_force_step; /**< 抓取力步进 (N) */
double position_accuracy; /**< 位置精度 (m) */
double force_accuracy; /**< 力精度 (N) */
int control_mode; /**< 控制模式 (0=位置,1=力,2=混合) */
bool position_feedback; /**< 是否位置反馈 */
bool force_feedback; /**< 是否力反馈 */
};
/**
* @struct GraspForceProfile
* @brief 抓取力曲线配置。
*/
struct GraspForceProfile {
enum ProfileType {
PROFILE_CONSTANT = 0, /**< 恒力 */
PROFILE_RAMP, /**< 斜坡 */
PROFILE_S_CURVE, /**< S型曲线 */
PROFILE_IMPULSE, /**< 脉冲 */
PROFILE_CUSTOM /**< 自定义 */
} type; /**< 曲线类型 */
double start_force; /**< 起始力 (N) */
double target_force; /**< 目标力 (N) */
double duration; /**< 持续时间 (秒) */
double rise_time; /**< 上升时间 (秒) */
double fall_time; /**< 下降时间 (秒) */
double peak_force; /**< 峰值力 (N) */
double hold_time; /**< 保持时间 (秒) */
std::vector<double> custom_points; /**< 自定义曲线点 */
};
8.3 核心代码实现
8.3.1 力控制器基类
/**
* @class ForceController
* @brief 力控制器基类，实现通用力控制逻辑。
*/
class ForceController : public controller_interface::ControllerInterface
{
public:
ForceController() = default;
controller_interface::InterfaceConfiguration command_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto &joint : joint_names_) {
config.names.push_back(joint + "/effort");
}
return config;
}
controller_interface::InterfaceConfiguration state_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto &joint : joint_names_) {
config.names.push_back(joint + "/position");
config.names.push_back(joint + "/velocity");
config.names.push_back(joint + "/effort");
}
return config;
}
controller_interface::return_type init(const std::string &controller_name) override
{
// 1. 加载力控制参数
if (!load_force_params()) {
return controller_interface::return_type::ERROR;
}
// 2. 初始化力控制器状态
state_ = ForceControlState();
state_.current_mode = ForceControlState::MODE_FORCE_CONTROL;
// 3. 初始化PID控制器
force_pid_.init(PIDParams{
.kp = params_.kp_force,
.ki = params_.ki_force,
.kd = params_.kd_force
});
position_pid_.init(PIDParams{
.kp = params_.kp_position,
.ki = 0.0,
.kd = 0.0
});
return controller_interface::return_type::OK;
}
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
{
// 1. 读取当前力传感器数据
double current_force = read_force_sensor();
double current_position = read_position_sensor();
// 2. 接触检测
detect_contact(current_force);
// 3. 根据控制模式执行控制
switch (state_.current_mode) {
case ForceControlState::MODE_FORCE_CONTROL:
execute_force_control(current_force, current_position, period);
break;
case ForceControlState::MODE_POSITION_CONTROL:
execute_position_control(current_force, current_position, period);
break;
case ForceControlState::MODE_HYBRID_CONTROL:
execute_hybrid_control(current_force, current_position, period);
break;
case ForceControlState::MODE_TRANSITION:
execute_transition_control(current_force, current_position, period);
break;
}
// 4. 写入命令接口
update_command_interfaces();
return controller_interface::return_type::OK;
}
protected:
/**
* @brief 执行力控制。
*/
virtual void execute_force_control(double current_force, double current_position, const rclcpp::Duration &period)
{
// 1. 计算力误差
state_.force_error = state_.target_force - current_force;
// 2. 力PID控制
double dt = period.seconds();
state_.force_output = force_pid_.update(state_.force_error, dt);
// 3. 输出限幅
state_.force_output = std::clamp(state_.force_output, -params_.max_force, params_.max_force);
// 4. 总输出
state_.total_output = state_.force_output;
// 5. 更新命令
for (size_t i = 0; i < joint_names_.size(); ++i) {
command_interfaces_[i].set_value(state_.total_output);
}
}
/**
* @brief 执行位置控制。
*/
virtual void execute_position_control(double current_force, double current_position, const rclcpp::Duration &period)
{
// 1. 计算位置误差
state_.position_error = state_.target_position - current_position;
// 2. 位置PID控制
double dt = period.seconds();
state_.position_output = position_pid_.update(state_.position_error, dt);
// 3. 输出限幅
state_.position_output = std::clamp(state_.position_output, -params_.max_force, params_.max_force);
// 4. 总输出
state_.total_output = state_.position_output;
// 5. 更新命令
for (size_t i = 0; i < joint_names_.size(); ++i) {
command_interfaces_[i].set_value(state_.total_output);
}
}
/**
* @brief 执行混合力位控制。
*/
virtual void execute_hybrid_control(double current_force, double current_position, const rclcpp::Duration &period)
{
// 1. 计算力误差和位置误差
state_.force_error = state_.target_force - current_force;
state_.position_error = state_.target_position - current_position;
// 2. 计算力控制和位置控制输出
double dt = period.seconds();
double force_out = force_pid_.update(state_.force_error, dt);
double pos_out = position_pid_.update(state_.position_error, dt);
// 3. 根据优先级合成输出
if (params_.force_priority) {
state_.total_output = force_out + pos_out * 0.1;
} else {
state_.total_output = pos_out + force_out * 0.1;
}
// 4. 输出限幅
state_.total_output = std::clamp(state_.total_output, -params_.max_force, params_.max_force);
// 5. 更新命令
for (size_t i = 0; i < joint_names_.size(); ++i) {
command_interfaces_[i].set_value(state_.total_output);
}
}
/**
* @brief 执行过渡控制。
*/
virtual void execute_transition_control(double current_force, double current_position, const rclcpp::Duration &period)
{
// 使用线性插值在力控制和位置控制之间切换
double progress = std::min(1.0, transition_progress_ / params_.transition_time);
if (progress < 0.5) {
// 保持当前模式
if (prev_mode_ == ForceControlState::MODE_FORCE_CONTROL) {
execute_force_control(current_force, current_position, period);
} else {
execute_position_control(current_force, current_position, period);
}
} else {
// 逐渐切换到新模式
double alpha = (progress - 0.5) * 2.0;
if (prev_mode_ == ForceControlState::MODE_FORCE_CONTROL &&
state_.current_mode == ForceControlState::MODE_POSITION_CONTROL) {
// 力控制切位置控制
double force_out = force_pid_.update(state_.force_error, period.seconds());
double pos_out = position_pid_.update(state_.position_error, period.seconds());
state_.total_output = (1 - alpha) * force_out + alpha * pos_out;
}
// 其他切换情况类似...
}
transition_progress_ += period.seconds();
}
/**
* @brief 接触检测。
*/
void detect_contact(double current_force)
{
if (current_force > params_.force_position_threshold) {
if (!state_.in_contact) {
state_.in_contact = true;
state_.contact_time = 0.0;
on_contact_detected();
}
state_.contact_time += 0.001; // 假设1ms周期
} else {
if (state_.in_contact) {
on_contact_lost();
}
state_.in_contact = false;
state_.contact_time = 0.0;
}
}
/**
* @brief 接触事件回调。
*/
virtual void on_contact_detected() { }
/**
* @brief 接触丢失事件回调。
*/
virtual void on_contact_lost() { }
std::vector<std::string> joint_names_;
std::vector<hardware_interface::CommandInterface> command_interfaces_;
std::vector<hardware_interface::StateInterface> state_interfaces_;
ForceControlParams params_;
ForceControlState state_;
PIDController force_pid_;
PIDController position_pid_;
double transition_progress_ = 0.0;
ForceControlState::ForceControlMode prev_mode_;
};
8.3.2 抓取力调节器
/**
* @class GraspForceRegulator
* @brief 抓取力调节器，实现力曲线控制和自适应调节。
*/
class GraspForceRegulator
{
public:
/**
* @brief 初始化抓取力调节器。
*/
void init(const ForceControlParams ¶ms)
{
params_ = params;
current_profile_ = GraspForceProfile();
current_profile_.type = GraspForceProfile::PROFILE_CONSTANT;
current_profile_.target_force = 5.0;
current_profile_.duration = 1.0;
// 初始化力PID
pid_.init(PIDParams{
.kp = params_.kp_force,
.ki = params_.ki_force,
.kd = params_.kd_force
});
}
/**
* @brief 设置抓取力曲线。
* @param profile 力曲线配置
*/
void set_force_profile(const GraspForceProfile &profile)
{
current_profile_ = profile;
profile_start_time_ = get_current_time();
profile_progress_ = 0.0;
}
/**
* @brief 执行力控制调节。
* @param current_force 当前力
* @param dt 时间步长
* @return 力控制输出
*/
double regulate(double current_force, double dt)
{
// 1. 计算当前目标力
double target_force = compute_target_force();
// 2. 计算力误差
double error = target_force - current_force;
// 3. PID控制
double output = pid_.update(error, dt);
// 4. 输出限幅
output = std::clamp(output, -params_.max_force, params_.max_force);
return output;
}
/**
* @brief 计算当前目标力。
*/
double compute_target_force()
{
double elapsed = get_current_time() - profile_start_time_;
double t = elapsed / current_profile_.duration;
if (t > 1.0) t = 1.0;
if (t < 0.0) t = 0.0;
switch (current_profile_.type) {
case GraspForceProfile::PROFILE_CONSTANT:
return current_profile_.target_force;
case GraspForceProfile::PROFILE_RAMP:
if (t < current_profile_.rise_time / current_profile_.duration) {
double rt = t / (current_profile_.rise_time / current_profile_.duration);
return current_profile_.start_force + (current_profile_.target_force - current_profile_.start_force) * rt;
} else if (t < (current_profile_.rise_time + current_profile_.hold_time) / current_profile_.duration) {
return current_profile_.target_force;
} else {
double ft = (t - (current_profile_.rise_time + current_profile_.hold_time) / current_profile_.duration) /
(current_profile_.fall_time / current_profile_.duration);
return current_profile_.target_force * (1 - ft);
}
case GraspForceProfile::PROFILE_S_CURVE:
{
// S型曲线: 使用三次多项式
double s = t;
return current_profile_.start_force + (current_profile_.target_force - current_profile_.start_force) *
(3 * s * s - 2 * s * s * s);
}
case GraspForceProfile::PROFILE_IMPULSE:
if (t < 0.1) {
return current_profile_.peak_force;
} else {
return current_profile_.target_force;
}
case GraspForceProfile::PROFILE_CUSTOM:
// 自定义曲线插值
if (!current_profile_.custom_points.empty()) {
int idx = (int)(t * (current_profile_.custom_points.size() - 1));
double frac = t * (current_profile_.custom_points.size() - 1) - idx;
if (idx + 1 < current_profile_.custom_points.size()) {
return current_profile_.custom_points[idx] * (1 - frac) +
current_profile_.custom_points[idx + 1] * frac;
}
}
return current_profile_.target_force;
}
return current_profile_.target_force;
}
/**
* @brief 自适应调节抓取力。
* @param sliding_detected 是否检测到滑动
* @param current_force 当前力
*/
void adapt_force(bool sliding_detected, double current_force)
{
if (sliding_detected) {
// 检测到滑动，增加目标力
double new_target = current_force * 1.2;
if (new_target > params_.max_force) {
new_target = params_.max_force;
}
// 更新当前曲线
current_profile_.target_force = new_target;
pid_.reset();
} else {
// 稳定状态，可以缓慢减少力
double target_force = current_profile_.target_force;
if (current_force > target_force * 1.5) {
// 力过大，减少目标力
current_profile_.target_force = current_force * 0.95;
if (current_profile_.target_force < params_.min_force) {
current_profile_.target_force = params_.min_force;
}
}
}
}
private:
ForceControlParams params_;
GraspForceProfile current_profile_;
double profile_start_time_ = 0.0;
double profile_progress_ = 0.0;
PIDController pid_;
};
8.3.3 末端执行器精准调控
/**
* @class EndEffectorController
* @brief 末端执行器精准控制器。
*/
class EndEffectorController : public controller_interface::ControllerInterface
{
public:
EndEffectorController() = default;
controller_interface::InterfaceConfiguration command_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto &joint : config_.joints) {
config.names.push_back(joint.name + "/position");
config.names.push_back(joint.name + "/effort");
}
return config;
}
controller_interface::InterfaceConfiguration state_interface_configuration() const override
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto &joint : config_.joints) {
config.names.push_back(joint.name + "/position");
config.names.push_back(joint.name + "/velocity");
config.names.push_back(joint.name + "/effort");
}
return config;
}
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
{
// 1. 读取当前状态
update_joint_states();
// 2. 计算目标位置和力
compute_target_state();
// 3. 执行控制
for (size_t i = 0; i < config_.joints.size(); ++i) {
// 位置控制
double pos_error = target_positions_[i] - joint_positions_[i];
double pos_cmd = position_pid_[i].update(pos_error, period.seconds());
// 力控制 (如果有力传感器)
double force_cmd = 0.0;
if (config_.joints[i].has_force_sensor) {
double force_error = target_forces_[i] - joint_efforts_[i];
force_cmd = force_pid_[i].update(force_error, period.seconds());
}
// 混合控制输出
double output;
if (config_.control_mode == 0) { // 位置控制
output = pos_cmd;
} else if (config_.control_mode == 1) { // 力控制
output = force_cmd;
} else { // 混合控制
output = pos_cmd * 0.7 + force_cmd * 0.3;
}
// 限幅
output = std::clamp(output, -config_.joints[i].max_force,
config_.joints[i].max_force);
command_interfaces_[i].set_value(output);
}
return controller_interface::return_type::OK;
}
private:
void update_joint_states()
{
for (size_t i = 0; i < config_.joints.size(); ++i) {
joint_positions_[i] = state_interfaces_[i * 3 + 0].get_value();
joint_velocities_[i] = state_interfaces_[i * 3 + 1].get_value();
joint_efforts_[i] = state_interfaces_[i * 3 + 2].get_value();
}
}
void compute_target_state()
{
// 根据当前任务计算目标位置和力
// 示例：逐步接近目标
for (size_t i = 0; i < config_.joints.size(); ++i) {
if (std::abs(target_positions_[i] - joint_positions_[i]) > 0.001) {
target_positions_[i] = joint_positions_[i] + 0.001;
}
}
}
EndEffectorConfig config_;
std::vector<double> joint_positions_;
std::vector<double> joint_velocities_;
std::vector<double> joint_efforts_;
std::vector<double> target_positions_;
std::vector<double> target_forces_;
std::vector<PIDController> position_pid_;
std::vector<PIDController> force_pid_;
std::vector<hardware_interface::CommandInterface> command_interfaces_;
std::vector<hardware_interface::StateInterface> state_interfaces_;
};
8.3.4 触觉反馈驱动的自适应力控制
/**
* @class TactileAdaptiveForceController
* @brief 触觉反馈驱动的自适应力控制器。
*/
class TactileAdaptiveForceController : public ForceController
{
public:
TactileAdaptiveForceController() = default;
controller_interface::return_type init(const std::string &controller_name) override
{
// 1. 调用基类初始化
if (ForceController::init(controller_name) != controller_interface::return_type::OK) {
return controller_interface::return_type::ERROR;
}
// 2. 初始化触觉传感器
tactile_sensor_.init(tactile_sensor_count_, 100);
return controller_interface::return_type::OK;
}
protected:
void on_contact_detected() override
{
// 接触检测到，启动力控制
state_.current_mode = ForceControlState::MODE_FORCE_CONTROL;
state_.target_force = initial_grasp_force_;
force_pid_.reset();
}
void on_contact_lost() override
{
// 接触丢失，切换到位置控制
state_.current_mode = ForceControlState::MODE_POSITION_CONTROL;
state_.target_position = safe_position_;
position_pid_.reset();
}
void execute_force_control(double current_force, double current_position, const rclcpp::Duration &period) override
{
// 1. 读取触觉传感器数据
tactile_sensor_.update();
auto pressure_dist = tactile_sensor_.get_pressure_distribution();
bool sliding = detect_sliding(pressure_dist);
double avg_pressure = tactile_sensor_.get_avg_pressure();
// 2. 自适应调节抓取力
if (sliding) {
// 检测到滑动，快速增加抓取力
state_.target_force *= 1.2;
if (state_.target_force > params_.max_force) {
state_.target_force = params_.max_force;
}
force_pid_.reset();
}
// 3. 根据压力调整力控制增益
if (avg_pressure > 50.0) {
// 高压区域，降低增益避免过冲
force_pid_.set_kp(params_.kp_force * 0.5);
} else if (avg_pressure < 10.0) {
// 低压区域，增加增益提高响应
force_pid_.set_kp(params_.kp_force * 1.5);
}
// 4. 执行力控制
ForceController::execute_force_control(current_force, current_position, period);
}
private:
bool detect_sliding(const std::vector<double> &pressure_dist)
{
// 计算压力方差，检测滑动
double mean = 0.0;
for (double v : pressure_dist) {
mean += v;
}
mean /= pressure_dist.size();
double variance = 0.0;
for (double v : pressure_dist) {
variance += (v - mean) * (v - mean);
}
variance /= pressure_dist.size();
// 如果方差变化超过阈值，认为滑动
static double prev_variance = 0.0;
double change = std::abs(variance - prev_variance);
prev_variance = variance;
return change > variance_threshold_;
}
TactileSensorManager tactile_sensor_;
double initial_grasp_force_ = 5.0;
double safe_position_ = 0.0;
double variance_threshold_ = 0.5;
int tactile_sensor_count_ = 16;
};
8.4 软件模式分析
抓取力控制与末端执行器调控子系统 ├── 力控制器 (ForceController) │ ├── 恒力控制器 (ConstantForceController) │ ├── 力-位置混合控制器 (HybridForcePositionController) │ ├── 力-速度混合控制器 (HybridForceVelocityController) │ ├── 阻抗控制器 (ImpedanceController) │ └── 自适应力控制器 (AdaptiveForceController) ├── 抓取力调节器 (GraspForceRegulator) │ ├── 力曲线管理器 (ForceProfileManager) │ ├── 自适应调节器 (AdaptiveRegulator) │ ├── 滑动检测器 (SlidingDetector) │ └── 力限制器 (ForceLimiter) ├── 末端执行器控制器 (EndEffectorController) │ ├── 位置控制器 (PositionController) │ ├── 力控制器 (ForceController) │ ├── 混合控制器 (HybridController) │ ├── 轨迹规划器 (TrajectoryPlanner) │ └── 安全保护器 (SafetyProtector) └── 触觉反馈驱动 (TactileFeedbackDriven) ├── 触觉传感器处理 (TactileProcessing) ├── 接触状态估计 (ContactEstimation) ├── 滑动检测 (SlidingDetection) └── 自适应增益调节 (AdaptiveGainAdjustment)
8.5 资深视角：力控制调试核心难点
8.5.1 力控制震荡
现象：力控制模式下，力信号出现高频震荡。
原因：
-
力控制增益过高。
-
传感器噪声被放大。
-
机械系统柔性导致控制不稳定。
解决方法：
-
降低力控制增益 (从 10 降至 5)。
-
增加力信号滤波：
filtered_force = 0.7 * filtered_force + 0.3 * raw_force;
-
添加力导数反馈，增加阻尼项。
8.5.2 力控制响应慢
现象：力控制响应缓慢，无法快速达到目标力。
原因：
-
力控制增益过低。
-
力传感器采样率不足。
-
机械系统惯性大。
解决方法：
-
增加力前馈：
force_output = pid_output + feedforward_force;
-
提高力传感器采样率。
-
使用加速度前馈，补偿惯性力。
8.5.3 抓取力过大损坏物体
现象：抓取易碎物体时，力过大导致物体损坏。
原因：
-
力控制目标设置过高。
-
力传感器校准不准。
-
缺少力限幅保护。
解决方法：
-
设置更小的力目标。
-
定期校准力传感器：
// 空载时校准零点 zero_offset_ = read_force_sensor();
-
添加力限幅保护：
if (current_force > max_force_limit) { stop_immediately(); }
8.5.4 力传感器漂移
现象：长时间运行后，力传感器零点漂移，影响控制精度。
原因：
-
温度变化导致传感器特性漂移。
-
长期应力导致应变片老化。
-
电路噪声累积。
解决方法：
-
定期自动校准。
-
使用温度补偿：
compensated_force = raw_force - temperature_drift * temperature;
-
使用周期性归零：
if (no_contact_time > 10.0) { zero_offset_ = current_force; }
8.5.5 力控制最佳实践
| 调试场景 | 推荐方法 | 调试关键点 |
|---|---|---|
| 首次力控制 | 从小力开始，逐步增加 | 响应速度、超调量 |
| 力传感器校准 | 空载校准，定期归零 | 零点漂移、温度补偿 |
| 力控制参数整定 | 先调整 P，再调整 I，D 慎用 | 增益调试、稳定性 |
| 抓取力曲线优化 | 使用 S 曲线减少冲击 | 曲线平滑度、执行时间 |
| 自适应力控制 | 结合触觉反馈实现自适应 | 检测灵敏度、响应时间 |
8.6 与其他模块的协同
| 模块 | 协同方式 | 调试关键点 |
|---|---|---|
| 硬件接口 | 提供力传感器数据，写入力矩命令 | 传感器噪声、命令饱和度 |
| 多模控制器 | 作为力控制基础层 | 模式切换、力-位置切换 |
| 动力学补偿 | 力控制时叠加动力学补偿 | 补偿时机、一致性 |
| FOC 控制 | 力控制输出转换为电流指令 | 电流限幅、响应时间 |
| PID 控制 | 力控制使用 PID 控制器 | PID 参数整定、抗饱和 |
| 灵巧手控制器 | 作为灵巧手的高级控制模式 | 多指协调、分配策略 |
| 实时通信 | 力控制周期高 (1ms) | 周期抖动、数据延迟 |
| 监控系统 | 记录力数据和控制参数 | 趋势分析、异常检测 |
AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。
更多推荐
1
0- 0
已为社区贡献83条内容
所有评论(0)