12345qiupeng
/
mpc_local_planner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added kinematic bicycle model with velocity input

master
Christoph Rösmann 2020-05-24 09:50:41 +02:00
parent aef38d0ccc
commit ec6a0c4406
4 changed files with 167 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
mpc_local_planner/cfg/test_mpc_optim_node.yaml
View File

@ -17,6 +17,15 @@ robot:
acc_lim_x: 0.0 # deactive bounds with zero
dec_lim_x: 0.0 # deactive bounds with zero
max_steering_rate: 0.5 # deactive bounds with zero
kinematic_bicycle_vel_input:
length_rear: 1.0
length_front: 1.0
max_vel_x: 0.4
max_vel_x_backwards: 0.2
max_steering_angle: 1.4
acc_lim_x: 0.0 # deactive bounds with zero
dec_lim_x: 0.0 # deactive bounds with zero
max_steering_rate: 0.5 # deactive bounds with zero
## Footprint model for collision avoidance
footprint_model:

110
mpc_local_planner/include/mpc_local_planner/systems/kinematic_bicycle_model.h Normal file
View File

@ -0,0 +1,110 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef SYSTEMS_KINEMATIC_BICYCLE_MODEL_H_
#define SYSTEMS_KINEMATIC_BICYCLE_MODEL_H_
#include <mpc_local_planner/systems/base_robot_se2.h>
#include <cmath>
namespace mpc_local_planner {
/**
* @brief Kinematic Bicycle Model with Velocity Input
*
* This class implements the dynamics for a kinematic bicycle model.
* Note, in this model the robot coordinate system is located at the center of gravity
* which is between the rear and the front axle.
* In case you want to define the coordinate system at the rear axle,
* please refer to the simplified equations simple_car.h (SimpleCarModel).
*
* [1] R. Rajamani, Vehicle Dynamics and Control, Springer, 2012.
* [2] J. Kong et al., Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design, IV, 2015.
*
* @see SimpleCarModel SimpleCarFrontWheelDrivingModel BaseRobotSE2 RobotDynamicsInterface
* corbo::SystemDynamicsInterface
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class KinematicBicycleModelVelocityInput : public BaseRobotSE2
{
public:
//! Default constructor
KinematicBicycleModelVelocityInput() = default;
//! Constructs model with given wheelbase
KinematicBicycleModelVelocityInput(double lr, double lf) : _lr(lr), _lf(lf) {}
// implements interface method
SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared<KinematicBicycleModelVelocityInput>(); }
// implements interface method
int getInputDimension() const override { return 2; }
// implements interface method
void dynamics(const Eigen::Ref<const StateVector>& x, const Eigen::Ref<const ControlVector>& u, Eigen::Ref<StateVector> f) const override
{
assert(x.size() == getStateDimension());
assert(u.size() == getInputDimension());
assert(x.size() == f.size() &&
"KinematicBicycleModelVelocityInput::dynamics(): x and f are not of the same size, do not forget to pre-allocate f.");
double beta = std::atan(_lr / (_lf + _lr) * std::tan(u[1]));
f[0] = u[0] * std::cos(x[2] + beta);
f[1] = u[0] * std::sin(x[2] + beta);
f[2] = u[0] * std::sin(beta) / _lr;
}
// implements interface method
bool getTwistFromControl(const Eigen::Ref<const Eigen::VectorXd>& u, geometry_msgs::Twist& twist) const override
{
assert(u.size() == getInputDimension());
twist.linear.x = u[0];
twist.linear.y = twist.linear.z = 0;
twist.angular.z = u[1]; // warning, this is the angle and not the angular vel
twist.angular.x = twist.angular.y = 0;
return true;
}
//! Set parameters
void setParameters(double lr, double lf)
{
_lr = lr;
_lf = lf;
}
//! Get length between COG and front axle
double getLengthFront() const { return _lf; }
//! Get length between COG and rear axle
double getLengthRear() const { return _lr; }
protected:
double _lr = 1.0;
double _lf = 1.0;
};
} // namespace mpc_local_planner
#endif // SYSTEMS_KINEMATIC_BICYCLE_MODEL_H_

4
mpc_local_planner/include/mpc_local_planner/systems/simple_car.h
View File

@ -44,8 +44,8 @@ namespace mpc_local_planner {
* in Robot Motion Planning and Control (Ed. J.-P. Laumond), Springer, 1998.
* (https://homepages.laas.fr/jpl/promotion/chap4.pdf)
*
* @see SimpleCarFrontWheelDrivingModel BaseRobotSE2 RobotDynamicsInterface
* corbo::SystemDynamicsInterface
* @see SimpleCarFrontWheelDrivingModel KinematicBicycleModelVelocityInput
* BaseRobotSE2 RobotDynamicsInterface corbo::SystemDynamicsInterface
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/

46
mpc_local_planner/src/controller.cpp
View File

@ -36,6 +36,7 @@
#include <mpc_local_planner/optimal_control/min_time_via_points_cost.h>
#include <mpc_local_planner/optimal_control/quadratic_cost_se2.h>
#include <mpc_local_planner/optimal_control/stage_inequality_se2.h>
#include <mpc_local_planner/systems/kinematic_bicycle_model.h>
#include <mpc_local_planner/systems/simple_car.h>
#include <mpc_local_planner/systems/unicycle_robot.h>
#include <mpc_local_planner/utils/time_series_se2.h>
@ -359,6 +360,14 @@ RobotDynamicsInterface::Ptr Controller::configureRobotDynamics(const ros::NodeHa
else
return std::make_shared<SimpleCarModel>(wheelbase);
}
else if (_robot_type == "kinematic_bicycle_vel_input")
{
double length_rear = 1.0;
nh.param("robot/kinematic_bicycle_vel_input/length_rear", length_rear, length_rear);
double length_front = 1.0;
nh.param("robot/kinematic_bicycle_vel_input/length_front", length_front, length_front);
return std::make_shared<KinematicBicycleModelVelocityInput>(length_rear, length_front);
}
else
{
ROS_ERROR_STREAM("Unknown robot type '" << _robot_type << "' specified.");
@ -516,6 +525,22 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::
ocp->setControlBounds(Eigen::Vector2d(-max_vel_x_backwards, -max_steering_angle), Eigen::Vector2d(max_vel_x, max_steering_angle));
}
else if (_robot_type == "kinematic_bicycle_vel_input")
{
double max_vel_x = 0.4;
nh.param("robot/kinematic_bicycle_vel_input/max_vel_x", max_vel_x, max_vel_x);
double max_vel_x_backwards = 0.2;
nh.param("robot/kinematic_bicycle_vel_input/max_vel_x_backwards", max_vel_x_backwards, max_vel_x_backwards);
if (max_vel_x_backwards < 0)
{
ROS_WARN("max_vel_x_backwards must be >= 0");
max_vel_x_backwards *= -1;
}
double max_steering_angle = 1.5;
nh.param("robot/kinematic_bicycle_vel_input/max_steering_angle", max_steering_angle, max_steering_angle);
ocp->setControlBounds(Eigen::Vector2d(-max_vel_x_backwards, -max_steering_angle), Eigen::Vector2d(max_vel_x, max_steering_angle));
}
else
{
ROS_ERROR_STREAM("Cannot configure OCP for unknown robot type " << _robot_type << ".");
@ -724,6 +749,27 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::
Eigen::Vector2d ud_ub(acc_lim_x, max_steering_rate);
_inequality_constraint->setControlDeviationBounds(ud_lb, ud_ub);
}
else if (_robot_type == "kinematic_bicycle_vel_input")
{
double acc_lim_x = 0.0;
nh.param("robot/kinematic_bicycle_vel_input/acc_lim_x", acc_lim_x, acc_lim_x);
double dec_lim_x = 0.0;
nh.param("robot/kinematic_bicycle_vel_input/dec_lim_x", dec_lim_x, dec_lim_x);
if (dec_lim_x < 0)
{
ROS_WARN("dec_lim_x must be >= 0");
dec_lim_x *= -1;
}
double max_steering_rate = 0.0;
nh.param("robot/kinematic_bicycle_vel_input/max_steering_rate", max_steering_rate, max_steering_rate);
if (acc_lim_x <= 0) acc_lim_x = corbo::CORBO_INF_DBL;
if (dec_lim_x <= 0) dec_lim_x = corbo::CORBO_INF_DBL;
if (max_steering_rate <= 0) max_steering_rate = corbo::CORBO_INF_DBL;
Eigen::Vector2d ud_lb(-dec_lim_x, -max_steering_rate);
Eigen::Vector2d ud_ub(acc_lim_x, max_steering_rate);
_inequality_constraint->setControlDeviationBounds(ud_lb, ud_ub);
}
else
{
ROS_ERROR_STREAM("Cannot configure control deviation bounds for unknown robot type " << _robot_type << ".");