多旋翼无人机

一、使用jmavsim仿真程序

        无人机半物理仿真程序就是通过软件程序模拟无人机的实际飞行状态，这使得我们可以在计算机上直接运行飞控程序，使得程序编写、测试工作省时省力。在PX4飞控程序中提供了一种简单有效的半物理仿真程序jMavSim。它使用Java程序模拟并运行无人机的仿真程序，并提供一个可视化的窗口程序方便使用者进行查看。

        对于PX4飞控程序需要使用Posix编译选项，运行在普通PC电脑上，并使用jMavSim编译选项。具体命令如下：

make posix_sitl_default jmavsim

        编译完成后就会在build/posix_sitl_default目录下生成一个可以在PC电脑上运行的可执行程序px4，并通过jMavSim模拟器进行运行。其运行效果如下图所示：

        启动jMavSim半物理仿真之后可以在px4程序控制台下执行自动起飞命令：

pxh> commander takeoff
INFO  [navigator] RTL HOME activated
INFO  [navigator] RTL: climb to 55 m (5 m above home)
INFO  [navigator] RTL: return at 55 m (5 m above home)
INFO  [navigator] RTL: descend to 55 m (5 m above home)
INFO  [navigator] RTL: loiter 5.0s
INFO  [navigator] RTL: land at home
INFO  [commander] Landing detected

        执行上面命令之后，无人机会自动起飞，悬停5秒之后自动着陆。我们可以通过jMavSim仿真窗口看到无人机的整个起飞、悬停、降落的全部过程。

        此外，我们还可以为jMavSim仿真程序设定起飞点，并通过地面站进行航迹点任务规划和执行。首先在Linux系统中的终端中设置环境变量，指定无人机的起飞点：

export PX4_HOME_LAT=41.7028393070
export PX4_HOME_LON=122.8985487988
export PX4_HOME_ALT=500.000
make posix_sitl_default jmavsim

        其中，PX4_HOME_LAT为起飞点纬度，PX4_HOME_LON为起飞点经度，PX4_HOME_ALT为起飞点海拔高度。最后，再次执行编译并启动半物理仿真程序，然后打开地面站程序，就可以看到当前无人机所在的位置了，如下图所示：

        注意，此时PX4程序是使用Posix标准编译并运行在普通PC电脑上的，并没有烧写到Pixhawk当中，PX4程序与地面站程序采用的是UDP网络通信来传输的Mavlink消息。

        我们可以通过地面站程序在地图上设定几个航迹点，并上传到无人机，然后通过鼠标在“Start Mission”上由左至右滑动启动航迹点任务。于是PX4飞控程序则可以通过jMavSim仿真程序执行航迹点任务，如下图所示：

 
二、自己编写半物理仿真程序

        在上一节中我们已经介绍了动力学建模的相关内容，在本节只给出仿真程序的实现代码：

int hil_task_main(int argc, char *argv[])
{
    float theta_t = 0.0f;
    long curr_time = 0;
    long last_time = 0;

    int output_sub = orb_subscribe(ORB_ID(actuator_outputs));
    int armed_sub = orb_subscribe(ORB_ID(actuator_armed));

    struct actuator_armed_s s_armed = { 0 };
    struct actuator_outputs_s s_output = { 0 };

    while (1) 
    {
        usleep(10 * 1000);
        bool updated = false;

        curr_time = hrt_absolute_time();
        theta_t = (float) (curr_time - last_time) / (float) (1000.0 * 1000.0);
        last_time = curr_time;

        orb_check(output_sub, &updated);
        if (updated) 
        {
            orb_copy(ORB_ID(actuator_outputs), output_sub, &s_output);
        }
        orb_check(armed_sub, &updated);
        if (updated) 
        {
            orb_copy(ORB_ID(actuator_armed), armed_sub, &s_armed);
        }

        //得到控制量输出
        output.v[0] = s_output.output[0];
        output.v[1] = s_output.output[1];
        output.v[2] = s_output.output[2];
        output.v[3] = s_output.output[3];

        //半物理仿真程序
        hil_cal(theta_t);

        //如果是锁定状态则清零
        if (!s_armed.armed) 
        {
            hil_zero();
        }
        if (Pos_global.v[AT(2, 0, Pos_global.n)] <= 0)
        {
            hil_zero();
        }

        //发布uORB，包括姿态、本地位置、全局位置
        hil_pub_att();              //函数内部实现略
        hil_pub_local_pos();        //函数内部实现略
        hil_pub_global_pos();       //函数内部实现略
    }

    return 0;
}

void hil_cal(double theta_t)
{
    //归一化并计算转速
    omega.v[0] = (output.v[0] + 1.0) / 2.0;
    omega.v[1] = (output.v[1] + 1.0) / 2.0;
    omega.v[2] = (output.v[2] + 1.0) / 2.0;
    omega.v[3] = (output.v[3] + 1.0) / 2.0;

    //计算角加速度
    Angular_Acc_from_omega(omega.v, &AngularVel_body.v[0], &AngularVel_body.v[1], &AngularVel_body.v[2]);

    //得到角速度
    AngularVel_body.v[0] += AngularVel_body.v[0] * theta_t;
    AngularVel_body.v[1] += AngularVel_body.v[1] * theta_t;
    AngularVel_body.v[2] += AngularVel_body.v[2] * theta_t;

    //得到欧拉角
    Angular_body.v[0] = AngularVelFromQuat(AngularVel_body.v, theta_t, 0);
    Angular_body.v[1] = AngularVelFromQuat(AngularVel_body.v, theta_t, 1);
    Angular_body.v[2] = AngularVelFromQuat(AngularVel_body.v, theta_t, 2);

    //根据欧拉角得到变换矩阵
    struct quat q_value = { 0 };
    double angle[3];
    angle[0] = Angular_body.v[0];
    angle[1] = Angular_body.v[1];
    angle[2] = Angular_body.v[2];
    hil_angle2q(angle, &q_value);
    TransMatrix_R_Q_set_value(&R_trans_matrix, q_value.w, q_value.x, q_value.y, q_value.z);

    //得到机体受力并通过变换矩阵得到本地坐标系下的受力
    F_body_from_omega(omega.v, &F_body.v[0], &F_body.v[1], &F_body.v[2]);
    matrix_mult(&F_local, &R_trans_matrix, &F_body);
    F_local.v[2] -= M_KG * G_MS2;

    //得到本地坐标系下的加速度
    Acc_local.v[0] = F_local.v[0] / M_KG;
    Acc_local.v[1] = F_local.v[1] / M_KG;
    Acc_local.v[2] = F_local.v[2] / M_KG;

    //得到本坐标系下的速度
    Vel_local.v[0] += Acc_ local.v[0] * theta_t;
    Vel_local.v[1] += Acc_ local.v[1] * theta_t;
    Vel_local.v[2] += Acc_ local.v[2] * theta_t;

    //得到本地坐标系下的位置
    Pos_local.v[0] += Vel_local.v[0] * theta_t;
    Pos_local.v[1] += Vel_local.v[1] * theta_t;
    Pos_local.v[2] += Vel_local.v[2] * theta_t;
}