modelling and implementing synchronization of dual wheeled ... · modelling and implementing...
TRANSCRIPT
Modelling and implementing synchronization of dualwheeled inverted pendulums with LEGO NXT
Kristofer BerglundJohan Ersvik
Svante Ekholm LindahlJohan Söderman
August 8, 2010
Embedded Control Systems course 2010Department of Information Technology
Uppsala university
Abstract
This project was undertaken to investigate the feasibility of synchronizing two un-stable balancing two-wheeled robots, while carrying a load together and following apredefined path.
The master-slave approach was chosen for this synchronization problem. The controlsystem was divided into two controllers, one balancing and one synchronizing, wherethe synchronizing controller output is fed to the underlying balance control (cascadecontrol). Tachometers in wheels were used to get position estimates in the plane. Thesepositions then give the angle and distance between robots. Sonar sensors were also usedto measure the angle and distance. By combining tachometers and sonar measurementsin a sensor fusion, improved estimates were obtained.
Individually, the tachometers and sonars had rather poor performance due to driftand inaccuracy, respectively. Combining both measurements improved performancesignificantly and the robots were able to carry the load.
1
Contents
1 Introduction 41.1 Problem description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.2 Group members and work allocation . . . . . . . . . . . . . . . . . . . . . . 41.3 LEGO NXT hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.3.1 Actuators and Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . 51.3.2 Bluetooth . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.4 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51.4.1 Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51.4.2 nxtOSEK . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2 Synchronization theory 62.1 Synchronization controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
3 Modelling and simulation 73.1 LegoWay controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.2 Leader-follower model and controller . . . . . . . . . . . . . . . . . . . . . . 93.3 Using planar coordinates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.3.1 Simulation using the planar coordinate estimates . . . . . . . . . . . 113.3.2 Time delay compensation . . . . . . . . . . . . . . . . . . . . . . . . 11
3.4 Using the sonar sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123.5 Sensor fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.5.1 Simulations of sensor fusion . . . . . . . . . . . . . . . . . . . . . . . 153.6 Final controller for the slave . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.6.1 Feedforward for slave . . . . . . . . . . . . . . . . . . . . . . . . . . . 153.7 Position controller for master . . . . . . . . . . . . . . . . . . . . . . . . . . 17
4 Implementation 184.1 Balance Task . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184.2 Communication Task . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184.3 Main Task . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4.3.1 Master position controller . . . . . . . . . . . . . . . . . . . . . . . . 204.3.2 Slave position controller . . . . . . . . . . . . . . . . . . . . . . . . . 20
2
5 Results 215.1 LegoWay controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215.2 Sonar sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215.3 Planar coordinates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215.4 Sensor fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 225.5 Final results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
6 Discussion 236.1 LegoWay controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 236.2 Synchronization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 236.3 Going forward . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
7 References 25
3
1 Introduction
1.1 Problem description
The overall goal of the project regarded synchronization of two self-balancing LEGO NXTrobots (LegoWays) cooperating on carrying a lengthy solid object. Both robots jointly ex-ecute a control algorithm for the dynamical alignment of their movements and each robotpreserves its upright position independently of the other one using a gyro.
1.2 Group members and work allocation
The group members were organized into working pairs; Johan Söderman and Johan Ersvik inworkteam one and Kristofer Berglund and Svante Ekholm Lindahl in workteam two. Workwas dynamically allocated to one of the workteams as it became available. Therefore, everymember has done a little of this and a little of that throughout the course. The whole groupwas often synchronized before moving on to the next stage of the project in an organizedmanner.For example, workteam one implemented the LegoWay controller while workteam two per-formed the synchronization simulations in Matlab, and workteam one modelled and imple-mented the sensor fusion while workteam two worked on the report.
1.3 LEGO NXT hardware
The Lego NXT brick is a small, mobile computer which can be extended with sensors andactuators (effectively becoming a mobile robot). C/C++ programs can be compiled anduploaded to the NXT brick with the nxtOSEK special firmware [2]. This firmware also addssupport wireless Bluetooth communication, C/C++ library for IO devices and so on.The NXT brick has two processors, see figure 1. The main processor is responsible forcontrolling the robot and executing programs, while the coprocessor samples the sensorswith 333 MHz and controls the motors.
Main processor CoprocessorModel ARM 7TDMI Atmel ATmega 48Type 32-bit RISC 8-bit RISC
Clock frequency 48 MHz 8 MHzMemory 64 kB 512 BStorage 256 KB 4 KB
Figure 1: Specifications of the two processors in the NXT brick. The processors are minimalisticand not very powerful.
4
1.3.1 Actuators and Sensors
When equipping the NXT brick with different sensors and actuators it is be able to interactwith the outside enviroment.
• Motor The motors are used to produce the torques needed to balance the robot. Todrive the motors, the NXT Brick generates the necessary analog DC voltage. Each mo-tor has a built-in rotation sensor, i.e. a tachometer, that measures speed and distance(in units of 1o).
• Sonic sensor The ultra sonic (sonar) sensor enables the NXT robot to detect objectsand measure the distance to them. The sensor is able to measure the distances from 0to 255 centimeters with a precision of ±3 cm.
• Gyroscope The HiTechnic gyro sensor is used to measure the pitch angular velocityof the robot with a precision of 1o/s. The angular velocity readings are inherentlybiased. This is solved by calibrating the sensor for about two seconds during the start-up sequence. During calibration, the average over the gyro sensor values is measured.The bias is then assumed to be constant during the run.
1.3.2 Bluetooth
Bluetooth is a wireless communication standard for very small distances. The NXT brick onlysupports the serial profile which gives a maximum throughput of 200 kB/s. Bluetooth pro-vides the means for two robots to communicate with each other. The latency of a Bluetoothmessage is estimated to around 30-100 ms.
1.4 Software
1.4.1 Matlab
MATLAB is a high-level programming language for numerical computing, data acquisitionand analysis [7]. It provided the means to calculate controller and observer matrices, simulatethe models and plot the results graphically.
1.4.2 nxtOSEK
nxtOSEK is a programming environment for the Lego NXT [2]. It allowed ANSI C/C++code to be compiled for the NXT brick using standard compilers such as GCC.
5
2 Synchronization theory
Consider a system with any number of plants (or actuators) p1, p2, . . . , pn. Two plants pi andpj are synchronized if they behave identically or coordinate in space and time to performa specific function. Such a synchronization is said to be controlled if there exists artificialinputs into the system. Synchronization control deals with controlled synchronization: whichinput should we apply to guarantee synchronization?[6] identifies two types of synchronization:
• Internal (mutual) synchronization: All plants in the system are on equal terms andcommunicate with cooperative algorithms. This defines the cooperative control field.
• External synchronization: The plants are ordered hierarchically; i.e. for two robots onegives orders to the other.
A logical approach to solving a synchronization problem, such as the one stated in section 1.1,is to model a dynamic mechanical system of mobile robots. The modelling typically results ina set of differential equations which forms the basis for designing a controller, often in statespace form. The most common approach for solving the dynamics of the system of mobilerobots is to use the a set of generalized coordinates q and the Euler-Lagrange equations.
2.1 Synchronization controllers
Given a system of mobile robots and a dynamic model, it is possible to design controllers forthe synchronization of the system. The controller aims to minimize a set of synchronizationerrors e = q− q where q is the vector of desired values for q. In other words, the set of errorse is the offset vector of q and must approach zero asymptotically.Such a controller requires exact knowledge of the positions, velocities and angles of the robots.If they are not, then many of these can be estimated using Kalman observers.We also mention some state of the art synchronization controllers:
• Adaptive controller. Rademakers [8] applied an adaptive law as defined by [1]; time-varying variables in controller design. The model improves with time, which results insmaller synchronization errors.
• Optimal controller. Zhu et al. [5] expanded the state space form with the synchro-nization error state variable to yield an optimal synchronous controller that provedbetter than ordinary master-slave synchronization.
• Formation approach. Sun et al. [4] applied synchronization control to the formationproblem. The total error was defined as the weighted average of control and formationerror. The controller was then constructed to minimize the total error instead.
6
3 Modelling and simulation
The approaches in 2.1 does not quite fit the Sherpa synchronization problem. Any modellingof the Sherpa problem into a single model inherently results in a very complex model withmany conflicting factors. For instance, the balancing of the robots required a high sample ratefor smooth control that the available Bluetooth communication channel could not provide.Therefore the controllers had to be implemented in the plants themselves, ruling out anyexternal controller implementations. Further, the processing time in the plants forced theapproach of implementing smaller controllers.The Sherpa problem was layered into two control problems. An outline of the leader-followercase is shown in figure 4 (a). In this approach, each plant runs a balancing controller re-sponsible for standing up and following a reference signal of yaw angle or yaw angle velocity,pitch angle and forward velocity. This reference signal can be manipulated by a synchroniza-tion controller which in turn can read the state variables of the LegoWay controller. Thesynchronization controllers communicate via Bluetooth.With this model, the synchronization and LegoWay controllers are in cascade. This meansthat the output of one controller is the input of the other. If the inner system (the LegoWay)is much faster than the outer system (the synchronization) this is not a problem[9].
3.1 LegoWay controller
The motorized two-wheeled inverted pendulum is a well-known problem in control theory.The important characteristics of the LEGO NXT balancing robot (LegoWay) is shown infigure 2. Modelling of the LegoWay includes determining the moments of inertia, mass of therobot, torques of the motors, the lengths, etc.
y
x
φyawv
zφpitch
y
vy
Figure 2: Simplified model of a single LegoWay.
7
Rosén et al.[3] showed that an LQG controller with integral action can be designed andapplied for the LegoWays. Such a controller was designed for the LegoWays using yaw angle,pitch angle and forward velocity as reference signals.Simulations were made with Matlab and the controller was tuned so that the performancewas considered good. A stepping algorithm was implemented to evaluate different weightmatrices. As the algorithm converged, some optimal weight matrices with regard to a linearcombination of the control errors were obtained. Testing the controller on the plants revealedmodel errors, mostly in the description of disturbances. Thus the weight matrices producedby the algorithm could not be used but instead served as a quite good starting point formanual tuning.
0 2 4 6 8
−0.1
−0.05
0
0.05
0.1
0.15
Time (s)
Velo
city
(m/s
)
velocityreference
0 2 4 6 8−100
−50
0
50
100
Time (s)
Yaw
(deg
)
yawreference
0 2 4 6 8−10
−5
0
5
10
Time (s)
Pitc
h (d
eg)
pitchreference
0 2 4 6 8−8
−6
−4
−2
0
2
4
6
8
Time (s)
Appl
ied
volta
ge (V
)
left voltageright voltage
Figure 3: The robot follows a track; a more advanced reference signal with simultaneous velocity,yaw steps and ramps.
Evaluation of the controller was based on performance in terms of balancing, velocity stepsand ramps, yaw angle turning and following a more advanced reference signal. In simulation,when a velocity step was introduced at t = 1 s the robot reached the reference speed in underone second. When a yaw angle step of π/3 was introduced at t = 1s, the robot reached thereference angle in under one second. To illustrate the performance of the controller in thesimulations, figure 3 shows the performance when given a more advanced reference signalwith simultaneous velocity, yaw steps and ramps.
8
An alternative LQG controller used yaw angular velocity as reference signal instead of yawangle. This controller had slightly faster behavior, and was believed to work better with thesynchronization controller when the yaw angle was controlled with P-regulation (see section3.3).
3.2 Leader-follower model and controller
One approach to the synchronization problem is the leader-follower approach. The modelis using hierarchical synchronization as there is only one-way communication. The leadersends information to the follower and it is then up to the follower to make decisions basedon available data. The schema is shown in figure 4 (a).The simple model for the leader-follower model is shown in figure 4 (b). The slave robotfollows the leader as if it were a trailer and thus should always face the back of the master.The objective is thus to minimize the angle θ and to keep the distance between the robots aconstant lr.The position control error is given by ep(t) = d(t) − lr where d(t) is equal to the distancebetween the robots. Assuming that the yaw error θ(t) is small, ep(t) can be minimized bydesigning a PI-controller for the velocity of the slave. The same applies to the yaw angle, sothe controller for both errors would be of the form:
[rv(t)rθ(t)
]= [Kv
p Kθp ][ep(t)θ(t)
]+ [Kv
i 0]∫ t
0
[ep(t)θ(t)
]dt (1)
Synchronization process
Bluetooth communication
LegoWay controller LegoWay controller
Balancing
Synchronization
Synchronization controller
Master Slave
Leader-follower schema
d
θ
(a) (b)
Figure 4: (a) Abstract view of the proposed leader-follower controller. Information flows only fromthe master to the slave and not in the other direction. (b) The leader-follower model is for thefollower to keep the distance at d equal to lr and minimize the angle θ.
9
x1x2
y2
y1
y
x
lr θ
φ2
φ1
v2
v1
Master (Leader)
Slave (Follower)
α
Figure 5: Model used for the leader-follower controller implementation.
3.3 Using planar coordinates
One way to calculate the control error is to introduce an x − y plane as in figure 5. TheLegoWay LQG controller produces estimates of the robot velocity and yaw based on measure-ments from the tachometers. Knowing the direction in which the robot is headed, numericalintegratation of the velocity produces the new coordinate estimates in the x−y plane. There-fore, the planar coordinate estimates are directly derived from the tachometer data.With planar coordinates in place, the objective to minimize the angle θ in figure 4 (b) is nowequivalent to minimizing ϕ2 − α instead. That is, we want that
ϕ2 = α = arctan y1(t)− y2(t)x1(t)− x2(t)
.
If the LegoWay controller takes the yaw angle as input signal, then rϕ2 = θ. If instead theLegoWay controller takes the yaw angular velocity as input signal, then a P-controller canbe designed such that:
rϕ2 = Kϕ2(ϕ2 − α)
The model is two-dimensional and as such the pitch angle reference signal is always set tozero:
rp(t) = 0 ∀ t
10
0 5 10 15 20 25 30−0.1
−0.08
−0.06
−0.04
−0.02
0
0.02
0.04
0.06
0.08
0.1
Time (s)
Velo
city
(m/s
)
slave velzslave velrmaster velzmaster velr
0 0.2 0.4 0.6 0.8 1 1.2 1.4
−0.3
−0.2
−0.1
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
Distance (m)
Dis
tanc
e (m
)
master posslave pos
(a) (b)
Figure 6: (a) Simulated LegoWay velocities. The slave reference signal is the output signal ofthe PI-controller. (b) Simulated LegoWay positions in the x − y plane, carrying the virtual bluelengthy object. The path of the leader LegoWay is a complicated one and presents a challenge forthe PI-controller.
3.3.1 Simulation using the planar coordinate estimates
The controller (1) was implemented in a MATLAB simulation. Approximating the time-continous system with a time-discrete one and taking yaw angular velocitiy as input signal,the control law becomes:
r(n) =
rϕ2(n)rp(n)rv(n)
=
Kϕ2
(ϕ2 − arctan
(y1(n)−y2(n)x1(n)−x2(n)
))0
Kpep(n) +Ki∑n
0 ep(n)
(2)
Two LegoWays were simulated with the LQG controller (see section 3.1). The leader wasset up to follow a preset reference signal r1(n), while the follower was given r2(n) = r(n)(the synchronization controller output signal) as reference signal. The simulation results areshown in figure 6.
3.3.2 Time delay compensation
The Bluetooth communication channel was estimated to have a maximum time delay of 100ms. Assuming that messages would only be sent five times per second, the master positioninformation would always be between 100 ms and 300 ms old. This would have a noticeablenegative effect on performance, since the response time for the slave is mostly due to thetime delay.To compensate for time delays and intermessage times, the master position estimates x ≈x1 and y ≈ y1 were introduced. If all messages are delayed by p samples, the slave canextrapolate location of the master from the received data p samples old. Then for each
11
0 5 10 15 20 25 30 353
2
1
0
1
2
3
4
5
6
7
Time (s)
Erro
r (cm
)
real errorest error
0 5 10 15 20 25 30 352
1
0
1
2
3
4
5
6
Time (s)
Erro
r (cm
)
real errorest error
(a) (b)
Figure 7: (a) Position error when a communication time delay of 100 ms and a message interval of200 ms are introduced. (b) Position error when extrapolating the position of the master, assumingconstant velocity and direction.
iteration, until another message is received, the slave continues to extrapolate the master’sposition under the assumption that the velocity and remains constant.The simulation results are shown in figure 7 in which follower is presented with some difficultsituations under high latency conditions. The position prediction works remarkably well, butit should also be noted here that these simulations were performed under ideal conditionsassuming no disturbances.
3.4 Using the sonar sensors
Another way to calculate the control errors in equation (1) is to use the sonar sensors. Bycombining two sonar senors, it is possible to measure the angle to an object ahead of therobot through means of trigonometry. Figure 8 show the setup of a pair of sonar sensorsand the search region where the object is assumed to be. Each sensor can detect objectswithin an angle of 50o. If the detected object is estimated to be outside the search region,the reading is assumed to be biased and is discarded. The sonar sensor data in general isprone to a large variance.By using equations for a general triangle the values for d and θ can be calculated. Theperimeter p is the existing triangle between both sensors and the target object. The altitudea is the distance between the ground and the ramp where the sensors are mounted. Themedian m is the distance between the slave robot and the object. These are given by:
12
d2d1
a m
w
θ
α
Left sonar Right sonar
Master robot
Slave robot
(a) (b)
Figure 8: Two sonar sensors can be used in pair to determine both the distance and the angleto another LegoWay.
p = (d1 + d2 + w)/2a = 2w−1
√p(p− d1)(p− d2)(p− w)
m = 0.5√
2d21 + 2d2
2 − w2
α = sin−1(a/m)θ = π/2− a
When using two sonar sensors at the same time, it is necessary to take into account that theycan interfere with each other. To avoid interference, a time delay of 10 ms between usage ofthe sensor was implemented. This is enough time for the first sonar signal to subside beforeusing the second sensor.In each iteration, the estimates of d(n+ 1) and θ(n+ 1) are compared to the estimates fromthe previous iteration, d(n) and θ(n). Unless the difference is within tolerance, the new datawill be regarded as inaccurate and rejected.
3.5 Sensor fusion
By combining the methods of estimating d and θ in sections 3.3 and 3.4, better estimatescan be produced. Using this sensor fusion, the synchronization controller receives estimatesof d and θ, which in turn are weighted avarages of measurements from sonar sensors andtachometers.While relying on tachometer data model and testing the synchronization on the real systemof robots, a problematic coordinate drift was detected. To solve this problem, the sensorfusion model included the bias of the coordinate estimates.
13
In the case that the estimates from the sonar sensors are deemed inaccurate and rejected, thedistance and angle would then be calulated only from the measurements from the tachome-ters, the best estimates at that time. Thus the measurements from the sonar sensor wereonly weighted into the sensor fusion if the measurements were relevant, otherwise not.Two models for the sensor fusion were constructed:
• Sensor fusion, model 1. The accumulated bias for the tachometers was estimatedand compensated using a Kalman filter for the sensor fusion. It was then possible toestimate the bias δθ, δd caused by the drift. The model and state variables were:
θ(n+ 1) = θ(n) + w1δθ(n+ 1) = δθ(n) + w2d(n+ 1) = d(n) + w3δd(n+ 1) = δd(n) + w4
The measurement variables were:θtachometer(n) = θ(n) + δθ(n)
θsonar(n) = θ(n) + v1dtachometer(n) = d(n) + δd(n)
dsonar(n) = d(n) + v2
• Sensor fusion, model 2. An alternative Kalman filter for sensor fusion used a feed-back to x-y coordinate position. The new state variables were:
θ(n+ 1) = θ(n) + w1d(n+ 1) = d(n) + w2
with the measurements variables:θtachometer(n) = θ(n) + v1
θsonar(n) = θ(n) + v2dtachometer(n) = d(n) + v3
dsensor(n) = d(n) + v4
Then the estimation of the bias was given by:
δθ = θ − θtachometerδd = d − dtachometer
The accumulated bias for the tachometer estimates is not included in this model. In-stead the bias for each iteration is calculated only after d and θ have been estimatedwith the geometry of figure 9. After that is done, then the bias in x,y-coordinates iscalculated as:
xmaster = (dtachometer − δd) sin(ϕslave + θtachometer − δθ) + xslaveymaster = (dtachometer − δd) cos(ϕslave + θtachometer − δθ) + yslave
This x, y-bias will then be used to compensate the measurements from the master innext iteration.
14
Tachometermeasurementθ
φslave
y
x
Position estimate
δθ
Figure 9: Geometry of the bias estimation.
3.5.1 Simulations of sensor fusion
For the first model, a problem that occured during simulations was that the arctan(x) functionexploded when was x is close to zero. This occured when the positions for the slave andthe master drifted into each other. Because of this problem, the first model was neverimplemented on the plants. Figure 10 shows the simulation results for this model.By using the approach in the second model, the problems caused by the arctan function areremoved. Figure 11 shows the simulation results for this model.
3.6 Final controller for the slave
At this point, a controller for the slave that is possible to implement using all available sensordata can be formulated. Using the sensor fusion estimates for the angle and distance errorseθ = θ and ep = d− lr results in the final position controller for the slave as:
r(n) =
rθ(n)rp(n)rv(n)
=
Kθθ(n)
0Kp
(d(n)− lr
)+Ki
∑n0
(d(n)− lr
) (3)
3.6.1 Feedforward for slave
Adding the master reference velocity rmv to the reference velocity for the slave results in afeedforward of this information. The advantage of using feedforward is that the slave neednot wait for an error to accumulate in order to start moving. This was believed to be agood contribution to maintaining the proper distance between the master and slave. Tocompensate for direction, the signal is multiplied by the cosine of the angle between robotdirections. The feedforward controller for the velocity is then given by:
r′v = rv + cos(ϕ1 − ϕ2)rmv
15
0 1 2 3 4 5 6 7x 104
100
50
0
50
100
150
200
250
300
Time [Samples]
Thet
a [D
egre
es]
Theta
truekalman esttachometer
0 1 2 3 4 5 6 7x 104
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
Time [Samples]
Dis
tanc
e [m
]
Distance
truekalman esttachometer
Figure 10: Simulation of the first sensor fusion model. The Kalman estimates remain good whilethe tachometer estimates accumulate an error. The distance estimates remain good when the yawangle estimates explode.
0 1 2 3 4 5 6 7x 104
100
50
0
50
100
150
200
250
300
Time [Samples]
Thet
a [D
egre
es]
Theta
truekalman esttachometer
0 1 2 3 4 5 6 7x 104
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
Time [Samples]
Dis
tanc
e [m
]
Distance
truekalman esttachometer
Figure 11: Simulation of the second sensor fusion model. The Kalman estimates remain good whilethe tachometer estimates accumulate an error. This model avoids the yaw angle estimate explosionexhibited in figure 10.
16
A simulation of the system of LegoWays using the feedforward controller and tachometerdata is shown in figure 12. The feedforward controller makes the slave robot react to changesfaster but also affects the behavior in other ways, especially because the master robot followsa complicated track. When following a straighter path, the feedforward reduced the totalerror by about half.
0 5 10 15 20 25 30 352
1
0
1
2
3
4
5
Time (s)
Erro
r (cm
)
real errorest error
0 5 10 15 20 25 30 351.5
1
0.5
0
0.5
1
1.5
2
2.5
3
3.5
Time (s)
Erro
r (cm
)
real errorest error
(a) (b)
Figure 12: The synchronization error without feedforward (a) and with feedforward (b) of themaster velocity reference signal. Simulations performed with assumed perfect knowledge (no timedelays or disturbances).
3.7 Position controller for master
A positioning controller was conceieved for the master robot, see figure 13. This controller,similar in design to the slave synchronization controller, was basically a proportional feedbackon the angle and distance to the next desired location. A lower speed limit (than for theslave) was also imposed to enable the slave to keep up (and to prevent the master from fallingover). A number of waypoints in the x, y-plane would be stored in an array. When the masterwas within distance of a waypoint, focus was shifted to the next after a time delay. Whenthe final waypoint was reached and that position was held.
Waypoint 1
Waypoint 2
Waypoint 3
Origin
Figure 13: Waypoints for the master robot.
17
4 Implementation
The robot logic was implemented using C++ and the nxtOSEK platform. The nxtOSEKenvironment was chosen for its use of standard C++, being based on the well tested GNUcompiler, as well as good execution speed of the code.The synchronization solution for the master and slave robot run a different set of threadswhich are scheduled by the nxtOSEK operating system. The programs share the samestructure, which uses three tasks that is running in parallel, the main task, the balancingtask and communication task.The LegoWay LQG controller was first implemented as a single linear program to evaluatethe performance of the LegoWays. Later the program was expanded with the synchronizationprogram and various helper threads were created which only run occasionally, i.e. about fivetimes per second.Figures 14 and 15 show schematics of the final master and slave programs. They differ inseveral ways:
• Both robots calculate two control laws in cascade, one being the balancing LQG con-troller the other being a controller for the position. The slave has the synchronizationPI-controller which makes use of the sensor fusion estimates, while the master has thewaypoint position controller which is only using the master tachometer estimates.
• The master sends messages via Bluetooth, while the slave receives.
• The slave collects sonar sensor data that is used in the sensor fusion. The master doesnot have access to this information.
4.1 Balance Task
An LQG controller is implemented in the LegoWay that handles the balancing of the robot.This task is the most time critical and it is scheduled once every 4 ms and has a higherpriority than the other tasks. The balancing task reads the values of a resource containingreference values for forward velocity and yaw velocity. These values are fed as input to theLQG controller. This resource is written to by the main synchronization task, TaskMain,and its use is surrounded by the acquisition and release of a resource lock.
4.2 Communication Task
The communication task handles the data transfer from the master to the slave that is neces-sary for the synchronization. It runs with a time period of 200 ms. A separate communicationtask is necessary since the send and receive operation can block for an unknown amount oftime. Because of this, it is important that the data to be sent is copied inside a resource lockto a temporary buffer so that the lock can be released before attempting to send the data.
18
Calibrate gyro sensor,Establish Bluetooth connection,
Signal thread start.
Wait for start signal
Main master thread Bluetooth slave thread
Read master position, and calculate distance and angle.
Positioning controller:Calculate input to balance
controller
Sleep until next cycle
Send data (struct)
Sleep until next cycle
Wait for start signal
Balancing thread
Get gyro reading
Read reference values
LQG control law, apply motor voltages
Update state vector estimates and position estimates
Sleep until next cycle
Figure 14: Schematic of the threads in the master program.
Calibrate gyro sensor,Establish Bluetooth connection,
Signal thread start.
Wait for start signal
Main slave thread Bluetooth slave thread
Read master position, and calculate distance and angle.
Read sonar sensor and calculate distance and angle.
Sensor fusion
Synchronization controller
Sleep until next cycle
Receive data (struct)
Sleep until next cycle
Wait for start signal
Balancing thread
Get gyro reading
Read reference values
LQG control law, apply motor voltages
Update state vector estimates and position estimates
Sleep until next cycle
Figure 15: Schematic of the threads in the slave program.
19
On the receiver end, the data is similarly written as a temporary copy before acquiring theread lock and copying the data into the shared resource.
4.3 Main Task
The main task handles the initial configuration before the other tasks start running. Duringthis time the gyro sensors are calibrated before the the robots are placed in upright positionand the balance task is started. Once the robot is standing up the master waits until a Blue-tooth connection with the slave has been established. After the two robots have connected,TaskMain signals the communication task to start sending (master) or receiving (slave) regu-lar updates of the current master position. TaskMain, TaskBalance and TaskCommunicationshare information about the master position using a resource.The main task is executing the actual synchronization model with a scheduling interval of100 ms. The calculation results of the synchronization is written to the shared LQG referencevector that is read by the balance task and fed to the LQG controller. This part of the codeis the general difference between the master and the slave program.
4.3.1 Master position controller
The position controller outlined in section 3.7 was implemented in the master. The robotthus attempts to follow the aforementioned predefined track that is stored in the program asa set of way-points containing a coordinate in the plane and a time duration that the masterwaits at that location before advancing to the next way-point.
4.3.2 Slave position controller
From the communications, the slave receives information about the master location and yawangle, as well as velocity estimates. The slave’s refined estimates are produced using thesensor fusion described in section 3.5. The controller (3) in section 3.6 implemented in theslave then produces reference signals for velocity and yaw angle velocity. The feed forwardterm, as described in section 3.6.1, is also added that takes the master reference speed intoaccount.
20
5 Results
5.1 LegoWay controller
The implementation of the LegoWay controller used model parameters based on availabletechnical specifications of the parts. As always, a physical mechanical system does notbehave as the model does in simulation. At first, the LegoWay controller did not behaveproperly even though the robot in simulation was performing nicely. The robot wobbledmuch back and forth or drove either forwards or backwards until hitting a wall and fallingover.Performance was evaluated by observation of the plant behavior. Modelling the disturbancesso that the gyro sensor data and the tachometer sensor data would be trusted equally bythe controller gave better results. The performance was then improved significantly whenchanging the parameter of the moment of inertia in the motor so that a higher voltage wouldbe applied. The moment was increased by a factor of 100 to yield the improved performance.One possible explaination is that the inertia is changed when force (from the weight ofthe robot) is applied to the wheel axis, another is that there exists a model error due tounlinearities in the motor. The model error is then reduced when changing the expectedvalue of the motor inertia.
5.2 Sonar sensors
Using only the sonar sensors, as described in section 3.4, failed during early testing. Theinformation was found to be very noisy and unrealiable, even though accurate results weresometimes produced. After incorrect information was discarded, progress was made. How-ever, the signal was still very noisy. The sonar sensors also had a very low sampling rate.Thus, using only the sonar sensors for the synchronization estimates was not feasible.
5.3 Planar coordinates
In this approach, described in section 3.3, the positions of the robots in the plane wereestimated. The LQG controller produces estimates of the robot velocity and yaw based onmeasurements from the tachometers. Knowing the direction the robot is headed, numericalintegratation of the velocity produces the new coordinate estimates. This information provedto have much less variance than that from the sonar sensors, but unfortunately the coordinateestimates accumulated a significant bias over time. The planes for the master and slavearbitrarily slide relative to each other.The planar drift has many possible explainations. First and foremost, there is a model error.Possibly, the velocity and yaw estimates are not good enough for use in the simple two-dimensional model. Secondly, the numerical integration of the velocity estimate is certainlyaccumulating an error over time. Finally, wheel spin or imperfection in the robot lineup couldexpand the bias further. A better method for numerical integration, such as Runge-Kutta of
21
fourth order as opposed to Euler’s method, could improve the performance slightly.The techniques for time delay compensation, although fruitful in simulation, did not improveperformance on the real system of robots. This is probably because of the inherit model errorwhen reducing the pair of unstable balancing robots to fixed points in the plane.
5.4 Sensor fusion
Only the second model for sensor fusion and bias was implemented, because of the results ofthe simulations. The sensor fusion compensated well for the drift. The previous estimateswere effecting the following in some negative way. At first, the Kalman filter did not performas it did in the simulations. This problem were solved by making the R1-covariance matrixsufficiently large, so the estimate is only dependent of meaurements data. Thus in practice,the Kalman filter then only results in a least-squares of the measurement data.
5.5 Final results
Combining the sensor fusion estimates with the feedforward for the slave improved the per-formance significantly on the real system of plants, despite mixed simulation results. Theonly problem with using the feedforward controller in combination with the sensor fusionmodel seems to be the Bluetooth communication delay of the feedforward signal.Using yaw angle velocity rather than yaw angle as a reference signal to the LQG controllerimproved performance.The final system of the LegoWays carrying a long stick is shown in figure 16. The slavepositions itself nicely even in difficult situations, such as when the master follows a longtrack and returns to its original position.
22
Figure 16: The real system of robots in action.
6 Discussion
In this system, some of the views and comments of the authors are presented. Some sugges-tions for possible future work are also given.
6.1 LegoWay controller
The LegoWay controller performs quite nicely, validating the results of Rosén et al [3]. Whilethe dynamics for the balancing was provided, modelling the LegoWay controller parameters,constructing and running simulations, implementing the controller and testing it on the plantswere also the most time-consuming tasks of the entire project. Finding adequate parametersand getting reasonable performance proved to be a daunting and complex problem to solvefrom scratch. This unfortunately severely limited the available time for solving the actualsynchronization problem.
6.2 Synchronization
Despite the limitations of the available hardware, the synchronization is well performed.By combining the relevant parts of sensor data through sensor fusion, poor information isdiscarded and the slave does an excellent job of keeping up with the master. It is howeverunclear whether or not there exists a bias in the rotation of the plane. The sensor fusiononly compensates for translations.
23
6.3 Going forward
This section presents some ideas for a possible continuation of the project.Two-way communication: While the synchronization works fine, master-slave synchro-nization is only one approach to the problem. Other interresting avenues to explore includecooperative algorithms, where both robots negotiate and reach decisions collectively.The feedforward of the master velocity is a small step towards a cooperative algorithm. Goingforward with two-way communication, the master could help out in keeping the distance (andpossibly even the direction). One way of doing this is to estimate the communication timeand to then start the drives more synchrounously.Improving the sensor fusion model: A better model of the sensor fusion can surely beaccomplished, for example by introducing the derivatives of d and θ. These new estimatescould be used to better estimate the positions in the plane.Making the time delay compensation work: The problem of high communication la-tency is one that was not adressed properly in the implementation. While the time delaycompensation performed well in simulations, it failed in reality. This was probably because ofa large model error and disturbances in the system. This avenue could certainly be exploredfurther by constructing and testing better models for the system of robots in the time delaycompensation.Better sonar sensors: The accuracy and variance of the sonar sensors included with theLEGO NXT kit leave much to wish for. The robots could use higher precision sonar sensorsthat would improve the measurements substantially.Particle filters: Camera sensors and Gaussian particle filters could also be used to improvethe measurements. There would certainly be enough processing power for these filters if softdeadlines are used. It is however in question if there is enough memory in the plants to runthe particle filters. To compute these filters on a nearby computer would not be possiblebecause of the lack of duplex Bluetooth channels in the plants.
24
7 References
References
[1] K. J. Astrom and B. Wittenmark. Adaptive Control. Addison-Wesley, 1994.
[2] Takashi Chikamasa. nxtosek/jsp ansi c/c++ with osek/µitron rtos for lego mindstormsnxt. http://lejos-osek.sourceforge.net/.
[3] Rosén et al. Two wheeled balancing lego robot. Thesis, Department of InformationTechnology, Uppsala University, 2009.
[4] Sun et al. A synchronization approach to trajectory tracking of multiple mobile robotswhile maintaining time-varying formations. IEEE Transactions on Robotics, 25(5):1074–1086, 2009.
[5] Zhu et al. Synchronization control of parallel dual inverted pendulums. IEEE, 2008.
[6] Alejandro Rodriguez-Angeles Hendrik Nijmeijer, Henk Nijmeijer. Synchronization of me-chanical systems. World Scientific Publishing, 2003.
[7] The Mathworks Inc. Matlab - the language of technical computing.http://www.mathworks.com/products/matlab.
[8] N.G.M. Rademakers. Adaptive Control of a CFT Master-Slave Robot System. PhD thesis,Eindhoven University of Technology, 2003.
[9] Lennart Ljung Torkel Glad. Reglerteknik: Grundläggande teori. Studentlitteratur, 2006.
25