# Control System Design and Methods for Collaborative Robots: Review
## 5 Control Methodologies
### Invariance Control Strategy  
The detailed development of a novel invariance control algorithm for dynamic constraints in a human-robot collision avoidance algorithm was designed and tested on a torque-controlled robotic manipulator [62]. The controller is responsible for handling the physical constraints (velocity/joint) and dynamic constraints (human movement) with external disturbances. The idea of virtual constraints defines a safe zone for human activity inside the collaborative workspace. The control scheme was built to maintain the admissible subset while keeping the error minimum. This is also applicable in the presence of external disturbances. Figure 8 presents the invariance control strategy with two control loops: the outer control loop has the nominal controller, and the inner loop has the invariance controller. The nominal control law and robot motion \((P_{des})\) generate control torques \((\tau_{no})\) that describe the desired behavior of the robotic manipulator. The invariance controller in the inner loop calculates the corrective control input \((\tau_{c})\), which is close to the nominal control input \((\tau_{no})\) and compensates for the effect of disturbances or violations of the constraints. The torque-controlled robotic systems is described as [62]  
\[\underbrace{\left[\begin{array}{c}\dot{q}\\ \ddot{q}\end{array}\right]}_{\dot{x}}=\underbrace{\left[\begin{array}{c} \dot{q}\\ -{M_{q}}^{-1}\big{(}C_{q}\dot{q}+g_{q}\big{)}\end{array}\right]}_{f(x)}+ \underbrace{\left[\begin{array}{c}0\\ {M_{q}}^{-1}\end{array}\right]}_{G(x)=\left[S_{1}(x)\ldots S_{\text{gt}}(x) \right]}(\tau_{c}-e_{\tau}) \tag{6}\]  
The nominal control law is designed to generate control torque \((\tau_{no})\) as  
\[\begin{split}\tau_{\text{no}}=& J(q)^{\top}\big{(}f_{\text{ext}}+M_{p}\bar{p}_{\text{des}}+D_{p}(\dot{p}_{\text{des}}- \dot{p})+K_{p}(p_{\text{des}}-p)\big{)}\\ &+C_{q}(q,\dot{q})+g_{q}(q)\end{split} \tag{7}\]  
where the mass matrix \({M_{q}}\in\mathbb{R}^{n_{q}\times n_{q}}\), a sufficiently smooth desired trajectory \(p_{\text{des}}\) and the positive definite Cartesian mass \({M_{p}}\in\mathbb{R}^{n_{p}\times n_{p}}\), stiffness \(K_{p}\in\mathbb{R}^{n_{p}\times n_{p}}\), and damping \({D_{p}}\in\mathbb{R}^{n_{p}\times n_{p}}\) matrices, Jacobian matrix \(J(q)\in\mathbb{R}^{n_{q}\times n_{q}}\), Coriolis and centripetal forces \(C_{q}(q,\dot{q})\in\mathbb{R}^{n_{q}}\), and the gravitational torques \(g_{q}(q)\in\mathbb{R}^{n_{q}}\). External forces \(f_{\text{ext}}\in\mathbb{R}^{n_{p}}\) are connected to the external torque \(\tau_{\text{ext}}\). The stiffness and damping parameters may be adapted, sometimes even online, to account for task requirements. Furthermore, solving a constrained minimization problem given below yields corrective control \((\tau_{c})\)  
\[argmin||\tau_{c}-\tau_{\text{no}}||^{2} \tag{8}\]  
Figure 8: Invariance control architecture.  
This strategy is tested on the dual-arm robot with seven DOF with direct human contact. One person exerts force to the end-effector and the robotic manipulator accurately tracks movement.