test_dynamics

Quadrotor.cpp 是动力学模型。

quadrotor_simulator_so3.cpp 是飞行器仿真控制器。

test_dynamics.cpp 是一个测试代码。

在CMakeLists.txt里加上：

bash
展开代码
# Add test_dynamics executable
add_executable(test_dynamics
  src/test_dynamics/test_dynamics.cpp)

target_link_libraries(test_dynamics
   quadrotor_dynamics
)

就像这样：

代码里加句话：

编译：

bash
展开代码
catkin_make

安装几个python包：

bash
展开代码
python3 -m pip install --upgrade pip
python3 -m pip install --upgrade seaborn pandas

执行test_dynamics，让其把数据输出出来到 utils_python/dynamics_data.csv

bash
展开代码
mkdir utils_python
devel/lib/so3_quadrotor_simulator/test_dynamics > utils_python/dynamics_data.csv

utils_python/dynamics_data.csv 里面的数据：

bash
展开代码
时间, 高度, 偏航角, 俯仰角, 滚转角, 角速度x, 角速度y, 角速度z, 电机转速

执行python绘图：

bash
展开代码
cd utils_python/
python visualize_dynamics.py

得到图：

• height_control.png - 高度控制性能
• attitude_control.png - 姿态控制性能
• motor_performance.png - 电机性能
• phase_analysis.png - 相空间分析
• comprehensive_analysis.png - 综合分析

test代码分析

1. 初始化部分 (第6-22行)

cpp
展开代码
double dt = 0.001;  // 仿真时间步长：1毫秒
QuadrotorSimulator::Quadrotor quad;  // 创建四旋翼仿真器对象

设置仿真时间步长为1毫秒
创建四旋翼飞行器仿真器实例
获取飞行器的物理参数：质量(m)、重力加速度(g)、螺旋桨推力系数(kf)

2. 悬停状态计算 (第14-22行)

cpp
展开代码
const double hover_rpm = std::sqrt(m*g/(4*kf));  // 计算悬停所需的电机转速

计算四旋翼悬停时所需的电机转速
根据物理公式：hover_rpm = √(mg/4kf)，其中：
- m: 飞行器质量
- g: 重力加速度
- kf: 螺旋桨推力系数
- 4: 四个电机

3. 主仿真循环 (第28-57行)

控制算法

程序实现了一个PD控制器来控制飞行器的高度：

cpp
展开代码
double KP = 8.0;    // 比例增益
double KD = 2.5;    // 微分增益
const double z_des = 0.5;  // 目标高度：0.5米

// PD控制律
thrust = m*g + KP*(z_des - state.x(2)) + KD*(0 - state.v(2));

控制律解释：

m*g: 重力补偿项
KP*(z_des - state.x(2)): 位置误差的比例控制
KD*(0 - state.v(2)): 速度误差的微分控制

外部力干扰测试 (第35-39行)

cpp
展开代码
if( i < 3000)
  quad.setExternalForce(Eigen::Vector3d(0, 0, -KP*z_des));  // 前3秒施加向下的干扰力
else
  quad.setExternalForce(Eigen::Vector3d(0, 0, 0));  // 后3秒移除干扰力

前3秒：施加一个向下的外部力干扰，测试控制器的抗干扰能力
后3秒：移除干扰力，观察系统恢复情况

4. 数据输出 (第42-44行)

cpp
展开代码
std::cout << i*dt << ", " << state.x(2) << ", " << euler(0) << ", " << euler(1) << ", " << euler(2) << ", " <<
    state.omega(0) << ", " << state.omega(1) << ", " << state.omega(2) << ", " << state.motor_rpm(0) << std::endl;

输出格式：时间, 高度, 偏航角, 俯仰角, 滚转角, 角速度x, 角速度y, 角速度z, 电机转速

反思

test_dynamics.cpp 这个代码是值得细看，这也可以单独调试，这是四旋翼飞行器的动力模型的一个测试文件。

so3_quadrotor_simulator 的作用

1. 核心功能

四旋翼物理仿真：基于 SO(3) 群的姿态表示，实现了完整的四旋翼动力学模型
传感器数据生成：提供里程计（odometry）和 IMU 数据，供规划器和控制器使用
控制指令执行：接收来自控制器的 SO3Command 指令并执行仿真

