【rotors】多旋翼无人机仿真（四）——参数补偿和PID控制

【rotors】多旋翼无人机仿真（一）——搭建rotors仿真环境
【rotors】多旋翼无人机仿真（二）——设置飞行轨迹
【rotors】多旋翼无人机仿真（三）——SE3控制
【rotors】多旋翼无人机仿真（四）——参数补偿和PID控制
【rotors】多旋翼无人机仿真（五）——多无人机仿真

本贴内容参考两位博主的内容：
月照银海似蛟
Reed Liao

1、前言

在上一节中，我们讲解了SE3算法和rotor的算法实现，本节中我们讲一讲rotor的参数补偿 和pid控制

2、绘制算法的收敛过程

通过se3控制，当我们指定目标点(x y z yaw)后无人机就会收敛至设定目标点，但是我们还关心收敛速度，这是评价一个控制算法的重要指标，为了直观算法的收敛过程，我们用python的matplotlib库绘制算法的收敛过程曲线

（1）设置目标点

修改hovering_example节点程序，将目标点设置为(1 1 1 0)，当然不修改也行，但修改了可以更直观对比xyz的收敛速度

// hovering_example.cpp
// 期望位置和偏航角
Eigen::Vector3d desired_position(1.0, 1.0, 1.0);
double desired_yaw = 0.0;

（2）编写绘制曲线程序

首先说一下绘制曲线的一个编程思路，当我们运行rotor时仿真时（如运行mav_hovering_example.launch），会有XXX/command/trajectory和XXX/odometry_sensor1/odometry的ros话题发布无人机的目标点和当前无人机位姿信息，那么我们只需要用python的rospy订阅这两个话题，保存一段时间的话题信息，最后用matplotlib库绘制相应曲线即可。
在~/UAV_rotors/src/rotors_simulator/rotors_gazebo/scripts文件夹下创建draw_curve.py，其内容为：

#! /home/用户名/anaconda3/envs/虚拟环境名字/bin/python
"""
绘制无人机的曲线误差
"""

import rospy
import time
import numpy as np
import matplotlib.pyplot as plt
from trajectory_msgs.msg import MultiDOFJointTrajectory
from nav_msgs.msg import Odometry

plt.rcParams['font.sans-serif'] = ['SimHei']    #指定字体为SimHei
plt.rcParams['axes.unicode_minus'] = False      #让-号显示正常

def trajectory_callback(msg):
    # trajectory回调函数
    global begin_time,desire_x,desire_y,desire_z
    if not begin_time:
        begin_time = msg.header.stamp.to_sec()
    desire_x = msg.points[0].transforms[0].translation.x
    desire_y = msg.points[0].transforms[0].translation.y
    desire_z = msg.points[0].transforms[0].translation.z

def odometry_callback(msg):
    # odometry回调函数
    global begin_time,time_list,x,y,z,desire_x,desire_y,desire_z
    if  begin_time:
        time_list.append(msg.header.stamp.to_sec() - begin_time)
        x['actual'].append(msg.pose.pose.position.x)
        y['actual'].append(msg.pose.pose.position.y)
        z['actual'].append(msg.pose.pose.position.z)
        x['desire'].append(desire_x)
        y['desire'].append(desire_y)
        z['desire'].append(desire_z)

def draw_curve():
    # 绘制曲线
    global time_list,x,y,z,trajectory_sub,odometry_sub,fig, axes
    print('draw curve...')
    # 取消订阅话题
    trajectory_sub.unregister()
    odometry_sub.unregister()
    # 绘制曲线
    fig, axes = plt.subplots(3, 1, figsize=(10, 15))
    axes[0].plot(time_list,x['desire'],'r',time_list,x['actual'],'b')
    axes[0].grid(True)
    axes[0].legend(['期望曲线','实际曲线'])
    axes[0].set_title('X')

    axes[1].plot(time_list, y['desire'], 'r', time_list, y['actual'], 'b')
    axes[1].grid(True)
    axes[1].legend(['期望曲线', '实际曲线'])
    axes[1].set_title('Y')

    axes[2].plot(time_list, z['desire'], 'r', time_list, z['actual'], 'b')
    axes[2].grid(True)
    axes[2].legend(['期望曲线', '实际曲线'])
    axes[2].set_title('Z')

    plt.show()



