Mobile Robotics COM596
Laboratory Session: week 6
Khepera Control Commands and Applications
Exercise 1: Khepera robot provides command kGetSpeed(ref) for collecting speed sensor data for
the two wheels. Left and right wheel speeds can also be set using the command kSetSpeed(ref,
left, right). The Khepera can also be stopped unconditionally using the command kStop(ref). The
Khepera uses a PID controller for controlling speed. The gains Kp, Ki, Kd can also be set using the
command kSetSpeedPID(ref, Kp, Ki, Kd).
Speed Control:
kGetSpeed(ref)
- returns a 2-element vector of current speeds
kSetSpeed(ref, left, right)
- sets the motor speeds, regulated by PID control
kStop(ref)
- stop Khepera (set speed to zero)
kSetSpeedPID(ref, Kp, Ki, Kd)
- sets the PID coefficients of the speed controller. If no values are specified, they are
reset to the defaults (3800, 800, 100)
Exercise 2: Khepera robot provides the following position sensor commands for controlling the
position using the commands kMoveTo(ref, left, right) and kSetPositionPID(ref, Kp, Ki,
Kd). Velocity and acceleration of the Khepera can also be set using the command
kSetProfile(ref, velLeft, accLeft, velRight, accRight)
Position Control:
kMoveTo(ref, left, right)
- use the position controller to move to a position specified by encoder counts
kSetPositionPID(ref, Kp, Ki, Kd)
- sets the PID coefficients of the position controller. If no values are specified, they are
reset to the defaults (3000, 20, 4000)
kSetProfile(ref, velLeft, accLeft, velRight, accRight)
- sets the velocity and acceleration profile of the position controller. If no values are
specified, they are reset to the defaults (vel=20, acc=64)
Exercise 3: Control the velocity and acceleration of the Khepera robot using the
command kSetProfile(ref, velLeft, accLeft, velRight, accRight) and kGetSpeed(ref)
In the earlier practical we have controlled the robot based on direct movement commands.
For an autonomous robot it will be necessary that the robot can maintain an intrinsic
estimate of the position.
What ever the robot does, it is effective only if the robot's wheels are turning. The wheel
revolutions can be sensed from the wheel encoders. From these readings the robot can
update its position where we assume that it started at the point (x=0, y=0, φ=0). The
general idea is that the average of the speeds of the two wheels gives a good estimate of
the distance travelled whereas the difference between the speeds tells how the bearing
angle of the robot changes.
Some geometric considerations lead to the formula for the x-coordinate, y-coordinate and
heading angle φ:
x ← x + Δx =x + 0.5*(vleft + vright) cos(φ)
y ← y + Δy =y + 0.5*(vleft + vright) sin(φ)
φ ← φ + Δφ = φ - 0.5*(vleft - vright)/(2R)
The formula contains, in addition to the wheel speeds (taken as counter values), the
parameter R that denotes the radius of the robot (or rather half the distance between its
wheels). The parameter (about 4cm) can be determined by measurement, but it may not be
sufficiently precise.
In order to calibrate the odometric formula you can make the robot turn while calculating
the angle. If the robot has turned exactly once (or a number of times for better precision)
the angle estimate can be checked and the "parameter" can be tuned until the measurement
is sufficiently correct.
The following Matlab codes will be useful for calculating the speed of the Khepera.
Example 6.1
function out = calcspd(weightsL, weightsR, reflex)
mL = weightsL(1);
mR = weightsR(1);
for i=2:9
mL = weightsL(i)*(1/1023)*reflex(i-1)+mL;
mR = weightsR(i)*(1/1023)*reflex(i-1)+mR;
end
out = [round(mL) round(mR)];
Exercise 4: Design two neural networks for controlling the two wheels of the
Khepera robot.
S1 and S2 proximity sensors work as inputs to the NN-L and S5 and S6 proximity sensors
work as inputs to NNR. Each motor of the wheel is used as output of the neural network.
Two neural networks for each wheel are shown in Figure 6.1. Design the neural networks
for the inputs and two wheels. Under these circumstances each sensor can detect obstacles
about 5cm away.
F1
F2
NN-R
NN-L
VL
VR
Sensor data {S1..S8}
S
VL V R
Figure 6.1: Control architecture for the Khepera robot.
The following Matlab codes will be useful to design the neural network controller which
will avoid lights.
Example 6.2: Avoid lights
function R = avoidlight(amb_stim, respons)
if(amb_stim(1)+amb_stim(2)+amb_stim(3)+amb_stim(4)+amb_stim(5)+amb_stim(6)<500)
weightsL = [5 1 2 4 4 2 1 1 1];
weightsR = [5 -1 -2 -4 -4 -2 -1 1 1];
R = calcSpd_prox(weightsL,weightsR,amb_stim);
else
R = respons;
end;
function out = calcspd(weightsL, weightsR, reflex)
mL = weightsL(1);
mR = weightsR(1);
for i=2:9
mL = weightsL(i)*(1/1023)*reflex(i-1)+mL;
mR = weightsR(i)*(1/1023)*reflex(i-1)+mR;
end
out = [round(mL) round(mR)];