2. 系统架构中的位置

200:265:/xd_ws/ego-planner/src/uav_simulator/so3_quadrotor_simulator/src/quadrotor_simulator_so3.cpp
展开代码
int
main(int argc, char** argv)
{
  ros::init(argc, argv, "quadrotor_simulator_so3");

  ros::NodeHandle n("~");

  ros::Publisher  odom_pub = n.advertise<nav_msgs::Odometry>("odom", 100);
  ros::Publisher  imu_pub  = n.advertise<sensor_msgs::Imu>("imu", 10);
  ros::Subscriber cmd_sub =
    n.subscribe("cmd", 100, &cmd_callback, ros::TransportHints().tcpNoDelay());
  ros::Subscriber f_sub =
    n.subscribe("force_disturbance", 100, &force_disturbance_callback,
                ros::TransportHints().tcpNoDelay());
  ros::Subscriber m_sub =
    n.subscribe("moment_disturbance", 100, &moment_disturbance_callback,
                ros::TransportHints().tcpNoDelay());

  QuadrotorSimulator::Quadrotor quad;

该节点通过以下接口与系统交互：

订阅的话题：

cmd（重映射为 so3_cmd）：接收来自 so3_control 控制器的 SO3Command 控制指令
force_disturbance：外部干扰力（可选）
moment_disturbance：外部干扰力矩（可选）

发布的话题：

odom（重映射为 /visual_slam/odom）：四旋翼的里程计信息，供规划器使用
imu：IMU 传感器数据

3. 在 ego-planner 中的使用

从配置文件可以看出，该节点在两种主要场景中使用：

A. 仿真模式

76:86:/xd_ws/ego-planner/src/planner/plan_manage/launch/simulator.xml
展开代码
  <node pkg="so3_quadrotor_simulator" type="quadrotor_simulator_so3" name="quadrotor_simulator_so3" output="screen">
        <param name="rate/odom" value="200.0"/>
        <param name="simulator/init_state_x" value="$(arg init_x)"/>
        <param name="simulator/init_state_y" value="$(arg init_y)"/>
        <param name="simulator/init_state_z" value="$(arg init_z)"/>

        <remap from="~odom" to="/visual_slam/odom"/>
        <remap from="~cmd" to="so3_cmd"/>
        <remap from="~force_disturbance" to="force_disturbance"/>    
        <remap from="~moment_disturbance" to="moment_disturbance"/>        
  </node>

B. 独立仿真测试

7:19:/xd_ws/ego-planner/src/uav_simulator/so3_quadrotor_simulator/launch/simulator_example.launch
展开代码
   <node pkg="so3_quadrotor_simulator"
         type="quadrotor_simulator_so3"
         name="quadrotor_simulator_so3"
         output="screen">
     <param name="rate/odom" value="100.0"/>

     <param name="simulator/init_state_x" value="$(arg init_x)"/>
     <param name="simulator/init_state_y" value="$(arg init_y)"/>
     <param name="simulator/init_state_z" value="$(arg init_z)"/>
     <remap from="~odom" to="/sim/odom"/>
     <remap from="~cmd" to="/so3_cmd"/>
     <remap from="~imu" to="/sim/imu"/>     
   </node>

4. 动力学模型特点

该仿真器实现了完整的四旋翼动力学模型：

13:21:/xd_ws/ego-planner/src/uav_simulator/so3_quadrotor_simulator/include/quadrotor_simulator/Quadrotor.h
展开代码
  struct State
  {
    Eigen::Vector3d x;      // 位置
    Eigen::Vector3d v;      // 速度  
    Eigen::Matrix3d R;      // 旋转矩阵（SO3 姿态表示）
    Eigen::Vector3d omega;  // 角速度
    Eigen::Array4d  motor_rpm; // 四个电机转速
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  };

完整的 6DOF 动力学：包含位置、速度、姿态和角速度
电机级仿真：模拟四个螺旋桨的转速
真实物理参数：质量、惯性矩阵、螺旋桨参数等
外部干扰支持：可以添加外部力和力矩干扰

5. 控制接口

161:181:/xd_ws/ego-planner/src/uav_simulator/so3_quadrotor_simulator/src/quadrotor_simulator_so3.cpp
展开代码
static void
cmd_callback(const quadrotor_msgs::SO3Command::ConstPtr& cmd)
{
  command.force[0]         = cmd->force.x;
  command.force[1]         = cmd->force.y;
  command.force[2]         = cmd->force.z;
  command.qx               = cmd->orientation.x;
  command.qy               = cmd->orientation.y;
  command.qz               = cmd->orientation.z;
  command.qw               = cmd->orientation.w;
  command.kR[0]            = cmd->kR[0];
  command.kR[1]            = cmd->kR[1];
  command.kR[2]            = cmd->kR[2];
  command.kOm[0]           = cmd->kOm[0];
  command.kOm[1]           = cmd->kOm[1];
  command.kOm[2]           = cmd->kOm[2];
  command.corrections[0]   = cmd->aux.kf_correction;
  command.corrections[1]   = cmd->aux.angle_corrections[0];
  command.corrections[2]   = cmd->aux.angle_corrections[1];
  command.current_yaw      = cmd->aux.current_yaw;
  command.use_external_yaw = cmd->aux.use_external_yaw;
}

接收的控制指令包含：

力矢量：期望的推力
姿态四元数：期望的姿态
控制增益：kR（姿态增益）和 kOm（角速度增益）
修正参数：推力修正、角度修正等

使用

启动出rviz

bash
展开代码
source devel/setup.bash
roslaunch ego_planner rviz.launch

启动完整仿真系统

bash
展开代码
# 启动完整仿真系统
source devel/setup.bash
roslaunch ego_planner run_in_sim.launch

roslaunch ego_planner run_in_sim.launch 启动后，日志会显示这些都启动：

ego_planner_node: 主规划器节点
traj_server: 轨迹服务器
waypoint_generator: 路径点生成器
mockamap_node: 模拟地图生成节点
quadrotor_simulator_so3: 四旋翼仿真器（我们刚才详细分析的节点）
so3_control: SO3控制器
odom_visualization: 里程计可视化
pcl_render_node: 点云渲染节点

根据参数配置，系统建立了一个仿真环境：

地图大小: 40x40x3 米的三维空间
初始位置: (-18, 0, 0)，无人机在 x=-18 米处起始
地图类型: Perlin噪声生成的随机障碍物地图，填充率 12%
传感器参数: 配置了深度相机用于感知（640x480分辨率）

要让无人机开始飞行，您需要：

在 RViz 顶部工具栏中点击 "2D Nav Goal" 工具
在地图上点击并拖拽，设置目标点和方向
系统会自动开始路径规划和飞行

起始点：

我设置点后，

一旦您设置了目标点，系统将：

状态转换: FSM: INIT → FSM: WAIT_TARGET → FSM: GEN_NEW_TRAJ → FSM: REPLAN_TRAJ
路径规划: EGO-planner 开始计算从当前位置到目标位置的安全轨迹
控制执行:

traj_server 将轨迹转换为位置指令
so3_control 将位置指令转换为 SO3Command
quadrotor_simulator_so3 执行控制指令，模拟无人机飞行

实时反馈: 无人机位置信息通过里程计话题反馈给规划器

玩单纯的飞机

独立使用（仅仿真器测试）

bash
展开代码
# 启动仿真器示例
roslaunch so3_quadrotor_simulator simulator_example.launch

会看到一个飞机：

也看到了：

无人机模型：显示在初始位置 (-4, 0, 4) 米处
坐标轴：世界坐标系和无人机坐标系
里程计轨迹：红色半透明的飞行轨迹
IMU 和里程计信息：实时数据显示

查看话题：

bash
展开代码
root@euler-MS-7D30:/xd_ws/ego-planner# rostopic list
/CircleNode_1/path
/clicked_point
/corrections
/goal
/initialpose
/map_generator/global_cloud
/motors
/move_base_simple/goal
/odom_visualization/cmd
/odom_visualization/covariance
/odom_visualization/covariance_velocity
/odom_visualization/height
/odom_visualization/path
/odom_visualization/pose
/odom_visualization/robot
/odom_visualization/robot_array
/odom_visualization/sensor
/odom_visualization/trajectory
/odom_visualization/velocity
/odom_visualization_1/path
/odom_visualization_1/pose
/odom_visualization_1/velocity
/odom_visualization_1/velocity_array
/position_cmd
/quadrotor_simulator_so3/force_disturbance
/quadrotor_simulator_so3/moment_disturbance
/rosout
/rosout_agg
/sim/imu
/sim/odom
/so3_cmd
/tf
/tf_static

发送一个控制命令：

步骤1：让飞机悬停在当前高度

bash
展开代码
rostopic pub -r 10 /position_cmd quadrotor_msgs/PositionCommand "
position: {x: -4.0, y: 0.0, z: 4.0}
yaw: 0.0"

步骤2：让飞机上升

bash
展开代码
rostopic pub -r 10 /position_cmd quadrotor_msgs/PositionCommand "
position: {x: -4.0, y: 0.0, z: 6.0}
yaw: 0.0"

步骤3：让飞机水平移动

bash
展开代码
rostopic pub -r 10 /position_cmd quadrotor_msgs/PositionCommand "
position: {x: 0.0, y: 0.0, z: 6.0}
yaw: 0.0"

步骤4：让飞机到达目标位置

bash
展开代码
rostopic pub -r 10 /position_cmd quadrotor_msgs/PositionCommand "
position: {x: 2.0, y: 1.0, z: 2.0}
yaw: 0.0"

反思

给了位置，飞机就飞过去，用的什么算法。

详细解释从 /position_cmd 到飞机实际飞行的完整控制流程：

1. 控制架构概览


展开代码
您的位置命令 → SO3控制器 → 仿真器 → 电机转速 → 飞机运动

2. 详细控制流程

第一步：位置命令处理

当您发布 /position_cmd 时：

96:118:/xd_ws/ego-planner/src/uav_simulator/so3_control/src/so3_control_nodelet.cpp
展开代码
void SO3ControlNodelet::position_cmd_callback(
  const quadrotor_msgs::PositionCommand::ConstPtr& cmd)
{
  des_pos_ = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
  des_vel_ = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
  des_acc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y,
                             cmd->acceleration.z);
  
  // 设置控制增益
  kx_ = Eigen::Vector3d(cmd->kx[0], cmd->kx[1], cmd->kx[2]);
  kv_ = Eigen::Vector3d(cmd->kv[0], cmd->kv[1], cmd->kv[2]);
  
  des_yaw_ = cmd->yaw;
  position_cmd_updated_ = true;
  publishSO3Command();
}

