Optimization-based Control for Bearing-only Target Search with a Mobile Vehicle

TL;DR

This paper presents an optimization-based control method for a mobile vehicle to locate an unknown target using only bearing measurements, without relying on global position data, validated through simulations.

Contribution

It introduces a bi-objective optimization framework that combines target estimation and vehicle control, enabling efficient target search without global positioning.

Findings

Effective target approach demonstrated in simulations

Controller does not require global position information

Bi-objective optimization improves search performance

Abstract

This work aims to design an optimization-based controller for a discrete-time Dubins vehicle to approach a target with unknown position as fast as possible by only using bearing measurements. To this end, we propose a bi-objective optimization problem, which jointly considers the performance of estimating the unknown target position and controlling the mobile vehicle to a known position, and then adopt a weighted sum method with normalization to solve it. The controller is given based on the solution of the optimization problem in ties with a least-square estimate of the target position. Moreover, the controller does not need the vehicle's global position information. Finally, simulation results are included to validate the effectiveness of the proposed controller.

Equations

$$m(k)=\varphi (k)+e(k),$$

$$x(k+1)$$

$$y(k+1)$$

$$\theta (k+1)$$

$$\begin{array}{rl}\underset{\omega (k)}{\mathrm{maximize}}& \left[\begin{array}{c}{f}_e(\mathbf{p}(k+1))\\ f_c(\mathbf{p}(k+1))\end{array}\right]\text{subject to}\text{eqref}eqn:model,\end{array}$$

$$f_e(\mathbf{p}(k+1))+\alpha f_c(\mathbf{p}(k+1)),$$

$$f_e(\mathbf{p}(k+1))=\det (C({\mathbf{p}}_T{)}^{-1})=\frac{1}{{\sigma }^4}\sum _{i<j}\frac{{\text{sin}}^2(\varphi (j)-\varphi (i))}{r^2(i)r^2(j)},$$

$$f_c(\mathbf{p}(k+1))=\sum _{i=k-n+2}^{k+1}\left(r^2(i-1)-r^2(i)\right)$$

$$\underset{\omega (k)}{\mathrm{maximize}}$$

$$r(k+1)⩽r(k)\text{and}(\text{ref}eqn:model),$$

$$v_c\left[\begin{array}{c}\text{cos}(\theta (k))\\ \text{sin}(\theta (k))\end{array}\right]=v_r(k)\left[\begin{array}{c}\text{sin}\varphi (k)\\ \text{cos}\varphi (k)\end{array}\right]+v_t(k)\left[\begin{array}{c}\text{cos}\varphi (k)\\ -\text{sin}\varphi (k)\end{array}\right].$$

$$x(k+1)-x(k)$$

$$y(k+1)-y(k)$$

$$\displaystyle\begin{aligned} \frac{2v_{c}}{\omega(k)}\text{sin}\Big{(}\frac{\omega(k)h}{2}\Big{)}&\text{cos}\Big{(}\theta(k)+\frac{\omega(k)h}{2}\Big{)}\\ =&h\big{(}v_{r}(k)\text{sin}(\phi(k))+v_{t}(k)\text{cos}(\phi(k))\big{)},\\ \frac{2v_{c}}{\omega(k)}\text{sin}\Big{(}\frac{\omega(k)h}{2}\Big{)}&\text{sin}\Big{(}\theta(k)+\frac{\omega(k)h}{2}\Big{)}\\ =&h\big{(}v_{r}(k)\text{cos}(\phi(k))-v_{t}(k)\text{sin}(\phi(k))\big{)}.\end{aligned}$$

$$\tan\big{(}\theta(k)+\frac{\omega(k)h}{2}\big{)}=\frac{v_{r}(k)\text{cos}(\phi(k))-v_{t}(k)\text{sin}(\phi(k))}{v_{r}(k)\text{sin}(\phi(k))+v_{t}(k)\text{cos}(\phi(k))},$$

$$\displaystyle\begin{aligned} \omega(k)&=\frac{2}{h}\arctan\Big{(}\frac{v_{r}(k)\text{cos}(\phi(k))-v_{t}(k)\text{sin}(\phi(k))}{v_{r}(k)\text{sin}(\phi(k))+v_{t}(k)\text{cos}(\phi(k))}\Big{)}\\ &~{}~{}-\frac{2}{h}\theta(k).\end{aligned}$$

$$v_{r}(k)h=\Big{\|}\frac{({\bf p}_{T}-{\bf p}(k))({\bf p}_{T}-{\bf p}(k))^{\prime}}{\|{\bf p}_{T}-{\bf p}(k)\|^{2}}({\bf p}(k+1)-{\bf p}(k))\Big{\|},$$

$$v_{t}(k)h=\Big{\|}\Big{(}\text{I}_{2}-\frac{({\bf p}_{T}-{\bf p}(k))({\bf p}_{T}-{\bf p}(k))^{\prime}}{\|{\bf p}_{T}-{\bf p}(k)\|^{2}}\Big{)}({\bf p}(k+1)-{\bf p}(k))\Big{\|},$$

$$\begin{array}{rl}& {\text{sin}}^2(\varphi (k+1)-\varphi (k))=\frac{v_t^2(k)}{(r(k)/h-v_r(k){)}^2+v_t^2(k)},\\ & r^2(k)-r^2(k+1)=2r(k)v_r(k)h-(v_r^2(k)+v_t^2(k))h^2.\end{array}$$

$$v_r^2(k)+v_t^2(k)$$

$$\underset{v_r}{\mathrm{maximize}}$$

