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 快速入门指南。
设置地平面时，不要忘记标记原点和坐标轴。
X轴将被设置为动捕系统的“北”，如果不使用指南针的话，它不需要指向真实磁北。
无人机贴动捕球
首先，为确保系统能准确捕捉无人机的姿态，必须在无人机上安装运动捕捉标记（动捕球），并要求标记布局具备立体且不对称的特性。
有关详细信息，请参阅 OptiTrack 刚体标记放置。
然后，将无人机放在地平面上，并将其与运动捕捉系统的X轴对齐。确保无人机机头与Motive系统中的X轴正方向对齐。
将无人机放置在动捕场地中，在 MOTIVE 软件的 3D 视图中可以看到对应的标记点，如下图所示，无人机上固定有四个标记点。
框选住刚体模型所有标记点，点击右键选择【Rigid Body】下的【Create From Selected Markers】，完成刚体创建。
这时 MOTIVE 软件右下角会出现该刚体的属性窗口，可以对该刚体的名称【Name】进行修改，可以对该刚体的数据流序号【Streaming ID】进行修改。
有关详细信息，请参阅 OptiTrack 创建刚体。
配置无人机参数
在无人机配置中设置以下参数，以确保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 启动选项。
## 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 实现无人机在室内环境下的精准定位与导航。希望本文能为您的无人机项目提供有价值的参考与指导！
参考资料：