树莓派基于Aruco检测的无人机跟随系统
版本信息
更新时间：2025.9.19
版本：V1.2
实验平台
本教程实验平台：幻思创新FanciSwarm® 树莓派Zero2W 无人机和个人PC的虚拟机Ubuntu20.04LTS环境，Ubuntu系统安装教程请点击查看。
项目概述
该项目主要实现了基于FFmpeg的视频流传输、OpenCV的实时视频显示、以及通过OpenCV的Aruco模块进行ArUco码检测和位姿估计。同时，项目中加入了多进程与线程管理，其中使用共享内存来处理并行任务。代码的主要功能包括：
- 从IP摄像头读取视频流并显示实时视频。
- 检测视频中的ArUco码，并估计其位置和姿态。
- 使用PID控制参数，根据Aruco码的位置信息生成控制指令，并发送至无人机飞控系统。
项目文件总览
camera_biaoding
├─ biaoding.py
└─ take_pic_for_biaoding.py
Aruco_detection
├─ aruco_detect.py
├─ datalink_serial.py
├─ mavcrc.py
├─ mavcrc.pyc
├─ mavlink.py
├─ mavlink.pyc
└─ __pycache__
├─ datalink_serial.cpython-38.pyc
├─ mavcrc.cpython-36.pyc
├─ mavcrc.cpython-38.pyc
├─ mavlink.cpython-36.pyc
└─ mavlink.cpython-38.pyc
serial_wifi_demo
├─ auto_ap.sh
├─ datalink_serial_wifi.py
└─ README.md
无人机串口配置
先按照官网教程学会连接无人机的手机APP
在APP中，把串口1波特率配置为460800
相机标定
相机标定是通过拍摄已知尺寸的棋盘格图像来获取相机的内参矩阵和畸变系数的过程。
以下是使用OpenCV对摄像头进行标定的步骤：
- 准备并打印棋盘格：
例如 9x6 的棋盘格 （直接下载附件或通过下载链接：https://github.com/opencv/opencv/blob/master/doc/pattern.png) - 拍摄棋盘格图像
使用摄像头从不同角度拍摄多个包含棋盘格的图像。确保棋盘格的所有角点都能在图像中清晰可见，大概拍20多张图片即可。 - 使用下面的OpenCV脚本进行相机标定：
文件在camera_biaoding中的biaoding.py
import cv2
import numpy as np
import glob
# 棋盘格规格（内角点的数量） {#棋盘格规格内角点的数量 }
chessboard_size = (9, 6)
# 准备对象点，例如(0,0,0), (1,0,0), (2,0,0) ..., (8,5,0) {#准备对象点例如000-100-200--850 }
objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)
# 存储所有的对象点和图像点 {#存储所有的对象点和图像点 }
objpoints = [] # 在真实世界坐标系中的3D点
imgpoints = [] # 在图像平面的2D点
# 获取棋盘格图片 {#获取棋盘格图片 }
images = glob.glob('calibration_images/*.jpg')
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# 找到棋盘格的角点
ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
# 如果找到足够的角点，添加对象点和图像点
if ret:
objpoints.append(objp)
imgpoints.append(corners)
# 显示角点
img = cv2.drawChessboardCorners(img, chessboard_size, corners, ret)
cv2.imshow('img', img)
cv2.waitKey(500)
cv2.destroyAllWindows()
# 标定摄像头 {#标定摄像头 }
ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
print("相机内参矩阵：\n", camera_matrix)
print("畸变系数：\n", dist_coeffs)
# 保存标定结果 {#保存标定结果 }
np.savez('calibration_data.npz', camera_matrix=camera_matrix, dist_coeffs=dist_coeffs)
- 使用标定结果：
标定完成后，你可以使用保存的 camera_matrix 和 dist_coeffs 进行图像校正或计算物体的3D位置，如在检测ArUco码时使用。 - 拍摄提示：
- 尽量从不同的角度、不同的距离拍摄棋盘格。
- 每张照片中的棋盘格应尽量占据较大的画面区域。
- 确保所有棋盘格点都清晰可见并没有因反光等原因模糊不清。
- 附加材料：
提供一个接收树莓派网络视频流时候的保存图片脚本，脚本在camera_biaoding文件包中的take_pic_for_biaoding.py,方便收集标定所用图片。
脚本启动后，按‘s’进行保存当前帧图片，按‘q’键退出。
import cv2
import os
import datetime
# 设置保存图片的文件夹 {#设置保存图片的文件夹 }
output_dir = "captured_images"
if not os.path.exists(output_dir):
os.makedirs(output_dir)
# 设置树莓派摄像头的视频流URL（替换为你的实际URL） {#设置树莓派摄像头的视频流url替换为你的实际url }
video_stream_url = "tcp://192.168.0.201:2024/stream" # 替换为实际的树莓派摄像头视频流URL
# 打开摄像头视频流 {#打开摄像头视频流 }
cap = cv2.VideoCapture(video_stream_url)
if not cap.isOpened():
print("无法打开视频流")
exit()
print("按下 's' 键保存当前帧，按 'q' 键退出程序")
while True:
# 捕获当前帧
ret, frame = cap.read()
if not ret:
print("无法读取视频帧")
break
# 显示实时视频流
cv2.imshow("Raspberry Pi Camera Stream", frame)
# 监听键盘输入
key = cv2.waitKey(1) & 0xFF
if key == ord('s'):
# 生成唯一的文件名并保存图片
timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
image_path = os.path.join(output_dir, f"image_{timestamp}.jpg")
cv2.imwrite(image_path, frame)
print(f"图片已保存: {image_path}")
elif key == ord('q'):
# 按下 'q' 键退出程序
print("退出程序")
break
# 释放摄像头资源并关闭窗口 {#释放摄像头资源并关闭窗口 }
cap.release()
cv2.destroyAllWindows()
Aruco码生成
提供一个在线网站：https://chev.me/arucogen/
需要根据Aruco字典和检测参数来生成不同规格的Aruco码，并需要在检测程序中对应修改。具体更改位置参考下文。
树莓派端配置
树莓派端作为中转，主要有如下功能：
- 将摄像头获取到的视频流传给PC端，用于二维码的检测。
- 获取PC端处理后的二维码位置信息。
- 将位置信息传给飞控，用于无人机的姿态和位置控制。
基于以上功能，需要对树莓派端做如下配置：
- 开机自启动
- 开机自启动配置
sudo gedit /etc/rc.local- 在“exit 0”上面加入
su pi -c "sh ~/auto_ap.sh"- 配置树莓派串口
- 配置IP地址
- 在datalink_serial_wifi.py第12行中配置：
HOST_IP = "192.168.0.201" # IP地址不固定，要考虑路由器可用IP范围- 在auto_ap.sh（自启动脚本，脚本文件在serial_wifi_demo中)：
更详细的树莓派配置说明在serial_wifi_demo的README文件中sudo ifconfig wlan0 192.168.0.201
PC端环境搭建
安装OpenCV-python依赖库，要注意下载的python包版本，不同版本的函数会有差别。
aruco-detection-new v0.1.0
└── opencv-contrib-python v4.6.0.66
└── numpy v1.24.4
pip install opencv-contrib-python==4.6.0.66
pip install numpy==1.24
pip install pynput
更改IP端口
datalink_serial.py第14行中
HOST_IP = "192.168.0.204" # IP地址不固定，要考虑路由器可用IP范围
aruco_detect.py中
ffmpeg_command = [
'ffmpeg',
'-i', 'tcp://192.168.0.204:2024', # IP摄像头地址
'-f', 'image2pipe',
'-pix_fmt', 'bgr24',
'-vcodec', 'rawvideo',
'-bufsize', '500k',
'-'
]
更改完IP之后就可以运行例程程序了
python3 aruco_detect.py
启动后可以看见两个窗口，一个为实时显示，另一个为Aruco码检测窗口，出现Aruco码时候会检测并打印其位置信息。
- 我们还提供了键盘解锁，起飞，降落，上锁功能
特别注意！下面操作可能触发飞机自动起飞，请先确保飞机状态良好，在飞行场地安全条件下再进行操作。
- 键盘按键"a" -- 解锁
- 键盘按键"d" -- 上锁
- 键盘按键"t" -- 起飞
- 键盘按键"l" -- 降落
代码说明
- 导入参数：获取预先计算得到的相机标定数据，路径需要视情况更改。
- 这些参数将用于校正图像中的畸变，计算图像的实际尺度，进而为Aruco码的位姿估计做准备。
# 加载相机标定数据 {#加载相机标定数据 }
with np.load('/home/wsg/桌面/calib_raspi/calibration_data.npz') as data:
camera_matrix = data['camera_matrix']
dist_coeffs = data['dist_coeffs']
f_x = camera_matrix[0, 0] # 获取水平焦距
f_y = camera_matrix[1, 1] # 获取垂直焦距
cx = camera_matrix[0, 2] # 图像中心的 x 坐标
cy = camera_matrix[1, 2] # 图像中心的 y 坐标
- Aruco字典和检测参数设置
- 定义了一个6x6、250个ID的Aruco字典，并创建了检测参数。这些字典和参数将用于后续的Aruco码检测。注意aruco.DICT_6X6_250参数要根据你生成的Aruco码来选择。
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
- 主程序入口
- 主程序通过多进程并发执行视频显示（video_display）和二维码检测（qr_code_detection）。共享内存用于在进程之间传递视频帧，互斥锁保证内存访问的线程安全。
if __name__ == '__main__':
W_img = 640
H_img = 480
shared_array = mp.Array('B', W_img * H_img * 3) # 共享内存，大小为图片像素总数
lock = mp.Lock() # 互斥锁，保证对共享内存的访问是线程安全的
ffmpeg_command = [
'ffmpeg',
'-i', 'tcp://192.168.137.234:2024', # IP摄像头地址，替换为你的摄像头IP
'-f', 'image2pipe',
'-pix_fmt', 'bgr24',
'-vcodec', 'rawvideo',
'-bufsize', '500k', # 增加缓冲区
'-'
]
p1 = mp.Process(target=video_display, args=(ffmpeg_command, shared_array, lock))
p2 = mp.Process(target=qr_code_detection, args=(shared_array, lock))
p1.start()
p2.start()
p1.join()
p2.join()
- 视频显示进程：video_display函数
- 该函数用于从IP摄像头获取视频流并显示实时视频。视频流是通过FFmpeg命令获取的，数据通过共享内存shared_array存储。为了防止多个进程同时读写共享内存，这里使用了互斥锁（lock）保证线程安全。
入口参数：ffmpeg_command: ffmpeg命令处理视频流数据
shared_array: 多进程共享内存对象，用于在多个进程间共享图像数据。
lock: 用于保护共享内存访问的线程/进程锁（防止读写冲突）。
def video_display(ffmpeg_command, shared_array, lock):
process = subprocess.Popen(ffmpeg_command, stdout=subprocess.PIPE)
W_img = 640
H_img = 480
while True:
raw_frame = process.stdout.read(W_img * H_img * 3)
if len(raw_frame) != W_img * H_img * 3:
break
frame = np.frombuffer(raw_frame, np.uint8).reshape((H_img, W_img, 3))
with lock:
np.copyto(np.frombuffer(shared_array.get_obj(), dtype=np.uint8).reshape(H_img, W_img, 3), frame)
cv2.imshow('Real-Time Video', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
process.terminate()
- ArUco码检测进程：qr_code_detection函数
def qr_code_detection(shared_array, lock):
W_img = 640
H_img = 480
aruco_size = 0.1 # ArUco码的实际边长，单位：米
kp_x = 0.6 #pid参数
kp_y = 0.6
kp_alt = 0.6
kp_yaw = 0.3
dl = datalink() # 实例化 datalink
data_thread = threading.Thread(target=dl.main)
data_thread.start()
while True:
with lock:
frame = np.frombuffer(shared_array.get_obj(), dtype=np.uint8).reshape(H_img, W_img, 3).copy()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
if ids is not None:
# 估计ArUco码的位姿
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners, aruco_size, camera_matrix, dist_coeffs)
for i in range(len(ids)):
# 获取ArUco码的旋转向量和位移向量
rvec = rvecs[i][0]
tvec = tvecs[i][0]
# 绘制检测到的ArUco码的边框和轴
aruco.drawDetectedMarkers(frame, corners)
#aruco.drawAxis(frame, camera_matrix, dist_coeffs, rvec, tvec, 0.1)
# 将位姿转换为控制指令，例如距离（dz_m）、左右偏移量（dx_m）和高度差（dy_m）
dz_m = tvec[2] # 前后距离
dx_m = tvec[0] # 左右偏移
dy_m = -tvec[1] # 高度偏移
print(f"Translation vector: {tvec}")
print(f"Rotation vector: {rvec}")
# ArUco码的中心点 (计算其相对于图像中心的偏移)
corner_points = corners[i][0]
x1, y1 = corner_points[0]
x2, y2 = corner_points[2]
center_x = (x1 + x2) / 2
center_y = (y1 + y2) / 2
# 计算相对于图像中心的偏移
dx_pixel = center_x - cx # 水平偏移量（像素）
dy_pixel = center_y - cy # 垂直偏移量（像素）
# 根据焦距计算偏航角 (yaw)（左右偏移量转换成弧度）
d_yaw = np.arctan(dx_pixel / f_x) # 水平偏移转为弧度
# 假设我们将无人机的目标距离设定为1.5米
dx_1 = dz_m - 1.5 # 距离与目标距离之差
dy_1 = dx_m
d_alt_1 = dy_m
#d_yaw = np.arctan(dx_m / f_x) # 获取yaw角
dl.set_pose(kp_x * dx_1, kp_y * dy_1, kp_alt * d_alt_1, kp_yaw * d_yaw)
label = f'dx: {dx_m:.2f} m, dy: {dy_m:.2f} m, dz: {dz_m:.2f} m, d_yaw:{d_yaw:.2f} rad'
cv2.putText(frame, label, (10, 30 + i * 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
else:
print("No ArUco marker detected.")
cv2.imshow('ArUco Code Detection', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
dl.socket_tcp.close()
data_thread.join()
- 共享内存与互斥锁：
○ shared_array：从共享内存中获取图像帧，并使用 lock 确保线程安全。
○ 使用 copy() 获取当前帧，以防止后续对共享内存的操作产生影响。 - Aruco码检测：
○ aruco.detectMarkers()：在每帧灰度图像中检测 ArUco 码，返回检测到的 ArUco 码角点、ID以及被拒绝的候选区域。
○ aruco.estimatePoseSingleMarkers()：通过检测到的 ArUco 码角点和相机参数估计 ArUco 码的旋转和位移。 - 位姿控制：
○ 通过 tvecs 提取距离信息，包括前后距离 dz_m、左右偏移 dx_m 和高度偏移 dy_m。
○ 计算 ArUco 码中心与图像中心的水平和垂直像素偏移量，并利用这些信息计算偏航角 d_yaw。
○ 假设目标距离为 1.5 米，通过 PID 控制器计算出距离误差，并将控制指令发送至无人机。 - 结果显示：
○ 在视频图像上绘制每个 ArUco 码的位姿信息，包括水平、垂直、深度距离和偏航角等信息。
○ 每当检测到 ArUco 码时，生成的控制指令会发送到与无人机或设备的通信链路中。 - 循环结束：
○ 通过按下 q 键退出程序，并关闭所有窗口和数据连接。
摄像头坐标系与无人机坐标系转换
-
estimatePoseSingleMarkers()
-
函数作用：估计marker在相机坐标系中的3D位姿。
- 参数：
- 第一个参数：
- 第二个参数：marker的尺寸
- 第三个参数：相机的内参矩阵
- 第四个参数：相机的畸变系数
- 第五个参数：输出marker坐标系到相机坐标系的旋转变换矩阵R
- 第六个参数：输出marker坐标系到相机坐标系的旋转变换矩阵T
- 注: marker坐标系用marker的位置有关, z轴向外。
-
输出参数：
- rvecs：每个 ArUco 标签对应的旋转向量（Rodrigues 向量），表示标签相对于相机坐标系的姿态。
- tvecs：每个 ArUco 标签对应的平移向量，表示标签在相机坐标系中的位置（单位：米）。
注意：这里是相机坐标系是以左上角为零点，xy平面平行于相机2D平面，z轴垂直于相机2D平面。
# 估计ArUco码的位姿
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners, aruco_size, camera_matrix, dist_coeffs)
for i in range(len(ids)):
# 获取ArUco码的旋转向量和位移向量
rvec = rvecs[i][0]
tvec = tvecs[i][0]
- 相机坐标系转化为控制指令，我们的控制是基于无人机坐标系的，所以还需要进行相机坐标系向无人机坐标系的转换。
# 将位姿转换为控制指令，例如距离（dz_m）、左右偏移量（dx_m）和高度差（dy_m）
dz_m = tvec[2] # 前后距离
dx_m = tvec[0] # 左右偏移
dy_m = -tvec[1] # 高度偏移
print(f"Translation vector: {tvec}")
print(f"Rotation vector: {rvec}")
# ArUco码的中心点 (计算其相对于图像中心的偏移)
corner_points = corners[i][0]
x1, y1 = corner_points[0]
x2, y2 = corner_points[2]
center_x = (x1 + x2) / 2
center_y = (y1 + y2) / 2
# 计算相对于图像中心的偏移
dx_pixel = center_x - cx # 水平偏移量（像素）
dy_pixel = center_y - cy # 垂直偏移量（像素）
# 根据焦距计算偏航角 (yaw)（左右偏移量转换成弧度）
d_yaw = np.arctan(dx_pixel / f_x) # 水平偏移转为弧度
# 假设我们将无人机的目标距离设定为1.5米
dx_1 = dz_m - 1.5 # 距离与目标距离之差
dy_1 = dx_m
d_alt_1 = dy_m
#d_yaw = np.arctan(dx_m / f_x) # 获取yaw角
- 控制无人机位姿函数，我们的x，y，z坐标，yaw轴姿态都是根据自身世界坐标系叠加偏移距离（角度）计算得出。
# 设置位姿函数
dl.set_pose(kp_x * dx_1, kp_y * dy_1, kp_alt * d_alt_1, kp_yaw * d_yaw)
# dx:m dy:m d_alt:m d_yaw:rad
def set_pose(self, dx, dy, d_alt, d_yaw):
if (self.pos_x != 0) and (self.pos_y != 0) and (self.att_yaw != 0):
global_dx = dx * math.cos(self.att_yaw) - dy * math.sin(self.att_yaw)
global_dy = dx * math.sin(self.att_yaw) + dy * math.cos(self.att_yaw)
self.x = self.pos_x + global_dx
self.y = self.pos_y + global_dy
self.z = self.relative_alt + d_alt
self.yaw = self.att_yaw + d_yaw
self.mav_drone.set_position_target_local_ned_send(0, 0, 0, 0, 0, self.x, self.y, self.z, 0, 0, 0, 0, 0, 0, self.yaw, 0)
键盘控制代码说明。
- 如果想要加入键盘控制功能，请根据如下操作：
- 在datalink_serial.py文件的导入库文件部分
pynput 是一个用于监控和控制鼠标、键盘输入的 Python 库。# !/usr/bin/env python # coding=utf-8 import time import sys, os import threading import math import struct, array, time, json from mavlink import * from socket import * from pynput import keyboard # 导入键盘监听库文件
2. 在datalink类中加入函数on_press:
定义了一个按键事件处理函数，用于监听键盘输入，根据不同的按键执行对应的操作，如解锁、锁定、起飞、降落及前后左右移动。def on_press(self,key): try: # 获取按下的按键 print('You pressed {0}'.format(key.char)) if key.char=='a' : self.set_arm() print('解锁') if key.char=='d' : self.set_disarm() print('锁定') if key.char=='t' : self.set_takeoff() self.set_xy_pose(0, 0, 0) print('起飞') if key.char=='l' : self.set_land() print('降落') except AttributeError: print('You pressed {0}'.format(key)) if format(key) == 'Key.up': self.set_xy_pose(0.2, 0, 0) print('前进') if format(key) == 'Key.down': self.set_xy_pose(-0.2, 0, 0) print('后退') if format(key) == 'Key.right': self.set_xy_pose(0, 0.2, 0) print('右移') if format(key) == 'Key.left': self.set_xy_pose(0, -0.2, 0) print('左移')
3. main()函数如下所示
首先我们创建了一个键盘监听对象，并开始进行键盘按键的监听，最后我们需要监听程序不断运行。def main(self): print('data link is starting...\n') self.socket_tcp.connect(self.host_addr) drone_thread = threading.Thread(target=self.drone) try: drone_thread.start() listener = keyboard.Listener(on_press=self.on_press) # 创建键盘监听对象 -------- listener.start() # 开始监听 -------- while True: self.mav_drone.heartbeat_send(MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0, MAV_STATE_STANDBY, 1) time.sleep(1) except Exception as e: print("Error sending message:", e) finally: drone_thread.join() self.socket_tcp.close() listener.join() # 等待监听线程 --------