无人驾驶差速机器人MPC代码详解

环境构建参考我的另一篇文章

Noetic差速机器人MPC控制

关于MPC与无人驾驶的理解参考文章

自动驾驶——模型预测控制（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);
            }
        }