第二步：SO3控制器计算

SO3控制器使用几何控制理论（不是简单的PID），核心算法：

58:85:/xd_ws/ego-planner/src/uav_simulator/so3_control/src/SO3Control.cpp
展开代码
// 计算总误差
Eigen::Vector3d totalError(Eigen::Vector3d::Zero());
if ( flag_use_pos ) totalError.noalias() += des_pos - pos_;  // 位置误差
if ( flag_use_vel ) totalError.noalias() += des_vel - vel_;  // 速度误差
if ( flag_use_acc ) totalError.noalias() += des_acc - acc_;  // 加速度误差

// 计算所需推力
force_ = mass_ * g_ * Eigen::Vector3d(0, 0, 1);  // 重力补偿
if ( flag_use_pos ) force_.noalias() += kx.asDiagonal() * (des_pos - pos_);  // 位置控制
if ( flag_use_vel ) force_.noalias() += kv.asDiagonal() * (des_vel - vel_);  // 速度控制
if ( flag_use_acc ) force_.noalias() += mass_ * ka.asDiagonal() * (des_acc - acc_) + mass_ * (des_acc);  // 加速度控制

// 计算期望姿态（基于推力方向）
Eigen::Vector3d b1c, b2c, b3c;
Eigen::Vector3d b1d(cos(des_yaw), sin(des_yaw), 0);  // 期望的yaw方向

