Udacity机器人软件工程师课程笔记（二十五) - 使用PID控制四轴飞行器 - 四轴飞行器（四旋翼）模拟器

1.四轴飞行器运动学和动力学模型

在讨论四轴飞行器时，明确定义两个参考坐标系会很有帮助：一个固定的世界坐标系$W$和一个牢固地附着到四轴飞行器的质心（CoM）的运动坐标系$B$。

假设运动坐标系B{B}$B$的x轴指向电动机编号1，y轴指向电动机编号2，并且当四轴飞行器电机静止在水平表面上时，z轴指向“上”。

$F_i$ = 电动机$i$的推力(N) 、 $M_i$ = 螺旋桨$i$对$B$的转矩(力矩)(N*m)， 其中i ∈ [1, 2, 3, 4]。

我们可以通过使用三个欧拉角来描述运动框架$B$相对于$W$的方向。具体来说，让我们定义一下横滚（侧倾）角（$\varphi$），俯仰角（$\theta$）和偏航角（$\psi$）作为固有（ZXY）旋转序列，其中，
换句话说，左乘一个向量p，由${}_B^{W}R$表示的$B$将给出以$W$表示的p的坐标。

$${\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]}_W={}_B^{W}R{\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]}_B$$

轴飞行器飞机具有许多功能，使它们在动力学和控制方面变得有趣起来。如前所述，3D空间中的刚体具有六个自由度。但是，四轴飞行器只有四个电机作为控制输入，因此四轴飞行器是欠驱动系统的一个示例。这意味着四轴飞行器不能独立控制每个平移和旋转自由度。

四轴飞行器的欠驱动特性迫使某些旋转运动和平移运动之间发生运动学耦合。为了使四轴飞行器向前或向后移动，${\mathbf{b}}_{\mathbf{x}}$表示“前进”方向，四轴飞行器必须围绕${\mathbf{b}}_{\mathbf{y}}$轴有一个正的俯仰角。同样，为了“向右”移动，即朝-方向移动${\mathbf{b}}_{\mathbf{y}}$轴，四轴飞行器必须具有正侧倾角${\mathbf{b}}_{\mathbf{x}}$。

从概念上讲，垂直方向的移动要简单得多。每个电机沿电机产生一个力${\mathbf{b}}_{\mathbf{z}}$与电机角速度平方成比例的轴，

$$F_i=k_F{\omega }_i^2$$

其中， $k_F$是常数，并且$i$ ∈[1，2，3，4]。每个电机的力沿着${\mathbf{b}}_{\mathbf{z}}$。如果电动机的净推力大于重力，则四旋翼将“向上”加速。当然，反之亦然。

偏航（yaw）运动-关于 ${\mathbf{b}}_{\mathbf{z}}$。电机位于${\mathbf{b}}_{\mathbf{x}}$ 轴，电动机1和3顺时针旋转（关于${\mathbf{b}}_{\mathbf{z}}$ 轴），而位于 ${\mathbf{b}}_{\mathbf{y}}$轴2和4逆时针旋转。需要相反的旋转对来抵消来自四轴飞行器机体上的旋转叶片的感应扭矩。每个转子转矩的大小与电动机角速度的平方成正比，

$$T_i=k_M{\omega }_i^2$$

其中， $k_M$是常数，并且$i$∈[1，2，3，4]。

根据牛顿第三定律，在四轴飞行器上产生的扭矩与螺旋桨的旋转方向相反。，对于Unity模拟器中的四轴飞行器(下图)，电机1和3位于${\mathbf{b}}_{\mathbf{x}}$轴上，并顺时针旋转(从上图可见)，而电机2和4位于${\mathbf{b}}_{\mathbf{y}}$轴上，并逆时针旋转。因此，来自电动机1和3的扭矩作用于+${\mathbf{b}}_{\mathbf{z}}$电机2和4的扭矩作用于-${\mathbf{b}}_{\mathbf{z}}$ 方向。

为了执行一个纯粹的偏航运动，即如果在不改变仰角的情况下进行旋转，一对电机的角速度增加的幅度与另一对电机的角速度减少的幅度应相同。

