# Data-Driven Control Algorithm for Snake Manipulator
## 3 Simulation of DDPG Control Based on 2-Link Model
### Environmental Object Establishment
\begin{split} P_{c2}^{\mathrm{T}}P_{c2}^{\mathrm{T}}& =a^{2}cos(\beta_{1})^{2}\dot{\alpha_{1}}^{2}+a^{2}\dot{\beta_{1}}^{2}+L_{c2}^{ 2}(\beta_{1}+\dot{\beta_{2}})^{2}\\ &+L_{c2}^{2}(cos(\beta_{1}+\beta 2))^{2}(\dot{\alpha_{1}}+\dot{ \alpha_{2}})^{2}\\ &+2 aL_{c2}cos(\beta_{1})cos(\alpha_{2})cos(\beta_{1}+beta_{2})( \dot{\alpha_{1}}^{2}+\dot{\alpha_{2}})\\ &+2 aL_{c2}sin(\beta_{1})cos(\alpha_{2})sin(\beta_{1}+beta_{2})( \dot{\beta_{1}}^{2}+\dot{\beta_{2}})\\ &+2 aL_{c2}cos(\beta_{1})cos(\beta_{1}+\beta_{2})(\dot{\beta_{1}}^ {2}+\dot{\beta_{2}})\end{split}  \tag{13}\]  
\(L_{c2}\) in Formulas (12) and (13) denotes the centroid length of connecting rod 2, which is only half of the length of a connecting rod. Formula (12) is substituted into Formula (6). Formula (13) is substituted into Formula (8). The total kinetic energy of connecting rod 1 and connecting rod 2 can be calculated. \[ \begin{split} E_{k}&=E_{k1}+E_{k2}\\ &=\frac{1}{2}\dot{\theta}^{T}H(\theta)\dot{\theta}\end{split}  \tag{14}\]  
The Lagrangian dynamic formula deduces the dynamic equation by making use of the difference between the kinetic energy and the potential energy of the mechanical system of the manipulator. The second kind of Lagrange equation is shown in Formula (15). The dynamic equation of the manipulator is shown in Formula (16). \[\frac{d}{d_{t}}\frac{\partial L}{\partial\dot{\theta}}-\frac{\partial L}{ \partial\dot{\theta}}=\tau \tag{15}\]  
\[H(\theta)\dot{\theta}+C(\theta,\dot{\theta}))\dot{\theta}+G(\theta)=\tau \tag{16}\]  
In Formula (16), \(H(\theta)\) denotes the equivalent inertia matrix. \(C(\theta,\dot{\theta})\) denotes a Coriolis matrix. \(G(\theta)\) denotes the gravity matrix. \(\tau\) denotes the moment. The complete dynamic expression equation Formula (17) can be deduced according to Formulas (15) and (16). \[\dot{\theta}=[H(\theta)]^{-1}(\tau-C(\theta,\dot{\theta})\dot{\theta}-G(\theta)) \tag{17}\]  
(3) Calculation and updated observations: According to the dynamic equation, the angular acceleration of the joint at the next moment is calculated. The joint angular acceleration integral operation is used to calculate the corresponding angle value and angular velocity value. It is outputted as an observation of the agent. (4) Calculation of the reward value: The reward value of each sampling is calculated according to the angle value and angular velocity value. The reward value for each process is the sum of all sample reward values. The simulation time for each episode is set to 10 s, and the sampling time is set to 0.05 s. Each episode is sampled 200 times. The process reward value function is set to the following. \[r_{t}=-\sum_{i}^{200}7(\theta_{i1}+\theta_{i2})^{2}+(\dot{\theta_{i1}}+\dot{ \theta_{i2}})^{2} \tag{18}\]  
In Formula (18), \(r_{t}\) denotes the sum of the reward values of the t process. \(\theta_{i1}\) denotes the angle value of joint 1 during the \(i\) time sampling. \(\dot{\theta_{i1}}\) denotes the angle velocity of joint 1 during \(i\) time sampling. Joint 2 is defined in the same manner.