if (force_.norm() > 1e-6)
  b3c.noalias() = force_.normalized();  // 推力方向
else
  b3c.noalias() = Eigen::Vector3d(0, 0, 1);

b2c.noalias() = b3c.cross(b1d).normalized();  // 构建正交坐标系
b1c.noalias() = b2c.cross(b3c).normalized();

Eigen::Matrix3d R;
R << b1c, b2c, b3c;  // 期望的旋转矩阵
orientation_ = Eigen::Quaterniond(R);

第三步：仿真器执行控制

仿真器接收SO3Command，转换为电机转速：

40:160:/xd_ws/ego-planner/src/uav_simulator/so3_quadrotor_simulator/src/quadrotor_simulator_so3.cpp
展开代码
// 计算姿态误差
float eR1 = 0.5f * (R12 * Rd13 - R13 * Rd12 + R22 * Rd23 - R23 * Rd22 +
                    R32 * Rd33 - R33 * Rd32);
float eR2 = 0.5f * (R13 * Rd11 - R11 * Rd13 - R21 * Rd23 + R23 * Rd21 -
                    R31 * Rd33 + R33 * Rd31);
float eR3 = 0.5f * (R11 * Rd12 - R12 * Rd11 + R21 * Rd22 - R22 * Rd21 +
                    R31 * Rd32 - R32 * Rd31);

