# Design and Motion Planning of a Biped Climbing Robot with Redundant Manipulator
## 3 Motion Planning
### Motion Planning of the Redundant Manipulator  
A 7-Dof redundant manipulator was selected so the redundant degree of freedom can play an important role in avoiding collisions between the manipulator and obstacles. A planning algorithm based on the gradient projection method is proposed in this section. The kinematic model of the manipulator is built using the D-H method, as shown in Figure 11. Coordinate (K) is the base coordinate of the manipulator and coordinate (\(i\)) (\(i\) = 7, 8, 9, 10, 11, 12) represents the coordinate of \(Ji\). The \(Z\)-axis in Figure 11 is defined as a rotation axis of joints. Point Q is located on the end of the manipulator and its trajectories are planned according to the tasks. The D-H parameters are shown in Table 2. According to the D-H method, vector \(q=(q_{\rm x}\ \eta_{\rm y}\ q_{\rm z})^{\rm T}\), which expresses the position of Q relative to coordinate (K), can be represented by the following equation:  
\[\mathbf{q}=\mathbf{T}_{6}^{\rm K}\mathbf{T}_{7}^{\rm G}\mathbf{T}_{8}^{\rm T}\mathbf{T}_{10}^{\rm 0 }\mathbf{T}_{11}^{\rm 10}\mathbf{T}_{12}^{\rm 11}\mathbf{\rm e}_{\rm q}, \tag{12}\]  
where \(\mathbf{\rm e}_{\rm q}\) = (0 0 -\(l_{7}\) 1)\({}^{\rm T}\) is the position of point Q relative to coordinate [12], \(T_{i}\ j\) represents the homogenous transformation matrix from \(\{i\}\) to \(\{j\}\). Figure 11: Kinematic model of the manipulator. According to the mathematical introduction of robotic manipulation [24], the generalized velocity of the end of the manipulator and the angular velocity of the joints are connected as:  
\[\mathbf{v}=\mathbf{\beta}, \tag{13}\]  
where, \(v\) denotes the generalized velocity of the end of the manipulator in (K), \(\mathbf{\beta}=\left[\dot{\theta}_{6}\ \dot{\theta}_{7}\ \dot{\theta}_{8}\ \dot{\theta}_{9}\ \dot{\theta}_{10}\ \dot{\theta}_{11}\ \dot{\theta}_{12}\right]^{ \mathrm{T}}\) denotes the angular velocity of the manipulator in its joint space, \(J\in\mathbb{R}^{6\times 7}\) denotes the Jacobian matrix of the manipulator. Generalized velocity \(v\) includes the linear velocities and angular velocities along the axes of (K) and can be represented as \(v\)= \(\left(\dot{q}_{x}\ \dot{q}_{y}\ \dot{q}_{z}\ \omega_{x}\ \omega_{y}\ \omega_{z}\right)^{ \mathrm{T}}\). The Jacobian matrix \(J\) can be calculated according to Equation (12), and the general solution to Equation (13) is given by the following equation [25]:  
\[\mathbf{\beta}=\mathbf{J}^{+}\mathbf{v}+\left(\mathbf{I}-\mathbf{J}^{+}\mathbf{J}\right)\mathbf{Z}, \tag{14}\]  
where \(J\in\mathbb{R}^{7\times 6}\) is the pseudoinverse matrix of \(J\) and can be denoted as \(J^{+}=J^{\mathrm{T}}\!\left(J\ \mathrm{T}^{\mathrm{T}}\right)^{-1}\). \(J^{+}v\) is the minimum norm solution of \(\beta\), which can guarantee that the manipulator tracks the planned trajectory with a minimum sum of squares of the angular velocity of the joints. \(\mathrm{I}\in\mathbb{R}^{7\times 7}\) is the identity matrix. Furthermore, \((I-J^{+})Z\) is a homogeneous solution of \(J^{+}v\) and can adjust the configuration of the manipulator to meet special demands under the premise of unchanged trajectory. \(Z\) is the optimizing index that contains the variable of \(\beta\). The resultant joint angular velocity can be regarded as a combination of the least solution of the minimum norm and a homogeneous solution created by the action of a projection operator \((I-J^{+}J)\), which describes the redundancy of the system, mapping an arbitrary \(\beta\) into the null space of the transformation. By applying various functions of \(\beta\) to compute vector \(Z\), the manipulator can be reconfigured to achieve a desired secondary criterion under the constraint of the specified end-effector velocity. In general, if the minimum distance between the manipulator and an obstacle is less than the safety threshold, the danger of collision will increase significantly.