As the AUV have four thrusters that can rotate in any direction its generates a vector U of possible Torque direction where T1,T2,T3 is the component x,y,z of thruster number 1.However the PD tracking controller generates a force vector Tb of the way where X,Y,Z are the forces and K,M,N are the moments over the gravitational center in the AUV.
 "U=\left( \begin{array}{c} T_1 \\ T_2 \\ T_3 \\ T_4 \\ T_5 \\ T_6 \\ T_7 \\ T_8 \\ T_9 \\ T_{10} \\ T_{11} \\ T_{12} \\ \end{array} \right)")
 "T_B=\left( \begin{array}{c} X \\ Y \\ Z \\ K \\ M \\ N \\ \end{array} \right)")
 "L=\left( \begin{array}{cccccccccccc} 1 & 0 & 0 & 1 & 0 & 0 & 1 & 0 & 0 & 1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 1 & 0 & 0 & 1 & 0 & 0 & 1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 1 & 0 & 0 & 1 &0 & 0 & 1\\ -0.3 & 0 & 0 & 0.3 & 0 & 0 & 0.3 & 0 & 0 & 0.3 & 0 & 0 \\ 0 & 0.3 & 0 & 0 & 0.3 & 0 & 0 & 0.3 & 0 & 0 & 0.3 & 0\\ 0 & 0 & 0.3 & 0 & 0 & 0.3 & 0 & 0 & 0.3 & 0 & 0 & 0.3 \\ \end{array} \right)")
However L can not be inverse and to solve the system it has to be applied and advanced algorithm to solve the system with a seed data .In the case of Matlab this can be made with fsolve(@myfun,x0) where x0 is the seed data .In this case seed data is a vector where each data is X/4,Y/4,Z/4,K/4,M/4 and N/4
Work in progress.......
Fill_time2.gif)
No comments:
Post a Comment