---

通常，四轴飞行器需要12个广义坐标才能完整描述其在3D空间中的位置和方向（位置3个，方向3个及其时间导数）。更具体地说，我们将这些广义坐标定义为

• x，y和z =四轴飞行器质心（CoM）的位置
• 侧倾角（$\varphi$），俯仰角（$\theta$）和偏航角（$\psi$） = 四轴飞行器运动坐标系$B$相对于固定世界坐标系$W$的方向
• $\dot{x}$，$\dot{y}$和 $\dot{z}$ = 四轴飞行器CoM的线速度
• $\dot{\varphi }$，$\dot{\theta }$和$\dot{\psi }$ = $B$相对于{W}的角速度

因此，四轴飞行器的全状态向量为

$$\xi =[x，y，z，\varphi ，\theta ，\psi ，\dot{x}，\dot{y}，\dot{z}，\dot{\varphi }，\dot{\theta }，\dot{\psi }]$$

在古典力学中，了解状态以及描述状态如何随时间变化的方程（即运动的动力学方程），就可以在将来的某个时间计算系统的状态。

当然，控制的目的是确定执行器输入的大小，以便获得所需的响应。四轴飞行器飞机很有趣，因为它们需要一个非零的电动机输入，以在空中保持简单的恒定位置。然后，在为四轴飞行器设计控制器时，一个诀窍是首先定义“平衡”（悬停）状态${\xi }_{\mathbf{e}\mathbf{q}}$，然后计算相对于以所需方式驱动系统的平衡的电动机速度变化。

计算平衡状态下悬停的标称电动机推力非常简单-这只是一个静力学问题。如果作用在$\widehat{{\mathbf{W}}_{\mathbf{z}}}$方向等于零，则世界坐标系的z方向上没有加速度。用粗体符号表示矢量，所有外力（重量+ 4个电机的推力）之和为：

在平衡悬停状态下，可以假定：

1. 所有电机产生相等的推力，即F1 = F2 = F3 = F4
2. 四轴坐标系$B$和世界坐标系$W$坐标系的Z轴是平行的。从而，${\mathbf{b}}_{\mathbf{z}}\cdot {\mathbf{W}}_{\mathbf{z}}=1$
   由于每个马达产生的推力是
   $$F_i=k_F{\omega }_i^2$$
   平衡悬停时的标称电动机速度为
   $$-mg+4F_i=0$$
   $$-mg+4k_F{\omega }_i^2=0$$
   $${\omega }_i=\sqrt{\frac{mg}{4k_F}}$$

尽管电机速度是由车载计算机直接控制的，但实际上，考虑净推力以及围绕侧倾，俯仰和偏航轴的力矩来控制四旋翼实际上更容易。这种抽象使将整体控制问题细分为各个组件变得更加容易。

例如，一个控制器可能负责维持四旋翼的位置和航向（偏航角），而另一控制器则影响四旋翼的姿态。通常，这种细分策略涉及将一个反馈循环“包装”或“嵌套”在另一个反馈循环内部。然后，将整个控制器称为级联控制器。

2.级联PID控制器

前面使用的海拔控制示例是SISO（单输入单输出）系统。实际上，大多数实际系统是MIMO（多输入，多输出）。处理MIMO系统的一种方法是级联 结构。

级联控制器的一般形式如下所示：

注意有两个闭环反馈控制器，其中主控制器环绕次级回路。这种安排通常要求内部（辅助）循环比外部（主要）循环执行得快得多（快10倍以上）。换句话说，如果外环以10 Hz的频率运行，则内环需要至少100 Hz的频率运行。由于更新频率的这种差异，外环的主要设定值对于内环而言基本上是静态的。

在四旋翼飞机的背景下，主控制器负责维持其3D位置和偏航角，而内环则控制姿态（即，侧倾角，俯仰角和偏航角）。该控制方案将允许操作员“放开”控制输入，并使四旋翼飞机在3D空间中保持稳定的位置和方向（假设外部干扰不太大！）。

在不深入研究所涉及的数学的情况下，让我们用$r$表示四旋翼飞行器当前3D位置与其目标位置之间的向量。

