Skip to content

Commit

Permalink
update model-based pid, very powerful
Browse files Browse the repository at this point in the history
  • Loading branch information
@ZhanyuGuo
ZhanyuGuo committed Jan 20, 2024
1 parent c67b16b commit 6881eac
Show file tree
Hide file tree
Showing 4 changed files with 69 additions and 42 deletions.
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ For the efficient operation of the motion planning system, we provide a series o

| Planner | Version | Animation |Paper
|:-------:|:-------:|:---------:|:---------:|
| **PID** | [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/pid_planner/src/pid_planner.cpp) | ![pid_ros.gif](assets/pid_ros.gif) | -
| **PID** | [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/pid_planner/src/pid_planner.cpp) | ![pid_ros.gif](assets/pid_ros.gif) | [Mapping Single-Integrator Dynamics to Unicycle Control Commands](https://liwanggt.github.io/files/Robotarium_CSM_Impact.pdf) p. 14
| **LQR** | [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/lqr_planner/src/lqr_planner.cpp) | ![lqr_ros.gif](assets/lqr_ros.gif) |-
| **DWA** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/dwa_planner/src/dwa.cpp) | ![dwa_ros.gif](assets/dwa_ros.gif) |[The Dynamic Window Approach to Collision Avoidance](https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf)
| **APF** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/apf_planner/src/apf_planner.cpp) | ![apf_ros.gif](assets/apf_ros.gif)|[Real-time obstacle avoidance for manipulators and mobile robots](https://ieeexplore.ieee.org/document/1087247)
Expand Down
1 change: 1 addition & 0 deletions src/core/local_planner/pid_planner/include/pid_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,7 @@ class PIDPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanne
double k_v_p_, k_v_i_, k_v_d_;
double k_w_p_, k_w_i_, k_w_d_;
double k_theta_;
double k_, l_;

double e_v_, e_w_;
double i_v_, i_w_;
Expand Down
105 changes: 64 additions & 41 deletions src/core/local_planner/pid_planner/src/pid_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,9 @@ void PIDPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::C
nh.param("k_w_i", k_w_i_, 0.01);
nh.param("k_w_d", k_w_d_, 0.10);
nh.param("k_theta", k_theta_, 0.5);
nh.param("k", k_, 1.0);
nh.param("l", l_, 0.2);

e_v_ = i_v_ = 0.0;
e_w_ = i_w_ = 0.0;

Expand Down Expand Up @@ -233,8 +236,9 @@ bool PIDPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
Eigen::Vector3d s_d(target_ps_map.pose.position.x, target_ps_map.pose.position.y, theta_d); // desired state
Eigen::Vector2d u_r(vt, wt); // refered input
Eigen::Vector2d u = _pidControl(s, s_d, u_r);
cmd_vel.linear.x = u[0];
cmd_vel.angular.z = u[1];

cmd_vel.linear.x = linearRegularization(base_odom, u[0]);
cmd_vel.angular.z = angularRegularization(base_odom, u[1]);
}

