路径跟踪算法---Stanley Method实现
前言
Stanley Controller也是基于几何追踪的轨迹跟踪控制器,和Pure Pursuit不同的是,其基于前轮中心点为参考点进行控制,没有预瞄距离,以前轮中心点与最近参考轨迹点进行横向误差与导航角误差的计算。
一、Stanley原理介绍
如图,黑色曲线表示预设路径,红色方框表示车辆当前运动状态,偏离预设路径,使用Stanley Controller的目的是让车辆追踪预设路径,通过控制车的转向,使其靠近预设路径,图中黑色方框代表期望状态。图中红色箭头表示车辆实际运动方向,黑色箭头表示期望车辆运动方向,绿色虚线表示距离车辆最近路径点的切线,e_y 表示车与路径的最短距离,即横向偏差,θ e 为切线与实际运动方向的夹角,即航向偏差,δ e 表示期望运动方向与切线的夹角。
假定车辆实际位置在预设路径上,此时没有横向偏差,只需要转过 θe 角度即可,期望车辆转过的角度为:
δ
=
θ
e
θ
e
=
θ
r
−
θ
x
\ δ = θe \\ θe = θr - θx
δ=θeθe=θr−θx
假定车辆实际运动方向与切线方向一致,此时没有航向偏差,只有横向偏差。车辆当前速度为v,t时间内运动距离为d,和速度存在一个系数k。
δ
=
δ
e
d
=
v
/
k
t
a
n
δ
e
=
e
y
/
d
=
k
∗
e
y
/
v
δ
e
=
t
a
n
−
1
(
k
e
y
/
v
)
\ δ = δe \\ d = v / k \\tan δ e = e_y / d = k *e_y / v \\ δ e = tan^{-1}(k e_y/v)
δ=δed=v/ktanδe=ey/d=k∗ey/vδe=tan−1(key/v)
同时,转向角不能任意,需要进行限幅处理,此时期望车转过的角度为(加入时间变化):
δ
(
t
)
=
δ
e
(
t
)
+
θ
e
(
t
)
=
θ
e
(
t
)
+
t
a
n
−
1
(
k
e
y
(
t
)
/
v
(
t
)
)
,
δ
(
t
)
∈
[
δ
m
i
n
,
δ
m
a
x
]
\ δ (t) = δ e(t) + θe(t) = θe(t) + tan^{-1}(k e_y(t)/v(t) ),δ (t) ∈ [δmin,δmax]
δ(t)=δe(t)+θe(t)=θe(t)+tan−1(key(t)/v(t)),δ(t)∈[δmin,δmax]
二、关键代码实现
作为测试,将获取到的导航角偏差 δ ×一个比例系数作为角速度,预设的速度 v 作为线速度发送到底盘进行控制,因为用的模型是差速模型,没有进行角度限幅处理,实际想达到好的实验结果,可能需要将导航角偏差送到 pid 控制器进行实际的速度计算。代码参考知乎:Raiden
using path_type = std::pair<std::pair<double, double>, double>;
//用amcl定位获取机器人位姿效果不好,故采用odom位姿,也可用其他定位方式
void StanleyController::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg){
// yaw =tf::getYaw(msg->pose.pose.orientation);
// tf::pointMsgToTF(msg->pose.pose.position,current_pose);
}
void StanleyController::OdomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg) {
yaw =tf::getYaw(odom_msg->pose.pose.orientation);
tf::pointMsgToTF(odom_msg->pose.pose.position,current_pose);
}
//两点间距离
auto StanleyController::distance(path_type p1,path_type p2) -> double{
return sqrt(pow(p1.first.first-p2.first.first,2) +
pow(p1.first.second-p2.first.second,2));
}
//最近距离路径点索引
auto StanleyController::GetMinDisIndex(path_type current_pose, std::vector<path_type> path) -> int{
double min_dis = INFINITY;
int idx = 0;
for(int i = 0;i<path.size();i++){
double dis = distance(path[i],current_pose);
if(dis < min_dis){
min_dis = dis;
idx = i;
}
}
return idx;
}
void StanleyController::robotPathCallBack(const nav_msgs::PathConstPtr& msg){
if(recv_path_flag){
nav_path_.poses = msg->poses;
recv_path_flag = false;
start_pose_.position.x = nav_path_.poses[0].pose.position.x;
start_pose_.position.y = nav_path_.poses[0].pose.position.y;
path_vector.clear();
for(int i = 0;i<nav_path_.poses.size();i++){
path_type temp_path = std::make_pair(std::make_pair(nav_path_.poses[i].pose.position.x,nav_path_.poses[i].pose.position.y),
tf::getYaw(nav_path_.poses[i].pose.orientation));
path_vector.push_back(temp_path);
}
}
}
void StanleyController::run(){
cmd_vel_pub = n_.advertise<geometry_msgs::Twist>("/cmd_vel",1);
pose_sub = n_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",10,
std::bind(&StanleyController::robotPoseCallBack,this,std::placeholders::_1));
path_sub = n_.subscribe<nav_msgs::Path>("/rc_path_pub",10,std::bind(&StanleyController::robotPathCallBack,this,std::placeholders::_1));
real_path_pub = n_.advertise<nav_msgs::Path>("rvizpath", 100, true);
odom_pose_sub = n_.subscribe<nav_msgs::Odometry>("odom", 10,
std::bind(&StanleyController::OdomCallback, this, std::placeholders::_1));
ros::Rate loop_rate(10);
recv_path_flag = true;
path.header.frame_id = "/map";
path.header.stamp = ros::Time::now();
path_type init_pose = std::make_pair(std::make_pair(0.0,0.0),0.0);
path_type now_pose = init_pose;
while(ros::ok()){
now_pose.first.first = current_pose.x();
now_pose.first.second = current_pose.y();
now_pose.second = yaw;
//判断获取到路径
if(path_vector.size()>0){
int current_idx = GetMinDisIndex(now_pose,path_vector);
path_type cur_path_point = path_vector[current_idx];
//横向偏差
double e_y = distance(now_pose,cur_path_point);
e_y = (now_pose.first.second - cur_path_point.first.second) * cos(cur_path_point.second) -
(now_pose.first.first - cur_path_point.first.first) * sin(cur_path_point.second) <=0 ? e_y : -e_y;
//航向偏差,转到-PI 到 +PI之间
double theta_e = cur_path_point.second -now_pose.second ; //参考点航向 - 自车航向角
if(theta_e > M_PI){
theta_e = theta_e - 2* M_PI;
} else if(theta_e < -M_PI){
theta_e = theta_e + 2* M_PI;
}
//转角,限幅
double delta =( theta_e + atan2(factor * e_y, speed) );
// if(delta > M_PI/3){
// delta = M_PI/3;
// } else if(delta < -M_PI/3){
// delta = -M_PI/3;
// }
//直接把转角当作角速度
vel.linear.x = speed;
vel.angular.z = delta * 2.5;
cmd_vel_pub.publish(vel);
}
ros::spinOnce();
loop_rate.sleep();
}
}
三、效果
最后,放一张效果图~~~
原文地址:https://blog.csdn.net/weixin_44126988/article/details/140109595
免责声明:本站文章内容转载自网络资源,如本站内容侵犯了原著者的合法权益,可联系本站删除。更多内容请关注自学内容网(zxcms.com)!