// 计算力矩（姿态控制）
float M1 = -cmd.kR[0] * eR1 - cmd.kOm[0] * eOm1 + in1;  // 滚转力矩
float M2 = -cmd.kR[1] * eR2 - cmd.kOm[1] * eOm2 + in2;  // 俯仰力矩  
float M3 = -cmd.kR[2] * eR3 - cmd.kOm[2] * eOm3 + in3;  // 偏航力矩

// 转换为四个电机的转速平方
float w_sq[4];
w_sq[0] = force / (4 * kf) - M2 / (2 * d * kf) + M3 / (4 * km);  // 电机1
w_sq[1] = force / (4 * kf) + M2 / (2 * d * kf) + M3 / (4 * km);  // 电机2
w_sq[2] = force / (4 * kf) + M1 / (2 * d * kf) - M3 / (4 * km);  // 电机3
w_sq[3] = force / (4 * kf) - M1 / (2 * d * kf) - M3 / (4 * km);  // 电机4

// 转换为实际转速
control.rpm[i] = sqrtf(w_sq[i]);

3. 控制算法特点

A. 不是简单的PID

这个系统使用的是几何控制理论（Geometric Control），比PID更高级：

位置控制：kx * (des_pos - pos_) - 位置误差反馈
速度控制：kv * (des_vel - vel_) - 速度误差反馈
姿态控制：基于SO(3)群的几何控制
推力分配：将期望推力和力矩分配到四个电机

B. 分层控制结构


展开代码
位置控制层 → 姿态控制层 → 电机控制层

位置层：计算需要的推力和期望姿态
姿态层：计算需要的力矩
电机层：将推力和力矩转换为四个电机的转速

C. 物理约束处理

80:90:/xd_ws/ego-planner/src/uav_simulator/so3_control/src/SO3Control.cpp
展开代码
// 限制控制角度不超过45度
double theta = M_PI / 2;
double c = cos(theta);
if (Eigen::Vector3d(0, 0, 1).dot(force_ / force_.norm()) < c)
{
  // 重新计算推力以满足角度约束
}

与PID的区别

传统PID：


展开代码
误差 = 目标 - 当前
输出 = Kp*误差 + Ki*积分 + Kd*微分

几何控制：


展开代码
位置误差 = 目标位置 - 当前位置
推力 = mg + kx*位置误差 + kv*速度误差
姿态 = 基于推力方向几何计算
力矩 = kR*姿态误差 + kOm*角速度误差

反思 SO3 几何控制算法

1. 理论基础：SO(3) 群和几何控制

SO(3) 是三维旋转群，表示所有可能的3D旋转。四旋翼的姿态控制本质上就是在 SO(3) 群上的控制问题。

四旋翼动力学模型：

\begin{align} m\ddot{x} &= mg + Rf \\ J\dot{\omega} &= M - \omega \times J\omega \\ \dot{R} &= R[\omega]_{\times} \end{align}

其中：

$m$ ：质量
$x$ ：位置
$R$ ：旋转矩阵（姿态）
$f$ ：推力向量
$J$ ：惯性矩阵
$\omega$ ：角速度
$M$ ：力矩

2. 控制算法核心思想

这个算法采用分层控制策略：


展开代码
位置控制 → 推力计算 → 姿态计算 → 力矩控制

3. 代码实现详解

A. 位置控制层（推力计算）

58:85:/xd_ws/ego-planner/src/uav_simulator/so3_control/src/SO3Control.cpp
展开代码
// 计算总误差
Eigen::Vector3d totalError(Eigen::Vector3d::Zero());
if ( flag_use_pos ) totalError.noalias() += des_pos - pos_;  // 位置误差
if ( flag_use_vel ) totalError.noalias() += des_vel - vel_;  // 速度误差
if ( flag_use_acc ) totalError.noalias() += des_acc - acc_;  // 加速度误差