可以从该位置误差的PID反馈中找到四旋翼质量中心的所需线性加速度。通过假设侧倾角和俯仰角保持在平衡悬停状态附近，可以使运动方程线性化，并根据所需的线性加速度来计算所需的侧倾角和俯仰角。位置控制器将所需的侧倾角，俯仰角和偏航角传递给姿态控制器。姿态控制器返回标称电动机速度的变化，该变化会将四旋翼移向这些所需的侧倾角和俯仰角。

3.环境设置

如果没有活动的ROS工作空间，则可以通过键入以下内容来创建一个：

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/
$ catkin_make

现在已经建立了工作区，将实验室资料库克隆或下载到工作区的src目录中：

$ cd ~/catkin_ws/src
$ git clone https://github.com/udacity/RoboND-Controls-Lab.git

下一步使用rosdep install安装缺少的依赖项：

$ cd ~/catkin_ws
$ rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y

catkin_make：

$ cd ~/catkin_ws
$ catkin_make

若没有，可以将以下行添加到.bashrc中，以自动获取所有新终端：

$ source ~/catkin_ws/devel/setup.bash

下载适用于OS的Quad模拟器的最新文件，并将其放在方便的位置。

注意： 需要将模拟器添加到本地系统/OS中，而不要添加到VM中。

可以阅读README.md在github上找到的更多信息。

4.模拟器设置

对于此模块的实验室组件，使用ROS与Udacity基于Unity的仿真环境DroneSim中的四旋翼接口。

DroneSim提供了一套全面的工具，可在开始调试四旋翼飞行控制器并对其进行故障排除时提供极大的帮助。

（1）更新ROS设置文件

在第一次启动DroneSim之前，我们需要确保已正确配置它以与Linux VM上的ROS Master通信。为此，检查ros_settings.txt配置文件。

注意：在大多数不使用虚拟机的系统上，不需要进行更新ros_settings.txt。

ros_settings.txt可以在以下位置找到该文件：

• Windows:
  DroneSim_Win_Data/ros_settings.txt
• MacOS
  DroneSim_OSX.app/Contents/ros_settings.txt

警告：请勿使用默认的Mac文本编辑器，因为它将导致字符问题。如果在VM上连接到ROS时遇到问题，请尝试将行尾的双引号更改vm-ip为正确的双引号。

下面是一个示例配置：

{
    "vm-ip" : "192.168.30.111",
    "vm-port" : 11311,
    "vm-override" : false,
    "host-ip": "0.0.0.0",
    "host-override" : false
}

vm-ip
Linux虚拟机的IP地址。可以在提供的Udacity虚拟机中选择的终端中输入echo $(hostname -I)来确定这个IP。

注意:VM的ip地址可能会改变。如果遇到问题，一定要检查VM的ip是否与ros_settings.txt中的ip匹配.

vm-port

虚拟机上的ROS主端口号。虽然端口11311是默认端口，但是可以通过查看roscore命令的控制台输出来进行验证，该命令作为ROS_MASTER_URI的一部分进行打印（例如 ROS_MASTER_URI=http://hostname:11311/）。

vm-override

这控制是否使用以上信息。如果要从虚拟机外部运行模拟器，则应将其设置为true。因此应设置为true。

host-ip

与主机关联的IP地址。如果host-override为false，则忽略此设置。

host-override

当且仅当正在更新host-ip设置时，才应将其设置为true 。

（2）启动模拟器

要启动模拟器，只需双击DroneSim可执行文件。如果要更改模拟器的分辨率，请执行以下操作。

• Windows：在模拟器启动时按住crtl键。
• Mac：在启动时按住Option键。
• Linux：添加-show-screen-selector作为从终端启动sim时的参数

（3） 按键命令和GUI概述

下面，将介绍模拟器中可用的一些基本GUI功能。除了以下说明之外，模拟器还提供了该模拟器使用的所有键映射的说明。键入“ L”唤起这个图例。

（4）使用键盘移动

要在不使用控制器的情况下围绕模拟器飞行四边形，只需单击左下角的“ Input On”（输入打开）按钮即可。要获取关键命令的列表，这些命令将可以在打开本地输入的情况下四处飞行。

