add 动力学横向误差模型

Dynamics/Kinematic_Lateral_Control_Error_Model.ipynb

@@ -0,0 +1,130 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 65,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import numpy as np\n",
+    "\n",
+    "class LateralErrorModel:\n",
+    "    \"\"\"\n",
+    "    This model describes the lateral motion of a vehicle and its response to various inputs.\n",
+    "\n",
+    "    Parameters:\n",
+    "        self.m (float): Mass of the vehicle (kg).\n",
+    "        self.Vx (float): Longitudinal velocity (self.m/s).\n",
+    "        self.C_alpha_f (float): Front tire cornering stiffness (N/rad).\n",
+    "        self.C_alpha_r (float): Rear tire cornering stiffness (N/rad).\n",
+    "        self.l_f (float): Distance from the vehicle's center of mass to the front axle (self.m).\n",
+    "        self.l_r (float): Distance from the vehicle's center of mass to the rear axle (self.m).\n",
+    "        self.I_z (float): Moment of inertia about the vertical axis (kg self.m^2).\n",
+    "        self.g (float): Acceleration due to gravity (self.m/s^2).\n",
+    "    \"\"\"\n",
+    "    def __init__(self, m, Vx, C_alpha_f, C_alpha_r, l_f, l_r, I_z, g):\n",
+    "        self.m = m\n",
+    "        self.Vx = Vx\n",
+    "        self.C_alpha_f = C_alpha_f\n",
+    "        self.C_alpha_r = C_alpha_r\n",
+    "        self.l_f = l_f\n",
+    "        self.l_r = l_r\n",
+    "        self.I_z = I_z\n",
+    "        self.g = g\n",
+    "        \n",
+    "    def GenerateStateSpace(self):\n",
+    "        A = np.array([[0, 1, 0, 0],\n",
+    "                           [0, -(2*self.C_alpha_f + 2*self.C_alpha_r) / (self.m * self.Vx), (2*self.C_alpha_f + 2*self.C_alpha_r) / self.m,\n",
+    "                            (-2*self.C_alpha_f*self.l_f + 2*self.C_alpha_r*self.l_r) / (self.m * self.Vx)],\n",
+    "                           [0, 0, 0, 1],\n",
+    "                           [0, -(2*self.l_f*self.C_alpha_f - 2*self.l_r*self.C_alpha_r) / (self.I_z * self.Vx),\n",
+    "                            (2*self.l_f*self.C_alpha_f - 2*self.l_r*self.C_alpha_r) / self.I_z,\n",
+    "                            (-2*self.l_f**2*self.C_alpha_f + 2*self.l_r**2*self.C_alpha_r) / (self.I_z * self.Vx)]])\n",
+    "        \n",
+    "        B = np.array([[0],\n",
+    "                           [2 * self.C_alpha_f / self.m],\n",
+    "                           [0],\n",
+    "                           [2 * self.l_f * self.C_alpha_f / self.I_z]])\n",
+    "        \n",
+    "        C = np.array([[0],\n",
+    "                           [(-2*self.C_alpha_f*self.l_f + 2*self.C_alpha_r*self.l_r) / (self.m * self.Vx) - self.Vx],\n",
+    "                           [0],\n",
+    "                           [(-2*self.l_f**2*self.C_alpha_f + 2*self.l_r**2*self.C_alpha_r) / (self.I_z * self.Vx)]])\n",
+    "        \n",
+    "        D = np.array([[0],\n",
+    "                           [self.g],\n",
+    "                           [0],\n",
+    "                           [0]])\n",
+    "        return A,B,C,D\n",
+    "    def compute_state_derivative(self, state, delta, psi_des, phi):\n",
+    "        \"\"\"\n",
+    "        Computes the derivative of the state vector using the lateral error dynamics model.\n",
+    "\n",
+    "        Parameters:\n",
+    "            state (numpy.ndarray): The current state vector [e_y, e_y_dot, e_psi, e_psi_dot].\n",
+    "            delta (float): The steering input.\n",
+    "            psi_des (float): The desired yaw rate input.\n",
+    "            phi (float): The roll angle input.\n",
+    "\n",
+    "        Returns:\n",
+    "            numpy.ndarray: The derivative of the state vector [e_y_dot, e_y_ddot, e_psi_dot, e_psi_ddot].\n",
+    "        \"\"\"\n",
+    "        A,B,C,D=self.GenerateStateSpace()\n",
+    "        state_dot = np.dot(A, state) + np.dot(B, delta) + np.dot(C, psi_des) + np.dot(D, np.sin(phi))\n",
+    "        return state_dot[:,0]\n",
+    "\n",
+    "\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 66,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "State derivative: [0.         2.97836748 0.         1.        ]\n"
+     ]
+    }
+   ],
+   "source": [
+    "model = LateralErrorModel(m=1000.0, Vx=20.0, C_alpha_f=10000.0, C_alpha_r=15000.0, l_f=1.5, l_r=1.0, I_z=3000.0, g=9.8)\n",
+    "\n",
+    "# Initial conditions and inputs\n",
+    "initial_state = np.array([0.0, 0.0, 0.0, 0.0])\n",
+    "delta_input = 0.1\n",
+    "psi_des_input = 0.0\n",
+    "phi_input = 0.1\n",
+    "\n",
+    "# Compute the derivative of the state using the model instance\n",
+    "state_derivative = model.compute_state_derivative(initial_state, delta_input, psi_des_input, phi_input)\n",
+    "\n",
+    "print(\"State derivative:\", state_derivative)"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "tftorch",
+   "language": "python",
+   "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
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}