# Autonomous Operation Method of Multi-DOF Robotic Arm Based on Binocular Vision
## 2 Materials and Methods
### Multi-DOF Robotic Arm Trajectory Planning Method
#### 2.3.1 Multi-DOF Robotic Arm Kinematics Modeling  
The robotic arm used in this study consists of five joints and one end effector. The kinematics of the robotic arm is modeled using the classical Denavit-Hartenberg (D-H) method. Figure 4 shows the kinematics model of the robotic arm as well as the joint coordinate system and parameter definitions. Define the previous joint coordinate system as \(n\) and the latter joint coordinate system as \(n+1\). The transformation matrix \({}^{nT}T_{n+1}\) between the two coordinate systems is as follows:  
\[{}^{nT}T_{n+1}=\left( \begin{array}{cccc}\cos\theta_{n+1}&-\sin\theta_{n+1} \cos\alpha_{n+1}&\sin\theta_{n+1}\sin\alpha_{n+1}&a_{n+1}\cos\theta_{n+1}\\ \sin\theta_{n+1}&\cos\theta_{n+1}\cos\alpha_{n+1}&-\cos\theta_{n+1}\sin \alpha_{n+1}&a_{n+1}\sin\theta_{n+1}\\ 0&\sin\alpha_{n+1}&\cos\alpha_{n+1}&d_{n+1}\\ 0&0&0&1\\ \end{array} \right) \tag{13}\]  
where \(\theta_{i}\) represents the angle between the two axes of \(X_{i}\) and \(X_{i+1}\), with the right rotation around the \(Z_{i}\) axis set to the positive direction; \(d_{i}\) represents the distance between the adjacent joints \(X_{i}\) and the \(X_{i+1}\) axis; \(a_{i}\) represents the distance between the adjacent joints \(Z_{i}\) and the \(Z_{i+1}\) axis; and \(\alpha_{i}\) represents the angle between the two axes of \(Z_{i}\) and \(Z_{i+1}\), with the right rotation around the \(X_{i}\) axis set to the positive direction. According to the established kinematic model of the robotic arm, the conversion matrix from base coordinate system 0 to end effector coordinate system 5 can be obtained as follows:  
\[{}^{0}T_{5}={}^{0}T_{1}(\theta_{1})^{1}T_{2}(\theta_{2})^{2}T_{3}(\theta_{3} )^{3}T_{4}(\theta_{4})=\left( \begin{array}{cccc}n_{x}&\ o_{x}&a_{x}&p_{x}\\ n_{y}&o_{y}&a_{y}&p_{y}\\ n_{z}&o_{z}&a_{z}&p_{z}\\ 0&0&0&1\\ \end{array} \right) \tag{14}\]  
where \((p_{x},\ p_{y},\ p_{z})^{T}\) is the position coordinate of the end effector of the robotic arm that we care about at the base coordinate of the arm base. The calculated end effector coordinates are as in Equation (15). \[\left\{\begin{array}{l}p_{x}{=}\cos\theta_{1}\times(a_{3}\!\cos(\theta_{2}+ \theta_{3})+a_{2}\cos\theta_{2}+a_{4}\!\cos(\theta_{2}+\theta_{3}+\theta_{4}) +d\!s\!\sin(\theta_{2}+\theta_{3}+\theta_{4})+a_{1})\\ p_{y}{=}\sin\theta_{1}\times(a_{3}\!\cos(\theta_{2}+\theta_{3})+a_{2}\cos \theta_{2}+a_{4}\!\cos(\theta_{2}+\theta_{3}+\theta_{4})+d_{5}\!\astwhere \(\omega_{1}\), \(\omega_{2}\), \(\omega_{3}\), and \(\omega_{4}\) represent the angular velocity of the first four joints of the robotic arm; \(\beta\) and \(\alpha\) are the weight coefficients of the optimization function; \(C_{1}\), \(C_{2}\), \(C_{3}\), and \(C_{4}\) are the corresponding joint rotation angles during the motion of the robotic arm; and \(F_{1}\) is the distance error function, which is defined as the spatial distance between the end effector coordinates \((x,y,z)\) and the target point \((x_{f},\,y_{f},\,z_{f})\) during the motion of the robotic arm, as shown in the following equation:  
\[F_{1}=\sqrt{\left(x-x_{f}\right)^{2}+\left(y-y_{f}\right)^{2}+\left(z-z_{f} \right)^{2}}\,. \tag{17}\]  
The constraints of the trajectory planning process are as follows:  
1. Joint constraint \[C_{2}+C_{3}+C_{4}=\kappa.\] (18)
2. Connecting rod constraint \[l_{obj}\leq l_{cur}\leq l_{\max}.\] (19)  
In the joint constraint, \(\kappa\) is the rotation angle of the end connecting rod of the robotic arm from the initial position to the end position in the base coordinate system 0. The angle varies from \(-160^{\circ}\) to \(160^{\circ}\). The sum of the rotation angles of the joints 2, 3, and 4 is equal to the rotation angle of the end connecting rod. The motion of joint 1 is not constrained by the joints.