【ROS&GAZEBO】多旋翼无人机仿真（一）——搭建仿真环境
【ROS&GAZEBO】多旋翼无人机仿真（二）——基于rotors的仿真
【ROS&GAZEBO】多旋翼无人机仿真（三）——自定义多旋翼模型
【ROS&GAZEBO】多旋翼无人机仿真（四）——探索控制器原理
【ROS&GAZEBO】多旋翼无人机仿真（五）——位置控制器
【ROS&GAZEBO】多旋翼无人机仿真（六）——SE(3)几何姿态控制器
【ROS&GAZEBO】多旋翼无人机仿真（七）——四元数姿态控制
【ROS&GAZEBO】多旋翼无人机仿真（八）——手把手编写四元数姿态控制器
原理
在gazebo编写控制器有两种方式，一种是直接编写gazebo插件，另一种是编写ros节点，这两种方式的共同点是都得使用C++编程和CMakeLists，对这两个工具都需掌握一定的熟练度。
上一篇的模型中，我们没有写任何C++代码，但是多旋翼是怎样飞起来的呢，我们来探索一下我们编写的urdf文件的原理。
打开reed_simulator/reed_gazebo/urdf/reed_quad_x_base.xacro，看到下面的代码：
<?xml version="1.0"?>
<robot name="reed_quad_x" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find rotors_description)/urdf/component_snippets.xacro" />
<!-- Instantiate reed_quad_x "mechanics" -->
<xacro:include filename="$(find reed_gazebo)/urdf/reed_quad_x.xacro" />
<!-- Instantiate a controller. -->
<xacro:controller_plugin_macro namespace="${namespace}" imu_sub_topic="imu" />
<!-- Instantiate a ROS message interface -->
<!-- <xacro:ros_interface_plugin_macro namespace="${namespace}" />-->
<xacro:if value="$(arg enable_mavlink_interface)">
<!-- Instantiate mavlink telemetry interface. -->
<xacro:default_mavlink_interface namespace="${namespace}" imu_sub_topic="imu" rotor_count="6" />
</xacro:if>
<!-- Mount an ADIS16448 IMU. -->
<xacro:default_imu namespace="${namespace}" parent_link="${namespace}/base_link" />
<xacro:if value="$(arg enable_ground_truth)">
<xacro:ground_truth_imu_and_odometry namespace="${namespace}" parent_link="${namespace}/base_link" />
</xacro:if>
<xacro:if value="$(arg enable_logging)">
<!-- Instantiate a logger -->
<xacro:bag_plugin_macro
namespace="${namespace}"
bag_file="$(arg log_file)"
rotor_velocity_slowdown_sim="${rotor_velocity_slowdown_sim}"
wait_to_record_bag="$(arg wait_to_record_bag)" />
</xacro:if>
</robot>
第一行代码是将component_snippets.xacro文件引用到本xacro文件中，作用类似于c++中的引用头文件，如下：
<xacro:include filename="$(find rotors_description)/urdf/component_snippets.xacro" />
第二行代码是插入reed_quad_x.xacro模型：
<xacro:include filename="$(find reed_gazebo)/urdf/reed_quad_x.xacro" />
这一行代码的作用是插入一个和ROS交流的插件，作用是发布控制器指令：
<xacro:controller_plugin_macro namespace="${namespace}" imu_sub_topic="imu" />
这几行是通过if判断命令来决定是否插入MAVLINK通信的插件：
<xacro:if value="$(arg enable_mavlink_interface)">
<!-- Instantiate mavlink telemetry interface. -->
<xacro:default_mavlink_interface namespace="${namespace}" imu_sub_topic="imu" rotor_count="6" />
</xacro:if>
这几行是插入了一个IMU插件，模拟了一个ADIS16448用来获取导航信息：
<!-- Mount an ADIS16448 IMU. -->
<xacro:default_imu namespace="${namespace}" parent_link="${namespace}/base_link" />
到此为止好像并没有发现控制器在哪里，不用着急，我们再深入的去挖掘，
打开reed_simulator/reed_gazebo/launch/mav_hovering_example.launch，在这里我们去找一找controller相关的信息
<launch>
<arg name="mav_name" default="reed_quad_x"/>
<arg name="world_name" default="basic"/>
<arg name="enable_logging" default="false" />
<arg name="enable_ground_truth" default="true" />
<arg name="log_file" default="$(arg mav_name)" />
<arg name="debug" default="false"/>
<arg name="gui" default="true"/>
<arg name="paused" default="true"/>
<!-- The following line causes gzmsg and gzerr messages to be printed to the console
(even when Gazebo is started through roslaunch) -->
<arg name="verbose" default="false"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find rotors_gazebo)/worlds/$(arg world_name).world" />
<arg name="debug" value="$(arg debug)" />
<arg name="paused" value="$(arg paused)" />
<arg name="gui" value="$(arg gui)" />
<arg name="verbose" value="$(arg verbose)"/>
</include>
<group ns="$(arg mav_name)">
<include file="$(find reed_gazebo)/launch/spawn_mav.launch">
<arg name="mav_name" value="$(arg mav_name)" />
<arg name="model" value="$(find reed_gazebo)/urdf/mav_generic_odometry_sensor.gazebo" />
<arg name="enable_logging" value="$(arg enable_logging)" />
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/>
</include>
<node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
<rosparam command="load" file="$(find reed_gazebo)/resource/lee_controller_$(arg mav_name).yaml" />
<rosparam command="load" file="$(find reed_gazebo)/resource/$(arg mav_name).yaml" />
<remap from="odometry" to="odometry_sensor1/odometry" />
</node>
<node name="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
</group>
</launch>
我们发现有这样的几行代码：
<node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
这里的意思是创建了一个lee_position_controller_node节点，表面意思就是创建一个位置控制器节点，那有位置控制器，那姿态控制器在哪呢，我第一次看到这里时怀疑位置控制器中已经包含了姿态控制，先带着疑惑来看看lee_position_controller_node是如何写的，找到位置控制器的路径：rotors_simulator/rotors_control/src/nodes/lee_position_controller_node.cpp。
看到这里发现了，这个节点是由c++写的，那是怎么做到的，来梳理一下这里面的逻辑，
int main(int argc, char** argv) {
ros::init(argc, argv, "lee_position_controller_node");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
rotors_control::LeePositionControllerNode lee_position_controller_node(nh, private_nh);
ros::spin();
return 0;
}
在文件最后是main函数，里面创建了一个lee_position_controller_node节点，前几行是ros的一个通用写法，如果熟悉ros就不会感到奇怪（不熟悉的建议花点时间学习ros的基础知识），
rotors_control::LeePositionControllerNode lee_position_controller_node(nh, private_nh);实例化了LeePositionControllerNode类。
这个类里面有什么呢，打开头文件：rotors_simulator/rotors_control/src/nodes/lee_position_controller_node.h
namespace rotors_control {
class LeePositionControllerNode {
public:
LeePositionControllerNode(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh);
~LeePositionControllerNode();
void InitializeParams();
void Publish();
private:
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
LeePositionController lee_position_controller_;
std::string namespace_;
// subscribers
ros::Subscriber cmd_trajectory_sub_;
ros::Subscriber cmd_multi_dof_joint_trajectory_sub_;
ros::Subscriber cmd_pose_sub_;
ros::Subscriber odometry_sub_;
ros::Publisher motor_velocity_reference_pub_;
mav_msgs::EigenTrajectoryPointDeque commands_;
std::deque<ros::Duration> command_waiting_times_;
ros::Timer command_timer_;
void TimedCommandCallback(const ros::TimerEvent& e);
void MultiDofJointTrajectoryCallback(
const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_reference_msg);
void CommandPoseCallback(
const geometry_msgs::PoseStampedConstPtr& pose_msg);
void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg);
};
}
看起来这个类不是很复杂，主要的函数就那么几个，void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg);是获取导航信息的回调函数，直觉告诉我这里面一定藏着关键信息，打开来看一下：
void LeePositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {
ROS_INFO_ONCE("LeePositionController got first odometry message.");
EigenOdometry odometry;
eigenOdometryFromMsg(odometry_msg, &odometry);
lee_position_controller_.SetOdometry(odometry);
Eigen::VectorXd ref_rotor_velocities;
lee_position_controller_.CalculateRotorVelocities(&ref_rotor_velocities);
// Todo(ffurrer): Do this in the conversions header.
mav_msgs::ActuatorsPtr actuator_msg(new mav_msgs::Actuators);
actuator_msg->angular_velocities.clear();
for (int i = 0; i < ref_rotor_velocities.size(); i++)
actuator_msg->angular_velocities.push_back(ref_rotor_velocities[i]);
actuator_msg->header.stamp = odometry_msg->header.stamp;
motor_velocity_reference_pub_.publish(actuator_msg);
}
前几行是将导航信息转换成Eigen格式的数据：
EigenOdometry odometry;
eigenOdometryFromMsg(odometry_msg, &odometry);
lee_position_controller_.SetOdometry(odometry);
lee_position_controller_.CalculateRotorVelocities(&ref_rotor_velocities);从名字来看是计算电机速度的函数，进去看一下：
void LeePositionController::CalculateRotorVelocities(Eigen::VectorXd* rotor_velocities) const {
assert(rotor_velocities);
assert(initialized_params_);
rotor_velocities->resize(vehicle_parameters_.rotor_configuration_.rotors.size());
// Return 0 velocities on all rotors, until the first command is received.
if (!controller_active_) {
*rotor_velocities = Eigen::VectorXd::Zero(rotor_velocities->rows());
return;
}
Eigen::Vector3d acceleration;
ComputeDesiredAcceleration(&acceleration);
Eigen::Vector3d angular_acceleration;
ComputeDesiredAngularAcc(acceleration, &angular_acceleration);
// Project thrust onto body z axis.
double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2));
Eigen::Vector4d angular_acceleration_thrust;
angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
angular_acceleration_thrust(3) = thrust;
*rotor_velocities = angular_acc_to_rotor_velocities_ * angular_acceleration_thrust;
*rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Zero(rotor_velocities->rows()));
*rotor_velocities = rotor_velocities->cwiseSqrt();
}
这四行代码的意思是ComputeDesiredAcceleration(&acceleration);计算期望的加速度，ComputeDesiredAngularAcc(acceleration, &angular_acceleration);计算期望的角加速度。
加速度和角加速度有什么用呢，学过多旋翼控制的都知道，多旋翼位置控制的结果是期望的加速度，姿态控制的结果是期望的角加速度，角加速度通过混控器最后得到期望的电机转速，这是整个的闭环过程，这里正好对应的是这两个控制过程，说明和我们猜想的一样，位置控制包含了整个位置和姿态控制过程。
Eigen::Vector3d acceleration;
ComputeDesiredAcceleration(&acceleration);
Eigen::Vector3d angular_acceleration;
ComputeDesiredAngularAcc(acceleration, &angular_acceleration);
这里将推力映射到z轴上，至于为什么要映射到z轴上，是因为传统多旋翼电机的推力只能垂直于z轴：
// Project thrust onto body z axis.
double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2));
这里的angular_acceleration_thrust是一个4*1的向量，由xyz角加速度和z轴推力组成：
Eigen::Vector4d angular_acceleration_thrust;
angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
angular_acceleration_thrust(3) = thrust;
最后经过控制分配，也叫混控器，angular_acc_to_rotor_velocities_即为控制分配矩阵：
*rotor_velocities = angular_acc_to_rotor_velocities_ * angular_acceleration_thrust;
*rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Zero(rotor_velocities->rows()));
*rotor_velocities = rotor_velocities->cwiseSqrt();
到这里，相信基本上应该有了rotors多旋翼控制器的思路，总结下来是创建了一个位姿控制器节点，里面包括了位置控制器、姿态控制器、混控器。位置控制器输出的结果是加速度，姿态控制器输出的是角加速度，角加速度和z轴推力经过混控器最终得到电机转速。
下一篇继续讲解位置和姿态控制器的原理。
喜欢的朋友可以点个赞，关注微信公众号相互交流：Reed UAV
评论（0）
您还未登录，请登录后发表或查看评论