// 计算所需推力
force_ = mass_ * g_ * Eigen::Vector3d(0, 0, 1);  // 重力补偿
if ( flag_use_pos ) force_.noalias() += kx.asDiagonal() * (des_pos - pos_);  // 位置控制
if ( flag_use_vel ) force_.noalias() += kv.asDiagonal() * (des_vel - vel_);  // 速度控制
if ( flag_use_acc ) force_.noalias() += mass_ * ka.asDiagonal() * (des_acc - acc_) + mass_ * (des_acc);  // 加速度控制

数学公式：

F = mg + K_x(x_d - x) + K_v(v_d - v) + mK_a(a_d - a) + ma_d

其中：

$K_x$ ：位置增益矩阵（对角矩阵）
$K_v$ ：速度增益矩阵
$K_a$ ：加速度增益矩阵
$x_d, v_d, a_d$ ：期望位置、速度、加速度

B. 推力角度限制

87:98:/xd_ws/ego-planner/src/uav_simulator/so3_control/src/SO3Control.cpp
展开代码
// 限制控制角度到45度
double theta = M_PI / 2;
double c = cos(theta);
Eigen::Vector3d f;
f.noalias() = force_ - mass_ * g_ * Eigen::Vector3d(0, 0, 1);
if (Eigen::Vector3d(0, 0, 1).dot(force_ / force_.norm()) < c)
{
  double nf = f.norm();
  double A = c * c * nf * nf - f(2) * f(2);
  double B = 2 * (c * c - 1) * f(2) * mass_ * g_;
  double C = (c * c - 1) * mass_ * mass_ * g_ * g_;
  double s = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
  force_.noalias() = s * f + mass_ * g_ * Eigen::Vector3d(0, 0, 1);
}

数学原理：这个约束确保推力方向与垂直方向的夹角不超过45度，保证控制稳定性。

约束条件： $\cos(\theta) = \frac{F \cdot [0,0,1]}{\|F\|} \geq \cos(45°) = \frac{\sqrt{2}}{2}$

C. 姿态计算（几何控制核心）

100:115:/xd_ws/ego-planner/src/uav_simulator/so3_control/src/SO3Control.cpp
展开代码
Eigen::Vector3d b1c, b2c, b3c;
Eigen::Vector3d b1d(cos(des_yaw), sin(des_yaw), 0);  // 期望的yaw方向

if (force_.norm() > 1e-6)
  b3c.noalias() = force_.normalized();  // 推力方向作为z轴
else
  b3c.noalias() = Eigen::Vector3d(0, 0, 1);

b2c.noalias() = b3c.cross(b1d).normalized();  // 叉积得到y轴
b1c.noalias() = b2c.cross(b3c).normalized();  // 叉积得到x轴

Eigen::Matrix3d R;
R << b1c, b2c, b3c;  // 构建旋转矩阵

orientation_ = Eigen::Quaterniond(R);

数学原理：这是推力方向约束的姿态计算，基于以下几何原理：

推力方向约束： $b_{3c} = \frac{F}{\|F\|}$ （推力方向作为机体z轴）
偏航约束： $b_{1d} = [\cos(\psi_d), \sin(\psi_d), 0]$ （期望偏航方向）
正交约束： $b_{2c} = b_{3c} \times b_{1d}$ ， $b_{1c} = b_{2c} \times b_{3c}$

最终旋转矩阵： $R = [b_{1c}, b_{2c}, b_{3c}]$

4. 控制增益分析

从配置文件可以看到：

yaml
展开代码
gains:
  pos: {x: 2.0, y: 2.0, z: 3.5}    # 位置增益
  vel: {x: 1.8, y: 1.8, z: 2.0}    # 速度增益
  rot: {x: 1.0, y: 1.0, z: 0.3}    # 姿态增益
  ang: {x: 0.07, y: 0.07, z: 0.02} # 角速度增益

增益设计原则：

Z轴增益更高：因为重力影响，需要更强的垂直控制
姿态增益适中：保证稳定性，避免过度振荡
角速度增益较小：提供阻尼，减少振荡

目录