注意：启用本地输入后，来自ROS的强制命令将被忽略。在测试飞行控制器时，请确保未启用该功能！

（5）改变观察方向

要更改场景摄像机相对于四轴飞行器的位置，可以使用键盘上的数字键。如果要更改场景摄像机与四边形之间的距离，可以使用鼠标上的滚轮。要将视点重设为原始视图，只需在相机视口中右键单击即可。

（6）话题

以下是模拟器发布/订阅的主题列表。有关相关消息类型的更多详细信息，只需使用命令$ rosmsg show。要通过命令行获取此主题列表，可以使用命令$ rostopic list。

/quad_rotor/cmd_force（已订阅）
通过该主题，力和扭矩可以传递给四旋翼。所有力单位均为牛顿，所有扭矩单位均为牛顿米。所有力和扭矩均符合ROS单位和坐标约定（REP-103），并将应用于或围绕相应的车身框架轴。

注意：cmd_force命令被模拟器“锁存”。这意味着将连续施加命令的力或扭矩命令，直到模拟器接收到新命令为止。

/quad_rotor/cmd_vel（已订阅）
通过该主题，可以在四边形的车体框架上设置速度。提供此主题仅用于调试目的。

/quad_rotor/image（已发布）
来自四轴深度相机的深度图像已发布到该主题。

/quad_rotor/imu（已发布）
四旋翼的IMU可估算线性加速度，角速度和四旋体的方向。我们不会在此控件实验室中使用此主题，而将使用pose主题提供的完美位置和方向。

/quad_rotor/pose（已发布）
本主题提供四边形的“地面真相”位置和方向，以惯性（世界）框架表示。在实际的四旋翼飞行器中，地面真实姿态无法供我们使用，而是可以使用许多传感器（例如GPS，IMU和大气压力传感器）进行估算。

（7）服务

/quad_rotor/camera_distance
设置摄像机到四轴飞行器机身的距离。单位为米。

/quad_rotor/camera_pose_type
设置相机姿势。以下是可接受值的列表。
0：与惯性坐标系的YZ平面正交，面向X的正方向。
1：垂直于惯性坐标系的XZ平面，面向Y方向正。
2：与惯性坐标系的XY平面正交，面向负Z方向。
3：透视图，从-X，-Y，+ Z象限面向四边形的主坐标系。

/quad_rotor/get_path
获得模拟器的路径

/quad_rotor/set_path
将路径加载到模拟器中

5.其他工具

拥有如此多的自由度，调试和故障排除可能是一个痛苦的过程。为了使事情更简单，我们提供了一些可以使生活更轻松的工具。

（1） 约束力和扭矩

可以限制四边形转子车体框架上的力和扭矩。如果仅尝试调试单个自由度，则此功能很有用。

示例：不允许沿四轴的X轴移动

$ rosservice call /quad_rotor/x_force_constrained "data: true"

示例：不允许围绕四轴的X轴旋转

$ rosservice call /quad_rotor/x_torque_constrained "data: true"

（2） 设定相机姿势

要设置相机姿势，可以在模拟器中右键单击并拖动，也可以使用以下服务呼叫，其中数据参数可以采用以下值：

0：与惯性框架的YZ平面正交，面向正X方向。
1：垂直于惯性框架的XZ平面，面向Y方向正。
2：与中间框架的XY平面正交，面向负Z方向。
3：透视图，从-X，-Y，+ Z象限面向四边形的主体框架。

$ rosservice call /quad_rotor/camera_pose_type "data: 0"

要将相机姿势重置为默认姿势，可以使用服务电话或右键单击。

（3） 设定相机距离

要设置相机与四轴机身之间的距离，可以使用该 /quad_rotor/camera_distance服务。例如，要将摄像头距离设置为20米，可以按以下方式调用服务：

$ rosservice call /quad_rotor/camera_distance "data: 20.0"

要将相机距离重置为默认值，只需在模拟器中右键单击即可。

（4） 禁用重力