$$\frac{v_c^{2}h}{2r}⩽v_r⩽v_c,$$

$$\begin{array}{rl}{f}_e(v_r)& =\frac{(v_c^2-v_r^2)h^2}{{\sigma }^4{r}^2(r^2-2r{v}_{r}h+v_c^2{h}^2{)}^2},\\ f_c(v_r)& =2r{v}_{r}h-v_c^2{h}^2.\end{array}$$

$$\begin{array}{rl}\underset{v_r}{\mathrm{minimize}}f(v_r)\text{subject to}& \frac{v_c^{2}h}{2r}⩽v_r⩽v_c,\end{array}$$

$$f(v_r)$$

$$\frac{f_e(v_r(k)))}{f_e(v_r(k-1))}$$

$$\rho =v_{c}h/r$$

$$v_{r}^{*}=\left\{\begin{array}[]{ll}v_{c},&\text{when}~{}\rho\in(0,\rho_{c}],\\ v_{z},&\text{when}~{}\rho\in(\rho_{c},1].\end{array}\right.$$

$$v_{r}^{*}=\left\{\begin{array}[]{ll}v_{z},&\text{when}~{}f(v_{z})<\beta,\\ v_{s},&\text{otherwise}.\end{array}\right.$$

$$\displaystyle\operatorname*{\text{minimize}}\limits_{{\bf p}_{T}}~{}\frac{1}{2}\sum_{i=1}^{k}\left\|m(i)-\text{arctan}\Big{(}\frac{x(i)-x_{T}}{y(i)-y_{T}}\Big{)}\right\|^{2}.$$

$$\displaystyle\begin{aligned} &\text{sin}\Big{(}m(i)-\arctan\big{(}\frac{x(i)-x_{T}}{y(i)-y_{T}}\big{)}\Big{)}\\ &=\frac{1}{r(i)}\big{(}\text{sin}(m(i))(y(i)-y_{T})-\text{cos}(m(i))(x(i)-x_{T})\big{)},\end{aligned}$$

$$\begin{array}{rl}\underset{{\mathbf{p}}_T}{\mathrm{minimize}}\frac{1}{2}\sum _{i=1}^k& \Vert x(i)\text{cos}(m(i))-y(i)(\text{sin}(m(i))\\ & -[\text{cos}(m(i)),-\text{sin}(m(i))][x_T,y_T{]}^{\prime }{\Vert }^2,\end{array}$$

$$\tan (m(i))=\frac{x(i)-x_T}{y(i)-y_T},i=0,1.$$

Full text

I Introduction

Target search is a problem of steering single or multiple mobile vehicles to a target with unknown position. It has a broad range of applications, such as search and rescue in disaster response settings [1], planetary and undersea exploration [2, 3], search for lost targets [4], and environmental monitoring [5]. In these scenarios, it is significant and challenging for mobile vehicles to reach target positions as fast as possible, which is the motivation of this work.

If the target position is known, quite a few approaches have been proposed to minimize the searching time, see [6, 7, 8, 9] and references therein. Clearly, these approaches cannot be directly applied here. To estimate unknown target positions, various sensor measurements have been utilized, such as bearing angles [10, 11], ranges [12], and time difference of arrivals [13, 14]. However, most of existing works only consider either controlling a mobile vehicle to a known target position as fast as possible [6, 7, 8, 9] or maximizing the estimation performance of sensor measurements [10, 11, 12, 13, 14, 15, 16, 17]. In this work, they are called the control objective and the estimation objective, respectively, and are both essential for the target search problem. Based on these observations, a naive idea is to separately study these two objectives [18, 19, 20], i.e., directly control the vehicle to the estimated target position.

Unfortunately, a reliable estimate cannot always be obtained, particularly in the initial stage of the target search task where sensor measurements are limited. Clearly, it is not necessary to control the vehicle to such an unreliable estimated position. In fact, the controller should depend on the quality of the target position estimate, which is again significantly affected by the controlled trajectory of the mobile vehicle. That is, the control and the estimation objectives should be jointly considered. To achieve it, we propose a bi-objective optimization problem subject to the dynamical constraint of a discrete-time Dubins vehicle, where the first objective is to control the target as fast as possible to the target (assuming it is known) and the second objective is to optimize the estimation performance of the unknown target position.

Usually, the vehicle cannot achieve the control and the estimation objectives simultaneously. Note that the trajectory for the control objective is a line segment from its initial position pointing at the true target, which potentially leads to a poor estimate of the target position [21]. However, the trajectory for the estimation objective significantly deviates from the line segment. Thus, it is essential to balance the two objectives. This work adopts a weighted sum method [22] to transform the bi-objective optimization problem into a composite optimization problem with a weighting parameter representing the relative importance between the two objectives. Although this idea has been widely adopted in [23, 24, 25, 26], it is still difficult to select an appropriate weighting parameter due to different physical meanings of the objective functions. For example, the objective functions in [26] represent the angle difference and the distance difference, which are also of different magnitudes. To this end, we rewrite the composite objective problem by normalizing the two objective functions and introducing a new weighting parameter. It proves to be much easier to select an appropriate value of the new weighting parameter, since both objective functions are normalized to be dimensionless and between zero and one.

However, the normalization always results in a complicated form of the composite objective function. Furthermore, the discrete-time Dubins vehicle [27] adopted in this work brings a challenging constraint in the composite optimization problem, which is different from the single integrators in [13, 14, 18, 19]. In the literature, evolutionary algorithms are commonly-used, such as the genetic algorithm in [23], pigeon inspired optimization method in [24], and value iteration-like algorithm in [28]. Moreover, the graph-based searching method is thoroughly investigated in [25, 26]. Different from the aforementioned approaches, we attack the complicated optimization problem by decomposing the forward velocity of the vehicle and then transforming the optimization problem into a solvable form. Particularly, an explicit solution can be directly obtained for some weighting parameters with this approach.

Then, the controller for target search can be given in terms of the solution to the optimization problem and the vehicle’s global position information. Since the solution to the optimization problem depends on the unknown target position, the controller cannot be directly implemented. To solve this problem, we design a recursive nonlinear least-square estimate [29] of the target position for the optimization-based controller by utilizing the certainty-equivalence principle. This substantially distinguishes our controller from those in [18, 19, 20].

In addition, the vehicle’s global position is not always available in GPS-denied environments, although it is used in [13, 14, 15, 16, 17, 18, 19, 20]. In this work, we further propose a controller without the vehicle’s global position. Observe that the vehicle only aims to reach the target position, for which the relative position information between the vehicle and the target is sufficient. Therefore, we utilize the bearing angles relative to the target in the vehicle’s own coordinate, and derive an optimization-based controller for the global position system (GPS) -denied target search problem together with the vehicle’s orientation angles from a compass. Similarly, the GPS information is not required in [30, 31] to estimate the target position, whereas they employ a group of vehicles to measure the bearing angles relative to their neighbors and to communicate individual estimates with their neighbors. In particular, the communication graphs have to be connected.

The rest of this paper is organized as follows. In Section II, the target search problem is formulated as a bi-objective optimization problem for a discrete-time Dubins vehicle. In Section III, we adopt the weighted sum method to solve the bi-objective optimization problem, which is rewritten by transformation and normalization. The optimization-based controller design is given in Section IV, with a recursive estimator and an optimization solver. Section V considers the target search problem in GPS-denied environments, where the controller is based on the bearing angles in the vehicle’s coordinate. Simulations are conducted in Section VI to illustrate the effectiveness of the proposed controllers. Finally, some conclusion remarks are drawn in Section VII.

II Problem formulation

II-A The target search problem with a mobile vehicle

Consider the scenario in Fig. 1, where a target is located at an unknown position ${\bf p}_{T}=[x_{T},y_{T}]^{\prime}\in\mathbb{R}^{2}$ and the vehicle’s position is ${\bf p}(k)=[x(k),y(k)]^{\prime}\in\mathbb{R}^{2}$ at the $k$-th time step in the global coordinate $\bf{\Sigma}$. We first assume that the GPS information ${\bf p}(k)$ is available and then remove it in Section V. The vehicle measures a noisy bearing angle $m(k)$ from itself to the target, i.e.,

[TABLE]

where \phi(k)=\arctan\big{(}({x(k)-x_{T}})/({y(k)-y_{T}})\big{)} denotes the true azimuth bearing angle, and $e(k)$ denotes the measurement noise.

In this work, we adopt the following discrete-time Dubins vehicle [27]

[TABLE]

to search the target, where $v_{c}$ is the constant forward velocity, $\omega(k)$ is the angular velocity to be tuned, $\theta(k)$ is the orientation, and $h$ is the sampling period. Since the forward velocity $v_{c}$ is finite, the vehicle can only move within a certain region in a sampling period, which is called the feasible region, see Fig. 1. The bearing-only target search problem is to sequentially design the angular velocity $\omega(k)$ only by the noisy bearing angles, such that the vehicle approaches the unknown target position as fast as possible.

Obviously, if ${\bf p}_{T}$ were known to the vehicle, the optimal angular velocity is the one that drives the vehicle to the closest waypoint to the target within the feasible region, i.e., ${\bf p}_{c}(k+1)$, which is called the control objective. Whereas this waypoint is not achievable for ${\bf p}_{T}$ is unknown and needs to be estimated. In light of [10], the optimal next waypoint to optimize the estimation performance for the unknown position ${\bf p}_{T}$ is illustrated as ${\bf p}_{e}(k+1)$ in the feasible region, which is called the estimation objective. However, this waypoint even cannot ensure the vehicle to approach the target.

Based on these observations, the optimal angular velocity $\omega^{*}(k)$ should simultaneously consider both the estimation and the control objectives. To this end, we formulate the following bi-objective optimization problem

[TABLE]

where the objective functions $f_{e}({\bf p}(k+1))$ and $f_{c}({\bf p}(k+1))$ represent the estimation and the control objectives, respectively. Notice that both objectives depend on the unknown position ${\bf p}_{T}$ and are essential to the target search problem.

II-B The controller architecture

To solve the bearing-only target search problem, we design a controller for the mobile vehicle including a recursive least-square estimator and an optimization solver. Fig. 2 shows the proposed controller architecture. Firstly, the online measurement $m(k)$ is utilized to estimate the unknown target position by the recursive least-square approach. Then, we solve the bi-objective optimization problem (3) in ties with the estimate and obtain the angular velocity $\omega^{*}(k)$.

The details of the controller design are elaborated in later sections.

III Solving the bi-objective optimization problem

To solve the bi-objective optimization problem (3), we adopt the weighted sum method [22] and combine the two objective functions of (3) into the following composite form

[TABLE]

where the weighting parameter $\alpha>0$ plays an important role in balancing the two objectives, but lacks physical meaning.

In this section, we first describe the concrete expressions of the two objective functions $f_{e}({\bf p}(k+1))$ and $f_{c}({\bf p}(k+1))$, and then solve the optimization problem.

III-A The estimation and the control objective functions

Assume the measurement noise $e(k)$ in (1) is white Gaussian noise with zero mean and $\sigma^{2}$ variance, i.e., $e(k)\sim\mathcal{N}(0,\sigma^{2})$. The estimation performance is commonly evaluated by the Cramer-Rao lower bound (CRLB) $C({\bf p}_{T})$ [32]. We utilize the most recent $n$ measurements at the current time step $k\geqslant n$, denoted as ${\bf m}_{n}(k)=[m(k-n+1),m(k-n+2),\ldots,m(k)]^{\prime}$, to measure the CRLB, and we adopt the D-optimal design. In light of Theorem 1 in [10], the estimation objective at the time step $k$ is explicitly expressed as

[TABLE]

where $i,j\in\{k-n+1,\ldots,k+1\}$ and $r(\cdot)=\|{\bf p}(\cdot)-{\bf p}_{T}\|$ is the Euclidean distance from ${\bf p}(\cdot)$ to ${\bf p}_{T}$, i.e., the range between the vehicle and the target.

For the control performance of the vehicle approaching the target, we consider to use the range difference within a sampling period, which evaluates the approaching speed of the vehicle to the target. We express the control objective by maximizing the sum of the difference of the squared range in $n$ sampling periods

[TABLE]

under the constraint $r(i)\leqslant r(i-1)$, since the vehicle is always required to approach the target. Observe that we employ the difference of the squared range in (6) instead of the absolute range difference for the ease of solving the optimization problem.

For brevity, we only utilize the vehicle’s current position and measurements to design the angular velocity, i.e., $n=1$ in (5) and (6). Then the composite optimization problem in (3)-(6) is expressed as

[TABLE]

where the first constraint ensures the vehicle to gradually approach the target, and the second constraint results from the dynamics of the Dubins vehicle.

Remark 1**.**

Both objective functions (5) and (6) depend on the measurements ${\bf{m}}_{n}(k)$, which are utilized to estimate the bearing angles and the ranges. In fact, a small number of the measurements, i.e., ${\bf{m}}_{n}(k)$ with a small $n$, are sufficient to evaluate both estimation and control performance at the $k$-th time step, since it has weak correlation with the measurement at the $i$-th time step for $i\ll k$. In this work, we consider a simple case of $n=1$.

In the rest of this section, we show how to solve the composite optimization problem (7).

III-B Transformation and normalization of the composite optimization problem (7)

III-B1 Transformation

Observe that it is very difficult to directly solve the composite optimization problem (7). We transform it into a solvable form by decomposing the forward velocity $v_{c}$ into $v_{r}(k)$ and $v_{t}(k)$, which denote the radial and tangential velocities of the vehicle relative to the target at the time step $k$, respectively, see Fig. 3.

Then, the following relation is clear from Fig. 3 that

[TABLE]

Assuming that $v_{r}(k),v_{t}(k)$ and $\phi(k)$ are constant within the sampling period from $kh$ to $(k+1)h$, we take Euler integral with respect to the continuous dynamics on the right side of (8) and obtain that

[TABLE]

Together with the Dubins vehicle dynamics (2), it holds that

[TABLE]

Furthermore, dividing the two equalities in (9) leads to that

[TABLE]

which implies the relation between $\omega(k)$ and $v_{r}(k),v_{t}(k)$, i.e.

[TABLE]

Moreover, the decomposition of $v_{c}$ shows that $v_{r}(k)h$ is exactly the projected distance of ${\bf p}(k+1)-{\bf p}(k)$ onto the direction of ${\bf p}_{T}-{\bf p}(k)$, i.e.,

[TABLE]

which is denoted as the bold dashed segment on the direction of ${\bf p}_{T}-{\bf p}(k)$ in Fig. 3. Similarly, $v_{t}(k)h$ can be represented as

[TABLE]

which is denoted as the bold dashed segment on the direction perpendicular to ${\bf p}_{T}-{\bf p}(k)$ in Fig. 3. Jointly with (11), (12) and the triangular geometric relationship in Fig. 3, it follows that

[TABLE]

Besides, it follows from (8) that

[TABLE]

Substituting (13) and (14) into the composite optimization problem (7) yields that

[TABLE]

where the dependence of $v_{r}(k)$ and $r(k)$ on $k$ is dropped for simplifying notations and

[TABLE]

III-B2 Normalization

Observe that $f_{e}(v_{r})$ and $f_{c}(v_{r})$ are used to quantify the objectives with different physical meanings, which results in the lack of physical understanding of the weighting parameter $\alpha$. Consequently, it is unclear how to provide an appropriate $\alpha$ to balance the estimation and the control objectives. To overcome this difficulty, we normalize the two objective functions in (16) to be dimensionless and with values in $[0,1]$. Then, the composite optimization problem (15) is rewritten in the following normalized form

[TABLE]

where the normalized objective function

[TABLE]

introduces a new weighting parameter $\beta\in[0,+\infty)$. See Appendix A for the detailed normalization process.

In contrast with the weighting parameter $\alpha$ in (15), the new weighting parameter $\beta$ in (18) can directly quantify the relative importance of the control objective to the estimation objective, which is also an advantage of the normalized optimization problem (17) over those in [23, 24, 25, 26] with simple weighted sums as the objective functions. To be more specific, $\beta=1$ in (18) means the control objective has the same importance as the estimation objective, while the physical meaning of $\alpha=1$ is unclear. Therefore, the new weighting parameter $\beta$ facilitates designers to emphasize the importance between the estimation and the control objectives.

Furthermore, solutions to the normalized optimization problems with different values of $\beta$ lead the vehicle to move in different trajectories. It is interesting to note that if $\beta\geqslant 4$, the solution of (17) results in the heading direction of the vehicle directly toward the estimate of the target position. In this case, the controller design ignores the estimation objective as in [18, 19, 20].

Remark 2**.**

We set a constant $\beta$ in this work. In fact, it can be seen as an optimization variable to be adjusted during the target search task, which, however, is beyond of the scope of this work. A heuristic idea is to design $\beta(k)\propto 1/r(k)$, since the target position may have already been well estimated when the vehicle is close to the target. Another idea is to measure estimation progress by using the following ratio

[TABLE]

and increase $\beta(k)$ if this ratio is close to unity, and decrease it otherwise.

III-C Solution to the normalized optimization problem (17)

Before providing the solution, we introduce an auxiliary variable

[TABLE]

to denote the level of completing the target search task. Specifically, $\rho>1$, i.e., $v_{c}h>r$, means that the vehicle’s traveling distance in a sampling period is strictly larger than its distance to the target. This implies that the vehicle is sufficiently close to the target. Hence, we only focus on the nontrivial case that $0<\rho\leqslant 1$.

Lemma 1**.**

The equation $\frac{\text{d}}{\text{d}v_{r}}{f}(v_{r})=0$ has at least one root in the interval $[v_{s},v_{c}]$, where $v_{s}={2rv_{c}^{2}h}/({r^{2}+v_{c}^{2}h^{2}})$.

Proof*.*

See Appendix B. $\blacksquare$

Let $\rho_{c}=({2-\sqrt{4\beta-\beta^{2}}})/({\beta-2})$ and $v_{z}$ denote the largest root of the equation $\frac{\text{d}}{\text{d}v_{r}}{f}(v_{r})=0$, $v_{r}\in[v_{s},v_{c}]$ by Lemma 1. Then, we provide the solution of (17) in the following theorem.

Theorem 1**.**

The optimal solution $v_{r}^{*}$ of the normalized optimization problem (17) is given as follows.

1. (a)

If $\beta\in[4,\infty)$, then $v_{r}^{*}=v_{c}$. 2. (b)

If $\beta\in[2,4)$, then

[TABLE] 3. (c)

If $\beta\in[1,2)$, then $v_{r}^{*}=v_{z}$. 4. (d)

If $\beta\in[0,1)$, then

[TABLE]

Proof*.*

See Appendix C. $\blacksquare$

Note that $v_{s}\leqslant v_{z}\leqslant v_{c}$. Theorem 1 shows that a larger $\beta$ results in a larger $v_{r}^{*}$. As we discussed in Section III-B that a larger $\beta$ emphasizes more the importance of the control objective, a larger radial velocity $v_{r}$ is derived to approach the target faster. This is also consistent with our intuition. Specifically, if $\beta\in[4,\infty)$, then $v_{r}^{*}$ directly takes its maximum value $v_{c}$, which implies that the vehicle almost ignores the estimation objective.

In addition, observe that finding $v_{z}$ is not needed in all cases in Theorem 1. This potentially saves computational cost, compared to the works in [23, 24, 25, 26, 28].

IV The optimization-based controller

It should be noted from Theorem 1 and (10) that the vehicle’s optimal angular velocity depends on the range $r(k)$ and the bearing angle $\phi(k)$, both of which are unknown and need to be estimated. Accordingly, we design a recursive least-square estimator, and utilize the certainty-equivalence principle for the controller design.

IV-A The recursive least-square estimator

Observing that the unknown $r(k)$ and $\phi(k)$ are both originated from the unknown position ${\bf p}_{T}$, we only need to obtain an optimal estimate of ${\bf p}_{T}$, which is the solution of the following nonlinear least-square minimization problem

[TABLE]

Similar to Stansfield estimator in[29], we also assume the measurement noise $e(k)$ is sufficiently small to justify the replacement of $m(i)-\arctan(({x(i)-x_{T}})/({y(i)-y_{T})})$ with \text{sin}\big{(}m(i)-\arctan((x(i)-x_{T})/(y(i)-y_{T}))\big{)}. Using the relation that

[TABLE]

we approximate the optimization problem (19) as

[TABLE]

where we ignore the rather weak effect of $1/r(i)$ in (20).

Since the optimization problem (21) is a standard least-square problem, we design a recursive algorithm to solve it, see Algorithm 1. In practice, the initial estimate of the target position $\widehat{\bf p}_{T}(1)$ in Algorithm 1 is given as a solution of the following equations

[TABLE]

Note that Algorithm 1 is developed under the assumption of small $e(k)$ and ignorance of $1/r(k)$, and might be biased. Nonetheless, it is verified by the simulation results in [33] to perform well with a small estimation error and to be of practical significance.

IV-B The controller for bearing-only target search

Now, the detailed controller providing the angular velocity is shown in Algorithm 2.

V Control for target search without GPS information

Observe that the controller in Algorithm 2 requires the vehicle’s position ${\bf p}(k)$ in the global coordinate, which is unavailable in GPS-denied environments. In this section, we consider the target search problem without the vehicle’s global position, and utilize the local bearing angles defined in the vehicle’s coordinate to solve it.

V-A * The target search problem without GPS information*

Without the vehicle’s global position ${\bf p}(k)$, we consider the target search problem in the vehicle’s coordinate ${\bf{\Sigma}}^{l}$. As Fig. 4 shows, this coordinate is attached to the vehicle with its positive x-axis coincident with the heading of the vehicle. The target’s position in this coordinate is denoted as ${\bf p}_{T}^{l}(k)=[x_{T}^{l}(k),y_{T}^{l}(k)]^{\prime}$ at the time step $k$, which is time-varying due to the moving vehicle’s coordinate. The orientation $\theta(k)$ is still accessible by a compass.

Moreover, the vehicle measures a local bearing angle $m^{l}(k)$ in the vehicle’s coordinate relative to the target, i.e.,

[TABLE]

where $\phi^{l}(k)=\arctan(y_{T}^{l}(k)/x_{T}^{l}(k))$ denotes the true local bearing angle, and $e^{l}(k)\sim\mathcal{N}(0,\sigma^{2})$ denotes the local measurement noise.

Then, the bearing-only target search problem becomes to sequentially design the angular velocity $\omega(k)$ for the vehicle (2) to approach the unknwon target position as fast as possible only by local noisy bearing angles. To solve this problem, we still design the controller depending on the solution to the bi-objective optimization problem (3).

V-B The optimization-based controller without GPS information

Similar to Section IV, a recursive least-square algorithm is designed in the proposed controller to estimate the target position ${\bf p}^{l}_{T}(k)$. We approximately solve the nonlinear least-square optimization problem

[TABLE]

by a linear least-square form, i.e.,

[TABLE]

The difference from Section IV lies in the time-varying target position in the vehicle’s coordinate due to the moving vehicle. Thus, the state equation of the target position ${\bf p}_{T}^{l}(k)$ is to be derived. According to the relation between global coordinate and the vehicle’s coordinate, it holds that

[TABLE]

Combining with the vehicle dynamics, we further obtain that

[TABLE]

where

[TABLE]

Jointly with (24) and (26), the recursive least-square estimator with only local bearing angles is given in Algorithm 3, where the initial estimate of the target position in the vehicle’s coordinate $\widehat{\bf p}_{T}^{l}(1)$ is selected as a solution of the following equations

[TABLE]

The angular velocity for the vehicle is also given in Algorithm 2 where we use Algorithm 3 to replace Algorithm 1 and the bearing angle and the range estimates are computed as

[TABLE]

and the rest of Algorithm 2 remain unchanged. Thus, we obtain the controller for the target search problem without GPS information.

VI Simulations

In this section, we illustrate the effectiveness of the proposed optimization-based controllers for the bearing-only target search problem. The initial state of the vehicle is set as ${\bf p}(0)=[0,0]^{\prime}$, $\theta(0)=0$, the constant forward velocity $v_{c}=4\text{m/s}$, the target position ${\bf p}_{T}=[100,100]^{\prime}\text{m}$, and its initial estimate $\widehat{\bf p}_{T}(0)=[40,80]^{\prime}\text{m}$. We take the weighting parameter $\beta=1$ unless particular specification, which means the control objective has the same importance as the estimation objective. Moreover, the sampling period is set as $h=0.25\text{s}$.

VI-A Simulation results with GPS information

With GPS information, the controller in Algorithm 1 is implemented in a target search task to approach an unknown target position. Fig. 5 shows the vehicle’s trajectory and the estimated target positions, where the small square in the upper left corner is the magnification of the results in the target’s neighbor. Clearly, the vehicle moves in a spiral curve and approaches the true target gradually, while the estimated target positions converges to its true position. It reveals that the vehicle achieves the estimation and the control objectives simultaneously with the weighting parameter $\beta=1$.

The cases of $\beta=0$ and $\beta=2$ are depicted in Fig. 6, which shows that the vehicle tends to circle the target for the estimation objective and to directly approach the target for the control objective, respectively. This also explains the rationality of the spiral trajectory in Fig. 5, as $\beta=1$ is between these two cases. The videos of the vehicle’s motions under the proposed controller in the cases of $\beta=0$, $\beta=1$, and $\beta=2$ are uploaded to https://www.dropbox.com/s/9dtpijap65gup50/video_motion.mp4?dl=0.

VI-B Simulation results without GPS information

For the target search problem without the vehicle’s global position ${\bf p}(k)$, we apply the controller in Algorithm 3 with local bearing angles. The results are shown in Fig. 7, where both of the vehicle’s trajectory and the estimated target positions gradually converge to the true target position. It is consistent with our results in Section V. Since the true target position is time-varying in the vehicle’s coordinate, the estimated target positions have larger fluctuation than those in Section VI-A.

In addition, we compare the two recursive least-square estimators of Algorithm 1 and 3 by denoting the estimation error as $e_{\text{est}}(k)=\|{\bf p}_{T}(k)-\widehat{\bf p}_{T}(k)\|.$ We observe that the $e_{\text{est}}(k)$ of Algorithm 3 converges to zero as that of Algorithm 1, although its value is larger in the initial stage and the convergence rate is slower due to lack of the vehicle’s GPS information. It implies that the GPS information is not necessary, but might improve the search performance, which is not studied in previous works [10, 11, 12, 13, 14].

VI-C The role of the weighting parameter $\beta$

In this subsection, we investigate the role of the weighting parameter $\beta$ in balancing the estimation and the control objectives, and provide some guidelines for choosing an appropriate $\beta$.

In Fig. 9, the time for the estimation error less than $1$m are $t=51$s, $t=64$s, and $t=145$s for the cases of $\beta=1$, $\beta=2$, and $\beta=3$. From Fig. 10, the time of estimated range in Algorithm 2 less than $1$m are $t=90$s, $t=71$s, and $t=28$s. That is, the larger the value of $\beta$, the slower the estimation convergence and the faster the vehicle approaching the estimated target position. In particular, we can observe from Fig. 10 that the estimated range easily tends to zero for a large $\beta$, i.e., the vehicle tends to approach the estimated target position. This is consistent with Section III-B that a larger $\beta$ emphasizes more the importance of the control objective. However, the estimate of the target position may be far from the true target position in the case of a larger $\beta$. Figure 11 further shows that the vehicle approaches a “wrong” target due to the large estimation error. Thus, the weighting parameter $\beta$ must be chosen carefully to balance the estimation and the control objectives.

To further explore the search performance with different values of $\beta$, we set a terminal condition of Algorithm 2 as $r(k)<v_{c}h$, indicating the completeness of the target search task. The times for completing the target search task with different values of $\beta$ are given in Fig. 12, where the search time initially decreases with a larger $\beta$, and then increases as $\beta$ become much larger. Finally, it keeps constant when $\beta\geqslant 4.5$. This implies that neither too small or too large value of $\beta$ is preferable in the target search problem.

VII Conclusion

We have derived an optimization-based controller for the discrete-time Dubins vehicle to address the bearing-only target search problem, where the vehicle was to be steered to the unknown target position as fast as possible and the estimation performance for the unknown target position was considered. In the proposed scheme, we estimated the unknown target position via the recursive estimator with bearing angles in global and the vehicle’s coordinate. Using the estimated target position, we formulated and solved a bi-objective optimization problem with the estimation and the control objectives. In particular, the solution was in terms of the predefined importance of the control objective relative to the estimation objective. Then, an optimization-based controller was obtained and drove the vehicle to approach the target as fast as possible while optimizing the estimation performance. Finally, simulations were given to verify the effectiveness of the proposed controller.

Appendix A Normalization of the optimization problem (15)

In the appendix, we denote $\dot{f}(\cdot),\ddot{f}(\cdot)$ as the derivative and second derivative of the function $f(\cdot)$ with respect to the variable in the bracket.

To normalize the objective function in (15), we take the derivative of $f_{e}(v_{r})$ and $f_{c}(v_{r})$ in (16) as

[TABLE]

Let $\dot{f}_{e}(v_{r})=0$, and we obtain a stationary point of $f_{e}(v_{r})$ denoted as $v_{s}={2rv_{c}^{2}h}/({r^{2}+v_{c}^{2}h^{2}})$. Thus, the increasing interval and decreasing interval of $f_{e}(v_{r})$ are $[{v_{c}^{2}h}/{2r},v_{s})$ and $[v_{s},v_{c}]$, respectively, and $f_{c}(v_{r})$ is always increasing in the feasible set $[{v_{c}^{2}h}/{2r},v_{c}]$. It follows that the Pareto-optimal set of the composite optimization problem (15) is $\left[v_{s},v_{c}\right].$

Instituting $v_{r}=v_{s}$ into $f_{e}(v_{r})$ and $f_{c}(v_{r})$, we obtain the ideal value of $f_{e}(v_{r})$ and the nadir value of $f_{c}(v_{r})$ below

[TABLE]

Similarly, taking $v_{r}=v_{c}$, we obtain the nadir value of $f_{e}(v_{r})$ and the ideal value of $f_{c}(v_{r})$ below

[TABLE]

Jointly with (16), (28) and (29), we can normalize the objective functions as

[TABLE]

Then the normalized objective function is given as

[TABLE]

with a new weighting parameter $\beta\in[0,+\infty)$.

Appendix B Proof of Lemma 1

The derivative of $f(v_{r})$ in (17) is computed as

[TABLE]

Let $\dot{f}(v_{r})=0$, and we have three solutions denoted as $z_{1},z_{2}$, and $z_{3}$. Since we only focus on the interval $v_{r}\in[v_{s},v_{c}]$, the first derivatives of $f(v_{r})$ at $v_{s}$ and $v_{c}$ are computed as follows

[TABLE]

Thus, $v_{s}$ is one of the roots of $\dot{f}(v_{r})=0$, which implies that $\dot{f}(v_{r})=0$ has at least one root in the interval $v_{r}\in[v_{s},v_{c}]$.

Appendix C Proof of Theorem 1

Obviously, solutions of the normalized optimization problem (17) exist in the Pareto-optimal set, thus we only need to focus on $f(v_{r})$ for $v_{r}\in[v_{s},v_{c}]$. Based on the proof of Lemma 1, the first derivative of $\dot{f}(v_{c})$ is indefinite, the sign of which is to be analyzed in the following.

To this end, we firstly consider the continuity of $\dot{f}(v_{r})$ and observe that there exists and only exists a discontinuity point, i.e., $v_{r}=(r^{2}+v_{c}^{2}h^{2})/2rh$, which makes the denominator of $\dot{f}(v_{r})$ equal to zero. Furthermore, this discontinuity point is larger than $v_{c}$, implying that the function $\dot{f}(v_{r})$ is continuous in the Pareto set $[v_{s},v_{c}]$.

Then, we take the second derivative of $f(v_{r})$ in $v_{r}\in[v_{s},v_{c}]$ and obtain that

[TABLE]

where $\Theta=r^{2}-v_{c}^{2}h^{2}$ and $\Xi=r^{2}-2v_{r}hr+v_{c}^{2}h^{2}$. Inserting the bounds of the Pareto-optimal set $v_{s},v_{c}$ into the second derivative yields that

[TABLE]

implying that the function $\dot{f}(v_{r})$ is increasing at $v_{s}$ and $v_{c}$.

Now, we focus on the sign of $\dot{f}(v_{c})$. Since the denominator is positive in $[v_{s},v_{c}]$, we only needs to analyze the sign of the numerator $(2-\beta)(v_{c}^{2}h^{2}+r^{2})+4rv_{c}h$.

In light of the fundamental inequality $a^{2}+b^{2}\geqslant 2ab,\forall a,b,$ we obtain that $\beta\geqslant 4$ is a sufficient condition of $\dot{f}(v_{c})\leqslant 0$. The upper subfigure in Fig.13 approximately depicts $\dot{f}(v_{r})$ in this case, where $\dot{f}(v_{r})$ is continuous in $[v_{s},v_{c}]$, $\dot{f}(v_{s})=0$, $\dot{f}(v_{c})\leqslant 0$, and (33) holds. Then the function curve of $f(v_{r})$ can be approximated corresponding to that of $\dot{f}(v_{r})$, where $f(v_{s})-f(v_{c})=\beta-1>0$ and $f(v_{c})=1$. Therefore, the minimum of $f(v_{r})$ is $f(v_{c})$.

Similarly, we have that $\beta\leqslant 2$ is a sufficient condition of $\dot{f}(v_{c})\geqslant 0$. There are two cases depicted in the upper subfigures in Fig. 14 and Fig. 15. In the first case, the other two zero points besides $v_{s}$ exist in $(v_{s},v_{c})$, and we denote the largest zero point as $v_{z}$. Since $f(v_{s})-f(v_{c})=\beta-1$, it holds that $f(v_{z})$ is the minimum if $\beta\geqslant 1$. For $\beta<1$, the minimum is the smaller one of $f(v_{s})$ and $f(v_{z})$. Fig. 14 depicts the function curves of $\dot{f}(v_{r})$ and $f(v_{r})$, where $\dot{f}(v_{s})=0$, the other two zero points of $\dot{f}(v_{r})$ are in $[v_{s},v_{c}]$, and the corresponding $f(v_{r})$ are marked with $\beta\geqslant 1$ and $\beta<1$, respectively. In the second case, the function $\dot{f}(v_{r})$ remains positive in $(v_{s},v_{c})$, since there is no zero points. It implies that $f(v_{r})$ always increases and $\beta<1$, as Fig. 15 shows. Therefore, the minimum of $f(v_{r})$ is $f(v_{s})$.

For the case of $\beta\in(2,4)$, it clearly holds that $f(v_{s})-f(v_{c})=\beta-1>0$. Let $(2-\beta)(v_{c}^{2}h^{2}+r^{2})+4rv_{c}h=0$, and we obtain the following relation that

[TABLE]

It follows that if \rho\in\big{(}0,(2-\sqrt{4\beta-\beta^{2}})/(\beta-2)\big{]}, then $\dot{f}(v_{c})\leqslant 0$. The function curves are shown in Fig.13. Otherwise, the function $\dot{f}(v_{c})>0$, and the function curves are similar to that in Fig. 14 marked by $\beta\geq 1$.

To summarize, we have following conclusions with different values of $\beta$.

1. (a)

If $\beta\in[4,\infty)$, then $\dot{f}(v_{c})\leqslant 0$. It follows from $f(v_{s})-f(v_{c})=\beta-1>0$ that the minimum of $f(v_{r})$ is $f(v_{c})$. The function curves are shown in Fig. 13.

2. (b)

If $\beta\in[2,4)$, together with $f(v_{s})-f(v_{c})=\beta-1>0$, there are two conditions that

i) If \rho\in\big{(}0,(2-\sqrt{4\beta-\beta^{2}})/(\beta-2)\big{]}, then $\dot{f}(v_{c})\leqslant 0$. Thus, the minimum of $f(v_{r})$ is $f(v_{c})$. The function curves are shown in Fig. 13.

ii) If \rho\in\big{(}(2-\sqrt{4\beta-\beta^{2}})/(\beta-2),1\big{)}, then $\dot{f}(v_{c})>0$. Thus, the minimum of $f(v_{r})$ is $f(v_{z})$, where $v_{z}$ is the largest root of $\dot{f}(v_{r})=0,v_{r}\in[v_{s},v_{c}]$. The function curves are shown in Fig. 14.

3. (c)

If $\beta\in[1,2)$, then $\dot{f}(v_{c})>0$. It follows from $f(v_{s})-f(v_{c})=\beta-1\geqslant 0$ that the minimum of $f(v_{r})$ is $f(v_{z})$. The function curves are shown in Fig. 14. 4. (d)

If $\beta\in[0,1)$, then $\dot{f}(v_{c})>0$. Together with $f(v_{s})-f(v_{c})=\beta-1<0$, there are two conditions that

i) If there is any root of $\dot{f}(v_{r})=0$ in the interval $(v_{s},v_{c}]$, then the minimum of $f(v_{r})$ is the smaller one of $f(v_{z})$ and $f(v_{s})$. The function curves are shown in Fig. 14.

ii) If there is no root of $\dot{f}(v_{r})=0$ in the interval $(v_{s},v_{c}]$, then the minimum of $f(v_{r})$ is $f(v_{s})$. The function curves are shown in Fig. 15.

The optimal solution $v_{r}^{*}$ of the normalized optimization problem (17) can be selected as in Theorem 1.