车辆运动学建模 | LQR轨迹跟踪 | C++实现

lvxiaof

已于 2025-07-05 11:08:57 修改

阅读量943

点赞数 11

CC 4.0 BY-SA版权

分类专栏： Control 文章标签：机器学习人工智能算法

于 2025-06-19 20:44:01 首次发布

本文链接：https://blog.youkuaiyun.com/lvxiaof/article/details/148773765

Control 专栏收录该内容

3 篇文章

订阅专栏

LQR定义及求解

LQR(Linear Quadratic Regulor,线性二次调节器)是最优控制理论中的一种经典方法，用于对线性系统进行状态反馈控制，它的目标是在系统动态满足一定条件下，使性能指标最小，即权衡系统性能与控制消耗的问题关系。

一、LQR适用的系统：

LQR用于线性时不变系统：

或者离散形式：

二、LQR的目标函数：

LQR控制器的目的是最小化以下性能指标：

三、LQR控制律：

车辆运动学模型线性化、离散化：

一、线性化

以后轴中心为车辆中心的单车运动学模型为例进行线性化，运动学模型如下图所示：

根据条件假设，低速环境下，车辆行驶路径的转弯半径变化缓慢，此时我们可以假设车辆偏航角（航向角）的变化率theta‘可近似等于车辆的角速度ω，根据车辆角速度，得

所以可得：

其中，。对上式采用泰勒展开，忽略高阶项可得：

于是状态量误差变化量表示为：为状态量误差

上式为基于状态误差量构造的线性状态空间方程。

二、离散化

采用前向欧拉法：

整理后：

式中，T为采样步长，I为单位矩阵，维度与矩阵A一致。

C++实现

轨迹构造

#include "reference_line.h"

MyReferencePath::MyReferencePath()
{
	int i = 0;
	double dx = 0.0, dy = 0.0;
	double ddx = 0.0, ddy = 0.0;

	this->refer_path = vector<vector<double>>(1000, vector<double>(4));
	//生成参考线轨迹
	for (i = 0; i < 1000; i++)
	{
		refer_path[i][0] = 0.1 * i;
		refer_path[i][1] = sin(refer_path[i][0] / 4.0) + cos(refer_path[i][0] / 4.0);
		//refer_path[i][1] = 1.0;

		refer_x.push_back(refer_path[i][0]);
		refer_y.push_back(refer_path[i][1]);
	}

	//计算航向角、曲率
	for (i = 0; i < refer_path.size(); i++)
	{
		if (i == 0)
		{
			dx = refer_path[i + 1][0] - refer_path[i][0];
			dy = refer_path[i + 1][1] - refer_path[i][1];
			ddx = refer_path[2][0] + refer_path[0][0] - 2 * refer_path[1][0];
			ddy = refer_path[2][1] + refer_path[0][1] - 2 * refer_path[1][1];
		}
		else if (i == refer_path.size() - 1)
		{
			dx = refer_path[i][0] - refer_path[i - 1][0];
			dy = refer_path[i][1] - refer_path[i - 1][1];
			ddx = refer_path[i][0] + refer_path[i - 2][0] - 2 * refer_path[i - 1][0];
			ddy = refer_path[i][1] + refer_path[i - 2][1] - 2 * refer_path[i - 1][1];
		}
		else
		{
			dx = refer_path[i + 1][0] - refer_path[i][0];
			dy = refer_path[i + 1][1] - refer_path[i][1];
			ddx = refer_path[i + 1][0] + refer_path[i - 1][0] - 2 * refer_path[i][0];
			ddy = refer_path[i + 1][1] + refer_path[i - 1][1] - 2 * refer_path[i][1];
		}
		refer_path[i][2] = atan2(dy, dx);
		//计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2).
		refer_path[i][3] = (ddy * dx - ddx * dy) / pow((dx * dx + dy * dy), 3 / 2);
	}


}

/**
* 计算跟踪误差
* @param robot_state  机器人状态
* @return
*/
vector<double> MyReferencePath::calcTrackError(vector<double> robot_state)
{
	double x = robot_state[0], y = robot_state[1];
	vector<double> d_x(refer_path.size()), d_y(refer_path.size()), d(refer_path.size());
	int i = 0;
	for (i = 0; i < refer_path.size(); i++)
	{
		d_x[i] = refer_path[i][0] - x;
		d_y[i] = refer_path[i][1] - y;
		d[i] = sqrt(d_x[i] * d_x[i] + d_y[i] * d_y[i]);
	}
	double min_idx = min_element(d.begin(), d.end()) - d.begin();

	double yaw = refer_path[min_idx][2];
	double k = refer_path[min_idx][3];
	double angle = normalizeAngle(yaw - atan2(d_y[min_idx], d_x[min_idx]));
	double error = d[min_idx];//误差
	if (angle < 0)
	{
		error *= -1;
	}

	return{ error, k, yaw, min_idx };

}

/**
* 角度归一化
* @param angle
* @return
*/
double MyReferencePath::normalizeAngle(double angle) {
	while (angle > PI) {
		angle -= 2.0 * PI;
	}
	while (angle < -PI) {
		angle += 2.0 * PI;
	}
	return angle;
}

LQR求解器

#include "LQRControl.h"

LQRControl::LQRControl(int n) : N(n) {};

/**
* 解代数里卡提方程
* @param A 状态矩阵A
* @param B 状态矩阵B
* @param Q Q为半正定的状态加权矩阵, 通常取为对角阵；Q矩阵元素变大意味着希望跟踪偏差能够快速趋近于零；
* @param R R为正定的控制加权矩阵，R矩阵元素变大意味着希望控制输入能够尽可能小。
* @return
*/

