ArduPilot二次开发实战：从零打造无人船（车）智能控制系统

1. 为什么选择ArduPilot进行无人船/车开发第一次接触ArduPilot时我被它的开源生态震惊了。这个诞生于2007年的自动驾驶系统如今已经支持几乎所有你能想到的移动平台——从天上飞的多旋翼到水里游的潜水器。特别是在无人船和无人车领域ArduPilot的成熟度远超其他开源方案。我去年用Pixhawk4飞控改造了一艘钓鱼无人船整个过程就像在玩高级乐高。ArduPilot的模块化设计让硬件扩展变得特别简单通过Telem串口接个485转接板就能控制推进电机用AUX输出口驱动舵机转向。最让我惊喜的是它的航点导航功能在地面站划个路线小船就能自动巡航实测定位精度能达到1米内。相比从头造轮子基于ArduPilot二次开发有三个明显优势硬件兼容性强支持Pixhawk全系列飞控各种传感器即插即用软件生态完善MAVLink协议、QGC地面站、SITL仿真工具链齐全社区支持给力遇到问题在GitHub提issue通常24小时内就有回复2. 开发环境搭建避坑指南在Ubuntu 20.04上配置ArduPilot开发环境时我踩过不少坑。第一次编译就遇到python依赖报错后来发现必须用特定版本的pip工具。这里分享我的标准化搭建流程# 安装基础工具链 sudo apt-get install git zip qtcreator cmake build-essential genromfs -y # 配置python3环境 sudo apt-get install python3-dev python3-pip sudo pip3 install future pymavlink MAVProxy # 克隆代码库建议用国内镜像 git clone https://gitee.com/ardupilot/ardupilot.git cd ardupilot git submodule update --init --recursive # 关键一步安装编译器 ./Tools/environment_install/install-prereqs-ubuntu.sh -y . ~/.profile编译Rover固件时有个隐藏技巧在waf configure阶段加上--debug参数这样调试时能看到更详细的日志。我常用的编译命令是这样的./waf configure --board Pixhawk1 ./waf build --target bin/ardurover遇到内存不足的情况可以试试在虚拟机设置里分配至少8GB内存。有次我只分配了4GB编译到90%时系统直接卡死不得不从头再来。3. MAVROS通信实战技巧让ROS和飞控通信就像教两个说不同语言的人对话。MAVROS这个翻译官有时候会闹脾气特别是波特率设置不匹配时。这里给出一个经过验证的launch文件配置launch arg namefcu_url defaultudp://:14550 / arg namegcs_url default / arg nametgt_system default1 / arg nametgt_component default1 / include file$(find mavros)/launch/node.launch arg namepluginlists_yaml value$(find mavros)/launch/apm_pluginlists.yaml / arg nameconfig_yaml value$(find mavros)/launch/apm_config.yaml / arg namefcu_url value$(arg fcu_url) / arg namegcs_url value$(arg gcs_url) / arg nametgt_system value$(arg tgt_system) / arg nametgt_component value$(arg tgt_component) / /include /launch测试时发现几个常见问题及解决方法连接超时检查飞控和电脑是否在同一网络防火墙是否放行了14550端口数据跳变尝试降低MAVROS的消息频率例如将IMU数据从50Hz降到30Hz控制延迟在QGC中关闭不必要的遥测数据上传4. Modbus电机控制详解用飞控直接控制工业电机听起来很酷但协议转换是个技术活。我用的AIM伺服电机支持Modbus-RTU协议需要特别注意以下几点电气隔离一定要在485总线上加磁耦隔离模块有次没隔离导致飞控串口烧毁超时重发工业环境干扰大建议实现自动重发机制CRC校验必须严格校验每个数据包我吃过电机突然暴走的亏这段CRC16校验代码实测稳定可以直接复用unsigned short CRC16(unsigned char *puchMsg, unsigned short usDataLen) { unsigned char uchCRCHi 0xFF; unsigned char uchCRCLo 0xFF; unsigned uIndex; while (usDataLen--) { uIndex uchCRCHi ^ *puchMsg; uchCRCHi uchCRCLo ^ auchCRCHi[uIndex]; uchCRCLo auchCRCLo[uIndex]; } return (uchCRCHi 8 | uchCRCLo); }控制电机转到指定位置的函数要处理好字节序。大端模式和小端模式搞反了电机可能会转到完全错误的角度void set_motor_position(uint32_t pos) { uint8_t buf[8] { 0x01, // 设备地址 0x06, // 功能码 (pos 24) 0xFF, (pos 16) 0xFF, (pos 8) 0xFF, pos 0xFF }; uint16_t crc CRC16(buf, 6); buf[6] crc 0xFF; buf[7] (crc 8) 0xFF; hal.serial(1)-write(buf, 8); }5. Guided模式深度开发让无人船自动执行复杂任务关键在于用好Guided模式。我总结出三种典型控制方式位置控制适合定点巡航Location target_loc; target_loc.lat 39.9087; // 北京纬度 target_loc.lng 116.3975; // 北京经度 rover.mode_guided.set_desired_location(target_loc);航向速度控制适合直线航行// 参数1航向角单位厘度18000表示180度 // 参数2速度m/s rover.mode_guided.set_desired_heading_and_speed(18000, 2.5);转向率控制适合避障机动// 参数1转向率单位厘度/秒 // 参数2速度m/s rover.mode_guided.set_desired_turn_rate_and_speed(5000, 1.0);在实际项目中我建议添加状态机管理。比如下面这个简单的任务流程阶段1航行到目标点阶段2到达后悬停30秒阶段3自动返航6. 自定义MAVLink消息全流程扩展MAVLink协议就像给飞控安装新的传感器。最近我给无人船加了水深传感器完整实现步骤是这样的修改消息定义文件message id31000 nameWATER_DEPTH field typefloat namedepth unitsmCurrent depth/field field typeuint8_t namequalitySignal quality/field /message注册消息发送函数void GCS_MAVLINK::send_water_depth() const { float depth get_sensor_data(); // 获取真实传感器数据 mavlink_msg_water_depth_send(chan, depth, 100); }QGC端解析 需要在CustomWidget.cc中添加对应的数据显示控件WaterDepthWidget::WaterDepthWidget(QWidget *parent) : QWidget(parent), ui(new Ui::WaterDepthWidget) { ui-setupUi(this); connect(_uas, UASInterface::waterDepthReceived, this, WaterDepthWidget::updateDepth); }实测发现消息ID最好选30000以上的避免与标准消息冲突。传输浮点数时要特别注意字节对齐问题否则地面站解析会出错。7. 参数系统高级用法ArduPilot的参数系统就像飞控的控制面板我经常用它来调PID参数。创建自定义参数的完整流程声明参数Parameters.hAP_Float my_pid_p; // 浮点型参数 AP_Int8 enable_debug; // 整型参数注册参数Parameters.cpp// Param: MY_PID_P // DisplayName: PID比例系数 // Description: 控制系统的比例项系数 // Range: 0 10.0 // User: Advanced GSCALAR(my_pid_p, MY_PID_P, 1.0), // Param: EN_DEBUG // DisplayName: 调试模式开关 // Description: 启用详细调试输出 // Values: 0:禁用,1:启用 GSCALAR(enable_debug, EN_DEBUG, 0),使用参数void control_loop() { float p my_pid_p.get(); if (enable_debug.get() 1) { gcs().send_text(MAV_SEVERITY_INFO, PID_P%.2f, p); } }有个实用技巧在QGC中可以把常用参数保存为配置文件下次烧录固件后一键加载不用再手动设置。8. 实机调试经验分享实验室仿真通过不代表现场能成功。去年夏天在太湖测试时烈日暴晒导致飞控温度飙升出现了这样的状况11:00 AM一切正常定位精度0.8米13:00 PMGPS频繁丢星航向漂移15:00 PM主控重启任务中断后来我们做了这些改进给飞控加装散热片和小风扇在代码中添加温度监控if (hal.analogin-board_voltage() 5.3) { gcs().send_text(MAV_SEVERITY_CRITICAL, 高温告警); reduce_cpu_load(); // 主动降频 }准备备用飞控做热切换另一个血泪教训水上测试一定要做好防水有次浪花打湿了电调导致电机突然满速旋转差点把螺旋桨打碎。现在我们的标准做法是所有接缝处打防水胶电路板喷三防漆准备应急断电开关