begin_time = None
time_list = []
draw_duration = 10 # 绘制时长，单位s
desire_x = 0
desire_y = 0
desire_z = 0
x = {
    
    'desire':[],'actual':[]}
y = {
    
    'desire':[],'actual':[]}
z = {
    
    'desire':[],'actual':[]}
trajectory_topic = "/firefly/command/trajectory"    # 轨迹topic名称
odometry_topic = "/firefly/odometry_sensor1/odometry"   # odometry topic 名称
rospy.init_node("draw_curve_node")
trajectory_sub = rospy.Subscriber(trajectory_topic,
                                  MultiDOFJointTrajectory,
                                  trajectory_callback,queue_size=10)
odometry_sub = rospy.Subscriber(odometry_topic,
                                  Odometry,
                                  odometry_callback, queue_size=10)
print('等待服务端发送轨迹指令...')
while(not begin_time):  # 等待接受到数据
    pass
print('正在接受数据...')
time.sleep(draw_duration)
draw_curve()

说明一下，第一行指定了要用的python解释器，由于ROS melodic的python是2.7版本的，许多库不能正常运行，因此这里我使用conda创建的python3.7环境的解释器运行该py文件，这里就修改为自己的python解释器位置即可；其次我绘制曲线时有中文，但是Ubuntu默认没有该字体，需要自己下载安装，安装方法见这里；最后，程序需要自己修改trajectory_topic和odometry_topic，我运行的无人机型号是firefly，如果是其他型号就自行修改topic name

（3）运行程序 绘制曲线

# 终端1
roscore
# 终端2
cd ~/UAV_rotors/src/rotors_simulator/rotors_gazebo/scripts
chmod 777 draw_curve.py
./draw_curve.py
# 终端3
roslaunch rotors_gazebo mav_hovering_example.launch

无人机起飞后，程序会记录10s的数据并绘制为曲线：

3、参数补偿

如果你运行mav_hovering_example_with_vi_sensor.launch或者修改无人机型号为其他型号时，重新绘制收敛曲线，你大概会发现结果存在着较大的静差，就像下面这样：

原因在于无人机的控制节点lee_position_controller_node并没有根据无人机型号的不同而自适应修改无人机的参数如质量，lee_position_controller_node里面的无人机参数是无深度摄像头的firefly型号飞机参数，当你无人机上加载深度摄像头或者其他型号无人机时，无人机的质量等参数就改变了，自然控制就不对了，存在着静差。当然想消除静差你可以修改无人机参数，但这样比较麻烦，得自己去看无人机建模文件，下面讲一种简单有效的参数补偿方法：

（1）打印无人机当前的期望加速度和角加速度

修改~/UAV_rotors/src/rotors_simulator/rotors_control/src/library/lee_position_controller.cpp，打印无人机当前的期望加速度和角加速度：

Eigen::Vector3d angular_acceleration;
ComputeDesiredAngularAcc(acceleration, &angular_acceleration);
std::cout << "angular_acceleration = "<<angular_acceleration.transpose() << std::endl;

double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2));
std::cout << "thrust = "<<thrust << std::endl;

期望加速度和角加速度是控制的关键，我们只要补偿使这两个变量正确即可

（2）运行程序，查看无人机悬停时的angular_acceleration和thrust

roslaunch rotors_gazebo mav_hovering_example_with_vi_sensor.launch

当无人机稳定悬停时，查看终端打印结果

可以看到稳定悬停时的升力thrust =16.5626，而角加速度angular_acceleration[1] = -3.14854

分析一下，正常情况下，无人机处于悬停时，升力应该是等于重力的，角加速度应该是全0向量，而这里升力并不等于重力， 角加速度有一项不为0，我们只需对这两部分补偿即可

（3）补偿参数

修改~/UAV_rotors/src/rotors_simulator/rotors_control/src/library/lee_position_controller.cpp，补偿参数：

Eigen::Vector3d angular_acceleration;
ComputeDesiredAngularAcc(acceleration, &angular_acceleration);
angular_acceleration[1] -= 3.14854; //angular_acceleration补偿

double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2));
double k = 16.5626 / (vehicle_parameters_.mass_ * vehicle_parameters_.gravity_);
thrust = k * thrust; //thrust补偿

（4）重新编译

cd ~/UAV_rotors
catkin build

（5）重新运行绘制曲线

