## 4 Consensus Algorithm for Optimal Obstacle Avoidance
\begin{cases}0&x_{i}\in\Pi\\ \left(\frac{R_{i}^{2}-\|x_{i}-O_{bj}\|^{2}}{\|x_{i}-O_{bj}\|^{2}-\tau_{j}^{2}} \right)^{2}&x_{i}\in\Gamma_{j},i=1,\ldots,n\\ \text{not defined}&x_{i}\in\Lambda_{j}\end{cases}  \tag{27}\]  
and  
\[g^{\prime}(\hat{X}) =\left[ \begin{array}{cc}\left(\frac{dm(x_{1})}{dx_{1}}\right)^ {T}&\left(\frac{dm(x_{2})}{dx_{2}}\right)^{T}&\ldots&\left(\frac{dm(x_{n})}{ dx_{n}}\right)^{T}\\ =\left[\begin{array}{cc}\left(\frac{dm(x_{1})}{dx_{1}}\right)^{T}&\left( \frac{dm(x_{2})}{dx_{2}}\right)^{T}&\ldots&\left(\frac{dm(x_{n})}{dx_{n}}\right) ^{T}\\ =g^{\prime}(X)\end{array}\right]^{T}\end{array} \right] \tag{28}\] \[=g^{\prime}(X)\]  
where \(g^{\prime}(\hat{X})\) means the derivative of \(g^{\prime}(X)\) for \(\hat{X}\). Moreover, when \(X(t)\to X_{\text{cs}}\), the global asymptotic stability or consensus of the closed-loop system is guaranteed. **Proof. ** The following equations can be obtained using Lemma 1 for this optimal consensus problem:  
\[T(\hat{X},\mathbf{U})=\hat{X}^{T}\mathbf{R}\hat{X}+\mathbf{h}(\hat{X})+\mathbf{U}^{T}\mathbf{R}_{1 }\mathbf{U} \tag{29}\]  
\[f(\hat{X},\mathbf{U})=\mathbf{U} \tag{30}\]  
where \(f(0_{mm\times 1},0_{mm\times 1})=0_{mm\times 1}\). \(\square\)  
By selecting \(V(\hat{X})\), which is an applicable Lyapunov function then  
\[V(\hat{X})=\hat{X}^{T}\mathbf{P}\hat{X}+g(\hat{X}) \tag{31}\]  
\(\mathbf{P}\) is the solution of the Riccati equation, which is expressed later. For the function in Equation (31), \(V(\hat{X})\) should be a valid Lyapunov function. It must be continuously differentiable with respect to \(\hat{X}\), and in this case, \(g(\hat{X})\) is continuously differentiable with respect to \(\hat{X}\). It can be observed from Equations (26) and (27) that in the safety area \(\Theta\), \(m(x_{i})\) will be continuously differentiable. If \(m(x_{i})\) and \(\frac{dm(x_{i})}{dx_{i}}\) are continuous at the boundary of the diagnostic zone, i.e., \(\|x_{i}-O_{bj}\|=R_{j}\), this is true. As Equation (27) means that \(\lim_{\|x_{i}-O_{bj}\|\to R_{j}^{*}}m(x_{i})=0=\lim_{\|x_{i}-O_{bj}\|\to R_{j}^ {*}}m(x_{i})\), \(m(x_{i})\) is continuous at \(\|x_{i}-O_{bj}\|=R_{j}\), and thus, continuous over \(\Theta\). Furthermore,  
\[\frac{dm(x_{i})}{dx_{i}}= \begin{cases}0&x_{i}\in\Pi\\ \frac{-4(R_{j}^{2}-\tau_{j}^{2})(R_{j}^{2}-\|x_{i}-O_{bj}\|^{2})}{\left(\|x_{ i}-O_{bj}\|^{2}-\tau_{j}^{2}\right)^{3}}\big{(}x_{i}-O_{bj}\big{)}&x_{i}\in \Gamma_{j}\\ \text{not defined}&x_{i}\in\Lambda_{j}\end{cases} . \tag{32}\]Therefore, \(\lim_{\|x_{i}-\mathbf{O}_{y}\|\to R_{j}^{-}}\ \frac{dm(x_{i})}{dx_{i}}=\mathbf{0}_{m \times 1}=\lim_{\|x_{i}-\mathbf{O}_{y}\|\to R_{j}^{+}}\ \frac{dm(x_{i})}{dx_{i}}\), which means that \(\frac{dm(x_{i})}{dx_{i}}\) is continuous at \(\|x_{i}-\mathbf{O}_{y}\|=R_{j}\), and thus, continuous over safety area \(\Theta\). As a result, \(g(\hat{\mathbf{X}})\) and \(\mathbf{V}(\hat{\mathbf{X}})\) are continuously differentiable for \(\hat{\mathbf{X}}\) in safety area \(\Theta\). The Hamiltonian for the consensus problem becomes  
\[ \begin{array}{ll}\mathbf{H}(\hat{\mathbf{X}},\mathbf{U})&=\mathbf{T}(\hat{\mathbf{X}},\mathbf{U})+V^{ \mathbf{T}}(\hat{\mathbf{X}})\mathbf{f}(\hat{\mathbf{X}},\mathbf{U})\\ &=\hat{\mathbf{X}}\mathbf{R}_{2}\hat{\mathbf{X}}+\mathbf{h}(\hat{\mathbf{X}})+\mathbf{U}^{\mathbf{T}}\mathbf{R }_{1}\mathbf{U}+[2\hat{\mathbf{X}}^{\mathbf{T}}\mathbf{P}+\mathbf{g}^{\mathbf{T}}(\hat{\mathbf{X}})]\mathbf{U} \end{array} . \tag{33}\]  
Setting \((\partial/\partial\mathbf{U})\mathbf{H}(\hat{\mathbf{X}},\mathbf{U})=\mathbf{0}\) results in the optimal control law:  
\[\mathbf{U}^{*}=\phi(\hat{\mathbf{X}})=-\frac{1}{2}\mathbf{R}_{1}^{-1}\mathbf{V}^{\prime}(\hat {\mathbf{X}})=-\mathbf{R}_{1}^{-1}\mathbf{P}\hat{\mathbf{X}}-\frac{1}{2}\mathbf{R}_{1}^{-1}\mathbf{g} ^{\prime}(\hat{\mathbf{X}}). \tag{34}\]  
From Equation (33), it follows that  
\[