控制器修改于高飞老师的Fast-250项目,下面对修改部分做说明
去除功能:取消自动起飞降落功能
增加功能:
- 增加动态参数功能(用于gazebo仿真时模拟遥控器的offboard与指令开关)
- 增加角速度控制
参数功能修改:
- use_bodyrate_ctrl 设置为true时起作用,为角速度控制
- auto_takeoff_land/no_RC 设置为true时为进行gazebo仿真(动态参数模拟遥控器开关),false时为真机实验
- gain/KAngR 三个增益参数需要小心设置,使用默认参数在gazebo中会震荡导致发散
- debug 开启发布debug话题
使用流程:
启动节点后,首先让无人机起飞悬停,然后拨动模式开关进入offboard模式,此刻无人机悬停;然后拨动指令开关进行接收轨迹,无人机会跟踪轨迹;最后拨回模式开关退出offboard模式降落。
在gazebo仿真时,首先在QGC设置里面打开虚拟遥感,然后通过QGC的takeoff按钮起飞,进入hold模式后rosrun rqt_reconfigure rqt_reconfigure
启动动态调参进行控制
真机实验时首先进入position模式,然后遥控起飞悬停,通过拨杆开关控制即可
Table of contents generated with markdown-toc
PX4飞控不是开箱即用的,需要比较复杂的前期调试,推荐花半天时间把官方用户文档 docs.px4.io 中的 "Introduction" 至 "Flight Log Analysis"的部分粗略读一下.
非常建议淘宝上买个十几元的凤凰飞控模拟器,先在电脑上熟悉飞行操作再上手.
- 用QGC烧写代码库中的 .px4 后缀的固件,该固件主要修改了几个飞控消息的发送频率,烧不进去的话联系我,固件版本很繁杂的;
- 机架选择并重启.
250mm轴距的穿越机: Generic 250 Racer
330mm轴距的: DJI F330
450mm轴距的: Generic Quadcopter 或 DJI F450
其他轴距的看着来 - 校准传感器、遥控器,其中5通道设置为飞行模式切换,7通道设置为Emergency Stop(推荐);
- 修改参数
CBRK_IO_SAFETY = 22027
CBRK_USB_CHK = 197848
MAV_1_CONFIG = TELEM 2 - 如果使用支持DShot的电调(推荐),修改参数
SYS_USE_IO = 0
DSHOT_CONFIG = DShot** (**是购买的电调所支持的最大值)
测试电机转向,如果转反了,无需重焊,在QGC的 MAVLink Console 页面用以下指令把转向反过来即可这里的"1"是电机编号,具体接线和操作参考 https://docs.px4.io/master/en/peripherals/dshot.htmldshot reverse -m 1 dshot save -m 1
- 如果使用的是PWM电调,记得做油门校准,百度搜索校准方法;
- 飞控自带的ekf2(首选)需要磁力计和气压计做状态估计,这两者在小飞机上受干扰大,导致状态估计和控制不稳定、精度不高,且经常需要飞控重启,但好处是可以使用定高定点控制.如希望高的控制精度和稳定性,且不想频繁重启飞控,推荐使用飞控自带的互补滤波替换ekf2,设置参数
SYS_MC_EST_GROUP = Q attitude estimator(no position)
SYS_HAS_BARO = 0
SYS_HAS_MAG = 0
随后重启即可.这一设置会导致无法启动飞控的定高定点功能,只能使用姿态控制 Stabilized 模式或者 角速度控制 Acro 模式,且两者都是油门直通的,即油门推杆直通飞控的油门输出,需要较多练习才能掌握.
- (必须)在Tuning页面下,调整 Hover Throttle,使得 Stabilize 模式下油门杆量为中时,飞机没有高度方向上的明显加减速.如果飞定高模式或定点模式,这一值调整的不对会导致定高时随机往上冲,比较危险. 在穿越机动力套件上,该值通常在20%附近,在较大轴距、较大重量的飞机上,该值通常在默认值50%附近.
- (可选但推荐)在Tuning页面下点击"Advance"可以打开PID调参页面,按需调整PID参数,具体参考 https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html https://docs.px4.io/master/en/config_mc/racer_setup.html
- 好的控制器参数的表现:角度模式下,控制起来非常跟手,很灵敏。飞机配重到装完所有设备时候的重量,快速来回打满方向杆,飞机不会翻。 注意:调参需要一定的飞机手控能力,因为需要先后飞角速度(Arco)模式和角度(Stabilized)模式,但如果需要高精度控制,PID调参是不可避免的.参数没有调好,会导致快速改变姿态的时候飞机翻掉,此时则必须进行PID调参了. PX4文档调参中的 THR_MDL_FAC参数 可改可不改, 这一参数会在后面的油门模型辨识中被辨识出来.
电脑的串口接飞控的 TELEM2 口, 如果使用的是支持串口硬件流控的设备(除了TX、RX还有RTX、CTX),推荐连接启用硬件流控以避免阻塞丢包.
mavros是PX4飞控同ros通信的包,推荐使用apt方式安装,有时需要外网
sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
sudo ./install_geographiclib_datasets.sh
以上参考 https://github.com/mavlink/mavros/blob/master/mavros/README.md#installation .
roscd mavros
cd launch
跳转到安装在/opt下的mavros的launch文件夹中,sudo修改其中的"px4_pluginlists.yaml"和"px4.launch",可以参考代码库中的这两个文件的一份拷贝.
px4_pluginlists.yaml主要是屏蔽了很多无关插件,免得发布和订阅大量无用的topic;
px4.launch是配置mavros同PX4和QGC的通信方式,其中"fcu_url"是同PX4飞控通信,要打开的串口名按实际计算机的设备名来设置,波特率设置921600;"gcs_url"是其与QGC通信的端口,推荐设置为"udp-b://@",即广播,此时局域网内任何一台电脑打开QGC就能连上PX4.
mavros连接上飞控之后会打印飞控的固件版本等信息,随后输入rostop hz /mavros/imu/data
会得到200Hz的imu数据,说明连接正常.
如果用了硬件流控,则设置 fcu_url = serial-hwfc:///path/to/serial/device[:baudrate]
具体使用参考 http://wiki.ros.org/mavros 和 https://github.com/mavlink/mavros/blob/master/mavros/README.md
PX4 Controller的节点名叫做 "px4ctrl", 其以Odometry和飞机的IMU为反馈, 接收期望的位置姿态等控制指令,对飞机进行高精度控制.
本px4ctrl控制器和实验室以前用的n3ctrl控制器虽然代码框架相似,但内部状态机、控制算法等部分全部重写了,以获得更高的稳定性和控制精度.
控制器共有三种模式,分别为
手动模式: 终端打印绿色的"MANUAL_CTRL(L1)", 此时控制器不起作用,飞控完全受遥控器控制.
定点模式: 终端打印绿色的"AUTO_HOVER(L2)", 此时控制器会通过代码接管飞控,飞控会处于"Offboard"模式.控制器依据odometry反馈做定点控制,此时拨动方向、油门等摇杆,飞机会慢慢移动,就像在飞大疆Mavic的定点模式一样,但此时不接收你的代码发出的指令.
指令控制模式: 终端打印绿色的"CMD_CTRL(L3)", 此时控制器允许执行你的代码发给它的飞行指令. 该模式需要你处在定点模式下,拨动6通道拨杆之后,再发送控制指令才能启动,以尽可能进行充分的安全检查.
Odometry的pose定义为前x,左y,上z,飞机机头指向x轴正向,油门拉力方向为z轴正向,一定要严格对齐!
Odometry的速度定义和ROS官方不太一样,这点需要注意.ROS官方定义速度为机体系下的,但大部分开源VIO的速度都是定义在相对世界系的,px4ctrl默认也是定义在世界系下的.如果你用的速度是定义在机体系下的,则把 input.cpp 里面的 #define VEL_IN_BODY 0 改成 1.
在QGC中设置并检查:
5通道: 飞行模式切换
6通道: 有分配拨杆但不设置任何功能
7通道: 紧急停桨(Emergency Stop)
8通道: 有分配拨杆但不设置任何功能,推荐分配一个能自动回弹的拨杆
它们在 px4ctrl 中的对应功能为
5通道: 切换px4ctrl控制(Offboard模式)或飞控的原飞行模式
6通道: 是否允许px4ctrl接收你的代码发给px4ctrl的控制指令
7通道: 紧急停桨(Emergency Stop)
8通道: 在飞控未解锁的状态下一键重启(飞控内选择ekf2估计器时常用)
自己看 px4ctrl_node.cpp 文件了解控制器的topic和service.
修改 run_ctrl.launch 中的两个topic到你用的topic名字. 用 rqt_graph
检查topic接收情况,确保控制器连接了
/mavros/state
/mavros/imu/data
/mavros/rc/in
/mavros/setpoint_raw/attitude
/你的odometry
.
并且检查 /mavros/imu/data
和/你的odometry
的频率是否在200Hz以上,/mavros/rc/in
在10Hz左右.
控制器默认运行在最低控制精度下,这里尽可能做到了自适应,因此默认参数通常就可用.
简要的控制器使用方式如下:
- 启动mavros和你的odometry节点
roslaunch mavros px4.launch roslaunch <你的odometry节点>
- 启动px4ctrl
roslaunch px4ctrl run_ctrl.launch
- 地面未解锁状态下检查.飞控未解锁状态下,拨动5通道拨杆,屏幕打印绿色的"[px4ctrl] MANUAL_CTRL(L1) --> AUTO_HOVER(L2)"即说明控制器检查通过,已切换到定点模式.再拨6通道摇杆,打印绿色的"[px4ctrl] TRIGGER sent, allow user command."说明可以切换到指令控制模式,允许外部控制指令接入.如遇到报错,按照显示的错误修正即可.
- 地面解锁飞机,手动起飞到合适高度,拨动5通道遥感切换到自动定点模式,随后拨动6通道遥感切换到控制指令接收模式.注意初期不熟悉odom稳定性的时候建议切模式前看一下定位有没有飘,否则可能模式一切定点飞机就窜出去了.
- 在拨动6通道切到控制指令接收模式时,控制器会有一个
/traj_start_trigger
的topic发出来,这个topic可以用作规划器启动的trigger,同时里面还有飞机此刻的pose. - 😆😆😆Trouble Shooting😆😆😆:
- 虽然px4ctrl会把检测到的错误打印出来,但有时候错误是飞控发出的,px4ctrl检测不到,表现为无法起飞、切定点没反应等等,这些飞控内的错误会打印在QGC地面站上.
- mavros报串口的错,检查是不是没有赋串口777权限(
sudo chmod 777 /dev/<串口名>
) ; 如果打开串口成功但没反应,检查线有没有接对,或者飞控的 MAV_1_CONFIG 参数有没有给错; - 如果报别的错,检查是不是漏装了 geographiclib.
- 使用ekf2作为飞控的状态估计时,经常会遇到降落后无法解锁,报姿态估计错误,此时拨一下遥控的8通道,飞控就会重启,错误得到解决.
- 如果切到定点模式后飞机持续往上或往下飞,排除定位飘了,那八成是因为油门杆没有回中,因为定点模式下是允许遥控器控制的。解决1:手动大致回中油门杆即可,因为中间附近设定了死区;解决2:config里面设置max_manual_vel为0即可,即关闭定点模式下的遥控控制功能。
- 如果切到定点模式后飞机位置上下来回浮动,排除定位飘了,那八成是油门模型估计导致的,是因为飞行时IMU震动太大导致油门模型估计的反馈量不准确。减震不佳的IMU加速度计震动在±5~10m/s^2,而减震好的IMU(大疆N3、雷迅Nora、X7等)加速度计震动通常不超过±0.3m/s^2,差距巨大。可以录一个飞行时imu的bag文件检查一下。虽然可以通过改控制器参数来缓解,但还是希望可以做好减震,这对整个飞机都好,因此该参数"rho2"设定成不可更改了。减震去淘宝买专用的imu海绵减震,贴飞控四个角下,期望震动应小于2m/s^2。
- (不推荐)接上一个问题,如果实在加不了减震,可以1:增大"rho2"到0.999或0.9995或0.9998,但动态性能明显变差; 或者2:自行修改油门模型估计函数“estimateThrustModel”,把原先的加速度反馈改成速度反馈,牺牲了少量动态性能,但效果提升明显;或者3:不介意控制的稳态误差的话,直接把油门模型估计注释掉也行。
配置好“auto_takeoff_land”中的几个参数之后, 发送topic “/px4ctrl/takeoff_land” 即可。
该topic内只有一个枚举类型的变量,赋值为 quadrotor_msgs::TakeoffLand::TAKEOFF 为自动起飞悬停,赋值为 quadrotor_msgs::TakeoffLand::LAND 为自动降落。
由于自动起降涉及到自动解锁等危险功能,因此代码中有比较严格的检查,请参照程序打印出的各类ERROR信息谨慎操作,这里不做赘述。
注意自动降落着陆后还需要经过约10s进行着陆检测和螺旋桨上锁,请耐心等待。
任意时候都可以通过切换遥控的模式通道(通道5)取得手动控制权, 在降落过程中拨通道6则会切换到悬停模式。
相关参数:
auto_takeoff_land:
enable: 使能自动起降功能
enable_auto_arm: 使能自动解锁。如不使能,则需要手动遥控器解锁后再发送起飞 topic 指令
takeoff_height: 自动起飞悬停高度
takeoff_land_speed: 起飞、降落的速度
😆😆😆Trouble Shooting😆😆😆:
- 如果发现自动起飞起不动,则说明悬停油门参数hover_percentage给小了,因为在自动起降过程中不会估计油门模型的参数。
- 如果自动起飞高度严重超调,则说明控制器反馈增益太小了,建议调大 gain: kp/kv.
- 如果起飞就炸机,建议检查 odometry。
- 如果自动降落电机怠速了很久(超过30s)都不停转,可能是PX4飞控的参数"MPC_MANTHR_MIN"被意外设置成了0,把它改成默认值0.08即可。
先读完快速使用教程,并且能够正确让飞机悬停或者按控制指令飞行,再修改这里的参数.
手动模式: 终端打印绿色的"MANUAL_CTRL(L1)", 此时控制器不起作用,飞控完全受遥控器控制.
定点模式: 终端打印绿色的"AUTO_HOVER(L2)", 此时控制器会通过代码接管飞控,飞控会处于"Offboard"模式,这是飞控接收外部指控指令所处的模式,是由px4ctrl的代码来启动的,如果启动失败,飞控会在mavros终端打印错误消息,如果连接了QGC地面站,也会有消息打出来.该模式下,控制器依据odometry反馈做定点控制.为了方便微调飞机的起飞位置,此时拨动方向、油门等摇杆,飞机会慢慢移动,就像在飞大疆Mavica的定点模式一样,但此时不接收你的代码发出的指令. 切换这一模式前,为确保安全,你不应该发送控制代码,你的控制指令应当在收到 /traj_start_trigger
之后才能发送,以确保不会忽然开飞.
指令控制模式: 终端打印绿色的"CMD_CTRL(L3)", 此时控制器允许执行你的代码发给它的飞行指令. 该模式需要按照确定的顺序来启动,以确保安全.首先飞机要处在定点模式下,此时把6通道拨杆从控制指令拒绝状态拨到使能状态,此刻px4ctrl会发一个/traj_start_trigger
出来,当下还尚未切换到指令控制模式,px4ctrl会等待外部指令,一旦接收到指令,模式就会切换,屏幕也会打印绿色的"[px4ctrl] AUTO_HOVER(L2) --> CMD_CTRL(L3)"
起飞降落模式不做介绍
模式状态机跳转: 优先级 手动模式>定点模式>指令控制模式. 低优先级向高跳转已在前几段介绍了,这里介绍高向低跳转. 任何模式下将5通道拨出定点模式,飞机将立刻退回到手控模式, 指令控制模式下将6通道拨离该模式,飞机会退到定点模式. 各个消息还有超时机制,odom、imu、rc等超时会退出到手动控制模式,command超时会退出到定点模式.
相关参数:
ctrl_freq_max: 控制频率,通常不用修改.
max_manual_vel: 定点模式下遥控器微调飞机位置时的最大速度,同时也是yaw角的最大角速度(弧度);
rc_reverse: 如果发现定点时遥控的控制方向和期望方向相反,就把这里面的对应的轴反一下.不过通常PX4内部都是修正好了的.
msg_timeout: 各个消息的超时时间.
控制器的控制框架是: 由你发送的控制指令中的高阶导的期望值(accel对应于角度控制模式,jerk对应于角速度控制模式)解算一个角度或角速度控制指令以及油门控制指令作为前馈,辅以你发送的控制量中的低阶导的期望值做串级反馈补偿.目前只用了PID 中的P. 串级PID参数的调试要从底层下层往上层调.
相关参数:
gain:
Kp0 ~ Kp2: 位置误差比例增益;
Kv0 ~ Kv2: 速度误差比例增益;
Kvi0 ~ Kvd2: 速度积分微分增益,实际没用上,都给0即可;
KAngR ~ KAngY: 角速度模式下的角度误差比例增益,角度模式没用;
这是由期望机体z轴加速度算出期望油门值(0~1)的映射关系. 两种模型都有部分参数是在线估计的,以更鲁棒.
简易推力模型是一个线性模型,认为油门值和产生的加速度是一个线性关系, 会在线根据期望机体z轴加速度和实际机体z轴加速度估计线性模型的斜率.
该模型下只有一个参数要给,为 hover_percentage(范围0~1),表示飞机在手飞角速度(Acro)模式下的大致悬停油门(即大致悬停时油门摇杆的杆量).该参数用作计算简易线性模型的斜率初值,会在切换到悬停时自动估计,因此其不必特别精确(误差30%以内均可),因此可以这样试着给: 250mm穿越机初始给0.2,大的机架可以适当增加到0.5, 首先把 print_value 设置为true, 随后拨5通道切换到定点模式, 即可看到控制器会在油门估计运行时不断输出估计的悬停油门值,记录下稳定值,将该值填入参数文件的 hover_percentage 即可。注意日常使用时关闭该参数,因为高频率打印数据很占CPU和IO。这个参数在/debugPx4ctrl
也是会录的,所以接收topic来看也行。
关于悬停油门给的不精确的后果,如果会往上略微冲一下(冲几厘米),说明该参数给大了,调小一些,往下掉一下,则说明给小了,理想情况是切到定点的时候不会有任何上窜或下落.
相关参数
print_value: 显示飞行过程中 “hover_percentage”(简易推力模型下)或者 “thr_scale_compensate”(精确推力模型下)的值.
accurate_thrust_model: 使能或失能精确推力模型;
hover_percentage: 简易推力模型的参数
mass: 只在精确推力模型中用
low_voltage: 低电压阈值,建议设置为电芯数*3.3V,该参数只在精确推力模型中使用;