# Segmented Hybrid Impedance Control for Hyper-Redundant Space Manipulators
## 4 System Control
### Segmented Impedance Controller  
The hybrid impedance control method presented in (20) imparts impedance characteristics to the robotic arm in the end-effector Cartesian space. However, the segmented force/position hybrid control established in this paper formulates control equations in the joint space. Therefore, the rest of this section provides the hybrid impedance control equations in the joint space. Building on the research findings in reference [5], we develop segmented hybrid impedance control suitable for hyper-redundant robotic arms. The impedance behavior has proven to be necessary in our scenario due to the extensive number of contact points and the vibrations caused by other controllers. Based on equation \(F_{GC,q}=\int_{d}^{T}F_{ext}\), the generalized forces exerted by the hyper-redundant robotic arm on external objects can be derived as follows:  
\[F_{GC,q}^{sec\,1}=J_{q_{sec\,1}}^{T}F_{ext}, \tag{23}\]  
where \(J_{q_{sec\,1}}^{T}\) represents the constraint Jacobian matrix at the contact point of the robotic arm. Referring to (19), we can derive the joint-space hybrid impedance control equations as follows:  
\[M_{A,d}\Big{(}\tilde{q}_{H,d}-P\tilde{q}_{d}\Big{)}+B_{A,d}\Big{(}\tilde{q}_{ H,d}-P\tilde{q}_{d}\Big{)}+K_{A,d}P\Big{(}q_{H,d}-q_{d}\Big{)}-(I-P)F_{d}=-F_{ ext}, \tag{24}\]  
where \(P=I-A^{T}\Big{(}AM^{-1}A^{T})^{-1}AM^{-1}\), \(M\in\mathbb{R}^{k\times n}\) is a mass matrix, \(A\in\mathbb{R}^{k\times n}\) is a Pfaffian constraints matrix in joint space and \(I\) is the identity matrix. \(\Big{(}I-P\Big{(}q_{H,d}\Big{)}\Big{)}\in\mathbb{R}^{n\times n}\) is the subspace matrix of the force constraints, which projects the constrained forces acting on the end-effector of the manipulator arm to the subspace of force constraints. \(P\Big{(}q_{H,d}\Big{)}\in\mathbb{R}^{n\times n}\), with rank \(n-k_{1}\), is the subspace for position control. \(q_{d}\) is the expected joint angle for the robotic arm, while \(M_{A,d}\), \(B_{A,d}\) and \(K_{A,d}\) are the parameter matrices of system inertia, damping and stiffness, all of which are positive definite matrices.  
In the segmented hybrid control task of the hyper-redundant robotic arm, the first segment of the arm contacts and leans on the target, with the control objective of ensuring that there is no relative motion between the first segment and the target. Therefore, the desired joint angles \(q_{d}\), desired angular velocities \(\tilde{q}_{d}\), and desired angular accelerations \(\tilde{q}_{d}\) are all zero matrices. Additionally, considering the precision requirements for position control of the first segment, only the cases with introduced inertia and damping are considered, making the impedance control not exhibit stiffness characteristics. This helps to avoid significant errors in the motion of the first segment of the arm due to trajectory corrections. As a result, the segmented hybrid impedance control equations can be expressed as follows:  
\[M_{H,d}\tilde{q}_{H,d}+B_{H,d}\dot{q}_{H,d}-(I-P)F_{d}=-F_{ext}, \tag{25}\]  
According to this equation, the reference trajectory for the robotic arm's joint angle acceleration can be determined as follows:  
\[\Delta\tilde{q}_{H,d}=M_{H,d}^{-1}\Big{[}(I-P)F_{d}-F_{ext}-B_{H,d}\Delta \tilde{q}_{H,d}\Big{]}, \tag{26}\]The corrected trajectory of the first segment of the robotic arm is:  
\[\begin{cases}\ddot{\mathrm{q}}_{\mathrm{H}}=\Delta\ddot{\mathrm{q}}_{\mathrm{H,d}} +\ddot{\mathrm{q}}_{\mathrm{d}},\\ \dot{\mathrm{q}}_{\mathrm{H}}=\Delta\dot{\mathrm{q}}_{\mathrm{H,d}}+\dot{ \mathrm{q}}_{\mathrm{d}},\end{cases} \tag{27}\]