# Design and Motion Planning of a Biped Climbing Robot with Redundant Manipulator
## 3 Motion Planning
### Motion Planning of the Climbing Mechanism  
From Figure 9, the motion of the climbing mechanism is inspired by an inchworm. A single step consists of two phases, the contraction phase and the extension phase. During the extension phase, the rear foot attaches to the surface of the handrail and the front foot extends forward along the designed trajectory until it contacts the surface. At the same time, the manipulator folds up and swings in the opposite direction of the front foot to reduce the flip torque of the rear foot. At the end of the extension phase, both the rear and front feet attach to the handrail, and the manipulator swings back to the vertical position to prepare for the contraction phase. During the contraction phase, the front foot remains in contact with the handrail and the rear foot contracts along a designed trajectory. The manipulator will also act as a counterweight to balance the CMBOT during the contraction phase. Figure 8: Flowchart of the work process of the CMBOT. The climbing mechanism can be regarded as a 5-Dof manipulator when one of the feet is in contact with the handrail while the other foot is not in contact. Without loss of generality, the extension phase is analyzed in the following. Figure 10 shows the kinematic model of the climbing mechanism which is built using the Denavit-Hartenberg (D-H) convention method [23]. Coordinate (B) is the base coordinate of the climbing mechanism, which is also the workspace coordinate of the climbing mechanism. Coordinate (\(i\)) (\(i\) = 1, 2, 3, 4, 5, 6) represents the coordinate of Ji. The \(Z\)-axis in Figure 10 is defined as the rotation axis of the joints (the \(Z\)-axis of J2, J3, J4 are vertical to paper and the positive direction faces outward). The \(Z\)-axis of J6, which connects the climbing mechanism, is opposite to the \(Z\)-axis with J3. Defined by the D-H method, \(\theta_{i}\) is the rotation angle of Ji. Point P is located at the center of the lower surface of the front foot and can be regarded as the end of the 5-Dof manipulator. The blue trajectory in Figure 10 is the trajectory of point P, and \(\mathbf{p}=(p_{\mathrm{x}},p_{\mathrm{y}},p_{\mathrm{z}})^{\mathrm{T}}\) is the displacement vector of P that is represented in coordinate (B). In Figure 10, \(H\) is the height of the trajectory, \(S\) is the displacement of the trajectory along the \(X\)-axis, and \(\varphi_{\mathrm{y}}\) and \(\varphi_{\mathrm{z}}\) represent the angle the front foot rotates about the \(Y\)-axis and \(Z\)-axis of coordinate (B). The D-H parameters are shown in Table 1. According to the D-H method, vector \(\mathbf{p}\) that expresses the position of P relative to coordinate (B) can be represented by the following equation:  
\[\mathbf{p}=\mathbf{T}_{1}^{B}\mathbf{T}_{2}^{\mathrm{T}}\mathbf{T}_{3}^{\mathrm{T}}\mathbf{T}_{5 }^{\mathrm{T}}\mathbf{\mathrm{e}}_{\mathrm{P}}, \tag{1}\]  
Figure 10: Kinematic model of the climbing mechanism. Figure 9: Schematic diagram of the motion of the climbing mechanism. where, \(\mathbf{e_{p}}=(0\ 0\ 0\ 1)^{T}\) is the position of point P relative to coordinate (5), and \(T_{j}^{i}\) represents the homogenous transformation matrix from \(\{i\}\) to \(\{j\}\) and can be expressed by the following equation:  
\[\mathbf{T_{j}^{i}}=\left[ \begin{array}{cccc}\cos(\theta_{i})&-\sin(\theta_{i })&0&a_{i}\\ \sin(\theta_{i})\cos(\alpha_{i})&\cos(\theta_{i})\cos(\alpha_{i})&-\sin(a_{i })&-\sin(a_{i})d_{i}\\ \sin(\theta_{i})\sin(\alpha_{i})&\cos(\theta_{i})\sin(\alpha_{i})&\cos( \alpha_{i})&\cos(\alpha_{i})d_{i}\\ 0&0&0&1\end{array} \right].