前言
无人机(Unmanned Aerial Vehicle),指的是一种由动力驱动的、无线遥控或自主飞行、机上无人驾驶并可重复使用的飞行器,飞机通过机载的计算机系统自动对飞行的平衡进行有效的控制,并通过预先设定或飞机自动生成的复杂航线进行飞行,并在飞行过程中自动执行相关任务和异常处理。
在前面的博客中,分析了 rotors_simulator
一个开源的无人机gazebo的仿真系统的一个控制接口(roll、pitch、yawrate、thrust),并通过键盘发布控制指令,使飞机飞了起来,但是真正实验过的人则知道,起控制会飞常难,需要一直调整键盘,稍微一不注意,无人机就飞走了。
其原因就是这个接口在无人机内部并没有位置控制的闭环。
在这篇文章中,分析了自动控制原理;并在这篇文章中分析了无人机各种模式的控制框图。
本篇博客主要就是基于无人机的控制原理与控制框图,基于PID控制器,利用rotors_simulator
的控制接口,实现无人机的位置控制。
其中在前两篇博客中已经实现了 高度和水平位置控制,本篇博客在其基础上继续实现最后的航向控制。
航向控制
无人机航向角度的控制框图如下:
由于rotors_simulator
选择的接口是 yawrate 所以仅需要做最外层的 外环角度控制器 输出 期望航向角速度即可。
P控制
// 航向角度控制
void PidPositionControllerNode::YawControl(double yaw_des_degree)
{
// 期望航向角度 单位度
double yaw_des_degree = 30;
// 将期望航向角转为 弧度单位
double yaw_des_rad = yaw_des_degree * RADIAN ;
// 获取无人机航向角度
// 获得无人机的姿态旋转矩阵
Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix();
// 根据旋转矩阵求得yaw角 单位rad
double yaw_cur_rad = atan2(R(1, 0), R(0, 0));
// 求得通过角度偏差 求得期望角速度 简单p控制
double yaw_rate_des_rad = 2 * (yaw_des_rad - yaw_cur_rad);
// 限制最大旋转角速度 单位弧度
double max_yaw_rate = 120 * RADIAN;
Math_doubleConstrain(yaw_rate_des_rad,-max_yaw_rate,max_yaw_rate);
des_yawrate_ = yaw_rate_des_rad ;
}
期望位置先固定为30度
控制效果如下:
收敛结果
收敛过程
经过4次震荡最终收敛
最大超调量为:0.262 弧度 相当于15度
收敛时间:26s
PD控制
由于稳态误差不大,存在较大超调,和收敛时间,选择加入微分环节
float PidPositionControllerNode::yaw_pid_controller(float pv,float sp)
{
// 控制器相关参数 根据被控对象要调整的量
// 控制参数
float Kp = 2 , Ki = 0 ,Kd = 0;
// 输出限幅
float max_output_pid = 120 , min_output_pid = -120 ;
// 积分输出限幅
float max_output_i = 10,min_output_i = -10 ;
// 控制器 偏差量
static float error = 0,error_last=0,error_last_last=0;
// 控制器输出量声明
static float output_p,output_i,output_d,output_pid;
// 偏差计算
error = sp - pv ;
// 控制器 各环节 输出 计算
output_p += ( Kp * (error - error_last) );
output_i += ( Ki * (error) );
output_d += ( Kd * (error - 2*error_last + error_last_last) );
// 更新偏差量
error_last_last = error_last ;
error_last = error ;
// 积分输出限幅
if(output_i>max_output_i* RADIAN)
{
// 积分输出限制输出最大值
output_i = max_output_i* RADIAN;
}else if(output_i<min_output_i* RADIAN)
{
// 积分输出限制输出最小值
output_i = min_output_i* RADIAN;
}
// 汇总输出
output_pid = output_p + output_i + output_d;
// std::cout<< "比例输出 : "<<output_p<<"|||" <<"微分输出"<<output_d<<std::endl;
// 输出限幅
if(output_pid>max_output_pid* RADIAN)
{
// 输出限制输出最大值
output_pid = max_output_pid* RADIAN;
}else if(output_pid<min_output_pid* RADIAN)
{
// 输出限制输出最小值
output_pid = min_output_pid* RADIAN;
}
// 控制器最终输出
return output_pid;
}
控制参数整定的思想如下:
增大比例系数P将加快系统的响应,它的作用于输出值较快,但不能很好稳定在一个理想的数值,不良的结果是虽较能有效的克服扰动的影响,但有余差出现,过大的比例系数会使系统有比较大的超调,并产生振荡,使稳定性变坏。积分能在比例的基础上消除余差,它能对稳定后有累积误差的系统进行误差修整,减小稳态误差。微分具有超前作用,对于具有容量滞后的控制通道,引入微分参与控制,在微分项设置得当的情况下,对于提高系统的动态性能指标,有着显著效果,它可以使系统超调量减小,稳定性增加,动态误差减小。
// 航向角度控制
void PidPositionControllerNode::YawControl(double yaw_des_degree)
{
// 期望航向角度 单位度
//double yaw_des_degree = 30;
// 将期望航向角转为 弧度单位
double yaw_des_rad = yaw_des_degree * RADIAN ;
// 先简单通过 比例 做一个航向保持
// 获取无人机航向角度
// 获得无人机的姿态旋转矩阵
Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix();
// 根据旋转矩阵求得yaw角 单位rad
double yaw_cur_rad = atan2(R(1, 0), R(0, 0));
// 求得通过角度偏差 求得期望角速度 简单p控制
// double yaw_rate_des_rad = 2 * (yaw_des_rad - yaw_cur_rad);
// 通过角度偏差 求得期望角速度 pid控制
double yaw_rate_des_rad = yaw_pid_controller(yaw_cur_rad,yaw_des_rad);
// 限制最大旋转角速度 单位弧度
double max_yaw_rate = 120 * RADIAN;
Math_doubleConstrain(yaw_rate_des_rad,-max_yaw_rate,max_yaw_rate);
des_yawrate_ = yaw_rate_des_rad ;
}
收敛结果
收敛过程
一次震荡即可收敛
超调量: 0.038弧度 即 2.23度
收敛时间为: 10s
结果总结
将两个航向值的变化曲线放在一起,明显串级PID控制器的效果更优
一次震荡即可收敛
超调量: 0.038弧度 即 2.23度
收敛时间为: 10s
至此无人机的位置控制即完成了。可以通过最终的控制函数
void PidPositionControllerNode::POSXYZYawControl(double pos_x_des,double pos_y_des,double pos_z_des,double yaw_des_degree)
完成 水平位置、垂直位置、航向角度的 空间位置控制。
评论(0)
您还未登录,请登录后发表或查看评论