## 2 Dynamic Modeling of Industrial Robot
### Assumptions and Description of Optimal Control Problem  
Based on the establishment of the coordinate system and force analysis mentioned above, according to [PERSON]'s second law and the moment of momentum theorem, the matrix representation of the dynamic equation of the industrial robot can be derived  
\[M(h)\ddot{h}+N(h,\dot{h})=I(h)F, \tag{5}\]  
where \(\dot{h}=\left(u\quad v\quad w\quad p\quad q\quad r\right)^{\text{T}}\), \(\ddot{h}=\left(\dot{u}\;\dot{v}\;\dot{w}\;\dot{p}\;\dot{q}\;\dot{r}\right)^{ \text{T}}\), \(F=\left(T_{x}\quad T_{y}\quad T_{z}\quad K_{T}\quad M_{T}\quad N_{T}\right)^{ \text{T}}\), \(N(h,\dot{h})=\left(N(1)\quad N(2)\quad N(3)\quad N(4)\quad N(5)\quad N(6) \right)^{T}\), \(N(1)=F_{x}-(m+m_{u})[(q\omega_{1}-v_{1}r)-(qw-vr)]\),\(N(2)=F_{y}-(m+m_{v})[(ru_{1}-u_{1}p)-(ru-wp)]\), \(N(3)=F_{z}-(m+m_{v})[(pv_{1}-u_{1}q)-(pv-u_{q})]\), \(N(4)=K_{x^{\prime}}|p|p+(z_{\text{b}}\cos\theta\sin\varphi-y_{\text{b}}\cos \theta\cos\phi)B-my_{\text{b}}(pv_{1}-u_{1}q)+m_{\text{2b}}(u_{1}r-pw_{1})\), \(N(5)=K_{y}|q|q+(x_{\text{b}}\cos\theta\cos\phi+z_{\text{b}}\sin\theta)B+mx_{ \text{b}}(pv_{1}-u_{1}q)-m_{\text{2b}}(w_{1}q-v_{1}r)\), \(N(6)=K_{z}|r|r-(y_{\text{b}}\sin\theta+x_{\text{b}}\cos\theta\sin\phi)B-mx_{ \text{b}}(ru_{1}-w_{1}p)+my_{\text{b}}(w_{1}q-v_{1}r)\)\(M(h)=diag\Big{(}m_{u}+m\quad m_{v}+m\quad m_{w}+m\quad m_{p}+I_{x}\quad m_{q}+I_{y} \quad m_{r}+I_{z}\Big{)}\), \(I(h)\) is the unit matrix, and \(m\) is the mass of industrial robot. By solving the dynamic equations, the motion laws described by the robot in the local coordinate system (i.e., \(u\), \(v\), \(w\), \(p\), \(q\), and \(r\)) can be obtained, and then converted to the geodetic coordinate system to obtain the motion laws described by the robot in the geodetic coordinate system  
\[\dot{x}_{h}=T\Big{(}\dot{x}\quad\dot{y}\quad\dot{z}\quad\dot{\phi}\quad\dot{ \theta}\quad\dot{\varphi}\Big{)}^{T}=T\Big{(}u\quad v\quad w\quad p\quad q \quad r\Big{)}^{T}, \tag{6}\]  
where \(T= \begin{pmatrix}T_{v}&0\\ 0&T_{\omega}\end{pmatrix} \). \(\dot{x}\),\(\dot{y}\),\(\dot{z}\) are the velocity of the robot along three axes in the geodetic coordinate system. In order to ensure that the robot can track the preset trajectory as soon as possible and smoothly and avoid the phenomenon of sudden changes in the control variables of the control system that affect the continuity of the control variables, the deviation of the system state variables and the control increment are added to the objective function. In actual robot tracking and control systems, in order to ensure the smooth operation of the robot, it is necessary to limit the speed and speed increment of the robot to a certain range of variation. Therefore, it is necessary to increase the constraints on the control quantity and control increment, which are \(\left\{ \begin{array}{l}u^{\prime}_{\min}\leq u^{\prime}\leq u^{\prime}_{ \max}\\ \Omega_{\min}\leq\Omega\leq\Omega_{\max}\end{array} \right.\) and \(\left\{ \begin{array}{l}\Delta F_{\min}\leq\Delta F\leq\Delta F_{\max}\\ \Delta\tau_{\min}\leq\Delta\tau\leq\Delta\tau_{\max}\end{array} \right.\), respectively. In the actual tasks of industrial robots, in order to shorten the attitude maneuvering time, it is inevitable to consume more energy. Therefore, it is necessary to consider both the performance improvement requirements in terms of maneuvering time and energy, and design a control system that balances efficiency and energy comprehensive indicators. Therefore, the optimal control problem for trajectory tracking of industrial robots can be expressed as: finding the control input, under certain constraint conditions, moving from the given initial state \(x(t_{0})=x_{0}\), \(t_{0}=0\) to the desired end state \(x(t_{f})=x_{f}\), and minimizing energy consumption. It is defined that \(x_{f}= \begin{pmatrix}h&x_{h}\end{pmatrix} ^{T}\) and \(u_{f}=F\), so trajectory tracking optimal control is essentially an energy optimal functional extremum problem.