ardupilot 姿态控制器最新流程（V4.4.4）
需积分: 5 146 浏览量
2024-01-22
14:55:06
上传
评论
收藏 242KB PDF 举报
开始
摇杆欧拉姿态
控制量
这个是cm度
计算目标欧
拉角
_attitude_target
_euler_angle_target.x
_euler_angle_target.y
_euler_angle_target.z
float euler_roll_angle_cd
float euler_pitch_angle_cd
float euler_yaw_rate_cds
摇杆
目标欧拉角
float euler_roll_angle
float euler_pitch_angle
float euler_yaw_rate
是否开启
角速率前馈
N
Y
目标四元数
_euler_angle_target.x = euler_roll_angle;
_euler_angle_target.y = euler_pitch_angle;
_euler_angle_target.z += euler_yaw_rate * _dt;
无前馈
有前馈
增加每个轴的欧拉旋转
加速度限制值
euler_accel
使用PI及
开方控制器计算
目标欧拉角速度
_euler_rate_target.x
_euler_rate_target.y
_euler_rate_target.z
使用目标欧拉角及目标
欧拉角的角速度计算机
体的目标角速度
1 0 sin
0 cos sin cos
0 sin cos cos
p
q
r
_ang_vel_target.x
_ang_vel_target.y
_ang_vel_target.z
根据最大角速度限制输
出的机体角速度
参数
ATC_RATE_P_MAX
ATC_RATE_R_MAX
ATC_RATE_Y_MAX
参数
ATC_ACCEL_P_MAX,60000
ATC_ACCEL_R_MAX,60000
ATC_ACCEL_Y_MAX,10000
_euler_rate_target.x = input_shaping_angle(wrap_PI(euler_roll_angle - _euler_angle_target.x), _input_tc, euler_accel.x, _euler_rate_target.x, _dt);
_euler_rate_target.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _euler_angle_target.y), _input_tc, euler_accel.y, _euler_rate_target.y, _dt);
_euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt, _rate_y_tc)
注1
注1
参数：
ATC_INPUT_TC
注2
attitude_controller_run_quat();
根据目标欧拉角及机体角速度
再次得到欧拉角速度
1 sin tan cos tan
0 cos sin
0 sin / cos cos / cos
p
q
r
_euler_rate_target.x
_euler_rate_target.y
_euler_rate_target.z
四元数姿态
控制
注3
_euler_angle_target.x = euler_roll_angle;
_euler_angle_target.y = euler_pitch_angle;
_euler_angle_target.z += euler_yaw_rate * _dt;
//计算目标四元数--- Compute quaternion target attitude
_attitude_target.from_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
//设置前馈请求到零---- Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
注3
清零前馈
目标欧拉角 目标四元数
_attitude_target
_euler_rate_target.zero();
_ang_vel_target.zero();
_ang_vel_target.x
_ang_vel_target.y
_ang_vel_target.z
获取实际的
姿态四元数
获取目标姿
态四元数
_attitude_target
attitude_body
计算姿态误
差、推力
角、推力误
差角
Quaternion attitude_body;
_ahrs.get_quat_body_to_ned(attitude_body);
注4
attitude_error
_thrust_angle
_thrust_error_angle
Vector3f attitude_error;
thrust_heading_rotation_angles(_attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle);
注5
注5
外环姿态控制器
_ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
注6
注6
得到地理坐标系下的
机体目标角速度
_ang_vel_body.x
_ang_vel_body.y
_ang_vel_body.z
增加每个轴最大目
标角速度限制
_ang_vel_body.x
_ang_vel_body.y
_ang_vel_body.z
参数
ATC_RATE_P_MAX
ATC_RATE_R_MAX
ATC_RATE_Y_MAX
把目标四元数旋转到
机体坐标系下
注7
注7
这里需要特别注意下面那个注释，这个说的是旋转，不是四元数，
// rotation from the target frame to the body frame
Quaternion rotation_target_to_body = attitude_body.inverse() * _attitude_target;
rotation_target_to_body
计算前馈的目标角速
度矢量
有前馈
_ang_vel_target.x
_ang_vel_target.y
_ang_vel_target.z
ang_vel_body_feedforward
Quaternion rotation_target_to_body = attitude_body.inverse() * _attitude_target;
Vector3f ang_vel_body_feedforward = rotation_target_to_body * _ang_vel_target;
_feedforward_scalar = 1.0f;
if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f)
{
_ang_vel_body.z = _ahrs.get_gyro().z;
} else if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE)
{
_feedforward_scalar = (1.0f - (_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE)
/ AC_ATTITUDE_THRUST_ERROR_ANGLE);
_ang_vel_body.x += ang_vel_body_feedforward.x * _feedforward_scalar;
_ang_vel_body.y += ang_vel_body_feedforward.y * _feedforward_scalar;
_ang_vel_body.z += ang_vel_body_feedforward.z;
_ang_vel_body.z = _ahrs.get_gyro().z * (1.0 - _feedforward_scalar) + _ang_vel_body.z * _feedforward_scalar;
} else
{
_ang_vel_body += ang_vel_body_feedforward;
}
修正外环控制器输出
的角度值，既得到内
环控制器输入值
_ang_vel_body.x
_ang_vel_body.y
_ang_vel_body.z
是否开启
角速率前馈
N
Y
_ang_vel_target.x
_ang_vel_target.y
_ang_vel_target.z
Quaternion attitude_target_update;
attitude_target_update.from_axis_angle(Vector3f{_ang_vel_target.x * _dt, _ang_vel_target.y * _dt, _ang_vel_target.z * _dt});
姿态更新目标四元数
姿态目标四元数
_attitude_target
_attitude_target = _attitude_target * attitude_target_update;
attitude_target_update
姿态目标四元数
归一化处理
_attitude_target
计算姿态误差
_attitude_target.normalize();
_attitude_ang_error = attitude_body.inverse() * _attitude_target;
注意第一次得到的四元数表示的是实际姿态和目标姿态一样
attitude_control->reset_yaw_target_and_rate(false); //复位目标偏航和偏航角速度
void AC_AttitudeControl::reset_yaw_target_and_rate(bool reset_rate)
{
// move attitude target to current heading
float yaw_shift = _ahrs.yaw - _euler_angle_target.z;
Quaternion _attitude_target_update;
_attitude_target_update.from_axis_angle(Vector3f{0.0f, 0.0f, yaw_shift});
_attitude_target = _attitude_target_update * _attitude_target;
if (reset_rate) {
// set yaw rate to zero
_euler_rate_target.z = 0.0f;
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_euler_angle_target, _euler_rate_target, _ang_vel_target);
}
}
注8
注8
attitude_body
注9
注9
外环控制器
内环控制器
attitude_control->rate_controller_run()
void AC_AttitudeControl_Multi::rate_controller_run()
{
// boost angle_p/pd each cycle on high throttle slew
update_throttle_gain_boost();
// move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
update_throttle_rpy_mix();
_ang_vel_body += _sysid_ang_vel_body;
Vector3f gyro_latest = _ahrs.get_gyro_latest();
_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
_motors.set_roll_ff(get_rate_roll_pid().get_ff());
_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
_motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
_motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
_sysid_ang_vel_body.zero();
_actuator_sysid.zero();
_pd_scale_used = _pd_scale;
_pd_scale = VECTORF_111;
control_monitor_update();
}
注10
gyro_latest
_ang_vel_body.x
_ang_vel_body.y
_ang_vel_body.z
内环PID控
制器
参数
ATC_ANG_PIT_P,4.5
ATC_ANG_RLL_P,4.5
ATC_ANG_YAW_P,4.5
参数
ATC_RATE_FF_ENAB
参数
ATC_RATE_FF_ENAB
参数
ATC_RAT_PIT_P,0.2
ATC_RAT_PIT_I,0.18
ATC_RAT_PIT_IMAX,0.5
ATC_RAT_PIT_D,0.001
ATC_RAT_RLL_P,0.2
ATC_RAT_RLL_I,0.18
ATC_RAT_RLL_IMAX,0.5
ATC_RAT_RLL_D,0.001
ATC_RAT_YAW_P,0.255
ATC_RAT_YAW_I,0.018
ATC_RAT_YAW_IMAX,0.5
ATC_RAT_YAW_D,0.018
作者：小历
时间：2024-1-22
版本：V2.0
注11
前馈的主要信息是机体目标角速度
_ang_vel_body.x
_ang_vel_body.y
_ang_vel_body.z
资源评论
