【rotors】多旋翼无人机仿真（一）——搭建rotors仿真环境
【rotors】多旋翼无人机仿真（二）——设置飞行轨迹
【rotors】多旋翼无人机仿真（三）——SE3控制
【rotors】多旋翼无人机仿真（四）——参数补偿和PID控制
【rotors】多旋翼无人机仿真（五）——多无人机仿真
本贴内容参考两位博主的内容：
月照银海似蛟
Reed Liao
在上一节中，我们讲解了SE3算法和rotor的算法实现，本节中我们讲一讲rotor的参数补偿 和pid控制
通过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的数据并绘制为曲线：
如果你运行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
从结果来看，静差被成功补偿掉了
补偿掉静差后，我们观察一下收敛速度，从(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调参方法，因为仿真中还不存在干扰，因此曲线不存在静差，因而不需要积分部分，先调整比例到曲线有超调，然后适当加入微分将超调减小。