关于无人驾驶控制系统的设计网上资料已经很多了,相信能点进来的也是接触过中国大学生无人驾驶方程式大赛(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)});
}
仿真效果演示
(懒得重新录一个清晰一点的视频了)可以看出是可以完成一定程度的切弯的,算法本身也易于进行拓展,但缺点就是计算时延过高,在运动学下大概为50ms,而动力学下为150ms,当然这也和预测域大小、硬件性能有关。
MPCC仿真演示
在FSAC无人系统上,各家车队的交流比较少,本文也仅仅是抛砖引玉,欢迎大家一起讨论。