三维导航向量场功能 Demo

本教程介绍三维导航向量场轨迹的构成方式、参数配置、机载程序启动流程及实机飞行步骤。

1. 轨迹原理

三维向量场跟踪路径由圆柱面与斜平面的交线构成：

圆柱面由 Cylinder 类定义，其初始化函数如下：

Cylinder(
    center=[0, 0, 0],
    r=50.,
    ori=2,
    k=[0.1, 0.1, 0.1]
)

斜平面由 InclinePlane 类定义，其初始化函数如下：

InclinePlane(
    normal_vector=[1., 1., 5.],
    point=[0., 0., 100.],
    k=[0.1, 0.1, 0.1]
)

在 gvf_inclined_circle.py 文件中，GVF 类的 etTangentV(self, pos) 函数通过切向量的符号设置飞行方向。以下方向均以地面站俯视视角为准：

切向量设置	飞行方向
`self.vt = np.cross(jac_1, jac_2)`	顺时针
`self.vt = -np.cross(jac_1, jac_2)`	逆时针

使用 VS Code 通过 SSH 远程连接机载电脑，然后进入程序目录：

cd /home/program/swiftwing

打开第一个终端，运行以下命令启动 MAVROS 连接：

roslaunch mavros swing.launch

保持 MAVROS 终端运行，再打开一个终端，启动三维轨迹跟踪控制节点：

roslaunch single_demo plane_inclinecircle_track.launch

启动节点后，需要等待程序获取无人机的飞行模式。只有终端中显示 POSCTL，才表示通信链路已经建立，可以继续准备切换至 Offboard 模式。

如果终端尚未显示无人机模式，请继续等待，不要立即切换至 Offboard 模式。

尚未获取无人机飞行模式

终端尚未获取无人机飞行模式。

终端显示 POSCTL 后，表示程序已经能够获取无人机的飞行模式。此时应先调整好无人机的位置和朝向，再准备切换至 Offboard 模式。

终端显示 POSCTL 飞行模式

终端显示 POSCTL，通信链路已建立。

切换至 Offboard 模式后，飞行器开始响应机载程序指令，并执行三维斜圆轨迹跟踪控制。

三维斜圆轨迹控制程序运行状态

飞行器进入 Offboard 模式并开始响应机载控制程序。