## 4 Consensus Algorithm for Optimal Obstacle Avoidance
\begin{array}{ll}\mathbf{V}^{\mathbf{T}}(\hat{\mathbf{X}})\mathbf{f}(\hat{\mathbf{X}},\mathbf{U})&= -2\hat{\mathbf{X}}^{\mathbf{T}}\mathbf{P}\mathbf{R}_{1}^{-1}\mathbf{P}\hat{\mathbf{X}}-\hat{\mathbf{X}}\mathbf{ P}\mathbf{R}_{1}^{-1}\mathbf{g}^{\prime}(\hat{\mathbf{X}})\\ &-\mathbf{g}^{\mathbf{T}}(\hat{\mathbf{X}})\mathbf{R}_{1}^{-1}\mathbf{P}\hat{\mathbf{X}}-\frac{1}{2}\bm {g}^{\mathbf{T}}(\hat{\mathbf{X}})\mathbf{R}_{1}^{-1}\mathbf{g}^{\prime}(\hat{\mathbf{X}})\end{array} . \tag{35}\]  
Substituting Equations (33) and (34) into (32) yields  
\[ \begin{array}{ll}\mathbf{H}(\hat{\mathbf{X}},\phi(X))&=\hat{\mathbf{X}}^{\mathbf{T}}(\mathbf{R} _{2}-\mathbf{P}\mathbf{R}_{1}^{-1}\mathbf{P})\hat{\mathbf{X}}-\mathbf{g}^{\mathbf{T}}(\hat{\mathbf{X}})\mathbf{ R}_{1}^{-1}\mathbf{P}\hat{\mathbf{X}}\\ &+\mathbf{h}(\hat{\mathbf{X}})-\frac{1}{2}\mathbf{g}^{\mathbf{T}}(\hat{\mathbf{X}})\mathbf{R}_{1}^{-1} \mathbf{g}^{\prime}(\hat{\mathbf{X}})\end{array} . \tag{36}\]  
For the consensus problem (7), we can prove that the control law (34) is an optimal solution using Lemma 1, but it is necessary to verify conditions (14) to (19). By satisfying condition (18) or causing Equation (36) to be zero, we can obtain  
\[\mathbf{R}_{2}-\mathbf{P}\mathbf{R}_{1}^{-1}\mathbf{P}=\mathbf{0} \tag{37}\]  
and demand  
\[-\mathbf{g}^{\mathbf{T}}(\hat{\mathbf{X}})\mathbf{R}_{1}^{-1}\mathbf{P}\hat{\mathbf{X}}+\mathbf{h}(\hat{ \mathbf{X}})-\frac{1}{4}\mathbf{g}^{\mathbf{T}}(\hat{\mathbf{X}})\mathbf{R}_{1}^{-1}\mathbf{g}^{\prime }(\hat{\mathbf{X}})=\mathbf{0}. \tag{38}\]  
Using Equations (34) and (36)-(38), it can be observed that  
\[\begin{array}{ll}\mathbf{H}(\hat{\mathbf{X}},\mathbf{U},\mathbf{V}^{\mathbf{T}}(\hat{\mathbf{X}}))& =\mathbf{U}^{\mathbf{T}}\mathbf{R}_{1}\mathbf{U}+h(\hat{\mathbf{X}})+\hat{\mathbf{X}}^{\mathbf{T}}\mathbf{R}_ {2}\hat{\mathbf{X}}+(2\hat{\mathbf{X}}^{\mathbf{T}}\mathbf{P}+\mathbf{g}^{\mathbf{T}}(\hat{\mathbf{X}})) \mathbf{U}\\ &=\mathbf{U}^{\mathbf{T}}\mathbf{R}_{1}\mathbf{U}+h(\hat{\mathbf{X}})+\hat{\mathbf{X}}^{\mathbf{T}}\mathbf{R}_ {2}\hat{\mathbf{X}}+(2\hat{\mathbf{X}}^{\mathbf{T}}\mathbf{P}+\mathbf{g}^{\mathbf{T}}(\hat{\mathbf{X}})) \mathbf{U}\\ &-\hat{\mathbf{X}}^{\mathbf{T}}(\mathbf{R}_{2}-\mathbf{P}\mathbf{R}_{1}^{-1}\mathbf{P})\hat{\mathbf{X}}\text { (using Equation (\ref{eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq: eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq: eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq:eq: eqsuch that the Lyapunov function (31) becomes  
\[ \begin{array}{ll}\vspace{0.2 cm}V(\hat{\mathbf{X}})&=g(\hat{\mathbf{X}})+\hat{\mathbf{X}}^{T }P\hat{\mathbf{X}}\\ &=\begin{cases}\mathbf{w}_{p}\mathbf{w}_{c}\mathbf{X}^{T}(\mathbf{L}\otimes\mathbf{I}_{m}\ )X&\mathbf{x}_{i}\in\Pi\\ g(\mathbf{X})+\mathbf{w}_{p}\mathbf{w}_{c}\mathbf{X}^{T}(\mathbf{L}\otimes\mathbf{I}_{m}\ )X&\mathbf{x}_{i}\in\Gamma_{j}\\ \textbf{not defined}&\mathbf{x}_{i}\in\Lambda_{j}\end{cases}\end{array} \;. \tag{41}\]  
Note that the property of \(\mathbf{L}\) in Equation (5) is used to convert \(\vspace{0.2 cm}V(\hat{\mathbf{X}})\) into \(\vspace{0.2 cm}V(\mathbf{X})\). If \(\hat{\mathbf{X}}\
eq\mathbf{0}\) or \(\mathbf{X}\
eq\mathbf{X}_{cs}\), based on the property of the Laplacian matrix, \(\mathbf{X}^{T}(\mathbf{L}\otimes\mathbf{I}_{m}\ )X\) will not be zero, but positive. Note that when \(\mathbf{X}=\mathbf{0}\) can lead to \(\mathbf{X}^{T}(\mathbf{L}\otimes\mathbf{I}_{m}\ )X=\mathbf{0}\), this is a special case of \(\mathbf{X}=\mathbf{X}_{cs}\) and \(\mathbf{X}_{cs}=\mathbf{0}\), which also implies \(\hat{\mathbf{X}}=\mathbf{0}\). Therefore, \(\mathbf{X}^{T}(\mathbf{L}\otimes\mathbf{I}_{m}\ )X>0\) if \(\hat{\mathbf{X}}\
eq\mathbf{0}\). Furthermore, \(g(\mathbf{X})\) is defined by Equations (26) and (27), and it is easily shown that \(g(\mathbf{X})>0\).