## 4 Consensus Algorithm for Optimal Obstacle Avoidance  
In this section, the consensus problem is expressed by the problem of optimal control. A closed consensus law of obstacle avoidance, which is a linear function of \((L\otimes\mathbf{I}_{m})X\) based on the local communication topology, is derived by the inverse optimal control method. \(\otimes\) denotes a Kronecker product that is used to extend the dimensions and \(\mathbf{I}_{m}\) denotes the identity matrix of dimension \(m\). For the sake of presentation, the error state is defined as follows:  
\[\mathbf{\hat{X}}=\left[\mathbf{\hat{x}}_{1}^{T}\mathbf{\hat{x}}_{2}^{T}\ldots,\mathbf{\hat{x}}_ {\mathbf{nj}}^{T}\right]\triangleq X-X_{\mathbf{cs}} \tag{4}\]  
Figure 2: MAUV consensus scene with an obstacle. where \(\mathbf{X}_{\mathbf{cs}}=\left[\mathbf{1}_{1\times n}\otimes\mathbf{\chi}_{\mathbf{cs}}^{T}\right]^{T}\) denotes the ultimate consensus state. For example, for motion in space, \(\mathbf{x}_{\mathbf{cs}}=\left[ \begin{array}{cc}\alpha_{x}&\alpha_{y}&\alpha_{z}\\ \end{array} \right]^{T}\), where \(\alpha_{x}\), \(\alpha_{y}\), and \(\alpha_{z}\) express the ultimate consensus position of the \(x\)-axis, \(y\)-axis, and \(z\)-axis, respectively. Based on the property of the Laplacian \(\mathbf{L}\) in Equation (2), when all AUVs reach the consensus, we obtain  
\[(\mathbf{L}\otimes\mathbf{I}_{\mathbf{m}})\mathbf{X}_{\mathbf{cs}}=\mathbf{0}_{nm\times 1} \tag{5}\]  
The ultimate consensus state \(\mathbf{X}_{\mathbf{cs}}\) is constant at the moment and when the AUV reaches the consensus, the consensus law \(\mathbf{U}\) reaches zero. Thus, the error of dynamics becomes  
\[\dot{\mathbf{X}}=\mathbf{U} \tag{6}\]  
If the system (Equation (6)) is asymptotically stable, it will reach the consensus. \[ \begin{split}\min:&\mathbf{J}=\mathbf{J}_{1}+\mathbf{J}_{2}+\mathbf{J }_{3}\\ &\mathbf{s}.\mathbf{t}.\dot{\mathbf{X}}=\mathbf{U}\end{split}  \tag{7}\]  
The function of the optimal obstacle avoidance is constructed, the formula for which consists of three cost functions, where \(\mathbf{J}_{1}\) expresses the control effort cost, \(\mathbf{J}_{2}\) denotes the consensus cost, and \(\mathbf{J}_{3}\) indicates the obstacle avoidance cost. First, the control effort cost is  
\[\mathbf{J}_{1}=\int_{0}^{\infty}\mathbf{U}^{T}\mathbf{R}_{1}\mathbf{U}\mathbf{dt} \tag{8}\]  
\[\mathbf{R}_{1}=\mathbf{w}_{\mathbf{c}}^{2}\mathbf{I}_{\mathbf{n}}\otimes\mathbf{I}_{\mathbf{m}} \tag{9}\]  
In Equation (8), \(\mathbf{J}_{1}\) is a regular quadratic, and in Equation (9), \(\mathbf{R}_{1}\) is a positive definite matrix. Furthermore, a scalar weighting parameter \(\mathbf{w}_{\mathbf{c}}\) is defined. Second, the consensus cost is  
\[\mathbf{J}_{2}=\int_{0}^{\infty}\dot{\mathbf{X}}^{T}\mathbf{R}_{2}\dot{\mathbf{X}}\mathbf{dt}=\int _{0}^{\infty}\dot{\mathbf{X}}^{T}\Big{(}\mathbf{w}_{\mathbf{p}}^{2}\mathbf{L}^{2}\otimes\mathbf{ I}_{\mathbf{m}}\Big{)}\dot{\mathbf{X}}\mathbf{dt} \tag{10}\]  
In Equation (10), the Laplacian matrix \(\mathbf{L}\) is established by the undirected and connected graph and it is symmetric. The weight of the consensus error is represented by \(\mathbf{w}_{\mathbf{p}}\). **Proposition 1** ([32]). : \(\mathbf{L}^{2}\) _is positive semidefinite, and when the graph is connected and undirected, \(\mathbf{L}^{2}\mathbf{I}_{\mathbf{n}\times 1}=\mathbf{0}_{\mathbf{n}\times 1}\)._  
**Remark 1**. : _Proposition 1 indicates that \(\mathbf{R}_{2}\) is a positive semidefinite matrix. The \(\mathbf{R}_{2}\) formula in Equation (10) can ensure that the optimal control law is the linear function of \(\mathbf{L}\), and it is entirely dependent on the flow of information in the topology, as expressed by the proof of Theorem 1._  
Finally, the obstacle avoidance cost is  
\[\mathbf{J}_{3}=\int_{0}^{\infty}\mathbf{h}\big{(}\mathbf{\dot{X}}\big{)}\mathbf{dt}, \tag{11}\]  
where \(\mathbf{h}\big{(}\mathbf{\dot{X}}\big{)}\) is constructed from an inverse optimal control method using Theorem 1.