mpc错误修正

yugit12 · Sep 14, 2022 · abd2b3f · abd2b3f
1 parent f821284
commit abd2b3f
Show file tree

Hide file tree

Showing 3 changed files with 44 additions and 17 deletions.
diff --git a/Controllers/MPC/mpc.ipynb b/Controllers/MPC/mpc.ipynb
@@ -77,24 +77,26 @@
     "    \"\"\"将模型离散化后的状态空间表达\n",
     "\n",
     "    Args:\n",
-    "        delta (_type_): 参考输入\n",
+    "        ref_delta (_type_): 参考的转角控制量\n",
+    "        ref_yaw (_type_): 参考的偏航角\n",
     "\n",
     "    Returns:\n",
     "        _type_: _description_\n",
     "    \"\"\"\n",
+    "\n",
     "    A = np.matrix([\n",
-    "      [1.0,0.0,-self.v*self.dt*math.sin(ref_yaw)],\n",
-    "      [0.0, 1.0, self.v*self.dt*math.cos(ref_yaw)],\n",
-    "      [0.0,0.0,1.0]])\n",
-    "    \n",
+    "        [1.0, 0.0, -self.v*self.dt*math.sin(ref_yaw)],\n",
+    "        [0.0, 1.0, self.v*self.dt*math.cos(ref_yaw)],\n",
+    "        [0.0, 0.0, 1.0]])\n",
+    "\n",
     "    B = np.matrix([\n",
     "        [self.dt*math.cos(ref_yaw), 0],\n",
     "        [self.dt*math.sin(ref_yaw), 0],\n",
-    "      [self.dt*math.tan(ref_delta), self.v*self.dt/(self.L*math.cos(ref_delta)*math.cos(ref_delta))]\n",
-    "                  ])\n",
+    "        [self.dt*math.tan(ref_yaw), self.v*self.dt /(self.L*math.cos(ref_delta)*math.cos(ref_delta))]\n",
+    "    ])\n",
     "\n",
     "    C = np.eye(3)\n",
-    "    return A,B,C\n",
+    "    return A, B, C\n",
     "\n"
    ]
   },
@@ -327,7 +329,7 @@
     "        if t != 0:\n",
     "            cost += cvxpy.quad_form(x[:, t] - xref[:, t], Q)\n",
     "\n",
-    "        A, B, C = ugv.state_space(delta_ref[0,t], xref[2,t])\n",
+    "        A, B, C = ugv.state_space(delta_ref[1,t], xref[2,t])\n",
     "        constraints += [x[:, t + 1]-xref[:, t+1] == A @ (x[:, t]-xref[:, t]) + B @ (u[:, t]-delta_ref[:,t]) ]\n",
     "\n",
     "        # if t < (T - 1):\n",

diff --git a/Controllers/MPC/mpc.py b/Controllers/MPC/mpc.py
@@ -59,11 +59,13 @@ def state_space(self, ref_delta, ref_yaw):
     """将模型离散化后的状态空间表达
 
     Args:
-        delta (_type_): 参考输入
+        ref_delta (_type_): 参考的转角控制量
+        ref_yaw (_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)],
@@ -72,8 +74,7 @@ def state_space(self, ref_delta, ref_yaw):
     B = np.matrix([
         [self.dt*math.cos(ref_yaw), 0],
         [self.dt*math.sin(ref_yaw), 0],
-        [self.dt*math.tan(ref_delta), self.v*self.dt /
-         (self.L*math.cos(ref_delta)*math.cos(ref_delta))]
+        [self.dt*math.tan(ref_yaw), self.v*self.dt /(self.L*math.cos(ref_delta)*math.cos(ref_delta))]
     ])
 
     C = np.eye(3)
@@ -227,14 +228,10 @@ def linear_mpc_control(xref, x0, delta_ref, ugv):
         if t != 0:
             cost += cvxpy.quad_form(x[:, t] - xref[:, t], Q)
 
-        A, B, C = ugv.state_space(delta_ref[0, t], xref[2, t])
+        A, B, C = ugv.state_space(delta_ref[1, t], xref[2, t])
         constraints += [x[:, t + 1]-xref[:, t+1] == A @
                         (x[:, t]-xref[:, t]) + B @ (u[:, t]-delta_ref[:, t])]
 
-        # if t < (T - 1):
-        # cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd)
-        # constraints += [cvxpy.abs(u[1, t + 1] - u[1, t]) <=
-        #                 MAX_DSTEER * ugv.dt]
 
     cost += cvxpy.quad_form(x[:, T] - xref[:, T], Qf)
 

diff --git a/code_test.ipynb b/code_test.ipynb
@@ -1,5 +1,25 @@
 {
  "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "1.5"
+      ]
+     },
+     "execution_count": 2,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "3/2"
+   ]
+  },
   {
    "cell_type": "code",
    "execution_count": null,
@@ -15,7 +35,15 @@
    "name": "python3"
   },
   "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.12"
   },
   "orig_nbformat": 4,