【踩坑记录-1】Ubuntu22.04+PX4+MAVSDK+Gazebo+QGC固定翼编队仿真遇到的问题

原创已于 2025-11-06 21:30:36 修改 · 1.9k 阅读

38 ·

CC 4.0 BY-SA版权

文章标签：

#python #无人机

于 2025-08-04 12:01:49 首次发布

心路历程。。。

1. Windows环境wsl2配置Ubuntu-22.04的PX4开发环境

Windows环境wsl2配置Ubuntu-22.04的PX4开发环境_wsl2 px4-优快云博客

注：

1）用这个安装wsl2：wsl --install Ubuntu-22.04 --web-download，不建议挂梯子

2. ROS的最简单安装——鱼香一键安装

ROS的最简单安装——鱼香一键安装_鱼香ros-优快云博客

注：

1）用这个换源，不用装ROS

3. ROS2+PX4 无人机编队仿真（一）环境开发教程

ROS2+PX4 无人机编队仿真（一）环境开发教程 - 知乎

注：
1）用这个装PX4，跳过ROS的步骤

2）jmavsim安装会报错，没有解决，但是不影响后续使用

4. 搭建一个简单的6机编队仿真

4.1 开启环境

用tmux打开gazebo，qgc，还有6个px4仿真节点。

为什么用tmux：因为【px4仿真节点不能在后台挂起！！！】不然会像没有阻塞了一样，会把cpu资源吃满（具体原因不知道，请大佬们补充），所以不用tmux会打开一堆终端，看着很烦。

用第三章装好的px4+gazebo环境，启动simgz。simgz这个脚本的默认名字应该是simulation-gazebo，我嫌长就改了。

因为我不需要开启gz仿真界面，所以我使用了无头模式，在simgz文件中第97行附近：

cmd += f'gz sim -s -r {args.model_store}/worlds/{args.world}.sdf'

我直接加了个-s，这样不管怎么样都会以无头模式启动gz仿真。如果想要看到仿真模型的话，把-s去掉即可。

wsl2打开gz仿真环境比较吃cpu资源（注意不是GPU），使用u9 275HX仿真6个节点就要占50%以上；无头模式6机节点只占用20%左右，之后可以做到20+节点以上的仿真规模。此外我一直不知道怎么让wsl2调用独显渲染软件，现在用的是opengl渲染。目前wslg和ubuntu22.04+gz版本存在不对应的问题，需要修改gz引擎，但是修改后还是无效。影响不大，不研究了。

gz也可以做到多个节点有头+多个节点无头的仿真。希望有头正常用下面的代码，无头的就在build前加 HEADLESS=1 （还未测试，测试后补充该部分）。这么做是为了让部分无人机搭载载荷，可以做多机编队+强化学习+目标识别跟踪功能的接入。

bash里的显式参数详见Gazebo 仿真 | PX4 Guide (v1.15)，注意我make的是px4_sitl_default，不是px_sitl，只是名字不一样而已，重新make一下就行

#!/bin/bash

tmux new-session -d -s formation 

tmux new-window -t formation -n Gazebo "python3 simgz"
tmux new-window -t formation -n QGC "./QGroundControl-x86_64.AppImage"   

for i in $(seq 0 5); do

    # 启动PX4实例
    pose_y=0
    pose_x=$((i * 50))
    pose="$pose_x,$pose_y"
    cmd="cd ~/PX4-Autopilot && \
    PX4_GZ_STANDALONE=1 \
    PX4_GZ_MODEL_POSE="$pose" \
    PX4_SYS_AUTOSTART=4003 \
    PX4_SIM_MODEL=gz_rc_cessna \
    ./build/px4_sitl_default/bin/px4 -i $i "

    tmux new-window -t formation -n PX4_$i "$cmd" 

done

tmux attach -t formation

Gazebo 仿真 | PX4 Guide (v1.15) 还提到了更轻量化的SIH仿真，比gz无头还要少占点资源。我正在尝试用gz+SIH仿真，成功了再补充。

我还在.simulation-gazebo\worlds\default.sdf里面40行修改了默认世界的地板大小，防止无人机降落的时候因为没有地板砸到地心里面去

