# Find object using only distance

I'm working on a extremely simple robot (very first project) that attempts to find the source of a Bluetooth signal. There are two motors that drive the platform and each has an encoder. We've already used a Kalman filter to calculate the approximate distance to the Bluetooth beacon within reasonable error.

I worked out a manual solution using some trig that solves the problem in theory, but it fails if there is any error (For example, it attempts to turn 73 degrees, but turns 60).

My question is how can I reasonably drive the motors based on the encoder data to continuously minimize the distance to the signal? Furthermore, is there a generic solution to problems like these? (I guess you might call it a stochastic "Hotter/Colder" problem)

• Welcome to Robotics, robits28371. This is a great first question! I hope you come back, with more questions and with answers for other users! – Chuck Jul 21 '16 at 18:11

There are two challenges that you will have to address. First is localization of the robot relative to the Bluetooth source; second is the controller that will drive the robot to the source.

To localize the robot, I would consider using a triangulation method. One robotic platform that addresses a similar problem is the Kilobot, which is used in swarm robotics research. The Kilobot has an IR sensor, which it uses to determine the distance to its neighbours--this sensor is very susceptible to noise. Furthermore, the Kilobot uses vibrating motors to move, which are even worse for control than low quality motor encoders. The triangulation method is given on page 794--the only difference will be that your robot will be green and the source will be blue. To triangulate, you must take a minimum of three relative distance measurements from the source at different known locations (approximated due to dubious encoder quality)--you can now use this coordinate as a desired position for the controller.

To control the robot, we need to drive the robot to the source by applying control inputs. Because you don't have any specific robot in mind, let's assume that you have a two-wheeled differential drive robot (TWDDR). TWDDRs can be modeled with unicycle dynamics of the following form: $$\dot{z}=\left[\begin{matrix}\dot{x}\\ \dot{y} \\ \dot{\theta} \end{matrix}\right] = \left[\begin{matrix}cos(\theta)&0\\sin(\theta)&0\\0&1\end{matrix}\right] \left[\begin{matrix}v\\\omega\end{matrix}\right],$$ where $x$ and $y$ are Cartesian coordinates of the robot, and $\theta \in (-\pi,\pi]$ is the angle between the heading and the $x$-axis. The input vector $\left[v, \omega \right]^T$ consists of linear and angular velocity inputs. For simplicity, I will assume that you can specify these inputs directly, which is reasonable for most off-the-shelf robots--if you want to implement this from scratch, then you have to specify velocities for the left and right wheels.

Driving the robot to the source is equivalent to shifting the origin of the state and driving the state of the robot to 0. For unicycle robots, a easy method of control is achieved by controlling a point, which is holonomic, some small distance $l$ away from the center of the the two wheels rather than controlling the unicycle robot directly. To do this, we can derive the following rotation matrix to transform the control law of the robot to the control law of the point: $$\dot{p}=\left[\begin{matrix}\dot{p_x}\\\dot{p_y}\end{matrix}\right]=\left[\begin{matrix}\text{cos}(\theta)&-l\text{sin}(\theta)\\\text{sin}(\theta)&l \text{cos}(\theta)\end{matrix}\right]\left[\begin{matrix}v\\\omega\end{matrix}\right]$$

$\dot{p}$ is the velocity of the point being controlled, and it is decomposed into its $x$ and $y$ components.

At this point, control is quite simple; we control the point directly! The dynamics become: $$\dot{z}=\left[\begin{matrix}\dot{x}\\ \dot{y} \\ \dot{\theta} \end{matrix}\right] = \left[\begin{matrix}cos(\theta)&0\\sin(\theta)&0\\0&1\end{matrix}\right] \left[\begin{matrix}v\\\omega\end{matrix}\right]=\left[\begin{matrix}cos(\theta)&0\\sin(\theta)&0\\0&1\end{matrix}\right] \left[\begin{matrix}cos(\theta)&sin(\theta)\\-\frac{sin(\theta)}{l}&-\frac{cos(\theta)}{l}\end{matrix}\right]\left[\begin{matrix}\dot{p}_x\\\dot{p}_y\end{matrix}\right],$$Setting $$\dot{p}=u=\left[\begin{matrix}x_{desired}\\y_{desired}\end{matrix}\right],$$ accomplishes the objective of driving the robot to the desired pose, where $u$ is the input, and $[x_{desired},y_{desired}]^T$ is the location of the source.

The gif below uses this controller, driving all of the robots from arbitrary initial positions to the desired position at $(-5,-5)$.

I have glossed over the challenges of noisy sensor and actuators, but the core idea has been presented--both triangulation and control should be possible in simulation, and with a little tweaking, it should work on a physical system.

• Whoa, thanks for the comprehensive answer. I will have to input motor speed for the left and right wheels, unfortunately, but that doesn't add too much complexity. – robits28371 Jul 11 '16 at 20:36
• No problem. Control of unicycle robots using individual wheel velocities as inputs has been discussed on the robotics stack exchange, so most of the math is done for you! – JSycamore Jul 11 '16 at 20:38