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

  • Post author:
  • Post category:其他



【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的数据并绘制为曲线:

image-20230126175744266



3、参数补偿

如果你运行

mav_hovering_example_with_vi_sensor.launch

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

image-20230126180056157

原因在于无人机的控制节点

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

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

image-20230126181049383

可以看到稳定悬停时的升力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

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

image-20230126181842336



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控制:

image-20230126182537603

 *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

image-20230126182807691

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



版权声明:本文为caiqidong321原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。