<size>5000 5000</size>

第89行修改了默认经纬坐标，要和下面编队代码里的对应，无人机会以默认经纬坐标作为初始的home坐标。

      <latitude_deg>39.91401</latitude_deg>
      <longitude_deg> 116.39715</longitude_deg>

4.2 开启仿真

下面就比较简单了

MAVSDK库坑比较多，需要多试错。QGC里面可以临时调无人机飞参，不需要用MAVSDK改

#!/usr/bin/env python3
"""
formationMain.py
"""

import asyncio, math, random
from mavsdk import System
from dronesConfig import UAV_CFGS, connect_uav, set_speed
from dronesBehavior import rtl_all, land_all, takeoff_all
from dronesBehavior import leader_task, follower_task 

HOME_LAT = 39.91401032678762
HOME_LON = 116.39717200165842

async def keyboard_listen(tasks, drones:System, home_lat, home_lon):
    """
    监听键盘输入，处理命令
    """
    while True:
        cmd = await asyncio.to_thread(input, ">> ")
        cmd = cmd.strip().lower()

        if cmd == "takeoff":
            for t in tasks: t.cancel()
            await asyncio.gather(*tasks, return_exceptions=True)
            await takeoff_all(drones)

        if cmd == "rtl":
            for t in tasks: t.cancel()
            await asyncio.gather(*tasks, return_exceptions=True)
            await rtl_all(drones)

        if cmd == "land":
            for t in tasks: t.cancel()
            await asyncio.gather(*tasks, return_exceptions=True)
            await land_all(drones)

        if cmd == "formation-test-1":
            for t in tasks: t.cancel()
            await asyncio.gather(*tasks, return_exceptions=True)
            dir_ref = {"hdg": 0.0}

            tasks = [asyncio.create_task(leader_task(drones[0], home_lat, home_lon, dir_ref))]
    
            for i in range(1, len(drones)):
                tasks.append(asyncio.create_task(follower_task(drones[i], drones[0], dir_ref, i)))

async def task_init():
    print('ready to start !')
    

async def main():
    """
    主协程，连接所有无人机，起飞，启动领机跟随任务(测试)
    """
    # 连接所有无人机
    drones_tuple = await asyncio.gather(*(connect_uav(c) for c in UAV_CFGS))
    drones = list(drones_tuple)                # 序保持与 UAV_CFGS 对应
    #  print(drones)
    lead = drones[0]                           # 约定首元素为领机

    home_lat, home_lon = HOME_LAT, HOME_LON

    # 创建空的初始化task
    tasks = asyncio.create_task(task_init())

    await keyboard_listen(tasks, drones, home_lat, home_lon)

if __name__ == "__main__":
    asyncio.run(main())

#!/usr/bin/env python3
"""
dronesBehavior.py
"""

import asyncio
import math
import random
from mavsdk import System

from dronesConfig import calculate_new_coordinates, calculate_positions_distance, calculate_relative_distance
from dronesConfig import set_speed

# ========== 飞行参数 ==========
ALT_TKOF    = 50.0          # 默认巡航高度
LEG_DIST    = 1000
TRAIL_DIST  = 200
HOME_R      = 200
REACH_R     = 200
CHECK_INT   = 1.0
SQRT3_2     = math.sqrt(3) / 2


# ========== 飞行逻辑（新版） ==========

