多旋翼无人机仿真 rotors_simulator：基于PID控制器的速度控制

月照银海似蛟龙

发布时间 2022.10.27阅读数 3089 评论数 0

在这里插入图片描述

前言

无人机（Unmanned Aerial Vehicle），指的是一种由动力驱动的、无线遥控或自主飞行、机上无人驾驶并可重复使用的飞行器，飞机通过机载的计算机系统自动对飞行的平衡进行有效的控制，并通过预先设定或飞机自动生成的复杂航线进行飞行，并在飞行过程中自动执行相关任务和异常处理。

在前面的博客中，分析了 rotors_simulator 一个开源的无人机gazebo的仿真系统的一个控制接口(roll、pitch、yawrate、thrust)，并通过键盘发布控制指令，使飞机飞了起来，但是真正实验过的人则知道，起控制会飞常难，需要一直调整键盘，稍微一不注意，无人机就飞走了。
其原因就是这个接口在无人机内部并没有位置控制的闭环。

在这篇文章中，分析了自动控制原理；并在这篇文章中分析了无人机各种模式的控制框图。

在前面的文章中，完成了无人机的水平位置、垂直位置、航向角度的空间位置控制。

相当于如果有个地面站，就可以完成自动航线，指点飞行等模式了。

无人机还有一个功能就是在悬停模式下，用遥控器控制无人机飞行。本篇博客就利用键盘，在前面做好的位置控制的基础上，做一个基于PID控制器的速度控制。

当有方向按键按下的时候，无人机按期望速度方向飞行，没有方向按键按下，无人机则自动悬停。

加入键盘速度控制指令

功能概述：通过键盘发布无人机期望速度控制指令，其中速度控制指令包括：

无人机机体坐标系线速度 linear.x 、linear.y 、linear.z
无人机机体坐标系航向角速度 angular.z

无人机位置控制解析该4个指令变量，赋值到位置控制控制器的期望速度。
无人机位置控制将期望位置设置为当前位置，使得速度控制模式得以实现。

键盘速度指令发送模块

此模块有开源功能包：teleop_twist_keyboard

该功能包可发送
geometry_msgs/Twist

在这里插入图片描述
发布的话题名称

键盘速度指令解析模块

    void PidPositionControllerNode::KeyboardCmdVelCallback(const geometry_msgs::TwistConstPtr& cmdvel_msg)
    {
        // 取出指令
        geometry_msgs::Twist cmd_vel = *cmdvel_msg;

        std::cout<< "线速度x : "<<cmd_vel.linear.x <<"|||" <<"线速度y :"<<cmd_vel.linear.y<<"|||" <<"线速度z :"<<cmd_vel.linear.z<<std::endl;
        std::cout<< "角速度x : "<<cmd_vel.angular.x <<"|||" <<"角速度y :"<<cmd_vel.angular.y<<"|||" <<"角速度z :"<<cmd_vel.angular.z<<std::endl;

        cmd_vel_x_ = cmd_vel.linear.x ;
        cmd_vel_y_ = cmd_vel.linear.y ;
        cmd_vel_z_= cmd_vel.linear.z ;
        cmd_vel_yaw_ = cmd_vel.angular.z ;

    }

geometry_msgs/Twist 消息类型的回调函数
将控制指令赋值给类的私有变量

键盘速度指令控制

    void PidPositionControllerNode::KeyboardCmdVelControl()
    {
        // 航向速度控制
        // 直接赋值 航向角速度
        des_yawrate_ = cmd_vel_yaw_ ;
        // 限制最大旋转角速度 单位弧度
        double max_yaw_rate = 15 * RADIAN;
        Math_doubleConstrain(des_yawrate_,-max_yaw_rate,max_yaw_rate);


        // 垂直速度控制 
        // 无人机期望垂直速度
        double vel_z_des = cmd_vel_z_;
        // 限制期望垂直速度
        vel_z_des = Math_doubleConstrain(vel_z_des,-3,3);
        // 无人机当前速度
        double vel_z_cur = odometry_.velocity.z();
        // 垂直速度 pid 输出 推力大小
        float  thrust_VelPidOutput = z_vel_pid_controller(vel_z_cur,vel_z_des);
        // 悬停时推力大小
        double loiter_thrust = 1.56779*9.81;
        // 转为电机油门
        double thrust_z = thrust_VelPidOutput + loiter_thrust;
        des_thrust_z_ = thrust_z;

        // 水平速度控制
        // 无人机期望水平速度
        double b_vel_x_des = cmd_vel_x_;
        double b_vel_y_des = cmd_vel_y_;
        // 无人机当前速度   机体坐标系下的
        double b_vel_x_cur = odometry_.velocity.x();
        double b_vel_y_cur = odometry_.velocity.y();
        // 通过 pid 控制器 计算期望加速度 gazebo坐标系下
        double b_acc_x_des = x_vel_pid_controller(b_vel_x_cur,b_vel_x_des);
        double b_acc_y_des = y_vel_pid_controller(b_vel_y_cur,b_vel_y_des);
        // 将期望加速度转为期望姿态角度
        des_pitch_ = b_acc_x_des*RADIAN;
        des_roll_ = - b_acc_y_des*RADIAN; //gazebo里是反的        

        // 控制量
        mav_msgs::RollPitchYawrateThrust roll_pitch_yawrate_thrust;
        roll_pitch_yawrate_thrust.thrust.z= des_thrust_z_;
        roll_pitch_yawrate_thrust.roll = des_roll_;
        roll_pitch_yawrate_thrust.pitch = des_pitch_;
        roll_pitch_yawrate_thrust.yaw_rate = des_yawrate_;


        Control_RollPitchYawrateThrust_pub_.publish(roll_pitch_yawrate_thrust);

    }

将控制指令的航向角速度经过限幅，直接赋值给控制接口
垂直期望速度经过限幅，赋值给垂直速度控制pid
水平期望速度赋值给水平速度控制pid