Whether acceleration PID control can be carried out for horizontal position in ardupilot?
Can you describe more about the situation? what do you want to achieve?
There’s definitely some position controller PIDs for XY axis and Z axis that can be adjusted independantly.
hi,nicie to meet you !
I want to add position acceleration controller，for example:
void AC_PosControl::run_xy_controller_mode1(float dt)
{
//获取EKF地速度限制
float ekfGndSpdLimit, ekfNavVelGainScaler;
AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
//获取位置
Vector3f curr_pos = _inav.get_position();
//获取KP
float kP = ekfNavVelGainScaler * _p_pos_xy.kP(); // scale gains to compensate for noisy optical flow measurement in the EKF
//防止除于0----- avoid divide by zero
if (kP <= 0.0f)
{
_vel_target.x = 0.0f;
_vel_target.y = 0.0f;
} else
{
//计算误差信息----- calculate distance error
_pos_error.x = _pos_target.x - curr_pos.x;
_pos_error.y = _pos_target.y - curr_pos.y;
//Constrain _pos_error and target position
//Constrain the maximum length of _vel_target to the maximum position correction velocity
//TODO: replace the leash length with a user definable maximum position correction
//约束位置误差和目标位置
//将水平目标的最大长度约束为最大位置校正速度
//TODO：用用户可定义的最大位置校正替换牵引带长度
if (limit_vector_length(_pos_error.x, _pos_error.y, _leash))
{
_pos_target.x = curr_pos.x + _pos_error.x;
_pos_target.y = curr_pos.y + _pos_error.y;
}
//得到目标速度
_vel_target = sqrt_controller(_pos_error, kP, _accel_cms);
}
//目前速度前馈期望速度信息---- add velocity feed-forward
if(sky_droid_loiter_brake_run_start==1)
{
_sky_brake_fd_coefficient2=constrain_int16(_sky_brake_fd_coefficient2,0,5);
_vel_target.x += _vel_desired.x*_sky_brake_fd_coefficient2;
_vel_target.y += _vel_desired.y*_sky_brake_fd_coefficient2;
}
else
{
_vel_target.x += _vel_desired.x*1;
_vel_target.y += _vel_desired.y*1;
}
sky_droid_log_tar_vel_x=_vel_target.x;
sky_droid_log_tar_vel_y=_vel_target.y;
//the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
//下一节将横滚/俯仰方向上的期望速度转换为横向/纵向框架中的加速度
Vector2f accel_target, cal_accel_target,vel_xy_p, vel_xy_i, vel_xy_d;
//检查车速是否被超越----- check if vehicle velocity is being overridden
if (_flags.vehicle_horiz_vel_override)
{
_flags.vehicle_horiz_vel_override = false;
} else
{
_vehicle_horiz_vel.x = _inav.get_velocity().x;
_vehicle_horiz_vel.y = _inav.get_velocity().y;
}
//计算速度误差------- calculate velocity error
_vel_error.x = _vel_target.x - _vehicle_horiz_vel.x;
_vel_error.y = _vel_target.y - _vehicle_horiz_vel.y;
//如果该控制器刚刚启动,则重置速度误差并过滤
if (_flags.reset_accel_to_lean_xy)
{
//复位滤波------Reset Filter
_vel_error.x = 0;
_vel_error.y = 0;
_vel_error_filter.reset(0);
_flags.reset_accel_to_lean_xy = false;
} else
{
//calculate rate error and filter with cut off frequency of 2 Hz
//计算速率误差和滤波器，截止频率为4 Hz
_vel_error.x = _vel_error_filter.apply(_vel_error.x, _dt);
_vel_error.y = _vel_error_filter.apply(_vel_error.y, _dt);
}
//TODO：约束速度误差和速度目标---- TODO: constrain velocity error and velocity target
//调用PI控制器----- call pi controller
_pid_vel_xy.set_input(_vel_error);
//获取P参数---- get p
vel_xy_p = _pid_vel_xy.get_p();
//update i term if we have not hit the accel or throttle limits OR the i term will reduce
//TODO: move limit handling into the PI and PID controller
//如果未达到加速或油门限制，则更新i项，否则i项将减少
//TODO：将极限处理移到PI和PID控制器中
if (!_limit.accel_xy && !_motors.limit.throttle_upper)
{
//速度更新
vel_xy_i = _pid_vel_xy.get_i();
} else
{
vel_xy_i = _pid_vel_xy.get_i_shrink();
}
//获取d参数------- get d
vel_xy_d = _pid_vel_xy.get_d();
//acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
//用于校正速度误差的加速度和用于补偿光流测量引起的EKF噪声的比例PID输出
accel_target.x = (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler;
accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;
//增加前馈到目标加速度输出------ Add feed forward into the target acceleration output
if(sky_droid_loiter_brake_run_start==1)
{
//数据计算增加
sky_droid_log_count++;
if(sky_droid_log_count>60000)
{
sky_droid_log_count=0;
}
//系数被限制在0到5中间
_sky_brake_fd_coefficient=constrain_int16(_sky_brake_fd_coefficient,0,5);
accel_target.x += _accel_desired.x*_sky_brake_fd_coefficient;
accel_target.y += _accel_desired.y*_sky_brake_fd_coefficient;
}
else
{
sky_droid_log_count=0;
accel_target.x += _accel_desired.x*1;
accel_target.y += _accel_desired.y*1;
}
//获取xy的加速度信息
_vehicle_accel_cm.x=_inav.get_accel().x;
_vehicle_accel_cm.y=_inav.get_accel().y;
Vector2f accel_xy_p, accel_xy_i, accel_xy_d;
//计算误差
_accel_error.x=accel_target.x-_vehicle_accel_cm.x;
_accel_error.y=accel_target.y-_vehicle_accel_cm.y;
//////////////////////////////////////////////////////////
//调用PI控制器----- call pi controller
_pid_accel_xy.set_input(_accel_error);
//获取P参数---- get p
accel_xy_p = _pid_accel_xy.get_p();
//update i term if we have not hit the accel or throttle limits OR the i term will reduce
//TODO: move limit handling into the PI and PID controller
//如果未达到加速或油门限制，则更新i项，否则i项将减少
//TODO：将极限处理移到PI和PID控制器中
if (!_limit.accel_xy && !_motors.limit.throttle_upper)
{
//速度更新
accel_xy_i = _pid_accel_xy.get_i();
} else
{
accel_xy_i = _pid_accel_xy.get_i_shrink();
}
//获取d参数------- get d
accel_xy_d = _pid_accel_xy.get_d();
//最终得到要传递到加速度环的PID信息值
cal_accel_target.x = (accel_xy_p.x + accel_xy_i.x + accel_xy_d.x) * 1;
cal_accel_target.y = (accel_xy_p.y + accel_xy_i.y + accel_xy_d.y) * 1;
// //对xy加速度采用PID控制得到控制量信息
// cal_accel_target.x=_pid_accel_xy.update_all(accel_target.x,_vehicle_accel_cm.x,500);
// cal_accel_target.y=_pid_accel_xy.update_all(accel_target.y,_vehicle_accel_cm.y,500);
//reset accel to current desired acceleration
if (_flags.reset_accel_to_lean_xy)
{
_accel_target_filter.reset(Vector2f(cal_accel_target.x, cal_accel_target.y));
_flags.reset_accel_to_lean_xy = false;
}
_accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f * 1));
//进行数据滤波
_accel_target_filter.apply(cal_accel_target, dt);
//把滤波数据传递进去
_accel_target.x = _accel_target_filter.get().x;
_accel_target.y = _accel_target_filter.get().y;
// the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles
// limit acceleration using maximum lean angles
//下一节将机体坐标系下的目标加速度转换到地理坐标系下目标的横滚和俯仰角度
//使用最大倾斜角度限制加速度
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());
float accel_max = MIN(GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f)), POSCONTROL_ACCEL_XY_MAX);
_limit.accel_xy = limit_vector_length(_accel_target.x, _accel_target.y, accel_max);
sky_droid_log_accel_target=_accel_target.x;
// update angle targets that will be passed to stabilize controller
//更新目标角度信息,应当传递到姿态控制器
accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);
}
I don’t know. Do you understand me
Check the complete parameter list and see if any of the PSC parameters do what you need.
https://ardupilot.org/copter/docs/parameters.html
Replacing those already in place might be difficult and unwarranted.
control, is not parameters
Hi @lixiaowei,
There’s no acceleration layer in the position controller’s horizontal axis I’m afraid. Instead it directly converts the desired accelerations (which come from the horizontal velocity control output) into lean angles.
Could you tell me why you would like to add an acceleration control? Is it because there are additional thrusters on the vehicle that don’t point downwards like regular multicopter motors? If yes then it may be interesting to look at how PeterHall added support for 6DOF copters. Another possibility is that @Leonardthall is working on a change for 4.4 that will remove the built-in assumption that multicopters only have downward thrust.
Hi @lixiaowei,
So far it doesn’t look like we need to add an acceleration loop. The physics of a multirotor let use a simple lean angle conversion. It looks like you are experimenting with it., what have you found?
I found that the vertical direction was controlled by an acceleration loop, so I wanted to try whether the horizontal position was possible. As a result, the experimental effect was not ideal
Yes, I think you will struggle to improve on what we have. This doesn’t need to be very fast and sits on top of a bunch of other controllers. So before you can really even evaluate your work you need to make sure your attitude controllers and Alt_Hold controller is perfect. Also we make the assumption that alt_hold is keeping 1g vertical. So there are a lot of things to make this hard for you.
Just curious after reading this thread… so if you were to send a SET_POSITION_TARGET_LOCAL_NED mavlink message with a bitmask to ignore everything but position and acceleration (or everything but acceleration), what would it do?
@jeff567 I don’t understand your question but the answer is either “nothing” or “exactly what you tell it to do”
If you command:
- position
- position and velocity
- position, velocity and acceleration
- velocity
- velocity and acceleration
- acceleration
This will all work.
If you command:
position and acceleration
It won’t work.
thank you very much
*if the copter adapt position, velocity and acceleration control,Will the control effect be good？
If the aircraft is built, setup, and tuned well this gives the most precise and fastest control you can get.
At present, there is no PID control for acceleration in horizontal position control
True, what is your point?
Whether the acceleration can be PID controlled, and then the control output can be converted into angle
Yes, I know exactly what you mean, but you need to make more effort to communicate your idea
- why you want to add an Accel loop to xy?
- how do you think this will improve things?
- what do you think is not working properly with the current arrangement?
1 Like