四旋翼部分代码解析（一）

2020.01.14向峻宏

飞机模型整体结构

a.JPG

这是四旋翼无人机的核心三大块，无人机、四旋翼和刚体部分，简单来说，一个飞行器就是一个刚体仿真加上环境仿真再加上飞控。

飞机模型刚体部分代码解析

include "rigidBody.h"
include "utils.h"
include 
include "odeint-v2/boost/numeric/odeint.hpp"
include 

namespace odeint = boost::numeric::odeint;


RigidBody::RigidBody()
{
  ticks = 0;/*计步器*/

  /* following data is obtained from shaojie simulator */
  mass = 0.74;/*质量*/
  double Ixx = 2.64e-3, Iyy = 2.64e-3, Izz = 4.96e-3;
  inertia = Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal();/*定义三轴的惯性矩*/

  position = Eigen::Vector3d::Zero();
  velocity = Eigen::Vector3d::Zero();
  //acceleration = Eigen::Vector3d::Zero();
  force = Eigen::Vector3d::Zero();

  attitude = Eigen::Quaterniond(Eigen::Matrix3d::Identity());
  angularVelocity = Eigen::Vector3d::Zero();
  torque = Eigen::Vector3d::Zero();/*定义了位置，速度，加速度，力，姿态矩阵，角速度，转矩*/

  updateInternalState();/*更新内部状态*/
}

void RigidBody::updateInternalState(void)
{
  internalState[0]  = position(0); 
  internalState[1]  = position(1); 
  internalState[2]  = position(2); 

  internalState[3]  = velocity(0); 
  internalState[4]  = velocity(1); 
  internalState[5]  = velocity(2); 

  //internalState[6]  = acceleration(0); 
  //internalState[7]  = acceleration(1); 
  //internalState[8]  = acceleration(2); 

  internalState[6]  = attitude.x(); 
  internalState[7]  = attitude.y(); 
  internalState[8]  = attitude.z(); 
  internalState[9]  = attitude.w(); 

  internalState[10] = angularVelocity(0); 
  internalState[11] = angularVelocity(1); 
  internalState[12] = angularVelocity(2); 
}
/*内部状态函数的数组声明*/

void RigidBody::operator()(const RigidBody::InternalState &x, RigidBody::InternalState &dxdt, const double /* t */)/*对当前状态量进行操作的函数*/
{
    Eigen::Vector3d x_dot, v_dot, omega_dot;
    Eigen::Vector4d q_dot;
    Eigen::Vector4d q(attitude.x(), attitude.y(), attitude.z(), attitude.w());

    Eigen::Vector3d omega = angularVelocity;
    Eigen::Matrix4d omegaMat = Omega(omega);

    x_dot = velocity;
    v_dot = force/mass;
    q_dot = 0.5*omegaMat*q;
    omega_dot = inertia.inverse() */*x_dot对应速度，v_dot对应加速度，q_dot对应四个姿态，omega_dot对应角速度*/
      (torque - omega.cross(inertia*omega));

    dxdt[0]  = x_dot(0); 
    dxdt[1]  = x_dot(1); 
    dxdt[2]  = x_dot(2); 

    dxdt[3]  = v_dot(0); 
    dxdt[4]  = v_dot(1); 
    dxdt[5]  = v_dot(2); 

    dxdt[6]  = q_dot(0); 
    dxdt[7]  = q_dot(1); 
    dxdt[8]  = q_dot(2); 
    dxdt[9]  = q_dot(3); 

    dxdt[10] = omega_dot(0); 
    dxdt[11] = omega_dot(1); 
    dxdt[12] = omega_dot(2); /*分别赋予当前状态值*/
}

void RigidBody::sim_step(double dt)/*模拟进行的一步*/
{
    odeint::integrate(boost::ref(*this), internalState, 0.0, dt, dt);/*这一步声明我没看懂,感觉是更新内部状态的*/

    position(0) =  internalState[0]; 
    position(1) =  internalState[1]; 
    position(2) =  internalState[2]; 

    velocity(0) =  internalState[3]; 
    velocity(1) =  internalState[4]; 
    velocity(2) =  internalState[5]; 

    attitude.x() =  internalState[6]; 
    attitude.y() =  internalState[7]; 
    attitude.z() =  internalState[8]; 
    attitude.w() =  internalState[9]; 
    
    attitude.normalize();

    angularVelocity(0) =  internalState[10]; 
    angularVelocity(1) =  internalState[11]; 
    angularVelocity(2) =  internalState[12]; /*将各种状态参量全部更新一遍*/
    
    updateInternalState();
    
    ticks += 1;/*同时计步器加1*/
}

void RigidBody::set_force(Eigen::Vector3d _force)
{
    force = _force;/*更新受力*/
}

void RigidBody::set_torque(Eigen::Vector3d _torque)
{
    torque = _torque;/*更新转矩*/
}

Eigen::Quaterniond RigidBody::get_attitude()
{
    Eigen::Quaterniond q(attitude.w(), attitude.x(), attitude.y(), attitude.z());
    return q;/*获得四个参量的q表*/
}

void RigidBody::set_attitude(Eigen::Quaterniond _attitude)
{
    attitude.w() = _attitude.w(); 
    attitude.x() = _attitude.x(); 
    attitude.y() = _attitude.y(); 
    attitude.z() = _attitude.z(); /*用获得的q表值来更新四个姿态参量*/
}

Eigen::Vector3d RigidBody::get_position()
{
    return Eigen::Vector3d(position(0), position(1), position(2));
}

void RigidBody::external_set_position(Eigen::Vector3d setting_vec)
{
    position(0) = setting_vec(0);
    position(1) = setting_vec(1);
    position(2) = setting_vec(2);
    updateInternalState();
}/*这同样是获得位置信息后再更新位置信息*/

Eigen::Vector3d RigidBody::get_velocity()
{
    return Eigen::Vector3d(velocity(0), velocity(1), velocity(2));
}

double RigidBody::get_mass()
{
    return mass;
}

Eigen::Vector3d RigidBody::get_angularVelocity() const
{
    return angularVelocity;
}

Eigen::Matrix3d RigidBody::get_inertia() const
{
    //TODO: get functions, should use constant?
    return inertia;
}/*更新飞机模型的速度，质量，角速度和惯性矩*/

这是rigidbody部分的代码解析，大家可以看看了解原理，希望有错的地方大家可以指出，后2天会将drone和quadrotor的代码解析发出。