# Segmented Hybrid Impedance Control for Hyper-Redundant Space Manipulators
## 4 System Control
### Segmented Hybrid Motion/Force Control  
In the segmented force/position hybrid control method, it is required that the first segment of the hyper-redundant manipulator comes into contact with external objects and achieves stable leaning. This process necessitates attention to position control at the contact point to maintain the stability of the external forces according to the contact direction. Representing the contact force between the manipulator's first segment and the target by \(\mathrm{F}_{\mathrm{ext}}\), the manipulator's dynamic equation can be expressed as follows:  
\[\mathrm{M(q)\ddot{q}+C\big{(}q,\dot{q}\big{)}+J_{\mathrm{sec1}}^{\mathrm{T}}(q )F_{tip}=\tau, \tag{28}\]  
In the force control problem, the velocity and acceleration of the manipulator arm according to the force tracking direction are typically neglected. Therefore, the dynamic equation of the manipulator arm can be simplified as follows:  
\[\mathrm{J}^{\mathrm{T}}(q)F_{tip}=\tau, \tag{29}\]  
The PI force controller equation can be expressed as follows:  
\[\tau=J_{\mathrm{sec1}}^{\mathrm{T}}(q)(F_{\mathrm{d}}+K_{\mathrm{fp}}F_{ \mathrm{e}}+K_{\mathrm{fi}}\int F_{\mathrm{e}}(t)dt), \tag{30}\]  
where \(F_{\mathrm{e}}=F_{\mathrm{d}}-F_{\mathrm{ext}}\), \(K_{\mathrm{fp}}\) and \(K_{\mathrm{fi}}\) are positive definite PI controller parameters. By combining the two previous equations, we obtain:  
\[K_{\mathrm{fp}}F_{\mathrm{e}}+K_{\mathrm{fi}}\int F_{\mathrm{e}}(t)dt=0, \tag{31}\]  
Based on this equation, it is evident that when both \(K_{\mathrm{fp}}\) and \(K_{\mathrm{fi}}\) are positive definite matrices, the force tracking error signal \(F_{\mathrm{e}}\) can converge to zero. However, if the manipulator arm has not experienced the anticipated contact collision with the target, the presence of force control can lead to significant velocity errors in the manipulator arm. Since the velocity term in typical force control tasks is relatively small, a positive definite parameter matrix \(K_{\mathrm{damp}}\) can be directly introduced to incorporate velocity damping terms. The modified control law becomes:  
\[\tau^{\mathrm{force}}=J^{\mathrm{T}}(q)(F_{\mathrm{d}}+K_{\mathrm{fp}}F_{ \mathrm{e}}+K_{\mathrm{fi}}\int F_{\mathrm{e}}(t)dt-K_{\mathrm{damp}}v), \tag{32}\]  
For the segmented position control of the manipulator, the control method employed is the traditional joint space position control. This involves establishing a correspondence between the workspace and the joint space using the Jacobian matrix. Based on the planned joint angle trajectory \(\dot{\mathrm{q}}_{\mathrm{d}}\), the PD controller for position control can be expressed as follows:  
\[\tau^{\mathrm{position}}=\hat{M}_{\mathrm{sec1}}(q)\big{(}\ddot{\mathrm{q}}_{ \mathrm{d}}(t)+K_{\mathrm{p1}}\mathrm{q}_{\mathrm{e1}}(t)+K_{\mathrm{d1}} \dot{\mathrm{q}}_{\mathrm{e1}}(t)\big{)}, \tag{33}\]  
where \(\tau^{\mathrm{position}}\) is the joint control torque of the manipulator, and \(\hat{M}_{\mathrm{sec1}}(q)\) denotes an estimated value of the manipulator system's mass matrix. \(K_{\mathrm{p}}\) and \(K_{\mathrm{d}}\) are coefficient matrices of the PD controller. Introducing the principle of the force/position hybrid control method, considering the scenario where the manipulator arm joints come into contact with an object and needs to move on the surface of the target and assuming the manipulator make contact with rigid target. The constrained dynamic equations can be written as follows:  
\[\mathrm{P}(\mathrm{q})\tau=\mathrm{P}(\mathrm{q})\big{(}\mathrm{M}(\mathrm{q}) \dot{\mathrm{q}}+\mathrm{\eta}(\mathrm{q},\dot{\mathrm{q}})\big{)}, \tag{34}\]  
where \(\mathrm{P}=\mathrm{I}-\mathrm{A}^{\mathrm{T}}(\mathrm{AM}^{-1}\mathrm{A}^{ \mathrm{T}})^{-1}\mathrm{AM}^{-1}\), \(\mathrm{M}\in\mathbb{R}^{\mathrm{k}\times\mathrm{n}}\) with a rank of \(\mathrm{K}_{1}\) is the subspace matrix of the force constraints. This matrix projects the constrained forces applied to the end-effector of the manipulator arm onto the subspace of force constraints.