【ArduPilot】Windows下使用Optitrack通过MAVProxy连接无人机实现定位与导航

在GPS信号弱或完全不可用的室内环境中，利用OptiTrack运动捕捉系统替代GPS，实现无人机的精确定位与导航已成为一种有效方案。本文详细介绍如何在Windows平台下，通过MAVProxy将OptiTrack系统与无人机连接，从而支持Loiter、Guided、RTL和Auto等多种位置控制模式。

环境：

Windows ：Windows 11

Motive ：2.2.0

MAVProxy ：1.8.71

ArduPilot ：4.4.4

配置动捕系统

有关硬件和软件设置，请参阅 OptiTrack 快速入门指南。

Quick Start Guide

设置地平面时，不要忘记标记原点和坐标轴。

X轴将被设置为动捕系统的“北”，如果不使用指南针的话，它不需要指向真实磁北。

无人机贴动捕球

首先，为确保系统能准确捕捉无人机的姿态，必须在无人机上安装运动捕捉标记（动捕球），并要求标记布局具备立体且不对称的特性。

有关详细信息，请参阅 OptiTrack 刚体标记放置。

Rigid Body Marker Placement

然后，将无人机放在地平面上，并将其与运动捕捉系统的X轴对齐。确保无人机机头与Motive系统中的X轴正方向对齐。

将无人机放置在动捕场地中，在 MOTIVE 软件的 3D 视图中可以看到对应的标记点，如下图所示，无人机上固定有四个标记点。

框选住刚体模型所有标记点，点击右键选择【Rigid Body】下的【Create From Selected Markers】，完成刚体创建。

这时 MOTIVE 软件右下角会出现该刚体的属性窗口，可以对该刚体的名称【Name】进行修改，可以对该刚体的数据流序号【Streaming ID】进行修改。

有关详细信息，请参阅 OptiTrack 创建刚体。

Creating Rigid Body

配置无人机参数

在无人机配置中设置以下参数，以确保EKF和视觉定位模块能正确处理外部导航数据。

AHRS_EKF_TYPE   3
EK3_ENABLE      1
EK2_ENABLE      0
COMPASS_USE     0
COMPASS_USE2    0
COMPASS_USE3    0
VISO_TYPE       1
VISO_POS_M_NSE  0.3
VISO_YAW_M_NSE  0.2
EK3_SRC1_POSXY  6
EK3_SRC1_POSZ   6
EK3_SRC1_YAW    6
EK3_SRC1_VELXY  0
EK3_SRC1_VELZ   0

使用MAVProxy连接Optitrack

1、连接无人机

需要先使用 MAVProxy 连接无人机。

打开 Windows 的 cmd 命令行终端，运行以下命令连接无人机。

mavproxy --master=COM1 --baudrate=115200 --out=udp:127.0.0.1:14550--master参数用于指定无人机正在通信的端口（串口、USB或网络地址/端口）。--master=COM1 --baudrate=115200
--master=/dev/ttyUSB0,57600
--master="com14"
--master=tcp:192.168.1.1:14550
--master=udp:192.168.1.1:14550--out参数用于将MAVLink数据包转发到远程设备（串口、USB或网络地址/端口），可以用于连接地面站。MAVlink通常使用端口14550进行基于IP的数据包转发，但如果需要，也可以使用其他端口。--out=udp:192.168.1.1:14550
--out=/dev/ttyUSB0,57600
--out=COM17,57600
--out=udpbcast:192.168.2.255:14550

有关启动命令的详细信息，请参阅 MAVProxy 启动选项。

Startup Options

##　2、加载OptiTrack模块

连接成功之后，可以打开地面站，如果 --out 设置为本机的 UDP 端口，地面站会自动连接上无人机。

之后在 MAVProxy 终端加载 optitrack模块。

module load optitrack

3、设置跟踪刚体ID

需要设置跟踪刚体 id 以匹配 Motive 中的设置，跟踪刚体 id 就是配置动捕时刚体的数据流序号【Streaming ID】。

optitrack set obj_id RIGID_BODY_STREAMING_ID

4、校正坐标系

Motive 和 ArduPilot 的坐标系都是右手坐标系。当 ArduPilot 的 Z 轴指向下方时，Motive 的 Y 轴指向上方。 Motive 中【Up Axis】需要选择【Y Up】。

5、配置IP地址（非Loopback模式）

如果 Motive 中数据流的本地接口不是设置为【loopback】，就需要设置以下参数，【SERVER_IP_ADDRESS】是 Motive 本地接口的IP地址，【CLIENT_IP_ADDRESS】是 MAVProxy 主机的IP地址。

optitrack set server SERVER_IP_ADDRESS
optitrack set client CLIENT_IP_ADDRESS

6、启动动捕数据推流

做好以上的配置之后，使用以下命令启动动捕数据推流。

optitrack start

如果在地面站消息处看到以下消息出现（初始位置可能有所不同），则无人机正在从 Optitrack 系统接收姿态数据。

EKF3 IMU0 is using external nav data EKF3 IMU0 initial pos NED = 0.1,-0.2,0.0 (m) EKF3 IMU0 ext nav yaw alignment complete

结语

通过以上步骤，您可以在 Windows 平台上利用 OptiTrack 运动捕捉系统，通过 MAVProxy 实现无人机在室内环境下的精准定位与导航。希望本文能为您的无人机项目提供有价值的参考与指导！

---

参考资料：

Optitrack for Non-GPS Navigation

MAVProxy Getting Started