重力可能是残酷的现实。特别是在处理姿态调整时。幸运的是，出于调试目的，我们可以在模拟器中禁用重力。为此，请/quad_rotor/gravity按以下方式调用服务：

$ rosservice call /quad_rotor/gravity "data: false"

（5） 设定姿势

要设置四轴飞行器姿势，请使用/quad_rotor/set_pose服务。以下服务调用会将四轴飞行器置回原点：

$ rosservice call /quad_rotor/set_pose "pose:
  position:
    x: 0.0
    y: 0.0
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 0.0" 

（6） 绘图使用 quad_plotter_node

这quad_plotter_node是一个方便的工具，可用于捕获和绘制四边形运动。在仿真中调整四转子时，这可能有用。

服务：

/quad_plotter/start_recording-开始记录剧情数据（姿势）
/quad_plotter/stop_recording-停止记录绘图数据（姿势）
/quad_plotter/clear_path_history -清除剧情历史记录（姿势）
/quad_plotter/clear_waypoints -明确的航点
/quad_plotter/load_waypoints_from_sim-从无人机模拟器获取航点
/quad_plotter/get_path_history -返回路径历史记录为位姿数组
/quad_plotter/plot_one-在给定平面上绘制2D路径（即将选择平面！）
/quad_plotter/plot_3d -创建3D图以透视图描述路径
/quad_plotter/plot_grid-创建一个网格图，显示4个不同的视图

示例：捕获网格图

从模拟器加载航点（可选） $ rosservice call /quad_plotter/load_waypoints_from_sim
开始记录姿势$ rosservice call /quad_plotter/start_recording "{}"
执行我们希望在模拟器中捕获的行为。
停止记录姿势 $ rosservice call /quad_plotter/stop_recording "{}"
生成图 $ rosservice call /quad_plotter/plot_gird "{}"
执行上述步骤后，应生成一个新的带有时间戳的PNG图像并将其放置在/quad_controller/output_data目录中。

6.PIDController类

class PIDController:
    def __init__(self, kp=0.0, ki=0.0, kd=0.0, max_windup=10):
        self.kp_ = kp
        self.kd_ = kd
        self.ki_ = ki

        self.alpha = 0.5

        self.target_ = 0.0

        self.max_windup_ = float(max_windup)

        self.umin = -100.0
        self.umax = 100.0

        self.last_timestamp_ = 0.0
        self.set_point_ = 0.0
        self.start_time_ = 0.0
        self.error_sum_ = 0.0
        self.error_past_ = 0.0

        self.u_p = [0]
        self.u_d = [0]
        self.u_i = [0]

    def reset(self):
        self.kp_ = 0.0
        self.kd_ = 0.0
        self.ki_ = 0.0
        self.target_ = 0.0
        self.last_timestamp_ = 0.0
        self.set_point_ = 0.0
        self.error_sum_ = 0.0
        self.error_past_ = 0.0

    def setTarget(self, target):
        self.target_ = float(target)

    def setKP(self, kp):
        self.kp_ = float(kp)

    def setKI(self, ki):
        self.ki_ = float(ki)

    def setalpha(self, alpha):
        self.alpha = float(alpha)

    def setKD(self, kd):
        self.kd_ = float(kd)

    def setMaxWindup(self, max_windup):
        self.max_windup_ = float(max_windup)

    def update(self, measured_value, timestamp):
        delta_time = timestamp - self.last_timestamp_
        self.last_timestamp_ = timestamp

        error = self.set_point_ - measured_value
        self.error_sum_ += error

        delta_error = error - self.error_past_
        self.error_past_ = error

        if self.error_sum_ > self.max_windup_:
            self.error_sum_ = self.max_windup_
        elif self.error_sum_ < -self.max_windup_:
            self.error_sum_ = -self.max_windup_

        p = self.kp_ * error
        i = self.ki_ * self.error_sum_
        d = self.kd_ * (self.alpha * delta_error / delta_time + (1 - self.alpha) * self.error_past_ / delta_time)
        u = p + i + d

        if u > self.umax:
            u = self.umax
        elif u < self.umin:
            u = self.umin

        self.u_p.append(p)
        self.u_d.append(d)
        self.u_i.append(i)

        return u

