This action might not be possible to undo. Are you sure you want to continue?
Per Ganestam - Nils Bengtsson August 21, 2009
2 . towards the vehicle. If the driver do not control the trailer it will become unstable and so to say. fall oﬀ. a PD controller is developed to stabilize a trailer when going in reverse.Abstract When reversing a vehicle with a trailer the driver has to take action to keep the trailer in a chosen path. If this is not taken care of in time the angle between the vehicle and the trailer will get more narrow and the joint connecting the vehicle and the trailer will probably break or even worse the trailer or the vehicle can take damage. With the help of a small remote controlled robot named RBbot. This report describes an automated control solution to this problem.
. . . . . . . . . . . .1 Mathematical model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .6 I2C . 2. . . . . . . . . 5 Conclusion 14 5. . . . . . . . . . . . . . . . 14 3 . . . . . . . . . . . . . . . . 3 The Model 3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1 Micro controllers . . . . . 4 4 5 5 5 5 5 5 6 6 6 7 8 8 10 11 11 11 11 11 12 12 13 . . . . . . . . . . . .1 Joystick . . . . . . . . . .1 Future improvements . . . . . . . . . 4. . . . . . . . . .5 Discretization 4. .4 Motor . . . . . . . . . . .7 Code extracts . . . . . . . . . . . . . . . . 2. . . . . 2. . . . . 2. . . . . . . . 2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1 ATMEL AVR 2. . . .4 Main node . . . . . . . . . . . . . . . 4. . . . . . . 2. . . . . . ATmega16 ATmega8 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .3 T-Mote Sky . . . . . . . . . . . . 4. . . .2 Contiki . . . . . . . . .1. . . . . . . . . . . . . . . . .1. . . . . . . . . . . . . . . . . . . . . . . . 2. 4 Implementation 4. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .7 Joystick . . . . .Contents 1 Introduction 2 RBbot 2. . . . . . . . . . . . . . . . . . .2 Simulink model . . . . . . . 4. . . . .5 Angle sensor . . . . . . . . . . . . . . . . . . . . . .3 Main node . . . . . . . . . . .3 Motors . . . . . . . . . . . . . . . . . . 3. .2 ATMEL AVR 2. . . . . . . . . . . . . . .6 Tuning . . . . . . . . . . .2 Angle sensor 4. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .1. . . . . .
There diﬀerent control ideas were tested with diﬀerent initial conditions and speeds to determine a controller and its parameters. In order to ﬁnd and implement a suitable controller to the stated problem a robot called RBbot is used. In the process of designing a controller for the RBbot. The problem with reversing a trailer is that it becomes an unstable system. If no action is taken by the driver the angle between the car and the trailer will narrow and eventually the only solution to straighten up the car and trailer is to go in forward direction. 2 RBbot Figure 1: RBbot with trailer and joystick 4 . The RBbot is a small three wheeled robot designed and constructed at the department of Automatic Control. This is the same as the well known problem of reversing a car with a trailer. LTH.1 Introduction In this project a controller that stabilize a vehicle with a trailer going in reverse has been found and implemented. a mathematical model was found and realized in Simulink. hence the trailer will fall towards the vehicle. It contains of a few programmable micro controllers and two individual controlled motor driven wheels as well as a remote joystick for steering. When a candidate to control the system was found it was implemented and further tested on the RBbot until a satisfying behavior was achieved. The RBbot used in this project was modiﬁed with a trailer connected to it and with an angle sensor in the joint.
These receive data to set a speed reference or return the current step position of the motors respectively. 2. It can react on a TCP/IP event triggered from the joystick or it will calculate a new control signal on a timer event. from now on called the main node. The one mounted on-board works as the main processor. All code is in C and compiled with the GCC compiler and a Contiki add on. some LED indicators and a wireless communication module. 5 . The main node has two diﬀerent events that can occur. The wireless communication use the TCP/IP protocol. The PI controller aﬀects the dynamics of the RBbot slightly but this is covered by the model for the main controller. A T-Mote Sky is a complete platform with a real-time operating system. 2. Also the I2C protocol is implemented in Contiki and ready to use.2.1. A timer will ﬁnish every 50 ms to create the control event. Hence the Rbbot and the trailer must be in line with each other and the joystick remain untouched during start up of the RBbot. do most of the work. Contiki works with processes that reacts on diﬀerent events.1 2. The other one is attached to the remote joystick. These also contain the wheel control which is a PI signal feedback controller and a low pass ﬁlter to follow the speed reference. a timer event or a TCP/IP event. Initially it calibrates both the joystick and the angle sensor. 2. The masters task is to read the angle sensor and send this data to the T-Mote Sky on the RBbot. one on the RBbot and one attached to the joystick.2 Contiki Since Contiki is a real-time operating system designed to some speciﬁc platforms. 2.g. there is already a lot of predeﬁned functions and hardware initializations. In addition it can also send information about the battery status and switch the two LEDs on the top of the RBbot on and oﬀ. To send and receive data between two T-Mote Sky platforms is trivial because of the built in communication functions.1 Micro controllers ATMEL AVR ATmega16 One ATMEL AVR ATmega16.1. in our case one called Contiki. among them the T-Mote Sky. A process can be triggered from e.3 T-Mote Sky There is two T-Mote Sky platforms.1. calculating the control signal and communicating with the three AVR micro controllers. called master. One for each motor. This make sure that a given speed is maintained without stationary errors.2 ATMEL AVR ATmega8 The RBbot use two ATmega8 micro controllers from ATMEL.3 Main node The T-Mote Sky on the RBbot. This one converts the analogue signals from the joystick to digital signals and transmit them to the RBbot. is mounted on the RBbot.
2. Since most data is larger than eight bit it has to be cut in smaller pieces before sending it over the I2C bus. some information about the angle between the Rbbot and the trailer is needed. 6 . There is one DC motor to each of the two wheels on the RBbot. To be sure that the speed reference is followed correct there is a position feedback from the motors with a resolution oﬀ 80 steps per revolution. 2.5 Angle sensor To control the RBbot in a chosen direction.6 I2C All communication between the T-Mote Sky and the AVR:s on the RBbot goes via an I2C bus. an angle between -90 and 90 degrees corresponds to a value between -270 and 270.4 Motor The motor is a DC step motor servo type and is programmed to follow a speed reference set by the user. Figure 2: One motor for each wheel 2. This is a simple but eﬃcient communication protocol that send eight bits of data at the time. All platforms and AVRs have the I2C protocol implemented. The angle sensor is a simple potentiometer and after calibration. This is done with an angle sensor in the joint where the trailer is attached to the RBbot.
7 Joystick The joystick has two axis. A dead zone around zero (in all directions) had to be implemented in the joystick to prevent none zero data to be sent when the joystick is centered. One of them switch the feedback on and oﬀ on the RBbot and the other activates the Indiana Jones theme song. 7 . This platform has an internal AD converter which read and translate the positions of the joystick with an x-value and a y-value each between negative one and one.Figure 3: The angle sensor in the joint between the RBbot and the trailer 2. This information is then sent through a wireless communication to the T-Mote Sky attached on the RBbot. one axis is for forward and reverse speed and one is for turning. The joystick also have two buttons implemented. The joystick is attached to a T-Mote Sky platform.
∆V . The ﬁrst three equations describe what happens with β when the connection joint move dx straight forward. cos(β − dβ)c = cos(β)c + sin(β)cos(β)dy (4) 1 cos(β)cos(dβ) + sin(β)sin(dβ) = cos(β) + sin(β)cos(β)dy (5) c 1 dβ = cos(β)dy (6) c The next two equations describe how the connection joint move when the RBbot drive. and their diﬀerence. as the turning speed which also is the control signal. dx = V (7) 8 . The RBbot movement can be divided into two problems. The speeds of the two wheels were not modelled individually but instead the average of the two were chosen as the speed. sin(β + dβ)c = sin(β)c + sin(β)cos(β)dx (1) 1 sin(β)cos(dβ) + cos(β)sin(dβ) = sin(β) + sin(β)cos(β)dx (2) c 1 dβ = sin(β)dx (3) c The following equations describe what happens with β when the connection joint move sideway. which is the case when the RBbot rotate.1 The Model Mathematical model The idea behind the model is to ﬁnd an equation that describe how β change when the RBbot move around. V .Figure 4: Joystick 3 3. straight forward and rotation.
b ∆V (8) a The angle β is also aﬀected by the rotation of the RBbot itself which add a third component to the ﬁnal equation. β) Robot to trailer angle. b) Robot arm. when moving with constant speed and without any turns. dβ = (9) V 1 b ˙ β = sinβ − (1 + cosβ)∆V (10) c a c After contemplated the above system it is clear that. were the inverted unstable pendulum correspond to a backward speed direction and the hanging down stable pendulum to a forward speed direction. It actually have some similarity with the pendulum. the system is stable in forward speed direction and unstable in backward speed direction. c) Trailer arm. dy = ∆V a Adding up all three dβ result in the ﬁnal state equation.Figure 5: a) Robot wheel distance center to center. The main problem with the project is also revealed here. when moving in reverse the trailer will ”fall oﬀ” from the intended direction if no compensation is made. 9 .
This is done by some step response experiments. Since the dynamics is diﬀerent at diﬀerent speeds a series of tests had to be done. To improve the model the dynamics of the motors had to be found. In the process of ﬁnding a good controller a range of various linear PID controllers were tested in diﬀerent starting angles and speeds. These experiments resulted in three PD controllers suitable in three speed intervals. Right: The arrow indicate the trailer center movement forward as the RBbot rotate and the connection joint move dy sideway. The reason why no I-part was needed was because of the integrator in the wheel control in the ATmega8s.2 Simulink model A ﬁrst simulation using the model given in the previous section resulted in an easily controlled system where only an error feedback was needed for stabilization. 10 . A ﬁrst order linear system was approximated to ﬁt the step response experiment data. starting with low speed and in steps increasing the speed. But this ﬁrst model was to simple to result in a suitable controller for the RBbot.Figure 6: Left: The arrow indicate the trailer center movement sideway as the connection joint move dx forward. 3. The experiments was done by driving the RBbot in the forward stable direction with the trailer connected. To ﬁnd the system corresponding to the motors dynamics a small program was implemented on the RBbot. measuring and logging the speed during the experiments.
4. The ﬁrst thing to be added was a dead zone compensation to remove unwanted signals due to the play in the stick.1 Joystick Included in the joystick source code was hardware initializations of the T-Mote Sky and an read out from the AD converter connected to the stick.2 Angle sensor The only source code to write on the master regarding the angle sensor was the I2C commands to transmit the angle value to the main node.4 Main node In the main node all data regarding angle. After this improvement. some base source code was obtained and used as a start for the implementation in this project. and joystick information is collected and processed. speed. 4.Figure 7: Simulink model 4 Implementation Regarding all programmable micro controllers. 11 . A part of the work process was to study and understand the existing codes. Initializations and read commands for the two buttons were written and at last the message structure containing all the joystick data was augmented with the button variables. redesigning and adding of new functions were done.3 Motors No changes had to be made in the code for the ATmega8 micro controllers due to some resent ambitious redesigning including a new PI controller combined with a low pass ﬁlter. The entire process is scheduled for 50 ms sample time. 4. 4. This also where the PD controller to stabilize the trailer is implemented.
Instead the implementation of the D-part used on the RBbot is a backward diﬀerence discretization with a low pass ﬁlter included. one for moving forward and one for reversing. 10] where a value of one is no ﬁlter at all. the D-part is a bit more problematic due to the fact that there is a lot of noise introduced when diﬀerentiating a discrete signal. D(tk ) = Td KTd N D(tk−1 ) − (y(tk ) − y(tk−1 )) Td + N h Td + N h (12) Where Td is the derivative constant.5 Discretization In order to implement a PD controller digital it has to be discretizised. However.Figure 8: Step response 4.6 Tuning The controller constants found from the simulation was used as a ﬁrst try to get a working controller on the RBbot. yref is the current reference value and y(tk ) is the angle in the joint between the trailer and the RBbot at time tk . One while standing still. 12 . 4. P (tk ) = K(yref (tk ) + y(tk )) (11) Where K is the proportional constant. These constants were not too bad and only some small changes had to be done in order to get the system tuned up in all three PD controllers. The P-part is trivial with the following discretization. The ﬁrst experiment implementing a controller with a D-part gave a to noisy control signal due to the lack of a low pass ﬁlter. K is the gain. The constant N is typically in the range [1. h is the sample time and N is the low pass ﬁlter constant. As expected some new problems were introduced on the RBbot that was not covered in the model.
One of these problems were that when a too rapid change in angle reference was done by the driver. When the speed is in the stable direction no compensation has to be done.8 m/s2 and for higher accelerations full control action transfer is applied. filter_reg = filter_reg . This made the system slightly slower but fast enough for convenient driving. if(ref_error >= 0) prop = 0.(filter_reg >> FILTER_SHIFT_REF) + filter_input. The control signal.8) prop = (ABS(acc) . angle_ref_filter = filter_output.1 − 0. The implementation of the D-part in the PD controller where on oﬀ is a help variable to prevent divide by zero.7143 give the ramp function the correct gain.5 . the control action is transfered to the retarding wheel. The controller were unable to keep the system stable and the trailer fell towards the RBbot. First the acceleration had to be messured.1) prop = 0. the proportion of control action transfered is decided by a ramp function. if(ABS(acc) < 0. The constant 0. filter_output = filter_reg >> FILTER_SHIFT_REF.7 Code extracts The following code is the low pass ﬁlter used on the angle reference value on RBbot. D = on_off*(Td*D_prev/(Td + N_filt*h) . is applied by increased speed on one wheel and decreased on the other. 4. hence the turning speed. 13 . the RBbot would overshout and then the compensation made by the driver caused driver-induced oscillations. In between.1)*0. else if(ABS(acc) < 0.7143. This was done by reading and diﬀerentiate the wheel speed and pass it through a low pass ﬁlter for noise reduction. This piece of code is the implementation of the acceleration compensation. else prop = 0.prop.0. The chosen solution to this problem was to implement a low pass ﬁlter on the angle reference signal. The ramp function is used between 0.5. In low acceleration no control action is transfered and in high acceleration all control action is transfered. Another problem was introduced during high acceleration. This is done in three steps. Secondly. The problem was due to saturation in acceleration which resulted in reduced control action on the wheel that tries to accelerate even more in speed direction. filter_input = angle_ref. To solve this problem two new functions had to be implemented. The variable prop spans from zero to one where zero is full control action on the left wheel and one is full control action on the right.(K*Td*N_filt) * (prev_ref_error-ref_error)/(Td + N_filt*h)).
5. In disadvance the lack of documentation made the learning process slow. also the way of deﬁning variables was diﬀerent to ordinary C-programming. turned out to good both for the discretization of the controller and the time to make all calculations and communications. if(speed > 0) prop = 0. A solution could be an adaptive control which increase the gain when there is more friction. The problems that was found later when implementing on the RBbot could have been covered by the model but perhaps with a greater eﬀort than testing and designing it on-board.1 Future improvements A possible improvement could be to put the acceleration estimations directly on the ATmega16 and improve the algorithm with a polynomial approximation if the processor resources is enough. This could be because of changes in the surface causing diﬀerent frictions. 50 ms. Contiki was a good help when using the T-Mote Sky.5. 5 Conclusion The controller managed to stabilize the trailer as intended in all possible speeds and accelerations. The main node T-Mote Sky failed to run the entire process with a sampling time of 10 ms which was tried at ﬁrst. 14 . During high acceleration the RBbot might change direction slightly. There was a big diﬀerence in the ability to drive the RBbot in reverse with the feedback switched on.else prop = 0. To get an even better controller a more advanced gain scheduling could be implemented with more controllers at diﬀerent speeds and with an interpolation between them. Many diﬀerent control designs and ideas could easily be tested to ﬁnd some suitable controllers. The ﬁnal choice of sampling time.5 + prop. It made the handle of threads. This could be solved with a feedback on the wheel positions to compensate for the error. There is a possible risk of stationary errors in the angle due to the lack of an integral part in the controller. ﬂoat numbers and TCP/IP communication easy. The Simulink model turned out to be useful in the level of complexity it was designed for.
This action might not be possible to undo. Are you sure you want to continue?