# ─── 领机 ────────────────────────────────────────────────
async def leader_task(lead:System, start_lat, start_lon, dir_ref):
    """
    起飞后首先向正北飞行一个航段（LEG_DIST），
    之后每段在当前航向上随机偏转 -60°…+60° 并继续前行。
    dir_ref["hdg"] 实时存储领机航向（真方位角，0°=北，顺时针为正）。
    """
    cur_lat, cur_lon = start_lat, start_lon
    heading          = 0.0                      # 初始航向北
    print("Leader task started")
    while True:
        # 计算下一目标点
        rad   =             math.radians(heading)
        n_off =  LEG_DIST * math.cos(rad)
        e_off =  LEG_DIST * math.sin(rad)
        tgt_lat, tgt_lon = calculate_new_coordinates(cur_lat, cur_lon, n_off, e_off)

        # 飞向目标
        await lead.action.goto_location(tgt_lat, tgt_lon, ALT_TKOF, heading)


        lead_true_heading = await lead.telemetry.heading().__anext__()

        dir_ref["hdg"] = round(lead_true_heading.heading_deg)                          # 向僚机广播当前航向
        print(f"Leader target: {tgt_lat:.6f}, {tgt_lon:.6f} (heading={heading:.1f}°), true heading={lead_true_heading}°)")
        # 等待到达
        while True:

            lead_true_heading = await lead.telemetry.heading().__anext__()
            pos               = await lead.telemetry.position().__anext__()
            dir_ref["hdg"] = round(lead_true_heading.heading_deg)                     # 更新航向

            if calculate_positions_distance(pos.latitude_deg, pos.longitude_deg, tgt_lat, tgt_lon) <= REACH_R:
                break
            await asyncio.sleep(0.5)

        await asyncio.sleep(0.5)                # 稳定片刻
        cur_lat, cur_lon = tgt_lat, tgt_lon     # 更新当前位置
        heading = (heading + random.uniform(-90.0, 90.0)) % 360.0


# ─── 僚机 ────────────────────────────────────────────────
_OFFSETS_BODY = {                                # 领机自坐标系下的编队位置
    1: (-TRAIL_DIST*SQRT3_2, -TRAIL_DIST*0.5),   # 左边中点
    2: (-TRAIL_DIST*SQRT3_2,  TRAIL_DIST*0.5),   # 右边中点
    3: (-TRAIL_DIST*2*SQRT3_2, 0.0),             # 底边中点
    4: (-TRAIL_DIST*2*SQRT3_2, -TRAIL_DIST),     # 左顶点
    5: (-TRAIL_DIST*2*SQRT3_2,  TRAIL_DIST)      # 右顶点
}

async def follower_task(fol:System, lead:System, dir_ref, i):
    """
    僚机保持与领机的等边三角形编队。
    所有计算均随领机实时航向旋转。
    """
    dx_body, dy_body = _OFFSETS_BODY.get(i, (0.0, 0.0))
    temp_delta = 300.0                         # “无穷远”延伸量，用于保证 goto 的航向一致
    print(f"follower_task[{i+1}] started")
    while True:
        # 读取领机状态
        pos_lead          = await lead.telemetry.position().__anext__()
        lead_true_heading = await lead.telemetry.heading().__anext__()
        pos_true_heading  = await fol.telemetry.heading().__anext__()
        dir_ref["hdg"]    = round(lead_true_heading.heading_deg)
        hdg               = dir_ref["hdg"]
        rad               = math.radians(hdg)

        if i == 1:
            print(f"drone[1] heading: {round(lead_true_heading.heading_deg)}°,\
                  drone[2] heading: {round(pos_true_heading.heading_deg)}°,\
                  delta: {round(pos_true_heading.heading_deg - lead_true_heading.heading_deg, 1)}° ")
        
        # 将机体坐标系偏移量旋转到地理坐标系 (北、东)
        n_off =  dx_body * math.cos(rad) - dy_body * math.sin(rad)
        e_off =  dx_body * math.sin(rad) + dy_body * math.cos(rad)

        # 期望位置及沿航向的“远点”坐标
        tgt_lat, tgt_lon = calculate_new_coordinates(pos_lead.latitude_deg, pos_lead.longitude_deg, n_off, e_off)
        n_inf = n_off + (TRAIL_DIST + temp_delta) * math.cos(rad)
        e_inf = e_off + (TRAIL_DIST + temp_delta) * math.sin(rad)
        tgt_lat_inf, tgt_lon_inf = calculate_new_coordinates(pos_lead.latitude_deg, pos_lead.longitude_deg, n_inf, e_inf)
        
        # 导航至“远点”，PX4 自动插值
        await fol.action.goto_location(tgt_lat_inf, tgt_lon_inf, ALT_TKOF, hdg)
       
        # 依据沿航向误差调整空速
        pos_fol = await fol.telemetry.position().__anext__()
        dn, de  = calculate_relative_distance(pos_fol.latitude_deg, pos_fol.longitude_deg, tgt_lat, tgt_lon)
        along_err = dn * math.cos(rad) + de * math.sin(rad)   # 正值表示超前

        tgt_spd = 20.0 - along_err / 20.0                    # 线性调节
        tgt_spd = max(10.0, min(40.0, tgt_spd))
        await fol.param.set_param_float("FW_AIRSPD_TRIM", round(tgt_spd, 1))

        await asyncio.sleep(CHECK_INT)