# 终端1
roscore
# 终端2
cd ~/UAV_rotors/src/rotors_simulator/rotors_gazebo/scripts
chmod 777 draw_curve.py
./draw_curve.py
# 终端3
roslaunch rotors_gazebo mav_hovering_example_with_vi_sensor.launch

从结果来看，静差被成功补偿掉了

4、PID控制

补偿掉静差后，我们观察一下收敛速度，从(0 0 0)到(1 1 1)需要5s的时间，感觉还是太久了，我们加入PID加快收敛速度：

（1）修改程序

修改~/UAV_rotors/src/rotors_simulator/rotors_control/include/rotors_control/lee_position_controller.h，加入PID控制函数声明：

class LeePositionController {
    
    
 public:
  LeePositionController();
  ~LeePositionController();
  void InitializeParameters();
  void CalculateRotorVelocities(Eigen::VectorXd* rotor_velocities) const;

  void SetOdometry(const EigenOdometry& odometry);
  void SetTrajectoryPoint(
    const mav_msgs::EigenTrajectoryPoint& command_trajectory);

  LeePositionControllerParameters controller_parameters_;
  VehicleParameters vehicle_parameters_;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 private:
  bool initialized_params_;
  bool controller_active_;

  Eigen::Vector3d normalized_attitude_gain_;
  Eigen::Vector3d normalized_angular_rate_gain_;
  Eigen::MatrixX4d angular_acc_to_rotor_velocities_;

  mav_msgs::EigenTrajectoryPoint command_trajectory_;
  EigenOdometry odometry_;

  void ComputeDesiredAngularAcc(const Eigen::Vector3d& acceleration,
                                Eigen::Vector3d* angular_acceleration) const;
  void ComputeDesiredAcceleration(Eigen::Vector3d* acceleration) const;
  Eigen::Vector3d position_error_pid(Eigen::Vector3d position_error) const; // 函数声明
};

修改~/UAV_rotors/src/rotors_simulator/rotors_control/src/library/lee_position_controller.cpp，定义pid函数：

Eigen::Vector3d LeePositionController::position_error_pid(Eigen::Vector3d position_error) const
{
    
    
  // position_error的PID控制
  Eigen::Vector3d Kp(7, 7, 7) , Ki(0, 0, 0),Kd(2.5,2.5,2.5); 
  Eigen::Vector3d max_output_pid(10,10,10) , min_output_pid(-10,-10,-10);
  Eigen::Vector3d max_output_i(5,5,5),min_output_i(-5,-5,-5);
  static Eigen::Vector3d error(0,0,0),error_last(0,0,0),error_last_last(0,0,0);
  static Eigen::Vector3d output_p(0,0,0),output_i(0,0,0),output_d(0,0,0),output_pid(0,0,0); 

  // 偏差计算
  error = position_error ;
  output_p +=  Kp.cwiseProduct(error - error_last) ;
  output_i +=  Ki.cwiseProduct(error) ;
  output_d +=  Kd.cwiseProduct(error - 2*error_last + error_last_last) ;
  // 更新偏差量
  error_last_last = error_last ;
  error_last = error ;

  output_i.cwiseMin(max_output_i);
  output_i.cwiseMax(min_output_i);
  

  // 汇总输出
  output_pid = output_p + output_i + output_d;

  output_pid.cwiseMin(max_output_pid);
  output_pid.cwiseMax(min_output_pid);
  // 控制器最终输出
  return output_pid; 
}

有许多地方可以加入PID，这里我们将位置误差的比例控制改为PID控制：

 *acceleration = (position_error_pid(position_error)   // PID控制
      + velocity_error.cwiseProduct(controller_parameters_.velocity_gain_)) / vehicle_parameters_.mass_
      - vehicle_parameters_.gravity_ * e_3 - command_trajectory_.acceleration_W;

（2）重新编译

cd ~/UAV_rotors
catkin build

（3）重新运行绘制曲线

# 终端1
roscore
# 终端2
cd ~/UAV_rotors/src/rotors_simulator/rotors_gazebo/scripts
chmod 777 draw_curve.py
./draw_curve.py
# 终端3
roslaunch rotors_gazebo mav_hovering_example_with_vi_sensor.launch

加入PID后收敛速度大概3s，继续调整pid参数或者在其他地方加入pid可以进一步提高收敛速度，说一下我pid调参方法，因为仿真中还不存在干扰，因此曲线不存在静差，因而不需要积分部分，先调整比例到曲线有超调，然后适当加入微分将超调减小。