so3_control 节点功能概述

so3_control 是一个基于 SO(3) 李群的四旋翼飞行器姿态控制器，主要功能包括：

1. 核心控制功能

• 位置控制：接收期望位置、速度、加速度指令，计算所需的推力和姿态
• 姿态控制：基于 SO(3) 李群理论，计算四旋翼的期望姿态四元数
• 推力控制：计算维持飞行所需的推力大小和方向
• 偏航控制：控制飞行器的偏航角度

2. 控制算法特点

• 使用 SO(3) 李群控制器，相比传统欧拉角控制器更稳定
• 支持 位置-速度-加速度 三阶控制
• 包含 45度倾角限制，确保飞行安全
• 支持 重力补偿 和 质量参数 配置

与其他节点的关联关系

输入接口（订阅的话题）

1. /planning/pos_cmd (quadrotor_msgs/PositionCommand)

   • 来自路径规划节点
   • 包含期望位置、速度、加速度、偏航角等信息
   • 是控制器的主要指令输入

2. /visual_slam/odom (nav_msgs/Odometry)

   • 来自视觉SLAM定位节点
   • 提供当前飞行器的位置、速度、姿态信息
   • 作为控制器的反馈信号

3. /imu (sensor_msgs/Imu)

   • 来自IMU传感器
   • 提供加速度信息用于控制计算

4. /motors (std_msgs/Bool)

   • 电机使能/禁用控制
   • 用于紧急停止或安全控制

5. /corrections (quadrotor_msgs/Corrections)

   • 来自校正节点
   • 提供卡尔曼滤波校正和角度校正参数

输出接口（发布的话题）

1. /so3_cmd (quadrotor_msgs/SO3Command)
   • 发送给四旋翼仿真器 quadrotor_simulator_so3
   • 包含计算出的推力、期望姿态、控制增益等
   • 是控制器的最终输出