7.悬停控制器

如前所述，四旋翼具有四个独立的自由度：

• 侧倾（绕X轴旋转）-导致Y轴同时平移
• 俯仰（绕Y轴旋转）-导致X轴同时平移
• 偏航（绕Z轴旋转）
• 沿Z轴运动

本质上，悬停控制器仅控制一个独立的自由度，即沿Z轴的运动。

一个简单的基于PID的悬停控制器计算给定目标高度与四旋翼飞机当前高度之间的差值以获得误差信号，然后尝试通过调整（控制）沿四旋翼飞机z轴的总推力来最小化此误差。

软件包中hover_controller_node提供了一个示例，该示例quad_controller利用了已实现的PID控制器类。

要测试此悬停控制器，需要启动四旋翼模拟器并启动hover_controller_node。

由于四旋翼模拟器使用ros主题和服务进行交互，因此必须在启动模拟器之前启动ROS Master。

为此，请打开一个新的终端并启动roscore

$ roscore

现在可以在主机平台上启动Quad仿真器了。

启动模拟器

对于每个主机平台（Win / Mac / Linux），围绕此过程的详细信息将有所不同，请参阅实验README文件的“Using the Simulator”部分。

如果提供了该选项，选择计算机系统可以舒适地处理的所需屏幕解决方案和图形质量，然后单击“确定”。

注意，屏幕左下角显示“ Connected to ROS”消息。

如果消息显示“正在连接到ROS”超过几秒钟，则可能要检查主机VM上的防火墙设置，或尝试重新启动模拟器。

此外，可以验证是否可以在以下/quad_rotor/pose主题上看到模拟器发布的姿势：

$ rostopic echo /quad_rotor/pose

如果看到正在发布的消息，那就很好。如果没有看到发布的消息，则可能再次需要检查主机VM上的防火墙设置，或尝试重新启动模拟器。

将模拟器正确连接到ROS后，启动hover_controller_node

$ roslaunch quad_controller hover_controller.launch

如果脚本正常运行，则四轴飞行器的视图应如下所示。如果不是，退出所有内容，然后重试步骤。

四旋翼的hover_controller_node正在运行，但它实际上是置于在地面上的。

这是因为控制器的默认目标高度设置为零。为了命令它在运行时移动到新的目标高度，我们将使用功能强大的ROS实用程序dynamic_reconfigure。

8.dynamic_reconfigure

dynamic_reconfigure是一个ROS软件包，利用它我们可以在运行时修改节点参数，而不必重新启动节点。通常，希望在运行节点时手动修改这些参数，尤其是出于测试或调整目的。

我们在关于节点和主题的讨论中简要讨论了参数服务器，并在arm_mover代码中探讨了参数的简单使用案例。

话虽如此，让我们快速回顾一下。参数服务器是一个共享字典，各种ROS节点都使用它来在运行时存储和检索参数。

它本来就很慢，因此主要用于静态数据，例如配置参数。

例如，在感知拾取和放置实验室中，使用实验室启动文件以参数的形式存储了“ 选择列表”，然后在节点中对其进行了检索。

ROS参数可以是以下数据类型：

• Integers (32-bit)
• Doubles
• Booleans
• Strings
• Lists
• Dictionaries
• …

为了更好地理解dynamic_reconfigure，我们使用它来控制hover_controller_node。

如果Quadrotor Sim已经开始仿真并且hover_controller_node节点启动并正在运行，则可以继续进行下一步。否则，在继续之前参考前面部分。

现在从一个新终端启动一个rqt_reconfigure实例，一个dynamic_reconfigure基于GUI的客户端程序：

$ rosrun rqt_reconfigure rqt_reconfigure

rqt_dynamic_reconfigure GUI的左侧面板列出了所有具有可重新配置参数的活动节点，这里为hover_controller。单击hover_controller显示该节点的可重新配置参数列表。
最初，四旋翼飞机将静止在地面上。将“目标”高度设置为10（单位为米），然后通过增加kp来开始调节控制器，直到产生的推力足够强到足以将四轴飞行器抬离地面为止。

