1.从阿克曼转向几何模型到自行车模型
相对无人机与机械臂来说,无人车系统的运动学模型非常简洁。尽管简洁,无人车的运动学模型也是非线性的。应用于具体控制算法时,有必要对原始运动学模型进行变形或线性化。本篇主要介绍无人车的运动学模型,并对原始非线性模型进行线性化。
运动学是从几何学的角度研究物体的运动规律,包括物体在空间的位置、速度等随时间而产生的变化,因此,车辆运动学模型应该能反映车辆位置、速度、加速度等与时间的关系。在车辆轨迹规划过程中应用运动学模型,可以使规划出的轨迹更切合实际,满足行驶过程中的运动学几何约束,且基于运动学模型设计出的控制器也能具有更可靠的控制性能。
汽车采用阿克曼转向轮,因此模型为如下图所示的阿克曼转向几何模型。
由以上阿克曼转向模型可得进一步简化为车辆单轨模型——自行车模型。
注意:以上公式可以准确估计低速场景下的车辆运动。 但是,对于高速情况,该公式估计的运动与实际无人车运行的情况有一定的差异,这与路面、轮胎材料都有关系。
2. 无人车运动学模型
以后轴中心为车辆中心的单车运动学模型
补充:推导方式二
参考:无人车系统(一):运动学模型及其线性化_无人集卡阿克曼转角模型-CSDN博客
3. 运动学模型线性化
为什么要线性化?
车辆动力学模型线性化的主要原因是为了设计控制器。非线性系统往往难以控制,因此需要将其转化为线性系统,以便于设计控制器。然而,需要注意的是,并非所有的车辆动力学模型都需要进行线性化。例如,车辆的运动学模型通常是非线性的,因此在设计控制器之前需要进行模型线性化。相反,车辆的动力学模型本身就是线性化模型,所以不需要进行线性化。
由于车辆的运动学模型是非线性的,所以在设计控制器之前需要进行模型线性化,而车辆的动力学模型本身就是线性化模型,所以不需要进行线性化。(所谓线性化,就是把不能写成X_dot=AX+Bu形式的非线性方程组,写成这种形式)
参照汽车运动学模型的线性化推导过程,可得到线性化后的无人车运动学模型。
1 线形化推导理论过程
2 运动学模型线形化
补充:推导方式二
参考:无人车系统(一):运动学模型及其线性化_无人集卡阿克曼转角模型-CSDN博客
说明
1.只有当在静态点附近非常小的变化时,线性化才有意义,否则线性化误差会非常大,即线性化的小偏差思想;
2.当工作点不同时,线性化的结果也不同;
3.线性化后的方程一般为增量形式。
4. 线性模型离散化
为什么要离散化?
由于计算机能够处理的数据都是离散化的,所以紧接着还要进行模型的离散化,主要使用积分中值定理和欧拉法可以进行推导。
补充:推导方式二
参考:无人车系统(一):运动学模型及其线性化_无人集卡阿克曼转角模型-CSDN博客
代码实现
1 python版本
import math import numpy as np class KinematicModel_3: """假设控制量为转向角delta_f和加速度a """ def __init__(self, x, y, psi, v, L, dt): self.x = x self.y = y self.psi = psi self.v = v self.L = L # 实现是离散的模型 self.dt = dt def update_state(self, a, delta_f): self.x = self.x+self.v*math.cos(self.psi)*self.dt self.y = self.y+self.v*math.sin(self.psi)*self.dt self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt self.v = self.v+a*self.dt def get_state(self): return self.x, self.y, self.psi, self.v def state_space(self, ref_delta, ref_yaw): """将模型离散化后的状态空间表达 Args: delta (_type_): 参考输入 Returns: _type_: _description_ """ A = np.matrix([ [1.0,0.0,-self.v*self.dt*math.sin(ref_yaw)], [0.0, 1.0, self.v*self.dt*math.cos(ref_yaw)], [0.0,0.0,1.0]]) B = np.matrix([ [self.dt*math.cos(ref_yaw), 0], [self.dt*math.sin(ref_yaw), 0], [self.dt*math.tan(ref_delta)/self.L, self.v*self.dt/(self.L*math.cos(ref_delta)*math.cos(ref_delta))] ]) return A,B
2 C++版本
KinematicModel.h #ifndef CHHROBOTICS_CPP_KINEMATICMODEL_H #define CHHROBOTICS_CPP_KINEMATICMODEL_H #include <Eigen/Dense> #include <cmath> #include <iostream> #include <vector> using namespace std; using namespace Eigen; class KinematicModel { public: double x, y, psi, v, L, dt; public: KinematicModel(); KinematicModel(double x, double y, double psi, double v, double l, double dt); vector<double> getState(); void updateState(double accel, double delta_f); vector<MatrixXd> stateSpace(double ref_delta, double ref_yaw); }; #endif // CHHROBOTICS_CPP_KINEMATICMODEL_H
KinematicModel.cpp #include "stanley/KinematicModel.h" /** * 机器人运动学模型构造 * @param x 位置x * @param y 位置y * @param psi 偏航角 * @param v 速度 * @param l 轴距 * @param dt 采样时间 */ KinematicModel::KinematicModel(double x, double y, double psi, double v, double l, double dt) : x(x), y(y), psi(psi), v(v), L(l), dt(dt) {} /** * 控制量为转向角delta_f和加速度a * @param accel 加速度 * @param delta_f 转向角控制量 */ void KinematicModel::updateState(double accel, double delta_f) { x = x + v* cos(psi)*dt; y = y + v*sin(psi)*dt; psi = psi + v / L * tan(delta_f)*dt; v = v + accel*dt; } /** * 状态获取 * @return */ vector<double> KinematicModel::getState() { return {x,y,psi,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}; }
main.cpp double x0 = 0.0, y0=-1.0,psi = 0.5,v=2,L=2,dt=0.1; //初始参数 KinematicModel ugv(x0,y0,psi,v,L,dt);//初始化参数 //机器人状态 vector<double>robot_state(4); robot_state = ugv.getState(); //获取车辆状态 ugv.updateState(0,delta_index[0]); //更新当前状态 //加速度设为0,恒速 x_.push_back(ugv.x); y_.push_back(ugv.y);
注意:运动学模型必须线性化之后再离散化才可供控制算法使用。
本文仅对运动学模型的推导和线性化离散化参考上述博主文献进行了整合,仅作为学习总结记录,如需详细理解请参考对应博主的讲解!