1108 lines
298 KiB
Plaintext
1108 lines
298 KiB
Plaintext
|
{
|
|||
|
|
"cells": [
|
|||
|
|
{
|
|||
|
|
"cell_type": "markdown",
|
|||
|
|
"metadata": {},
|
|||
|
|
"source": [
|
|||
|
|
"# 2.4 阿克曼运动学MPC\n",
|
|||
|
|
"本片笔记描述阿克曼底盘的运动学MPC控制算法"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "markdown",
|
|||
|
|
"source": [
|
|||
|
|
"## 阿克曼运动学模型建模\n",
|
|||
|
|
"\n",
|
|||
|
|
"系统的状态变量为：\n",
|
|||
|
|
"* $x$：机器人前向方向为正\n",
|
|||
|
|
"* $y$：机器人左侧方向为正\n",
|
|||
|
|
"* $\\theta$：机器人航向角\n",
|
|||
|
|
"\n",
|
|||
|
|
"系统的控制变量为：\n",
|
|||
|
|
"* $v$：机器人速度\n",
|
|||
|
|
"* $\\delta$：前轮转角\n",
|
|||
|
|
"\n",
|
|||
|
|
"根据两轮自行车模型，可以得到机器人的运动学模型：\n",
|
|||
|
|
"* $\\dot{x} = v \\cos(\\theta)$\n",
|
|||
|
|
"* $\\dot{y} = v \\sin(\\theta)$\n",
|
|||
|
|
"* $\\dot{theta} = \\frac{v \\tan(\\delta)}{L}$\n",
|
|||
|
|
"\n",
|
|||
|
|
"其中，$L$为机器人的轴距\n",
|
|||
|
|
"\n",
|
|||
|
|
"离散化后，可以得到：\n",
|
|||
|
|
"* $x_{t+1} = x_t + v_t \\cos(\\theta_t) \\Delta t$\n",
|
|||
|
|
"* $y_{t+1} = y_t + v_t \\sin(\\theta_t) \\Delta t$\n",
|
|||
|
|
"* $\\theta_{t+1} = \\theta_t + \\frac{v_t \\tan(\\delta_t)}{L} \\Delta t$\n",
|
|||
|
|
"\n",
|
|||
|
|
"这里，我们假设机器人的速度和转角是控制变量，即$v_t$和$\\delta_t$是已知的，我们可以通过上述公式计算出下一个时刻的状态。\n",
|
|||
|
|
"\n",
|
|||
|
|
"上述模型是一个非线性模型，我们可以通过线性化的方式来近似这个模型，这样就可以使用MPC来进行控制。\n",
|
|||
|
|
"\n",
|
|||
|
|
"使用泰勒级数在参考点$(\\bar{x}, \\bar{u})$处线性化模型(每步都需要重新线性化模型)：\n",
|
|||
|
|
"\n",
|
|||
|
|
"$ f(x,u) \\approx f(\\bar{x}, \\bar{u}) + \\frac{\\partial f}{\\partial x} (x - \\bar{x}) + \\frac{\\partial f}{\\partial u} (u - \\bar{u})$\n",
|
|||
|
|
"\n",
|
|||
|
|
"重写为状态空间的形式：\n",
|
|||
|
|
"\n",
|
|||
|
|
"$ f(x,u) \\approx f(\\bar{x}, \\bar{u}) + A|_{x=\\bar{x},u=\\bar{u}}(x - \\bar{x}) + B|_{x=\\bar{x},u=\\bar{u}}(u - \\bar{u})$\n",
|
|||
|
|
"\n",
|
|||
|
|
"其中\n",
|
|||
|
|
"\n",
|
|||
|
|
"$\n",
|
|||
|
|
"A =\n",
|
|||
|
|
"\\quad\n",
|
|||
|
|
"\\begin{bmatrix}\n",
|
|||
|
|
"\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial \\theta} \\\\\n",
|
|||
|
|
"\\end{bmatrix}\n",
|
|||
|
|
"\\quad\n",
|
|||
|
|
"=\n",
|
|||
|
|
"\\displaystyle \\left[\\begin{matrix}0 & 0 & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0\\end{matrix}\\right]\n",
|
|||
|
|
"$\n",
|
|||
|
|
"\n",
|
|||
|
|
"and\n",
|
|||
|
|
"\n",
|
|||
|
|
"$\n",
|
|||
|
|
"B = \n",
|
|||
|
|
"\\quad\n",
|
|||
|
|
"\\begin{bmatrix}\n",
|
|||
|
|
"\\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n",
|
|||
|
|
"\\end{bmatrix}\n",
|
|||
|
|
"\\quad\n",
|
|||
|
|
"= \n",
|
|||
|
|
"\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & 0\\\\ \\sin{\\left(\\theta \\right)} & 0\\\\ \\frac{\\tan{\\left(\\delta \\right)}}{L} & \\frac{v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]\n",
|
|||
|
|
"$\n",
|
|||
|
|
"\n",
|
|||
|
|
"矩阵A和B是雅可比矩阵\n",
|
|||
|
|
"\n",
|
|||
|
|
"所以，离散化后的模型为：\n",
|
|||
|
|
"\n",
|
|||
|
|
"* $ x_{t+1} = x_t + (f(\\bar{x}, \\bar{u}) + A|_{x=\\bar{x},u=\\bar{u}}(x - \\bar{x}) + B|_{x=\\bar{x},u=\\bar{u}}(u - \\bar{u})) \\Delta t $\n",
|
|||
|
|
"\n",
|
|||
|
|
"* $ x_{t+1} = (I + A\\Delta t)x_t + B\\Delta t u_t + \\Delta t (f(\\bar{x}, \\bar{u}) - A\\bar{x} - B\\bar{u}) $\n",
|
|||
|
|
"\n",
|
|||
|
|
"写为矩阵形式：\n",
|
|||
|
|
"\n",
|
|||
|
|
"* $ x_{t+1} = A'x_t + B'u_t + C' $\n",
|
|||
|
|
"\n",
|
|||
|
|
"* $ A' = I + \\Delta t A|_{x=\\bar{x},u=\\bar{u}} $\n",
|
|||
|
|
"\n",
|
|||
|
|
"* $ B' = \\Delta t B|_{x=\\bar{x},u=\\bar{u}} $\n",
|
|||
|
|
"\n",
|
|||
|
|
"* $ C' = \\Delta t (f(\\bar{x}, \\bar{u}) - A|_{x=\\bar{x},u=\\bar{u}}\\bar{x} - B|_{x=\\bar{x},u=\\bar{u}}\\bar{u}) $\n"
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"collapsed": false
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "markdown",
|
|||
|
|
"source": [
|
|||
|
|
"## 常微分方程模型\n",
|
|||
|
|
"用于仿真车辆运动，可作为真值来验证MPC控制的效果"
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"collapsed": false
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 20,
|
|||
|
|
"outputs": [],
|
|||
|
|
"source": [
|
|||
|
|
"import numpy as np\n",
|
|||
|
|
"from scipy.integrate import odeint\n",
|
|||
|
|
"from scipy.interpolate import interp1d\n",
|
|||
|
|
"import cvxpy as cp\n",
|
|||
|
|
"\n",
|
|||
|
|
"import matplotlib.pyplot as plt\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.style.use(\"ggplot\")\n",
|
|||
|
|
"\n",
|
|||
|
|
"import time\n",
|
|||
|
|
"\n",
|
|||
|
|
"# 定义常量\n",
|
|||
|
|
"N = 3 # 状态变量 x, y, theta\n",
|
|||
|
|
"M = 2 # 控制变量 v, delta\n",
|
|||
|
|
"T = 40 # 预测视野 \n",
|
|||
|
|
"DT = 0.2 # 离散步长 [s]\n",
|
|||
|
|
"L = 0.3 # 车辆长度 [m]\n",
|
|||
|
|
"\n",
|
|||
|
|
"# 使用连续性方程\n",
|
|||
|
|
"def kinematics_ode_model(x,t,u):\n",
|
|||
|
|
" \"\"\"\n",
|
|||
|
|
" Returns the set of ODE of the vehicle model.\n",
|
|||
|
|
" 返回车辆模型的ODE模型\n",
|
|||
|
|
" \"\"\"\n",
|
|||
|
|
"\n",
|
|||
|
|
"\n",
|
|||
|
|
" dxdt = u[0] * np.cos(x[2])\n",
|
|||
|
|
" dydt = u[0] * np.sin(x[2])\n",
|
|||
|
|
" dthetadt = u[0] * np.tan(u[1]) / L\n",
|
|||
|
|
"\n",
|
|||
|
|
" dqdt = [dxdt, dydt, dthetadt]\n",
|
|||
|
|
"\n",
|
|||
|
|
" return dqdt\n",
|
|||
|
|
"\n",
|
|||
|
|
"\n",
|
|||
|
|
"def predict(x0, u):\n",
|
|||
|
|
" \"\"\" \"\"\"\n",
|
|||
|
|
"\n",
|
|||
|
|
" x_ = np.zeros((N, T + 1))\n",
|
|||
|
|
"\n",
|
|||
|
|
" x_[:, 0] = x0\n",
|
|||
|
|
"\n",
|
|||
|
|
" # solve ODE\n",
|
|||
|
|
" for t in range(1, T + 1):\n",
|
|||
|
|
"\n",
|
|||
|
|
" tspan = [0, DT]\n",
|
|||
|
|
" x_next = odeint(kinematics_ode_model, x0, tspan, args=(u[:, t - 1],))\n",
|
|||
|
|
"\n",
|
|||
|
|
" x0 = x_next[1]\n",
|
|||
|
|
" x_[:, t] = x_next[1]\n",
|
|||
|
|
"\n",
|
|||
|
|
" return x_"
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"collapsed": false,
|
|||
|
|
"ExecuteTime": {
|
|||
|
|
"end_time": "2024-10-24T03:04:45.623427Z",
|
|||
|
|
"start_time": "2024-10-24T03:04:45.594094Z"
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 21,
|
|||
|
|
"outputs": [
|
|||
|
|
{
|
|||
|
|
"name": "stdout",
|
|||
|
|
"output_type": "stream",
|
|||
|
|
"text": [
|
|||
|
|
"CPU times: user 1.82 ms, sys: 15 μs, total: 1.83 ms\n",
|
|||
|
|
"Wall time: 1.84 ms\n"
|
|||
|
|
]
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"source": [
|
|||
|
|
"%%time\n",
|
|||
|
|
"\n",
|
|||
|
|
"u_bar = np.zeros((M, T))\n",
|
|||
|
|
"u_bar[0, :] = 1.0 # m/s\n",
|
|||
|
|
"u_bar[1, :] = np.radians(-np.pi / 4) # rad\n",
|
|||
|
|
"\n",
|
|||
|
|
"x0 = np.zeros(N)\n",
|
|||
|
|
"x0[0] = 0\n",
|
|||
|
|
"x0[1] = 1\n",
|
|||
|
|
"x0[2] = np.radians(0)\n",
|
|||
|
|
"\n",
|
|||
|
|
"x_bar = predict(x0, u_bar)"
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"collapsed": false,
|
|||
|
|
"ExecuteTime": {
|
|||
|
|
"end_time": "2024-10-24T03:04:46.199286Z",
|
|||
|
|
"start_time": "2024-10-24T03:04:46.187194Z"
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 22,
|
|||
|
|
"outputs": [
|
|||
|
|
{
|
|||
|
|
"data": {
|
|||
|
|
"text/plain": "<Figure size 640x480 with 2 Axes>",
|
|||
|
|
"image/png": "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
|
|||
|
|
},
|
|||
|
|
"metadata": {},
|
|||
|
|
"output_type": "display_data"
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"source": [
|
|||
|
|
"# plot trajectory\n",
|
|||
|
|
"plt.subplot(2, 2, 1)\n",
|
|||
|
|
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
|
|||
|
|
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
|
|||
|
|
"plt.axis(\"equal\")\n",
|
|||
|
|
"plt.ylabel(\"y\")\n",
|
|||
|
|
"plt.xlabel(\"x\")\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.subplot(2, 2, 2)\n",
|
|||
|
|
"plt.plot(np.degrees(x_bar[2, :]))\n",
|
|||
|
|
"plt.ylabel(\"theta(t) [deg]\")\n",
|
|||
|
|
"# plt.xlabel('time')\n",
|
|||
|
|
"\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.tight_layout()\n",
|
|||
|
|
"plt.show()"
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"collapsed": false,
|
|||
|
|
"ExecuteTime": {
|
|||
|
|
"end_time": "2024-10-24T03:04:46.816219Z",
|
|||
|
|
"start_time": "2024-10-24T03:04:46.759086Z"
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "markdown",
|
|||
|
|
"source": [
|
|||
|
|
"## 线性化状态空间模型"
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"collapsed": false
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 23,
|
|||
|
|
"outputs": [],
|
|||
|
|
"source": [
|
|||
|
|
"def get_linear_model(x_bar, u_bar):\n",
|
|||
|
|
" \"\"\"\n",
|
|||
|
|
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
|
|||
|
|
" \"\"\"\n",
|
|||
|
|
"\n",
|
|||
|
|
" x = x_bar[0]\n",
|
|||
|
|
" y = x_bar[1]\n",
|
|||
|
|
" theta = x_bar[2]\n",
|
|||
|
|
"\n",
|
|||
|
|
" v = u_bar[0]\n",
|
|||
|
|
" delta = u_bar[1]\n",
|
|||
|
|
"\n",
|
|||
|
|
" A = np.zeros((N, N))\n",
|
|||
|
|
" A[0, 2] = -v * np.sin(theta)\n",
|
|||
|
|
" A[1, 2] = v * np.cos(theta)\n",
|
|||
|
|
" A_lin = np.eye(N) + DT * A\n",
|
|||
|
|
"\n",
|
|||
|
|
" B = np.zeros((N, M))\n",
|
|||
|
|
" B[0, 0] = np.cos(theta)\n",
|
|||
|
|
" B[1, 0] = np.sin(theta)\n",
|
|||
|
|
" B[2, 0] = v * np.tan(delta) / L\n",
|
|||
|
|
" B[2, 1] = v / (L * np.cos(delta) ** 2)\n",
|
|||
|
|
" B_lin = DT * B\n",
|
|||
|
|
"\n",
|
|||
|
|
" f_xu = np.array(\n",
|
|||
|
|
" [v * np.cos(theta), v * np.sin(theta), v * np.tan(delta) / L]\n",
|
|||
|
|
" ).reshape(N, 1)\n",
|
|||
|
|
" C_lin = DT * (\n",
|
|||
|
|
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
|
|||
|
|
" )\n",
|
|||
|
|
"\n",
|
|||
|
|
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)"
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"collapsed": false,
|
|||
|
|
"ExecuteTime": {
|
|||
|
|
"end_time": "2024-10-24T03:04:48.033205Z",
|
|||
|
|
"start_time": "2024-10-24T03:04:48.027595Z"
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 24,
|
|||
|
|
"outputs": [
|
|||
|
|
{
|
|||
|
|
"name": "stdout",
|
|||
|
|
"output_type": "stream",
|
|||
|
|
"text": [
|
|||
|
|
"CPU times: user 2.04 ms, sys: 1.79 ms, total: 3.83 ms\n",
|
|||
|
|
"Wall time: 2.54 ms\n"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"name": "stderr",
|
|||
|
|
"output_type": "stream",
|
|||
|
|
"text": [
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:14: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" A[0, 2] = -v * np.sin(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:15: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" A[1, 2] = v * np.cos(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:19: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[0, 0] = np.cos(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:20: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[1, 0] = np.sin(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:21: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[2, 0] = v * np.tan(delta) / L\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:22: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[2, 1] = v / (L * np.cos(delta) ** 2)\n"
|
|||
|
|
]
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"source": [
|
|||
|
|
"%%time\n",
|
|||
|
|
"\n",
|
|||
|
|
"u_bar = np.zeros((M, T))\n",
|
|||
|
|
"u_bar[0, :] = 1.0 # m/s\n",
|
|||
|
|
"u_bar[1, :] = np.radians(-np.pi / 4) # rad\n",
|
|||
|
|
"\n",
|
|||
|
|
"x0 = np.zeros(N)\n",
|
|||
|
|
"x0[0] = 0\n",
|
|||
|
|
"x0[1] = 1\n",
|
|||
|
|
"x0[2] = np.radians(0)\n",
|
|||
|
|
"\n",
|
|||
|
|
"x_bar = np.zeros((N, T + 1))\n",
|
|||
|
|
"x_bar[:, 0] = x0\n",
|
|||
|
|
"\n",
|
|||
|
|
"for t in range(1, T + 1):\n",
|
|||
|
|
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
|
|||
|
|
" ut = u_bar[:, t - 1].reshape(M, 1)\n",
|
|||
|
|
"\n",
|
|||
|
|
" A, B, C = get_linear_model(xt, ut)\n",
|
|||
|
|
"\n",
|
|||
|
|
" xt_plus_one = np.dot(A, xt) + np.dot(B, ut) + C\n",
|
|||
|
|
"\n",
|
|||
|
|
" x_bar[:, t] = np.squeeze(xt_plus_one)"
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"collapsed": false,
|
|||
|
|
"ExecuteTime": {
|
|||
|
|
"end_time": "2024-10-24T03:04:48.720648Z",
|
|||
|
|
"start_time": "2024-10-24T03:04:48.703990Z"
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 25,
|
|||
|
|
"outputs": [
|
|||
|
|
{
|
|||
|
|
"data": {
|
|||
|
|
"text/plain": "<Figure size 640x480 with 2 Axes>",
|
|||
|
|
"image/png": "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
|
|||
|
|
},
|
|||
|
|
"metadata": {},
|
|||
|
|
"output_type": "display_data"
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"source": [
|
|||
|
|
"# plot trajectory\n",
|
|||
|
|
"plt.subplot(2, 2, 1)\n",
|
|||
|
|
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
|
|||
|
|
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
|
|||
|
|
"plt.axis(\"equal\")\n",
|
|||
|
|
"plt.ylabel(\"y\")\n",
|
|||
|
|
"plt.xlabel(\"x\")\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.subplot(2, 2, 2)\n",
|
|||
|
|
"plt.plot(np.degrees(x_bar[2, :]))\n",
|
|||
|
|
"plt.ylabel(\"theta(t)\")\n",
|
|||
|
|
"# plt.xlabel('time')\n",
|
|||
|
|
"\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.tight_layout()\n",
|
|||
|
|
"plt.show()"
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"collapsed": false,
|
|||
|
|
"ExecuteTime": {
|
|||
|
|
"end_time": "2024-10-24T03:04:49.296366Z",
|
|||
|
|
"start_time": "2024-10-24T03:04:49.239223Z"
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "markdown",
|
|||
|
|
"source": [
|
|||
|
|
"对比两个轨迹，可以看到线性化的模型能够很好的近似真实的模型"
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"collapsed": false
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "markdown",
|
|||
|
|
"metadata": {},
|
|||
|
|
"source": [
|
|||
|
|
"#### Linear MPC Formulation 线性MPC表述\n",
|
|||
|
|
"\n",
|
|||
|
|
"Linear MPC makes use of the **LTI** (Linear time invariant) discrete state space model, wich represents a motion model used for Prediction.\n",
|
|||
|
|
"线性MPC使用**LTI**（线性时不变）离散状态空间模型，该模型表示用于预测的运动模型。\n",
|
|||
|
|
"\n",
|
|||
|
|
"$x_{t+1} = Ax_t + Bu_t$\n",
|
|||
|
|
"\n",
|
|||
|
|
"The LTI formulation means that **future states** are linearly related to the current state and actuator signal. Hence, the MPC seeks to find a **control policy** U over a finite lenght horizon.\n",
|
|||
|
|
"LTI表述意味着**未来状态**与当前状态和执行器信号之间是线性相关的。因此，MPC试图找到一个有限长度视野上的**控制策略**U。\n",
|
|||
|
|
"\n",
|
|||
|
|
"$U={u_{t|t}, u_{t+1|t}, ...,u_{t+T|t}}$\n",
|
|||
|
|
"\n",
|
|||
|
|
"The objective function used minimize (drive the state to 0) is:\n",
|
|||
|
|
"用于最小化（将状态驱动至0）的目标函数为：\n",
|
|||
|
|
"\n",
|
|||
|
|
"$\n",
|
|||
|
|
"\\begin{equation}\n",
|
|||
|
|
"\\begin{aligned}\n",
|
|||
|
|
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}\\\\\n",
|
|||
|
|
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
|
|||
|
|
" & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
|
|||
|
|
"\\end{aligned}\n",
|
|||
|
|
"\\end{equation}\n",
|
|||
|
|
"$\n",
|
|||
|
|
"\n",
|
|||
|
|
"Other linear constrains may be applied,for instance on the control variable:\n",
|
|||
|
|
"其他线性约束也可以应用，比如对控制变量的约束\n",
|
|||
|
|
"\n",
|
|||
|
|
"$ U_{MIN} < u_{j|t} < U_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 $\n",
|
|||
|
|
"\n",
|
|||
|
|
"The objective fuction accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
|
|||
|
|
"目标函数考虑了状态偏离0的二次误差以及控制输入序列的二次误差。矩阵Q和R是权重矩阵，用于调整系统响应\n",
|
|||
|
|
"Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
|
|||
|
|
"由于目标是跟踪参考信号（例如轨迹），目标函数被重写为：\n",
|
|||
|
|
"\n",
|
|||
|
|
"$\n",
|
|||
|
|
"\\begin{equation}\n",
|
|||
|
|
"\\begin{aligned}\n",
|
|||
|
|
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}\n",
|
|||
|
|
"\\end{aligned}\n",
|
|||
|
|
"\\end{equation}\n",
|
|||
|
|
"$\n",
|
|||
|
|
"\n",
|
|||
|
|
"where the error w.r.t desired state is accounted for:\n",
|
|||
|
|
"其中考虑了相对于期望状态的误差：\n",
|
|||
|
|
"\n",
|
|||
|
|
"$ \\delta x = x_{j,t,ref} - x_{j,t} $"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "markdown",
|
|||
|
|
"metadata": {},
|
|||
|
|
"source": [
|
|||
|
|
"# Problem Formulation: Study case 研究案例问题表述\n",
|
|||
|
|
"\n",
|
|||
|
|
"In this case, the objective function to minimize is given by:\n",
|
|||
|
|
"在这种情况下，要最小化的目标函数为：\n",
|
|||
|
|
"\n",
|
|||
|
|
"https://borrelli.me.berkeley.edu/pdfpub/IV_KinematicMPC_jason.pdf\n",
|
|||
|
|
"\n",
|
|||
|
|
"$\n",
|
|||
|
|
"\\begin{equation}\n",
|
|||
|
|
"\\begin{aligned}\n",
|
|||
|
|
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} (x_{j} - x_{j,ref})^TQ(x_{j} - x_{j,ref}) + \\sum^{t+T-1}_{j=t+1} u^T_{j}Ru_{j} + (u_{j} - u_{j-1})^TP(u_{j} - u_{j-1}) \\\\\n",
|
|||
|
|
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
|
|||
|
|
" & x_{j+1} = Ax_{j}+Bu_{j} \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
|
|||
|
|
" & u_{MIN} < u_{j} < u_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
|
|||
|
|
" & \\dot{u}_{MIN} < \\frac{(u_{j} - u_{j-1})}{ts} < \\dot{u}_{MAX} \\quad \\textrm{for} \\quad t+1<j<t+T-1 \\\\\n",
|
|||
|
|
"\\end{aligned}\n",
|
|||
|
|
"\\end{equation}\n",
|
|||
|
|
"$\n",
|
|||
|
|
"\n",
|
|||
|
|
"\n",
|
|||
|
|
"Where R,P,Q are the cost matrices used to tune the response.\n",
|
|||
|
|
"R,P,Q是用于调整响应的成本矩阵。\n"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "markdown",
|
|||
|
|
"metadata": {},
|
|||
|
|
"source": [
|
|||
|
|
"## PRELIMINARIES 准备工作\n",
|
|||
|
|
"\n",
|
|||
|
|
"* linearized system dynamics 线性化系统动力学\n",
|
|||
|
|
"* function to represent the track 轨迹函数\n",
|
|||
|
|
"* function to represent the **reference trajectory** to track 跟踪的**参考轨迹**函数"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 28,
|
|||
|
|
"metadata": {
|
|||
|
|
"ExecuteTime": {
|
|||
|
|
"end_time": "2024-10-24T03:05:25.387867Z",
|
|||
|
|
"start_time": "2024-10-24T03:05:25.379154Z"
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
"outputs": [],
|
|||
|
|
"source": [
|
|||
|
|
"\"\"\"\n",
|
|||
|
|
"Control problem statement.\n",
|
|||
|
|
"控制问题陈述。\n",
|
|||
|
|
"\"\"\"\n",
|
|||
|
|
"\n",
|
|||
|
|
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
|
|||
|
|
" \"\"\"\n",
|
|||
|
|
" Computes a reference path given a set of waypoints\n",
|
|||
|
|
" 给定一组路径点，计算参考路径\n",
|
|||
|
|
" \"\"\"\n",
|
|||
|
|
"\n",
|
|||
|
|
" final_xp = []\n",
|
|||
|
|
" final_yp = []\n",
|
|||
|
|
" delta = step # [m]\n",
|
|||
|
|
"\n",
|
|||
|
|
" for idx in range(len(start_xp) - 1):\n",
|
|||
|
|
" section_len = np.sum(\n",
|
|||
|
|
" np.sqrt(\n",
|
|||
|
|
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
|
|||
|
|
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
|
|||
|
|
" )\n",
|
|||
|
|
" )\n",
|
|||
|
|
"\n",
|
|||
|
|
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
|
|||
|
|
"\n",
|
|||
|
|
" fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)\n",
|
|||
|
|
" fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)\n",
|
|||
|
|
"\n",
|
|||
|
|
" final_xp = np.append(final_xp, fx(interp_range))\n",
|
|||
|
|
" final_yp = np.append(final_yp, fy(interp_range))\n",
|
|||
|
|
"\n",
|
|||
|
|
" dx = np.append(0, np.diff(final_xp))\n",
|
|||
|
|
" dy = np.append(0, np.diff(final_yp))\n",
|
|||
|
|
" theta = np.arctan2(dy, dx)\n",
|
|||
|
|
"\n",
|
|||
|
|
" return np.vstack((final_xp, final_yp, theta))\n",
|
|||
|
|
"\n",
|
|||
|
|
"\n",
|
|||
|
|
"def get_nn_idx(state, path):\n",
|
|||
|
|
" \"\"\"\n",
|
|||
|
|
" Computes the index of the waypoint closest to vehicle\n",
|
|||
|
|
" 计算最接近车辆的路径点的索引\n",
|
|||
|
|
" \"\"\"\n",
|
|||
|
|
"\n",
|
|||
|
|
" dx = state[0] - path[0, :]\n",
|
|||
|
|
" dy = state[1] - path[1, :]\n",
|
|||
|
|
" dist = np.hypot(dx, dy)\n",
|
|||
|
|
" nn_idx = np.argmin(dist)\n",
|
|||
|
|
"\n",
|
|||
|
|
" try:\n",
|
|||
|
|
" v = [\n",
|
|||
|
|
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
|
|||
|
|
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
|
|||
|
|
" ]\n",
|
|||
|
|
" v /= np.linalg.norm(v)\n",
|
|||
|
|
"\n",
|
|||
|
|
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
|
|||
|
|
"\n",
|
|||
|
|
" if np.dot(d, v) > 0:\n",
|
|||
|
|
" target_idx = nn_idx\n",
|
|||
|
|
" else:\n",
|
|||
|
|
" target_idx = nn_idx + 1\n",
|
|||
|
|
"\n",
|
|||
|
|
" except IndexError as e:\n",
|
|||
|
|
" target_idx = nn_idx\n",
|
|||
|
|
"\n",
|
|||
|
|
" return target_idx\n",
|
|||
|
|
"\n",
|
|||
|
|
"\n",
|
|||
|
|
"def get_ref_trajectory(state, path, target_v):\n",
|
|||
|
|
" \"\"\"\n",
|
|||
|
|
" Adapted from pythonrobotics\n",
|
|||
|
|
" 获取参考轨迹\n",
|
|||
|
|
" 从当前位置开始，截取路径上的一段作为参考轨迹，其中v=固定的，参考的方向转角为0\n",
|
|||
|
|
" \"\"\"\n",
|
|||
|
|
" xref = np.zeros((N, T + 1))\n",
|
|||
|
|
" dref = np.zeros((M, T + 1))\n",
|
|||
|
|
"\n",
|
|||
|
|
" # sp = np.ones((1,T +1))*target_v #speed profile\n",
|
|||
|
|
"\n",
|
|||
|
|
" ncourse = path.shape[1]\n",
|
|||
|
|
"\n",
|
|||
|
|
" ind = get_nn_idx(state, path)\n",
|
|||
|
|
"\n",
|
|||
|
|
" xref[0, 0] = path[0, ind] # X\n",
|
|||
|
|
" xref[1, 0] = path[1, ind] # Y\n",
|
|||
|
|
" xref[2, 0] = path[2, ind] # Theta\n",
|
|||
|
|
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
|
|||
|
|
" dref[1, 0] = 0.0 # speed\n",
|
|||
|
|
"\n",
|
|||
|
|
" dl = 0.05 # Waypoints spacing [m]\n",
|
|||
|
|
" travel = 0.0\n",
|
|||
|
|
"\n",
|
|||
|
|
" for i in range(T + 1):\n",
|
|||
|
|
" travel += abs(target_v) * DT # current V or target V?\n",
|
|||
|
|
" dind = int(round(travel / dl))\n",
|
|||
|
|
"\n",
|
|||
|
|
" if (ind + dind) < ncourse:\n",
|
|||
|
|
" xref[0, i] = path[0, ind + dind]\n",
|
|||
|
|
" xref[1, i] = path[1, ind + dind]\n",
|
|||
|
|
" xref[2, i] = path[2, ind + dind]\n",
|
|||
|
|
" dref[0, i] = 0.0\n",
|
|||
|
|
" dref[1, i] = target_v\n",
|
|||
|
|
" else:\n",
|
|||
|
|
" xref[0, i] = path[0, ncourse - 1]\n",
|
|||
|
|
" xref[1, i] = path[1, ncourse - 1]\n",
|
|||
|
|
" xref[2, i] = path[2, ncourse - 1]\n",
|
|||
|
|
" dref[0, i] = 0.0\n",
|
|||
|
|
" dref[1, i] = 0.0\n",
|
|||
|
|
"\n",
|
|||
|
|
" return xref, dref"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "markdown",
|
|||
|
|
"metadata": {},
|
|||
|
|
"source": [
|
|||
|
|
"## MPC \n",
|
|||
|
|
"\n",
|
|||
|
|
"test single iteration\n",
|
|||
|
|
"测试单次迭代"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 29,
|
|||
|
|
"metadata": {
|
|||
|
|
"ExecuteTime": {
|
|||
|
|
"end_time": "2024-10-24T03:05:26.555645Z",
|
|||
|
|
"start_time": "2024-10-24T03:05:26.481067Z"
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
"outputs": [
|
|||
|
|
{
|
|||
|
|
"name": "stdout",
|
|||
|
|
"output_type": "stream",
|
|||
|
|
"text": [
|
|||
|
|
"===============================================================================\n",
|
|||
|
|
" CVXPY \n",
|
|||
|
|
" v1.5.3 \n",
|
|||
|
|
"===============================================================================\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Your problem has 203 variables, 243 constraints, and 0 parameters.\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: It is compliant with the following grammars: DCP, DQCP\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: (If you need to solve this problem multiple times, but with different data, consider using parameters.)\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: CVXPY will first compile your problem; then, it will invoke a numerical solver to obtain a solution.\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Your problem is compiled with the CPP canonicalization backend.\n",
|
|||
|
|
"-------------------------------------------------------------------------------\n",
|
|||
|
|
" Compilation \n",
|
|||
|
|
"-------------------------------------------------------------------------------\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Compiling problem (target solver=OSQP).\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Reduction chain: CvxAttr2Constr -> Qp2SymbolicQp -> QpMatrixStuffing -> OSQP\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Applying reduction CvxAttr2Constr\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Applying reduction Qp2SymbolicQp\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Applying reduction QpMatrixStuffing\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Applying reduction OSQP\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Finished problem compilation (took 5.022e-02 seconds).\n",
|
|||
|
|
"-------------------------------------------------------------------------------\n",
|
|||
|
|
" Numerical solver \n",
|
|||
|
|
"-------------------------------------------------------------------------------\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Invoking solver OSQP to obtain a solution.\n",
|
|||
|
|
"-------------------------------------------------------------------------------\n",
|
|||
|
|
" Summary \n",
|
|||
|
|
"-------------------------------------------------------------------------------\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Problem status: optimal\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Optimal value: 4.687e+02\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Compilation took 5.022e-02 seconds\n",
|
|||
|
|
"(CVXPY) Oct 24 11:05:26 AM: Solver (including time spent in interface) took 3.240e-03 seconds\n",
|
|||
|
|
"CPU times: user 77.8 ms, sys: 7.17 ms, total: 85 ms\n",
|
|||
|
|
"Wall time: 87.9 ms\n"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"name": "stderr",
|
|||
|
|
"output_type": "stream",
|
|||
|
|
"text": [
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:14: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" A[0, 2] = -v * np.sin(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:15: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" A[1, 2] = v * np.cos(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:19: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[0, 0] = np.cos(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:20: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[1, 0] = np.sin(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:21: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[2, 0] = v * np.tan(delta) / L\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:22: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[2, 1] = v / (L * np.cos(delta) ** 2)\n"
|
|||
|
|
]
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"source": [
|
|||
|
|
"%%time\n",
|
|||
|
|
"\n",
|
|||
|
|
"# 限制条件\n",
|
|||
|
|
"MAX_SPEED = 1.5 # m/s\n",
|
|||
|
|
"MAX_STEER = np.radians(30) # rad\n",
|
|||
|
|
"REF_VEL = 1.0 # 目标路径参考速度\n",
|
|||
|
|
"\n",
|
|||
|
|
"#获取参考轨迹，线性插值，三个点[0,0],[3,0],[6,0]\n",
|
|||
|
|
"track = compute_path_from_wp([0, 3, 6], [0, 0, 0], 0.05)\n",
|
|||
|
|
"\n",
|
|||
|
|
"# Starting Condition 初始条件\n",
|
|||
|
|
"x0 = np.zeros(N)\n",
|
|||
|
|
"x0[0] = 0 # x\n",
|
|||
|
|
"x0[1] = -0.25 # y\n",
|
|||
|
|
"x0[2] = np.radians(-0) # yaw\n",
|
|||
|
|
"\n",
|
|||
|
|
"# starting guess 开始猜测\n",
|
|||
|
|
"u_bar = np.zeros((M, T))\n",
|
|||
|
|
"u_bar[0, :] = REF_VEL # v\n",
|
|||
|
|
"u_bar[1, :] = 0.1 # delta\n",
|
|||
|
|
"\n",
|
|||
|
|
"# dynamics starting state w.r.t world frame 与世界坐标系相关的动力学起始状态\n",
|
|||
|
|
"x_bar = np.zeros((N, T + 1)) # 4x21\n",
|
|||
|
|
"x_bar[:, 0] = x0\n",
|
|||
|
|
"\n",
|
|||
|
|
"# prediction for linearization of costrains 用于约束线性化的预测\n",
|
|||
|
|
"# 这部分应用线性模型，得到预测的轨迹\n",
|
|||
|
|
"for t in range(1, T + 1):\n",
|
|||
|
|
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
|
|||
|
|
" ut = u_bar[:, t - 1].reshape(M, 1)\n",
|
|||
|
|
" A, B, C = get_linear_model(xt, ut) # 获取在t - 1时刻的线性近似模型\n",
|
|||
|
|
" xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
|
|||
|
|
" x_bar[:, t] = xt_plus_one # 获取t时刻的状态\n",
|
|||
|
|
"\n",
|
|||
|
|
"# x_bar是根据猜测的u_bar获取的预测状态估计\n",
|
|||
|
|
"\n",
|
|||
|
|
"# CVXPY Linear MPC problem statement CVXPY线性MPC问题陈述\n",
|
|||
|
|
"x = cp.Variable((N, T + 1)) # 4x21维，状态向量\n",
|
|||
|
|
"u = cp.Variable((M, T)) # 2x20维，控制向量\n",
|
|||
|
|
"cost = 0\n",
|
|||
|
|
"constr = []\n",
|
|||
|
|
"\n",
|
|||
|
|
"# Cost Matrices\n",
|
|||
|
|
"Q = np.diag([10, 10, 10]) # state error cost 状态误差成本\n",
|
|||
|
|
"Qf = np.diag([10, 10, 10]) # state final error cost 最终状态误差成本\n",
|
|||
|
|
"R = np.diag([10, 10]) # input cost 输入成本\n",
|
|||
|
|
"R_ = np.diag([10, 10]) # input rate of change cost 输入变化率成本\n",
|
|||
|
|
"\n",
|
|||
|
|
"# Get Reference_traj 获取参考轨迹，根据当前位置截取的路径上的一系列点，并赋值目标速度和转角\n",
|
|||
|
|
"# x_ref 表示参考状态，d_ref表示参考转角\n",
|
|||
|
|
"x_ref, d_ref = get_ref_trajectory(x_bar[:, 0], track, REF_VEL)\n",
|
|||
|
|
"\n",
|
|||
|
|
"# Prediction Horizon 预测视野\n",
|
|||
|
|
"for t in range(T):\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Tracking Error 跟踪误差\n",
|
|||
|
|
" cost += cp.quad_form(x[:, t] - x_ref[:, t], Q)\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Actuation effort 执行努力\n",
|
|||
|
|
" cost += cp.quad_form(u[:, t], R)\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Actuation rate of change 变化率\n",
|
|||
|
|
" if t < (T - 1):\n",
|
|||
|
|
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Kinrmatics Constrains (Linearized model) 运动学约束（线性化模型）\n",
|
|||
|
|
" A, B, C = get_linear_model(x_bar[:, t], u_bar[:, t])\n",
|
|||
|
|
" constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
|
|||
|
|
"\n",
|
|||
|
|
"# Final Point tracking 最终点跟踪\n",
|
|||
|
|
"cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
|
|||
|
|
"\n",
|
|||
|
|
"# sums problem objectives and concatenates constraints. 求和问题目标并连接约束。\n",
|
|||
|
|
"constr += [x[:, 0] == x_bar[:, 0]] # starting condition 初始条件\n",
|
|||
|
|
"constr += [u[0, :] <= MAX_SPEED] # max speed 最大速度\n",
|
|||
|
|
"constr += [u[0, :] >= 0.0] # min_speed (not really needed) 最小速度（实际上不需要）\n",
|
|||
|
|
"constr += [cp.abs(u[1, :]) <= MAX_STEER] # max steer 最大转向\n",
|
|||
|
|
"# for t in range(T):\n",
|
|||
|
|
"# if t < (T - 1):\n",
|
|||
|
|
"# constr += [cp.abs(u[0,t] - u[0,t-1])/DT <= MAX_ACC] #max acc\n",
|
|||
|
|
"# constr += [cp.abs(u[1,t] - u[1,t-1])/DT <= MAX_STEER] #max steer\n",
|
|||
|
|
"\n",
|
|||
|
|
"prob = cp.Problem(cp.Minimize(cost), constr) # 构建问题\n",
|
|||
|
|
"solution = prob.solve(solver=cp.OSQP, verbose=True) # 求解"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 30,
|
|||
|
|
"metadata": {
|
|||
|
|
"ExecuteTime": {
|
|||
|
|
"end_time": "2024-10-24T03:05:27.460730Z",
|
|||
|
|
"start_time": "2024-10-24T03:05:27.349153Z"
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
"outputs": [
|
|||
|
|
{
|
|||
|
|
"data": {
|
|||
|
|
"text/plain": "<Figure size 640x480 with 4 Axes>",
|
|||
|
|
"image/png": "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
|
|||
|
|
},
|
|||
|
|
"metadata": {},
|
|||
|
|
"output_type": "display_data"
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"source": [
|
|||
|
|
"x_mpc = np.array(x.value[0, :]).flatten()\n",
|
|||
|
|
"y_mpc = np.array(x.value[1, :]).flatten()\n",
|
|||
|
|
"theta_mpc = np.array(x.value[2, :]).flatten()\n",
|
|||
|
|
"v_mpc = np.array(u.value[0, :]).flatten()\n",
|
|||
|
|
"delta_mpc = np.array(u.value[1, :]).flatten()\n",
|
|||
|
|
"\n",
|
|||
|
|
"# simulate robot state trajectory for optimized U\n",
|
|||
|
|
"x_traj = predict(x0, np.vstack((v_mpc, delta_mpc)))\n",
|
|||
|
|
"\n",
|
|||
|
|
"# plt.figure(figsize=(15,10))\n",
|
|||
|
|
"# plot trajectory\n",
|
|||
|
|
"plt.subplot(2, 2, 1)\n",
|
|||
|
|
"plt.plot(track[0, :], track[1, :], \"b\")\n",
|
|||
|
|
"plt.plot(x_ref[0, :], x_ref[1, :], \"g+\")\n",
|
|||
|
|
"plt.plot(x_traj[0, :], x_traj[1, :]) #根据mpc优化后的a和delta，预测的轨迹\n",
|
|||
|
|
"plt.axis(\"equal\")\n",
|
|||
|
|
"plt.ylabel(\"y\")\n",
|
|||
|
|
"plt.xlabel(\"x\")\n",
|
|||
|
|
"\n",
|
|||
|
|
"# plot v(t)\n",
|
|||
|
|
"plt.subplot(2, 2, 3)\n",
|
|||
|
|
"plt.plot(v_mpc)\n",
|
|||
|
|
"plt.ylabel(\"v_in(t)\")\n",
|
|||
|
|
"# plt.xlabel('time')\n",
|
|||
|
|
"\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.subplot(2, 2, 2)\n",
|
|||
|
|
"plt.plot(theta_mpc) \n",
|
|||
|
|
"plt.ylabel(\"theta(t)\") # 航向角\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.subplot(2, 2, 4)\n",
|
|||
|
|
"plt.plot(delta_mpc)\n",
|
|||
|
|
"plt.ylabel(\"d_in(t)\") # 前轮转角\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.tight_layout()\n",
|
|||
|
|
"plt.show()\n",
|
|||
|
|
"\n",
|
|||
|
|
"# 下图展示的结果并不准确\n",
|
|||
|
|
"# 这是因为在做约束条件中，xt+1 = Axt + But + C时，A，B，C是线性化的模型，且是基于猜测的x_bar和u_bar来线性化的\n",
|
|||
|
|
"# 所以需要通过滚动优化，获得更准确的猜测和状态猜测\n",
|
|||
|
|
"# 这里有一个问题，那是否要将这次计算的u，作为下一次的猜测基础？"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "markdown",
|
|||
|
|
"metadata": {},
|
|||
|
|
"source": [
|
|||
|
|
"## full track demo "
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 31,
|
|||
|
|
"metadata": {
|
|||
|
|
"ExecuteTime": {
|
|||
|
|
"end_time": "2024-10-24T03:11:52.521780Z",
|
|||
|
|
"start_time": "2024-10-24T03:11:26.227974Z"
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
"outputs": [
|
|||
|
|
{
|
|||
|
|
"name": "stderr",
|
|||
|
|
"output_type": "stream",
|
|||
|
|
"text": [
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:14: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" A[0, 2] = -v * np.sin(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:15: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" A[1, 2] = v * np.cos(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:19: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[0, 0] = np.cos(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:20: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[1, 0] = np.sin(theta)\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:21: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[2, 0] = v * np.tan(delta) / L\n",
|
|||
|
|
"/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_9821/761030008.py:22: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
|
|||
|
|
" B[2, 1] = v / (L * np.cos(delta) ** 2)\n"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"name": "stdout",
|
|||
|
|
"output_type": "stream",
|
|||
|
|
"text": [
|
|||
|
|
"CVXPY Optimization Time: Avrg: 0.1320s Max: 0.1657s Min: 0.1066s\n"
|
|||
|
|
]
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"source": [
|
|||
|
|
"track = compute_path_from_wp(\n",
|
|||
|
|
" [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
|
|||
|
|
")\n",
|
|||
|
|
"\n",
|
|||
|
|
"# track = compute_path_from_wp([0,10,10,0],\n",
|
|||
|
|
"# [0,0,1,1],0.05)\n",
|
|||
|
|
"\n",
|
|||
|
|
"sim_duration = 200 # time steps\n",
|
|||
|
|
"opt_time = []\n",
|
|||
|
|
"\n",
|
|||
|
|
"x_sim = np.zeros((N, sim_duration))\n",
|
|||
|
|
"x_sim[0,0] = 0 # x\n",
|
|||
|
|
"x_sim[1,0] = -0.25 # y\n",
|
|||
|
|
"x_sim[2,0] = np.radians(0.0) # yaw\n",
|
|||
|
|
"u_sim = np.zeros((M, sim_duration - 1))\n",
|
|||
|
|
"\n",
|
|||
|
|
"MAX_SPEED = 1.5 # m/s\n",
|
|||
|
|
"MAX_D_ACC = 0.5 # m/s^2\n",
|
|||
|
|
"MAX_STEER = np.radians(30) # rad\n",
|
|||
|
|
"MAX_D_STEER = np.radians(30) # rad/s\n",
|
|||
|
|
"\n",
|
|||
|
|
"REF_VEL = 1.0 # m/s\n",
|
|||
|
|
"\n",
|
|||
|
|
"# Starting Condition\n",
|
|||
|
|
"x0 = np.zeros(N)\n",
|
|||
|
|
"x0 = x_sim[:, 0]\n",
|
|||
|
|
"\n",
|
|||
|
|
"# starting guess\n",
|
|||
|
|
"u_bar = np.zeros((M, T))\n",
|
|||
|
|
"u_bar[0, :] = 1.0 # v\n",
|
|||
|
|
"u_bar[1, :] = 0.0 # delta\n",
|
|||
|
|
"\n",
|
|||
|
|
"for sim_time in range(sim_duration - 1):\n",
|
|||
|
|
"\n",
|
|||
|
|
" iter_start = time.time()\n",
|
|||
|
|
"\n",
|
|||
|
|
" # dynamics starting state\n",
|
|||
|
|
" # 获取当前时刻的状态，x_sim是通过ode计算出的真值\n",
|
|||
|
|
" x_bar = np.zeros((N, T + 1))\n",
|
|||
|
|
" x_bar[:, 0] = x_sim[:, sim_time]\n",
|
|||
|
|
"\n",
|
|||
|
|
" # prediction for linearization of costrains\n",
|
|||
|
|
" # 获取各参考点处的线性化模型参数\n",
|
|||
|
|
" for t in range(1, T + 1):\n",
|
|||
|
|
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
|
|||
|
|
" ut = u_bar[:, t - 1].reshape(M, 1)\n",
|
|||
|
|
" A, B, C = get_linear_model(xt, ut)\n",
|
|||
|
|
" xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
|
|||
|
|
" x_bar[:, t] = xt_plus_one\n",
|
|||
|
|
"\n",
|
|||
|
|
" # CVXPY Linear MPC problem statement\n",
|
|||
|
|
" # 构建MPC问题和求解器\n",
|
|||
|
|
" x = cp.Variable((N, T + 1))\n",
|
|||
|
|
" u = cp.Variable((M, T))\n",
|
|||
|
|
" cost = 0\n",
|
|||
|
|
" constr = []\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Cost Matrices\n",
|
|||
|
|
" Q = np.diag([20, 20, 0]) # state error cost\n",
|
|||
|
|
" Qf = np.diag([30, 30, 0]) # state final error cost\n",
|
|||
|
|
" R = np.diag([10, 10]) # input cost\n",
|
|||
|
|
" R_ = np.diag([30, 10]) # input rate of change cost\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Get Reference_traj\n",
|
|||
|
|
" x_ref, d_ref = get_ref_trajectory(x_bar[:, 0], track, REF_VEL)\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Prediction Horizon\n",
|
|||
|
|
" for t in range(T):\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Tracking Error\n",
|
|||
|
|
" cost += cp.quad_form(x[:, t] - x_ref[:, t], Q)\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Actuation effort\n",
|
|||
|
|
" cost += cp.quad_form(u[:, t], R)\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Actuation rate of change\n",
|
|||
|
|
" if t < (T - 1):\n",
|
|||
|
|
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
|
|||
|
|
" constr += [\n",
|
|||
|
|
" cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
|
|||
|
|
" ] # max acc rate of change\n",
|
|||
|
|
" constr += [\n",
|
|||
|
|
" cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
|
|||
|
|
" ] # max steer rate of change\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Kinrmatics Constrains (Linearized model)\n",
|
|||
|
|
" A, B, C = get_linear_model(x_bar[:, t], u_bar[:, t])\n",
|
|||
|
|
" constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Final Point tracking\n",
|
|||
|
|
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
|
|||
|
|
"\n",
|
|||
|
|
" # sums problem objectives and concatenates constraints.\n",
|
|||
|
|
" constr += [x[:, 0] == x_bar[:, 0]] # starting condition\n",
|
|||
|
|
" constr += [u[0, :] <= MAX_SPEED] # max speed\n",
|
|||
|
|
" constr += [u[0, :] >= 0.0] # min_speed (not really needed)\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Solve\n",
|
|||
|
|
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
|
|||
|
|
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
|
|||
|
|
"\n",
|
|||
|
|
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
|
|||
|
|
" # 将本次计算出的u_bar作为下次猜测的起点\n",
|
|||
|
|
" u_bar = np.vstack(\n",
|
|||
|
|
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
|
|||
|
|
" )\n",
|
|||
|
|
" \n",
|
|||
|
|
" # 本次的执行值\n",
|
|||
|
|
" u_sim[:, sim_time] = u_bar[:, 0]\n",
|
|||
|
|
"\n",
|
|||
|
|
" # Measure elpased time to get results from cvxpy\n",
|
|||
|
|
" opt_time.append(time.time() - iter_start)\n",
|
|||
|
|
"\n",
|
|||
|
|
" # 用ode模型仿真车辆运动\n",
|
|||
|
|
" # move simulation to t+1\n",
|
|||
|
|
" tspan = [0, DT]\n",
|
|||
|
|
" x_sim[:, sim_time + 1] = odeint(\n",
|
|||
|
|
" kinematics_ode_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
|
|||
|
|
" )[1]\n",
|
|||
|
|
"\n",
|
|||
|
|
"print(\n",
|
|||
|
|
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
|
|||
|
|
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
|
|||
|
|
" )\n",
|
|||
|
|
")"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 33,
|
|||
|
|
"metadata": {
|
|||
|
|
"ExecuteTime": {
|
|||
|
|
"end_time": "2024-10-24T03:14:58.916991Z",
|
|||
|
|
"start_time": "2024-10-24T03:14:58.730488Z"
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
"outputs": [
|
|||
|
|
{
|
|||
|
|
"data": {
|
|||
|
|
"text/plain": "<Figure size 1500x1000 with 4 Axes>",
|
|||
|
|
"image/png": "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
|
|||
|
|
},
|
|||
|
|
"metadata": {},
|
|||
|
|
"output_type": "display_data"
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"source": [
|
|||
|
|
"# plot trajectory\n",
|
|||
|
|
"grid = plt.GridSpec(4, 5)\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.figure(figsize=(15, 10))\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.subplot(grid[0:4, 0:4])\n",
|
|||
|
|
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
|
|||
|
|
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
|
|||
|
|
"plt.axis(\"equal\")\n",
|
|||
|
|
"plt.ylabel(\"y\")\n",
|
|||
|
|
"plt.xlabel(\"x\")\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.subplot(grid[0, 4])\n",
|
|||
|
|
"plt.plot(u_sim[0, :])\n",
|
|||
|
|
"plt.ylabel(\"v(t) [m/ss]\")\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.subplot(grid[1, 4])\n",
|
|||
|
|
"plt.plot(np.degrees(u_sim[1, :]))\n",
|
|||
|
|
"plt.ylabel(\"delta(t) [rad]\")\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.subplot(grid[2, 4])\n",
|
|||
|
|
"plt.plot(x_sim[2, :])\n",
|
|||
|
|
"plt.ylabel(\"theta(t) [rad]\")\n",
|
|||
|
|
"\n",
|
|||
|
|
"plt.tight_layout()\n",
|
|||
|
|
"plt.show()"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": null,
|
|||
|
|
"outputs": [],
|
|||
|
|
"source": [],
|
|||
|
|
"metadata": {
|
|||
|
|
"collapsed": false
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"kernelspec": {
|
|||
|
|
"name": "python3",
|
|||
|
|
"language": "python",
|
|||
|
|
"display_name": "Python 3 (ipykernel)"
|
|||
|
|
},
|
|||
|
|
"language_info": {
|
|||
|
|
"codemirror_mode": {
|
|||
|
|
"name": "ipython",
|
|||
|
|
"version": 3
|
|||
|
|
},
|
|||
|
|
"file_extension": ".py",
|
|||
|
|
"mimetype": "text/x-python",
|
|||
|
|
"name": "python",
|
|||
|
|
"nbconvert_exporter": "python",
|
|||
|
|
"pygments_lexer": "ipython3",
|
|||
|
|
"version": "3.8.5"
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
"nbformat": 4,
|
|||
|
|
"nbformat_minor": 4
|
|||
|
|
}
|