// publish next target_ps_map pose
Expand All @@ -257,57 +261,76 @@ bool PIDPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
* @param u_r refered input
* @return u control vector
*/
Eigen::Vector2d PIDPlanner::_pidControl(Eigen::Vector3d s, Eigen::Vector3d s_d, Eigen::Vector2d u_r)
{
Eigen::Vector2d u;
Eigen::Vector3d e = s_d - s;
// Eigen::Vector2d PIDPlanner::_pidControl(Eigen::Vector3d s, Eigen::Vector3d s_d, Eigen::Vector2d u_r)
// {
// Eigen::Vector2d u;
// Eigen::Vector3d e = s_d - s;

// double e_x = e[0];
// double e_y = e[1];
// double e_theta = e[2];

double e_x = e[0];
double e_y = e[1];
double e_theta = e[2];
// double v_d = std::hypot(e_x, e_y) / d_t_;
// double w_d = e_theta / d_t_;

double v_d = std::hypot(e_x, e_y) / d_t_;
double w_d = e_theta / d_t_;
// if (std::fabs(v_d) > max_v_)
// v_d = std::copysign(max_v_, v_d);
// if (std::fabs(w_d) > max_w_)
// w_d = std::copysign(max_w_, w_d);

if (std::fabs(v_d) > max_v_)
v_d = std::copysign(max_v_, v_d);
if (std::fabs(w_d) > max_w_)
w_d = std::copysign(max_w_, w_d);
// double e_v = v_d - u_r[0];
// double e_w = w_d - u_r[1];

double e_v = v_d - u_r[0];
double e_w = w_d - u_r[1];
// i_v_ += e_v * d_t_;
// i_w_ += e_w * d_t_;

i_v_ += e_v * d_t_;
i_w_ += e_w * d_t_;
// double d_v = (e_v - e_v_) / d_t_;
// double d_w = (e_w - e_w_) / d_t_;

double d_v = (e_v - e_v_) / d_t_;
double d_w = (e_w - e_w_) / d_t_;
// e_v_ = e_v;
// e_w_ = e_w;

e_v_ = e_v;
e_w_ = e_w;
// double v_inc = k_v_p_ * e_v + k_v_i_ * i_v_ + k_v_d_ * d_v;
// double w_inc = k_w_p_ * e_w + k_w_i_ * i_w_ + k_w_d_ * d_w;

double v_inc = k_v_p_ * e_v + k_v_i_ * i_v_ + k_v_d_ * d_v;
double w_inc = k_w_p_ * e_w + k_w_i_ * i_w_ + k_w_d_ * d_w;
// if (std::fabs(v_inc) > max_v_inc_)
// v_inc = std::copysign(max_v_inc_, v_inc);
// if (std::fabs(w_inc) > max_w_inc_)
// w_inc = std::copysign(max_w_inc_, w_inc);

if (std::fabs(v_inc) > max_v_inc_)
v_inc = std::copysign(max_v_inc_, v_inc);
if (std::fabs(w_inc) > max_w_inc_)
w_inc = std::copysign(max_w_inc_, w_inc);
// double v_cmd = u_r[0] + v_inc;
// if (std::fabs(v_cmd) > max_v_)
// v_cmd = std::copysign(max_v_, v_cmd);
// else if (std::fabs(v_cmd) < min_v_)
// v_cmd = std::copysign(min_v_, v_cmd);

double v_cmd = u_r[0] + v_inc;
if (std::fabs(v_cmd) > max_v_)
v_cmd = std::copysign(max_v_, v_cmd);
else if (std::fabs(v_cmd) < min_v_)
v_cmd = std::copysign(min_v_, v_cmd);
// double w_cmd = u_r[1] + w_inc;
// if (std::fabs(w_cmd) > max_w_)
// w_cmd = std::copysign(max_w_, w_cmd);
// else if (std::fabs(w_cmd) < min_w_)
// w_cmd = std::copysign(min_w_, w_cmd);

double w_cmd = u_r[1] + w_inc;
if (std::fabs(w_cmd) > max_w_)
w_cmd = std::copysign(max_w_, w_cmd);
else if (std::fabs(w_cmd) < min_w_)
w_cmd = std::copysign(min_w_, w_cmd);
// u[0] = v_cmd;
// u[1] = w_cmd;

u[0] = v_cmd;
u[1] = w_cmd;
// return u;
// }

/**
* @brief Execute PID control process (with model)
* @param s current state
* @param s_d desired state
* @param u_r refered input
* @return u control vector
*/
Eigen::Vector2d PIDPlanner::_pidControl(Eigen::Vector3d s, Eigen::Vector3d s_d, Eigen::Vector2d u_r)
{
Eigen::Vector2d u;
Eigen::Vector3d e(s_d - s);
Eigen::Vector2d sx_dot(k_ * e[0], k_ * e[1]);
Eigen::Matrix2d R_inv;
R_inv << cos(s[2]), sin(s[2]), -sin(s[2]) / l_, cos(s[2]) / l_;
u = R_inv * sx_dot;

return u;
}
Expand Down
3 changes: 3 additions & 0 deletions src/sim_env/config/planner/pid_planner_params.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,9 @@ PIDPlanner:

k_theta: 0.7

k: 1.0
l: 0.2

# lookahead
lookahead_time: 0.5
min_lookahead_dist: 0.3
Expand Down

0 comments on commit 6881eac

Please sign in to comment.