无人机姿态控制（Copter Attitude Control）（5）

无人机姿态控制（Copter Attitude Control）

下图解释了无人机姿态控制的基本流程

 

 下面描述了对每个轴所做的事情。P 控制器将角度误差（目标角度与实际角度之间的差异）转换为所需的旋转速率，然后 PID 控制器将旋转速率误差转换为高级电机命令。

图表的“平方根控制器”部分显示了与角度控制的 P 控制器一起使用的曲线。

 

 

下图显示了从驾驶员输入到PWM输出的代码路径

 

 

在每次更新时（即 Pixhawk 上的 400hz，APM2.x 上的 100hz）都会发生以下情况：

• 调用顶层 flight-mode.cpp 的“update_flight_mode()”函数。此函数检查飞行器的飞行模式（即“control_mode”变量），然后调用适当的 <flight mode>_run() 函数（即 stabilize_run 用于稳定模式， rtl_run 用于 RTL 模式等）。<flight mode>_run() 函数可以在适当命名的 mode_<flight mode>.cpp 文件中找到（即 mode_stabilize.cpp、 mode_rtl.cpp等）。

• <flight mode>_run 函数负责将用户的输入（在 g.rc_1.control_in、g.rc_2.control_in 等中找到）转换为适合该飞行模式的倾斜角、旋转率、爬升率等. 例如， AltHold 将用户的横滚和俯仰输入转换为倾斜角（以度为单位），偏航输入转换为旋转速率（以度/秒为单位），油门输入转换为爬升率（以厘米/秒为单位）。

• <flight mode>_run 函数必须做的最后一件事是将这些所需的角度、速率等传递到姿态控制和/或位置控制库（它们都保存在 AC_AttitudeControl 文件夹中 ） 。

• AC_AttitudeControl 库提供了 5 种可能的方式来控制车辆的姿态，最常见的 3 种如下所述。

  • angle_ef_roll_pitch_rate_ef_yaw() ：它接受用于滚动和俯仰的“地球坐标系”-惯性坐标系下的角度，以及用于偏航的“地球坐标系”速率。例如，提供此函数 roll = -1000, pitch = -1500, yaw = 500 表示将车辆向左倾斜 10 度，向前倾斜 15 度并以 5 度/秒的速度向右旋转。

  • angle_ef_roll_pitch_yaw() ：这接受滚动、俯仰和偏航的“地球坐标系”角度。与上面类似，除了提供 500 的偏航意味着将飞机旋转到北向东 5 度。

  • rate_bf_roll_pitch_yaw() ：它接受用于俯仰和偏航的“机体坐标系”速率（以度/秒为单位）。例如，提供此函数 roll = -1000, pitch = -1500, yaw = 500 将导致车辆以 10 度/秒的速度向左滚动，以 15 度/秒的速度向前倾斜，并以 5 度/秒的速度围绕车辆的 z 轴旋转。

在对这些函数进行任何调用后， 将调用AC_AttitudeControl::rate_controller_run() 。 这会将上面列出的方法的输出转换为滚动、俯仰和偏航输入，这些输入通过它的set_roll、set_pitch、set_yaw 和 set_throttle 方法发送到 AP_Motors 库 。

•  AC_PosControl库 允许对飞机进行 3D 位置控制。通常只使用更简单的 Z 轴（即高度控制）方法，因为更复杂的 3D 位置飞行模式（即 Loiter）使用 AC_WPNav 库。无论如何，这个库的一些常用方法包括：

  • set_alt_target_from_climb_rate() ：这接受以 cm/s 为单位的爬升率并更新绝对高度目标

  • set_pos_target() ：这接受一个 3D 位置向量，它是按照厘米的的偏移量

如果调用 AC_PosControl 中的任何方法，则飞行模式代码也必须调用 AC_PosControl::update_z_controller() 方法。这将运行 z 轴位置控制 PID 循环并将低级油门级别发送到 AP_Motors 库。如果调用任何 xy 轴方法，则 必须调用AC_PosControl::update_xy_controller() 。

• AP_Motors 库包含“电机混合”代码。此代码负责将从 AC_AttitudeControl 和 AC_PosControl 库接收的横滚、俯仰、偏航和油门值转换为绝对电机输出（即 PWM 值）。所以更高级别的库将使用这些功能：

  • set_roll()、set_pitch()、set_yaw() ：接受范围为 -4500 ~ 4500 的 roll、pitch 和 yaw 值。这些不是所需的角度或速率，而只是一个值。例如 set_roll(-4500) 表示尽可能快地向左滚动。

  • set_throttle() ：接受 0 ~ 1000 范围内的绝对油门值。0 = 电机关闭，1000 = 全油门。

• 每种帧类型（quad、Y6、传统直升机）都有不同的类别，但在每个类别中都有一个“ output_armed ”函数，负责将这些滚动、俯仰、偏航和油门值转换为 pwm 输出。这种转换通常包括实施一个“稳定性补丁”，当输入请求超出框架的物理限制时，该补丁处理一个控制轴优先于另一个控制轴（即最大油门和最大侧倾对于 quad 是不可能的，因为一些电机必须更少比其他人引起滚动）。在“output_armed”函数的底部，有一个对 hal.rcout->write() 的调用，它将所需的 pwm 值传递给 AP_HAL 层。

• AP_HAL库（硬件抽象层）为所有板提供一致的接口。特别是 hal.rc_out_write() 函数将使从 AP_Motors 类接收到的指定 PWM 出现在电路板的适当 pwm 引脚输出上。