# 起飞
async def takeoff_all(drone:System):
    for i, d in enumerate(drone):
        await d.action.arm()
        await asyncio.sleep(1)                 # 等待解锁完成
        await d.action.takeoff()
        await asyncio.sleep(1)                 # 等待起飞完成
        print(f"drone[{i+1}] armed and taking off...")
    await asyncio.sleep(5)

    for i, d in enumerate(drone):
        await set_speed(d)                    # 设置所有无人机速度参数
        smax = await d.param.get_param_float("FW_AIRSPD_MAX",) 
        smin = await d.param.get_param_float("FW_AIRSPD_MIN")
        scur = await d.param.get_param_float("FW_AIRSPD_TRIM")
        print(f"drone[{i+1}] speed set: max={smax}, min={smin}, trim={scur}")
    await asyncio.sleep(1)


# 返回起始点
async def rtl_all(drones:System):
    """
    所有无人机返回起始点
    """
    await asyncio.gather(*(d.param.set_param_float("FW_AIRSPD_TRIM", 40) for d in drones))
    await asyncio.gather(*(d.action.return_to_launch() for d in drones))

# 降落
async def land_all(drones:System):
    """
    所有无人机降落
    """
    await asyncio.gather(*(d.action.land() for d in drones))

if __name__ == "__main__":
    
    async def main():
        drone = System()
        await drone.connect(system_address="udp://:14540")
        await drone.action.arm()
        await drone.action.takeoff()    
        await drone.action.return_to_launch
        test = await drone.telemetry.heading
        print(test)
    
    async def test():
        drone = System()
    
    asyncio.run(main())

#!/usr/bin/env python3
"""
dronesConfig.py
"""

import asyncio, math, random
from mavsdk import System
from geopy import Point, distance
from geopy.distance import geodesic, great_circle
from math import radians, cos


# ========== 机队配置==========
UAV_CFGS = [
    {"tag": "LEAD",   "ctl": 14540, "grpc": 50051},   # 领机
    {"tag": "DRONE1", "ctl": 14541, "grpc": 50052},   # 僚机 1
    {"tag": "DRONE2", "ctl": 14542, "grpc": 50053},   # 僚机 2
    {"tag": "DRONE3", "ctl": 14543, "grpc": 50054},   # 僚机 3
    {"tag": "DRONE4", "ctl": 14544, "grpc": 50055},   # 僚机 4
    {"tag": "DRONE5", "ctl": 14545, "grpc": 50056},   # 僚机 5
    # …继续追加 {"tag": "DRONE3", ...}
] # 要和仿真启动的数量一致

async def set_speed(drone):
    """设置无人机速度/加速度等，参数不对的话在这里调"""
    param = drone.param
    await param.set_param_float("FW_AIRSPD_MAX", 50.0)      # 空速
    await param.set_param_float("FW_AIRSPD_TRIM", 20.0)     # 巡航基准
    await param.set_param_float("FW_AIRSPD_MIN", 10.0)      # 最小空速，同步安全下限(默认7会STALL)
    await param.set_param_float("FW_THR_MAX", 1.0)          # 推力最大值，若推力不足，可放到 1 (=100 %)
    await param.set_param_float("RTL_RETURN_ALT", 50)       # 返航高度
    # await param.set_param_float("TECS_SPDWEIGHT", 0.8)    # 0.0=高度绝对优先，2.0=速度绝对优先
    # await param.set_param_float("TECS_TIME_CONST", 3.0)   # TECS更灵敏，默认 5s，太低会震荡

