ArduPilot-Mocap转换解析

针对小直升机在optitrack移植过程中出现的两个Bug进行解析:

1）高度的控制如何实现，在直升机中，存在油门和总螺距的一起联动问题。
2）在切换的瞬间，会有一个大的偏差，需要查看log并想办法修复这个问题.

问题1) 解答：

在loiter模式下, 摇杆的位置对应的是给定的target_climb_rate, 详见代码如下:

对应语句为

target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());

其中考虑了遥控器的死区.
然后通过upodate_z_controller()函数计算出需要的油门控制量, 由于loiter模式下有几个子模式,仅截取其中一个子模式进行说明

上图中 pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); 函数中,会将上面计算出来的target_climb_rate转换成 _pos_target 或者_vel_desired
同时,遥控器的油门通道值还会影响高度通道acc的积分值, 详细见下图中_attitude_control.get_throttle_in()函数

再次跳转进入run_z_controller()函数中,可以看到由于之前油门值影响了_pos_target 或者_vel_desired, 进而_pos_error.z开始起作用,后面的PID运算开始发挥作用

直到下图的内容, 最终将计算出来 thr_out 送出去

因此,通过前面的分析,可以,得到如下结论: 通过油门摇杆的改变, 间接改变了油门的输出量, 其中用到PID控制器, 油门量对应的给定的爬上速率.
而我们需要将联动的两个通道也和 thr_out关联起来即可.
在ArduPilot中修改直升机 主桨转速 的函数在 copter.cpp中的 throttle_loop中 50Hz, heli_update_rotor_speed_targets();

跳转进该函数, 可得到下图:

这其中的get_pilot_desired_rotor_speed()函数为获取辅助通道的值

通过仿真测试得到如果输入值为1800则对应的值为0.8, 猜测为 (1800−1000)∗0.001=0.8(1800-1000)*0.001 = 0.8(1800−1000)∗0.001=0.8
上文中提到的油门通道的thr_out 平衡点的输出值为 0.5

问题2) 解答：

在模式的切换时候, 是有Bug的.
在模式切换的时候, 下图的loiter_nav->init_target():

跳转进去会有一个xy_controller的初始化函数,如下

_pos_control.init_xy_controller();

继续跳转进去得

上图显示的是已经修改后的loiter模式初始化部分,可以看出对于四旋翼, 其悬停的时候_ahrs.roll_sensor是在零度附近的. 而直升机在悬停的时候,是有一个可以设置的参数来进行调节的, 默认为3度. 故在这个地方,也就是模式的切换初期,我们需要先将这个数值给减去,(后面可以看到, 还会再次进行这个值的累加, 因此这个地方时还是有必要先减去这个数值的)
同时, 在 void AC_PosControl::run_xy_controller(float dt)函数中( AC_PosControl.cpp文件中)

这个两个计算vel_xy_i的地方也需要进行首次进入的积分值清零, 如下图中的reset_I(); 所示. 对于Vector2f AC_PID_2D::get_i_shrink() 也需要做同样的处理.

这样在后面的给定姿态角度的时候,就不会出现突变的问题了

注：在高度控制中，check_for_ekf_z_reset()中， shift_alt_target()函数，在模式切换的时候，会对高度通道产生影响，需要针对小直升机进行屏蔽，另外，实际飞机测试中，由于小直升机存在“围绕原点”画圈的现象，通过增大位置环的D值，可以实现定点效果。