Robotics/Robotic Modelization_English.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "[<---中文版本](http://nbviewer.jupyter.org/github/w407022008/All-of-Notes/blob/master/Robotics/Robotic%20Modelization.ipynb)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Spatial transformation\n",
    "\n",
    "\n",
    "$R_i^{i-1}$——Means that the coordinate system i is **rotated** with respect to the coordinate system i-1.  \n",
    "    \n",
    ">For example:$\\left\\lgroup \\matrix{\\cos(\\alpha) & -\\sin(\\alpha) & 0 \\cr sin(\\alpha) & cos(\\alpha) & 0 \\cr 0 & 0 & 1} \\right\\rgroup$ represent a rotation around axis-z \n",
    "\n",
    "$P_I^{i-1}$——Means that the coordinate system i is **translated** with respect to the coordinate system i-1.   \n",
    "$T_I^{i-1}=\\left\\lgroup \\matrix{R_i^{i-1} & P_i^{i-1} \\cr 0 & 1} \\right\\rgroup$——Means that the coordinate system i is **transformed** with respect to the coordinate system i-1.  \n",
    "\n",
    "Global frame: $T_0^0 = I$  \n",
    "\n",
    "Description of the i-th coordinate system in the global standard system: $T_i^0=T_1^0T_2^1···T_i^{i-1}=(T_{i+1}^i)^{-1}T_{i+1}^0$    \n",
    "\n",
    "**Attention！**Note the difference between left and right multipling:\n",
    "$$T_1^0=I\\left\\lgroup \\matrix{P_1^0} \\right\\rgroup \\left\\lgroup \\matrix{R_1^0} \\right\\rgroup = I\\left\\lgroup \\matrix{1 & P_1^0 \\cr 0 & 1} \\right\\rgroup \\left\\lgroup \\matrix{R_1^0 & 0 \\cr 0 & 1} \\right\\rgroup =\\left\\lgroup \\matrix{R_1^0 & P_1^0 \\cr 0 & 1} \\right\\rgroup \\neq I\\left\\lgroup \\matrix{R_1^0} \\right\\rgroup \\left\\lgroup \\matrix{P_1^0} \\right\\rgroup $$ \n",
    "$$=I\\left\\lgroup \\matrix{R_1^0} \\right\\rgroup \\left\\lgroup \\matrix{P_1^1} \\right\\rgroup=\\left\\lgroup \\matrix{R_1^0 & 0 \\cr 0 & 1} \\right\\rgroup \\left\\lgroup \\matrix{1 & P_1^1 \\cr 0 & 1} \\right\\rgroup$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Newton-Euler Equation of Motion  \n",
    "     \n",
    "   - ** Forward recursion ** \n",
    "        - **For prismatic joint:**\n",
    "$$r_{C_i}^{s_0} = r_{O_{i-1}}^{s_0} + R^{s_0}_{s_i} (r^{s_i}_{O_{i-1}\\rightarrow C_i}) = r_{O_{i-1}}^{s_0} + R_{s_i}^{s_0}(\\theta_{O_{i-1}}·z^{s_i}_{O_{i-1}})$$\n",
    "$$V_{C_i}^{s_0} = V_{O_{i-1}}^{s_0} + \\omega _{i}^{s_0} \\times R^{s_0}_{s_i}(r_{O_{i-1}\\rightarrow C_i}^{s_i}) + R^{s_0}_{s_i} (\\dot \\theta_{O_{i-1}}·z^{s_i}_{O_{i-1}})$$\n",
    "$$\\omega _i^{s_0} = \\omega_{i-1}^{s_0}$$\n",
    "$$\\dot V_{C_i}^{s_0} = \\dot V_{O_{i-1}}^{s_0} + \\dot \\omega_i^{s_0} \\times r_{O_{i-1}\\rightarrow C_i}^{s_0} + \\omega_i^{s_0} \\times (\\omega_i^{s_0} \\times r_{O_{i-1}\\rightarrow C_i}^{s_0}) + \\ddot \\theta_{O_{i-1}} · z^{s_0}_{O_{i-1}} + 2\\omega_i^{s_0} \\times \\dot \\theta_{O_{i-1}}·z^{s_0}_{O_{i-1}}$$\n",
    "$$\\dot \\omega_i^{s_0} = \\dot \\omega_{i-1}^{s_0}$$\n",
    "$$F_{C_i} = m_i\\dot V_{C_i}$$\n",
    "$$M_i =\\frac{\\partial L}{\\partial t}=\\frac{\\partial I_{C_i}\\omega_i}{\\partial t}= I_{C_i}\\dot \\omega_i + \\omega_i \\times I_{C_i}\\omega_i$$\n",
    "        - **For rotational joint:**\n",
    "$$r_{C_i}^{s_0} = r_{O_{i-1}}^{s_0} + r_{O_{i-1}\\rightarrow C_i}^{s_0}$$\n",
    "$$V_{C_i}^{s_0} = V_{O_{i-1}}^{s_0} + \\omega _{i}^{s_0} \\times r_{O_{i-1}\\rightarrow C_i}^{s_0}$$\n",
    "$$\\omega _i^{s_0} = \\omega_{i-1}^{s_0} +\\dot\\theta_{O_{i-1}} · z_{O_{i-1}}^{s_0}$$\n",
    "$$\\dot V_{C_i}^{s_0} = \\dot V_{O_{i-1}}^{s_0} + \\dot \\omega_i^{s_0} \\times r_{O_{i-1}\\rightarrow C_i}^{s_0} + \\omega_i^{s_0} \\times (\\omega_i^{s_0} \\times r_{O_{i-1}\\rightarrow C_i}^{s_0})$$\n",
    "$$\\dot \\omega_i^{s_0} = \\dot \\omega_{i-1}^{s_0} + \\omega_{i-1}^{s_0} \\times \\dot \\theta_{O_{i-1}}·z_{O_{i-1}}^{s_0} + \\ddot \\theta_{O_{i-1}}·z_{O_{i-1}}^{s_0}$$\n",
    "$$F_{C_i} = m_i\\dot V_{C_i}$$\n",
    "$$M_i =\\frac{\\partial L}{\\partial t}=\\frac{\\partial I_{C_i}\\omega_i}{\\partial t}= I_{C_i}\\dot \\omega_i + \\omega_i \\times I_{C_i}\\omega_i$$\n",
    "   - **Backward recursion**  \n",
    "$$f_{i-1,i} = f_{i,i+1} - f_i + F_{C_i}$$\n",
    "$$m_{i-1,i} = r_{O_{i}\\rightarrow O_{i+1}} \\times f_{i,i+1} + m_{i,i+1} - r_{O_i\\rightarrow C_i} \\times f_i -m_i + r_{O_i\\rightarrow C_i} \\times F_{C_i} + M_i$$\n",
    "$$=m_{i,i+1} + M_i - m_i + r_{C_{i}\\rightarrow O_{i+1}} \\times f_{i,i+1} + r_{O_{i}\\rightarrow C_{i}} \\times f_{i-1,i}$$\n",
    "$$\\tau_{O_{i-1}}=z_{O_{i-1}}^{s_0}·f_{i-1,i} \\text{, if the joint is of type P}$$\n",
    "$$\\tau_{O_{i-1}}=z_{O_{i-1}}^{s_0}·m_{i-1,i} \\text{, if the joint is of type R}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Serial robot modelization  \n",
    "\n",
    "if you haven't understood the spatial rotation matrix, you can click [here](http://nbviewer.jupyter.org/github/w407022008/All-of-Notes/blob/master/Robotics/The%20spatial%20rotation_English.ipynb) to have a look."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## - Jacques matrix：\n",
    "For a mechanism with m degrees of freedom, assume that each generalized coordinate is $q_r$,(r=1,2,...,m)  \n",
    "\n",
    "Then, there is an equation of motion at a point p on the mechanism: $\\dot q_e = J_e·\\dot q_r = \\left\\lgroup \\matrix{v^0_{ex} \\cr v^0_{ey} \\cr v^0_{ez} \\cr \\omega ^0_{ex} \\cr \\omega ^0_{ey} \\cr \\omega ^0_{ez}} \\right\\rgroup = \\begin{bmatrix} J_{11} & \\cdots & J_{1m} \\\\ J_{21} & \\cdots & J_{2m} \\\\ J_{31} & \\cdots & J_{3m} \\\\ J_{41} & \\cdots & J_{4m} \\\\ J_{51} & \\cdots & J_{5m} \\\\ J_{61}& \\cdots &J_{6m} \\end{bmatrix} \\begin{pmatrix} \\dot q_1 \\\\ \\dot q_2 \\\\ \\vdots \\\\ q_m \\end{pmatrix} = \\begin{bmatrix} \\$_1& \\$_2 & \\cdots & \\$_n \\end{bmatrix} \\begin{pmatrix} \\dot q_1 \\\\ \\dot q_2 \\\\ \\vdots \\\\ q_m \\end{pmatrix} $\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "where, $J_{(1->6),r}= \\frac {\\partial \\vec r_p^0}{\\partial \\vec q_r} \\vec e_{p(1->6)}^0$，  \n",
    "$\\vec r_p$ is the coordinates of the target p in the global coordinate system, there is $\\vec r_p=\\sum _{i=1}^n \\alpha _i \\vec q_i$  \n",
    "$\\vec e_{p(1->6)}$ is the direction vector of 6 degrees of freedom of the global frame."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "thus：\n",
    "\n",
    "- For a **rotating joint** i, at target p there is:$ \\$_i \\dot q_i = \\begin{pmatrix} v_x^i \\\\ x_y^i \\\\ v_z^i \\\\ \\omega_x^i \\\\ \\omega_y^i \\\\ \\omega_z^i \\end{pmatrix}= \\begin{bmatrix} \\vec z_i \\times \\vec{O_ip} \\\\ \\vec z_i \\end{bmatrix} \\dot q_i$\n",
    "  \n",
    "- For a **translational joints** i, at target p there is: $ \\$_i \\dot q_i = \\begin{pmatrix} v_x^i \\\\ x_y^i \\\\ v_z^i \\\\ \\omega_x^i \\\\ \\omega_y^i \\\\ \\omega_z^i \\end{pmatrix}= \\begin{bmatrix} \\vec z_i \\\\ \\vec 0 \\end{bmatrix} \\dot q_i$\n",
    "\n",
    "- For a space link with both a rotating joint and a translational joint, at target p there is: $\\dot q_p = \\begin{bmatrix} J \\end{bmatrix}_{trans}·\\begin{pmatrix} \\dot q_1 \\\\\\vdots \\\\ \\dot q_n \\end{pmatrix}_{trans} + \\begin{bmatrix} J \\end{bmatrix}_{rot}·\\begin{pmatrix} \\dot q_1 \\\\ \\vdots \\\\ \\dot q_n \\end{pmatrix}_{rot} $ "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## - Equation of Motion for multibody\n",
    "\n",
    ">**Lagrange function**: $$\\frac{\\partial}{\\partial t}(\\frac{\\partial L}{\\partial \\dot q}) - \\frac{\\partial L}{\\partial q}=Q$$\n",
    "where $$L=T-V,  T=\\frac{1}{2}\\dot q M\\dot q^T= \\frac{1}{2}\\sum_{i,j}M_{i,j}(q)\\dot q_i \\dot q_j$$\n",
    "\n",
    "$\\Rightarrow $$$\\underline{\\underline{M}}(q)\\ddot q+\\underline{\\underline{C}}(q,\\dot q)\\dot q+\\underline{g}(q)=\\underline{\\underline{Q}}u$$  \n",
    "where\n",
    "\n",
    "$$ \\begin{cases} \\underline{\\underline{M}}(q)=\\sum_{i=1}^n m^i J_v^{iT} J_v^i + J_{\\omega}^{iT} R^i \\bar I^i R^{iT} J_{\\omega}^i &\n",
    "J_v^i=\\begin{bmatrix}J_{1:3,:1:i}&0_{3\\times(n-i)}\\end{bmatrix}& J_{\\omega}^i=\\begin{bmatrix}J_{4:6,:1:i}&0_{3\\times(n-i)}\\end{bmatrix} \\\\\n",
    "C_{i,j}(q,\\dot q)=\\frac{1}{2}\\dot q^T\\frac{\\partial M}{\\partial q}= \\frac{1}{2}\\sum_{k=1}^n(\\frac{\\partial M_{ij}}{\\partial q_k}+\\frac{\\partial M_{ik}}{\\partial q_j}-\\frac{\\partial M_{kj}}{\\partial q_i})\\dot q_k  \\\\\n",
    "g_i(q)=\\frac{\\partial V}{\\partial q_i}  \\\\\n",
    "\\underline{\\underline{Q}}u=\\Upsilon=\\sum_iJ_i^T\\bar{\\tau_{S_i}^{adj}} + \\sum_iJ_i^T\\bar{\\tau_{S_i}^{ext}}= \\tau + \\sum_iJ_i^T\\begin{pmatrix} \\vec{f_i}\\times \\vec{C_iP} \\\\ \\vec{f_i} \\end{pmatrix} &\n",
    "J_i=\\begin{bmatrix}J_{:,1:i}&0_{6\\times(n-i)}\\end{bmatrix} \\end{cases}$$  "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "Assuming that $x=\\begin{pmatrix} q\\\\ \\dot q\\end{pmatrix}$ is the state variable of the system，the system has a equilibrium point $x_e=\\begin{pmatrix} q_e \\\\ 0 \\end{pmatrix}$，then the state equation：\n",
    "$$ \\dot x=f(x)+g(x)u=\\begin{pmatrix}\\dot q \\\\ \\underline{\\underline{M}}^{-1}(q)(-\\underline{\\underline{C}}(q,\\dot q)\\dot q - \\underline{g}(q)) \\end{pmatrix} +\\begin{pmatrix} 0 \\\\ \\underline{\\underline{M}}^{-1}(q)\\underline{\\underline{Q}} \\end{pmatrix}u$$\n",
    "So the stable input at the eqilibrium point.：$u_e=\\underline{\\underline{Q}}^{-1}\\underline{g}(q_e)$  \n",
    "The state equation is writen as：\n",
    "$$ \\dot X=AX+BU$$\n",
    "where:\n",
    "$$A=\\begin{bmatrix}O & I \\\\ -M^{-1}_{x_e}\\frac{\\partial g}{\\partial q}|_{x_e} & O \\end{bmatrix}, B=\\begin{bmatrix} O \\\\ M^{-1}_{x_e}Q|_{x_e} \\end{bmatrix}, X=x-x_e, U=u-u_e$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## - Inverse of the equation of motion\n",
    "EOM: $\\dot q_e = J_e·\\dot q_r$  \n",
    "\n",
    "   - when $m\\neq n$, $J\\in R^{n\\times m}$, then $J^{\\dagger}$ is the pseudo-inverse \n",
    "       - if m<n, $J^{\\dagger}=(J^TJ)^{-1}J^T$, but the equation has only an approximated solution, such that $\\dot q=arg\\min_{\\dot q} ||\\dot X -J\\dot q||^2$, meaning that the end portion of the degree of freedom is fixed. For example, a five degree of freedom manipulator does not meet the six degree of freedom requirements.\n",
    "\n",
    "       - if m>n, $J^{\\dagger}=J^T(JJ^T)^{-1}$, and the equation has an infinite number of solutions, of which (m-n) redundant variables can be optimized.  \n",
    "   - when m=n, J is square,  \n",
    "    \n",
    "       - if $|J|\\neq0$, there is $J^{-1}$, then $\\dot q_r =J^{-1}·\\dot q_e$  \n",
    "        \n",
    "       - if |J|=0, at this time, the Jacobian matrix is called a singular matrix, indicating that some joint degrees of freedom overlap at this time, and the end reaches a certain level of envelope.\n",
    "        The singular configuration equation can be established as follows:   $$\\frac{\\partial r}{\\partial q_i}·(\\frac {\\partial r}{\\partial q_{n-1}} \\times \\frac {\\partial r}{\\partial q_n})=\\left\\lgroup \\matrix{\\frac{\\partial r_x}{\\partial q_i} & \\frac{\\partial r_x}{\\partial q_{n-1}} & \\frac{\\partial r_x}{\\partial q_n} \\cr \\frac{\\partial r_y}{\\partial q_i} & \\frac{\\partial r_y}{\\partial q_{n-1}} & \\frac{\\partial r_y}{\\partial q_n}\\cr \\frac{\\partial r_z}{\\partial q_i} & \\frac{\\partial r_z}{\\partial q_{n-1}} & \\frac{\\partial r_z}{\\partial q_n}} \\right\\rgroup=0$$and$$\\frac {\\partial r}{\\partial q_{n-1}} \\neq \\frac {\\partial r}{\\partial q_n}\\neq0$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Parallel robot modelization\n",
    "\n",
    "Suppose a parallel robot has n branches, each with m joints. Take one of the branches to study, and there is speed at any p point on the platform:\n",
    "$$ \\dot q_p = \\sum_i^m\\$_i \\dot q_i $$\n",
    "Assuming that the rth joint is controlled in the m joints, then there is a $\\$^{rec}_r$, so that:\n",
    "$$ \\$^{rec}_r \\$_i=0, \\forall i=r$$\n",
    "there is:\n",
    "$$\\$^{rec}_r \\dot q_p = \\$^{rec}_r \\sum_i^m\\$_i \\dot q_i= \\$^{rec}_r \\$_r \\dot q_r$$\n",
    "writen as：\n",
    "$$ D\\dot q_p = B \\dot q_r $$\n",
    "\n",
    "## - Singularity\n",
    "- If the matrix D is irreversible, there is a phenomenon of concurrent lines.\n",
    "- If the matrix B is irreversible, there is a phenomenon of parallel lines."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Differential motion (disturbance):\n",
    "\n",
    "1. The differential from **the generalized joint coordinate system $R_r$** to **the global Cartesian coordinate system $R_0$**：\n",
    "    \n",
    "    $$(\\dot p·dt)=\\delta p=J·\\delta q_r=J·(\\dot q_r dt)$$   \n",
    "    \n",
    "2. The differential from **the global Cartesian coordinate system $R_0$** to **the global Cartesian coordinate system $R_0$**：  \n",
    "    a.**Small disturbance transformation**：$\\delta ^j_{j^*}=\\left\\lgroup \\matrix{1 & -\\delta _z & \\delta _y & dx \\cr \\delta _z & 1 & -\\delta_x & dy \\cr -\\delta_y & \\delta_x & 1 & dz \\cr 0 & 0 & 0 & 1} \\right\\rgroup$ indicates that the j coordinate system has a spatial transformation with respect to itself, resulting in a $j^*$ coordinate system.  \n",
    ">For example: Originally there is a space link whose end is represented in $R_0$ as:\n",
    "$$T_n^0 = IT_1^0T^1_2···T_N^{n-1}$$  \n",
    "Now a disturbance is generated at the i-th joint, and the i-coordinate system changes slightly. Then the end of the space link is represented in R0 as:\n",
    "$$\\bar T^0_n= IT_1^0T^1_2···T^{i-2}_{i-1}(T^{i-1}_i·T^i_{i^*})T_{i+1}^{i^*}···T_N^{n-1}$$\n",
    "Here, it is not difficult to imagine that the space transformation $T_{i+1}^{i^*}$ is actually $T_{i+1}^{i}$. Because no matter who the previous coordinate system of the i+1 coordinate system is, he is rotated or moved the same size relative to it.  \n",
    "\n",
    "    b. **Decomposition of disturbance transformation matrix**：  \n",
    "        \n",
    "    if $$T^{0}_{i^*}=T^{0}_i+dT$$ there is：$$dT=T^{0}_{i^*}-T^{0}_i=(\\delta ^0_{i^*}-1)T_i^{0}=T_i^{0}(\\delta ^i_{i^*}-1)=T_i^{0}\\delta$$  \n",
    "where, $\\delta^i_{i^*}$ is actually $T_{i+1}^{i^*}$, which is the perturbation of the i coordinate system relative to its own coordinate system. And $\\delta ^0_{i^*}$ indicates the disturbance of the i coordinate system relative to the coordinate system $R_0$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Path planning  \n",
    "   \n",
    "   Path planning can be divided into two-point path planning and multi-point path planning; it can also be divided into generalized joint space path planning and Cartesian coordinate space path planning; or multi-point path planning and specified trajectory path planning.  \n",
    "    \n",
    ">For example：**multi-point path planning**, usually, in order to avoid a large impact on the mechanism, a polynomial equation is used to construct a high-order spline curve, so that the path is obtained by the difference.  \n",
    "\n",
    ">Generate a 4th spline$\\vec q(t)=\\sum^4_{i=0} \\vec a_it^i=\\sum _{i=0}^4 a_i(\\frac{h}{T})^i$, where T is the total time and h is between 0 and 1.  \n",
    "\n",
    ">We can solve the weighting factor $\\vec a_i$ by given the boundary conditions, thereby getting$\\vec q(h)=\\vec q(h|a_i)$\n",
    "\n",
    "We also regard the **multi-point path non-stop problem** as a prescribed trajectory path problem. Then, the difference may be multi-pointed by the **high-order Lagrangian difference polynomial ** or ** fourth-order basis function piecewise curve **, and configured as a curve P(s), where s is an arc length. Known by the disturbance transformation,$P(S|\\delta s)=I\\prod ^n_{i=0} P^{i-1}_i \\delta ^i_{i^*}$，where $P(S|\\delta s)$ represents that in $\\delta t$ time, the pose matrix of the end of the space link in the global coordinate system."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Error Analysis  \n",
    "   \n",
    "   In fact, the disturbance we described earlier is the existence of an error. The error can exist at any joint. It can exist in the control precision, the precision of the part manufacturing, the precision of the assembly of the component, the presence of measurement accuracy, etc. It can be said that Errors exist always more or less.\n",
    "    \n",
    ">The generalized degree of freedom error of the servo positioning device is normal distribution by the potentiometer; the generalized degree of freedom error of the servo positioning device is uniformly distributed by the encoder.For the deviation of the geometrical parameters of the member, the error of the robot assembled with the measured component is uniquely determined. For a robot on a design drawing or a robot assembled with any component, it can be considered to be randomly distributed uniformly within its corresponding tolerance zone.\n",
    "\n",
    "**Variance and covariance of end effector pose**  \n",
    "$$V=\\left\\lgroup \\matrix{V(dx,dx) & V(dx,dy) & V(dx,dz) & V(dx,\\delta x) & V(dx,\\delta y) & V(dx,\\delta z) \\cr\n",
    "V(dy,dx) & V(dy,dy) & V(dy,dz) & V(dy,\\delta x) & V(dy,\\delta y) & V(dy,\\delta z) \\cr\n",
    "V(dz,dx) & V(dz,dy) & V(dz,dz) & V(dz,\\delta x) & V(dz,\\delta y) & V(dz,\\delta z) \\cr\n",
    "V(\\delta x,dx) & V(\\delta x,dy) & V(\\delta x,dz) & V(\\delta x,\\delta x) & V(\\delta x,\\delta y) & V(\\delta x,\\delta z) \\cr\n",
    "V(\\delta y,dx) & V(\\delta y,dy) & V(\\delta y,dz) & V(\\delta y,\\delta x) & V(\\delta y,\\delta y) & V(\\delta y,\\delta z) \\cr\n",
    "V(\\delta z,dx) & V(\\delta z,dy) & V(\\delta z,dz) & V(\\delta z,\\delta x) & V(\\delta z,\\delta y) & V(\\delta z,\\delta z) \\cr} \\right\\rgroup$$\n",
    "where： \n",
    "$\\left\\{\n",
    "\\begin{aligned}\n",
    "V(dx)=\\sum_{i=1}^n \\alpha _{x_i}^2V(\\delta _i);\\\\\n",
    "V(dy)=\\sum_{i=1}^n \\alpha _{y_i}^2V(\\delta _i); \\\\ \n",
    "V(dz)=\\sum_{i=1}^n \\alpha _{z_i}^2V(\\delta _i);\\\\\n",
    "V(\\delta x)=\\sum_{i=1}^n \\beta _{x_i}^2V(\\delta _i);\\\\\n",
    "V(\\delta y)=\\sum_{i=1}^n \\beta _{y_i}^2V(\\delta _i);\\\\ \n",
    "V(\\delta z)=\\sum_{i=1}^n \\beta _{z_i}^2V(\\delta _i);\\\\\n",
    "V(dx,dy)=\\sum_{i=1}^n \\alpha_{x_i}\\alpha_{y_i}V(\\delta_i);\\\\\n",
    "V(dx,dz)=\\sum_{i=1}^n \\alpha_{x_i}\\alpha_{z_i}V(\\delta_i);\\\\ \n",
    "V(dy,dz)=\\sum_{i=1}^n \\alpha_{y_i}\\alpha_{z_i}V(\\delta_i); \n",
    "\\end{aligned}\n",
    "\\right.$\n",
    "\n",
    " \n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The end pose error is close to a normal distribution. Assume that the error variable is a three-dimensional variable (dx, dy, dz).Its cumulative distribution density function:\n",
    "$$f(dx,dy,dz)=\\frac{1}{(2\\pi)^{\\frac32}det(\\mathbf{V})}EXP(-\\frac12[dx,dy,dz]\\mathbf{V}^{-1}[dx,dy,dz]^T)$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "if $\\mathbf{V}=\\left\\lgroup \\matrix{\\sigma _1 \\cr & \\sigma_2 \\cr & & \\sigma_3} \\right\\rgroup$,  \n",
    "\n",
    "then $f(dx,dy,dz)=\\frac{1}{(2\\pi)^{\\frac32}\\sigma_1\\sigma_2\\sigma_3}EXP(-\\frac12((\\frac{dx}{\\sigma_1})^2+(\\frac{dy}{\\sigma_2})^2+(\\frac{dz}{\\sigma_3})^2))$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**概率分布**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$P(|\\omega_i|\\leq R_i)=\\int_{-R_i}^{R_i}f(\\omega_i)d\\omega_i$  \n",
    "\n",
    "其中，\n",
    "\n",
    "$f(\\omega_i)=\\frac{1}{(2\\pi)^{\\frac32}\\sigma_i}EXP(-\\frac12(\\frac{\\omega_i}{\\sigma_i})^2)$"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.6.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}