【ROS&GAZEBO】多旋翼无人机仿真（八） |
您所在的位置：网站首页 › 无人机命名简单易懂 › 【ROS&GAZEBO】多旋翼无人机仿真（八） |
【ROS&GAZEBO】多旋翼无人机仿真（八）
【ROS&GAZEBO】多旋翼无人机仿真（八）——手把手编写四元数姿态控制器
Reed Liao
分类：建模仿真
个人专栏：【ROS&GAZEBO】多旋翼无人机仿真 发布时间 2022.06.09阅读数 4520 评论数 0
【ROS&GAZEBO】多旋翼无人机仿真（一）——搭建仿真环境【ROS&GAZEBO】多旋翼无人机仿真（二）——基于rotors的仿真【ROS&GAZEBO】多旋翼无人机仿真（三）——自定义多旋翼模型【ROS&GAZEBO】多旋翼无人机仿真（四）——探索控制器原理【ROS&GAZEBO】多旋翼无人机仿真（五）——位置控制器【ROS&GAZEBO】多旋翼无人机仿真（六）——SE(3)几何姿态控制器【ROS&GAZEBO】多旋翼无人机仿真（七）——四元数姿态控制【ROS&GAZEBO】多旋翼无人机仿真（八）——手把手编写四元数姿态控制器 前言因为一些乱七八糟的事情(ˉ▽ˉ；)…，大概有三周没有更新了，这周终于有时间来更新一下四元数姿态控制了，o(￣▽￣)ブ。上一篇讲了四元数控制的原理，详情请移步此处，废话不多说，这一篇我们来编写四元数控制器，将其应用于rotors中。 改写rotors姿态控制器为了省事，本人就没有去从头开始写姿态控制器插件，而是在rotors的姿态控制器上进行修改。当然感兴趣的朋友可以仿造rotors插件的写法，从零开始写（就是会比较麻烦，还可能会面对ros和gazebo中杂七杂八的bug）。 下面我们正式开始，先打开lee_position_controller.cpp文件，（如果对rotors的层次不理解的话，建议回到我前面的文章学习【ROS&GAZEBO】多旋翼无人机仿真（四）——探索控制器原理） 找到其中的CalculateRotorVelocities函数，这里就是最核心的位置和姿态控制代码。这里我也贴出来了，并且用中文进行注释了，方便大家学习： 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); // 映射油门到机体z轴 double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2)); // 组合角加速度和油门输出 Eigen::Vector4d angular_acceleration_thrust; angular_acceleration_thrust.block(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(); }而姿态控制器在此次：ComputeDesiredAngularAcc，所以我们要改掉这里，这里我的做法是复制ComputeDesiredAngularAcc这个函数，将其重命名为：ComputeDesiredAngularAcc_Q，接口和原生的一样，这也方便我们去修改。 void LeePositionController::ComputeDesiredAngularAcc_Q(const Eigen::Vector3d& acceleration, Eigen::Vector3d* angular_acceleration) const { }接下来，进入主题： 这里首先模仿rotors的写法，做个断言，判断一下angular_acceleration有没有： assert(angular_acceleration);下面获取当前的四元数和期望的航向角： // 获取当前四元数 Eigen::Quaterniond quat = odometry_.orientation; // 获取期望航向角 double yaw_d = command_trajectory_.getYaw();然后获取期望的加速度，需要注意的一点是：由位置控制器计算期望加速度的时候是误差-期望，和我们平常的逻辑是反的，因此需要取反： // 归一化加速度 Eigen::Vector3d accel_norm; // 由于计算期望加速度是误差-期望，因此，这里需要取反 accel_norm = -acceleration / acceleration.norm();然后计算期望的滚转和俯仰角，我这里用了简化的方法进行计算，需要注意的是俯仰滚转和XY位置的对应关系： // 简化的方法计算期望滚转和俯仰 double pitch_d = -accel_norm.x() / acceleration.z(); double roll_d = accel_norm.y() / acceleration.z();然后将期望的角度转化为期望四元数： Eigen::Quaterniond quat_d = Eigen::AngleAxisd(yaw_d, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch_d, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(roll_d, Eigen::Vector3d::UnitX());这一步可能会比较难理解，这里计算过程用的是轴角的方法，具体的轴角是什么大家也可以移步至我前面的文章，先不管具体的细节，反正通过这一步得到了期望四元数。 归一化一下期望四元数： quat_d.normalize();如果看了我上一篇文章，下面就很好理解了，看一下具体过程就行了，这里的k是四元数的增益，根据实际情况进行调节： // 四元数误差增益 double k = 4.0; // 计算四元数误差 Eigen::Quaterniond quat_err = quat_d * quat.inverse(); // 计算期望角速度 Eigen::Vector3d angle_error = k * sgn(quat_err.w()) * quat_err.vec();这里是角速度误差计算期望角加速度的步骤，用了最简单的误差，想得到更好的效果，可以换成PID等方法，这里留给大家自由发挥了。 // 计算角速度误差 Eigen::Vector3d angular_rate_error = angle_error - odometry_.angular_velocity; // 计算期望角加速度 *angular_acceleration = angular_rate_error.cwiseProduct(normalized_angular_rate_gain_);整个过程就完成了，具体代码如下： void LeePositionController::ComputeDesiredAngularAcc_Q(const Eigen::Vector3d& acceleration, Eigen::Vector3d* angular_acceleration) const { assert(angular_acceleration); // 获取当前四元数 Eigen::Quaterniond quat = odometry_.orientation; // 获取期望航向角 double yaw_d = command_trajectory_.getYaw(); // 归一化加速度 Eigen::Vector3d accel_norm; // 由于计算期望加速度是误差-期望，因此，这里需要取反 accel_norm = -acceleration / acceleration.norm(); // 简化的方法计算期望滚转和俯仰 double pitch_d = -accel_norm.x() / acceleration.z(); double roll_d = accel_norm.y() / acceleration.z(); Eigen::Quaterniond quat_d = Eigen::AngleAxisd(yaw_d, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch_d, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(roll_d, Eigen::Vector3d::UnitX()); quat_d.normalize(); // 四元数误差增益 double k = 4.0; // 计算四元数误差 Eigen::Quaterniond quat_err = quat_d * quat.inverse(); // 计算期望角速度 Eigen::Vector3d angle_error = k * sgn(quat_err.w()) * quat_err.vec(); // 计算角速度误差 Eigen::Vector3d angular_rate_error = angle_error - odometry_.angular_velocity; // 计算期望角加速度 *angular_acceleration = angular_rate_error.cwiseProduct(normalized_angular_rate_gain_); }移植rotors_control另外，我在这里将rotors_control移植到了reed_control包，将LeePositionController改成了 QuatPositionController，这样做的目的是为了使整个工程代码逻辑更清晰和更容易移植。此处代码就不贴出来了，代码我上传至了gitee仓库：https://gitee.com/liaoluweillw/reed_simulator.git 结尾到处为止，四元数姿态控制器就写完了。欢迎感兴趣的小伙伴添加微信 Reed_UAV 进行交流：
打赏 0 点赞 0 收藏 0 分享 微信 微博 QQ 图片 上一篇：【ROS&GAZEBO】多旋翼无人机仿真（七）——四元数姿态控制 下一篇：STM32CubeMX+Keil实现 STM32F4 SDcard+SPI Flash读卡器 |
【本文地址】
今日新闻 |
推荐新闻 |