Acknowledgements As a graduating student in control engineering, the subject of multi-agent systems and autonomous robotics immediately caught my eye. I thought it would be a very interesting and challenging subject to study as my master thesis and to end my studies with. It also gave me the opportunity of doing practical experiments. Since most of the courses in the education are mainly theoretical, I thought of this as an extra advantage of choosing this subject. Now my thesis is finished I can say that it was indeed very challenging and interesting to do. Hereby I present the report of my work in the underlying manuscript. However, I couldn’t have done it without the help of some people, and I would like to thank them for their help and support during the process of the project. First of all I would like to thank Professor De Keyser for giving me this opportunity and for providing insight knowledge now and then to help me move on. I would like to thank especially Daniel Neamtu, with whom I worked together for a big part of the work, as this is a part of his project in the research group. A thanks goes also to my thesis guide, Ramona Hodrea, who provided guidance, insight in the matter and support when needed. I would also like to thank other members of the control research group, Clara, Bart, Ernesto, Adrian, Andrès, Marc and Istvan with whom I could often have a discussion when problems occurred. At last, I would also like to thank some people for providing me with moral support in more difficult times, my parents Kris and Christine, my sister Elien, my girlfriend Nele and my close friends Bart and Maarten. The master thesis has given me a lot of knowledge in autonomous robotics, but also a lot of work experience which you cannot gather with lectures. It was very challenging, but also satisfying, and therefore I find it a nice ending of the great time I had at the faculty of engineering of Ghent University.
Maarten Goossens. Ghent, 25-05-2010
Leader-follower formation control for multi-robot systems Maarten Goossens Supervisor(s): Prof. dr. ir. Robain De Keyser, ir. Ramona Hodrea Abstract -- This article is the summary of a Master Thesis in control engineering. The purpose is to investigate multi-agent systems and to implement a practical realization of a leaderfollower formation in order to understand the control and occurring phenomena in mobile robot systems. The chosen application also serves as a real life simulation for traffic situations. Keywords -- mobile robot, multi-agent systems, tracking control, leader-follower formation, traffic simulation.
of a multi-agent system and a mobile robot its own. Two models were studied, one for a mobile robot tracking another robot, and one for the kinematics of a two-wheeled mobile robot. This results in models with multiple inputs and multiple outputs which are coupled and non-linear. A non-linear extended predictive self-adaptive control (NEPSAC) strategy was implemented in order to maintain the position of the follower robot with respect to the leader robot when the last one changes its driving characteristics. IV. PRACTICAL IMPLEMENTATION OF A LEADER-FOLLOWER
I. INTRODUCTION The field of multi-agent systems is an interesting subject in control engineering today. It already offers a lot of applications today, but it also provides numerous subjects for research and potential applications for the future. In the first section, the state-of-the-art is presented. The second part presents two models and the implementation of a model based predictive control on one of the models. Ultimately, a practical implementation of a leader-follower formation with mobile robots is investigated and realized. II. MULTI-AGENT SYSTEMS The field of multi-agent systems is a wide area with a huge number of applications. The architectures of systems can be divided in different ways. Based on the connection between agents in terms of sharing and making use of information from other agents, the architectures can be divided in centralized, hierarchical, decentralized and hybrid. The second way of making a difference in several structures is according to the mission of each vehicle in the multi-agent environment with respect to the others. The purpose can be leader-following, behavioral or maintaining a virtual structure. There is also a series of different control strategies in multiagent systems, from conventional optimization-based to behavior-based like potential field methods. Examples of applications of multi-agent systems are search and rescue, mapping, traffic simulations, formation holding, etc. III. MODEL BASED PREDICTIVE CONTROL Depending on the application of the multi-agent system, models can be derived for the whole system and for one agent on its own. This is useful for prediction in a model based predictive control and for getting familiar with the kinematics M. Goossens is with the Control Engineering Department, Ghent University (UGent), Gent, Belgium. E-mail:
[email protected] .
FORMATION
The practical implementation of a multi-agent system was done in order to analyze the possibilities and real-life control. The mission that was chosen is a leader-follower formation, where several robots track each other head to tail on a certain distance (chain of robots). The elementary part in this system is a robot tracking the one in front of it. A. Practical set-up The SRV-1Q robots were chosen from Surveyor Corporation for reasons that these robots are open-source, expandable, user friendly and financially attractive. The position from one robot to the other is done by blob-detection with an on-board camera. The distance and angle to the leader robot are extracted from the coordinates of the blob. The robots are controlled by an input that is proportional to the voltage sent to the motors. B. Control in 2 directions In order to control the position with respect to the leader robot, the two controlled variables (the distance and the angle) are decoupled. They are both controlled with a separate Pcontroller. Since we have position control, there is an integrator in the loop which delivers a zero steady-state error. The inputs to the robot are calculated by the two separate control loops. In the end, the two control loops are combined in order to have one value for each input of the robot. C. Line tracking The next step is to track a leader robot at higher speeds. For this we focused on straight line tracking in order to be able to investigate the formation responses later. First a P-controller was implemented. This results in an averaged steady-state error, because the reference is now actually a ramp. The steady-state error is dependent on the gain of the controller and the speed of the leader. Extra measures must be taken in order to keep the robots on a straight line. The motors of the SRV-1Q robots are pretty
inaccurate and are controlled in open loop. A simple on/off control was implemented to cover this issue. In this part a lot of non-linearities were discovered, especially in the motor characteristics, being the relation between the inputs of the motors and the actual speed of the robot. The characteristic shows saturation at higher inputs and a dead-zone at low inputs. The time constant of the motors is different in a moving state than starting from standstill. The minimum real speed of the robot is not close to zero, but a jump is observed in the characteristic.
The responses of the two followers are shifted in time due to a serious delay time in the control loop of the first two robots. With a PI-controller, the two followers don’t reach their setpoint at the same time instant due to the time delay and disturbances. This introduces a real average steady-state error in the distance from the second follower to the leader. This situation is typical for a leader-follower formation where each robot measures and acts on its own and the tracking control is not perfect. This creates the major point for further research in this area. V. CONCLUSIONS AND FURTHER RESEARCH
Figure 1: Motor characteristic
The motor characteristics are also slightly different for different SRV-1Q robots. Measures were taken for the Pcontroller algorithm to bridge the dead zone in the characteristic. In order to get an idea of the characteristics of the system a model was identified. This was done by examining the step input response for the follower robot. The motor characteristics were approximated by a first order transfer function. The model was completed by adding an integrator, a delay time, the saturation and an approximation for the dead zone. The model performs good static results. For example, the steady-state errors are simulated very accurate, if the motor characteristics are linearized around the correct working point. If the working point is too far from the linearization point or if dynamics are simulated, the model does not suffice due to the linear approximations and significant disturbances in reality. In order to improve the straight line tracking, a PI-controller was implemented. By introducing a second integrator in the loop, the steady-state error is decreased. Due to disturbances and the presence of the dead zone in the motor characteristic, a small averaged error is observed in the tracking response. When the speed of the leader robot changes, the PI-controller is not working optimal due to the non-linearity in the motor characteristic, but the performance is much better than with the P-controller. D. Formation analysis In the last stage the formation was extended from two robots to three robots to analyze the formation response. The distance from a certain follower to the robot in front is controlled. The controllers on the two followers are tuned so that they have the same tracking response. The first phenomenon observed is that the tracking response of the second follower is more oscillatory than the first follower. This is the consequence of the fact that the first follower doesn’t maintain a steady-state speed, but has some oscillations. The tracking response of the first follower serves as the input to the second follower. The reference for the second follower is thereby no clean ramp. With the P-controller, the averaged steady-state errors of the two tracking responses of the followers are added.
The area of multi-agent systems is an exciting subject with lots of possible applications. Good results were observed with the model predictive control. On a curved path the response is much slower due to the coupling of the inputs and the outputs. Due to hardware limitations, this advanced control could not be practically implemented. A good and fast control in two directions was obtained when the leader changes its position. The decoupling of the two control loops is an approximation which is visible, but not very significant. For line tracking control at higher speeds, the best results were obtained with a PI-controller. A formation of three robots was realized and the responses were analyzed. This subject, and especially the practical implementation of it, still has a lot of potential for further research. As mentioned before, the particular part in a leader-follower formation is the tracking problem. The system of a mobile robot has a lot of non-linearities which could be taken better into account with more advanced controllers, like neural networks or fuzzy logic. By adding more measurement hardware, like encoders or an IMU board, the performance of a single robot could be improved and thereby also the performance of the entire formation. On the other hand, the phenomenon in a chain of robots due to delay time and imperfect tracking control could be improved by making communication possible between the different robots in the formation. Also modeling of the entire formation and model predictive control could improve the performance. To implement all of this some changes to the existing hardware have to be made. Other research could go in the direction of obstacle avoidance or mapping. REFERENCES [1]
[2] [3]
Gain Luca Mariottini, Fabio Morbidi (2007). Leader-follower formations: Uncalibrated vision-based localization and control. 2007 IEEE International conference on robotics and automation, Roma, Italy, 10-14 April 2007. Maria Isabel Ribeiro, Pedro Lima (2002). Kinematic models of mobile robots. Instituto Superior Tecnico (IST), Portugal. De Keyser R. (2003). A ‘Gent’le approach to predictive control.
Index 1.
Introduction ..................................................................................................................................... 1
2.
State of the art ................................................................................................................................ 2
3.
2.1.
Architectures of multi-agent systems ..................................................................................... 2
2.2.
Control strategies for cooperative control of multi-agent systems ........................................ 4
2.3.
Applications of multi-agent systems ....................................................................................... 5
Theoretical aspects.......................................................................................................................... 7 3.1.
3.1.1.
Manual tuning ................................................................................................................. 8
3.1.2.
Autotuners....................................................................................................................... 8
3.1.3.
Ziegler-Nichols ................................................................................................................. 9
3.2.
4.
5.
PID control ............................................................................................................................... 7
Model based predictive control – The EPSAC approach ....................................................... 10
3.2.1.
MBPC principle .............................................................................................................. 10
3.2.2.
The EPSAC approach...................................................................................................... 11
3.2.3.
NEPSAC for non-linear systems ..................................................................................... 12
Model predictive control ............................................................................................................... 14 4.1.
Model 1: kinematic leader-follower model[12]....................................................................... 14
4.2.
Model 2: Kinematic robot model[11] ...................................................................................... 16
4.3.
NEPSAC implementation and results .................................................................................... 17
Practical implementation of a leader-follower formation ............................................................ 20 5.1.
Practical set-up ...................................................................................................................... 20
5.1.1.
The SRV-1Q robot .......................................................................................................... 20
5.1.2.
Communication interfaces ............................................................................................ 21
5.1.3.
Hardware test of the Motors ........................................................................................ 23
5.1.4.
Blackfin camera ............................................................................................................. 24
5.2.
Position measurement .......................................................................................................... 26
5.2.1. 5.3.
Blob requirements and consequences .......................................................................... 29
Control in two directions ....................................................................................................... 31
5.3.1.
Set-up ............................................................................................................................ 31
5.3.2.
Control strategy ............................................................................................................. 32
5.3.3.
Control script ................................................................................................................. 34
5.3.4.
Experimental results ...................................................................................................... 36
5.4.
5.4.1.
Matlab ........................................................................................................................... 39
5.4.2.
Simulink ......................................................................................................................... 40
5.4.3.
C# ................................................................................................................................... 40
5.4.4.
C-interpreter onboard ................................................................................................... 41
5.5.
Line tracking .......................................................................................................................... 42
5.5.1.
Practical set-up 2 ........................................................................................................... 42
5.5.2.
P-controller .................................................................................................................... 44
5.5.3.
Model identification ...................................................................................................... 52
5.5.4.
PI-controller ................................................................................................................... 61
5.6.
6.
Control script languages and communication....................................................................... 38
Formation analysis ................................................................................................................ 71
5.6.1.
Control parameters of the follower values ................................................................... 72
5.6.2.
Gathering data ............................................................................................................... 72
5.6.3.
Experimental results ...................................................................................................... 73
Conclusions and further research ................................................................................................. 78
Dutch summary (Nederlandse samenvatting) ...................................................................................... 82 Appendices .......................................................................................................................................... 109 APPENDIX 1: Matlab basic commands ............................................................................................ 110 APPENDIX 2: Matlab image processing ........................................................................................... 112 APPENDIX 3: P control script 2D control ......................................................................................... 113 APPENDIX 4: Matlab script -Receiving data 1 robot........................................................................ 115 APPENDIX 5: Matlab control script.................................................................................................. 116 APPENDIX 6: C# control script ......................................................................................................... 118 APPENDIX 7: on-board C control script ........................................................................................... 125 APPENDIX 8: Robot motor characteristics ...................................................................................... 127 APPENDIX 9: on-board PI-controller script...................................................................................... 130 APPENDIX 10: Gathering data from formation ............................................................................... 132 APPENDIX 11: Model m-file............................................................................................................. 134 APPENDIX 12: NEPSAC control script .............................................................................................. 135 APPENDIX 13: Leader control script ................................................................................................ 139 References ........................................................................................................................................... 141
1. Introduction
During the last decades the interest in multi-agent systems grew strongly. It was as usually driven from military application, because of the fact that in a lot of applications a group of small autonomous vehicles are usually much stronger than one big vehicle. One of the major advantages of small autonomous vehicles in this area is that they can be used for situations which are too dangerous for human beings. A group of vehicles then can offer a lot more applications, flexibility and robustness than one bigger vehicle. The autonomous vehicles can be land vehicles, water vehicles or air vehicles. In this master thesis we concentrated on land vehicles, more specifically on mobile robots. The first part of this manuscript is devoted to multi-agent systems in general. Different architectures and control methods are described and compared to each other. Several applications for multi-agent systems are presented as well. One application was chosen to be practically implemented, namely a decentralised leader-follower formation of several mobile robots, or a ‘chain of robots’. This is a good first approximation for real life traffic on highways, providing more insight on the behaviour and control of such systems. A predictive self-adaptive control, NEPSAC, strategy was implemented for a leader-follower formation. A mathematical model was needed, therefore two models were studied. One of these models was used in the predictive control strategy. The models also give more insight on the complexity and coupling between the inputs and outputs of a mobile robot and the consequences of it in a leader-follower formation. For the practical implementation we purchased three mobile robots. The type of robot is described and some tests are performed in order to have a better knowledge of the system with possibilities and limitations in terms of propulsion and measurements. As we want to investigate the behaviour of a leader-follower formation with multiple robots, the elementary part in this configuration is the tracking of one robot after another. This is the biggest part in the manuscript as it also took the biggest amount of time. We start from a static positioning at a big distance to tracking at high speeds at small distances. Initially a P-controller was implemented. This is extended to a PI-controller. A lot of practical issues are encountered, investigated and taken into account. The idea was also to implement the advanced NEPSAC algorithm. However, due to practical limitations this was not possible to do on these robots. In the last part we implement the tracking onto a third robot in order to form a short chain of robots and investigate the controller and distance phenomena between the followers and the leader of the formation. Some phenomena are observed and they form, together with a further expansion of the formation length, the basis for further research. 1
2. State of the art
The beginning of my master thesis consisted mostly of reading literature on the matter. This was necessary since it’s a complete new field for me. The literature was mostly about cooperative control in multi-agent systems. These agents can be land vehicles, water vehicles or unmanned aerial vehicles (UAVs). There are a lot of aspects that could be described considering cooperative control, like architectures of multi-agent systems, control methods, applications, communication, collision and obstacle avoidance, etc. In this literature study, I focused mostly on the first two aspects, as these are the most important to understand and to determine in order to start with a theoretical and practical study of an application that consists of several ‘vehicles’. The first part will describe different architectures of multi-agent systems with advantages and disadvantages. The second part will describe different control methods. The descriptions will be held ‘describing’ and not too much into detail. The goal is to sketch the different possibilities and not to go too deep into the mathematics. This will be done later in the thesis for our particular choice, or should be read in the literature itself. I will also highlight some typical applications of multi-agent systems, in order to give the reader an idea of the use of studies in this field.
2.1.Architectures of multi-agent systems The architecture of multi-agent systems can be divided in two different ways. The first way to differentiate architectures is by the way each vehicle receives its orders. The second way is more about the architecture itself, about what does each vehicle look at to control its own movement. As mentioned, the first way to divide the different architectures, is by the way the vehicles receive their information or orders. It goes from sending all the information from every vehicle to one collector point (PC) and redistributing the information to each vehicle, to only communicating between the closest neighbors, or no communication between the different robots at all. In the following we describe the centralized, hierarchical, decentralized and hybrid architecture. Centralized: The entire team is coordinated from one point. All vehicles send their information to this central controller, which calculates the control inputs and sends it to the vehicles. The problem with this architecture is that if this one central controller fails, the whole system fails. Another disadvantage is the communication. A lot of signals must be received and sent through the same point, which makes the system rather fast overloaded, especially when the group consists of a large amount of vehicles. The advantage on the other hand, is that it’s easy to control, since we have all the information from every vehicle in the same location.
2
Hierarchical: This is actually the same as centralized, but there are a few central controllers, which each control a part of the whole team. Each robot or controller controls the actions of a small amount of other robots. This way we get a hierarchical structure. It is more efficient in communication than the centralized architecture. If a controller, that is positioned high in the control tree, fails though, we still have the same problem as in the centralized structure. Decentralized: The vehicles take action, or control themselves based only on the information they get from the local situation, or their direct neighbors. This structure is highly robust to failure of the total system, but it is hard to create a coherency within the entire team. In a decentralized system, each vehicle makes its own decisions or controls to react on its local environment. The global reaction is a consequence of the different local interactions. A slightly different, but analogue architecture is ‘distributed’. In a distributed system, each vehicle also makes its own decisions or controls, but they rely on global information and/or global goals, instead of only local information and/or local goals. The global information is provided by a centralized information centre. The controls are created by each vehicle itself though, and not by a centralized control centre. The decentralized and distributed architectures have no centralized control centre, whereas they are immune to failure of such a single point in the control architecture. Another advantage is that the group can easily be extended or diminished with one or multiple vehicles. There is no need to adapt a centralized controller for this. Also the group architecture (formation), can easily be adapted. In the distributed structure, the overall system’s formation is even more exact, thanks to the global information that is provided. Hybrid: This is the combination of local control with global control approaches. In this way we try to achieve global coherency and robustness to failure of a centralized controller at the same time. In [1], there are some examples of coordinated control of land vehicles which illustrate some of these architectures. The picture below represents the idea of a centralized, hierarchical and decentralized or distributed system.
Figure 1: multi-agent architectures
3
A second way to differentiate architectures is not the way each vehicle gets its information, but rather where the information is coming from, or what the goal of each vehicle in the group is in respect to the other vehicles. Leader-following: One vehicle is defined as the ‘leader’ of the group. This vehicle tracks random or a predefined path in space. The other vehicles follow the leader in order to keep their position with respect to the leader constant. This way, each vehicle in the group is tracking an analogue path and keeping the formation constant. Behavioral: Each agent has several desired behaviors prescribed. The overall control for a particular vehicle then, is the average of the control for each behavior. Examples of several behaviors are collision avoidance, obstacle avoidance, formation keeping, etc. Since each vehicle controls its own actions, this could be described as a decentralized control structure. Virtual structure: The entire formation of vehicles is considered to be a single structure, as a rigid body. The formation is held constant no matter what. This limits the amount of applications. For example in obstacle avoidance, the whole structure must deflect, and not only the particular vehicle. This structure also lends itself to a centralized implementation, in order to keep the global formation as fixed as possible.
2.2.Control strategies for cooperative control of multi-agent systems When it comes to the most common control strategies for multi-agent systems, there are two major strategies. The first one is optimization-based and the second one is behavior-based or the potential field technique. Both techniques will be shortly explained in the following paragraph. Optimization-based: This strategy mostly relies on finding a control input that minimizes a cost function. In first place, this is a centralized control architecture, since all information is gathered, processed and distributed to the individual vehicles to minimize the global cost function. As mentioned before, a distributed architecture is preferred, due to failure robustness. This can be done by decomposing the global problem into sub-problems, where each sub-problem is assigned to a different vehicle. To make this decomposition possible, one should only use local goals, as there is a minimum of interconnection then. This makes the decomposition a little bit easier to construct. The global cost function in this case is the sum of all local cost functions. In this case we should rather use the term decentralized instead of distributed though. Local control goals can include fuel consumption, distance to neighbors, travel speed, etc. All local information should be able to gather with the sensory of the vehicle in particular. The control laws can be designed by reactive controls (conventional PID, LQR,…) or by model predictive control methods. A model predictive 4
control method we could use is the (N)EPSAC method. Another method is described in [3]. In this last paper there’s also a description on how the control method is adapted to either a centralized architecture or a decentralized architecture. Behavior-based: Each vehicle has preprogrammed schemes to execute upon certain sensor inputs. For example, when two vehicles come too close to each other, the collision avoidance scheme is executed. These schemes are often combined, or based on artificial potential field methods. This in order to keep the control laws as simple as possible. In the potential field method, every vehicle is modeled as a moving particle inside an artificial potential field, that is composed by attractive and repulsive potentials, depending on the environment. For example, when a vehicle wanders too far from the others, it will get a positive potential to get back into the formation. When vehicles come too close on the other hand, a negative potential will avoid collision. The total potential field is the superposition of all separate fields coming from collision and obstacle avoidance, formation keeping, etc. The gradient of the total potential field on a vehicle can be interpreted as an artificial force working on the vehicle, which will be the control law for each individual vehicle. This approach is mainly decentralized, which makes it very failure robust on one hand, but on the other hand it’s limited in a fundamental theoretical understanding of how the global system is behaving. The global reaction must often be determined by extensive empirical data collecting. More explicit explanations on the artificial potential field methods can be found in [2] and [4].
Figure 2: Potential field control
2.3.Applications of multi-agent systems The use of an unmanned vehicle has several advantages. It can, for example, be used in situations that are too dangerous for human beings like fire, toxic environments, … The vehicles can also be made much smaller, as a human doesn’t have to fit into it. This makes it again much more suitable for several applications. Next to the advantages of one unmanned vehicle, several more advantages come from the cooperation between these vehicles. Several small vehicles can be stronger, more flexible, easier to handle, cheaper, more robust, delivering a higher performance… than 1 big vehicle for example. Also 5
from nature, we can see that working in a group has serious advantages, a school of fish or a group of birds for example. Following, we give some examples of typical applications where a group of vehicles is preferred:
Search and rescue: a coordinated group of vehicles can cover an area much faster than one vehicle, and thereby find something or someone much faster.
Mapping
Moving objects: a lot of agents with a small power can make the overall power greater than the sum of all the small powers, by coordinating the contact points for example.
Multitarget observation: when you want to track several objects that can go different directions.
Traffic simulations: Simulation of the phenomena happening in a dense traffic situation.
Competitions: Robot wars or robot soccer for example.
Formation: Each robot can have his own function, and all the robots can be complementary for example. By staying in a formation they are very strong together.
Figure 3: multi-agent systems applications
6
3. Theoretical aspects
3.1.PID control The PID controller is a feedback controller widely used in industrial systems. The controller calculates the error value between a measured process output and a setpoint for that output. It adjusts the inputs in order to minimize this error. PID controllers are the best controllers if the underlying process is unknown. However, in order to have the best performance the parameters of the controller should be tuned according to the nature of the system. The PID controller consists of 3 terms, with each one specific parameter: the proportional, the integral and derivative term. A block diagram of a PID controller is shown in the picture below.
Figure 4: PID control block diagram
The proportional term takes into account the current error. If the value of this parameter is set very high, the system will be very reactive, but will also overshoot and oscillate a lot. Lower values result in a system that is less responsive, but also more stable. If only the P-term is used in the loop and there is no integrator in the system, the output will have a steady-state error in the response to a step input. The integral term takes into account the sum of recent errors. It will result in a steady-state error which is equal to zero for a step input and it makes the control loop much more responsive. This term causes overshoot and less stability. The derivative term bases its reaction on the rate at which the error has been changing. This term stabilizes the control loop and reduces the overshoot caused by the integral term. The calculated input is the sum of the three terms. By tuning the three parameters correctly, according to the specific system, the reaction of the loop can be optimized. If the system is known,
7
the parameters can be calculated theoretically. If the system is not perfectly known, empirical tuning methods exist, of which a few will be shortly described here. The PID controller is popular and efficient, but it has limitations. The system should be linear for example and if there is a lot of noise in the measurements, the derivative term amplifies the noise and makes the loop unstable. 3.1.1. Manual tuning As mentioned before, the output of the controller, or the input to the process, is the sum of three terms. Ach term has its own general characteristics. The three terms cannot be optimized separately, but a good response can be achieved by tuning manually if the typical characteristics of each parameter is known. The characteristics are given in the table below.
Figure 5: PID manual tuning guidelines
By testing and keeping in mind the above mentioned characteristics of the different parameters, a satisfactory response can be reached by tuning manually. 3.1.2. Autotuners Another way of tuning the parameters of a PID controller is by using autotuners. Autotuners give a relation between suggested values for the three PID parameters and characteristics of a relay-test response. An example and the characteristics of a relay-test response are given in the figure blow.
Figure 6: Relay test
8
The table below shows how the PID parameters are determined according to three of these autotuners.
Figure 7: Autotuner parameter formulas
The parameter Kc is given by the following relation between the input and output of the relay test: 𝐾𝑐 =
4. 𝑑 𝜋. 𝑎
The first two autotuners mentioned are not valid for pure second order systems. For this kind of systems, the third autotuning method should be used. In this method a delay time is incorporated in the relay test. 3.1.3. Ziegler-Nichols The Ziegler-Nichols tuning method is not based on purely experimental results. It applies to first order systems with or without delay time and it is used when the main characteristics of the system are known. The relation between the system characteristics and the PID parameter values were determined empirically. The figure and table below show how the values are determined.
Figure 8: Ziegler-Nichols
9
3.2.Model based predictive control – The EPSAC approach In model based predictive control (MBPC) a process model is used on-line for calculating predictions of the future plant output and for optimizing future control actions. Calculating the future plant outputs depend on the quality of the available plan model. The optimizing future control actions are determined by minimizing a cost function. This cot function includes the errors between the desired and predicted process outputs and the required control effort. The most important differences between a MBPC control method and a PID are that the MBPC method needs a good model and requires more computational effort, but the result is much more optimized. The two control methods are used in industry as complementary control methods. The PID controller controls the greater part of control loops, while MBPC takes care of the fewer, but economically more important loops. EPSAC, or NEPSAC for non-linear systems, is one method in the group of MBPC control methods. The principle of MBPC control methods will be explained shortly. 3.2.1. MBPC principle The principle[22], or strategy, of a model based predictive control is explained according to the figure below.
Figure 9: MBPC principle
At each moment, the process output y(t+k) is predicted over a time horizon k=1..N2. This is done by simulating with the model, based on previous inputs and outputs and the future inputs (u(t+k|t)). 1) A reference trajectory, r(t+k|t), is defined as the way we want to guide the output to the setpoint. 2) The control actions, u(t+k|t), are calculated in order to minimize a specified cost function, depending on the predicted control errors (r(t+k|t)-y(t+k|t)). In most MBPC methods there is some structuring of the future control law. This is characterising for the specific method. This will be explained later for the EPSAC method. 10
3) The first element u(t|t) of the optimal control vector u(t+k|t) is actually applied to the real process. The next time sample the vectors are shifted one position, a new measurement y(t+1) is taken and the whole process is repeated. The process is modelled as shown in the picture below (generic process model).
Figure 10: Generic process model
The disturbance n(t) includes the effects in the measured output y(t) which do not come from the model input u(t) via the available model. These non-measurable disturbances have a stochastic character with non-zero average value and can be modelled by a coloured noise process (n(t) = C(q1
)/D(q-1)*e(t)).
The prediction of the output is then given by: y(t+k|t) = x(t+k|t) + n(t+k|t). The prediction of x(t+k|t) is done by the model (x(t) = f(x(t-1), x(t-2),…, u(t-1), u(t-2),…)). The prediction of the noise n(t+k|t) is done by the disturbance model mentioned above. 3.2.2. The EPSAC approach In EPSAC, the future response is considered as being the cumulative effect of two effects: y(t+k|t) = ybase(t+k|t) + yoptimize(t+k|t) ybase(t+k|t): Effect of the past control and the ‘base’ control scenario ubase(t+k|t), which is defined arbitrarely for linear systems (e.g.: ubase(t+k|t) = 0). yoptimize(t+k|t): Effect of the optimizing future control actions: δu(t+k|t) = u(t+k|t)- ubase(t+k|t), for k=1…Nu. Nu is the chosen control horizon. The figure below clarifies the EPSAC control idea.
Figure 11: EPSAC approach
11
From this figure it’s obvious that yoptimize(t+k|t) is the response to a series of impulse inputs and a step input. The impulse input δu(t|t) results in a contribution hk δu(t|t), δu(t|t) in hk-1 δu(t|t),… The step input δu(t+Nu-1|t) results in gk- Nu+1 δu(t+Nu-1|t). The cumulative effect of all impulses and the step are then given by: yoptimize(t+k|t) = hk δu(t|t) + in hk-1 δu(t|t) + … + gk- Nu+1 δu(t+Nu-1|t) The parameters hi (i = 1…N2) are the coefficients of the unit impulse response, while the parameters gi (i = 1…N2) are the parameters of the unit step response. This leads to the main EPSAC equation: Y = Ybase + GU, where:
The cost function is defined as a quadratic form in U: V(U) = [R- Ybase-GU]-1[R- Ybase-GU]. The optimal solution is then given by: U* = [GTG]-1GT(R-Ybase). The actual input to the process is determined by: u(t) : ubase(t|t) + U*(1). The algorithm can easily be expanded to MIMO systems by the use of matrices. 3.2.3. NEPSAC for non-linear systems The previously explained strategy is valid for linear systems, but for non-linear systems some modification to the algorithm must be made. This is because in the EPSAC approach superposition is used in the prediction of the future outputs(y(t+k|t) = ybase(t+k|t) + yoptimize(t+k|t)). Superposition cannot be used in non-linear systems. In order for the theory to be operational on these non-linear systems too, yoptimize(t+k|t) must be made equal to zero. This can be done by the right choice of ubase(t+k|t) and implementing an iterative procedure, as is explained below: 1) Select an initial ubase(t+k|t), with k = 1…Nu-1. The closer to the optimal u(t+k|t) this is chosen, the better. A goos initial guess is ubase(t+k|t) = ubase(t+k|t-1). 2) Calculate the optimizing inputs δu(t+k|t) by the same procedure as described before. The reulting controls u(t+k|t) = ubase(t+k|t) + δu(t+k|t) are not optimal because the superpostion principle does not hold for non-linear systems. However, the calculated inputs are closer to the optimal inputs than the initial guess of ubase(t+k|t) alone. 3) Now choose ubase(t+k|t) equal to the previously calculated u(t+k|t) and return to step 2.
12
By implementing this iteration, the optimizing inputs δu(t+k|t), and tehreby also the optimizing predicted outputs yoptimize(t+k|t) are driven to zero, resulting in optimal inputs for the non-linear system. This procedure needs to be repeated every sample. At last we also have to mention that for non-linear systems the G-matrix for optimization has to be modified every sample, according to the working point the system is working around at that moment.
13
4. Model predictive control
4.1.Model 1: kinematic leader-follower model[12] Mobile robots move in a horizontal plane. This means that 2 coordinates are necessary to determine the position of a robot and 1 coordinate to determine the orientation in this plane. These 3 coordinates can be chosen relative to a fixed world frame, or relative to another robot, with his own position and orientation. In the second case the world frame is actually fixed to the first, also called ‘the leader’, robot. The picture below shows this set-up, and how the coordinates can be chosen. A leader-follower formation is built from couples of 2 robots. There is always 1 leader and 1 follower. The leader acts independently and the follower tracks this leader as good as possible. In other words, the situation between the 3rd robot and the 2nd is exactly the same as between the 2nd and the 1st robot. For that reason, the tracking problem of the follower with respect of the leader is the clue and the basic element to understand and to control.
Figure 12: model 1
The position of the follower robot (F) relative to the leader robot (L), is determined by the three coordinates (ρ, ψ, β). ρ is the distance between the two robots, ψ and β determine the orientation in the plane of the two robots with respect to each other. If we want to maintain a leader-follower formation, this means that these 3 parameters should stay constant, or that the inputs have to be chosen in a way that brings these three parameters in the fastest way to the desired values. The inputs to the robots are the linear speed, which is in the same direction of the robot’s orientation, and the rotational speed, which is defined here as the derivative of θF (with respect to the world frame). The system, being the follower robot, has thus 3 controlled variables (outputs) and 2 control variables (inputs). The two inputs have an effect on all three of the outputs. In other words, there is a coupling between the three outputs and the two inputs. The system that is defined here, is a MIMO 14
system and can be represented by a model. It’s obvious that the model is a static model, which calculates the future path, based on the present input and the last output (or position). The state of the model is chosen the same as the outputs, being the parameters ρ, ψ and β. We derive the model by finding the relation between the derivatives of these parameters and the two inputs. The result will be a number of differential equations. The immediate change of the first parameter, ρ, over time can be determined by looking at the speed vectors projected on the direction of the line that connects the two robots. With the parameters shown in the picture above, we come the following equation: ρ = 𝑣𝐿 cos ψ − π − 𝑣𝐹 cos β + π − π ρ = 𝑣𝐹 cos β + π − 𝑣𝐿 cos ψ The immediate change of the second parameter, ψ, over time can be determined in the same way. It is influenced by the difference in the speed vectors projected on the perpendicular direction to the line that connects the two robots, the distance between the robots and the rotational speed of the robots. The second differential equation is determined below: ψ=
−𝑣𝐹 sin 𝛽 + ψ − π 𝑣𝐿 sin ψ − π + − 𝜔𝐿 𝜌 𝜌
− sin 𝛽 + ψ sin ψ 𝑣𝐹 + 𝑣𝐿 − 𝜔𝐿 𝜌 𝜌 The third and last differential equation is the immediate change of the third parameter, 𝛽, over time. ψ=
This can be done by just differentiating the equation by which 𝛽 is defined: 𝛽 = 𝜃𝐿 − 𝜃𝐹 𝛽 = 𝜔𝐿 − 𝜔𝐹 The model of this 2-robot system is given by 3 coupled non-linear differential equations. These cannot be solved analytically. To solve the equations we make use of the ‘ode45’ command in Matlab and determine at which times exactly we want to know the values of the three parameters. The command solves the differential equations simultaneously and numerically. The times we want to know the values are multiples of the sample time we set. The model can be solved when a start position and input are given. The Matlab script for this can be found in appendix 11. The script is made as a function, so it can be called in the control script that will be explained later. The parameters are the velocity and rotational speed of both the leader and follower, the sample time and the start values. The leader robot moves independently and the inputs to this robot can be chosen arbitrary. The goal is thus to find the best combination of inputs for the follower robot, based on the previous values of
15
the outputs and inputs, to keep the three output parameters as close to the setpoint as possible, and this under the influence changing input values for the leader robot. Since the model is a static (kinematic) model, it serves as a prediction of the future path, based on only the present input and the previous output (or position). The dynamics in this model don’t come from the robots itself, but from the coupling of the inputs to the outputs. To find the right combination of inputs, we can use a model predictive control, like EPSAC. Since the model is nonlinear, it’s more appropriate to implement NEPSAC.
4.2.Model 2: Kinematic robot model[11] In the previous model, the inputs of the follower robot were defined as the velocity and the rotational speed. The robot was represented as a vector, which is not the case in reality. A robot mostly consists of 2 wheels, left and right, which are powered separately by motors. The combination of the left en right speed of the motors result in a certain velocity and rotational speed. With another model, which will be presented here, it’s possible to determine the position of a robot in a fixed world frame, based on only the speeds of the left and right wheels. Part of this model, which converts the speeds of the two wheels into the velocity and rotational speed could actually be used as a conversion to determine the desired speeds of the two wheels in the previous model. This model, which focuses on the kinematics of one robot on its own, has two inputs, the speeds of the wheels, and 3 outputs, the position and orientation coordinates with respect to the world frame. If both the leader robot and the follower robot are represented in this world frame, the distance and orientation of the two robots relative to each other can be determined, and a formation control algorithm, like NEPSAC, which is presented before, can be developed. Since the approach is completely similar to the one applied before, I will only describe the kinematics of one robot here. It’s convenient to understand that there is a coupling between the speeds of the two wheels and the velocity and angular speed. In the section about the practical position control, we decouple these 2 outputs and inputs. The figure below shows the robot with its coordinates and orientation in the world frame.
Figure 13: Model 2
16
The coordinates of the robot in the world frame (X,Y) are (x, y, θ). The first two determine the position, the third one determines the orientation. For a mobile robot, or any moving object, an immediate centre of rotation can be found (ICC). This centre of rotation is on a line that is perpendicular to the forward speed. If the speed of the left and right wheels are called v L and vR, and the distance between the two wheels is L, then the rotational speed of the robot around this centre of rotation can be determined by the following equations: 𝑣𝑅 (𝑡) 𝐿 𝑅+ 2 𝑣𝐿 (𝑡) 𝜔 𝑡 = 𝐿 𝑅− 2 𝜔 𝑡 =
𝑣𝑅 𝑡 − 𝑣𝐿 𝑡 𝐿 𝑣 𝑡 = 𝜔 𝑡 . 𝑅 = 1/2(𝑣𝑅 𝑡 − 𝑣𝐿 𝑡 ) 𝜔 𝑡 =
These formulas determine the coupling between the rotational speed and the velocity, due to the difference and the sum of the speeds of the two wheels. To determine a model for the position and orientation of the robot in the world frame, we have to rotate the speed vector between the frame that is fixed with the robot and the world frame, as shown below: 𝑥 = 𝑣 𝑡 . cos 𝜃 𝑡 𝑦 = 𝑣 𝑡 . sin 𝜃 𝑡 𝜃 = 𝜔(𝑡) If the world frame is thought to be fixed with a leader robot, then the three parameters from the first model can easily be calculated again.
4.3.NEPSAC implementation and results The first model is chosen to implement the model based predictive control on. As mentioned before, the model is kinematic and non-linear. The model can predict the three outputs, based on the current position and inputs for both leader and follower, being ρ, ψ and β, as described above. The follower robot does not know the inputs that are given to the leader. The only variables he knows are the previous inputs applied and the past and current outputs. With the help of the NEPSAC model predictive control we can determine the best future inputs to maintain the desired values in the outputs and this with a minimum cost for the inputs. The Matlab code of the implementation can be found in appendix 12. The system is non-linear, therefore we applied the non-linear EPSAC. This means that we make use of the series/parallel 17
implementation and that there is an iteration loop in order to reduce the optimizing inputs. This is necessary because ubase should be as close to the optimal input as possible in order to avoid superposition, which cannot be used with a non-linear model. Another modification that must be made with the non-linear model predictive script is the determination of the G-matrices. If we were working with a linear model these would be fixed. Because of the non-linearities the G-matrices have to be determined every sample because we are working in a different working point every sample, especially when the inputs of the leader change. The G-matrices are being calculated as the difference between a simulation with the previous inputs and a simulation with the previous inputs plus a step, relative to the step height. The other parts of the control script are a straight forward implementation as described in [22]. Too see the controller performance, we applied a change or ‘disturbance’ in each of the inputs of the leader robot. The results of changing the linear speed of the leader is given in the graphs below. The graphs represent, the three outputs (ρ, ψ, β) as described in the model above, the chosen inputs to the leader (vL, wL), the optimized calculated inputs by the controller to the follower (vF, wF) and the ‘optimization inputs (Udelta vF, Udelta wF) which should be smaller than 10-3 for the superposition in the algorithm to be valid.
Figure 14: NEPSAC results - change in v_L
Conclusion – The linear speed of the leader robot is chosen to change 4 times, first three time an increase of 0.1 m/s and then back to the original speed of 0.2 m/s. We can obviously see that the distance is controlled very well (rho). It first increases, because of the sudden higher speed of the leader, but the controller makes that the follower follows quite fast. We can also see in the input of the follower for the linear speed, that a higher speed than the equilibrium speed is applied first in order to make up for the lost terrain that the leader caused by accelerating. After that the follower
18
converges rather quickly to the same speed as the leader. The optimization process is working properly because the optimized inputs are held beneath the desired value of 10-3. At last we can also see that although only the linear speed is changing, there is also a small change in the angles (psi & beta) and an input for the rotational speed of the follower. These effects are very small (~10-10) and due to the very small linearizing in the calculation of the G-matrices. If the step that is used to calculate the matrices is reduced by a factor 10, the input of the rotational speed of the follower and the effect in the angles are also reduced by a factor 10. This effect can thereby be neglected. A second test is performed by applying a change in the rotational speed of the leader. The results of the simulations are given in the graphs below.
Figure 15: NEPSAC results - change in w_L
Conclusion – We can see that a short change in the rotational speed of the leader has a much bigger influence. The three outputs are influenced at the same time and they all have to be brought back to their own setpoint, while all influencing each other. It’s obvious that also from the set-up of the model, this is a difficult situation and certainly when the cost should still be minimized. Therefore it takes a very long time for the outputs to be brought back to the setpoints. Both inputs of the follower act now. The optimizing inputs are still within the desired area.
19
5. Practical implementation of a leader-follower formation
5.1.Practical set-up 5.1.1. The SRV-1Q robot After getting familiar with the basics of formations and mobile robots, it’s interesting to implement and investigate a practical implementation of a mobile robot formation. In order to do this, we chose a mobile robot that is sophisticated enough to have a good control. This means that we need some kind of measurement device or sensor to measure the absolute or relative position of the robot, means of communication, calculation centre on- or off-board, a decent driving mechanism and an open-source design in order to have enough application freedom. In the same time, the robot should not be too expensive, as the goal is to have many robots and it should be possible to finance them. The mobile robot that was the best candidate to suffice all the requirements is the ‘Surveyor SRV-1Q blackfin’ robot from Surveyor Corporation. The robot is constructed and sold by the company ‘inertia-labs’, a company in California, USA.
Figure 16: SRV-1Q robot
All features and hardware specifications can be found in on the website of the surveyor corporation[23]. We will highlight the most important aspects of this robot here. The driving mechanism of the robot consists of 2 tank-like threads, powered by 4 wheels, each with its own dc motor as propulsion. The motors are controlled 2 by 2, left and right. It’s not possible to control the 4 wheels separately, which would be unnecessary because of the tracks. The camera on board is a ‘blackfin’ camera of which different resolution settings can be chosen and it has a frame rate of 30fps. There is an on-board Flash memory with a C interpreter which can interpret simple programs written in a limited C language. The programming protocol for these C programs can be found in on the website of the Surveyor corporation. The on-board chip makes it possible for the robot to act autonomously, according to the program that is uploaded into the memory. 20
The robot also has several pre-programmed functions, which can be used when connected to the robot through an interface on a PC, but we will come back to that later. The command protocol can be found on the website of the Surveyor corporation. The communication of the robot with other hardware, like a PC, is done over a Wi-Fi network. The robot has its own network and a fixed IP address (169.254.0.10), which makes it possible to connect over TCP/IP communication means in different interfaces. 5.1.2. Communication interfaces There are several ways of opening a communication channel with the robot. We made use of 2 direct interfaces, ‘Putty’ and ‘Tera term’, in which it is easy to send commands directly to the robot. The others are programs with built-in TCP/IP communication protocols, like Matlab or C#. These last interfaces give us the possibility to send commands and receive data in the form of an algorithm. The putty and tera term pro interfaces can be downloaded for free from internet and are quite user friendly. For this the IP address (169.254.0.10) and connection port from the PC (10001) are necessary.
Figure 17: Robot communication interfaces
As mentioned before, these interfaces are only good for direct contact with the robot, meaning command and immediate response, without the possibility of processing or saving data in between. In order to do that we developed our own functions in Matlab with the help of the instrument control toolbox, which can establish a TCP/IP connection between the PC and the robot. The written functions are described below and can be found in appendix 1.
survConnect.m: Connects to the robot through the appropriate IP-address and port. The output is an object, which is connected to.
survDisconnect.m: Disconnects the communication with the robot. The input is the object, which is connected to.
21
setBin.m: Sets the range of YUV indexes by which a range of colours is determined. This is stored in the robots memory. When the blob detection on-board is activated, the software will detect pixels with a colour that lies in the given ranges. YUV indexes of a colour are similar to the better known RGB indexes. The conversion is the following:
Y
= + 0.299R + 0.587G + 0.114B
U
= 0.492(B - Y) = - 0.147R - 0.289G + 0.436B
V
= 0.877(R - Y) = + 0.615R - 0.515G - 0.100B
Another way of determining the YUV index of a pixel in the camera’s view, is to use the ‘vp’ function of the camera that immediately returns the index of the given pixel. The use of the ‘vp’ command can be found in the description of the functions (website Surveyor corp.).
survGetBlob.m: Captures an image and looks for the pixels in the picture with an index that lies in the YUV ranges, set by the setBin.m command. The robot returns a string of data, which contains the area and the coordinates of the blob. This command parses the string and outputs the area, the x- and y-coordinate as doubles to the workspace.
setResolution.m: Sets the resolution of the camera. The input is a, b, c or d, which correspond respectively to the resolutions 160x120, 320x640, 640x480 and 1280x1024.
driveMotors.m: Sends a drive command to the robot. The inputs are the left speed and the right speed. The speeds are scaled speeds, from -100 to +100, being the maximum backward and forward speed respectively.
convertRange.m: The actual speed values that have to be sent to the robot must lie in the interval between 0 and 256. This command converts the [-100, 100] range to the [0, 256] range.
We also have some Matlab functions that don’t make use of this instrument control toolbox, but the connection is made over a TCP/IP connection that is written in Java. We found these functions on the website of the Drexel University. We only used these functions for testing in the beginning of the practical implementation and for getting a picture on the remote PC. For writing commands and reading information that is needed for further control of the robot, we chose to write our own functions, as mentioned above. Later on in this script we will see that we also made use of Simulink and C# as interfaces with the robot. The same functions are needed there. The idea behind the functions are the same, but the implementation is different. These scripts can also be found in one of the appendixes, but we will come back to that in the appropriate section.
22
5.1.3. Hardware test of the Motors As mentioned in the previous section, the command that we can send to the motors involves the left speed and the right speed. These inputs are not the actual speeds though, but rather a value that is proportional to the voltage to the motors. The values that we can give as inputs are values between -100 and 100, or if we only focus at the forward movement from 0 to 100. The first thing we want to do, is to check with what actual speed, in m/s, the inputs correspond. Since the motors are controlled in open loop, we have to be sure that the robot is at full speed before measuring. Therefore we constructed the following test:
Figure 18: Motor test set-up
We measure the time that it takes the robot to cover 1m distance at various inputs [0, 100]. The same input is applied to the left and the right motors, in order to move in a straight line forward. For every input we do the measurement 5 times and then we take the average. We did this because the starting and stopping of the chronometer happens visually, and in this way we want to decrease measurement inaccuracies. Once more I want to draw the attention to the fact that this is in regime on a flat surface. The motors have a time constant with which the speed is built up, but we will also come back to that later. Since the speed is controlled in open loop this will differ if the surface is not flat or if it is made out of another material. This test has as a purpose to have an idea how good the motors are and what the limitations are in this situation. The result of the test is given in the graph
Speed (m/s)
below, which shows the relation between the input and the real output speed. 0,5 0,45 0,4 0,35 0,3 0,25 0,2 0,15 0,1 0,05 0 0
10
20
30
40
50
Input Figure 19: Motor test result
23
The graph of the test shows us some interesting specifications of the driving mechanism on the SRV1Q robot. First of all, the robot doesn’t move at an input that is lower than a certain threshold. When the battery is fully charged the threshold is 18. This means that we cannot build up from a zero speed. There is a so called ‘dead-zone’ between input 0 and input 18. Next we see that the speed saturates around 0.45 m/s. As mentioned before, the input is proportional to the voltage that is applied to the motors. For DC motors, as we have here, it’s normal that they saturate at high input voltages. This means we only have a quasi-linear input-speed behaviour around the input 30. This will be the optimal area to work in. 5.1.4. Blackfin camera The only feedback or measurements we have from the robot are the measurements we do with the camera on-board. A picture of the camera that is used, is given below.
Figure 20: Blackfin camera
By ‘measurements’ we mean that the camera films in front of the robot, with a frame rate of 30fps. The goal is to get information out of this picture, by using image processing, by which we can determine the relative position of the robot with respect to the part of the image that we are tracking. The image processing can be done off or on-board. For on-board image processing, there is a pre-programmed function available, but we will come back to that later. In first instance, the purpose of the project is that the robot can track another object, or ultimately another robot. In order to do that, it should be able to distinguish the ‘leader’ from the rest of the environment, that is also captured in the picture. To accomplish this, we make use of the so called ‘blob-detection’. What it actually does is the following: We mark the leader or the object that should be tracked by a coloured shape that is quite different from the rest of the environment. By image processing we can determine the coordinates of the centre of the blob and the amount of pixels of it with respect to the image coordinate frame. The picture below makes it a bit clearer.
24
Figure 21: Picture coordinate frame
The red area is the whole picture, where the yellow area is the blob that we detect. The origin of a frame captured by a camera is always placed in the upper left corner. Every row of pixels corresponds to a certain y-coordinate and every column of pixels corresponds to a certain xcoordinate. As mentioned before, the blob detection can be done on- or off-board. In first instance, we wanted to accomplish our own image processing algorithm with the help of the Matlab Image processing toolbox. The program works, but seemed not to be very useful because of disturbing communication delays between the robot and a remote PC over the Wi-Fi network. Image processing The image processing program can be found in appendix 2. The idea is that we can take in a video in which the blob, in this case, is a light, or at least something very white with respect to the background. The first part of the program is the blob detection applied on a fixed picture that is stored on the hard drive of the PC. The second part is the image processing applied on the video from the robot. The idea of the algorithm is the following 1) Take in an image: for this we make use of the Java based Matlab code get_srv_image.m. In order to wait for the image, a pause must be incorporated after the image request. 2) Convert the coloured image to an image that consists only of grey. Then we select an interval of grey intensity [240, 260], which corresponds to a very light grey, or in other words, white. 3) Look for areas (coherent discs with radius 20 pixels), that consist of pixels that belong to this grey intensity interval. 4) Convert the picture to a black and white picture, with only the areas selected before set as white. The rest is made black. 5) With the Matlab command centroids() (image processing toolbox) it’s possible then to determine the centre of the blob. If the centre is plotted as a star on the original picture, you can see the centre moving together with the light, as a real-time video. This is one of the only algorithms where we made use of the Java based Matlab functions. Since we’re not using this image processing algorithm any further, because of delay in sending the image 25
from the robot to the remote PC, we didn’t find it useful replacing the functions by stand-alone Matlab functions. The other way of doing the blob detection is with the pre-programmed on-board command ‘vb’, or ‘vblob’ for on-board C-program. With this command the on-board controller looks for all pixels with a colour that belong to the intervals of YUV indexes that have to be specified upfront. The command looks for these pixels and fits a box around the areas where there are neighbouring pixels that all lie in the defined colour region. It returns a string with the following information: amount of blobs, amount of pixels of the blobs, the two x-coordinates of the left and right side of the square fitted to the blob and the two y-coordinates of the upper and lower side of the square fitted to the blob. The pictures below show images from the on-board camera. It shows how the box is fitted around the coloured region.
Figure 22: Blob detection
5.2.Position measurement Now that we are able to detect a blob in the picture we should extract information about the relative position of the robot with respect to the leader or the object that must be tracked. As mentioned in the previous section, we can determine the area of the blob (in amount of pixels), the x-coordinate and the y-coordinate of the blob in the picture. The idea is to translate this information into a distance to the blob and an angle at which the robot looks at it. This is shown in the picture below, where the blob is generated by the lamp.
Figure 23: Position determination
26
The suspicion was that the distance has an influence on the area of the blob and also on the ycoordinate of the centre of the blob. The angle, or the perpendicular divergence, should influence the x-coordinate of the centre of the blob. To confirm this and to determine the accuracy of the camera we did some tests. In the first test, we determine the three values (area, center_x, center_y) at different distances to the blob. The robot keeps on looking straight to the blob, in other words, there is no sideways deviation. The picture below clarifies the idea of the test.
Figure 24: Camera test 1
Area (pixels)
The test is done 3 times and the results are given graphically in the pictures below. 8000 7000 6000 5000 4000 3000 2000 1000 0 -1000 0
Test 1 Test 2 Test 3 10
20
30
40
50
y-coordinate of th center of the blob (pixels)
Distance to the blob (cm)
120 100 80 60
Test 1
40
Test 2
20
Test 3
0 0
10
20
30
40
50
Distance to the blob (cm) Figure 25: Camera test 1 results
The results of the tests show us that both the area and the y-coordinate of the centre of the blob change with a change in the distance of the robot to the blob. The area decreases if the distance is 27
bigger. The y-coordinate of the centre of the blob increases if the distance becomes bigger. This means that the centre of the blob will relatively move downwards if the robot is moved away from the blob. There is some measurement inaccuracy, and this especially in the area measurement. If 3 measurements are taken at the same position, the area may vary with some 100 pixels. The measurement in the y-coordinate of the centre of the blob is quite stable, which means that mostly we have the same result for different measurements in the same situation. If it varies, it’s not more than 1 or 2 pixels. This has as a consequence that the y-coordinate of the centre of the blob will be most useful to determine the distance from the robot to the blob. We must remark that the ycoordinate is not exactly linear with the distance, but the further we go, the lesser the difference in the y-coordinate of the centre of the blob when there is a difference in the distance. This means that we will have the best results, or best resolution so to say, at small distances. The measurement inaccuracies that can appear are less important too then. If we try to keep the distance that corresponds to a value for the y-coordinate of the centre of the blob of 60 for example, we can assume that the characteristic is linear in the neighbourhood of that point. In the second test, we don’t put the robot in positions that are on a straight line to the blob, but in positions that deviate perpendicularly at a distance of 30 cm, like shown in the picture below. Again we determine at each position the three values (area, center_x, center_y).
Figure 26: Camera test 2
Also this test is done 3 times and the graph below confirms our suspicion.
28
x-coordinate of the center of the blob (pixels)
300
-15
250 200 150
Test 1 Test 2
100
Test 3 50 0 -10
-5
0
5
10
15
Perpendicular distance to the blob (cm) Figure 27: Camera test 2 results
The graph shows indeed that the x-coordinate of the centre of the blob changes with the perpendicular distance from the robot to the blob, or in other words, the angle. The measurements are practically always the same and the characteristic is very linear, which makes the x-coordinate of the centre of the blob perfectly useful for controlling the angle from the robot to the blob. The limitations on the angle under which the camera can still detect a blob is about ±25°. 5.2.1. Blob requirements and consequences The characteristics that we measured above are very useful, but are highly dependent on the kind of blob, the position of the blob with respect to the camera and the resolution. We did the same tests with two kind of blobs. The first one was with a lamp. The lamp has as advantage that the colour doesn’t change with the orientation of the blob or light influence from outside or from artificial light in the room. The disadvantage though, is that the lamp doesn’t really have a clear border in the picture, so measurement inaccuracies are bigger. Also, the lamp is placed close to the floor which is quite reflecting and sometimes adds an area to the blob or will be seen as a second blob. The other blob we tested, is made of a coloured paper (yellow) which is a different colour from the objects in the environment of the blob. The advantage of this blob is that it has a clear border, and if the colour ranges are set right the measurements are very stable and accurate. The disadvantage is that the colour changes under the influence of other light situations. The way to avoid this is to widen the colour ranges. The ranges cannot be set too wide either, because otherwise other objects in the neighbourhood will be detected as blobs too. This requires some trial and error to find the optimal colour ranges. The yellow paper we use has the following ranges: Y = [170,210], U = [75,
29
115], V = [120,150]. The characteristics presented in the previous section are for this yellow paper blob. The position of the blob with respect to the camera is important too, especially the vertical position. If the blob is placed completely above the height of the camera, we get the characteristics as presented in the previous section. If the blob is placed completely beneath the height of the camera, the characteristic of the y-coordinate of the centre of the blob will be reversed, meaning that the ycoordinate will decrease with an increasing distance between the robot and the blob. If the blob is placed on the same height level as the camera, the measurements for the y-coordinate of the centre of the blob will have a very limited range. This can be understood as following. When the distance between the robot and the blob decreases, the upper edge of the blob moves up in the picture, while the lowed edge moves down. The area increases indeed, but the y-coordinate of the centre of the blob will only change slightly, as the upper and lower edge ‘movement’ in the picture compensate each of the partially. If the vertical centre of the blob is exactly put on the height of the camera there is no change of the y-coordinate of the centre of the blob at all.
Figure 28: Position of the blob
30
5.3.Control in two directions As mentioned before, the goal is to form a formation of several robots, which can follow each other maintaining a certain distance from each other. In order to do that, the first step is to make a controller for one robot to follow another, or to follow a certain object. Once this step is completed, we can go to multiple robots which will each handle the same control structure. This can be done, because our goal is to make a formation that consists of agents, which calculate the control actions based on only their own measurements. In this section, I will describe the work we’ve done in order to get a good tracking. In first instance, we tried to track a light bulb, and this in 2D. This means that we were able to control the distance, and the angle as well. Using the light bulb, and the big distance as well have some characteristics and consequences. In the process of developing this tracking control, we also spent a lot of work investigating how we can do it practically, with respect to the communications. Several programming methods are investigated with respect to advantages and disadvantages in the next section. 5.3.1. Set-up The set-up is more or less the same as described before. The blob consists of a light bulb fixed in a wooden holder. The choice for a light bulb was because the colour doesn’t change under different lighting conditions. The centre of the light is always seen as a purely white colour, which is good distinguishable from the rest of the environment. This was also chosen because the light bulb emits enough light in order to cover a decent area. This is necessary because the goal of this part was to control the robot at a big distance. If the area of the blob is not big enough, the camera won’t be able to see it. The YUV-indexes of the white light are the following: Y = (255, 255) U = (128, 128) V = (128, 128) The resolution of the camera is set to 640x480. This resolution was chosen because we maintain a big distance and it’s necessary to have enough pixels to determine the blob area and also to have a sufficient measurement accuracy. The last reason is because, as we showed in the set-up section, that at a big distance the y-coordinate of the centre of the blob doesn’t change a lot with a difference in the distance. We also have to remark that the characteristic of the y-coordinate of the centre of the blob with respect to the distance is inverted in comparison with the one described before. This is because the blob is under the height of the camera. The characteristic is given in the picture below.
31
y-coordinate of th center of the blob (pixels)
165 155 145
Test 1 Test 2
135
Test 3 test 4
125 115 0
50
100
150
200
250
Distance to the blob (cm) Figure 29: Camera test on y-coordinate of the centre of the blob
As we can see in the graph, the measurements are not very accurate. There are some ‘disturbances’, or variations on the measurements when different measurements are taken in the same situation. This is because of the reason mentioned before, that the light bulb doesn’t have a clear contour. This is an aspect that makes the control more difficult, but also more interesting. We don’t have this problem if we use a coloured paper for example. We applied this later on a shorter distance, but we first developed control with this characteristic. We can also see that the camera gets ‘blinded’ when the robot comes too close to the light. This is not really a problem as we aim to control at a big distance (1m) anyway. 5.3.2. Control strategy The robot we are using, the Surveyor SRV-1Q, has 2 tracked wheels. The inputs to the robot are two speeds, the left and the right speed, in a range from -100 to 100. The robot is controlled open loop and has thereby no speed control onboard, so the inputs are actually values that are proportional to the applied voltage to the motors. As described before, we take measurements to control both the distance, or straight forward movement, and the angle, or rotational movement. As both movement modes are controlled by the two tracked wheels, the two movements are obviously coupled. This means that we have two inputs, the left and right voltage inputs, and two outputs, the y-coordinate and the x-coordinate of the centre of the blob. This is a MIMO-system of which both inputs are coupled to both outputs.
Figure 30: MIMO system
32
To have an optimal control strategy, it’s impossible to split the control of both outputs. The model for this MIMO control is a kinematic model which determines both outputs, based on both inputs, as described earlier in the script. The implementation of this was not done practically because of the limitations of the current communication channels and onboard processor, but we will come back to that later. In first instance, we will split the control of the two outputs. The control of both outputs is calculated separately, which gives us values for both inputs, for each control loop. Afterwards, both control loops are combined to determine 1 value for each of the two inputs. As described before, the distance from the robot to the blob is determined by the y-coordinate of the centre of the blob. Since we want to keep the robot at a fixed distance from the blob, this means that we want to keep the y-coordinate at the same value. We assume the variation of the y-coordinate linear with a variation in the distance. We can assume this if the variation of the distance is not too big. Also we can see in the figure above that the variation of the y-coordinate, with the exception of the measurement inaccuracies, is quite linear around the distance of 1m. The idea is to implement a P controller in both control loops. For the forward movement this means that both inputs are proportional to the error, being the difference between the measured ycoordinate and the reference in the y-coordinate. For the rotational movement we can do exactly the same. Only the inputs are not applied to both left and right, but only to one of them. One input will be different from zero and one input will be zero. In that way, the robot will turn. To combine the two control loops, we add the input values of both control loops. In that way, the robot will move forward or backward, based on the y-coordinate of the centre of the blob, and it will turn as a consequence of a difference in the speeds of the two wheels, based on the x-coordinate of the centre of the blob. The idea is given in the picture below. This is not the actual control script, but is rather an illustration of the control idea.
Important to mention here is that the reference for the x-coordinate of the centre of the blob is the middle of the picture, being 640/2 = 320. This is because we want the robot to follow the blob as a tail, in other words, the robot should look straight to the blob. This means that the blob should be in the centre of the picture, and since the origin of the picture coordinate frame are in the upper left corner, the reference for the x-coordinate is 320, and not 0. If one desires the robot not to look
33
straight to the blob, but to keep a certain angle between the orientation of the robot and the straight line with the blob, this reference in the x-coordinate can be set to another value. It’s obvious from the ‘idea script’ in the picture above that the rotational speed (‘speed_rotation’) is always positive, due to the ‘abs’ function. If the x-coordinate is bigger than the reference value, this means that the blob is in the right part of the picture and that the robot should turn to the right. This is done by adding the rotational speed value to the left wheel, which makes the robot turn right. If the blob is in the left part of the picture, it’s the other way around. 5.3.3. Control script The entire control script can be found in appendix 3. I will highlight in this section the ideas behind the different parts in the control script. The script is written with the C commands that are appropriate for the onboard processor (PicoC interpreter), so that it can be uploaded onto the robot and can run the control onboard. Control scripts were also made for Matlab and C#, but we will come back to that later. The first part of the script consists of defining all parameters used in the algorithm. Important to remark is that the PicoC interpreter can only work with integers, and not with floating point numbers. This is the reason why everything is declared as ‘int’. Also the colour and references are set. After that the control loop is started. The following code requests the information, being the area and x-coordinates and y-coordinates of the square fitted around the blob. The x-coordinate and y-coordinate of the centre of the blob are calculated:
Important to notice is that the loop waits till the information arrives. This means that we don’t set a certain sampling time, but that we do the control as fast as possible, as soon as the information is gathered. Next, the two P-controllers are applied and the inputs are added in order to get one value for each input.
34
In the last part of the control script above, the forward control is only applied when the difference between the setpoint in the y-coordinate and the measured y-coordinate of the centre of the blob is more than 8 pixels. This is done to avoid oscillations because of the inaccuracy of the measurements. This also means that we have an inaccuracy of about 4cm on the distance of 1m. The gains for the two P-controllers were tuned manually. There is one more aspect that caused trouble though. The controllers worked good for higher speeds, but not for lower speeds, or in other words, small errors on the controlled variables. This is due to the dynamics of the motors. As mentioned in a previous section, the motors have a threshold, under which the robot doesn’t move. The reaction was that the robot doesn’t do anything as long as we stay under this threshold, and that there is a certain jump from the zero speed to the minimum speed. To solve this issue, we added an offset to the inputs, which brings the desired input speeds that were originally beneath the threshold now above the threshold. This has as a consequence that we also have a reaction for small errors in the controlled variables.
35
Since we add a constant value to the speeds calculated by the two control loops, we need to lower the gains. This because otherwise the reaction is too strong and there is too much overshoot. The idea is schematically shown in the pictures below.
Figure 31: Dead-zone in motor characteristic
The last part of the script sends some variables that we used in the script to a remote PC. The program for receiving this information is described in the next section. 5.3.4. Experimental results The data the algorithm sends to the remote PC are the left and right inputs to the motors, the time and the x-coordinate and y-coordinate of the centre of the blob. In order to receive this, a TCP/IP connection has to be made with the robot and this program should wait for the data sent by the robot. We can implement this in Matlab. The only purpose of this program is to receive data. It doesn’t send any data to the robot and it waits infinitely till the data arrives. This means that the program will in no way interfere or delay the control of the robot itself. The receiving program can be found in appendix 4. With the function ‘sscanf(temp, ‘%d %d %d %d %d’)’, we can split the data, that was saved as a string, in the 5 parts, which actually contain the requested information. The only thing we send to the robot is in the beginning, ‘Q’, in order to start the onboard C program. We uploaded the C program onto the robot and activate it through the Matlab program, which also collects and plots the data afterwards. The most interesting part of a response is shown in the picture below.
36
Figure 32: P-control positioning experimental results
The different parts and characteristics of the control response are explained below: PART 1 – There is no change in the position of the object. The robot is looking towards the light, which corresponds with a setpoint for the x-coordinate of 340, and has the desired distance, which corresponds with a setpoint for the y-coordinate of 300. PART 2 – Now we move the object that needs to be tracked, being the lamp. We move it sideways, leaving an error in the x-coordinate or the angle between the robot and the object, and slightly backwards, leaving an error in the y-coordinate or the distance between the object and the robot. The response in this part is that both left and right get a positive input, which is proportional to the error on the y-coordinate. The two inputs are not the same though. This is the result of the error on the x-coordinate. To compensate for this error, the right wheel has a slightly lower speed than the left wheel, causing the robot to turn to the right. The response of this difference is not immediately visible in the x-coordinate in part 2. This is because the difference between the input in the left and right wheel is relatively small with respect to the input to go forward. The gain in the loop of going forward is bigger than the gain in the angle control. This causes the robot to control the distance, or y-coordinate, mainly first, and only afterwards the angle. PART 3 – The effect described in the previous part is completed here. In this part the y-coordinate, or distance, is stable around the setpoint for the y-coordinate. There is no input anymore to go forward. The only inputs remaining are the ones to control the angle, or x-coordinate. Now the difference is relatively significant and the robot turns quickly to its setpoint on the x-coordinate. The figure below
37
gives the effect of part 2 and 3 visually. If the lamp goes further from and to the side of the robot, the robot first moves forward in order to control the distance, and controls the angle only at the end.
Figure 33: 2D controller effect
PART 4 – This part is the consequence of the previous part and demonstrates again the drawback of decoupling. While in the previous part the robot is turning towards the light, which means that only one wheel is moving, the distance to the light changes again. Now the distance is too small. The controller will add to both inputs the same amount again, and the y-coordinate, or the distance, takes over the priority again. This causes the x-coordinate, or the angle, to overshoot a little bit to the other side. Here the angle is controlled quite quickly because the error is small. The small error can thereby be controlled by the relatively small difference between the speed inputs in left and right. PART 5 – In this part we get again the overshoot of the y-coordinate, or distance, as a consequence of the angle control. The overshoot is very small and controlled very fast. PART 6 – In the end the setpoint on both the x- and y-coordinate are reached and the robot stays in rest at the desired distance and desired angle. With this response we have clearly shown that both inputs are controlled pretty well. The effect of decoupling is visible, but not very significant. The control on the distance has priority above the control on the angle, because of a higher gain and it works on both wheels, while the angle control only works on 1 wheel. The control on the angle happens mainly afterwards and causes the distance to change a little bit again. The overshoots are very limited with respect to the initial errors.
5.4.Control script languages and communication During the development of the controller, we tried a lot of strategies in order to run the controller. First we tried to do the control on a remote PC and communicate with the robot over the Wi-Fi 38
connection. The reason for this is because it is convenient and user friendly. It’s also easier to save and plot responses than if you run the control on-board. We programmed the script first in Matlab, since this is the language that was best known to us. This didn’t seem to work very properly because of variable delay and sample times. We wanted to improve the communication by using a program that is more suited for real-time applications. We tried to implement the control in Simulink, but came to the conclusion that Simulink is too limited to do TCP/IP communication for our application. Next we tried to program it in C#. This worked, but the delay was still too big. Then we came to the conclusion that it was the Wi-Fi communication that is the problem, so we programmed the script onboard, which has a much better result. We will now shortly describe how we attempted to do the several communications and what the advantages and disadvantages of each are. 5.4.1. Matlab As mentioned in a previous section, we can make use of the Matlab Instrument control toolbox to communicate with the robot. The TCP/IP connection is made an object in the workspace it can be handled as a normal variable. We constructed some functions in order to read from the robot and drive the motors. The control is done on the PC. The Matlab control script is similar to the one described above and can be found in appendix 5. Matlab is the first way we tried, because we are familiar with it and it’s very convenient to store all kinds of variables in vectors during the control and thereby easy to plot. The drawback is that the sample time is long and variable, as shown in the picture below.
Figure 34: Matlab delay time
The control script requests the information of the x-coordinate and y-coordinate of the centre of the blob and waits for it, in order to process and determine the right inputs. As can be seen in the picture above, this waiting generally takes between 0.5s and 1s, which is rather long. The wait time, and thereby the sample time at which we can control the robot, is variable and sometimes shows peaks of several seconds. At this sample time, it’s impossible to control the robot, unless the movement of the object that needs to be tracked is very slow, but this is not the purpose and thereby the Matlab script cannot be used.
39
5.4.2. Simulink In first instance we thought that it was due to the slow working of Matlab that the delay time for receiving the information from the robot was so long. So we tried to apply the script in a program that is made for real-time applications and has thereby much faster processing on a PC. We first tried to implement the control algorithm in Simulink. Here we can make use of the same Instrument control toolbox as in Matlab, by means of the blocks shown in the picture below.
Figure 35: Simulink TCP/IP blocks
There is a serious problem with these blocks though. There can only be one “TCP/IP Send” and one “TCP/IP Receive” block in the model. Since we need to send and receive different signals (receive information about the blob, send motor drive command, receive motor drive command response, …) with a different structure, and the structure of the received signal must be set beforehand in the receive block, it’s impossible to implement our algorithm in the Simulink environment. 5.4.3. C# Since simulink was not an option to implement the algorithm, because of the TCP/IP blocks that are too limited, we tried the C# language. Some people advised us to use this language, because it’s very fast and it’s the best C language for networking, or communicating over a TCP/IP connection. Since this was a completely new language for us, we had to get ourselves familiar with the basics or it. After digging into the C# language protocols, syntax and TCP/IP communication methods, we managed to implement the algorithm completely. The C# script of the control algorithm can be found in appendix 6. The script provides a graphical user interface with buttons to start and stop the control and also boxes to set the motor values manually. Although it took quite some time to get to this point, it was not sufficient for our application. Again we determined the time that was necessary to go through one loop of the control algorithm, which consists mostly of waiting for the blob information of the robot and sending the motor commands. The delay time, or sample time with the control script run in C#, is shown in the picture below.
40
Figure 36: C# delay time
As we can see in this picture, the delay time is mostly around 200ms, but has quite a lot of peaks going to 600ms or even 1s. The sample time response is better with respect to the Matlab figure, but still not very useful because of the higher peaks. After doing this experiment we concluded that the delay time could not be coming from the software we were using, but rather from the connection over Wi-Fi itself. This attempt was not a complete waste of time though, because it learned us the important basics of working with C#, which is a very popular language. The knowledge will be convenient to have for the future. 5.4.4. C-interpreter onboard After several attempts to run the control algorithm on board of a remote PC, we had to conclude that the communication over Wi-Fi takes too much time in order to control the robot properly. The only choice left is then to control the robot on-board. The algorithm is written in C, according to the Cinterpreter guide (Website Surveyor corp.). The control algorithm written for the onboard processor was explained in the previous section. It now requires of course an extra program off board, on the remote PC, written in Matlab, in order to get some data and plot the responses and variables of interest. The sample time with this approach was determined and is shown in the picture below.
Figure 37: Onboard C-interpreter delay time
41
As we can see in the plot above the sample time we can handle is much smaller with this on-board control. In this case the time delay is because of processing the picture and lies around 17ms. With this sample time, it’s possible to apply a good control to the robot. We must remark that also the resolution of the picture has an influence on the sample time. This plot is made at the lowest resolution. The delay time with higher resolutions is higher, but still very low and stable with respect to the controls that need communication over Wi-Fi. This means that all control should be implemented onboard of the robot. Because of the limited capacities of the on-board processor, the amount and complexity of the control algorithms are very limited. This is the reason why the NEPSAC control could not be implemented practically.
5.5.Line tracking What we have been doing till now, is trying to track a light bulb in 2 coordinates, the distance and the angle. The reference distance was around 1m and the accuracy up to a few centimetres. In order to create a situation that leans more towards reality, we wanted to decrease the distance between the robot and the object to be tracked. The tracked object was replaced by a second SRV-1Q robot. This because in the end we want to have a real leader, which follows a predefined path and some follower robots that track the leader robot as good as possible. In order to be able to do this, some modifications must be made. The light bulb for example, that was used as the tracked object till now, is no option anymore. The fact that we replace this light bulb and that we decrease the distance between the robots have several consequences which will be described in this section. Furthermore we will focus on the straight line tracking. As mentioned in the previous section, there is a low coupling between the distance and angle controller loop. In order to get a good tracking control in the straight line first, we will focus on this. This way, the system is in principle simplified to a SISO system, as the two inputs to the left and right motors are theoretically the same and we only have to measure the distance. We will see later that, because of imperfections, this is not entirely the case. Because we work in one direction, we will also be able to study better the effects of the whole formation, by which I mean the distance from the 3rd car to the 1st for example. These effects are much more difficult to see when the robots are moving in a 2D-plane. 5.5.1. Practical set-up 2 As mentioned before, we would like to track a robot instead of the static light bulb. In order to do this we have to provide the tracked robot of a blob which the follower can detect. The light bulb we used is too large to mount on the back of the robot, and the robot itself has no obvious colour or shape that can be seen as a blob by the follower. The advantage of using a light bulb was that the colour doesn’t change if the blob is moved under different lighting conditions. Therefore, we first thought of mounting some led lights. Mounting this was not obvious though. On the other hand, the light bulb also has the disadvantage of light scattering and reflection on surrounding objects, like the 42
floor for example. This means that it doesn’t have a real contour and that measurements show some noise, by which I mean that the coordinates of the centre of the blob can change pretty drastically when the light moves. The effects are especially obvious for a small light, which we should need to be able to mount it on the back of a robot. So, unless some modifications are made to the surroundings, to avoid reflection for example, and to the robot in order to be able to mount such a light, a light is not the best choice anymore. Because of that we chose to create a blob out of coloured paper. The colour we chose is strong yellow, because it’s quite rare in the environment we work in. The YUV ranges this colour lies in under the lighting conditions we work in, are Y = [140,210], U = [80, 115], V = *120,150+. As mentioned, since we don’t use a light bulb anymore, the colour changes under different lighting conditions. That is why we now need a range of colours instead of one fixed combination of YUV values. This also requires that every time tests are done, the conditions must be the same. In our case that is with all window blinds closed and the artificial light turned on. The coloured blob is a rectangle of 1.5cm high, 7cm width and on a height of about 8cm. In order to mount it on the robot in front, we fixed it on a carton and glued the carton to the steel back of the robot. The carton is a good choice because it’s quite light and will go easy with the movement of the robot itself, without vibrating too much. A picture of the construction is given below. The pictures are taken from the robot camera. The second picture is a representation of how well the robot can detect the blob.
Figure 38: Blob detection at close distance
Before, we were keeping a big distance between the tracked object and the tracking robot. This required the resolution of the camera to be set rather high in order to see enough coloured pixels to form a blob and to see an obvious difference in pixels when there is a difference in the distance. Since we want to work on a closer and more realistic distance, we are able to use a lower resolution. The resolution has a big influence on the sample time we can handle to control the robot. In the control algorithm, we request the information onboard of the robot from the camera. The program waits for this information and then moves on. The image processing takes the longest time though. There are four resolutions available and they each require a certain time to process the image. The bigger the image, or resolution, the higher the processing time. The typical sample time that can be handled with the different resolutions are listed below:
43
1280x1024: 520ms 640x480: 200 ms 320x240: 90ms 160x120: 17ms With the construction of the blob we made here and the short distance we want to maintain, we are have very good measurements with the lowest resolution, and we can thereby use the lowest sample time, which already improves the control drastically. The sample time is not always exactly this value, but can vary a few ms sometimes. This is a consequence of the inaccurate hardware components on the motors. 5.5.2. P-controller The P-controller in the straight forward movement is actually the same as before. The total command that is applied to the motors in the end will not have the P-controller in the sideways, or control of the angle, direction. Since we are working with the lowest resolution, the gain and references will be different as before. For the second robot, which is tracking the first one, we can again determine the characteristic of the camera, but now with the lowest resolution and around the setpoint of 15cm. The way of working is exactly the same as described before for the first test of the
y-coordinate of th center of the blob (pixels)
camera. The result is given in the picture below. 43 41 39 37 35 33 31 29 27 25 5
10
15
20
Distance to the blob (cm) Figure 39: Camera characteristic at close distance
As can be seen from this characteristic, we can assume it to be linear around the point of 15cm is a very small area. 1 cm of distance difference corresponds to approximately 1 pixel difference. Important to mention is that the characteristic above is data from one test. As mentioned before, the camera has an accuracy of about 1 pixel, which means that if different tests in the same condition are done, the measurement of the y-coordinate of the centre of the blob might differ 1 pixels in the different measurements. This means also for the control that we are only able to control with an accuracy of 2cm. The robot we are working with is supposed to be exactly the same as the first one. Although, as mentioned before, the performance of the hardware components is not exactly the same, so we 44
measured the motor characteristics again. By motor characteristic I mean the relationship between the input [0, 100], and the real steady-state speed (m/s). Also this characteristic is measured in the same way as before, and shown in the picture below. 0,6 Speed (m/s)
0,5 0,4 0,3 0,2 0,1 0 0
10
20
30
40
Input Figure 40: Motor characteristic
The input offset that we use in order to bridge the dead-zone for this robot is equal to 21. We will discuss the value of the gain of the P-controller later, so we leave this variable for the moment. There is no control on the angle of the tracking robot with respect to the direction the leader robot moves. This requires that both robots move in a perfect straight line when both inputs to the left and right motors are the same. While testing we noticed this is absolutely not the case. All robots suffer from a combination of hardware inaccuracy and disturbances. The disturbances for example come from the grooves in the floor, which are not perfectly smooth. So, some kind of control on the angle is necessary, or the robots will lose each other on long paths. Therefore, we add to one of the two inputs, left or right, a value of 15 when the x-coordinate of the centre of the blob more than 10 pixels away from the centre of the picture. This is done by adding the following code to the algorithm:
With this strategy, we introduce a short but pulse in one of the two inputs, which brings the robot back in line with the leader. Because of the reasonably high value of the added input, the pulse is high and short, and the robot is brought back in the line with the leader in a very fast way. This has as 45
an advantage that it doesn’t influence the distance control, or straight line movement, too much or too long. The complete P-controller implementation can be found in appendix 7. P-controller results We upload the P-controller script into the memory of the second robot. The first robot, called “robot 1” will serve as leader, for the second robot, called ”robot 2”, to track. To the first robot we apply a constant input value in order to let it drive with a constant speed. The second robot will track the first one by using the P-controller implementation. The value for the gain of the P-controller and the input to robot 1 will determine the reaction of the tracking algorithm. The input to the first robot is chosen constant at 30 first, and then changes with a step to 40. The choice for these rather high values will be explained later on. We will first show the results below and discuss them afterwards. The results for the distance between the leader robot and the follower are shown in ‘pixels’. I chose to keep it in this unit, since both the measurement and control are done in pixels. Another reason is that the relation between the ‘pixel’ unit and ‘meters’ is not entirely linear, as can be seen from the camera characteristic. So if we convert the distance to meters with a linear characteristic, this could give a wrong image of the reaction of the robot to the distance. As a consequence, we leave the units in ‘pixels’. To have a rough idea though of the distance in ‘meters’, in the camera characteristic we can see that 1 pixel corresponds more or less to 1cm. Because of the same reasons of non-linearity and being the actual measured and control unit, the left and right inputs are kept to the values in the interval of [-100, 100], and not converted to m/s.
Figure 41: P-controller test with GAIN = 0.6
46
Figure 42: P-controller test with GAIN = 0.8
Figure 43: P-controller test with GAIN = 1
Conclusion 1: Constant speed - The first conclusion we can draw from these responses is that the follower always goes to an average constant speed or input. When the input to the first robot is 30, the constant input (average) to which the follower converges, is 24. When the input to the first robot is 40, the steady-state input of the follower is 30. The goal is of course that the follower robot tracks the leader robot with the same speed at a constant distance, and this after some transients. We can 47
see that the follower converges to constant speeds, but the input value is different. This is because the characteristics of the motors of the different robots are different. The measured characteristics for the three robots we have, can be found in appendix 8. The characteristics are measured in the same way for all the robots, as is explained in the first section of the manuscript for the first robot. From these characteristics we can determine the steady-state speed of the first robot for an input of 30 and 40 in m/s. The true speed for the first robot for an input of 30 is about 0.38m/s, and the true speed for an input of 40 is about 0.438 m/s. For the second robot then, the true speed for an input of 24 is 0.382 m/s, and for an input of 30 it is 0.439 m/s. As expected the follower robot tracks the leader robot with a constant speed that in reality (m/s) is the same as the speed of the leader. The average speed we are talking about here, doesn’t depend on the gain of the P-controller. The higher the gain, the faster the speed rises, but the more oscillations occur around the steady-state speed. This last aspect can give some problems because when the oscillations are too big, the speed can come under the minimum input speed, being 21 for robot 2, and the robot might stop. To develop a non-zero speed again, it takes the robot much longer than to change speed while driving. This might cause the robot to oscillate even more. This effect will be explained later. Conclusion 2: Steady-state error – The second conclusion we can draw, is that if the first robot is in standstill, or comes to a standstill after moving, the steady-state error, being the difference to the setpoint on the y-coordinate of the centre of the blob (35), is zero. When the first robot is moving with a constant speed (input 30 or 40), one can obviously see that the average of the distance, or ycoordinate, is not around the setpoint, but that there exists a steady-state error. This can be explained by looking at the control loop. The motors of the robot are DC-motors and typically have a first order transfer function between the voltage applied to the motors and the speed. The input we give is actually proportional to the input voltage, as explained before. Furthermore, the measurement we do is the distance, by means of the y-coordinate of the centre of the detected blob. This means that there is an integrator in the direct branch of the control loop. The last two elements in this direct branch, are a time delay and the gain of the P-controller. The picture below shows the idea of the control loop, with perfect motors, by which I mean there is no influence of the dead-zone or saturation. The values that are given here will be explained later.
Figure 44: P-controller block diagram control loop
When the leader robot moves over a certain distance, this means that a step is applied to the control loop. Since there is already one integrator in the system, the steady-state error will be zero. When the leader robot moves at a constant speed, this corresponds to applying a ramp input. To achieve a steady-state error equal to zero on a ramp input, two integrators in the direct branch are required. 48
Since we only have one integrator, there will be a steady-state error in the distance from the follower robot to the leader robot. A P-controller is actually a linear relationship between the error on the distance and the input to the motors, as can also be seen from the picture below.
Figure 45: Explanation steady-state error
When the robot requires a constant speed, or input to the motors, in the case of a P-controller this means that it requires a constant error. From this we can also see obviously the influence of the gain of the P-controller. The higher the gain, the higher the slope of the linear curve. This means that for the same input speed, the error decreases with a higher gain of the P-controller. We can also see this in the results. The steady-state errors on the distance between the leader and the follower robot, in terms of pixels, are read from the responses above and summarized in the table below. Gain
center_y
error
center_y
error
(pixels)
(pixels)
(pixels)
(pixels)
Input robot 1 = 30
Input robot 1 = 40
0,6
41
6
48
13
0,8
40
5
45
10
1
38
3
44
9
Figure 46: TABLE 1: Steady-state errors P-controller
The error decreases when the gain increases or when the speed, or the input, of the first robot is lower. Conclusion 3: Oscillations – As mentioned before, we can see that oscillations in the input, or speed, and as a consequence, also in the distance between the leader and follower robot increase if the gain of the P-controller is increased. This is quite normal, since a higher gain results in a faster reaction, and due to the time constant and inertia of the system, in overshoot with respect to the desired speed (or steady-state error). The higher the oscillations, the less constant the distance between the leader and follower is kept. This is the price you pay for a faster reaction. It becomes problematic though when the oscillations are so big that the speed actually has to go under the minimum speed of the robot. As we measured the motor characteristics of the robots, we also noticed that there is a minimum input to get the robot moving. The characteristics of robot 1 and 2 are given in the pictures below.
49
0,6
0,4
0,5 Speed (m/s)
Speed (m/s)
0,5
0,3 0,2
0,4 0,3 0,2
0,1
0,1
0
0 0
20
40
60
0
10
20
Input
Input
Figure 47: Motor characteristic robot 1
Figure 48: Motor characteristic robot 2
30
40
With the minimum input to the motors also corresponds a real minimum speed. For the second robot, the minimum input is 21, which corresponds to a real speed of about 0.33m/s. When the oscillations around the steady-state error on the distance are so big that the distance becomes the actual desired distance, or in other words that it touches the zero error, the speed drops to zero too, because of the P-controller. The follower is then in standstill, while the leader is still moving with a constant speed forward. Because of this, the error will become bigger again and the follower robot will move forward again. This effect causes the follower robot to track the leader in a chocking way, as is the case with the gain of 1 for the P-controller of the follower robot at an input of 30 for the leader robot. This effect is stressed in the pictures below.
Figure 49: Dead zone influence
50
The fact that the speed drops to zero already when the y-coordinate of the centre of the blob is equal to 34 or 36, and not exactly to the setpoint of 35, is a consequence of the following line in the control script:
This part is added to the script to reduce noise influence or measurement inaccuracies. The ycoordinate of the centre of the blob can differ 1 pixel when two measurements are done in exactly the same situation. If this part is not added to the script, due to the measurement noise small oscillations can occur when the leader is actually standing still. The effect that the follower robot tracks the leader robot in a chocking way can also occur when the gain is low, but when the speed of the leader is too low. As explained before, the follower robot converges to the same real speed as the leader robot. When this speed of the leader robot is lower than the minimum speed of the follower robot, it’s impossible for the follower robot to converge to this speed. The only choice the robot has then is to move at a higher speed, and when it’s too close or when the error on the desired distance is zero, to switch to a zero speed. The same chocking way of tracking is observed. An example is shown in the graphs below, with a gain of 40 for the Pcontroller and an input of 23 for the leader robot.
51
Figure 50: Effect of a too low speed of the leader
It can obviously be seen in the graphs that the robot accelerates till a zero error, at which the speed drops to zero and then waits till the error is bigger again. The input to the first robot of 23, which corresponds to a real steady-state speed of 0.278 m/s. The minimum speed of the second robot, which corresponds to an input of 21, is 0.33 m/s. The speed of the leader is thereby lower than the minimum speed of the follower, and this makes it impossible for the second robot to converge to this speed. Once the input of the leader is 30, which corresponds to a real speed of 0.387 m/s, the speed of the leader is higher than the minimum speed of the follower robot. As a consequence the follower robot can converge to the same speed and stay at a steady-state error in the distance, which is a normal response for the P-controller, as can also be seen in the second part of the graphs above. 5.5.3. Model identification Identification In this part we try to get an idea of the main characteristics of the robot. As mentioned before, we know that the DC-motors have a first order model transfer function, and since we have a feedback on the position and control with the motor input, there also is an integrator in the loop. The first order motor model consists of a time constant and a gain. Next to the dynamics of the motors and the integrator, there is also a time delay between the instant of measuring and acting on the measurement. The time delay, motor time constant and gain are the parameters we want to identify.
52
In order to do that, we have to take into account the dead-zone of the motors. Since we would like to control the robot in motion, it’s important that we measure the characteristics in the environment of a good working point and not from standstill. The figure below clarifies the idea of the dead zone. Notice that this picture is only a graphical representation of the fact that at low inputs, the speed is zero. In reality the lowest speed is not zero and there is also saturation in the higher inputs. These effects are not represented on the figure.
Figure 51: Identification strategy
On the figure, the ‘Input’ means the input to the motors, so a value which is proportional to the voltage applied to the motors in the interval [-100,100]. The output is the actual steady-state speed of the robot at that specific input value. As mentioned before, there is saturation at higher inputs and the start speed is not zero, but it also has a certain offset. This characteristic will determine the gain of the first-order transfer function, but we will come back to that later. In order to determine the delay time and time constant, it’s obvious that we have to work round a working point at which the robot is moving, as the reaction from standstill to a certain speed and the reaction from a certain speed to another speed will be different. To determine these values, we apply a step input, and look at the output response of the robot. The goal is thus to apply a step input to the follower robot which is already moving at a certain speed. As known, the only feedback, or measurement, we have is the distance, to another robot. This means that another robot is needed to drive at the same speed and in front of the robot we want to measure the characteristics of. This robot with the blob, or coloured paper, fixed on the back, provides the feedback, or measurement. At a certain moment we will then apply a step input to the second robot, which will cause the distance to decrease. The measurement from this distance, or the y-coordinate of the centre of the blob, will then be our step response. The figure below expresses the idea.
53
Figure 52: Identification strategy 2
We have to mention again that the robots are not very accurate, in the way that when you put the same input on both sides, that they don’t move exactly straight forward. In order to get a step response that is as ‘clean’ as possible, we guide the first robot by hand in a straight line and for the second robot we apply the same technique as before. When the x-coordinate of the centre of the blob differs more than 10 pixels from the centre of the picture, we apply a pulse of 15 in the input of one of the two sides. This brings the robot back in the same line with the first robot. However, these impulses also give small disturbances to the distance measurement. In order to avoid these pulses as much as possible, we try to get the robot on a straight line in first instance by putting a difference in input of the left and right wheels. At last we also have to notice that, as mentioned before, the two robots don’t have the same input versus real speed characteristic and that the minimum speeds are also different. The speed is therefore chosen to be higher than the least of the two minima in speed of the two robots. The input to the two different robots are of course also different then in order to have the same real speed (m/s). The result of the step response is given in the graph below.
Figure 53: Step response for identification
Since we put a step in the input, or in other words, in the setpoint to the speed, and we measure the distance to the robot in front, this means that the step response is at first a constant distance and 54
when the step is applied, the distance should, after a period of transient, exist of a ramp with constant slope. This is also what we see in the result that is given in the graph above. In the first part we have a more or less constant distance, or in other words the y-coordinate of the centre of the blob is constant. The distance changes a little bit under the influence of measurement noise and disturbances, from for example the surface roughness and lighting conditions under which the robot moves. When the step in both inputs is applied, the distance doesn’t change immediately. The distance is held constant for a while longer. About 600ms after applying the step, the distance starts to decrease. This means that there is a time delay of about 600ms between the time of sending the command and the time the motors actually react. This time delay can also be seen in the effect of applying a pulse. In the circle that is indicated by number 2 we can see that the distance is increasing a little bit again. This is obviously the effect of not driving straight forward, or a sideways movement. This can be stated since, as a reaction to it, there immediately occurs a pulse in the speed of the right input. The effect of this pulse can be seen about 600ms later, indicated by the circle with number 4. The distance decreases in an accelerated way, as a consequence of the impulse in the right motors, which brings the robot back in line with the first robot. It can be said with great certainty that this accelerated decreasing of the distance is the consequence of the impulse, since the duration of both the impulse and the acceleration are the same. Considering the explanation about the sideways movement, the effects indicated by the circles with number 2 and 4 cancel each other out. We draw a line that approximates the last part, with constant slope, and the average of the effects of the sideways movement. By doing that we can estimate that the constant slope, as a consequence of the increased speed input of the second robot, starts at a point that is about 200ms later than when the step input starts to have effect. This is shown in the circle that is indicated by number 1. The time constant is thus 200ms. In order to complete the transfer function of the motors, we also need to determine the gain. This can easily be done by looking at the motors characteristic of the real steady-state speed versus the input to the motors. The characteristics for the three robots can be found in appendix 8. As mentioned before, the characteristics show a saturation among the higher inputs, which corresponds to a maximum speed of about 0.55m/s. In order to approximate this function, we only take into account the lower inputs, where we can assume the characteristic to be linear. For the second robot, this part of the characteristic is shown in the graph below.
55
0,45 0,4
y = 0,017x - 0,037
Speed (m/s)
0,35 0,3 0,25 0,2 0,15 0,1 0,05 0 19
20
21
22
23
24
25
Input Figure 54: Motor characteristic robot 2
The measurement points are approximated by a linear function. The slope of this function determines the gain of the 1st order transfer function, because it converts the values of the input to the output values, or speed, when in steady-state. After this identification, we come to the following model for the robot: 𝐻 𝑠 =
0.017 . 𝑒 −0.6𝑠 1 + 0.2𝑠
In order to have a realistic model though, we should include the non-linearity of the dead-zone somehow. The algorithm that was described before, calculates the necessary input by the means of the P-controller, and afterwards a certain offset is added in order to bridge the dead-zone. The offset that is added in the algorithm is set to 21. The linear function by which we approximated the characteristic before crosses the x-axis at 2.18 (= 0.037/0.017). The figure below illustrates how we can model the fact that we add the offset in the algorithm.
Figure 55: Simulating dead-zone
56
To model this effect we have to add an input of 18.82 to the value calculated by the controller. To make this complete we also need to insert a block that makes sure that an input of less than 21 to the system is actually the same as an input of 0, because of the threshold. To accomplish this, we add a block in front of the process with the following code:
An important remark is that the input of the system is an input to the motors, which comes from the controller. This controller computes its output value based on the error. In the algorithm, the measurement, and also the control in done in the units of ‘pixels’. The output of the modelled motors though, is in the units of ‘m/s’. This means that before the error is given to the controller, it should be modified from ‘meters’ to ‘pixels’. We approximate this conversion of units by a factor of 100, which means that 1m corresponds to 100cm. This is an approximation in the neighbourhood of 15cm and is only valid in this neighbourhood, because of the camera that has an offset and a saturation on its own, as was discussed before. To make the model complete, also the maximum speed should be taken into account. This is done by adding a saturation block with boundaries of -0.55m/s. and 0.55m/s. To summarize, we show the complete control loop in a block diagram in Simulink:
Figure 56: Simulink model robot
As mentioned before, the characteristics of the different robots, are similar but in values quite different. The time constant, delay time and the sample time are more or less the same for every robot, but the gain and offset are different. The block diagram shown above is thereby only valid for the second robot (IP: 169.254.0.11).
57
The values of the gain and offset, can be determined for the first (IP: 169.254.10) and 3rd robot (IP: 169.254.0.12) in exactly the same way as done for the 2nd robot (IP: 169.254.0.11). The motor characteristics and approximated linear characteristics for these robots can be found in appendix 8. Important to remember is that the model is a rough approximation of the real system, due to nonlinear affects as the dead-zone and the saturation of both the motors and the camera. We will point out to the inaccuracies because of these effects later on. Model validation We will now validate the identified model, by doing a simulation with a P-controller and comparing it to the experimentally derived results. In the experiment, we applied a step input of 30 and later of 40 to the first robot, the leader. The gain was set to 0.8 and the steady-state error was measured to be around 5 pixels when the first robot had an input of 30, and around 10 when the input to the first robot is 40. In order to simulate, the reference for the second robot is actually the position of the first robot with 15cm difference. As mentioned before, the first robot has the same dynamics of the second and third robot. The delay time is around 600ms and the time constant 200ms. Similarly as we did before for the second robot, the characteristic for the first robot can be approximated by a linear characteristic. The linearization can be found in appendix 8. The gain of the linear characteristic is 0.015 and it crosses the x-axis at an input of 5.57. This means that as an input, instead of 30 and 40, the input should be set to 24.33 (= 30-5.87) and 34.33 (= 40-5.57). The Simulink block diagram below shows the complete control loop for the leader and follower, robot 1 and robot 2.
Figure 57: Simulink model robot 1 & 2
In the simulation results are shown: The input to robot 1 (24.33 and 34.33), the actual speed of robot 1 (in m/s), the input to robot 2, the real speed of robot 2 (in m/s), the error on the distance between the two robots (in m and in pixels) and the position of the two robots. The simulation results are shown in the picture below.
58
Figure 58: Simulation identified model 1
In the first part of the simulation, where the input to the first robot is 24.33, or 30 in reality, the response of the simulation is the same as measured in the experiment. The speed of the second robot converges to the speed of the first one, being 0.38m/s, and the steady-state error is around 5 pixels. This is exactly what was measured experimentally (table 1), so for this working point the model holds, at least for the steady-state part. Regarding the dynamics or transient, the experiment is much more noisy and oscillatory due to disturbances, which cannot be predicted by the model. However, for the second part we can see a serious difference between the simulations and the experimental results. This is due to the non-linearity in the motor characteristic. From the motor characteristic of the first robot, we know that an input of 40 corresponds to a real speed of about 0.438m/s. In the simulation we see that the first robot is assumed to drive at a much higher speed. This is because of the saturation of the motor characteristic of the first robot in reality. In the model a linear characteristic is assumed, and the saturation is not brought into account. In order to have a realistic simulation for this input, we should linearize the motor characteristic around the input that is equal to 40. The figures below show the difference in linearization in the different working points for the first robot. 0,5
0,44 Speed (m/s)
Speed (m/s)
y = 0,002x + 0,355
y = 0,015x - 0,085
0,4 0,3 0,2 0,1 0
0,435 0,43 0,425 0,42
0
10
20 Input
Figure 59: Motor characteristic robot 1
30
40
0
20
40 Input
60
59
Around the input of 40, the linear characteristic has a slope of 0.002 and crosses the x-axis at an input of -177.5 (= -0.355/0.002). This means that the gain of the first order transfer function of the motors should be 0.002 and that due to the dead-zone, the inputs to the first robot should be 207.5 (= 177.5+30) and 217.5 (= 177.5+40), instead of 24.33 and 34.33. The same problem occurs with the second robot of course, since the real speed of the second robot will converge to the same speed as the leader robot. The real speed of the first robot, corresponding to an input of 40 is 0.438 m/s, which corresponds to an input of 30 for the second robot. This means that the second robot should be linearized around this working point. The figures below show the difference in linearization in the different working points for the first robot. 0,5
0,49
Speed (m/s)
0,4
y = 0,009x + 0,167
0,48
y = 0,017x - 0,037
0,47
0,3
0,46
0,2
0,45 0,44
0,1
0,43 0
0,42 18
20
22
24
26
0,41
Input
0
10
20
30
40
Figure 60: Motor characteristic robot 2
The linear characteristic of the second robot has now a slope of 0.009 instead of 0.017 and it crosses the x-axis at an input of-18.56. This means that the gain of the first order transfer function of the motors should be 0.009 and that due to the dead-zone, the offset should be 39.56 (= 18.56+21). Simulation with these parameters gives the following result:
60
Figure 61: Simulation identified model 2
In the second part of the simulation, the speed of the first robot is now indeed around 0.438m/s, which it also is in reality. The error in the second part of the simulation is around 10 pixels, as was also determined by the experiments. The first part of the simulation gives wrong results now, as a consequence of the same problem as was described before for the second part. Out of this we can conclude that the model is valid, but only in a very small area of the working point at which the motor characteristic is linearized and for steady-state phenomena. For prediction with more complicated controllers or algorithms and dynamic investigations the model is too inaccurate because of too many non-linearities that are being neglected by linearizing the model. 5.5.4. PI-controller In the previous section, we implemented a P-controller on the follower robot in order track the leader. The result was good for appropriate speeds and within certain boundaries for the gain of the P-controller in order to avoid the dead-zone non-linearity. However, when the leader robot moves with a constant speed, the input to the control loop is a ramp. Since there is only one integrator in the loop, there exists a steady-state error, of which the value is dependent on the gain of the Pcontroller and the speed of the leader robot. In this section I will describe how we attempted to remove the steady-state error as much as possible. This is done by adding a second integrator to the loop. Theoretically, the steady-state error should then be equal to zero. In first instance, we attempted to achieve this by using an auto-tuner to implement a PID. Secondly, since we have an idea of the dynamics (model), we tried to implement a PI by means of the Ziegler-Nichols algorithm. At last we tuned the PI manually.
61
Auto-tuning algorithm As described before, we are working with a process that can theoretically be represented by a pure second order transfer function in combination with a time delay. In order to implement a PID, tuned by the means of an auto-tuner, we make use of the Kaiser-Raika auto-tuning method. This method requires a relay test with a certain time delay applied to the system. In first instance, the time delay is chosen to be equal to zero. Important to notice, is that the relay test should be done around a correctly chosen working point, in order to have a good approximation of the PID parameters. The figure below shows this idea.
Figure 62: Working point for auto-tuning
The working point of the input for this test is chosen around 25, with a relay step of 4. The same practical set-up occurs as with the step response we measured before. The robot of which the measurement must be taken, requires a leader robot to drive in front with the same average speed, or in other words, the speed around which the follower robot must oscillate. The robot will oscillate between an input of 21 and 29. The implementation can easily be done by replacing the code of the P-controller, that was developed before, by the following code:
There is again a speed pulse compensation implemented in the algorithm, in order to stay in the same line with the leader robot. The result of this relay test is shown in the graphs below. The upper graph represents the inputs to the motors and the second graph represents the measurement of the y-coordinate of the centre of the blob, which is supposed to be proportional to the distance.
62
Figure 63: Relay test result
From these results, we can see that this test is not really useful. Both the frequency and the amplitude of the response are variable and not very close to a certain average value. We did the test several times and long enough, to conclude that we couldn’t determine good parameters for the PID in this way. Ziegler-Nichols Since we identified a model in the previous section, that was based on the response of a step input and the motors can be represented by a first order transfer function, we attempted to apply the Ziegler-Nichols tuning method. This method consists of tuning a PI- or PID-controller for a linear first order system with time delay based on the step response. The picture below shows the idea and the formulas for the parameters.
Figure 64: Ziegler-Nichols tuning
As explained in the previous section, we apply a step to the system. The response which is measured is the distance to the robot in front, which means that we don’t measure a response as in the picture above, but actually the integral of this response. The different values that are needed for the tuning algorithm can be calculated. The delay time, or the parameter noted here as ‘L’, was 0.6s. The parameter ‘a’ we can estimate by calculating the slope of the step response in the speed, which has the topology as in the picture above. We estimated the time constant at 0.2s for a step difference of 3 in the input. This results in a slope of 15 (= 3/0.2) for the step response in the speed. The time delay 63
being 0.6s, the parameter ‘a’ can be estimated at 9 (= 0.6*15). Inserting the values into the formulas for the PI-controller give us the following parameters: P = 0.105 I = 1.8 If we implemented this into the control algorithm, which will be described in the next section, the result was not good. The robot became unstable, or at least way too reactive so that it loses the blob in front him rather quickly. This can be explained by the multiple non-linearities of the system and the fact that the Ziegler-Nichols method was designed to work only with linear systems. The approximations and neglecting the non-linearities were apparently too big for the Ziegler-Nichols method to work properly. Modifications of the control algorithm script The script that is uploaded onto the onboard memory of the robot is similar to the one of the Pcontroller. The script can be found in appendix 9, and the differences will be discussed here. The P-controller only needed the current error, being the difference between the setpoint and the current measurement. In the implementation of a PI- or PID-controller, also the previous and the second previous error are needed at the same time instant. A PID is characterised by a gain Kp, an integrator time constant Ti and a differentiator time constant Tp. The output of the controller, or the input to the process is calculated by the following formula: 𝑢 𝑡 = 𝐾𝑝 [𝑒 𝑡 +
1 𝑇𝑖
𝑡 0
𝑒 𝜏 𝑑𝜏 + 𝑇𝑑
𝑑𝑒 ] 𝑑𝑡
From this formula, a transfer function can be derived: 𝐼 𝐻 𝑠 = 𝑃 + + 𝐷. 𝑠 𝑠 The parameters P, I and D are related to Kp, Ti and Tp in the following way: 𝑃 = 𝐾𝑝 𝐾 𝐼= 𝑝 𝑇 𝑖 𝐷 = 𝐾𝑝 . 𝑇𝑑 In order to implement this in the algorithm, the PID must be made discrete and is preferably written in an easy form to implement this in the code. The conversion can be found in [24]. The discrete implementation is called the “incremental PID” and has the following form: 𝑢 𝑘 = 𝑢 𝑘 − 1 + 𝑐0 . 𝑒 𝑘 + 𝑐1 . 𝑒 𝑘 − 1 + 𝑐2 . 𝑒 𝑘 − 2 The parameters in this formula are given by: 64
𝑇𝑠 𝑇𝑑 + ] 𝑇𝑖 𝑇𝑠 2𝑇𝑑 𝑐1 = −𝐾𝑝 [1 + ] 𝑇𝑠 𝑇𝑑 𝑐2 = 𝐾𝑝 𝑇𝑠
𝑐0 = 𝐾𝑝 [1 +
It are the values of these three parameters that are inserted into the program that we upload onto the robot. The errors of the three time instants are saved in 3 variables Et, Et1 and Et2 and they are shifted every new sample (Et Et1, Et1 Et2). Important to remark is that the onboard C-interpreter can only work with integer numbers and not with floating point numbers. Since the values of c0, c1 and c2 are not too big and a rounding of them results in a serious error, we try to implement it in a way that the rounding is only done at the end of the calculations. To do this, we multiply the values of c0, c1 and c2 with 105, and divide by 105 again when the calculation of u(k) is completed:
Next, also the part where we put the speed to 0 when the error is less than 1 pixel, is erased. This was necessary because with the second integrator in the loop, the error will go to zero and still has to move at the same speed as the leader robot. If the ‘zero-zone’ is still there, the speed will fall to zero instead of holding on the constant speed of the leader. This has as a consequence though that in standstill the robot is very sensitive to measurement noise and might be oscillating a bit around its equilibrium position. The rest of the script is the same as with the P-controller. The offset, the pulse compensation for compensating left and right motions, etc. are still present and work in the same way as before. Manual tuning Since the auto-tuning and the Ziegler-Nichols method don’t give results that are useful and the model is only valid in a small area of certain working points and very sensitive to the non-linearities of the system, we tuned a PI-controller manually. In first instance we set the I- and D-factor to 0. The P-term of the equation was increased till an unstable situation was reached. Then the value of this P-term was decreased again for about 40%. After this we started to increase the I-factor. After some trial and error with these two parameters, the following values were obtained: P = 2.6 65
I = 0.7647 This means that the values for c0 and c1, according to the formulas described earlier, become the following: C0 = 2.61300 C1 = -2.60000 The result of the an experiment with these values in the algorithm is shown in the graphs below. The input to the first robot is 32 on the left and 33 on the right. The difference in input to the left and right motors of the leader robot is there to cover a little bit for the sideways movement. The first robot tracks a straight line here. The line tracking for the first robot is implemented in the same way as we control the angle for the followers. That is why we also see some peaks in the input of the first robot. The line is red tape which glued to the floor. When the leader robot tends to move sideways, the red line is detected to move away from the centre of the picture. An impulse is then given to one of the motors in order to keep the leader on the line. The script for the leader can be found in appendix 13. Because of the same reason as mentioned with the P-controller, we keep the units of the plots in the values they are actually controlled in, being the input to the motors as a value within the interval of [100, 100+, and the distance, or error, in ‘pixels’.
Figure 65: PI-controller experimental result
Conclusion 1 – Constant speed: In the graphs we can obviously observe that the speed of the second robot is around a steady-state value. The value of this input is 29, which corresponds to a real speed 66
of the second robot of 0.424. This speed is the same as the real speed of the leader robot, which corresponds here to an input value of 33 to the motors. This means that the follower robot tracks the leader robot with the same average speed. This is the same conclusion as with the P-controller. Conclusion 2 – error: The difference between the P-controller and the PI-controller now, is in the error, being the difference between the setpoint and the current value of the y-coordinate of the centre of the blob. One can see that the error is brought back to 0, or in other words, the ycoordinate of the centre of the blob is controlled to the setpoint. The reason that measurements go up a little bit, is because of disturbances. The disturbances come from aspects like the roughness of the floor, the lighting, variations in the speed of the leader robot,… These disturbances are the same as with the P-controller. The way the controller reacts to the disturbances is quite the same. With the P-controller the position is brought back to the steady-state error that corresponds to the desired speed, with the PI-controller the error is attempted to be brought back to zero. Because of these peaks due to disturbances, the distance between the two robots shows averaged a certain steadystate error. Even though the steady-state error is only averaged and quite small, an attempt to remove this steady-state error is to increase the value of I so that the controller can react much faster to disturbances. If the I-factor is increased to a value of 0.8 while the value of P is held constant, we get the following result:
Figure 66: PI-controller - higher parameter values
The reaction is indeed much stronger, but then we get the non-linear effect again. The second robot reacts with a much higher speed, and as a consequence also the overshoot is much bigger. This results in a need for speeds that are lower than the minimum forward speed of the robot. This 67
means that the input to the motors will receive a negative value now. The switching between the two speed directions is too slow though to have a good effect on the response. While the second robot reverses its speed, the first keeps on driving forward and the error is quickly switched to a positive value again. As a consequence of the slow back-reversing (negative positive) of the input to the motors of the second robot, the distance between the two robots becomes quite large again. This results in a ‘shocking’, or oscillating movement of the second robot and the steady-state error is not really decreased. An option to avoid this oscillatory effect, is to increase the speed drastically. However, when the speed of the first robot is increased very much, the impulses to the motors of the second robot are not as effective anymore, because of the saturation in the motor characteristics. At high speeds the follower tends to go out of the moving line of the first robot and loses the blob. There also exists a higher danger for collisions when the leader stops due to the speed. Another attempt was to introduce a differential action, or in other words, set the value for D different from 0. All attempts led to an unstable response though. This is also probably because of the noisy response. For this we implemented a moving average filter (over 3 samples). This makes the response a lot more fluent, but not as accurate as in reality, which resulted in a control action that was way too weak and had even more delay. A last attempt to decrease the average steady-state error existed of increasing the I-factor even more. This causes a response with serious overshoot. Due to the big overshoot the robot’s motors have the time to switch from positive to negative values and the other way around. This results in an averaged error which is actually equal to zero, but also with high oscillations around it. It’s a solution that is not very nice, but it works if it’s really important for the average error to be 0, even though the peak errors are much bigger now. The problem is that this is possible to do for one robot tracking a robot that is moving at a constant speed. It’s obvious that if this robot with high oscillations needs to be tracked by a third robot, which has more or less the same characteristics, that at this small distance from each other, this results in a collision of the 2 follower robots. In the investigation of the P-controller we applied two different speeds for the leader, in order to see how the controller performs in another working point. This will also be investigated in this section with the PI-controller which is described before. The higher input for the leader we apply is 38. The experimental result is given in the graphs below.
68
Figure 67: PI-controller result at higher speed
The first thing we have to mention from these graphs, is that the higher the speed, the more difficult the robots can stay in a straight line. This can be seen from the difference in left and right input to the motors. The leader for example, is tracking a line and the right input is almost always higher than the left input. Also for the follower, robot 2, the difference in the left and right inputs is almost constant. This means that the hardware inaccuracies of the robots become much more explicit at higher speeds. Thanks to the primitive control we implemented, of adding extra input to the left or right motor in order to stay on a straight line, the robots are actually driving in a straight line behind each other. The actual speed will due to this be a little bit higher than was intended with the inputs we gave to the leader robot. In the distance measurement between the follower and the leader, we can see that there exists an averaged steady-state error now of about 5 pixels. The controller attempts to get the distance to its setpoint, but is not as effective anymore as at lower speeds and is still undergoing the disturbances from the environment. This is quite logical, because when the follower gets a disturbance, the leader increases the distance between the two robots much faster because of the higher speed. The reaction of the follower is still the same as before, but should be reacting faster for a good response. This together with the fact that we are now controlling with the same linear controller in another part of the non-linear motor characteristic results in a response that is not optimal anymore. Important to notice is that the average steady-state error is much smaller than with a P-controller. With a gain of 0.8 for the P-controller we had an error around these speeds of about 10 pixels. With the PI-controller the error is reduced already with 50%, which is already a very good improvement of the control. At lower speeds we could not increase the parameters of the PI-controller anymore because of the dead-zone that should be avoided. At higher speeds the working point is further away 69
from the dead-zone and we can increase the parameters in order to have a faster reaction and a better response. The parameters of the PI-controller are tuned to the following values: P = 3.2 I = 0.8 The result with these controller parameters is given in the graphs below.
Figure 68: retuned PI-controller result at higher speed
From these experimental results we can see that the averaged error is again around 3 pixels and that the reaction is similar to the one we had at lower speeds before. We can conclude that the PI controller works better than the P-controller with respect to the average steady-state error, if tuned well, around a certain working point. We prefer the smoothest response, where the robot might have a small averaged error on the distance because of disturbances. This means that this is the fastest reaction that is still going smooth, at that working point. If the controller is used in a working point which is higher than the one it was tuned for, the response is not optimal anymore, but is much better than with the P-controller. If the PI-controller is tuned according to the new working point, it results again in an optimal response. This is a normal phenomenon if the transition from one working point to another is not linear, while a P- or PIcontroller is a linear control method.
70
5.6.Formation analysis The ultimate goal of the project is to investigate what happens in a full leader-follower formation. With this I mean a chain of robots, of which each robot tracks the robot in front of him. The controls are only based on its own measurements. This means that each robot is completely autonomous and that there is no communication between different robots. Each robot tracks the robot in front of him, and is being tracked by the robot behind him. In first instance we investigate this with 3 ‘identical’ robots, namely the SRV-Q1. The robots are supposed to be identical, being the same product and coming from the same producer, but as was shown several times before, this is not the case. Especially the motor characteristics, being the relation between the input to the motors and the real speed in m/s, can differ quite a lot for different robots. The motor characteristics of all three robots was measured and can be found in appendix 8. In order to track the robot in front of a certain follower, this robot is also provided with a carton with a coloured paper that serves as the blob. The blobs are mounted on the robots to be as similar as possible, but also here small differences between the robots exist. This together with the fact that also the cameras of different robots are mounted not exactly in the same position and angle, causes that different robots might have a different setpoint on the y-coordinate of the centre of the blob. The purpose is to create a situation where each robot reacts, or tracks the robot in front of him, in the same way. The characteristics of the different robots are similar, but not the same. Because of this, the values of the controller values will differ a little bit for different robots, in order to create the same tracking reaction. The way of tuning the parameters for the third robot is exactly the same as described before for the first follower, being the second robot. The values of the different controller, P and PI, will be given later on. As mentioned before, we would like to investigate the reaction of a chain of robots, of which each robot only controls based on the measurements and information he got himself. We control the robots so that the tracking pattern of each follower to the robot in front of him, which he tracks, is the same. The interesting part is then what the pattern is from each follower to the real leader, being the first robot in the chain. Since we own 3 robots, we will investigate what the patter is from the third robot to the first with respect to the distance. The first robot drives with a certain constant speed, that was pre-programmed in its memory. The second robot sees this through the camera and tracks the first robot in a way that was described before while tuning the controllers. The robot’s control loop can be described by a second order control loop with a delay time and non-linear effects. The third robot will track the second robot in the same way when this one is moving at a constant speed. The interesting part here is that the first robot is actually driving at a constant speed, the second tracks this one with a certain response which is not perfect from the beginning. This means 71
that the third robot doesn’t get a perfect ramp as input, but the dynamic path of the second robot. This will influence the tracking reaction of the third robot with respect to the second robot, but also to the first robot. All robots can be represented by a second order control loop with a time delay. This means that for a chain of robots, the reaction of the second robot to the first and of the third to the second can both be represented as second order, but that the reaction of the third robot with respect to the first robot can be represented as fourth order.
Figure 69: Formation block diagram
This is an interesting simulation of what happens in real highway traffic for example. In this case cars are also tracking each other on a straight line based on their own information and measurements. The controller in that case is a human being, which is a quite more complicated controller, but our situation is a good first approximation. A small difference is also that in real traffic, decisions are not only taken on the distance to the car in front, but the driver mostly has a bigger view on the environmental traffic too. 5.6.1. Control parameters of the follower values As described before, we tune the parameters of the third robot in the same way as we did for the second robot in order to achieve the same tracking response. The characteristics of both follower robots are similar but not exactly the same in values. The time delay and sample time are the same, but the motor characteristics are different. We will discuss both the P- and PI-controller. The values for both robots and controllers are summarized in the table below. Robot 2 (Follower 1) P-Controller
P 0.8
PI-Controller P 2.6 I
0.7647
Robot 3 (Follower 2) 0.6 1.15 0.4
Figure 70: TABLE 2: Controller values for the follower robots
5.6.2. Gathering data As mentioned before, we would like to know what the distance is between the first two robots and between the two last robots, but also between the first and the last robot. Since each robot only looks at the blob, or the robot, in front of him, the follower robot can thereby only measure the distance to the robot in front of him. The distance between the last follower (third robot) and the leader (first robot) can thereby not be measured directly. However, this distance can be measured by 72
adding the distance between the first and the second robot and the distance between the second and third robot. By doing this, the length of the robots itself are not taken into account, but this is not a problem because we want to investigate the pattern in the distance changes and the length of a robot is just a constant. The distance is in first instance expressed in the unit of ‘pixels’, the y-coordinate of the centre of the blob. For the same reason as before and also because we want to investigate the pattern in distance changes, the adding of distances and also the graphs are expressed in the unit of ‘pixels’. In order to have correct values for the distance between the leader and the second follower, the distances between should be added at the same time instant. The way data is collected till now, is that each sample the robot sends the information to the remote PC. Since the sample time can slightly differ a few milliseconds, just adding the data of the second and third robot will introduce an error. In order to avoid this, we add the two data arrays at chosen real time instants, and not based on the local sample times of the different robots. We choose a sample time of 25ms, which is of course bigger than the sample time the robot sends the information (around 17ms). When data is needed at a time instant that lies between 2 data points, an interpolation is done as approximation. The m-file for gathering data can be found in appendix 10. 5.6.3. Experimental results P-controller The result of the 3-robot-formation, one leader (robot 1) and two followers (robot 2 and robot 3), with a P-controller controlling the distance, is given in the graph below. The speed pattern of the leader robot is the same as described before in the tracking part. The setpoint for the second robot is 34, while the one for the third robot is 30. The different setpoints correspond the same real distance (15cm). The difference in the setpoint is because of a small difference in camera characteristics of the two robots and a small difference in the height of the two blobs.
73
Figure 71: Formation P-controller - experimental result
Conclusion 1 – Delay time: As one can see in the upper right graph, the second robot tracks the leader as described before. The third robot tracks the second robot with the same controller. The response is more or less the same, but delayed. This is because there is a delay time between the reading of the information and actually executing the appropriate control action. This delay time was described before, and was around 0.6s. The first follower sees the leader accelerating, which increases the distance between the two, but the reaction comes a delay time later, which causes the second robot to see the acceleration only then. Conclusion 2 – Steady-state error: As mentioned before, the first follower follows the leader robot with a steady-state error or about 6 pixels when the leader is moving with an input of 30 and 12 pixels when the leader is moving with an input of 40. The second follower follows the first follower with the same average steady-state error. When we look at the graph that represents the distance from the third robot with respect to the leader, the first robot, we can see that the steady-state errors are added. The steady-state error of the third robot with respect to the leader robot is 12 when the leader robot is moving with an input of 30 and 24 when the input to the leader is 40. The transition between the two steady-state errors is also more fluently instead of a step-response, because of the delay time. Conclusion 3 – Oscillations: The reaction pattern of the third robot is quite the same as the second robot, only shifted over the delay time. The oscillations of the third robot are slightly bigger though. This is because the second robot is not exactly moving with a steady-state speed, but has oscillations on its own. This is the effect we get in a chain of robots. The leader robot moves at a constant speed. The first leader reacts by moving at a steady-state error and some oscillations around this steady74
state error. The second follower follows the first follower with the same steady-state error, but since the first follower is not moving at a perfect steady speed, the oscillations of the second follower are bigger. This is mostly because of the delay time between getting the information and actually braking or accelerating. This causes the responses to be actually in counter phase. When we look at the distance of the second follower to the leader, the oscillations in the distance are slightly damped again. This is because the delay-time causes the responses of the first and second follower to be shifted over a time period so that the oscillations of the two followers are in counter phase, which causes the ‘accordion effect’. In this way, the time delay causes the bigger oscillations in the distance between the first and second follower in first place, but also to damp them in the distance between the second follower and the leader. PI-controller Also the responses of the PI-controller are investigated. The result is given in the graph below. The input speed to the leader robot is 33 and in this case the leader is tracking the red line to stay in a straight line. The setpoint for the second robot is 39, while the setpoint for third robot is 33. The corresponding distances are the same and equal to 18cm. The distance was chosen slightly higher as with the P-controller because of higher collision danger while testing with the PI-controller.
Figure 72: Formation PI-controller - experimental result
Conclusion 1 – Delay time: The conclusion in this matter is the same as with the P-controller. The controller cannot do anything about a delay time in the loop. An open loop delay time is always present in the closed loop too. Again the responses of the first and second follower are very similar. The response was discussed in the part about PI tracking. There is an average error of about 3 pixels 75
because of disturbances, but it’s smaller than with a P-controller (5 pixels). Also the oscillations of the second follower are slightly bigger than these of the first follower, because of the same reasons as with the P-controller. Conclusion 2 – Steady-state error: as mentioned before a small averaged error exists in the responses of the two followers because of disturbances (3 pixels), but the distance is always attempted to be brought back to the setpoint. This has a different consequence if we look at the graph that corresponds to the distance between the leader and the second follower. The distance between 2 robots is always attempted to be brought back to the setpoint, but because this doesn’t happen at the same time for both robots, the distance between the second follower and the leader is actually never brought back to the setpoint. This combination of 2 good responses causes the distance between the second follower and the leader to have an actual steady-state error. This is the drawback of having no communication between different robots. They all control their own distance, but because this doesn’t happen at the same time instant, the overall distance between a follower further in the chain and the leader shows a steady-state error. If the chain of robots is made longer, the average steady-state error will become larger if we go further downstream in the chain. General conclusion – Formation: As a general conclusion, we can state that the pattern in the control of the distance of the several followers is dependent on two phenomena. The first phenomenon is the tracking of each follower on its own. The reaction of different followers is the same with respect to the robot in front of each of them. The best reaction is obtained with a PI-controller, because it results in the smallest average error while still having a continuous driving pattern and keeping the follower robot very close to its setpoint. The controller is not ‘perfect’ in the way that it has an averaged error and that it only performs optimal in the neighbourhood of the working point, or speed, for which the controller was tuned. This is due to the non-linearities in the motor characteristic like the saturation and dead-zone phenomena as described in an earlier section. When the distance from a certain follower to the leader is observed, the average steady-state errors that in each of the robot’s tracking response occur are added, resulting in an averaged steady-state error that is equal to the sum of the separate average errors. In order to avoid phenomena like this, the control algorithm of each of the robots on its own to control the distance to the robot in front of them should be a higher level control, which can preferably take the non-linearities into account. Some ideas for further research on this are stated in the last section. The second influence on the formation tracking is the delay time, which causes a shift in the tracking responses of the separate robots. This results in the fact that the robots don’t reach their setpoint at the same time. This is also known as the ‘accordion effect’. This effect is also observed in real-life traffic. In order to avoid this, the followers should get more information of what is going on further upstream than just the robot in front of them. In that way they could anticipate in their own movement without waiting on the response of the robot in front them, which should decrease the delay time, and thereby give a faster and more accurate response in the control of the followers with
76
respect to the actual leader of the formation. This requires that the followers communicate their measurements with each other, which is also an aspect worth investigating in further research.
77
6. Conclusions and further research
In the first part of the manuscript we described two theoretical models for a leader-follower formation. The first model focussed on the kinematics of a follower robot tracking a leader robot, while the second model focussed more on the kinematics of a mobile robot on its own. The two models were derived and analyzed. The most important conclusion is that the models are non-linear and that there is a coupling between the inputs and the outputs of the MIMO system. The first model was used in a model based predictive control method NEPSAC, applied in order to control the relative position of the follower with respect to the leader, taking into account the coupling between the different inputs and outputs of the system. In a straight line tracking the results are very satisfactory for different speeds. When the leader also moves sideways, the controller reacts good, but the settling time is much longer due to the coupling previously mentioned. The purpose was to use this NEPSAC algorithm in the practical implementation to optimize the control. Due to hardware limitations this was not possible and the model based predictive control remained a theoretical implementation. The practical implementation of the leader-follower formation started with the analysis of the mobile robot, the SRV-1Q. Tests were performed in order to know the system. First the motors were tested and a characteristic between motor inputs and the real speed was measured. The motor characteristic seemed to have a non-linear shape. There is saturation at higher speeds and there is a dead-zone at low speeds. By dead-zone we mean that at low input values (0 to 18) the real speed remains equal to 0 m/s. Once we go above the ‘threshold’, the speed is different from zero. The characteristic also shows a minimum speed, around 0.2 m/s. The position measurement of the robot with respect to an object or a leader robot is done by ‘blob detection’ on-board of the robot with the help of the on-board camera. The coordinates of the blob on the picture are shown to be related to the distance and the angle of the robot with respect to the blob. Also a characteristic for the camera was measured and is assumed to be linear around a certain working point. First we controlled in two directions, having as controlled variables the distance and the angle. The practical set-up consisted of a lamp serving as the object to be tracked, and this at a distance of 1m. A P-controller has been implemented for both outputs, which were considered decoupled. The response of the robot is fast and accurate. The effects of decoupling the two control loops are observed in the response, but they are not very significant. We implemented the control algorithm in different software packages (Matlab, Simulink and C#), in an attempt to have a remote control (from a PC). We concluded that the Wi-Fi communication introduced a too big variable time delay to have a good remote control. The control must be on-
78
board, where a C interpreter is available. As a result of the limitations of this on-board processor the NEPSAC algorithm could not be implemented. The next step was to improve the straight line tracking, in order to have a robot following a leader robot at a certain speed. Theoretically, this is control in one direction. Because of inaccuracies of the robot motors, they tend to move sideways even if the left and right motors have the same inputs. A simple on/off control was successfully implemented in the sideways direction. The distance control between the leader and the follower was first performed with a P-controller. The result was that the follower robot tracks the leader robot with the same speed, but with a steady-state error on the distance. This was concluded to be normal due to the presence of only one integrator in the loop, while applying a ramp as the input. Also the influence of the dead-zone and corresponding higher time constant was analyzed. This effect causes the gain of the controller and the working speed of the leader to have limitations on their values. A model was derived by the means of approximating the motors by a first order transfer function and analyzing a step-response of the leader. The dead-zone effects were modelled as good as possible. The model was tested by comparing the simulation results with the experimental results. Statically the model suffices around the working point it was derived. If the speed of the robots is for example increased, the simulations obtained are different from the experimental results. This phenomenon was analyzed and proved to be due to the non-linearities in the motor characteristics. Due to the same reasons, the dynamical response obtained with the model is not similar to the experimental results. In the next step we implemented a PI-controller, in order to reduce the steady-state error previously mentioned. Several methods were attempted to tune the controller, for example an autotuner and the Ziegler-Nichols theory, but no good results were achieved with these methods. The controller was tuned manually and delivers good results. The steady-state error is at a certain speed reduced to zero. Due to disturbances in the motors and from the environment, errors on the distance different from zero occur, but they are always attempted to be controlled back to zero by the controller. However, the average error over time is thereby different from zero. When another speed is applied, the controller does not work optimal anymore because of the non-linearity in the motor characteristics, resulting in a small steady-state error. However, the response is still a big improvement with respect to the P-controller. It is proven that by retuning the parameters the same good response as before can be achieved at other working speeds. At last, the tracking set-up was extended with a third robot, forming a real leader-follower formation of 3 robots, a ‘chain of robots’. The third robot was tuned in the same way as we did for the other robot before. Now the distance between the second follower and the leader of the formation is analyzed. The first conclusion is that with a P-controller the steady-state errors of the two leaders are added and form the steady-state error on the distance between the second follower and the leader. With a PI-controller we saw that due to the delay time, there is a time shift in the responses of the two followers. Because of this phenomenon and the influence of the disturbances mentioned 79
before, a real steady-state error was observed. This is the result of a tracking control which is not ‘perfect’ and of the lack of communication between the robots. As a general conclusion we can state that a lot of aspects regarding the mobile robots as a system, the tracking problem and the formation, was investigated and that good tracking results were achieved. The two robots are able to follow the leader at a quite small distance in between. However, the responses are not optimal yet, and due to that it offers a lot of possibilities for improvement and further research on the subject. As mentioned in the last part, the formation results are mostly a consequence of two major characteristics, the tracking from one robot to the one in front and the possibility of communicating between robots. We implemented a control on the robots where each robot is stand-alone and where the distance between robots is controlled by a linear controller, being a P- or a PI-controller. I will suggest here a few ideas of possible further research in order to improve the overall formation control or extend the applications of the robots: 1) Neural networks and fuzzy logic: The system was proved to have a lot of non-linearities, especially in the motor characteristics. The most important non-linearities are the dead-zone and the saturation in the characteristic from the input to the real speed of the motors. The non-linearities are also introduced by the fact that the switching from a zero speed to a nonzero speed takes a longer time than switching between speeds when the robot is already moving. These non-linearities cause a lot of trouble if we try to control the distance between robots with a linear approach like a P- or PI-controller, as was discussed in the manuscript. More advanced control algorithms like neural networks or fuzzy logic might be able to deal a lot better with these non-linearities. I think that especially the saturation and higher time constant that occur in switching from zero speed to a real speed can be taken much more into account with these techniques. A possibility would be to make use of speed-dependant parameters instead of fixed parameters. 2) Communication: As discussed in the section where we investigate the formation response, there is a big time delay in the response of one robot to the other. The time shift in the robots’ responses causes an accordion effect because the distance between a certain follower and the actual leader never reaches the setpoint. This effect will be increased with a higher number of followers. This is typical for a chain of robots where each robot can only decide control action based on the movement of the robot directly in front of him. In order to improve the overall formation control we suggest that the robots should be able to communicate, so that a follower knows what happen with all the robots in front of him, and can take a control action before the robot directly in front of him actually changes his movement. In this way the delay time is decreased. The means of communications have to be investigated. Communication is now only possible with Wi-Fi, which has a big time delay, as we proved during testing. An option is to look for other communication ways which are much faster but still have to be implemented on the robots. In case the chain of robots is 80
very long, the Wi-Fi may satisfy the needs as the delay time increases with the length of the robot chain which creates more time for the follower to communicate. 3) Modelling of a total formation: The complete formation may have a certain behaviour which could be modelled, for example the reaction of control on the distance between robot 1 and 2 on the distance between robot 9 and 10. This could be of the greatest interest when communication is possible. The distance between several robots can provide the necessary information to the correct robots for use of higher level (model based) control. 4) NEPSAC for control in two directions: In the beginning of the manuscript I explained that the system, being the tracked robot, has two inputs (left and right wheel speed) and two outputs (the distance and the angle). The two outputs and the two inputs are coupled. We implemented a control which decouples these two outputs from each other. A kinematic model was described in the first section of the manuscript. With the help of a model predictive control, like NEPSAC, onboard of the robot, the best combination of inputs can be determined in order to have the fastest control in both outputs together. The issue is now that the onboard processor is very limited and incapable of running an algorithm of model predictive control where numerical integration is needed for example. In order to make this possible, the processor should be extended by a much stronger calculation centre. 5) Improve hardware characteristics: The biggest issues we had are the non-linearities of the motors, the limited capabilities of the processor and the poor communication between a robot and other hardware. In order to be able to have a better control, these hardware problems could be avoided by replacing certain parts of the robots by more accurate and much higher performing hardware components. To improve the performance for example, some encoders or a IMU board would be convenient. 6) Improve distance measuring: The distance is now measured by the camera onboard of the robot. The camera has an accuracy of 1 pixel, which corresponds to about 1 cm. In order to have a better distance measuring, one could install infra-red sensors with a mm-precision. For angle measuring an IMU board could be helpful. 7) Collision avoidance: There are a lot applications that can be thought of with mobile robots. One example is collision avoidance. This could be done by getting more out of the picture than we do now. Working with different colours in the environment for example could be a start in this direction. Also communication of the different robots with each other might be convenient here to get a quick ‘mapping’ of the area, which is then known to each robot.
81
Dutch summary (Nederlandse samenvatting)
82
Inhoudsopgave
83
1. Inleiding
Het onderwerp van Multi-agent systemen is zeer interessant geworden de laatste jaren. Het ontstond oorspronkelijk uit militaire toepassingen, omwille van de vele mogelijkheden, flexibiliteit en robuustheid van Multi-agent systemen van onbemande voertuigen. Deze voertuigen kunnen lucht-, water- of landvoertuigen zijn. In deze thesis focussen wij ons op mobiele robots op land. Dit gedeelte is een samenvatting van de eigenlijke scriptie, die hiervoor te vinden is. Het geeft een redelijk uitgebreid overzicht van alle aspecten in de thesis, maar voor een complete beschrijving raad ik de lezer aan de complete Engelse tekst te lezen. De theoretische aspecten die in het begin van de scriptie besproken worden zijn niet opgenomen in deze samenvatting. In het eerste deel van de thesis maakte ik kennis met Multi-agent systemen door een literatuurstudie. Aansluitend werden twee modellen en de toepassing van modelgebaseerde controle erop bestudeerd. Een praktische implementatie van een leider-volger formatie werd uitgevoerd met mobiele robots. De verschillende aspecten van zowel de robots op zichzelf als de uiteindelijke fenomenen van een uitgebreide formatie werden bestudeerd.
2. state of the art Het begin van mijn thesis bestond vooral uit het lezen van literatuur over het onderwerp van mobiele ‘Multi-agent’ systemen aangezien dit een volledig nieuw terrein was voor mij. Het is nodig mij er eerst wat vertrouwd mee te maken eer er zelf een bijdrage aan te leveren. Aangezien dit gedeelte vooral beschrijvend is en uitvoerig uitgeschreven werd in mijn thesis zelf zal ik hier niet te diep op ingaan in deze samenvatting. Allereerst worden de architecturen voor Multiagent systemen beschreven. Deze kunnen ingedeeld worden op 2 manieren. Daarna worden dan een aantal algemene aanpakken besproken om een Multi-agent systeem te controleren. 2.1. Architecturen van Multi-agent systemen Een eerste onderverdeling kan gemaakt worden op basis van de manier waarop de verschillende agenten in het systeem hun informatie of controle orders ontvangen:
Gecentraliseerd: alle informatie wordt samen gebracht in een centraal systeem. Alle agenten worden ook vanuit dit systeem aangestuurd. Het voordeel is dat alle informatie op één plaats tezamen is gebracht, wat een enorm voordeel kan zijn voor de controle van het volledige systeem. Het nadeel is duidelijk wanneer deze centrale eenheid faalt, dat het hele systeem faalt. Ook een goede communicatie is nodig om dit op te stellen. 84
Hiërarchisch: Het gecentraliseerde systeem wordt onderverdeeld in subsystemen. Elke robot stuurt een aantal robots onder zich aan. Qua communicatie is dit efficiënter dan een gecentraliseerd systeem. Het falen van een agent hoog in de hiërarchie heeft nog steeds tot gevolg dat een groot deel of het volledige systeem dan faalt.
Gedecentraliseerd: Elke agent in het systeem werkt als een eenheid op zichzelf. Dit wil zeggen dat deze zijn eigen informatie verzamelt en op basis van deze informatie zijn eigen controle acties berekent en uitvoert. Een lichte variant is een ‘Gedistribueerd’ systeem. Hierbij neemt elke agent nog steeds zijn eigen beslissing, maar baseert hij zich daarbij soms op globale informatie.
Hybride: Elke combinatie van bovenstaande architecturen.
Een tweede manier om de architecturen van Multi-agent systemen in op te delen is meer op basis van waar de informatie vandaan komt, of met welke aspecten de agenten rekening houden:
Leider-volger: Eén agent wordt aangeduid als leider van de groep en de andere agenten volgen deze agent in een bepaalde geometrische vorm, een formatie. Een belangrijke variant hierop is dat, buiten de eerste en de laatste agent, elke agent zowel leider als volger is en er zo een ‘ketting’ van robots ontstaat.
Gedragsmatig: Elke agent heeft verschillende ingebouwde gedragspatronen afhankelijk van de situatie. Een voorbeeld is het volgen in een formatie, maar als deze agent te dicht bij een voorwerp komt schakelt het gedrag over naar ‘obstacle avoidance’.
Virtuele structuur: Bij deze structuur wordt de gehele formatie als een eenheid bekeken. Bijvoorbeeld, bij het toepassen van ‘obstacle avoidance’ moet de gehele formatie rond het obstakel gaan en niet juist de ene agent die gevaar loopt er tegenaan te gaan.
2.2.Controlestrategieën Er zijn twee groepen waarin we de controlestrategieën van Multi-agent systemen kunnen opdelen:
Optimalisatie gebaseerd: Deze strategie zoekt naar een optimale input voor elke agent gebaseerd op het minimaliseren van een kostfunctie. Deze strategie leunt in eerste plaats naar een gecentraliseerde architectuur. Een optie is het gehele systeem op te delen, zelfs tot op het niveau van 1 agent, en lokale kostfuncties te definiëren om op deze manier een meer gedecentraliseerde architectuur mogelijk te maken. Tot deze groep behoren echter ook de reactieve
controlestrategieën,
zoals
PID,
LQR,…
alsook
de
modelgebaseerde
voorspellingsstrategieën.
Gedragsgebaseerd: Elke agent heeft een aantal voorgeprogrammeerde schema’s die hij uitvoert afhankelijk van de inputs van de sensors. Dikwijls worden verschillende schema’s samen gebracht. Een voorbeeld hiervan is de potentiële veld strategie. Hierbij wordt een artificieel potentieel veld berekend aan de hand van allerlei omgevingsfactoren (afstand tot 85
andere robots, afstand tot obstakels,…), om zo tot een fictieve kracht te komen, die de inputs van de agent zullen bepalen.
2.3.Toepassingen van Multi-agent systemen De toepassingen voor Multi agent systemen zijn bijna oneindig omwille van diverse redenen. Een reden is bijvoorbeeld dat meestal vele kleine agenten veel sterker en flexibeler zijn dan één grote agent. Een aantal voorbeelden zijn: search en rescue, mapping, verplaatsen van objecten, observeren van verschillende objecten, simuleren van drukke verkeerssituaties, competities, formaties,…
3. Modelgebaseerde controle 3.1.Model 1: kinematisch leider-volger model Een eerste model wordt opgesteld waarbij we een formatie beschouwen van 2 robots, bestaande uit een leider en een volger. Dit kan dus ook een tracking model genoemd worden dus. De positie van een mobiele robot in het vlak wordt bepaald door 2 coördinaten, de oriëntatie door een derde. We definiëren de afstanden en hoeken zoals in onderstaande figuur.
Figure 73: Model 1
Het model wordt afgeleid in de thesis zelf en wordt hier herhaald: ρ = 𝑣𝐹 cos β + π − 𝑣𝐿 cos ψ ψ=
− sin 𝛽 + ψ sin ψ 𝑣𝐹 + 𝑣𝐿 − 𝜔𝐿 𝜌 𝜌 𝛽 = 𝜔𝐿 − 𝜔𝐹
Dit is een niet-lineair model met 3 outputs (ρ, ψ, β) en 4 inputs (vL, wL, vF, wF), bestaande uit 3 gekoppelde differentiaalvergelijkingen. De lineaire en rotationele snelheid van de leider wordt gekozen, en de snelheden van de volger moeten optimaal berekend worden met als doel het behouden van de gewenste waarde in de outputs. Het model is een statisch model en heeft daarom enkel nood aan de vorige toestand en de ogenblikkelijke inputs om het toekomstige pad te 86
berekenen of te voorspellen. Aangezien dit een niet-lineair model is dat niet analytisch kan worden opgelost, wordt er gebruik gemaakt van numerieke integratie. De matlab code voor dit kan terug gevonden worden in bijlage 8.
3.2.Model 2: kinematisch model voor een mobiele robot Als tweede model bekijken we de inputs en outputs van een robot op zichzelf, alsook de koppeling ertussen. Hier wordt een mobiele robot bekeken met 2 wielen links en rechts, waarvan de snelheid de inputs zijn. De outputs zijn dan de positie en oriëntatie van de robot in een algemeen coördinatensysteem. Hier wordt ook het verband gelegd tussen de inputs van het linker en rechter wiel en de lineaire en rotationele snelheid die daaruit resulteert. Daarvoor is het belangrijk te weten dat voor elk bewegend punt in de ruimte een ogenblikkelijk centrum van rotatie kan gedefinieerd worden, alsook voor de bewegende robot. Door het bepalen van dit punt en het in rekening brengen van de afmetingen van de robot kan een model worden afgeleid. Dit wordt hieronder kort voorgesteld.
Figure 74: Model 2
𝑣𝑅 (𝑡) 𝐿 𝑅+2 𝑣𝐿 (𝑡) 𝜔 𝑡 = 𝐿 𝑅−2 𝜔 𝑡 =
𝑣𝑅 𝑡 − 𝑣𝐿 𝑡 𝐿 𝑣 𝑡 = 𝜔 𝑡 . 𝑅 = 1/2(𝑣𝑅 𝑡 − 𝑣𝐿 𝑡 ) 𝜔 𝑡 =
𝑥 = 𝑣 𝑡 . cos 𝜃 𝑡 𝑦 = 𝑣 𝑡 . sin 𝜃 𝑡 𝜃 = 𝜔(𝑡) Ook hier zijn beide inputs en outputs opnieuw gekoppeld met elkaar en gebeurt het bepalen van de positie en oriëntatie met behulp van een niet-lineair model. Eenzelfde strategie kan worden toegepast als bij ‘Model 1’ voor het bepalen van optimale inputs als reactie op storingen of veranderingen. Het coördinatensysteem kan bijvoorbeeld gehecht worden aan de leider. 87
3.3.NEPSAC implementatie en resultaten Het eerste model werd gekozen om een modelgebaseerde controle voor te ontwerpen. We wensen nu zelf de inputs van de leider te kiezen en met een modelgebaseerde controle de optimale inputs te bepalen zodanig dat de outputs terug gebracht worden naar de wenswaarde en dit met een minima aan energie. Een modelgebaseerde controle kan daarvoor goed dienen aangezien de drie outputs en de twee inputs gekoppeld zijn met elkaar. De dynamica in het systeem komt hier niet van traagheden van motors of dergelijke, maar van de dynamica van het pad en de koppeling tussen de inputs en de outputs. De matlab code voor de NEPSAC implementatie kan terug gevonden worden in bijlage 9. Eerst wordt de lineaire snelheid van de leider aangepast. Het resultaat is te zien in onderstaande figuur.
Figure 75: NEPSAC results 1
Conclusie – Zoals te zien is in de figuur brengt de controle de afstand tussen de twee wagens terug naar de wenswaarde, op elk niveau van de snelheid van de leider. De input voor de lineaire snelheid van de volger is eerst hoger om verloren terrein in te halen en convergeert daarna naar dezelfde waarde als de leider. De veranderingen in de hoeken en input voor de rotationele snelheid zijn te verwaarlozen en zijn het gevolg van linearisatie in het berekenen van de G-matrices. Nu veranderen we de rotationele input van de leider, terwijl de lineaire snelheid gelijk gehouden wordt. Onderstaande figuur toont het resultaat.
88
Figure 76: NEPSAC results 2
Conclusie – Een verandering in de rotationele snelheid van de leider is duidelijk heel wat moeilijker te controleren. Dit is duidelijk omdat het een invloed heeft op alle drie de te controleren veranderlijken. De settling tijd voor deze ‘storing’ of verandering van werkpunt heel wat langer tn gevolge van de koppeling tussen de verschillende inputs en outputs.
4. Praktische implementatie van een leider-volger formatie 4.1.Praktische uitvoering 4.1.1. De SRV-1Q robot Na een kort overzicht van mobiele robotica en een literatuurstudie over formaties van mobiele robotten, is het interessant om een praktische toepassing uit te werken en te onderzoeken. Om dit te kunnen verwezenlijken hebben we nood een robot. De robot moet enerzijds gesofistikeerd genoeg zijn om bijvoorbeeld afstandsmetingen te doen of een controle algoritme te kunnen uitvoeren. De robot moet anderzijds ook financieel interessant zijn omwille van het feit dat we een formatie willen vormen en we dus in staat moeten zijn meerdere robots aan te kopen. De gekozen robot is de Surveyor SRV-1Q van Surveyor coorporation.
Figure 77: SRV-1Q robot
89
Alle toepassingen en hardware specificaties kunnen terug gevonden worden op de website van Surveyor. De robot heeft een aandrijfmechanisme dat bestaat uit 4 gelijkstroommotoren, welke 2 per 2 aangestuurd worden. Er is een on-board camera met verschillende resoluties. Er is ook een onboard Flash geheugen waar men gelimiteerde C-programma’s kan naar uploaden. De communicatie met de PC wordt gedaan over een Wi-Fi verbinding. 4.1.2. Communicatie De communicatie tussen de robot en een PC kan gedaan worden door verschillende programma’s. Ze maken allemaal een TCP/IP-verbinding met de robot, maar het gebruik is anders. De programma’s ‘Putty’ en ‘Tera term’ worden gebruikt om snel een verbinding te maken met de robots en rechtstreeks commando’s te sturen of data te ontvangen. Het opslaan en verwerken van data is bij deze programma’s niet echt aan de orde. Om een meer gesofisticeerde communicatie te hebben tussen een PC en robot, creëerden we onze eigen functies in Matlab. Met behulp van deze functies kan men een verbinding aanmaken, data versturen, data ontvangen en data verwerken. Dit werd gedaan voor de controle van de robot op een PC. De functies werden in beperktere mate ook aangemaakt in C# en C voor het uploaden op de robot. De matlab commando’s kunnen terug gevonden worden in appendix 1. 4.1.3. Hardware testen van de motors Belangrijk om te weten is dat de enige input die we naar de robot kunnen sturen, de inputs naar de linker en rechter motors zijn. De waarde van deze input ligt in een bereik van [-100,100] en is evenredig verondersteld met de spanning die over de gelijkstroommotors gelegd wordt, of met andere worden de rotatiesnelheid van de motors. Het eerste wat we deden was onderzoeken hoe de inputs relateren met de werkelijke snelheden. Uit de test volgt het onderstaand resultaat. De negatieve inputs geven een analoge karakteristiek. 0,5
Speed (m/s)
0,4 0,3 0,2 0,1 0 0
10
20
30
40
50
Input Figure 78: NEPSAC results 1
90
De twee belangrijke kenmerken die bepalend zullen zijn voor het vervolg van de praktische implementatie, is dat de karakteristiek een saturatie vertoont naar een snelheid van 0.45 m/s en dat er een ‘dode zone’ is in de lagere inputs. Bij volledig opgeladen batterij is de laagste input om beweging te hebben een input van 18. De snelheid bij deze minimale input is ook niet gelijk aan nul, maar heeft een startwaarde van ongeveer 0.2 m/s. 4.1.4. Blackfin camera en afstandsmeting De metingen worden gedaan met de on-board camera van de robot. Elk beeld dat de camera neemt kan worden gezien als een matrix van pixels, waarbij de oorsprong gelegen is in de linker bovenhoek van het beeld. Om een object, of uiteindelijk een andere robot, te volgen, moeten we in staat zijn de nodige informatie uit dit beeld te halen. Hiervoor hebben we ons eigen image processing algoritme ontwikkeld in Matlab, maar deze moest gebruikt worden op een PC en kon niet on-board geïmplementeerd worden. Daarom maken we gebruik van een voorgeprogrammeerde functie op de robot die regio’s van een bepaald kleur kan detecteren (‘blobs’), er een rechthoek rond fit en de coördinaten (x1,x2,y1,y2) van de zijden en de oppervlakte (in # pixels) van de blob terug geeft als meting. De kleur wordt vastgelegd door het specificeren van YUV-indexen. Om de nodige informatie te verkrijgen berekenen we de coördinaten van het centrum van de blob. Na testen volgt dat de y-coördinaat van het centrum van de blob, of met andere woorden de hoogte in het beeld,de beste aanduiding geeft van de afstand tussen te robot en de blob. De x-coördinaat geeft de beste aanduiding voor de hoek onder welke de robot naar de blob kijkt. De oppervlakte van de blob staat ook in verband met de afstand, maar vertoont een grote spreiding op de metingen. Belangrijk is nog op te merken dat de relatie tussen de afstand en de y-coördinaat van het centrum van de blob niet perfect lineair is. Als laatste hierbij merken we nog op dat de vorm en de plaats van de blob die gedetecteerd moeten worden van groot belang zijn. Als de blob boven of onder de hoogte van de camera op de robot geplaatst wordt, zal de karakteristiek tussen de afstand en de y-coördinaat omkeren. Als de blob een licht is, ondervindt deze amper invloed van het omgevingslicht, maar is de contour van de blob niet echt duidelijk en kan er ook reflectie optreden, wat in meer spreiding op de metingen resulteert. Als de blob een gekleurd papier is, resulteert dit in stabiele metingen, maar is er heel wat storing van de omgevingsfactoren.
4.2.Controle in 2 richtingen Het elementaire gedeelte van een formatie, die bestaat uit een leider en verschillende volgers, is het 2-robot element. We willen komen tot een formatie van robots waar de ene de andere volgt, ook wel ‘tracking’ genaamd, met metingen en controle beslissingen die elke robot op zichzelf neemt. Om tot een goede tracking te komen hebben we verschillende stappen doorlopen. Eerst en vooral probeerden we de robots in 2 richtingen te controleren op een grote afstand, met een lichtbron als 91
het te volgen object. We hebben daarbij verschillende opties voor de communicatie of het uivoeren van de controle algoritmen doorlopen. In tweede instantie wilden we overgaan van een te volgen object naar een werkelijke andere robot die moet gevolg worden en wilden we ook controle hebben op een realistische korte afstand tussen de robots. Dit vergde enkele praktische aanpassingen. Om de effecten van een formatie daarna te kunnen bestuderen hebben we ons gefocust op de controle in 1, voorwaartse, bewegingsrichting en geprobeerd deze te optimaliseren. Deze aspecten zullen in dit gedeelte en de volgende besproken worden. 4.2.1. Praktische opstelling De praktische opstelling is besproken in de vorige sectie. We werken hier met een lamp die wit licht emitteert en daarbij een redelijk gebied definieert als de blob, wat nodig is op een grote afstand. De resolutie van de camera is ingesteld op 640x480, en de karakteristiek van de camera, zijnde de afstand tot de blob in relatie met de y-coördinaat van het centrum van de blob, is gegeven in
y-coordinate of th center of the blob (pixels)
onderstaande figuur.
165 155 145
Test 1 Test 2
135
Test 3 125
test 4
115 0
50
100
150
200
250
Distance to the blob (cm) Figure 79: Camera test
We zien dat er een duidelijk bruikbare relatie bestaat tussen de afstand en de gemeten grootheid. We zien ook dat er wat ruis, of spreiding zit op de metingen, en dit omwille van redenen die in de vorige sectie besproken werden. 4.2.2. Controlestrategie Het systeem bestaat uit 2 inputs, namelijk de waarden in het gebied [-100,100], die evenredig zijn met de spanning die aan de linker en rechter motors wordt aangelegd. De twee outputs zijn de afstand tot de blob en de hoek waaronder de blob gezien wordt. Zoals aangetoond in de sectie waar een kinematisch model werd uitgelegd, zijn de twee inputs en outputs gekoppeld. Voor de controle van de twee outputs gaan we deze hier in eerste plaats toch ontkoppelen. Het idee is een Pcontroller te implementeren in beide richtingen om zo de positie naar de wenswaarde te brengen. 92
Voor de voorwaartse richting berekenen we een input voor beide motors, links en rechts, die evenredig is met het verschil tussen de y-coördinaat van het centrum van de blob en de wenswaarde die overeen komt met een afstand van 1m. Voor de zijwaartse richting, of de controle van de hoek, berekenen we een input voor één van beide motoren die evenredig is met het verschil tussen de x-coördinaat van het centrum van de blob en de wenswaarde die overeenkomt met een hoek van 0°. Daarna tellen we de bekomen inputs op om 1 waarde van de input voor beide motoren te bekomen. Onderstaande code verduidelijkt het idee. Het volledige script kan terug gevonden worden in appendix 3.
Het eigenlijke controle script bevat enkele elementen meer die de volgende doeleinden hebben:
Meting doen met behulp van de blob detectie en berekening van het centrum.
De waarde van de P-controller wordt vermenigvuldigd met 100 en later in het script weer gedeeld om afrondingsfouten te vermijden, en dit omwille van het feit dat de on-board controller alleen kan werken met gehele getallen.
Een zone van 8 pixels rond de setpoint van de afstand waar de voorwaartse controle wordt uitgeschakeld, om oscillaties ten gevolge van ruis uit te schakelen.
De berekende inputs vermeerderen met 18 of -18, afhankelijk van het teken van de berekende input, om de dode zone van de motors te overbruggen.
93
Het resultaat wordt gegeven in onderstaande figuur.
Figure 80: Resultaat controle in 2 richtingen
PART 1 – Het te volgen object is in stilstand, dus zo is ook de robot. PART 2 – Het te volgen object wordt zijdelings en achteruit bewogen, wat een fout in zowel de x- als y-coördinaat van het centrum van de blob nalaat. Beide inputs krijgen een positieve waarde, maar niet dezelfde omwille van de zijdelingse controle. De controle van de hoek is niet onmiddellijk duidelijk in dit gedeelte, omdat het verschil van de twee inputs niet groot is ten opzichte van de waarde van de inputs ten gevolge van de controle van de afstand. PART 3 – In dit deel krijgen we het vervolg van het vorige gedeelte. De afstand is nu min of meer geregeld, en de voorwaartse input wordt bij gevolg klein. Hierdoor is het verschil in de twee inputs ten gevolge van controle op de hoek nu relatief groter. We zien dan ook dat de x-coördinaat nu snel naar de setpoint gebracht wordt. De controle van de afstand krijgt hier dus eigenlijk een natuurlijke voorrang op de controle van de hoek. Dit is een gevolg van het ontkoppelen van de twee outputs. PART 4&5 – De y-coördinaat heeft een kleine overshoot ten gevolge van de controle van de hoek of de x-coördinaat. Dit is opnieuw een gevold van het ontkoppelen van de twee outputs. Er is een voortdurende wisselwerking tussen de twee outputs. De fouten ten gevolge van dit zijn echter zeer klein ten opzichte van de oorspronkelijke fout ten gevolge van het bewegen van het object. Ze worden ook redelijk snel weggewerkt. PART 6 – De setpoints voor beide outputs zijn bereikt en het systeem is nu in rust.
94
4.3.Controle scripts en communicatie Zoals eerder besproken gebeurt de communicatie tussen de robot en een PC over Wi-Fi, met behulp van een TCP/IP connectie. Om de ontwikkelde controle scripts uit te voeren hebben we verschillende manieren geprobeerd. We sommen ze hier kort op. De eerste optie die we uitprobeerden was Matlab, omdat we hier het meest mee vertrouwd zijn. Het is zeer handig om te programmeren, maar de delay time in de controle is variabel en soms veel te groot. Het Matlab controle script kan terug gevonden worden in appendix 5. De tweede optie was simulink, omdat dit programma meer gericht is op real-time toepassingen. De mogelijkheden om een TCP/IP connectie te maken zijn echter veel te beperkt voor onze toepassing. De derde optie was het programmeren in C#, omdat dit programma ook veel sneller bleek te zijn voor real-time toepassingen en goed uitgewerkt is voor TCP/IP connecties. De delay time was beter dan met Matlab, maar nog steeds te groot en te variabel. Het C# controle script kan terug gevonden worden in appendix 6. De vierde en laatste optie was het programmeren in de gelimiteerde C programmataal die we kunnen uploaden in het geheugen van de robot. De sample time die we nu kunnen hanteren is rond 17ms en redelijk stabiel. Het probleem lag dus duidelijk in de Wi-Fi verbinding. De ‘C-interpreter’ onboard is echter zeer beperkt en niet geschikt voor eventueel toekomstige complexere controle algoritmen. Dit is de reden waarom het NEPSAC algoritme niet praktisch geïmplementeerd kon worden.
4.4.Controle in 1 richting – ‘Line tracking’ Zoals eerder vermeld willen we nu overgaan naar een meer realistische opstelling. We gaan over van een statische verplaatsing met een object naar een tracking van een andere robot die een constante snelheid heeft. Dit met het oog op het vormen van een formatie van meerdere robots die elkaar volgen. We focussen ons hier op het tracken in een rechte lijn, of met andere woorden op het controleren van de afstand tussen de robots, en niet de hoek. Om de kwaliteit van de metingen te verhogen kunnen we op deze korte afstand over gaan naar de laagste resolutie van de camera. De blob wordt nu ook niet meer gevormd door een lichtbron, maar door een gekleurd papier dat wordt gemonteerd op de achterzijde van de leiderrobot. De sample time die we kunnen hanteren bij deze lage resolutie is ook veel verbeterd, zijnde 17ms, ten opzichte van 180ms voorheen. Bij de laagste resolutie stemt 1 pixel verschil in de y-coördinaat van het centrum van de blob ongeveer overeen met 1cm verschil in afstand. De nauwkeurigheid van de metingen is nu +- 1 pixel, in tegenstelling tot 8 pixels voorheen. We merken tot slot nog op dat de volgerrobot nu een nieuwe robot is, en dat de robot die gebruikt werd in het eerste gedeelte als leider benoemd wordt. 95
4.4.1. P-controller In eerste instantie hanteren we dezelfde controlestrategie als hiervoor beschreven, de P-controller in de controlelus voor de afstand tussen de twee robots. Zoals eerder vermeld is het nu een nieuwe robot die het tracken zal uitvoeren. Theoretisch gezien zouden de robots allemaal identiek dezelfde moeten zijn. Dit is echter niet het geval. De karakteristieken zijn van gelijke aard, maar niet exact dezelfde. In bijlage 8 kunnen de motorkarakteristieken van de drie verschillende robots die we ter onzer beschikking hebben terug gevonden hebben. Het is ‘robot 2’ die hier de volgerfunctie zal uitvoeren en ‘robot 1’ die de taak van leider op zich neemt. De helling en offset van de verschillende robotkarakteristieken is licht verschillend voor de verschillende robots. Theoretisch gezien zouden de robots rechtuit moeten rijden wanneer de inputs naar de linker en rechter motors gelijk is. Dit is in de praktijk echter niet het geval. Storingen, maar vooral onnauwkeurige hardware componenten zorgen ervoor dat de robots een zijwaartse beweging uitvoeren. Om hiervoor te compenseren voeren we alsnog een simpele controle uit op de xcoördinaat van het centrum van de blob. Als deze zich meer dan 10 pixels verwijderd bevindt van het centrum van het beeld, krijgen de linker of de rechter motors, afhankelijk van in welke richting de afwijking gaat, een extra inputpuls van 15 of 20. Dit creëert in principe een on/off controle op de xcoördinaat om de robots in een rechte lijn te houden. Resultaten en conclusies Als test wordt aan de leiderrobot een constante input gegeven, wat correspondeert met een constante snelheid. De snelheid wordt in het midden van de test verhoogd van een input 30 naar een input 40. De resultaten van de test met een waarde voor de P-controller van 0.8 worden hieronder weergegeven.
Figure 81: Resultaat P-controller
96
Conclusie 1: constante snelheid - Het is duidelijk uit de grafiek die de inputs voor de 2e robot weergeeft, dat deze gemiddeld een constante waarde aanhouden. De gemiddelde waarde van de inputs voor robot 2 zijn niet gelijk aan de input voor robot 1 omwille van een verschillende motorkarakteristiek voor de twee robots. Als we de inputs van beide robots vertalen naar een werkelijke snelheid van m/s volgens de respectievelijke karakteristieken is de waarde voor beide robots wel steeds dezelfde. Conclusie 2: steady-state fout – Uit de grafiek voor de y-coördinaat van het centrum van de blob (de afstand tussen de twee robots) is ook duidelijk te zien dat deze niet gemiddeld rond de setpoint is, maar een steady-state fout vertoont. Als de leiderrobot stopt wordt de fout echter volledig weg gewerkt. De fout vergroot als de gewenste snelheid vergroot. Dit is te verklaren omwille van het feit dat de inputs van de robot evenredig zijn met de snelheid en we meten de afstand tussen robots, of de positie. Dit betekent dat we inherent 1 integrator in de lus hebben. Dit verklaart waarom de steady-state fout bij een stapinput (veranderen van de positie van de leider) gelijk aan nul is. Wanneer de leider echter met een constante snelheid beweegt, is dit equivalent met het aanleggen van een rampinput aan de volgerrobot. Om hier een steady-state fout gelijk aan nul te hebben zou er echter een tweede integrator aanwezig moeten zijn. De steady-state fout is afhankelijk van de helling van de rampinput en de statische versterking van de regelkring. Een samenvatting van de steadystate fout in functie van de versterking van de P-controller wordt hieronder gegeven. Gain
center_y
error
center_y
error
(pixels)
(pixels)
(pixels)
(pixels)
Input robot 1 = 30
Input robot 1 = 40
0,6
41
6
48
13
0,8
40
5
45
10
1
38
3
44
9
Figure 82: TABEL 1: P-controller waarden en steady-state fouten
Conclusie 3: oscillaties - Tot slot is er nog een fenomeen dat extra aandacht vraagt. Wanneer de versterking van de P-controller te groot is, resulteert dit in een sterke reactie maar vanzelfsprekend ook meer overshoot. Overshoot betekent hier dat de volgerrobot te dicht bij de leiderrobot komt. Hier kan het gebeuren dat de robot te veel wil afremmen en zelfs wil stoppen of achterwaarts zou willen rijden. Hier ontstaat er een complicatie met de non-lineariteit van de dode zone die we eerder bespraken. Als de vereiste snelheid lager is dan de minimum snelheid van de robot valt de snelheid op nul. Het switchen tussen een nulsnelheid en een snelheid die buiten de dode zone ligt vergt echter veel meer tijd dan het switchen tussen snelheden die buiten de dode zone liggen. Aangezien dit een serieuze vertraging met zich meebrengt en de leiderrobot ondertussen nog steeds met een constante snelheid vooruit rijdt, wordt de fout op de afstand tussen de twee robots weer snel groter. Eenmaal de vereiste snelheid weer boven de grens is wil de robot weer vertrekken, maar omwille van de veel grotere tijdsconstante in het switchen tussen een snelheid nul en een werkelijke
97
snelheid, is dit niet optimaal en ontstaat een schokkerige manier van tracken. Een voorbeeld hiervan is gegeven in de grafiek hieronder, met een waarde voor de P-controller van 1.
Figure 83: P-controller – dode zone
Eenzelfde patroon van tracken (schokkerig), kan voorkomen wanneer de snelheid van de leiderrobot zodanig gekozen wordt dat deze lager ligt dan de werkelijke minimale snelheid van de volgerrobot. Het is voor de volgerrobot dan onmogelijk om de leider te tracken met eenzelfde gemiddelde snelheid, aangezien deze snelheid zich in zijn dode zone gebied bevindt. 4.4.2. Identificatie van een model voor de volgerrobot Identificatie Om een idee te krijgen van de karakteristiek van de robot proberen we er een model voor op te stellen. Aangezien we een afstand, of dus positie meten, weten we dat er een integrator in de rechtstreeks tak aanwezig is. Verder gaan we ervan uit dat een gelijkstroommotor kan voorgesteld worden door een eerste order transfer functie met in de noemer de tijdsconstante en in de teller een evenredigheidsfactor tussen de input, zijnde de spanning, en een output, zijnde de werkelijke snelheid. Tot slot is er ook een delay time die we in rekening zullen brengen. Om deze factoren te identificeren voeren we een stapexperiment uit. Dit wil zeggen dat we de robot aan een constante snelheid laten rijden achter de leiderrobot en dan plots de input van de volgerrobot verhogen terwijl de leider nog steeds met de originele snelheid rijdt. De responsie in de afstand tussen de twee robots zal ons de nodige informatie opleveren. De staptest wordt weergegeven in de figuur hieronder.
98
Figure 84: Identificatie
Conclusie 1 – In het gebied aangeduid met het cijfer 4 zien we dat de afstand in een verhoogde snelheid verkleint. Dit is het gevold van de puls in de rechter input, die dient om de robot op een recht pad te houden. De puls is het gevolg van toename van de afstand die te zien is in de cirkel met cijfer 2, die het gevolg is van een zijdelingse beweging van de robot. De tijd tussen de puls (3) en de reactie (4) bedraagt 600ms, wat ons de delay time oplevert. We zien dit ook in de stapresponsie zelf. De afstand vermindert pas 600ms na het aanleggen van de stapinput. Conclusie 2 - Als we de hiervoor besproken effecten wegdenken en een gemiddelde helling in de afstand tekenen komen we uit dat ongeveer 200ms na het effect van de stapinput de snelheid reeds op de gewenste snelheid is. De tijdsconstante is dus bijgevolg 200ms. De statische versterkingsfactor in de eerste orde transfer functie kunnen we halen uit de motorkarakteristiek die we opmaten. Deze geeft exact het verband tussen de input en de steadystate werkelijke snelheid. De evenredigheidsfactor rond een input van 22 voor robot 2 bedraagt 0.017. De bekomen transfer functie ziet er als volgt uit: 𝐻 𝑠 =
0.017 . 𝑒 −0.6𝑠 1 + 0.2𝑠
Om de dode zone in de motorkarakteristiek en de offset die we geven in het controle algoritme enigszins ook te kunnen modelleren benaderen we motorkarakteristiek door een gefitte rechte, in de buurt van het werkingspunt, zoals ook te zien is in appendix 8. De rechte snijdt de x-as bij een waarde van 2.18. In het controle algoritme vermeerderen we de input met een waarde van 21, wat hier 99
overeen komt met een waarde van 18.82. We moeten nu nog de dode zone zelf invoeren, die zegt dat indien de input in absolute waarde kleiner is dan 18.82, de input gelijk gesteld wordt aan nul. Tot slot moeten we ook de conversie tussen ‘m’ en ‘pixels’ nog invoeren omdat de transfer functie een uitput geeft in ‘m/s’, maar de controle gebeurt in ‘pixels’. Het complete model voor de volgerrobot wordt hieronder weergegeven.
Figure 85: Geïdentificeerd model
Validatie van het geïdentificeerde model De kwaliteit van het geïdentificeerde model wordt getest aan de hand van de P-controller die we eerder praktisch implementeerden. Het simulatieresultaat wordt hieronder weergegeven, waarbij de input voor de volgerrobot nu de output van de leiderrobot is. De leiderrobot werd op dezelfde manier gelineariseerd.
Figure 86: Geïdentificeerd model 2 robots
100
Figure 87: Simulatie P-controller
De conclusie is dat de steady-state fout op de afstand tussen de twee robots en de reële snelheid (in m/s) voor de lagere input (correspondeert met een input 30 in de praktijk) van de leider gelijk is aan experimenteel opgemeten waarde. De fout en reële snelheid die worden gesimuleerd voor de hogere input (correspondeert met een input 40 voor de leiderrobot in de praktijk) komt helemaal niet overeen. Dit is het gevolg van het feit dat de modellen niet gelineariseerd zijn om dit werkingspunt, maar om het werkingspunt van de lagere input. Indien we de modellen voor beide robots lineariseren rond de hogere input voor de leiderrobot, wordt opnieuw een correcte steadystate fout gesimuleerd, maar niet meer voor de lagere input. We zien ook dat in de praktijk veel meer oscillaties en storingen voorkomen die we hier niet kunnen simuleren. Het dynamisch gedrag van de robots kan met dit model dus niet echt bestudeerd worden. Het statisch gedrag kan enkel bestudeerd worden in de buurt van het werkingspunt rond welk de robots zijn gelineariseerd. Uiteraard zijn er in de praktijk nog meer non-lineariteiten die hier niet gemodelleerd zijn, zoals de saturatie van de camera, de hogere tijdsconstante in het switchen tussen een snelheid gelijk aan nul en een snelheid verschillend van nul, saturatie van de motorkarakteristieken, storingen, vibraties,… 4.4.3. PI-controller In een vorige sectie hebben we besproken hoe we een P-controller implementeerden en kwamen we tot de conclusie dat bij een constante snelheid als referentie voor de leider, de afstand tussen de leiderrobot en volgerrobot een steady-state fout vertoont. We zullen daarom een tweede integrator introduceren in de regelkring, door middel van een PI –controller. We hebben een poging ondernomen om aan autotuner (Kaiser-Raika) toe te passen. Het resultaat van de relay-test was niet echt bruikbaar. De tweede poging bestond uit het toepassen van de theorie van Ziegler-Nichols omdat we de hoofdkenmerken van de robot geïdentificeerd hebben. 101
Vanwege de vele non-lineariteiten in ons systeem leidde dit ook tot een onstabiel gedrag in de realiteit. Het model is zoals eerder besproken enkel geldig in een kleine omgeving van het werkingspunt en is niet betrouwbaar voor dynamische verschijnselen, alsook omwille van de nonlineariteiten. Deze pogingen zijn besproken in de Engelse tekst van de scriptie, maar worden hier niet herhaald omwille van het feit dat ze geen goede uitkomst baden. De PI-controller werd dan handmatig getuned tot een goed resultaat bekomen werd. De transfer functie van de PI(D)-regelaar wordt hier weergegeven, met de conversie tussen de verschillende factoren en tijdsconstanten: 𝐼 𝐻 𝑠 = 𝑃 + + 𝐷. 𝑠 𝑠 𝑃 = 𝐾𝑝 𝐾 𝐼= 𝑝 𝑇 𝑖 𝐷 = 𝐾𝑝 . 𝑇𝑑 Het incrementele PID algoritme heeft de volgende vorm: 𝑢 𝑘 = 𝑢 𝑘 − 1 + 𝑐0 . 𝑒 𝑘 + 𝑐1 . 𝑒 𝑘 − 1 + 𝑐2 . 𝑒 𝑘 − 2 𝑇𝑠 𝑇𝑑 + ] 𝑇𝑖 𝑇𝑠 2𝑇𝑑 𝑐1 = −𝐾𝑝 [1 + ] 𝑇𝑠 𝑇𝑑 𝑐2 = 𝐾𝑝 𝑇𝑠
𝑐0 = 𝐾𝑝 [1 +
De waarde van P werd getuned tot 2.6 en de waarde van I werd getuned tot 0.7647. Het experimentele resultaat is hieronder weergegeven. De input van de leiderrobot wordt constant gehouden op een waarde van 32 links en 33 rechts. De pieken in de inputgrafiek van de leiderrobot, is omdat deze nu een uitgetekende rechte lijn op de vloer volgt, om ook de leider op een recht pad te houden.
102
Figure 88: Resultaat PI-controller
Conclusie 1: Constante snelheid – Opnieuw zien we dat de input van de volgerrobot gemiddeld naar een input gaat die correspondeert met dezelfde werkelijke snelheid van de leiderrobot (cfr. Bijlage 8 en P-controller). Conclusie 2: fout – In de grafiek die de afstand tussen de twee robots voorstelt (y-coördinaat van het centrum van de blob) zien we dat de afstand steeds naar nul geregeld wordt, wat de bedoeling is van de extra integrator in de lus, maar er ook steeds weer vandaan geslagen wordt. Dit gebeurt omwille van storingen, zoals de ruwheid van de vloer, de zijdelingse bewegingen, de lichtinval, … De controlelus brengt de afstand wel steeds terug naar de wenswaarde. Omwille van deze storingen, die de volgerrobot steeds even doen vertragen en dus een grotere afstand tussen de robots creëert, is er wel een gemiddelde fout (3 pixels) in de afstand die verschillend is van nul. We trachtten dit op te lossen door een grotere overshoot te geven aan de responsie (hogere P- en I-waarde van de controller), maar dit resulteert opnieuw in snelheden of input die gelegen zijn in de dode zone, met grotere vertragingen en grotere afstanden tot gevolg, zoals besproken werd bij de P-controller. Indien de waarde van de I-factor zeer groot genomen wordt, is de overshoot zodanig groot dat er tijd genoeg is om te switchen tussen inputwaarden over de dode zone heen. De gemiddelde fout is dan gelijk aan nul, maar grote oscillaties zijn hiervoor nodig, en deze zijn niet gewenst. Ook bij de PI-controller legden we een hogere snelheid of input aan de leiderrobot. Met dezelfde PIcontroller resulteert dit in een kleine steady-state fout op de afstand van ongeveer 5 pixels voor een input van 38 voor de leider. Deze steady-state fout is het gevolg van het feit dat we met dezelfde lineaire controller controleren in een ander werkingsgebied van het niet-lineaire systeem. Indien we
103
de P- en I-waarden ietwat aanpassen bekomen we dezelfde response als hiervoor besproken, maar bij een hogere snelheid.
4.5.Analyse van een formatie Nu we het tracking gedeelte, zijnde een formatie van 2 robots waarbij de ene de andere volgt, volledig onderzocht hebben en een controle met P- en PI-controller geïmplementeerd hebben, is het interessant de formatie uit te breiden naar 3 robots en het gedrag te bestuderen. We bedoelen hiermee dat alle robots volledig autonoom de metingen en de controle uitvoeren om de afstand tussen een bepaalde robot en de robot daarvoor zo goed mogelijk te controleren. We zullen bestuderen hoe dit resulteert op het behoud van de afstand tussen de derde robot, of ‘tweede volger’ en de eerste robot, de leider van de formatie. Om deze response voor te stellen, tellen we de afstand tussen de leider en eerste volger en de afstand tussen de eerste volger en tweede volger op bij elkaar, op hetzelfde tijdstip. Zoals geweten worden de afstanden hier steeds uitgedrukt in ‘pixels’, omdat hier ook de controle op gebeurt. De lengte van de robots zelf wordt bij deze bewerking gelijk aan nul verondersteld, aangezien dit toch maar een constante term verschil is en geen invloed heeft op het karakter van de karakteristieken die we willen bestuderen. Het optellen van de twee afstanden tot de totale afstand tussen de leider en de tweede volger vereist dat het optellen gebeurt op hetzelfde tijdstip om een betrouwbare meting voor te stellen. De tijdsvectoren van de doorgestuurde data van de twee volgers is niet exact hetzelfde omwille van kleine variaties in de sample time. Hiertoe bepalen we onze eigen sample time en interpoleren we tussen datapunten indien nodig. De code voor het genereren van deze grafieken kan terug gevonden worden in bijlage 10. Het is de bedoeling dat elke volger dezelfde tracking karakteristieken heeft in het volgen van een robot die een constante snelheid heeft. De parameters van de twee volgers zijn hiertoe verschillend, als gevolg van het verschil in de motorkarakteristieken. De karakteristieken zijn afgeleid uit de experimenten die we vroeger bespraken, alleen werden die nu gedaan met 3 robots. De resultaten worden hierna kort besproken voor de P-controller en de PIcontroller. 4.5.1. P-controller – formatie Het resultaat van het experiment wordt hieronder gegeven en besproken. We stellen de inputs aan de eerste robot (leider), de afstanden van de tweede robot (eerste volger) tot de eerste robot en de derde robot (tweede volger) tot de tweede robot, en de afstand van de derde robot tot de eerste robot voor. Op te merken is nog dat de setpoint voor elke volger iets anders is omwille van praktische verschillen in de hoogte van de blob en de camera op de drie verschillende robots.
104
Figure 89: Formatie P-controller
Conclusie 1: Delay tijd – We zien dat de karakteristieken voor de afstanden tussen 2 robots dezelfde zijn voor de eerste en tweede volger. De karakteristieken zijn echter wel over de delay time van de regelkring (600ms) verschoven. Conclusie 2: Steady-state fout – Zoals eerder besproken vertonen de karakteristieken bij de Pcontroller een steady-state fout afhankelijk van de statische versterking van de kring en de snelheid van de leider. De steady-state fout in de afstanden tussen 2 robots is steeds dezelfde (6 pixels). In de afstand tussen de tweede volger en de leider worden deze twee steady-state fouten gesuperponeerd wat resulteert in een dubbele steady-state fout van de tweede volger tot de leider van de formatie. Conclusie 3: Oscillaties – In beide karakteristieken die de afstand tussen twee naburige robots voorstellen zien we oscillaties. In de afstand tussen de tweede en de eerste volger zijn deze wel iets groter dan in de afstand tussen de eerste volger en de leider. Dit is het gevolg van het feit dat de eerste volger een robot moet tracken die aan een constante snelheid beweegt, terwijl de tweede volger een robot moet tracken die zelf al oscillaties heeft. Ten gevolge van de tijdsverschuiving van de twee karakteristieken worden deze oscillaties echter weer een beetje uitgedempt in de afstand tussen de tweede volger en de leider. 4.5.2. PI-controller – formatie In deze test werd 1 snelheid aangehouden, zoals ook eerder gedaan werd, en om de redenen die eerder besproken werden bij de PI-controller. Het resultaat van de test is hieronder weergegeven op dezelfde manier als bij de P-controller.
105
Figure 90: Formatie PI-controller
Conclusie 1: Delay time en steady-state fout– net zoals bij de P-controller is ook hier dezelfde delay time aanwezig, wat ervoor zorgt dat de karakteristieken van de twee afstanden een tijdsverschuiving hebben ten opzichte van elkaar. Hier heeft dit echter een serieuzer gevolg. Zoals in het gedeelte van de PI-controller besproken werd, brengt de controller de afstand steeds naar de setpoint, maar omwille van storingen wordt deze steeds weer weggeslagen wat een gemiddelde, maar geen constante, fout geeft in de karakteristiek. Door de tijdsverschuiving tussen de karakteristieken van de twee volgers, bereiken deze hun setpoint niet op hetzelfde moment, wat resulteert in het feit dat de afstand tussen de tweede volger en de leider nooit aan de setpoint geraakt. Dit resulteert in een werkelijke steady-state fout in deze response. Dit is een zeer interessant fenomeen en zal worden verder gezet indien men de formatie uitbreid. Dit is onvermijdelijk indien geen perfecte controle van elke robot mogelijk is en er geen communicatie tussen de robots is. Dit is dan ook het aanknopingspunt voor verder onderzoek.
5. Conclusies en verder onderzoek In het eerste deel hebben we twee modellen voor de kinematische eigenschappen van een leidervolger formatie en een mobiele robot op zichzelf bestudeerd. We hebben op één model een modelgebaseerde controle (NEPSAC) toegepast. Het resultaat was vooral goed in eer rechte lijn. Bij controle in 2 richtingen is de settling tijd veel hoger door het gekoppeld zijn van de verschillende inputs en outputs van het systeem. Deze controle kon niet praktisch geïmplementeerd worden door de gelimiteerde capaciteiten van de beschikbare hardware. 106
De praktische implementatie van een leider-volger formatie begon met het onderzoek van de beschikbare robots. We stelden vast dat er heel wat non-lineariteiten zijn in de motor karakteristieken, zoals een dode zone, saturatie en een minimum snelheid. De afstandsmeting tussen een robot en het te volgen object werd gerealiseerd door ‘blob detectie’, met behulp van de on-board camera. De coördinaten van de ‘blob’ staan in vast verband tot de afstand en de hoek waaronder de robot zich bevindt ten opzichte van de blob. Eerst werd er een controle ontworpen in 2 richtingen. Het te volgen object bestond hier uit een lamp. De controle van de twee outputs (afstand en hoek) werd ontkoppeld en twee P-controllers werden geïmplementeerd. Het resultaat was bevredigend. De ontkoppeling van de twee lussen is te zien in de response, maar is niet zeer significant. Het controle algoritme werd geïmplementeerd in verschillende software pakketten. We kwamen tot de conclusie dat het onmogelijk is een controle te implementeren over de Wi-Fi verbinding heen. De enige goede controle is mogelijk on-board op de robot. De beperkte capaciteiten van de on-board processor limiteren echter wel de toepassingen. In een volgende stap analyseerden en verbeterden we de controle in 1 richting, aan hogere en constante snelheden. Om onnauwkeurigheden in de hardware van de motors te compenseren implementeerden we toch een primitieve on/off controle op de hoek. In de voorwaartse richting werd eerst een P-controller geïmplementeerd. De volger volgt de leider aan dezelfde snelheid, maar met een steady-state fout. Om deze fout weg te werken verbeterden we de controle door het implementeren van een PI-controller. De reduceerde de gemiddelde fout sterk. De fout kan echter niet terug gebracht worden tot exact nul als gevolg van de limiterende non-lineariteiten en storingen. De controller brengt de afstand wel steeds terug tot de setpoint. In het laatste gedeelte brachten we er een derde robot bij, om een werkelijke formatie te realiseren. De robot werd aangestuurd met dezelfde controllers. Deze moest echter wel opnieuw getuned worden als gevolg van een verschillende motorkarakteristiek. Vooral de afstand tussen deze derde robot, of tweede volger, en de eerste robot, of leider, werd bestudeerd. Bij een P-controller worden de steady-state fouten op de afstanden tussen alle robots gesuperponeerd. Bij een PI-controller ontstaat er ten gevolge van de delay tijd in beide volger robots en het oscillerende gedrag een werkelijke steady-state fout op de afstand tussen de tweede volger en de leider. Dit veroorzaakt een accordeon effect, wat ook in werkelijke verkeerssituaties geobserveerd wordt. Dit is ten gevolge van de afwezigheid van communicatie tussen de verschillende volgers en een ‘imperfecte’ tracking controle. Zoals blijkt uit de resultaten kan de controle van een formatie van mobiele robots nog zeker en vast dienst doen voor verder onderzoek. De problematiek bestaat uit twee thema’s, zijnde het tracken van een robot, of een twee-robot formatie en de mogelijkheid tot communicatie om de volledige formatie te verbeteren. Het grootste knelpunt in de tracking problematiek is zijn de niet-lineaire 107
motoren wegens een dode zone, verschillende tijdsconstanten en saturatie, en de openloop controle ervan. Hieronder worden kort een aantal ideeën opgesomd die een aanknopingspunt kunnen vormen voor verder onderzoek.
Neurale netwerken en fuzzy logic: Deze niet-lineaire controlestrategieën kunnen beter uitgerust om het niet-lineaire systeem dat de robots zijn te controleren. Ze kunnen misschien beter omgaan met de dode zone, maar zijn zeker beter geschikt in het saturatiegedeelte van de motorkarakteristieken. Ik denk hierbij aan snelheidsafhankelijke factoren in de controller.
Communicatie: Indien er communicatie mogelijk zou zijn tussen de verschillende robots, kan een volger sneller op de hoogte gebracht worden van het gebeuren ‘up-stream’ in de formatie,wat hem in staat moet stellen sneller en preciezer te reageren. Dit zal zeker het geval zijn indien de formatie uitgebreid wordt naar meer dan 3 robots.
Modellering van een formatie
NEPSAC voor betere controle in 2 richtingen: Zoals aangetoond werd, is de controle van de twee outputs gekoppeld. Met het kinematisch model en een modelgebaseerde controle zou de controle in 2D beter kunnen verlopen.
Verbeteren van de hardware componenten: motors, encoders, IMU-board, meer geavanceerde processor,…
Verbeteren
van
de
afstandsmeting:
Aanpassen
van
de
omgeving,
infrarood
afstandsmeting,…
Andere toepassingen: Met de robots kunnen ook andere toepassingen dan een leaderfollower formatie bedacht worden. Er kan een toepassing uitgedacht worden met collision avoidance, mappen van een regio,…
108
Appendices
109
APPENDIX 1: Matlab basic commands function surv=survConnect() surv = tcpip('169.254.0.10', 10001); set(surv, 'InputBufferSize', 30000); fopen(surv); fwrite(surv,'V'); pause(0.3); x=fscanf(surv); disp(x);
function survDisconnect(surv) fclose(surv); delete(surv); clear surv disp('Disconnected');
function setBin(surv, bin, ymin, ymax, umin, umax, vmin, vmax) command=strcat('vc',int2str(bin),int2str(ymin), int2str(ymax), int2str(umin),... int2str(umax), int2str(vmin), int2str(vmax)); fprintf(surv, command); disp('Bin set');
function [area center_x center_y data_read] = survGetBlob( surv, bin ) if get(surv, 'BytesAvailable')>0 fread(surv, get(surv, 'BytesAvailable')); end command=strcat('vb', int2str(bin)); fprintf(surv, command); pause(1); area=-1; center_x=-1; center_y=-1; data_read = 'vb1 0'; while (get(surv, 'BytesAvailable') > 0) data_read = fscanf(surv) ; end if length(data_read) <= 7 return % elseif strcmp(strcat('##vb', int2str(bin),' 0'), data_read(1:7)) % return % elseif strcmp(strcat('##vb', int2str(bin)), data_read(1:5)) % return end split = regexp(data_read,'\ ','split'); if length(split)<9 return end area = str2double(split(2)); x1 = str2double(split(4));
110
x2 = str2double(split(5)); y1 = str2double(split(6)); y2 = str2double(split(7)); center_x = (x1+x2)/2; center_y = (y1+y2)/2; end
function setResolution(surv, res) % res - 0 for 160x120 % - 1 for 320x240 % - 2 for 640x480 % - 3 for 1280x1024 switch res case 0 command='a'; fprintf(surv,command); case 1 command='b'; fprintf(surv,command); case 2 command='c'; fprintf(surv,command); case 3 command='d'; fprintf(surv,command); otherwise disp('Bad value for resolution'); end
function driveMotors(surv,left,right) left=convertRange(left); right=convertRange(right); command=[uint8('M'), uint8(left), uint8(right), uint8(0)]; fwrite(surv, command);
function [y] = twosComplement(x) if sign(x)==1 y=round(127/100*x); else y=round(255+126/100*x); end
111
APPENDIX 2: Matlab image processing %%% Blob detection in a fixed picture (saved on hard drive) % clear; % % % % % % % % % % % % % % %
I=imread('picture0003.jpg'); I2=rgb2gray(I); BW=roicolor(I2,240,260); se = strel('disk',10); BW=imopen(BW, se); imshow(BW) L = bwlabel(BW); props = regionprops(L, 'basic'); centroids = cat(1, props.Centroid); imshow(BW) hold on; plot(centroids(:,1), centroids(:,2), 'b*'); hold off;
%% Blob detection in video stream from SRV robot % surveyor = initializeRobot('169.254.010'); data_count = 0; while(data_count<=100) % Stop after 100 frames data = get_srv_image(surveyor); myWait(0.05); % imshow(data) x=rgb2gray(data); BW=roicolor(x,240,260); se = strel('disk',20); BW=imopen(BW, se); L = bwlabel(BW); props = regionprops(L, 'basic'); centroids = cat(1, props.Centroid); imshow(BW) hold on; %
plot(centroids(:,1), centroids(:,2), 'b*'); hold off; data_count = data_count + 1;
end
112
APPENDIX 3: P control script 2D control #define width 640 #define height 480 int ref_x; int ref_y; int a; int Count; int center_x; int center_y; int left; int right; int setoff; int t; int Kp_f; int Kp_r; int Ut_f; int Ut_r; int Et_r; int Et_f; vcolor(1, 255, 255, 128, 128, 128, 128); delay(10); ref_x = 320; ref_y= 300; Kp_f = 110; Kp_r = 8; for (Count = 1; Count <= 220; Count++){ vcap(); a=vblob(1, 0); if(a>0){ center_x=(blobx1+blobx2)/2; center_y=(bloby1+bloby2)/2; } else printf("No blobs detected\n"); Et_f = ref_y-center_y; Ut_f = Kp_f*Et_f/100; Et_r = ref_x-center_x; Ut_r = Kp_r*Et_r/100; if(center_x < ref_x){ left = 0; right = Ut_r; } if(center_x > ref_x){ left = -Ut_r; right = 0; }
if((center_y-ref_y > 8) || (center_y-ref_y < -8)){ left = left + Ut_f; right = right + Ut_f;
113
} if (center_y-ref_y > 0){ setoff = -18; } else setoff = 18; left = left+setoff; right = right+setoff;
if(left>35) left=35; if(left<-35) left=-35; if(right>35) right=35; if(right<-35) right=-35; t = time(); printf("%d %d %d %d %d\n",left, right, t, center_x, center_y);
motors(left,right); } motors(0,0); exit();
114
APPENDIX 4: Matlab script -Receiving data 1 robot surv=survConnect('169.254.0.10'); surv.Timeout = 30; fprintf(surv,'Q'); data = zeros(200,5); for i=1:200 temp = fscanf(surv) ; data(i,:) = sscanf(temp,'%d %d %d %d %d'); end survDisconnect(surv); figure(1) subplot(2,1,1); plot(1:length(data(:,1)),data(:,1),1:length(data(:,2)),data(:,2)); legend('left','right'); xlabel('Time (samples)'); subplot(2,1,2); plot(1:length(data(:,4)),data(:,4),1:length(data(:,5)),data(:,5)); legend('center_x','center_y'); xlabel('Time (samples)');
115
APPENDIX 5: Matlab control script width = 640; halfwidth = width / 2; ref_x = halfwidth; ref_y = 293; left_vector = []; right_vector = []; center_x_vector = []; center_y_vector = []; time_vector = []; gain_forward = 0.5; gain_rotation = 40; for i=1:200 tic left=0; right=0; area =-1; while area == -1 [area center_x center_y data_read] = survGetBlob(rob, 1); end; center_x_vector = [center_x_vector; center_x-ref_x]; center_y_vector = [center_y_vector; center_y-ref_y]; if abs(ref_y-center_y)<10 speed_forward = 0; else speed_forward = gain_forward*(ref_y - center_y) + sign(ref_y center_y)*17; end %
speed_forward = gain_forward*(ref_y - center_y)/ref_y; speed_rotation = gain_rotation*abs(ref_x-center_x)/ref_x; if center_x <= halfwidth left = speed_forward; right = speed_forward + speed_rotation; else left = speed_forward + speed_rotation; right = speed_forward; end if left>40 left = 40; elseif left<-40 left=-40; end; if right>40 right = 40;
116
elseif right<-40 right=-40; end; disp(left); disp(right); driveMotors(rob,left,right); disp('*****************'); left_vector = [left_vector; left]; right_vector = [right_vector; right]; time_vector = [time_vector; toc]; end driveMotors(rob,0,0); figure(1); hold on; subplot(3,1,1); plot(1:length(left_vector),left_vector(1:length(left_vector)),1:length(righ t_vector),right_vector(1:length(right_vector))); legend('motor left', 'motor right'); subplot(3,1,2); plot(1:length(center_x_vector),center_x_vector(1:length(center_x_vector)),1 :length(center_y_vector),center_y_vector(1:length(center_y_vector))); legend('center x - x ref', 'center y - y ref'); subplot(3,1,3); plot(1:length(time_vector),time_vector(1:length(time_vector))); legend('time_delay');
117
APPENDIX 6: C# control script using using using using using using using using using using using using using
System; System.Collections.Generic; System.ComponentModel; System.Data; System.Drawing; System.Linq; System.Text; System.Windows.Forms; System.Net.Sockets; System.IO; System.Threading; System.Text.RegularExpressions; System.Diagnostics;
namespace Surv3 { public partial class Form1 : Form { TcpClient client; NetworkStream stream; public Form1() { InitializeComponent(); } private void button1_Click(object sender, EventArgs e) { bool connected = true; Int32 port = 10001; String server = "169.254.0.10"; byte[] myReadBuffer = new byte[1024]; try { client = new TcpClient(server, port); stream = client.GetStream(); } catch (ArgumentNullException err) { MessageBox.Show("ArgumentNullException: {0}"); connected = false; } catch (SocketException err) { MessageBox.Show("SocketException: {0}"); connected = false; } if (connected) { textBox3.AppendText("Connected succesfully\r\n"); SendMessage("c"); //textBox3.AppendText("Resolution set\r\n"); stream.Flush(); Thread.Sleep(500); SendMessage("vc1255255128128128128"); stream.Flush();
118
//Thread.Sleep(200); //int numberOfBytesRead = stream.Read(myReadBuffer, 0, myReadBuffer.Length); textBox3.AppendText("Bin 1 set to white\r\n"); textBox3.AppendText("Pause of 2 seconds\r\n"); Thread.Sleep(2000); Connect_btn.Enabled = false; button2.Enabled = true; button3.Enabled = true; checkBox1.Enabled = true; button4.Enabled = true; textBox1.Enabled = true; textBox2.Enabled = true; textBox3.AppendText("Ready\r\n"); } else textBox3.AppendText("Failed to connect"); } private void button2_Click(object sender, EventArgs e) { stream.Close(); client.Close(); Connect_btn.Enabled = true; button2.Enabled = false; button3.Enabled = false; checkBox1.Enabled = false; button4.Enabled = false; textBox2.Enabled = false; textBox1.Enabled = false; textBox3.AppendText("Disconnected\r\n"); }
void SendMessage(String message) { Byte[] data = System.Text.Encoding.UTF8.GetBytes(message); stream.Write(data, 0, data.Length); } private void checkBox1_CheckedChanged(object sender, EventArgs e) { byte[] myReadBuffer = new byte[1024]; int numberOfBytesRead; if (checkBox1.Checked) { SendMessage("l"); textBox3.AppendText("Lasers on\r\n"); } else { SendMessage("L"); textBox3.AppendText("Lasers off\r\n");
119
} stream.Flush(); Thread.Sleep(50); numberOfBytesRead = stream.Read(myReadBuffer, 0, myReadBuffer.Length); } private void button3_Click(object sender, EventArgs e) { double left = double.Parse(textBox1.Text); double right = double.Parse(textBox2.Text); driveMotors(left, right); } void driveMotors(double left, double right) { sbyte l=(sbyte)left; sbyte r=(sbyte)right; byte[] myByteArray = { 77, convertRange(l), convertRange(r), 0 }; stream.Write(myByteArray, 0, myByteArray.Length); stream.Flush(); } private byte convertRange(sbyte x) { byte y = 0; if (Math.Sign(x) == 1) y = (byte)Math.Round(127 / 100 * (double)x); if (Math.Sign(x) == -1) y = (byte)Math.Round(255 + 126 / 100 * (double)x); return y; } private Blob readBlob() { byte[] myReadBuffer = new byte[1024]; StringBuilder myCompleteMessage = new StringBuilder(); int numberOfBytesRead = 0; string[] parts; char[] delimiters = new char[] { '#', ' ', '-', '\n', 'v', 'b', 'M' }; Blob b = new Blob(); Boolean found = false; string value; int counter = 0; SendMessage("vb1"); stream.Flush(); //Thread.Sleep(180); while (!found) { //SendMessage("vb1"); //stream.Flush(); //Thread.Sleep(10); numberOfBytesRead = stream.Read(myReadBuffer, 0, myReadBuffer.Length); myCompleteMessage.AppendFormat("{0}", Encoding.ASCII.GetString(myReadBuffer, 0, numberOfBytesRead)); value = myCompleteMessage.ToString(); //MessageBox.Show(value);
120
parts = value.Split(delimiters, StringSplitOptions.RemoveEmptyEntries); if (parts.Length > 3) if (Int16.Parse(parts[1]) > 0) { b.Area = Int32.Parse(parts[2]); b.Center_x = (Int32.Parse(parts[3]) + Int32.Parse(parts[4])) / 2; b.Center_y = (Int32.Parse(parts[5]) + Int32.Parse(parts[6])) / 2; found = true; } //counter++; //if (counter == 2) //{ // b.Area = -1; // b.Center_x = -1; //b.Center_y = -1; //found = true; //} } return b; } class Blob { private int area = -1; private int center_x = -1; private int center_y = -1; private long time; private double left; private double right; public int Area { get { return area; } set { area = value; } } public int Center_x { get { return center_x; } set { center_x = value; } } public int Center_y { get { return center_y; } set
121
{ center_y = value; } } public long Time { get { return time; } set { time = value; } } public double Right { get { return right; } set { right = value; } } public double Left { get { return left; } set { left = value; } } public override String ToString() { return time.ToString() + " " + center_x.ToString() + " " + center_y.ToString() + " " + left.ToString() + " " + right.ToString() + "\n"; } } private void button4_Click(object sender, EventArgs e) { int MAX = 50; int ref_x; int ref_y; int Count; int center_x; int center_y; double left = 0; double right = 0; int setoff; int t; List
list = new List(); double Ut_f; double Ut1_f; int Et_f; int Et1_f; int Et2_f; double Ut_r; int Ut1_r; int Et_r; int Et1_r; int Et2_r; ref_x = 320;
122
ref_y = 280; Blob b; textBox3.AppendText("Control started\r\n"); Stopwatch st = new Stopwatch(); st.Start(); for (Count = 1; Count <= 50; Count++) { b = readBlob(); if (b.Area != -1) { Et_f Ut_f Et_r Ut_r
= = = =
ref_y - b.Center_y; Et_f * 0.7; ref_x - b.Center_x; Et_r * 0.3;
if (b.Center_x < ref_x) { left = 0; right = Ut_r; } if (b.Center_x > ref_x) { left = -Ut_r; right = 0; }
if (b.Center_y - ref_y > 0) { setoff = -21; } else setoff = 21;
if ((b.Center_y - ref_y > 10) || (b.Center_y - ref_y < -10)) { left = left + Ut_f; right = right + Ut_f; } left = left + setoff; right = right + setoff;
if (left > MAX) left = MAX; if (left < -MAX) left = -MAX; if (right > MAX) right = MAX; if (right < -MAX) right = -MAX; } else { left = 0;
123
right = 0; } //driveMotors(left, right); driveMotors(0, 0); b.Left = left; b.Right = right; b.Time = st.ElapsedMilliseconds; list.Add(b); textBox3.AppendText("Blob: " + b.ToString() + "\r\n"); textBox3.AppendText(left.ToString() + " " + right.ToString() + "\r\n"); } //driveMotors(0, 0); TextWriter tw = new StreamWriter("date.txt"); foreach (Blob b1 in list) { tw.WriteLine(b1.ToString()); } tw.Close(); textBox3.AppendText("Control finished\r\n"); } private void button1_Click_1(object sender, EventArgs e) { Form2 form = new Form2(); form.Stream = stream; form.Client = client; form.Show(); } } }
124
APPENDIX 7: on-board C control script int int int int int int int int int int int int int int int
ref_x; ref_y; a; Count; center_x; center_y; left; right; setoff; t; Kp_f; Ut_f; Ut_r; Et_f; center_y1;
vcolor(1, 140, 210, 80, 115, 120, 150); delay(10); ref_x = 80; ref_y = 35; Kp_f = 40; center_y = ref_y; for (Count = 1; Count <= 1500; Count++){ center_y1 = center_y; vcap(); a=vblob(1, 0); if(a>0){ center_x = (blobx1+blobx2)/2; center_y = (bloby1+bloby2)/2; } else{ center_y = ref_y; center_x = 160; } Et_f = center_y-ref_y; Ut_f = Kp_f*Et_f/100; if (center_y-ref_y > 0){ setoff = 21; } else setoff = -21;
if((center_y-ref_y > 1) || (center_y-ref_y < -1)){ left = Ut_f + setoff; right = Ut_f + setoff; } else{ left = 0; right = 0;
125
} if((center_x-ref_x < -10)&&(left > 0)){ right = right + 15; } else if ((center_x-ref_x < -10)&&(left < 0)){ right = right + 0; } else if ((center_x-ref_x > 10)&&(left > 0)){ left = left + 15; } else if ((center_x-ref_x > 10)&&(left < 0)){ left = left + 0; } if(left>50) left=50; if(left<-50) left=-50; if(right>50) right=50; if(right<-50) right=-50; t = time(); printf("%d %d %d %d %d\n",left, right, t, center_x, center_y);
motors(left,right); } motors(0,0); exit();
126
APPENDIX 8: Robot motor characteristics ROBOT 1 (169.254.0.10)
ml = mr 21 25 27 29 31 33 35 40
Exp 1 (s) 4,22 3,29 2,94 2,81 2,38 2,43 2,43 2,21
Straight line (1m) Exp 2 (s) Exp 3 (s) 4,13 4,08 3,05 3,17 2,87 2,93 2,61 2,6 2,54 2,6 2,41 2,23 2,35 2,24 2,37 2,26
Average (s) 4,143333 3,17 2,913333 2,673333 2,506667 2,356667 2,34 2,28
Speed (m/s) 0,241351569 0,315457413 0,343249428 0,374064838 0,39893617 0,424328147 0,427350427 0,438596491
0,5 Speed (m/s)
0,4 0,3 0,2 0,1 0 0
10
20
30
40
50
Speed (m/s)
Input 0,45 0,4 0,35 0,3 0,25 0,2 0,15 0,1 0,05 0
y = 0,015x - 0,085
0
5
10
15
20
25
30
35
Speed (m/s)
Input 0,44 0,438 0,436 0,434 0,432 0,43 0,428 0,426 0,424 0,422
y = 0,002x + 0,355
0
10
20
30 Title (s)
40
50
127
ROBOT 2 (169.254.0.11)
ml = mr 20 22 24 26 28 30 33 35
Straight line (1m) Exp 2 (s) Exp 3 (s) 3,25 3,16 2,63 2,85 2,48 2,63 2,69 2,72 2,38 2,42 2,26 2,44 2,11 2,11 2,02 2,1
Exp 1 (s) 3,19 3,07 2,73 2,74 2,31 2,13 2,21 2,06
Average (s) 3,2 2,85 2,613333 2,716667 2,37 2,276667 2,143333 2,06
Speed (m/s) 0,3125 0,350877193 0,382653061 0,36809816 0,421940928 0,439238653 0,466562986 0,485436893
0,6 Speed (m/s)
0,5 0,4 0,3 0,2 0,1 0 0
5
10
15
20
25
30
35
40
Input
0,5 Speed (m/s)
0,4
y = 0,017x - 0,037
0,3 0,2 0,1 0 19
20
21
22
23
24
25
Input
0,49 y = 0,009x + 0,167
0,48 0,47 0,46 0,45 0,44 0,43 0,42 0,41 0
5
10
15
20
25
30
35
40
128
ROBOT 3 (169.254.0.12)
ml = mr 17 19 21 23 25 28 30 35 40
Straight line (1m) Exp 2 (s) Exp 3 (s) 3,56 3,54 3,18 3,12 2,65 2,81 2,51 2,41 2,37 2,28 2,21 2,26 2,13 2,17 1,9 1,94 1,88 1,92
Exp 1 (s) 3,63 3,34 2,86 2,72 2,3 2,2 2,09 1,83 1,82
Average (s) 3,576667 3,213333 2,773333 2,546667 2,316667 2,223333 2,13 1,89 1,873333
Speed (m/s) 0,279589935 0,31120332 0,360576923 0,392670157 0,431654676 0,449775112 0,469483568 0,529100529 0,533807829
0,6
Spped (m/s)
0,5 0,4 0,3 0,2 0,1 0 0
10
20
30
40
50
Input 0,5 y = 0,019x - 0,049
Speed (m/s)
0,4 0,3 0,2 0,1 0 0
5
10
15
20
25
30
Speed (m/s)
Input 0,54 0,53 0,52 0,51 0,5 0,49 0,48 0,47 0,46 0,45 0,44
y = 0,011x + 0,128
0
5
10
15
20 Input
25
30
35
40
129
APPENDIX 9: on-board PI-controller script int int int int int int int int int int int int int int int int int int int
ref_x; ref_y; a; Count; center_x; center_y; left; right; setoff; t; C0; C1; C2; Ut_f; Ut1_f; Ut_r; Et_f; Et1_f; Et2_f;
vcolor(1, 140, 210, 80, 115, 120, 150); delay(10); ref_x = 80; ref_y = 38; center_y = ref_y; Et_f = 0; Et1_f = 0; Ut_f = 0; C0 = 341280; C1 = -340000; C2 = 0; for (Count = 1; Count <= 1400; Count++){ Et2_f = Et1_f; Et1_f = Et_f; Ut1_f = Ut_f; vcap(); a=vblob(1, 0); if(a>0){ center_x = (blobx1+blobx2)/2; center_y = (bloby1+bloby2)/2; } else{ center_y = ref_y; center_x = 80; } Et_f = center_y-ref_y; Ut_f = Ut1_f + C0*Et_f/100000 + C1*Et1_f/100000 + C2*Et2_f/100000;
130
if (Ut_f > 0){ setoff = 18; } else setoff = -18; left = Ut_f + setoff; right = Ut_f + setoff; if((center_x-ref_x < -10)&&(left > 0)){ right = right + 20; } else if ((center_x-ref_x < -10)&&(left < 0)){ right = right + 0; } else if ((center_x-ref_x > 10)&&(left > 0)){ left = left + 20; } else if ((center_x-ref_x > 10)&&(left < 0)){ left = left + 0; } if(left>70) left=70; if(left<-70) left=-70; if(right>70) right=70; if(right<-70) right=-70; t = time(); printf("%d %d %d %d %d\n",left, right, t, center_x, center_y);
motors(left,right); } motors(0,0); exit();
131
APPENDIX 10: Gathering data from formation surv1 = survConnect('169.254.0.10'); surv1.Timeout = 30; surv2 = survConnect('169.254.0.11'); surv2.Timeout = 30; surv3 = survConnect('169.254.0.12'); surv3.Timeout = 30; fprintf(surv1,'Q'); fprintf(surv2,'Q'); fprintf(surv3,'Q'); data1 = zeros(1500,3); data2 = zeros(1350,5); data3 = zeros(1350,5); for i=1:1500 temp1 = fscanf(surv1) ; data1(i,:) = sscanf(temp1,'%d %d %d'); end for i=1:1350 temp2 = fscanf(surv2) ; data2(i,:) = sscanf(temp2,'%d %d %d %d %d'); temp3 = fscanf(surv3) ; data3(i,:) = sscanf(temp3,'%d %d %d %d %d'); end survDisconnect(surv1); survDisconnect(surv2); survDisconnect(surv3); time_vector_1 = (data1(:,3)-data1(1,3))/1000; time_vector_2 = (data2(:,3)-data2(1,3))/1000; time_vector_3 = (data3(:,3)-data3(1,3))/1000; figure(1) subplot(2,3,1); plot(time_vector_1,data1(:,1),time_vector_1,data1(:,2)); legend('left','right'); ylabel('Robot 1'); xlabel('Time (s)'); subplot(2,3,2); plot(time_vector_2,data2(:,1),time_vector_2,data2(:,2)); legend('left','right'); ylabel('Robot 2'); xlabel('Time (s)'); subplot(2,3,3); plot(time_vector_3,data3(:,1),time_vector_3,data3(:,2)); legend('left','right'); ylabel('Robot 3'); xlabel('Time (s)'); % subplot(2,3,4); % plot(time_vector_1,data1(:,4),time_vector_1,data1(:,5)); % legend('center_x','center_y'); % ylabel('Robot 1'); % xlabel('Time (s)'); subplot(2,3,5);
132
plot(time_vector_2,data2(:,4),time_vector_2,data2(:,5)); legend('center_x','center_y'); ylabel('Robot 2'); xlabel('Time (s)'); subplot(2,3,6); plot(time_vector_3,data3(:,4),time_vector_3,data3(:,5)); legend('center_x','center_y'); ylabel('Robot 3'); xlabel('Time (s)');
amount_samples_1 = floor(time_vector_1(length(time_vector_1)))/0.05; amount_samples_2 = floor(time_vector_2(length(time_vector_2)))/0.05; amount_samples_3 = floor(time_vector_3(length(time_vector_3)))/0.05; amount_samples = min([amount_samples_1,amount_samples_2,amount_samples_3]); time_vector = zeros(amount_samples,1); value_vector_2 = zeros(amount_samples,1); value_vector_3 = zeros(amount_samples,1); for i = 1:amount_samples t = i*0.05; time_vector(i) = t; j1 = 1; j2 = 1; j3 = 1; while time_vector_1(j1)<=t; j1 = j1+1; end; while time_vector_2(j2)<=t; j2 = j2+1; end; while time_vector_3(j3)<=t; j3 = j3+1; end; value_vector_2(i) = (data2(j2,5)-data2(j2-1,5))/(time_vector_2(j2)time_vector_2(j2-1))*(t-time_vector_2(j2-1))+data2(j2-1,5); value_vector_3(i) = (data3(j3,5)-data3(j3-1,5))/(time_vector_3(j3)time_vector_3(j3-1))*(t-time_vector_3(j3-1))+data3(j3-1,5); end; figure(2) subplot(3,1,1); plot(time_vector,value_vector_2,time_vector,value_vector_3); legend('Robot 2','Robot 3'); ylabel('center_y'); xlabel('Time (s)'); subplot(3,1,2); plot(time_vector,value_vector_2); legend('center_y to leader'); ylabel('Robot 2'); xlabel('Time (s)'); subplot(3,1,3); plot(time_vector,value_vector_2+value_vector_3); legend('center_y to leader'); ylabel('Robot 3'); xlabel('Time (s)');
133
APPENDIX 11: Model m-file function [rho,psi,beta] = sim_model(v_F,w_F,v_L,w_L,X_0,Ts) % v_L = 0.2; % w_L = 0; Ns = 21; function dx = model(t,x) dx = zeros(3,1); dx(1) = cos(x(3)+x(2))*v_F - cos(x(2))*v_L; dx(2) = -sin(x(3)+x(2))/x(1)*v_F + sin(x(2))/x(1)*v_L - w_L; dx(3) = -w_F + w_L; end time_vector = []; for i = 0:Ns; time_vector = [time_vector i*Ts]; end; [T,X] = ode45(@model,time_vector,X_0); rho = X(:,1); psi = X(:,2); beta = X(:,3); % plot(T,X(:,1),'.',T,X(:,2),'*',T,X(:,3),'+'); end
134
APPENDIX 12: NEPSAC control script % NEPSAC control for 2 vehicles in leader-follower formation %% Initialisations clear; Ns = 1000; Ts = 0.18; N1 = 1; N2 = 2; Nu = 1; %% Initialisation matrices Na = 1; Nb = 0; U = []; for i = 1:N2+1; U = [U; 0.2 0]; end; X = []; for i = 1:N2+1+Na; X = [1 pi 0; X]; end; Inp = [0.2 0]; Inp_plot = Inp; U_delta_plot = [0 0]; Y_vector = [1 pi 0]; Ytk_base = []; R1 = []; R2 = for i = 1:N2; R1 = [R1; R2 = [R2; R3 = [R3; end;
[]; R3 = []; 1]; pi]; 0];
input_L_vector = []; %% Control loop for Tm = 1:Ns; % Leader inputs - disturbances if Tm<30 v_L = 0.2; w_L = 0; elseif Tm>=30 && Tm<40 v_L = 0.2; w_L = 0.1; elseif Tm>=40 && Tm<50 v_L = 0.2;
135
w_L = 0; elseif Tm>=50 && Tm<60 v_L = 0.2; w_L = 0; elseif Tm>= 60 v_L = 0.2; w_L = 0; end; input_L_vector = [input_L_vector; v_L, w_L]; % Process simulation Inp_dist = Inp; [Yt1 Yt2 Yt3] = sim_model(Inp_dist(1),Inp_dist(2),v_L,w_L,Y_vector(1,:),Ts); Yt = [Yt1(2) Yt2(2) Yt3(2)]; Y_vector = [Yt; Y_vector]; % Model prediction [Xt1 Xt2 Xt3] = sim_model(Inp(1),Inp(2),v_L,w_L,Y_vector(2,:),Ts); Xt = [Xt1(2) Xt2(2) Xt3(2)]; nt = Yt - Xt; X = [0 0 0; X((1:(N2+1+Na)-1),:)]; X(N2+1+1,:) = Y_vector(1,:); delta_u = [1; 1]; count = 0; limit = 50; while ((abs(delta_u(1))>1E-3) || (abs(delta_u(2))>1E-3)) && (count
136
G_21 = (psi2(N1+1:N2+1)-psi1(N1+1:N2+1))/step_size; G_31 = (beta2(N1+1:N2+1)-beta1(N1+1:N2+1))/step_size; [rho1, psi1, beta1] = sim_model(Inp(1),Inp(2),v_L,w_L,X(N2+1,:),Ts); [rho2, psi2, beta2] = sim_model(Inp(1),Inp(2)+step_size,v_L,w_L,X(N2+1,:),Ts); G_12 = (rho2(N1+1:N2+1)-rho1(N1+1:N2+1))/step_size; G_22 = (psi2(N1+1:N2+1)-psi1(N1+1:N2+1))/step_size; G_32 = (beta2(N1+1:N2+1)-beta1(N1+1:N2+1))/step_size; G1 = [G_11 G_12]; G2 = [G_21 G_22]; G3 = [G_31 G_32]; % Calculation optimal inputs delta_u = inv(transpose(G1)*G1 + transpose(G2)*G2 + transpose(G3)*G3)*[transpose(G1)*(R1-Ytk_base(:,1)) + transpose(G2)*(R2Ytk_base(:,2)) + transpose(G3)*(R3-Ytk_base(:,3))]; U(:,1) = U(:,1)+delta_u(1); U(:,2) = U(:,2)+delta_u(2); Inp = U(N2+1,:); count = count + 1; end; Ut = U(N2+1,:); Inp = Ut; Inp_plot = [Inp_plot; Ut]; U_delta_plot = [U_delta_plot; transpose(delta_u)]; end;
%% Plots Y_vector_plot = []; for i = length(Y_vector(:,1)):-1:1; Y_vector_plot = [Y_vector_plot; Y_vector(i,:)]; end; time_vector = []; for i = 0:Ns; time_vector = [time_vector; i*Ts]; end; figure(1); hold on; subplot(4,2,1); plot(time_vector, Y_vector_plot(:,1)); title('Output 1'); ylabel('Rho (m)'); xlabel('Time (s)'); subplot(4,2,3); plot(time_vector, Y_vector_plot(:,2)); title('Output 2'); ylabel('Psi (rad)'); xlabel('Time (s)');
137
subplot(4,2,5); plot(time_vector, Y_vector_plot(:,3)); title('Output 3'); ylabel('Beta (rad)'); xlabel('Time (s)'); subplot(4,2,7); plot(time_vector, U_delta_plot(:,1), time_vector, U_delta_plot(:,2)); title('Optimizing control inputs follower'); legend('Udelta v_F (m/s)','Udelta w_F (rad/s)'); xlabel('Time (s)'); subplot(4,2,2); plot(time_vector(1:length(time_vector)-1), input_L_vector(:,1)); title('Input leader 1 - distaurbance'); xlabel ('Time(s)'); ylabel('v_L (m/s)'); subplot(4,2,4); plot(time_vector(1:length(time_vector)-1), input_L_vector(:,2)); title('Input leader 2 - distaurbance'); xlabel ('Time(s)'); ylabel('w_L (rad/s)'); subplot(4,2,6); plot(time_vector, Inp_plot(:,1)); title('Input follower 1 - optimized'); xlabel ('Time(s)'); ylabel('v_F (m/s)'); subplot(4,2,8); plot(time_vector, Inp_plot(:,2)); title('Input follower 2 - optimized'); xlabel ('Time(s)'); ylabel('w_F (rad/s)');
138
APPENDIX 13: Leader control script int int int int int int int int int int int int int int int int int int int
ref_x; ref_y; a; Count; center_x; center_y; left; right; setoff; t; C0; C1; C2; Ut_f; Ut1_f; Ut_r; Et_f; Et1_f; Et2_f;
vcolor(1, 90, 120, 105, 125, 190, 220); delay(10); ref_x = 80; for (Count = 1; Count <= 1550; Count++){ if (Count<100){ left = 0; right = 0; } else if (Count>=100 && Count<400){ left = 32; right = 33; } else if (Count>=400 && Count<900){ left = 32; right = 33; } else if (Count>=900 && Count<7000){ left = 0; right = 0; } else if (Count>=7000 && Count<9000){ left = 0; right = 0; } else if (Count>=9000){ left = 0; right = 0; } vcap(); a=vblob(1, 0); if(a>0){ center_x = (blobx1+blobx2)/2; center_y = (bloby1+bloby2)/2;
139
} else{ center_y = ref_y; center_x = 69; } if((center_x-ref_x < -10)&&(left > 0)){ right = right + 20; } else if ((center_x-ref_x < -10)&&(left < 0)){ right = right + 0; } else if ((center_x-ref_x > 10)&&(left > 0)){ left = left + 20; } else if ((center_x-ref_x > 10)&&(left < 0)){ left = left + 0; }
t = time(); printf("%d %d %d \n",left, right, t);
motors(left,right); } motors(0,0); exit();
140
References [1]
L.E. Parker (2008). Multiple mobile robot systems. Springer handbook of robotics, 40:921941.
[2]
O. Ilaya, C. Bil, M. Evans (2008). Control design for unmanned aerial vehicle swarming. IMechE, 220:549-567.
[3]
W. Ren, R.W. Beard (2002). Virtual structure based spacecraft formation control with formation feedback. AIAA Guidance, Navigation, and control conference and exhibit 5-8 August 2002.
[4]
T. Paul, T.R. Krogstad, J.T. Gravdahl (2008). Modeling of UAV formation flight using 3D potential field. Simulation modeling practice and theory, 16:1453-1462.
[5]
W. Ren, N. Sorensen (2008). Distributed coordination architecture for multi-robot formation control. Robotics and autonomous systems, 56:324-333.
[6]
Z. Jin, R.M. Murray (2009). Stability and performance analysis with double-graph model of vehicle formations.
[7]
J. How, E. King, Y. Kuwata (2004). Flight demonstrations of cooperative control for UAV teams. AIAA 3rd ‘Unmanned Unlimited’ technical conference, workshop and exhibit 20-23 September 2004.
[8]
K. Mathia, G. Lafferrriere, A. Willems (2006). Cooperative control of unmanned vehicle formations. Euro UAV 2006 Conference and exhibition, 6-8 June 2006, Paris, France.
[9]
K. Mathia, G. Lafferrriere, T. Titensor (2007). Cooperative control of UAV platoons. Euro UAV 2007 Conference and exhibition, 13-15 June 2006, Paris, France.
[10]
Aveek K., James P., John Spletzer, Camillo J. Taylor (2002). A vision-based formation control framework. IEEE transactions on robotics and automation, vol. 18, no. 5, October 2002.
[11]
Maria Isabel Ribeiro, Pedro Lima (2002). Kinematic models of mobile robots. Instituto Superior Tecnico (IST), Portugal.
[12]
Gian Luca Mariottini, Fabio Morbidi (2007). Leader-follower formations: Uncalibrated visionbased localization and control. 2007 IEEE International conference on robotics and automatio, Roma, Italy, 10-14 April 2007.
[13]
J.L. Martinez, A. Mandow, J. Morales (2005). Approximating kinematics for tracked mobile robots. The international journal of robotics research, Vol. 24, No. 10, October 2005.
[14]
Guy Campion, Georges Bastin, Brigitte D’Andréa-Novel (1996). Structural properties and classification of kinematic and dynamic models of wheeled mobile robots.
[15]
Liu Shi-Cai, Tan Da-Long, Liu Guang-Jun (2007). Robust leader-follower formation control of mobile robots based on a second order kinematics model. ActaAutomatica Sinica, Vol. 33, No. 9, September 2007. 141
[16]
Aveek K. Rafael Fierro, John Spletzer (2002). A vision-based formation control framework. University of Pennsylvania (2002).
[17]
Takaaki Yamada, Keigo Watanabe, Kazuo Kiguchi (2001). Dynamic model and control for a holonomic omnidirectional mobile robot. Autonomous robots, 11, 173-189, 2001.
[18]
Fan Z., Koren Y. Wehe D. (1995). Tracked mobile robot control: hybrid approach. Control Engineering Practice 3(3): 329-336.
[19]
Ahmadi M., Polotski V. Hurteau R. (2000). Path tracking control of tracked vehicles. Proceedings of the IEEE International conference on robotics and automation, San Francisco, CA, pp. 2938-2943.
[20]
S.S. Stankovic, M.J. Stanojevic, D.D. Siljak (2000). Decentralized overlapping control of a platoon of vehicles. IEEE Transactions on control systems technology 8(5): 816-832, 2000.
[21]
Rafael Fierro, Peng Song, Aveek Das, Vijay Kumar (2000). Cooperative control of robot formations. Grasp Lab. University of Pennsylvania, Philadelphia PA, USA, 2000.
[22]
De Keyser R. (2003). A ‘Gent’le approach to predictive control.
[23]
www.surveyor.com
[24]
De Keyser R. Modelleren en regelen van dynamische systemen.
142
