一天精通无人机第 50 讲 高级篇系列:趣味航线规划

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

一、三角形

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

file

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

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个航迹点通过外部控制系统发送给无人机,并通过半物理仿真可以看到无人机的飞行线路:

file

二、圆形

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

file

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

file

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

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所示:

file

三、三叶草

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

file

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

file

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

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++;
}

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

file

四、阿基米德螺旋线

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

file

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

file

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

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++;
}

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

file

五、综合轨迹

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

file

下期预告:《高级篇完结》

file