[MPCC in FSAC]基于MPCC的无人系统控制设计思路

Vonct

已于 2023-12-04 14:59:50 修改

阅读量1k

点赞数 23

文章标签：自动驾驶汽车

于 2023-12-04 14:56:11 首次发布

本文链接：https://blog.csdn.net/vonct/article/details/134781569

版权

关于无人驾驶控制系统的设计网上资料已经很多了，相信能点进来的也是接触过中国大学生无人驾驶方程式大赛（FSAC）的。
本文将从AMZ的MPCC出发，抛砖引玉地介绍一下自己的丐版实现。

MPCC简单介绍

网上很多讲用于无人驾驶控制的MPC的资料，但大多都是基于误差的（参考北理的《无人驾驶模型预测控制》），当然不可否认的是其在仿真或者试车调试中都有不错的效果。

但在无人驾驶方程式大赛中，走赛道中心线显然不是最优路线，在不依靠路径优化的情况下，如何走出最短的路线，这就是MPCC希望解决的。

他的核心思想就是车辆的行驶路程（Frenet坐标系的s）也放到MPC的状态量。例如，当前s=1m，在预测域中，s = 1.1，s = 1.2，s = 1.3，s = 1.4，s = 1.5，在计算跟踪误差时，就可以根据s插值得到在每个步长下的期望参考点。

比如s=1.1，对应上图的蓝圈圈起来的中心线参考点，这时候可以基于车辆当前位置（这里的当前可以是实际的位置，也可以指预测域的位置）算出横向误差ec，纵向误差el。

那么怎么样才能使车辆计算出一个可以完成切弯动作的转角（以及车速）呢？

1. 令横向误差的惩罚权重小于纵向误差；

2. 整个预测域下车辆走过的路程也作为误差项。

再结合下图来简单理解，Penalty1让算法能算出行驶路径①②以及相对应的控制指令，而Penalty2能让算法选择②（内圈比外圈短）

核心就是这个，但我讲的比较精简，实际上，开源的MPCC代码比较复杂，这也是我为什么想要写一个精简且易于改进的MPCC的初衷。话虽如此，还是十分建议去看看相关论文及代码。

个人认为，MPCC理论上是FSAC中最佳控制实现方案。

精简版NMPCC设计思路

算法步骤

偶然间刷到了这个视频，发现貌似能用于实现NMPCC，在结合上述的MPCC思想后得到了不错的仿真效果。

在看完上面这个视频后，相信以及对NMPC、Multi-shooting等概念有了一些了解，接下来再看看这个开源仓库，主要是看casadi_CPP的用法，以及整个框架，相信看完后大家也有一些思路了。

简单地讲讲步骤：

1.基于期望车速v_desired，预测步长Ts以及当前车辆位置s，得到一条baseline（也就是刚刚提到的预测域下每个步长下待优化状态量对应的参考点）

2. 构建代价函数，这个没什么好说的，就是按照MPCC的来，除了横纵向误差惩罚以及路径长度惩罚，也加上了控制量变化率惩罚。

3. 设置约束，主要就是转角大小按需约束，但要注意的是航向角设置约束时，因其周期性，只设-2PI ~ 2PI是不行的（要设起码-3PI ~ 3PI）。为了防止车辆过分的切弯以至于驶出赛道，添加赛道几何约束。

4. 通过casadi求解问题

一部分代码

完整代码就不放出来了，其实写起来也很简单，主要讲下比较核心的。

1. getDesiredStates

//Eigen::Matrix<double, n_states_, N_ + 1> desired_states;   
for(int i = 1; i <= N_; i++){
        s_ref.push_back(s_ref[i-1] + des_v_ * dt_);
        //根据s插值获得参考点
        tmp1 = track_->calc_position(s_ref[i]);
        tmp2 = track_->calc_position(s_ref[i - 1]);
        //global coordinate
        desired_states(0,i) = tmp1(0);
        desired_states(1,i) = tmp1(1);
        Eigen::Vector2d direction = tmp1 - tmp2;
        double temp_theta    = std::atan2(direction(1),direction(0));
        if(temp_theta < 0){
            temp_theta += 2*PI; // 将theta限制在[0,2*PI]
        }
        desired_states(2,i) = temp_theta;
        bound = getTrackBoundary(tmp1(0),tmp1(1),temp_theta);
        xl_.push_back(bound(0));
        yl_.push_back(bound(1));
        xr_.push_back(bound(2));
        yr_.push_back(bound(3));
        lambda_x_vec_.push_back(bound(4));
        lambda_y_vec_.push_back(bound(5));
        b_max_vec_.push_back(bound(6));
        b_min_vec_.push_back(bound(7));
        geometry_msgs::PoseStamped ps;
        ps.pose.position.x = tmp1(0);
        ps.pose.position.y = tmp1(1);
        ps.header.frame_id = frame_id_;
        ref.poses.push_back(ps);
    }

2. 代价函数

    //set costfunction
    //tracking & control error
    for(int i = 1; i < N_; i++){
        casadi::MX ec =  casadi::MX::sin(phi(i)) * (x(i) - X_ref(0,i)) - casadi::MX::cos(phi(i)) * (y(i) - X_ref(1,i));
        casadi::MX el = -casadi::MX::cos(phi(i)) * (x(i) - X_ref(0,i)) - casadi::MX::sin(phi(i)) * (y(i) - X_ref(1,i));
        casadi::MX err = casadi::MX::vertcat({ec,el});
        //std::cout<<"X_err is: "<<X_err<<std::endl;
        //std::cout<<"X_err size is: "<<X_err.size()<<std::endl;//[3,1]
        casadi::MX U_0 = U(all,i);
        cost += casadi::MX::mtimes({err.T(),Q_,err});
        cost += casadi::MX::mtimes({U_0.T(),R_,U_0});
        cost += casadi::MX::mtimes({U_0.T(),R_,U_0});
    }
    casadi::MX ec =  casadi::MX::sin(phi(N_)) * (x(N_) - X_ref(0,N_)) - casadi::MX::cos(phi(N_)) * (y(N_) - X_ref(1,N_));
    casadi::MX el = -casadi::MX::cos(phi(N_)) * (x(N_) - X_ref(0,N_)) - casadi::MX::sin(phi(N_)) * (y(N_) - X_ref(1,N_));
    casadi::MX err = casadi::MX::vertcat({ec,el});
    cost += casadi::MX::mtimes({err.T(),Q_N_,err});
    //length error
    for(int i = 1; i <= N_; i++){
        casadi::MX s = casadi::MX::sqrt(casadi::MX::pow((X(all,i)-X(all,i-1)),2));
        cost += casadi::MX::mtimes({s.T(),S_,s});
    }
    //delta control error
    for(int i = 1; i < N_; i++){
        casadi::MX U_1 = U(all,i);
        //td::cout<<"U_1 is: "<<repr(U_1)<<std::endl;// MX(opti681_x_2[2:4])
        casadi::MX U_0 = U(all,i - 1);
        cost += casadi::MX::mtimes({(U_1 - U_0).T(),DR_,(U_1 - U_0)});
    } 
    //delta theta error
    for(int i = 1; i < N_; i++){
        casadi::MX X_1 = X(all,i);
        casadi::MX X_0 = X(all,i - 1);
        cost += casadi::MX::mtimes({(X_1 - X_0).T(),DQ_,(X_1 - X_0)});
    }