# ========== 通讯连接 ==========
async def connect_uav(cfg):
    drone = System(port=cfg["grpc"])
    await drone.connect(system_address=f"udp://:{cfg['ctl']}")
    async for h in drone.telemetry.health():
        if h.is_global_position_ok and h.is_home_position_ok:
            break
    return drone


# ========== 经纬换算 ==========

# 将距离转换为纬度lat和经度lon（替换原函数offset）
def calculate_new_coordinates(lat, lon, north_m, east_m):
    """
    支持正北/正南、正东/正西四象限位移：
    - north_m  >0 向北，<0 向南
    - east_m   >0 向东，<0 向西
    """
    origin = Point(lat, lon)

    # 先沿南北方向
    if north_m:
        bearing_ns = 0 if north_m > 0 else 180
        origin = geodesic(meters=abs(north_m)).destination(origin, bearing_ns)

    # 再沿东西方向
    if east_m:
        bearing_ew = 90 if east_m > 0 else 270
        origin = geodesic(meters=abs(east_m)).destination(origin, bearing_ew)

    return round(origin.latitude, 7), round(origin.longitude, 7)

# 计算两个纬度lat和经度lon距离（替换原函数dist_m）
def calculate_positions_distance(x1, y1, x2, y2):
    return distance.distance((x1, y1), (x2, y2)).m

# 计算目标点相对于基准点的东西南北距离
def calculate_relative_distance(lat, lon, ref_lat, ref_lon, earth_radius=6378137.0):
    """
    计算坐标点 (lat, lon) 相对于参考点 (ref_lat, ref_lon) 的
    向北距离 north 和向东距离 east。

    参数
    ----
    lat, lon        : 目标点的纬度、经度 (度)
    ref_lat, ref_lon: 参考点的纬度、经度 (度)
    earth_radius    : 地球半径 (米)，默认值为 6378137.0 米
    返回
    ----
    north : float   向北位移，单位 m；北为正，南为负
    east  : float   向东位移，单位 m；东为正，西为负
    """
    # 经纬度差换算成弧度
    d_lat = radians(lat - ref_lat)
    d_lon = radians(lon - ref_lon)

    # 参考纬度（或两点纬度平均）转弧度，用于计算东西向缩放
    mean_lat = radians((lat + ref_lat) / 2.0)

    # 向北、向东位移
    north = d_lat * earth_radius
    east  = d_lon * earth_radius * cos(mean_lat)

    return north, east

if __name__ == "__main__":

    # 基准点 (纬度, 经度)
    base_point = (40.7128, -74.0060)  # 纽约
    # 目标点 (纬度, 经度)
    target_point = (40.6130, -75.0058)  # 纽约附近某点

    n,e = calculate_relative_distance(*target_point, *base_point) # 前面减后面
    print(n,e)

4.3 开启测试

测试内容是，无人机起飞，然后主机朝着一个方向飞一段距离，随后随机转向。从机则会一直跟随主机，形成三角形编队。

输入./start_sih.sh开启仿真环境

qgc和px节点会在一个终端中叠放，tmux使用方法自行百度。

然后出现qgc界面，里面是6架无人机

再打开一个终端，输入python3 formationMain.py

出现 “ ready to start ! ”

输入takeoff，随后无人机会依次起飞

输入formation-test-1，无人机会形成三角形编队

输入rtl，无人机会飞回初始点附近盘旋

输入land，无人机会降落

不管C不C掉formationMain，都可以对无人机进行控制，飞一半感觉参数不好，想调参也ok，C掉代码后改完再启动就行。

但是如果要选择退出仿真，注意一定【要C掉所有的PX节点】，不然可能会有进程还在挂着的bug，导致下一轮仿真出问题。

4.4 资源占用

（Y7000P，U9 275HX + 5060）：

5.下一步工作

5.1 优化编队算法

目前仿真效果还比较差，固定翼形成编队的方法与四旋翼不同，还得做很多优化。

