MARSIM / cascadePID_node.cpp

1.代码主要作用

主要作用就是：

        级联PID控制器控制四旋翼无人机。订阅odom和cmd_pose主题以获取无人机的当前状态和期望姿态。

        然后，它使用级联PID控制器计算所需的电机转速并将其发布到cmd_RPM主题。

2.级联PID控制器

    级联PID控制器的输入是：

        1. 期望位置，速度和加速度

        2. 期望姿态

        3. 当前位置，速度和姿态

    级联PID控制器在四旋翼无人机的应用：

        * 外环pid控制器： 控制无人机的位置和速度

        * 内环pid控制器： 控制无人机的姿态

    外环PID控制器负责将无人机的当前位置和速度控制到期望的位置和速度。内环PID控制器负责将无人机的当前姿态控制到期望的姿态。

3.SE(3) 控制器 

SE(3)控制器是一种用于控制四旋翼无人机的非线性控制器。SE(3)是指欧拉空间，是三维空间的旋转和平移的集合。SE(3)控制器将四旋翼无人机的控制视为一个整体，使用非线性控制理论来控制无人机的姿态和位置。

SE(3)控制器的工作原理如下：

1. 首先，SE(3)控制器将无人机的当前状态作为输入，计算出期望的状态。
2. 然后，SE(3)控制器使用非线性控制理论计算出所需的控制输出。
3. 最后，SE(3)控制器将控制输出作为输入，驱动无人机飞行。

4. 级联PID和SE(3) 的区别

级联PID控制器和SE(3)控制都是用于控制四旋翼无人机的控制方法。两者的主要区别在于：

• 级联PID控制器是基于PID控制器的控制方法，而SE(3)控制是基于非线性控制理论的控制方法。
• 级联PID控制器将四旋翼无人机的控制分为多个子系统，每个子系统使用一个PID控制器进行控制。SE(3)控制则将四旋翼无人机的控制视为一个整体，使用非线性控制器进行控制

 5.代码

#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <cascadePID.hpp>
#include <std_msgs/Float32MultiArray.h>
#include <geometry_msgs