ardupilot loiter 控制算法 说明.pdf

ardupilot loiter 控制算法 ardupilot 实现悬停控制核心思想： 1. 采集操作者操作的遥控器输入值（输入信号）进而转换成目标角度； 2. 通过目标角度计算出期望加速度； 3. 通过期望加速度计算得到期望速度； 4. 期望速度积分得到目标位置； 5. 目标位置计算目标速度； 6. 然后通过期望目标速度计算出期望目标加速度； 7. 期望目标加速度转换成姿态控制器的目标角度。 把期望目标加速度传递到姿态控制器，结合实际的无人机位置信息、速度信 息、姿态信息、姿态角速度信息通过PID 控制器实现无人机的定点控制。 定点控制开始 遥控控制 输入数据 期望角度 期望 期望加速度前馈 加速度 期望 速度 期望速度前馈 + + 目标 目标 + 目标姿态 + 目标位置  P 目标速度  PID 低通滤波  PID  PID 无人机 − − 加速度 姿态角 − 角速度 − 实际 传感器 姿态角速度 实际姿态角 实际速度 实际位置 1. ardupilot 留待模式初始化 留待模式初始化，主要实现采集操作者操作的遥控器输入值（输入信号）转 换成期望角度；期望角度转换成期望加速度。 设定飞行器的目标加速度，同时初始化悬停位置和速度等信息。 bool Copter::ModeLoiter::init(bool ignore_checks) { //定位信息OK,ignore_checks= 1才可以设置，这是没有解锁的情况下 if (copter.position_ok() || ignore_checks) { //遥控器信号没有丢失 if (!copter.failsafe.radio) { float target_roll, target_pitch; // 申请简单模式传递到飞行输入 update_simple_mode(); //转换飞行员目标输入到倾斜角度 get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); //处理飞行员的横滚，俯仰输入 loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); } else {