ROS固定翼仿真|反正弦向量场全流程部署与实践

前面我们系统学习了反正弦向量场制导算法的理论建模与核心设计原理，理论落地工程是算法验证的核心环节，本期文章我们聚焦算法工程化落地，基于迅翼开源仿真平台，手把手教大家将反正弦向量场制导算法完整部署到PX4+Gazebo固定翼无人机仿真平台，从算法编写、脚本调用到仿真运行实现全流程打通，步骤清晰、逻辑连贯，新手也能一键复刻跑通全部流程。

一、基础准备（必看！环境搭对少踩坑）

算法落地的前提是完成基础仿真环境的配置，所有操作均基于Ubuntu系统完成，使用软件仿真即可实现全部验证。

首先准备一台搭载Ubuntu20.04系统的电脑，在系统中完成ROS环境、QGC地面站的安装，同时下载并编译PX4开源飞控源码，环境配置的详细步骤可直接参考迅翼官方使用文档。环境配置完成后，通过Git工具将开源项目仓库(https://gitee.com/swiftwing007/swiftwing-simulation)拉取到本地文件夹中。然后新建ROS工作空间命名为swiftwing，将拉取的git文件包复制到工作空间目录下，重命名为src文件夹覆盖掉原src文件夹。然后重新编译当下ROS工作空间会自动生成对应的功能包。

环境就绪后，使用VSCode软件打开swiftwing项目文件夹，在项目路径 /src/single_demo/scripts/gvf_lib 下新建Python文件，在该文件中编写反正弦向量场的算法核心代码，完成算法的封装实现。

开发注意：编写算法代码时，需保证算法文件中封装好对应的类与调用接口，且算法核心输出为二维单位化引导向量。

二、核心调用逻辑（算法+PX4交互流程）

本次算法落地的核心是实现算法解算与无人机飞控的闭环交互，无需关注PX4底层的姿态、油门等底层控制逻辑，仅需通过上层速度指令完成制导，整个交互流程简洁清晰，核心闭环逻辑可总结为「算法输出方向 → 生成速度指令 → PX4执行 → 反馈位置 → 算法再解算」，具体流程如下：

1. PX4 通过 ROS发布无人机本地位置 [x,y,z]；
2. 反正弦向量场制导算法读取平面位置 [x,y]，计算出最优方向的二维单位化引导向量；
3. 乘以巡航速度生成水平速度指令，叠加高度控制生成垂向指令，拼接为三维速度指令 [vx, vy, vz]；
4. 通过 control_send 接口下发给 PX4 飞控（Offboard 模式）；
5. 循环执行，直到无人机收敛到目标路径。

三、实操步骤：脚本调用+仿真运行

1. 工程依赖与路径配置

完成算法编写后，在项目路径 /src/single_demo/scripts 下新建Python运行脚本文件，该脚本是连接算法与无人机PX4飞控的核心载体，也是本次落地的核心文件。
运行脚本的正常执行需要使用到以下两个模块：

• gvf_lib/arcsin_vf.py：反正弦向量场算法实现（提供 ArcsinVF 类）
• uav_lib/spawn_uav.py：固定翼无人机封装接口（提供 uav 类）
  Python默认仅检索当前目录下的模块，无法直接跨目录导入自定义库，因此我们通过__file__内置变量自动定位工程根目录，将算法库所在路径手动添加至Python系统搜索路径sys.path中，确保脚本可以正常导入依赖模块，配置代码如下：

dir_mytest = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
dir_gvflib = dir_mytest + "/scripts/gvf_lib"
sys.path.insert(0, dir_gvflib)

完成路径配置后，即可在脚本中导入ROS、数值计算等基础依赖，同时导入上述两个核心自定义模块，完整的依赖导入代码如下：

#!/usr/bin/env python3
import rospy
import numpy as np
import sys
import os

from gvf_lib.arcsin_vf import ArcsinVF 
from uav_lib.spawn_uav import uav

---

2. 算法应用与核心脚本编写

2.1 初始化配置与类实例化

完成所有依赖导入后，首先进行核心参数配置与类的实例化操作，这是算法调用与无人机控制的基础步骤，所有配置参数均可根据仿真需求灵活修改，无需额外调整其他代码逻辑，配置代码如下：

# 固定翼无人机巡航速度配置
cruise_speed = 15
# 初始化反正弦向量场算法类，指定跟踪路径类型 circle-圆形路径 line-直线路径 curve-曲线路径
arcsin = ArcsinVF("circle")

# 初始化无人机实例，指定机型为固定翼plane，单机仿真编号为0
uav_type = "plane"
uav_index = 0
uav_handle = uav(uav_type, uav_index)

2.2 核心闭环控制逻辑编写

基于ROS的持续循环控制，是实现算法与PX4飞控交互的核心，该部分代码完整实现了无人机位置获取→算法解算→速度指令生成→指令下发的全流程闭环，可直接复用至其他制导算法的落地场景，代码如下：

# 持续运行，直至关闭ROS节点
while not rospy.is_shutdown():
    # 1. 获取PX4发布的无人机本地三维位置 [x,y,z]
    pose = np.copy(uav_handle.local_pose)
    # 2. 传入平面位置[x,y]，调用算法解算二维单位引导向量
    unit_vector = arcsin.calVector(pose[0: 2])
    # 3. 水平速度指令生成：巡航速度*单位方向向量
    vector_sp = cruise_speed * unit_vector
    # 4. 垂直速度指令生成：比例高度控制，这里设置目标高度50m
    v_h = (50. - pose[2]) 
    # 5. 拼接三维速度指令 [vx, vy, vz]
    vector_sp = np.append(vector_sp, v_h)

    # 6. 向PX4飞控下发速度控制指令
    control_type = "vel"
    uav_handle.control_send(vector_sp, control_type)

    # 控制频率休眠，固定100Hz解算频率，匹配PX4飞控周期
    rospy.sleep(0.01)

脚本注意：

1. 反正弦向量场为二维平面制导算法，仅对无人机的平面位置[x,y]进行解算，不介入高度控制环节；
2. 算法输出为单位向量，仅表征方向，速度大小由巡航速度独立控制，算法特性不受速度影响；
3. 向固定翼无人机发送控制指令时，应相应传入控制类型vel以执行速度控制模式；
4. 高度控制为比例控制，可自行修改期望飞行高度。

2.3 完整可运行脚本整合

将上述路径配置、依赖导入、初始化配置与核心控制逻辑整合，即可得到完整的可运行脚本，复制到对应路径即可直接运行，完整代码如下：

#!/usr/bin/env python3
import rospy
import numpy as np
import sys
import os

# 跨目录路径配置，确保可导入自定义算法库
dir_mytest = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
dir_gvflib = dir_mytest + "/scripts/gvf_lib"
sys.path.insert(0, dir_gvflib)

# 导入算法类与无人机交互类
from gvf_lib.arcsin_vf import ArcsinVF 
from uav_lib.spawn_uav import uav

# 核心参数配置与实例化
cruise_speed = 15
arcsin = ArcsinVF("circle")
uav_handle = uav("plane", 0)

# 初始化ROS控制节点，节点名全局唯一，不可重复
rospy.init_node("arcsin_track")

# 主控制循环，实现持续闭环控制
while not rospy.is_shutdown():
    pose = np.copy(uav_handle.local_pose)
    unit_vector = arcsin.calVector(pose[0: 2])
    vector_sp = cruise_speed * unit_vector
    v_h = (50. - pose[2]) 
    vector_sp = np.append(vector_sp, v_h)
    uav_handle.control_send(vector_sp, "vel")
    rospy.sleep(0.01)

2.4关键调用接口说明

为方便理解与二次开发，这里整理脚本中所有核心调用接口的功能与传参规范，所有接口均为封装好的上层接口，无需关注底层实现细节，按需调用即可：

接口名称	功能	传参 / 输出
ArcsinVF("circle")	初始化算法，指定路径类型	可传入路径类型：“circle”/“line”/”curve“
arcsin.calVector(xy)	计算二维单位引导向量	传入平面位置 [x,y]，输出引导向量
uav_handle.local_pose	获取无人机位置	输出[x,y,z]
control_send(cmd, "vel")	发送无人机控制指令	输入三维速度指令

3. ROS节点配置与仿真完整启动

3.1 launch启动文件编写

为简化仿真启动流程，我们通过编写ROS的launch文件实现一键启动所有节点。在项目路径 /src/single_demo/launch 下新建.launch格式文件，在文件中编写如下内容，配置需要启动的无人机节点与算法节点：

<launch>
	<group>	
		<node name="single_plane" pkg="uav_controller" type="vector_fw.py" output="screen"/>
		<node name="cmd_node" pkg="single_demo" type="arcsin_vf_track.py" output="screen"/>
	</group>
</launch>

3.2 仿真环境与算法节点启动

所有配置完成后，通过如下两步即可完成全部仿真启动，按照以下顺序执行，避免启动失败：

1. 打开Ubuntu终端，输入指令 roslaunch px4 single_plane.launch ，指令执行后将自动拉起Gazebo仿真界面与PX4飞控后台，此时打开QGC地面站，软件将自动与仿真飞控建立连接，完成基础仿真环境的启动；

2. 新建一个Ubuntu终端，输入指令 roslaunch single_demo plane_arcsin_track.launch ，指令执行后将自动调用无人机控制节点与编写的反正弦向量场算法节点，算法开始实时解算并下发控制指令。

3.3 仿真效果验证

指令全部执行完成后，即可在QGC地面站中看到固定翼无人机按照算法规划的路径稳定飞行，本次以圆形路径跟踪为例，无人机将先自动爬升至50m目标高度，随后沿预设圆形轨迹平滑飞行，无剧烈转弯和轨迹漂移，跟踪效果如下：

总结

本次基于迅翼开源仿真平台，完成了反正弦向量场制导算法从理论原理到工程落地的完整闭环，依托PX4+Gazebo仿真平台，通过简洁的Python脚本实现算法的快速调用与无人机的控制，整个开发流程无需关注复杂的底层通信与飞控逻辑，所有核心代码均围绕算法本身展开，降低算法部署在PX4的部署门槛。

该调用方式不仅适用于反正弦向量场算法，也可迁移至其他制导算法的仿真验证，仅需替换算法类的调用接口，即可快速完成新算法的落地测试，大幅提升算法开发与验证的效率。

如有需要领取源码文件，请在后台私信领取，即可自行尝试部署至仿真平台。

END

后续会陆续带来规划、控制相关的文章、仿真分享。