基于横向轨迹误差法(Cross-track Error）P 导航二维控制 实现无人机水平面导航控制
【摘要】
基于横向轨迹误差法(Cross-track Error）P 导航二维控制 实现无人机水平面导航控制
算法核心思想算法实现方法实现无人机水平面导航控制算法优化方向
算法核心思想
首先我们的...
算法核心思想
首先我们的目的是控制被控对象，靠近目标轨迹并且沿着目标轨迹运行。在实际运行中被控目标可能受外界干扰，与目标轨迹存在一定偏差，运用横向轨迹误差法(Cross-track Error），通过目标轨迹与被控对象当前位置的距离，不断的计算调整被控对象的状态，使其不断的靠近目标轨迹。Cross-track Error的值越大，被控对象偏离原行驶方向的角度就越大，随着Cross-track Erro的减小，被控对象偏离原行驶方向的角度要不断减小，直至轨迹重合。由于 由 Cross-track Error 转换成 调整速度有PID—P控制实现得。所有此算法估且叫为 基于Cross-track Error 的P 导航二维控制。
单纯的Pcontroller会导致被控对象在目标轨迹附近波动前行，这种波动可能非常小，以至于在某些场景下我们可以忽略。
算法实现方法
如上图所示表明了什么是 crosstrack角度 和 crosstrack距离
实现方法就是通过三角函数关系 将 crosstrack距离 在x，y方向上进行分解 转换成纠正速度
实现无人机水平面导航控制
更新 由 crosstrack 计算出来的 大地坐标系下得 x y 方向 的 纠正 速度
#define CROSSTRACK_GAIN 0.4f
// 更新 由 crosstrack 计算出来的 大地坐标系下得 x y 方向 的 纠正 速度
void QuadrotorControl::Update_Nav_CrossTrack_Speed_To_Earth(double Angle_PreToNext_rad,double Angle_PreToCur_rad,double Distance_CurToPre)
{
// Angle_PreToNext_rad 上一航点到 下一航点的方位角
// Angle_PreToCur_rad 上一航点到 当前位置的方位角
// Angle_Nav_CrossTrack_Error_rad 偏离原始航线的 角度
double Angle_Nav_CrossTrack_Error_rad = Angle_PreToNext_rad - Angle_PreToCur_rad ;
// 计算偏航距的距离 用来记录 实际轨迹偏差
Nav_CrossTrack_Error_Distance_True_ = sin(Angle_Nav_CrossTrack_Error_rad) * Distance_CurToPre ;
// 被限幅得偏航距
// 最大偏航距为 5m，对应速度2m/s
double Nav_CrossTrack_Error_Distance_Constrain = Math_fConstrain(Nav_CrossTrack_Error_Distance_True_, -5.0f, 5.0f);
// 把偏航距的值 转化为 速度 再分解到 大地坐标系下
Nav_CrossTrack_Speed_To_Earth_x_ = -CROSSTRACK_GAIN * Nav_CrossTrack_Error_Distance_Constrain *sin(Angle_PreToNext_rad);
Nav_CrossTrack_Speed_To_Earth_y_ = CROSSTRACK_GAIN * Nav_CrossTrack_Error_Distance_Constrain *cos(Angle_PreToNext_rad);
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
计算 自动航线 时的 期望速度 （（水平方向） 大地坐标系下的 【加上了由crosstrack计算的纠正速度】
// 计算 自动航线 时的 期望速度 （（水平方向） 大地坐标系下的
void QuadrotorControl::calc_nav_rate(double max_speed )
{
Update_Nav_CrossTrack_Speed_To_Earth(Angle_PreToNext_rad_ ,Angle_PreToCur_rad_ ,Distance_CurToPre_);
// 把最大速度进行分解 xy 方向 然后加上 CrossTrack_Speed
double Desire_Velocity_x_world = cos(Angle_PreToNext_rad_) * max_speed + Nav_CrossTrack_Speed_To_Earth_x_;
double Desire_Velocity_y_world = sin(Angle_PreToNext_rad_) * max_speed + Nav_CrossTrack_Speed_To_Earth_y_;
//将世界坐标系下速度转化为 机体坐标系下的速度
double Desire_Velocity_x_body = Desire_Velocity_x_world * cos(UAV_yaw_* DEG2RAD) + Desire_Velocity_y_world * sin(UAV_yaw_* DEG2RAD);
double Desire_Velocity_y_body = Desire_Velocity_y_world * cos(UAV_yaw_* DEG2RAD) - Desire_Velocity_x_world * sin(UAV_yaw_* DEG2RAD);
UAV_VelocityControl(Desire_Velocity_x_body,Desire_Velocity_y_body,0,0);
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
自动飞向一点 水平方向 用CrossTrack算法
// 自动飞向一点 水平方向 用CrossTrack算法
/*
参数：
double Pose_Pre_x , Pose_Pre_y ; // 上一航点的 x，y位置
double Pose_Next_x, Pose_Next_y ; // 下一航点的 x，y位置
*/
bool QuadrotorControl::Fly_To_Point_CrossTrack_XY(double Pose_Pre_x,double Pose_Pre_y, double Pose_Next_x,double Pose_Next_y)
{
Cruise_Speed_Set_ = 3 ; //设定巡航速度
//最大飞行速度
double Max_Nav_Speed = 0; // m/s
//当前x，y 的位置
double Pose_Cur_x = UAV_Pose_.position.x ;
double Pose_Cur_y = UAV_Pose_.position.y ;
// 计算各角度
Angle_PreToCur_rad_ = atan2(Pose_Cur_y-Pose_Pre_y,Pose_Cur_x-Pose_Pre_x); // 上一航点到当前位置的方位角 以上一航点为原点
Angle_PreToNext_rad_ = atan2(Pose_Next_y-Pose_Pre_y,Pose_Next_x-Pose_Pre_x); // 上一航点到下一航点的方位角 以上一航点为原点
// 计算各距离
double D_x_temp = Pose_Cur_x - Pose_Pre_x ;
double D_y_temp = Pose_Cur_y - Pose_Pre_y ;
Distance_CurToPre_ = sqrtf(D_x_temp*D_x_temp+D_y_temp*D_y_temp);//当前点到上一航点得距离
D_x_temp = Pose_Next_x - Pose_Pre_x ;
D_y_temp = Pose_Next_y - Pose_Pre_y ;
Distance_NextToPre_ = sqrtf(D_x_temp*D_x_temp+D_y_temp*D_y_temp);//下一航点到上一航点得距离
//计算垂点和下 一个航点的距离
double Distance_VerticalToNext = Distance_NextToPre_ - Distance_CurToPre_*cos(Angle_PreToNext_rad_-Angle_PreToCur_rad_);
bool IsFinalPoint=1;//是否是最后一个航点 先做无刹车得
if (IsFinalPoint) //如果不是最后一个航点
{
//判断是否在转弯过程中
bool IsTurnHeading=0;
if(IsTurnHeading)//正在转弯 Stabilize_Yaw.target - Stabilize_Yaw.current 判断 //可能是协调转弯的
{
Max_Nav_Speed = Math_fmin(Cruise_Speed_Set_, 4);
}else{
Max_Nav_Speed = Cruise_Speed_Set_ ;
}
//由当前速度来限制最大速度 ，防止加速阔快 减速过快
double compound_speed = Math_fMagnitude_three( UAV_TwistBody_.linear.x , UAV_TwistBody_.linear.y , 0);
Max_Nav_Speed = compound_speed + Math_fConstrain((Max_Nav_Speed - compound_speed), -1,+1);
}else{ // 是最后一个航点
// 这一部分也可以作为 即将到点的刹车使用
if (Distance_VerticalToNext >= 30)
{
Max_Nav_Speed = Cruise_Speed_Set_;
}
if ((Distance_VerticalToNext > 20) &&
(Distance_VerticalToNext < 30))
{
Max_Nav_Speed = Math_fmin(4, Cruise_Speed_Set_);
}
else if (Distance_VerticalToNext <= 20)
{
Max_Nav_Speed =Math_fmin((Distance_VerticalToNext * 0.2f),Cruise_Speed_Set_);
}
//该加上高度处理部分， 先不做这一部分
}
// 根据 最终求得 最大飞行速度 结合crosstrack 得到实际 期望速度 x，y Nav_CrossTrack_Speed_To_Earth_x Nav_CrossTrack_Speed_To_Earth_y
calc_nav_rate(Max_Nav_Speed );
//判断是否到点
if(Distance_VerticalToNext<Math_fConstrain(Max_Nav_Speed * 3,0.1,0.3))
{
return true ;
}else{
return false ;
}
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
算法优化方向
- 单纯的Pcontroller会导致被控对象在目标轨迹附近波动前行，这种波动可能非常小，以至于在某些场景下我们可以忽略。 可以加入D控制 ，来减少波动。
- 上面是二维的，无人机可以实现3维运动，下篇博客将z方向加进去。
- 结束阶段，没有加入减速控制，和最后的结束精度还没有优化。
文章来源: blog.csdn.net，作者：月照银海似蛟龙，版权归原作者所有，如需转载，请联系作者。
原文链接：blog.csdn.net/qq_32761549/article/details/104654409
【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱：
cloudbbs@huaweicloud.com
- 点赞
- 收藏
- 关注作者
评论（0）