环境构建参考我的另一篇文章
关于MPC与无人驾驶的理解参考文章
针对轨迹跟踪ref_trajectory_tracking_gazebo.launch及其所涉及的代码详解
1. ref_trajectory_tracking_gazebo.launch
1.1 加载机器人模型
<!-- ************** Robot model *************** -->
<param name="robot_description" command="$(find xacro)/xacro $(find servingbot_description)/urdf/servingbot.urdf.xacro" if="$(eval model == 'serving_bot')"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find gazebo_ros)/launch/empty_world.launch"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
</include>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model servingbot -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw) -param robot_description" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="50.0" />
</node>
1.2 生成参考轨迹
参考轨迹是由两个python文件生成的,主要任务就是根据给定的图形选项生成路径。trajectory_type为图形选项。
<!-- ************** Reference trajectory generation ************** -->
<node name="mpc_trajectory_generation" pkg="mpc_ros" type="mpc_trajectory_generation.py" if="$(eval controller == 'mpc')">
<param name="trajectory_type" value="$(arg trajectory_type)" />
</node>
<node name="dwa_trajectory_generation" pkg="mpc_ros" type="dwa_trajectory_generation.py" if="$(eval controller == 'dwa')">
<param name="trajectory_type" value="$(arg trajectory_type)" />
</node>
1.3 MPC节点
启动MPC节点,并加载了机器人运行参数。可以看到启动的节点名称为tracking_reference_trajectory。
<!-- ************** MPC Node ************** -->
<node name="MPC_tracking" pkg="mpc_ros" type="tracking_reference_trajectory" output="screen" if="$(eval controller == 'mpc')" >
<rosparam file="$(find mpc_ros)/params/mpc_local_square_params.yaml" command="load" />
<!--rosparam file="$(find mpc_ros)/params/mpc_local_params.yaml" command="load" /-->
<!--rosparam file="$(find mpc_ros)//params/mpc_local_epitrochoid_params.yaml" command="load" /-->
</node>
1.3.1 控制参数
其中重要的参数有max_speed最大速度,path_length轨迹跟踪参与计算的长度(并不是把整条轨迹都参与计算了),controller_freq计算频率
# Parameters for control loop
pub_twist_cmd: true
debug_info: false
delay_mode: true
max_speed: 0.8 # unit: m/s
waypoints_dist: -1.0 # unit: m, set < 0 means computed by node
path_length: 3.0 # unit: m
goal_radius: 0.5 # unit: m
controller_freq: 10
1.3.2 MPC参数
包括mpc_steps步长,mpc_ref_vel期望的参考速度等
# Parameter for MPC solver
mpc_steps: 20.0
mpc_ref_cte: 0.0
mpc_ref_vel: 0.5
mpc_ref_etheta: 0.0
mpc_w_cte: 2000.0
mpc_w_etheta: 1000.0
mpc_w_vel: 2000.0
mpc_w_angvel: 0.0
mpc_w_angvel_d: 10.0
mpc_w_accel: 10.0
mpc_w_accel_d: 10.0
mpc_max_angvel: 1.0
mpc_max_throttle: 0.5 # Maximal throttle accel
mpc_bound_value: 1.0e3 # Bound value for other variables
2 CmakeLists
根据1.3主要使用的节点为tracking_reference_trajectory,定位到CmakeLists。可知该节点主要是trackRefTraj.cpp和trackRefTrajNode.cpp两个文件。
ADD_EXECUTABLE( tracking_reference_trajectory src/trackRefTraj.cpp src/trackRefTrajNode.cpp )
TARGET_LINK_LIBRARIES(tracking_reference_trajectory ipopt ${catkin_LIBRARIES} )
3 C++代码
相关的c++代码都在trackRefTraj.cpp和trackRefTrajNode.cpp两个文件中。
3.1 main函数
main函数非常简单,主要就是启动MPCNode
int main(int argc, char **argv)
{
//Initiate ROS
ros::init(argc, argv, "MPC_Node");
MPCNode mpc_node;
ROS_INFO("Waiting for global path msgs ~");
ros::AsyncSpinner spinner(mpc_node.get_thread_numbers()); // Use multi threads
spinner.start();
ros::waitForShutdown();
return 0;
}
3.2 订阅话题及回调函数
odomCB,接收外部的odom数据
pathCB,为空,没有任何执行
desiredPathCB,将接收到的参考轨迹转化到odom坐标系下(重要)
goalCB,接收到终点回调函数,接收到终点后才会进行计算
amclCB,定位回调函数
_sub_odom = _nh.subscribe("/odom", 1, &MPCNode::odomCB, this);
_sub_path = _nh.subscribe( _globalPath_topic, 1, &MPCNode::pathCB, this);
_sub_gen_path = _nh.subscribe( "desired_path", 1, &MPCNode::desiredPathCB, this);
_sub_goal = _nh.subscribe( _goal_topic, 1, &MPCNode::goalCB, this);
_sub_amcl = _nh.subscribe("/amcl_pose", 5, &MPCNode::amclCB, this);
_controller_freq,控制函数,周期调用(重要)
//Timer
_timer1 = _nh.createTimer(ros::Duration((1.0)/_controller_freq), &MPCNode::controlLoopCB, this); // 10Hz //*****mpc
3.3 desiredPathCB
这一函数有几个关键点
3.3.1 设置轨迹离当前点最近处为起点,编号为min_idx
for(int i = min_idx; i < N; i++)
{
dx = totalPathMsg->poses[i].pose.position.x - px;
dy = totalPathMsg->poses[i].pose.position.y - py;
tf::Quaternion q(
totalPathMsg->poses[i].pose.orientation.x,
totalPathMsg->poses[i].pose.orientation.y,
totalPathMsg->poses[i].pose.orientation.z,
totalPathMsg->poses[i].pose.orientation.w);
tf::Matrix3x3 m(q);
m.getRPY(roll, pitch, yaw);
if(abs(pre_yaw - yaw) > 5)
{
cout << "abs(pre_yaw - yaw)" << abs(pre_yaw - yaw) << endl;
pre_yaw = yaw;
}
if(min_val > sqrt(dx*dx + dy*dy) && abs((int)(i - min_idx)) < 50)
{
min_val = sqrt(dx*dx + dy*dy);
min_idx = i;
}
}
3.3.2 参考轨迹的长度不超过最大设定,设定值为1.3.1中的path_length。
for(int i = min_idx; i < N ; i++)
{
if(total_length > _pathLength)
break;
_tf_listener.transformPose(_odom_frame, ros::Time(0) ,
totalPathMsg->poses[i], _odom_frame, tempPose);
mpc_path.poses.push_back(tempPose);
total_length = total_length + _waypointsDist;
}
这里_waypointsDist为固定值,默认相邻两点间距都是一样的
if(_waypointsDist <= 0.0)
{
double gap_x = totalPathMsg->poses[1].pose.position.x - totalPathMsg->poses[0].pose.position.x;
double gap_y = totalPathMsg->poses[1].pose.position.y - totalPathMsg->poses[0].pose.position.y;
_waypointsDist = sqrt(gap_x*gap_x + gap_y*gap_y);
}
3.4 _controller_freq
3.4.1 曲线拟合
将odom坐标系下的路径拟合为多项式
for(int i = 0; i < N; i++)
{
const double dx = odom_path.poses[i].pose.position.x - px;
const double dy = odom_path.poses[i].pose.position.y - py;
x_veh[i] = dx * costheta + dy * sintheta;
y_veh[i] = dy * costheta - dx * sintheta;
}
// Fit waypoints
auto coeffs = polyfit(x_veh, y_veh, 3);
3.4.2 延迟
px_act, py_act, theta_act三项为MPC的起点约束,约束设为dt时间后即考虑延迟时间dt
if(_delay_mode)
{
// Kinematic model is used to predict vehicle state at the actual moment of control (current time + delay dt)
const double px_act = v * dt;
const double py_act = 0;
const double theta_act = w * dt; //(steering) theta_act = v * steering * dt / Lf;
const double v_act = v + throttle * dt; //v = v + a * dt
const double cte_act = cte + v * sin(etheta) * dt;
const double etheta_act = etheta - theta_act;
state << px_act, py_act, theta_act, v_act, cte_act, etheta_act;
}
else
{
state << 0, 0, 0, v, cte, etheta;
}
3.4.3 MPC求解
// Solve MPC Problem
vector<double> mpc_results = _mpc.Solve(state, coeffs);
设定IPOPT迭代的初始值
vars[_x_start] = x;
vars[_y_start] = y;
vars[_theta_start] = theta;
vars[_v_start] = v;
vars[_cte_start] = cte;
vars[_etheta_start] = etheta;
设定等式约束
for (int i = 0; i < n_constraints; i++)
{
constraints_lowerbound[i] = 0;
constraints_upperbound[i] = 0;
}
constraints_lowerbound[_x_start] = x;
constraints_lowerbound[_y_start] = y;
constraints_lowerbound[_theta_start] = theta;
constraints_lowerbound[_v_start] = v;
constraints_lowerbound[_cte_start] = cte;
constraints_lowerbound[_etheta_start] = etheta;
constraints_upperbound[_x_start] = x;
constraints_upperbound[_y_start] = y;
constraints_upperbound[_theta_start] = theta;
constraints_upperbound[_v_start] = v;
constraints_upperbound[_cte_start] = cte;
constraints_upperbound[_etheta_start] = etheta;
设定不等式约束
for (int i = 0; i < _angvel_start; i++)
{
vars_lowerbound[i] = -_bound_value;
vars_upperbound[i] = _bound_value;
}
// The upper and lower limits of angvel are set to -25 and 25
// degrees (values in radians).
for (int i = _angvel_start; i < _a_start; i++)
{
vars_lowerbound[i] = -_max_angvel;
vars_upperbound[i] = _max_angvel;
}
// Acceleration/decceleration upper and lower limits
for (int i = _a_start; i < n_vars; i++)
{
vars_lowerbound[i] = -_max_throttle;
vars_upperbound[i] = _max_throttle;
}
设定约束矩阵并求解,fg[0]为优化函数
CppAD::ipopt::solve<Dvector, FG_eval>(
options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
constraints_upperbound, fg_eval, solution);
void operator()(ADvector& fg, const ADvector& vars)
{
// fg[0] for cost function
fg[0] = 0;
cost_cte = 0;
cost_etheta = 0;
cost_vel = 0;
/*
for (int i = 0; i < _mpc_steps; i++)
{
cout << i << endl;
cout << "_x_start" << vars[_x_start + i] <<endl;
cout << "_y_start" << vars[_y_start + i] <<endl;
cout << "_theta_start" << vars[_theta_start + i] <<endl;
cout << "_v_start" << vars[_v_start + i] <<endl;
cout << "_cte_start" << vars[_cte_start + i] <<endl;
cout << "_etheta_start" << vars[_etheta_start + i] <<endl;
}*/
for (int i = 0; i < _mpc_steps; i++)
{
fg[0] += _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2); // cross deviation error
fg[0] += _w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2); // heading error
fg[0] += _w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2); // speed error
cost_cte += _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2);
cost_etheta += (_w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2));
cost_vel += (_w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2));
}
cout << "-----------------------------------------------" <<endl;
cout << "cost_cte, etheta, velocity: " << cost_cte << ", " << cost_etheta << ", " << cost_vel << endl;
// Minimize the use of actuators.
for (int i = 0; i < _mpc_steps - 1; i++) {
fg[0] += _w_angvel * CppAD::pow(vars[_angvel_start + i], 2);
fg[0] += _w_accel * CppAD::pow(vars[_a_start + i], 2);
}
cout << "cost of actuators: " << fg[0] << endl;
// Minimize the value gap between sequential actuations.
for (int i = 0; i < _mpc_steps - 2; i++) {
fg[0] += _w_angvel_d * CppAD::pow(vars[_angvel_start + i + 1] - vars[_angvel_start + i], 2);
fg[0] += _w_accel_d * CppAD::pow(vars[_a_start + i + 1] - vars[_a_start + i], 2);
}
cout << "cost of gap: " << fg[0] << endl;
// fg[x] for constraints
// Initial constraints
fg[1 + _x_start] = vars[_x_start];
fg[1 + _y_start] = vars[_y_start];
fg[1 + _theta_start] = vars[_theta_start];
fg[1 + _v_start] = vars[_v_start];
fg[1 + _cte_start] = vars[_cte_start];
fg[1 + _etheta_start] = vars[_etheta_start];
// Add system dynamic model constraint
for (int i = 0; i < _mpc_steps - 1; i++)
{
// The state at time t+1 .
AD<double> x1 = vars[_x_start + i + 1];
AD<double> y1 = vars[_y_start + i + 1];
AD<double> theta1 = vars[_theta_start + i + 1];
AD<double> v1 = vars[_v_start + i + 1];
AD<double> cte1 = vars[_cte_start + i + 1];
AD<double> etheta1 = vars[_etheta_start + i + 1];
// The state at time t.
AD<double> x0 = vars[_x_start + i];
AD<double> y0 = vars[_y_start + i];
AD<double> theta0 = vars[_theta_start + i];
AD<double> v0 = vars[_v_start + i];
AD<double> cte0 = vars[_cte_start + i];
AD<double> etheta0 = vars[_etheta_start + i];
// Only consider the actuation at time t.
//AD<double> angvel0 = vars[_angvel_start + i];
AD<double> w0 = vars[_angvel_start + i];
AD<double> a0 = vars[_a_start + i];
//AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3);
AD<double> f0 = 0.0;
for (int i = 0; i < coeffs.size(); i++)
{
f0 += coeffs[i] * CppAD::pow(x0, i); //f(0) = y
}
//AD<double> trj_grad0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0 + 3 * coeffs[3] * CppAD::pow(x0, 2));
AD<double> trj_grad0 = 0.0;
for (int i = 1; i < coeffs.size(); i++)
{
trj_grad0 += i*coeffs[i] * CppAD::pow(x0, i-1); // f'(x0) = f(1)/1
}
trj_grad0 = CppAD::atan(trj_grad0);
// Here's `x` to get you started.
// The idea here is to constraint this value to be 0.
//
// NOTE: The use of `AD<double>` and use of `CppAD`!
// This is also CppAD can compute derivatives and pass
// these to the solver.
// TODO: Setup the rest of the model constraints
fg[2 + _x_start + i] = x1 - (x0 + v0 * CppAD::cos(theta0) * _dt);
fg[2 + _y_start + i] = y1 - (y0 + v0 * CppAD::sin(theta0) * _dt);
fg[2 + _theta_start + i] = theta1 - (theta0 + w0 * _dt);
fg[2 + _v_start + i] = v1 - (v0 + a0 * _dt);
fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(etheta0) * _dt));
fg[2 + _etheta_start + i] = etheta1 - ((theta0 - trj_grad0) + w0 * _dt);//theta0-trj_grad0)->etheta : it can have more curvature prediction, but its gradient can be only adjust positive plan.
//fg[2 + _etheta_start + i] = etheta1 - (etheta0 + w0 * _dt);
}
}