现在控编队的方法是，从机获取主机位置，解算自己应该去的位置（期望点），然后发一个期望点沿着主机航向往前伸几百米的一个点，去的方法是goto_location（注意该方法yaw参数无效，写了也无效），这是因为PX4里面定义无人机到达期望点附近L1距离（大概是70m，需要参阅px4官方文档1.1.5版本）时立即切换到下一任务，如果下一项是 LOITER，横向控制器会在距航点约L1距离+盘旋半径处启动“切圆”以切入半径 NAV_LOITER_RAD 的圆航线，也就是说，如果发的是期望点，无人机只会在期望点附近盘旋。即使你发了一个“期望点沿着主机航向往前伸几百米的一个点”，因为无人机要切入这个点的盘旋轨迹，在这之前就已经会偏离航线了，所以我们只能发一个既不会离得期望点太远，又不会让无人机切入盘旋的点。

为什么不能太远？我如果发了一个期望点沿着主机航向往前伸非常远的一个点会怎么样？：goto_location() 是通过 MAV_CMD_NAV_WAYPOINT 把“目标经纬高+期望航向”一次性交给飞控，飞控安装固定翼导航通用的 L1 直线-截获算法处理：飞控先让飞机以 L1 转向距离为半径做协调转弯截获至“当前点-目标点”这条假想的无限长直线，然后沿该线飞抵目标附近。其几何特征决定了实际轨迹包含一段圆弧加一段直线。

举例：假如我无人机在0，0位置，机头朝北，我希望它尽快进入x+10，机头朝北的航线。如果我给它发了一个10，1000的点，那无人机一定是斜着过去的。如果我发的近一些，无人机就可以被这些点牵引着逐渐拉正自己的位置。（有点饿了，思路还没捋顺，想到啥写啥了，之后再写的文邹邹点）所以发的这个goto点是有讲究的，还需要进一步研究。，以替代其默认的 L1 算法。

另外，follower_task里面用了速度控制，就是从机飞到期望点前面了，那就减速，在后面就加速，但是要注意的是，无人机实际goto的点并不是期望点！在一条线上还好说，如果无人机飞的比较偏，它的位置和速度用上面这种简单的约束条件是很难保证的，如下图所示，左下方的飞机永远无法达到期望点。

所以我在考虑用dubins去做约束，参考：

https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning/DubinsPath

思路如下，等我捋顺了再补充解释

为什么不用mision load的方法？用dubins算好轨迹了以后上传航点，用速度把无人机约束到期望点附近不好么？：因为要做其他算法接入，应该有一个库，给无人机定义期望点了以后，算法会想方设法把无人机“固定”到那个点，上传任务点不够灵活，而且这个算法未来要移植到某型无人机上，应该让算法适配性更好，剥离度要高。

为什么不用offboard？理由同上，我们并不是做px4，未来的硬件与px关联也不大，只是临时做个仿真找找手感

5.2 还要做什么

模拟数据链

群控地面站

6. 其他

Q：UAV_CFGS = [
{"tag": "LEAD", "ctl": 14540, "grpc": 50051}, ...]是什么

A：在 PX4 的多实例 gz SITL 中，每启动一次 px4 二进制都会按 -i 参数自动向上偏移一组端口与 MAV_SYS_ID。实例 0 对应 sysid 1、MAVLink 端口 14540；实例 1 对应 sysid 2、端口 14541。只要后续客户端显式连到各自的端口，它们在传输层即已隔离，互不阻塞。如果不定义端口，那么所有无人机的端口都会默认为14540，导致只有一架无人机被监听。gRPC 端口同理，顺序是从50051开始。也要给每个无人机按顺序分配一个gPRC号。

PX4 原生脚本里只为 0 – 9 号实例按公式 14540 + instance 生成开发者 API 端口；从第 10 架（instance ≥ 10）开始，脚本把所有实例都重定向到 14549，如果超过10架无人机，需要修改px4-rc.mavlink，把启动脚本改成线性递增。gPRC端口没有上限。

Q：UserWarning: Protobuf gencode version 5.29.0 is exactly one major version older than the runtime version 6.31.1 at action/action.proto. Please update the gencode to avoid compatibility violations in the next runtime release.

A：不影响使用，不想看到这行提醒建议降级proto解决：

pip uninstall protobuf
pip install protobuf==5.29.0