力学模型

lgsvl_msgs/VehicleControlData.h

#include

目标点处的切线与全局坐标系的x轴之间的夹角目标航向角double

kappa;

std::shared_ptrLateralControlError

头文件

public:MPCController();~MPCController();void

Init();bool

ComputeLongitudinalErrors(const

VehicleState

QueryNearestPointByPosition(const

double

全局规划路径中的所有点std::vectorTrajectoryPoint

ts_

方向盘的转角到轮胎转动角度之间的比值系数Eigen::MatrixXd

matrix_a_coeff_;Eigen::MatrixXd

int

steer_single_direction_max_degree_

0.0;

wheel_single_direction_max_degree_

0.0;

MPCController::~MPCController()

{}//

MPCController::LoadControlConf()

{ts_

传动比steer_single_direction_max_degree_

40.0;

方向盘的最大转角角度wheel_single_direction_max_degree_

steer_single_direction_max_degree_

steer_ratio_

Matrix::Zero(basic_state_size_,

basic_state_size_);matrix_a_(0,

1.0;matrix_a_(1,

Matrix::Zero(basic_state_size_,

basic_state_size_);matrix_a_coeff_(1,

-(cf_

Matrix::Zero(basic_state_size_,

cf_

Matrix::Zero(basic_state_size_,

初始化离散型车辆横向动力学模型

Matrix::Zero(basic_state_size_,

matrix_b_

Matrix::Zero(basic_state_size_,

1);//

Matrix::Zero(basic_state_size_,

basic_state_size_);matrix_q_(0,

3.0;

MPCController::Wheel2SteerPct(const

double

wheel_single_direction_max_degree_

100;

MPCController::ComputeControlCommand(const

VehicleState

所有轨迹点全局规划器提供的全局路径trajectory_points_

planning_published_trajectory.trajectory_points;//

x_0均为误差量UpdateState(localization);//

更新状态矩阵

解释因为矩阵Ad的计算涉及到车速vx所以每往后走一个时间步都要同步更新一下系统横向动力学模型中的Ad矩阵UpdateMatrix(localization);//

control_matrix

Matrix::Zero(basic_state_size_,

1);//

横向误差横向误差变化率航向误差航向误差变化率纵向位置误差纵向速度误差Matrix

lower_state_bound(basic_state_size_,

1);Matrix

upper_state_bound(basic_state_size_,

1);const

std::numeric_limitsdouble::max();lower_state_bound

-1.0

control_cmd.at(0);control_matrix(1,

发布控制命令double

MPCController::UpdateState(const

VehicleState

智能指针std::shared_ptrLateralControlError

lat_con_err

std::make_sharedLateralControlError();//

lat_con_err

中ComputeErrors(vehicle_state.x,

vehicle_state.y,

vehicle_state.angular_velocity,

lat_con_err);//

lat_con_err-lateral_error_rate;

朝向误差matrix_state_(3,

lat_con_err-heading_error_rate;

station_error_;

MPCController::UpdateMatrix(const

VehicleState

std::max(vehicle_state.velocity,

计算连续型车辆横向动力学模型

Matrix::Identity(matrix_a_.cols(),

(matrix_i

MPCController::ComputeErrors(const

double

查询距离车辆当前位置最近的路径点作为目标点TrajectoryPoint

y);const

std::cos(target_point.heading);const

double

std::sin(target_point.heading);double

lateral_error

横向误差使用的是车身坐标系和全局坐标系的变换公式计算的double

heading_error

纵向速度误差lat_con_err-lateral_error

lateral_error;lat_con_err-heading_error

heading_error;lat_con_err-lateral_error_rate

lateral_error_dot;lat_con_err-heading_error_rate

angular_v

MPCController::QueryNearestPointByPosition(const

double

PointDistanceSquare(trajectory_points_.front(),

y);size_t

PointDistanceSquare(trajectory_points_[i],

y);if

CalculateKernel(std::vectorc_float

*P_data,

CalculateEqualityConstraint(std::vectorc_float

*A_data,

因为这俩都是稀疏矩阵而且很大如果直接存储太消耗资源所以使用列压缩的方式转化为了3个特征数组

};}

定值matrix_initial_x_(matrix_initial_x),

非定值matrix_u_lower_(matrix_u_lower),

定值matrix_u_upper_(matrix_u_upper),

定值matrix_x_lower_(matrix_x_lower),

定值matrix_x_upper_(matrix_x_upper),

定值恒定为0max_iteration_(max_iter),

horizon_(horizon),eps_abs_(eps_abs)

{state_dim_

MpcOsqp::CalculateKernel(std::vectorc_float

*P_data,

非零元素值)std::vectorstd::vectorstd::pairc_int,

c_float

代表按列columns.resize(num_param_);//

临时变量//

state_total_dim].emplace_back(i

control_dim_

作用用来计数遍历到当前列不包含该列时累计的非零元素的数量int

ind_p

按列P_indptr-emplace_back(ind_p);for

(const

对P_data-emplace_back(row_data_pair.second);P_indices-emplace_back(row_data_pair.first);ind_p;}}P_indptr-emplace_back(ind_p);

}//

Eigen::VectorXd::Zero(state_dim_

(horizon_

MpcOsqp::CalculateEqualityConstraint(std::vectorc_float

*A_data,

Eigen::MatrixXd::Identity(control_dim_,

control_dim_);//

Eigen::MatrixXd::Zero(state_dim_

(horizon_

Eigen::MatrixXd::Identity(state_dim_

(horizon_

Eigen::MatrixXd::Identity(num_param_,

num_param_);matrix_constraint.block(state_dim_

(horizon_

表示为按列压缩格式std::vectorstd::vectorstd::pairc_int,

c_float

columns;columns.resize(num_param_

1);int

(std::fabs(matrix_constraint(j,

i))

{A_indptr-emplace_back(ind_A);for

(const

{A_data-emplace_back(row_data_pair.second);A_indices-emplace_back(row_data_pair.first);ind_A;}}A_indptr-emplace_back(ind_A);

}//

MpcOsqp::CalculateConstraintVectors()

{//

Eigen::MatrixXd::Zero(state_dim_

(horizon_

Eigen::MatrixXd::Zero(state_dim_

(horizon_

{lowerInequality.block(control_dim_

state_dim_

matrix_u_lower_;upperInequality.block(control_dim_

state_dim_

{lowerInequality.block(state_dim_

state_dim_,

matrix_x_lower_;upperInequality.block(state_dim_

state_dim_,

Eigen::MatrixXd::Zero(state_dim_

(horizon_

upperEquality;lowerEquality.block(0,

state_dim_,

matrix_initial_x_;upperEquality

lowerEquality;//

*(c_malloc(sizeof(OSQPSettings)));//

指针为空代表分配内存失败if

{osqp_set_default_settings(settings);settings-polish

true;settings-scaled_termination

max_iteration_;

库中的一个结构体用于存储二次规划QP问题的输入数据OSQPData

OSQPData*

*(c_malloc(sizeof(OSQPData)));size_t

kernel_dim

个特征数组CalculateEqualityConstraint(A_data,

A_indices,

{c_free(data-A);c_free(data-P);c_free(data);

}//

MpcOsqp::Solve(std::vectordouble*

control_cmd)

{CalculateGradient();CalculateConstraintVectors();OSQPData*

data

求解器的工作空间包括定义问题、设置参数、执行求解等功能这里创建了一个名为

osqp_workspace

求解器的工作空间的函数这个函数的作用是根据传入的数据(data)和设置(settings)来设置和配置

OSQP

求解器来解决已经设置好的凸二次规划问题osqp_solve(osqp_workspace)

表示你正在使用

求解器工作空间来执行求解操作osqp_solve(osqp_workspace);//

workspace

osqp_workspace-info-status_val;if

(status

{osqp_cleanup(osqp_workspace);FreeData(data);c_free(settings);return

false;}else

nullptr表示求解器没有成功生成解osqp_cleanup(osqp_workspace);FreeData(data);c_free(settings);return

false;}size_t

清理内存osqp_cleanup(osqp_workspace);FreeData(data);c_free(settings);return

true;

ComputePathProfile(std::vectordouble*

headings,

dkappas);private:std::vectorstd::pairdouble,

double

ReferenceLine::ReferenceLine(const

double

函数作用由于全局路径规划期仅仅给出了全局路径信息即所有点的xy坐标而没有任何关于这条全局路径的其他几何信息如每个路径点的

朝向角/距离/曲率/曲率变化率

而我们在局部规划中使用的参考线其实是和全局路径重合的也就是说参考线上的所有路径点就是全局路径的所有路径点

但是要注意我们需要的参考线不能仅仅只有每个路径点的坐标还需要有每个路径点的各个几何信息如

朝向角/距离/曲率/曲率变化率

因此我们直接计算全局路径中每个路径点的几何信息然后汇总到一起就是我们需要的参考线信息

bool

ReferenceLine::ComputePathProfile(std::vectordouble*

headings,

到达该点时所累积的路径长度起点到该点的几何路径长度kappas-clear();

曲率dkappas-clear();

1].second);}dxs.push_back(x_delta);dys.push_back(y_delta);}//

(std::size_t

{headings-push_back(std::atan2(dys[i],

dxs[i]));}//

0.0;accumulated_s-push_back(distance);

index

ny));accumulated_s-push_back(end_segment_s

distance);distance

1));}x_over_s_first_derivatives.push_back(xds);y_over_s_first_derivatives.push_back(yds);}//

(std::size_t

1));}x_over_s_second_derivatives.push_back(xdds);y_over_s_second_derivatives.push_back(ydds);}//

(std::size_t

x_over_s_first_derivatives[i];double

yds

y_over_s_first_derivatives[i];double

xdds

x_over_s_second_derivatives[i];double

ydds

y_over_s_second_derivatives[i];double

kappa

注2这里对曲率的计算使用的是精确公式而代码中我们在进行动力学建模计算连续误差型状态空间方程的状态量x时如航向误差变化率则是将曲率视为了半径的倒数这是一种对曲率的估算方式kappas-push_back(kappa);}//

(std::size_t

1));}dkappas-push_back(dkappa);}return

true;

角速度绕z轴转动的角速度vehicle_state_.angular_velocity

加速度线性加速度vehicle_state_.acceleration

}//

{vehicle_state_.planning_init_x

msg-pose.pose.position.x;vehicle_state_.planning_init_y

msg-pose.pose.position.y;first_record_

msg-pose.pose.position.x;vehicle_state_.y

msg-pose.pose.position.y;vehicle_state_.last_velocity

vehicle_state_.velocity;vehicle_state_.last_v_time

vehicle_state_.cur_v_time;vehicle_state_.last_v_err

5;//

类型的变量用于存储姿态信息的四元数表示tf::Quaternion

q;//

消息中的姿态信息msg-pose.pose.orientation转换为

tf::Quaternion

库中的四元数表示tf::quaternionMsgToTF(msg-pose.pose.orientation,

q);//

的对应成员变量中tf::Matrix3x3(q).getRPY(vehicle_state_.roll,

vehicle_state_.yaw);vehicle_state_.heading

vehicle_state_.yaw;vehicle_state_.velocity

std::sqrt(msg-twist.twist.linear.x

msg-twist.twist.linear.y);vehicle_state_.cur_v_err

ROS

infile;infile.open(src/mpc_control/data/reference_line.txt);

将文件流对象与文件连接起来assert(infile.is_open());

若失败则输出错误消息并终止程序运行std::vectorstd::pairdouble,

double

std::atof(y.c_str());xy_points.push_back(std::make_pair(pt_x,

pt_y));}//

补全每个参考点的信息当前只有每个参考点的位置坐标std::vectordouble

accumulated_s;std::vectordouble

dkappas;std::unique_ptrshenlan::control::ReferenceLine

reference_line

std::make_uniqueshenlan::control::ReferenceLine(xy_points);reference_line-ComputePathProfile(headings,

accumulated_s,

planning_published_trajectory;for

(size_t

xy_points[i].first;trajectory_pt.y

xy_points[i].second;trajectory_pt.v

5.0;

headings[i];trajectory_pt.kappa

kappas[i];planning_published_trajectory.trajectory_points.push_back(trajectory_pt);}//

ROS

提供的一个宏用于打印消息到ROS的日志系统中这里表示节点初始化完成ROS_INFO(init

!);//

条待发布的消息超过这个数量后新的消息将被丢弃ros::Publisher

control_pub

nh.advertiselgsvl_msgs::VehicleControlData(/vehicle_cmd,

1000);//

nh.advertiselgsvl_msgs::VehicleControlData(/acc_pub_cmd,

1000);//

cmd;std::unique_ptrshenlan::control::MPCController

mpc_controller

std::make_uniqueshenlan::control::MPCController();mpc_controller-Init();lgsvl_msgs::VehicleControlData

control_cmd;lgsvl_msgs::VehicleControlData

control_cmd_pub;

{mpc_controller-ComputeControlCommand(vehicle_state_,

endl;cout

endl;control_cmd.acceleration_pct

cmd.acc;control_cmd.target_gear

lgsvl_msgs::VehicleControlData::GEAR_DRIVE;control_cmd.target_wheel_angle

-cmd.steer_target;control_pub.publish(control_cmd);control_cmd_pub.acceleration_pct

vehicle_state_.acceleration;acc_pub.publish(control_cmd_pub);ros::spinOnce();loop_rate.sleep();}return