# Design and Motion Planning of a Biped Climbing Robot with Redundant Manipulator
## 3 Motion Planning
### Motion Planning of the Redundant Manipulator
To reduce the possibility of collision, the manipulator should move in the opposite direction of obstacles and increase the minimum distance until it exceeds the safety threshold. It is necessary to determine the minimum distance and direction vector between the manipulator and obstacles. However, calculating the minimum distance between the manipulator and obstacles is difficult, since the shapes of the manipulator and obstacles are irregular. To solve this problem, the manipulator and obstacles are simplified as a combination of regular shapes based on their structural characteristics, as shown in Figure 12. The manipulator is simplified as a set of spheres that can cover all surfaces of itself. The climbing mechanism and railway bridge are simplified as a set of cuboids and act as the obstacles to the manipulator. \begin{table}
\begin{tabular}{c c c c c} \hline \hline \multirow{2}{*}{**Joint**} & **Link Torsional Angle** & **Link Length** & **Link Offset** & **Joint Angle** \\  & \(\mathbf{\alpha_{i}}\) (\(\mathbf{\varsigma}\)) & \(\mathbf{a_{i}}\) (\(\mathbf{mm}\)) & \(\mathbf{d_{i}}\) (\(\mathbf{mm}\)) & \(\mathbf{\theta_{i}}\) (\(\mathbf{\varsigma}\)) \\ \hline
6 & 0 & 0 & 0 & \(\theta_{6}\) \\
7 & \(-90\) & \(l_{3}\) & 0 & \(\theta_{7}\) \\
8 & 0 & \(l_{4}\) & 0 & \(\theta_{8}\) \\
9 & 0 & \(l_{5}\) & 0 & \(\theta_{9}\) \\
10 & 90 & 0 & \(l_{6}\) & \(\theta_{10}\) \\
11 & \(-90\) & 0 & 0 & \(\theta_{11}\) \\
12 & 90 & 0 & 0 & \(\theta_{12}\) \\ \hline \hline \end{tabular}
\end{table} 
Table 2: D\(-\)H parameters of the manipulator. Since spheres and cuboids are typical convex shapes, the minimum distance between them can be calculated by the Gilbert-Johnson-Keerthi (GJK) algorithm [26]. Unlike many other distance algorithms, the geometry data does not have to be stored in a specific format. Let C\({}_{i}\) be labels for \(i\)th virtual spheres and D\({}_{j}\) be the labels for \(j\)th virtual cuboids. The minimum distance between C\({}_{i}\) and D\({}_{j}\) and the closest points in C\({}_{i}\) and D\({}_{j}\) can be calculated efficiently based on the GJK algorithm. The distance between C\({}_{i}\) and D\({}_{j}\) is denoted by \(d_{ij}\), and the unit direction vector that points from the closest point in D\({}_{j}\) to the closet point in C\({}_{i}\) is denoted by \(u_{ij}\) (see Figure 13). Let \(d_{\rm s}\) be the safety threshold during locomotion. If \(d_{ij}\) is large enough to avoid the danger of a collision, the manipulator will not execute the obstacle avoidance algorithm. In contrast, if \(d_{ij}\) is smaller than \(d_{\rm s}\), the manipulator should change configuration to avoid obstacle D\({}_{j}\). In the meantime, the most effective way to avoid the obstacle in this situation is to move C\({}_{i}\) away from D\({}_{j}\) in the direction of \(u_{ij}\). Then, the related velocity can be calculated by:  
\[\mathbf{s}_{ij}=\left\{ \begin{array}{c}\mathbf{0}\,d_{ij}>d_{\rm s}\\ \frac{v_{\rm max}}{2}\Big{(}\cos\!\left(\pi\frac{d_{ij}}{d_{\rm s}}\right)+1 \Big{)}\mathbf{u}_{ij}\,d_{ij}\leq d_{\rm s}\end{array} \right.. \tag{15}\]  
In Equation (15), \(s_{ij}\) denotes the escape velocity of sphere C\({}_{i}\) in the direction of \(u_{ij}\) and \(v_{\rm max}\) denotes the maximum escape velocity. Additionally, the escape velocity will be generated when \(d_{ij}\leq d_{\rm s}\) and increase to the maximum with the decrease of \(d_{ij}\). If \(s_{ij}\
eq 0\), sphere C\({}_{i}\) should escape D\({}_{i}\) at a speed of \(s_{ij}\) until \(s_{ij}\) becomes 0. The escape motion should have the following equation:  
\[\mathbf{s}_{ij}=J_{\rm C_{i}}\,\beta, \tag{16}\]  
Figure 12: Manipulator and railway bridges simplified as spheres and cuboids. Figure 13: Schematic diagram of minimum distance between virtual spheres and obstacles. where \(J_{C_{i}}\) is the Jacobian of the center of sphere \(C_{i}\). If there are \(k(k>1)\) pairs of spheres and cuboids whose minimum distances are less than \(d_{s}\), the related escape velocity and related Jacobian can be renamed \(s_{k}\) and \(j_{k}\).