可以尝试使用的Ziegler-Nichols调整方法hover_zn_tuner_node或使用的转动调优算法hover_twiddle_tuner_node。确保运行：

$ roslaunch quad_controller hover_controller.launch

在尝试运行hover_zn_tuner_node或hover_twiddle_tuner_node之前
要运行hover_zn_tuner_node：

$ rosrun quad_controller hover_zn_tuner_node

运行twiddle_tuner_node：

$ rosrun quad_controller hover_twiddle_tuner_node

9.姿态控制器

现在，让我们通过添加其余的独立自由度：

• 侧倾（绕X轴旋转）-导致Y轴同时平移
• 俯仰（绕Y轴旋转）-导致X轴同时平移
• 偏航（绕Z轴旋转）

姿态控制器本质上控制姿态，即四旋翼的侧倾、俯仰和偏航角。

基于PID的姿态控制器运行PID控制器类的三个独立实例，分别用于控制侧倾、俯仰和偏航角。

根据四旋翼的期望角度值与当前角度值之间的差异来计算各个误差值。控制器然后尝试通过调整（控制）沿四旋翼的x，y和z轴的总扭矩来最小化此误差。

套件中attitude_controller_node提供的示例实现，该实现quad_controller利用已实现的PID控制器类。

要测试此Attitude Controller，需要遵循与之前类似的步骤，并确保针对每种类型的控制器都重新开始操作，并且一次最多只能运行一个控制器：

1. 启动roscore：

$ roscore

2. 启动模拟器：参考实验室README文件的“使用模拟器”部分，以获取特定于操作系统的启动模拟器的说明。

3. 运行titude_controller_node：

$ roslaunch quad_controller attitude_controller.launch

我们会观察到即使attitude_controller_node正在运行，四旋翼的行为也没有改变。

这是因为默认目标侧倾，俯仰和偏航设置为零。就像悬停控制器一样，使用dynamic_reconfigure设置目标值，并分别针对三个自由度侧倾，俯仰和偏航分别调整PID控制器。

需要记下这些PID参数，因为将在位置控制器中使用它们。

注意：ZN和Twiddle Tuner节点仅适用于Hover Controller，它们不能与Attitude Controller一起正常工作。

10.位置控制器

到现在为止，我们有一个经过微调的悬停和一个姿态控制器。

如级联控制中所述，位置控制器位于级联PID控制器的外环中，负责命令姿态和推力矢量，以便在三维空间中实现目标定向。

要控制四旋翼飞机沿其x轴的运动，必须调整俯仰角（沿y轴的旋转）。沿着y轴的运动可以通过调节侧倾（沿着x轴的旋转）来控制。

位置控制器根据分别从四旋翼的期望和当前y位置和x位置之间的差计算得出的误差值来确定期望的侧倾角和俯仰角值。

然后将这些所需的侧倾角和俯仰角馈送到在内部回路中运行的姿态控制器，以进行最终的侧倾角和俯仰角控制。

另外，位置控制器还包括先前讨论的悬停控制器，用于控制四旋翼沿z轴的运动。

在quad_controller软件包中提供了position_controller_node的示例实现，该软件包利用已实现的PID控制器类。

要测试此位置控制器，需要执行与之前类似的步骤，并确保针对每种控制器类型都重新开始操作，并且一次不能运行多个控制器

1. 启动roscore：

sh $ roscore

2. 启动模拟器：按照README文件中提供的步骤进行操作。

3. 运行position_controller_node：

sh $ roslaunch quad_controller position_controller.launch

和以前一样，必须使用dynamic_reconfigure设置x，y和z位置的目标值，并为x和y控制器调整PID控制器。

我们必须已经从悬停控制器中保存了PID参数，这些参数可以直接用于z控制器。同样，姿态控制器保存的参数可以重复使用。

注意：ZN和Twiddle Tuner节点仅适用于Hover Controller，它们将无法与Positional Controller一起正常使用。

现在我们可以使用PID控制器在空间中控制位置和姿态了，接下来我们将通过深度神经网络来确定一个移动目标的位置，然后实现控制四轴飞行器来跟随目标。