MatrixXd LQRControl::calRicatti(MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R)
{
	MatrixXd Qf = Q;
	MatrixXd P = Qf;
	MatrixXd P_;

	for (int i = 0; i < this->N; i++)
	{
		P_ = Q + A.transpose() * P * A - A.transpose() * P * B * (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;

		if ((P_ - P).maxCoeff() < EPS && (P - P_).maxCoeff() < EPS)
		{
			break;
		}

		P = P_;
	}

	return P_;
}

/**
* LQR控制器
* @param robot_state
* @param refer_path
* @param s0
* @param A
* @param B
* @param Q
* @param R
* @return
*/
double LQRControl::lqrControl(vector<double> vehcle_state,
	vector<vector<double>> refer_path,
	double s0, MatrixXd A, MatrixXd B,
	MatrixXd Q, MatrixXd R)
{
	MatrixXd X(3, 1);
	X(0, 0) = vehcle_state[0] - refer_path[s0][0];
	double x0 = X(0, 0);
	X(1, 0) = vehcle_state[1] - refer_path[s0][1];
	double x1 = X(1, 0);
	X(2, 0) = vehcle_state[2] - refer_path[s0][2];
	double x2 = X(2, 0);

	MatrixXd P = calRicatti(A, B, Q, R);
	MatrixXd K = (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;
	MatrixXd u = -K * X;//u(0, 0)速度差，//u(1, 0)前轮转角差
	double u1 = u(1, 0);
	return u(1, 0);
}

运动学模型

#include "KinematicModel.h"
/**
* 机器人运动学模型构造
* @param x 位置x
* @param y 位置y
* @param psi 偏航角
* @param v 速度
* @param l 轴距
* @param dt 采样时间
*/
KinematicModel::KinematicModel(double x, double y, double theta, double v, double l, double dt) :
	x(x), y(y), theta(theta), v(v), L(l), dt(dt) {}

/**
* 控制量为转向角delta_f和加速度a
* @param accel 加速度
* @param delta_f 转向角控制量
*/
void KinematicModel::updatestate(double accel, double delta_f)
{
	x = x + cos(theta) * v * dt;
	y = y + sin(theta) * v * dt;
	theta = theta + v / L * tan(delta_f) * dt;
	double temp = tan(delta_f);
	v = v + accel * dt;
}

vector<double> KinematicModel::getstate()
{
	return { x, y, theta, v };
}

/**
* 将模型离散化后的状态空间表达
* @param ref_delta 名义控制输入
* @param ref_yaw 名义偏航角
* @return
*/
vector<MatrixXd> KinematicModel::stateSpace(double ref_delta, double ref_yaw)
{
	MatrixXd A(3, 3), B(3, 2);
	A << 1.0, 0.0, -v * dt * sin(ref_yaw),
		0.0, 1.0, v * dt * cos(ref_yaw),
		0.0, 0.0, 1.0;

	B << dt * cos(ref_yaw), 0,
		dt * sin(ref_yaw), 0,
		dt * tan(ref_delta) / L, v * dt / (L  *cos(ref_delta) * cos(ref_delta));

	return { A, B };
}

主函数

#include "reference_line.h"
#include "LQRControl.h"
#include "KinematicModel.h"
#define DELTA_MAX  (3.1415926 / 6.0)
#define DELTA_MIN  (-3.1415926 / 6.0)
int main()
{

	double dt = 0.1;//时间间隔
	double L = 2.0; //轴距
	double veh_v = 2.0; //初始速度
	double x_0 = 0.0;
	double y_0 = -3.0;
	double veh_theta = 0.0;
	int N = 100;
	int i = 0;

	MatrixXd Q(3, 3);
	Q << 3, 0, 0,
		 0, 3, 0,
		 0, 0, 3;

	MatrixXd R(2, 2);
	R << 2.0, 0.0,
		 0.0, 2.0;

	//保存车辆运动轨迹
	vector<double> x_, y_;
	MyReferencePath reference_path;
	KinematicModel vehcle(x_0, y_0, veh_theta, veh_v, L, dt);
	LQRControl lqr(N);
	vector<double> veh_state;
	vector<double>one_trial;
	double ref_k = 0.0, ref_yaw = 0.0, s0 = 0.0;
	double ref_delta = 0.0, delta = 0.0;//期望前轮转角


	//开始跟踪参考线
	for ( i = 0; i < 500; i++)
	{
		veh_state = vehcle.getstate();//获取车辆x，y，航向角，速度信息

		one_trial = reference_path.calcTrackError(veh_state);//获取车辆与参考线匹配点的距离、匹配点的曲率、航向、索引。
		ref_k = one_trial[1];
		ref_yaw = one_trial[2];
		s0 = one_trial[3];

		ref_delta = atan2(L * ref_k, 1);//计算参考前轮转角
		vector<MatrixXd> state_space = vehcle.stateSpace(ref_delta, ref_yaw);//获取A.B矩阵

		delta = lqr.lqrControl(veh_state, reference_path.refer_path, s0, state_space[0], state_space[1], Q, R);//计算控制量前轮转角

		if (delta >= DELTA_MAX)
		{
			delta = DELTA_MAX;
		}
		else if (delta < DELTA_MIN)
		{
			delta = DELTA_MIN;
		}

		delta = ref_delta + delta;

		vehcle.updatestate(0, delta);//加速度设为0，恒速

		x_.push_back(vehcle.x);
		y_.push_back(vehcle.y);

	}

	return 0;
}

车辆形式路径已经存入x_,y_ 中，可自行画图。

参考博客

【自动驾驶】LQR控制实现轨迹跟踪 | python实现 | c++实现