lighthouse定位系统入门教程-单机螺旋上升直线下降

说明

• 介绍如何实现单台无人机的单机螺旋上升直线下降

• 本文基于无人机多机套件主机测试

• Crazyflie无人机集群套件，采购地址

仿真飞行：

• 生成螺旋上升轨迹的点坐标的脚本

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

# 参数设置
height = 1  # 螺旋上升的总高度
num_points = 45  # 坐标点数量
radius = 0.3  # 螺旋半径的一半，宽度为0.5米
turns = 2  # 螺旋圈数

# 生成三维坐标点
z = np.linspace(0, height, num_points)
theta = np.linspace(0, 2 * np.pi * turns, num_points)
x = radius * np.cos(theta) + 0.3
y = radius * np.sin(theta)

# 将坐标点写入文件
with open('spiral_coordinates.csv', 'w') as f:
    for xi, yi, zi in zip(x, y, z):
        f.write(f'{xi},{yi},{zi}\n')

# 绘制螺旋图像
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(x, y, z)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('3D Spiral')

plt.show()

• 生成轨迹，参考

• 进入脚本目录

roscd crazyswarm/scripts/

• 脚本代码

#!/usr/bin/env python

import numpy as np

from pycrazyswarm import *
import uav_trajectory

if __name__ == "__main__":
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    allcfs = swarm.allcfs

    traj1 = uav_trajectory.Trajectory()
    traj1.loadcsv("spiral_coordinates.csv")

    TRIALS = 1
    TIMESCALE = 1.0
    for i in range(TRIALS):
        for cf in allcfs.crazyflies:
            cf.uploadTrajectory(0, 0, traj1)

        allcfs.takeoff(targetHeight=0.1, duration=2.0)
        timeHelper.sleep(2.5)
        for cf in allcfs.crazyflies:
            pos = np.array(cf.initialPosition) + np.array([0.2, 0, 0.1])
            cf.goTo(pos, 0, 2.0)
        timeHelper.sleep(2.5)

        allcfs.startTrajectory(0, timescale=TIMESCALE)
        timeHelper.sleep(traj1.duration * TIMESCALE + 2.0)

        allcfs.land(targetHeight=0.06, duration=10.0)
        timeHelper.sleep(10.0)

• 运行脚本

python3 spiral_coordinates.py --sim

• 启动后，无人机起飞0.1米，螺旋上升1米，然后直线下降

真机飞行

• 1号无人机放置在基站测试区域中心

• 通过choose.py配置无人机，打开choose.py，勾选使用1号无人机

python3 chooser.py 

• 终端下启动launch文件

roslaunch crazyswarm hover_swarm.launch

• 启动后，会弹出rviz，并显示对应的位置

• 新终端，启动脚本

python3 spiral_coordinates.py