多旋翼无人机

          第二节 趣味航线规划

 

 

        有了外部控制系统，我们就可以在树莓派上编写很多有趣的程序来控制无人机的飞行了，在本节中我们将利用几个数学函数规划出几个无人机的飞行航线，并通过半物理仿真完成这些航线的飞行。

 

一、三角形
        为了使无人机能够按一个等边三角形进行飞行，只需要为其设定三角形的3个顶点坐标，作为无人机的目标点即可。在本地坐标系下我们设等边三角形的3个顶点坐标分别为（30,0）、（-15,-25.9）、（15,-25.9）如下图所示：

        于是我们可以编写程序为无人机设定其目标行点：

float alt = -10.0f;
float yaw = 0.0;
int secs = 2;
int i = 0;
waypoint_s waypoints[4];
        
waypoints[i].is_local_sp = true;
waypoints[i].x = 30.0;
waypoints[i].y = 0.0;
waypoints[i].z = alt;
waypoints[i].yaw = yaw;
waypoints[i].accept_opt = WP_ACCEPT_OPT_XY | WP_ACCEPT_OPT_Z | WP_ACCEPT_OPT_YAW;
waypoints[i].loiter_secs = secs;
i++;    
    
waypoints[i].is_local_sp = true;
waypoints[i].x = -15.0;
waypoints[i].y = -25.9;
waypoints[i].z = alt;
waypoints[i].yaw = yaw;
waypoints[i].accept_opt = WP_ACCEPT_OPT_XY | WP_ACCEPT_OPT_Z | WP_ACCEPT_OPT_YAW;
waypoints[i].loiter_secs = secs;
i++;
        
waypoints[i].is_local_sp = true;
waypoints[i].x = -15.0;
waypoints[i].y = 25.9;
waypoints[i].z = alt;
waypoints[i].yaw = yaw;
waypoints[i].accept_opt = WP_ACCEPT_OPT_XY | WP_ACCEPT_OPT_Z | WP_ACCEPT_OPT_YAW;
waypoints[i].loiter_secs = secs;
i++;

waypoints[i].is_local_sp = true;
waypoints[i].x = 30.0;
waypoints[i].y = 0.0;
waypoints[i].z = alt;
waypoints[i].yaw = yaw;
waypoints[i].accept_opt = WP_ACCEPT_OPT_XY | WP_ACCEPT_OPT_Z | WP_ACCEPT_OPT_YAW;
waypoints[i].loiter_secs = secs;
i++;

        将这4个航迹点通过外部控制系统发送给无人机，并通过半物理仿真可以看到无人机的飞行线路：

二、圆形
        为了使无人机的飞行航线是一个标准的正圆形，我们可以将目标位置写成关于角度的参数方程：

        其中x和y就是圆形航线上的坐标点，r为圆弧半径，θ为随时间增大的角度。半径为15米的圆形如下图所示：

        我们可以使用一个循环来完成无人机圆形航线的规划：

for (float angle = M_PI / 2.0; angle < M_PI * 2.0 + M_PI / 2.0; angle += 0.001)
{
    float x = r * sinf(angle) + r;
    float y = r * cosf(angle);

    waypoints[i].is_local_sp = true;
    waypoints[i].x = x;
    waypoints[i].y = y;
    waypoints[i].z = alt;
    waypoints[i].yaw = yaw;
    waypoints[i].accept_opt = 0;
    waypoints[i].loiter_secs = 0;
    i++;
}

        通过半物理仿真可以看到运行结果如16-5所示：

 

三、三叶草
        与圆形类似的，我们可以利用三叶草图形的极坐标方程和参数方程来计算航线：

        三叶草的图形如下图所示：

        我们可以使用一个循环来完成无人机三叶草航线的规划：

for (float angle = M_PI / 2.0; angle < M_PI + M_PI / 2.0; angle += 0.001)
{
    float p = r * cosf(3 * angle);
    float x = p * sinf(angle);
    float y = p * cosf(angle);
    
    waypoints[i].is_local_sp = true;
    waypoints[i].x = x;
    waypoints[i].y = y;
    waypoints[i].z = alt;
    waypoints[i].yaw = yaw; 
    waypoints[i].accept_opt = 0;
    waypoints[i].loiter_secs = 0;
    i++;
}

 

        通过半物理仿真可以看到运行结果如下图所示：

 

四、阿基米德螺旋线

        与三叶草类似的，阿基米德螺旋线的参数方程为：

        其中，a为角度为0时起点到原点的距离，b相邻曲线的距离。阿基米德螺旋线的图形如下图所示：

        我们可以使用一个循环来完成无人机阿基米德螺旋线的航线规划：

for (float angle = 0.0; angle < M_PI * 2.0 * circle + M_PI / 2.0; angle += step)
{
    float r = a + b * angle;
    float x = r * sinf(angle);
    float y = r * cosf(angle);
    
    waypoints[i].is_local_sp = true;
    waypoints[i].x = x;
    waypoints[i].y = y;
    waypoints[i].z = alt;
    waypoints[i].yaw = yaw; 
    waypoints[i].accept_opt = 0;
    waypoints[i].loiter_secs = 0;
    i++;
}

        通过半物理仿真可以看到运行结果如下图所示：

 

五、综合轨迹

        最后我们可以将上述的4调航线串联到一起，形成一系列集合图形所组成的趣味航线，并通过半物理仿真查看其运行结果，如下图所示：