Slides Mecatro 1a

Introduction to MechatronicsDiscover robots with the Lego Mindstorms

Loïc Cuvillon, Bernard Bayle

Ecole Nationale Supérieure de Physique de StrasbourgStrasbourg University

March 13, 2009

Introduction to Mechatronics 1 / 97

You are free to copy, to modify, to adapt and distribute this document.Latex sources of this document should be available on the author’s webpage.

LEGO and Mindstorms NXT are trademarks of the LEGO Group, which doesnot sponsor, authorize or endorse this document.


Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication

Part I

Hardware and Programming under Linux



Outline

1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware

2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API

3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT



Outline






References

NXC Programming

[NXC07] J.C. Hansen, NXT Power Programming, Variant Press, 2007

[NXC] J.C Hansen, Not eXactly C Programmer’s Guide,http://bricxcc.sourceforge.net/nbc/

[NXCT] D. Benedettelli. , Programming LEGO NXT Robots using NXC,http://bricxcc.sourceforge.net/nbc/nxcdoc/NXC_tutorial.pdf

NXT Bluetooth and USB communication

[LINXT] Lego Mindstorms NXT brick in Linux via USB,http://sourceforge.net/projects/linxt/

[BLUE] J. Schultz, Lego, Bluetooth and Linux, http://www.cs.uleth.ca/benkoczi/3720/data/NXT_Bluetooth_handout-jeremy.pdf

[NXTLIBC] Lego Mindstorms NXT Bluetooth library in C,http://www.quietearth.us/nxtlibc.htm



References

LEGO resources and documentation

[LEGO] Bluetooth Developer Kit and Hardware Developer Kit,http://mindstorms.lego.com/eng/Overview/NXTreme.aspx



Overview

Outline






Overview

Lego Mindstorms Overview

NXT InputsNXT Outputs

Sound Sensor

Touch Sensor

Light Sensor

Servo Motors

Ultrasonic Sensor

with position encoders

(contact switch)

(pressure in dB)(distance in cm)

(light intensity in %)

LCD screen

Buttons

NXT processorwith Bluetooth



Overview

NXT brick

battery level

NXT brick name(configurable)

USB 2.0 connectorbluetooth indicatorB : ON, invisible

B< : ON, visible

B<> : ON, connected

usb indicatorusb : connected, working

]−[ : connected, not working

loudspeaker

On/Enter button

Clear/Go back buttonNaviguation buttons

S4S1 S2 S3Sensor port

OUT_A OUT_B OUT_CServo outputs



Overview

NXT brick Menus

On/off

Connections

Visability

Searchto bluetooth device)(search and connect

Motor rotation

Sound dBA

Ultrasonic cm

Sound files

Software File(bytecode executableupload from computer)

(LCD display distance from sensor)

Delete Files

Volume

NXT version(firmware version)

NXT ProgramSettingsBluetoothMy files View (testing)

NXT Program Example :

= "Forward until an object detection, then turn left. Repeat"

NXT files

savedin



Overview

NXT-G (LabView)

advance graphical interface for programming NXT

functional block and control flow



Overview

Some fun NXT Projects

Rubik’s Cube Solverby Daniele Benedettelli

NXTway GSby Y. Yamamoto



NXT Brick Hard/Firm-ware

Outline







Hardware specification

S1 S2 S3 S4

−PWM signal generator

8bit AVR Co−Processor

−A/D converter for analogic sensor

(light,sound,touch )

USB 2.0 connector

−high speed communication line

on Port S4 (for future use)

RS485 UARTBluetooth Chip−class 2−10 meters range

Amtel 32 bit ARM Processor

−48MHz, 64KB RAM

−256KB FLASH

−I2C interface with AVR proc.and digital sensor (ultrasonic)

−single and double H−bridge

(OUT_A and OUT_BC)

LB1930 & LB1836

AT91SAM7S256




Hardware specification

I2C: Inter-IntegratedCommunication need 3 lines(data,clock,ground)

UART: Universal AsynchronousReceiver Transmitter(serial line/port)

Note:

-each sensor port (S1-4) can be usedwith digital or analog sensor.-6 wires by port: 3 for I2C + 3 foranalog signals




NXT Firmware

Memory

Boot Recovery(SAM−BA)

ROM

FLASH

RAM

64K

Firmware

(sound, .rxe...)

Files

Running executables

256K

Processor

Boot loader/boot code

LEGO firmwarewith CodeByte interpreter

Executables (.rxe)

Software

Boot code/loader

initialization of the hardware

load the firmware

Firmware : a basic operating system

multi-thread (thread: light task)

provides an API for I/O

(displays the menus)

interprets ByteCode executables(.rxe)

firmware and files stored inFLASH memory

BOOT CODE stored in RAMallow to update firmware



Outline






NBC/NXC basics

Outline






NBC/NXC basics

NBC/NXC

NBC: Next Byte Codes

name of an assembly language

name of an open source assembler available under MPL license

produce executable code for the native LEGO firmware

NXC: Not eXactly C

high level programming language: C-like syntax

compiler built upon nbc

produce ByteCode executables (.rxe)

NBC and NXC available for Linux, MAC OS, Windows

custom Firmwares to replace LEGO firmwares are available : LeJOS(JAVA-VM) for Java programming



NBC/NXC basics

Compilation and Execution

Compilation under linux

nbc compiler available from [NXC]

options of nbc available with ”nbc -help”

compilation of a source file *.nxc

nbc toto.nxc -O toto.rxe

Run execution

toto.rxe available in Program Files folder via menus after upload

start/stop via buttons

3−Upload of .rxe file

USB cable

1−Source editing

2−Compilation

4−Execution

Cross−compilation process



NBC/NXC basics

USB Upload

Detection of the NXT brick

”lsusb”: listing of connected USB device

Bus 002 Device 004: ID 0694:0002 Lego Group

via NXC

need complex configuration of linux device manager (udev)

upload command

nbc -d -S=usb toto.rxe

via LiNXT (prefered method, [LINXT])

LiNXT: Perl script with automatic search of the brick

command to upload, download and get NXT information

to get help ”linxt -h”

to upload a file (as root user):

linxt -u toto.rxe



NXC structure and statements

Outline







Statements 1

comments:

//This is a comment/ * This is a comment * /

variablesbool, byte, unsigned char, char 8 bit

unsigned int, short, int 16 bitunsigned long, long 32bit

string Array of bytestruct User defineArrays Array of any type

declaration of variable

int i;int x=0x10; // 16 in hexadecimalint y=10; // 10string msg=’’toto’’;




Statements 2

assignment

x=y+2;x+=y; // equivalent to x=x+y;

arrays (no pointers)

byte myvector[2]; // 2 byte arrayint myarray[]; // int array (no dimension)ArrayInit(myarray,0,10); // alocation of 10 null entriesmyarray[0]= 15;

conditions= equal!= not equal

truefalse

Expr True if Expr!=0&& logical AND between conditions|| logical OR between conditions




Control structures

if, if . . . else

if (x==1){x=2; z=3;}

elsey=4;

for

for (i=1; i<10; i++){

;}

while

while (x<10){x++ ; // something needed !!}

do.. while

do{ x++ ;}

while (x<10)




Control structures

switch

switch (x){

case 1 :z=3;break;

case 2 :// some codebreak;

default:// some codebreak;

}

repeat and until control structure available but not C standard




Program structure: Tasks

task music (){ PlayTone(440,3000); }

task motion (){

while(true){ OnFwd(OUT_A,50);

Wait(1000); }}

task main (){ // motion

start music; //and musicstart motion; //in parallel

} // execution

multithreading in NXT=concurrent execution of tasks

number of task and function mustbe < 256

at least an init task called ”main”needed

API

precedes(task1,task2,..);launch the task after end of main

start task1 or StartTask(task1) ;

StopAllTasks();




Program structure: Functions

parameter passed by value

return for non-void function

void foo (int x){x++;}

task main{

int y=1;foo(y);

//y still equal to 1}

parameter passed by reference

void foo (int &x) // !!{x++;}

task main{

int y=1;foo(y); //y now equal to 2foo(2); // ERROR

(one-variable)}




Preprocessor

Preprocessor directive

include files

#include ’’foo.h’’#include <foo.h> //error no libray path

macro and define (recommended!)

#define TIMEOUT 200 //0.2 ms timeout#define NORMAL_SPEED 50



NXC I/O API

Outline






NXC I/O API

some functions of the API

Timings

wait(2000);// sleep for 2s

x=CurrentTick();// sys time in ms

Math Functions

-Cos, Sin, Abs, Acos in degree-returns 100 times the value

x=cos(60); //returns 50

Strings

msg=numToStr(i)// convert number in string

msg=StrCat(str1,str2)// concatenation

Sound

Play tone (440,500);//440: frequency//500: duration



NXC I/O API

LCD API

task main (){int i=1;TextOut(0,LCD_LINE1,"toto:");

// display toto on line 1NumOut(20,LCD_LINE1,i);

//display i on line 1 and column 20Wait(3000);ClearScreen();LineOut(40,40,60,60);

//plot a line from (40,40) to (60,50) coordinatesWait(3000);

}



NXC I/O API

Output Control API : Servo-motor

Unregulated mode

no regulation (speed can vary according to the load)

reference = power in % , output selection= OUT_A, OUT_BC, OUT_ABC. . .

task main(){OnFwd(OUT_AC,85); //motor AC fwd at 85% of powerWait(1500); // run for 1.5 sOnRev(OUT_A,90); //motor A reverse at 90%,

//C still going fwd at 85%Float(OUT_C); //motot C power off, free decelerationWait(1500);Off(OUT_ABC); //motor ABC power off with braking

}



NXC I/O API


Regulated mode

PID regulation of the motor speed (power will increase if load increases)

reference = speed in % of maximal speed

task motor_status(){while(1)

NUMOUT(0,LCD_LINE1,MotorActualSpeed(OUT_A));//diplay not speed but power!!

}

task main() {start motor_status;

OnFwdReg(OUT_A, 40, OUT_REGMODE_IDLE);//same as OnFwd, no regulation

Wait(5000);OnFwdReg(OUT_A,40,OUT_REGMODE_SPEED); //speed regulat ionWait(5000);Off(OUT_A);StopAllTasks();

}



NXC I/O API


Synchronized mode

only for a pair of motors (OUT_xy)

synchronized the rotation (useful to go in straight line for mobile robot)

if one motor slow down, the other adapts

last parameter: steering or turn percentage

task main() {OnFwdSync(OUT_AC, 50, 0); //same as OnFwd, no regulationWait(2000);OnFwdSync(OUT_AC, 40 , 50); //motor A stopWait(2000);OnFwdSync(OUT_AC, 40 , 100);

//motor A speed opposite of CWait(2000);Off(OUT_AC);

}



NXC I/O API


Position-based mode

regulation of rotation position in degree

#define degree 180

task main() {RotateMotor(OUT_AC,56, degree);

//rotation of 180 degree at 56% of power}



NXC I/O API

Advance Output API : Write File

Position-based mode

wite written in flash memory

return value of the API: LDR_FILEEXIST, LDR_FILEISFULL,. . .

read file API : OpenFileRead(), Read()

task main() {byte handle;int result;result=CreateFile("test_write.rxe",256,handle);

//file of 256 bytesif (result==LDR_SUCCESS)

{ int i=3;result=Write(handle,i);

}CloseFile(handle);}



NXC I/O API

Input API : Sensors

Configuration of the input ports

1 a TYPE (TOUCH,SOUND. . . ) with SetSensorType(port,TYPE)2 a MODE (RAW, BOOL. . . ) with SetSensorMode(port,MODE)

TYPE DescriptionSENSOR_TYPE_TOUCH

SENSOR_TYPE_LIGHT_ACTIVE light sensor with LED onSENSOR_TYPE_LIGHT_INACTIVE light sensor with LED off

SENSOR_TYPE_SOUND_DB sound pressure in dBSENSOR_TYPE_TOUCH I2C sensor (Ultrasonic)

MODE DescriptionSENSOR_MODE_RAW value between 0 < x < 1024SENSOR_TYPE_BOOL 0 if x<512, else 1

SENSOR_TYPE_PERCENT value between 0 < x < 100SENSOR_TYPE_EDGE count number of transition

SENSOR_TYPE_TOUCH count number of level change



NXC I/O API

Input API : Sensors

Default configuration of the input ports

high-level functions with the default mode : SetSensorXXXX(port)

task main() {SetSensorType(S1,SENSOR_TYPE_TOUCH);SetSensorMode(S1,SENSOR_MODE_BOOL)SetSensorTouch(S1); //idem than the 2 previous line

SetSensorSound(S3);//Sound sensor on port 3, default pourcent?

SetSensorLight(S4); // port 4, default mode : percentSetSensorLowSpeed(S2);

// I2C sensor (Ultrasonic,..) on port 2}



NXC I/O API

Input API : Sensors and rotation

Reading

analog sensor: SensorValue(SX) or the equivalent macro SENSOR_X

for I2C ultrasonic sensor: SensorUS(port)

rotation count on servo_motor: MotorRotationCount(OUT_X)

task main() {bool cont; int dist;

SetSensorTouch(S1);SetSensorLowspeed(S2);ResetRotationCount(OUT_A);

while(1){ cont=SENSOR_1; // or cont=SensorValue(S1);

dist=SensorUS(S2);if (cont==1)

TextOut(0,LCD_LINE1,"Pressed");else TextOut(0,LCD_LINE1,"Release");if (MotorRotationCount(OUT_A) > 180)

TextOut(0,LCD_LINE2,"Half-rotation!"); } }



NXC I/O API

One example : 3 implementation

Move forward but avoid obstacles

#define NEAR 15 //cm

task main(){

SetSensorLowspeed(S4);

while(true){OnFwd(OUT_AC,50);

while(SensorUS(S4)>NEAR);//do nothing: waitOff(OUT_AC);OnRev(OUT_C,40); //turn

//angle linked to wait timeWait(800);

}

}


task main(){SetSensorLowspeed(S4);

while(true){

OnFwd(OUT_AC,50);

if (SensorUS(S4)<NEAR){ Off(OUT_AC);

OnRev(OUT_C,40);Wait(800); }

}}



NXC I/O API

One example : 3 implementation


mutex motor_mutex;

task obstacle_test(){ while(1)

{if (SensorUS(S4)<NEAR){Acquire(motor_mutex);Off(OUT_AC);OnRev(OUT_C,40);Wait(800);Release(motor_mutex);}

}}

task move(){

while(true){ Acquire(motor_mutex);OnFwd(OUT_AC,50);Release(motor_mutex); }}

task main(){SetSensorLowspeed(S4);start obstacle_test; start move; }

Multiple tasks

running in concurrency

mutex: mutual exclusion =preventthe 2 tasks to control motorssimultaneously

a task block on Acquire(), ifanother has already Acquire themutex → must wait for theRelease



Outline






Bluetooth Protocol

Outline






Bluetooth Protocol

Bluetooth

wireless protocol

for electronics (phone headset), computer peripherals (WiiMote,keyboard)

2.4 GHz short-range radio frequency bandwidth : unlicensed frequency

Class

class 1: 100 mW ≈ 100meters

class 2: 2.5 mW ≈ 10 meters (LEGO NXT)

Search and Pairing

search service available to find new Bluetooth device

pairing: trust communication by learning of a common passkey-input of the passkey on both device-key used to encrypt communication



Bluetooth Protocol

Bluetooth piconet

Master and slaves devices (max: 7 slaves)

on master by piconet (slaves synchronized on his clock)

slaves can not initiate a data packet transfer

slaves can send a response packet right after the master one(synchronous communication)

slaves can not communicate between them



Bluetooth Protocol

Bluetooth NXT Communication Protocol

NXT Direct Command (also for USB communication)

direct command interpreted by the NXT Slave firmware and translatedinto functions

any Bluetooth device (phone, PC, NXT,..) can send command viaBluetooth packets

NXT Direct Command Telegram

length of the datagram (without the 2 Length bytes)command type

0x00: Direct command, response required0x80: Direct command, no response0x02: Means a response datagram from slave

command code (Ex: 0x0B=GetBatteryLevel 0x03=PlayTone)

parameters of the command, or return value

byte 1

Length, LSB Length, MSBCommand

TypeCommand

byte 2 byte 3 byte 4 byte 5 byte 6 byt..

Parameters..................



Bluetooth communication with PC

Outline







Pairing between PC and NXT -1

Get NXT Bluetooth address

each Bluetooth as an unique address on 6 bytes (MAC address)

1 command hcitool : give name and Bluetooth address of available device

>hcitool scanScanning ...

00:16:53:04:B3:46 NXT

2 LiNXT -i , while NXT connected by USB

>sudo ./linxt -iDevice information:NXT name: NXT02Bluetooth address: 00:16:53:02:f3:31




Pairing between PC and NXT -2

Pairing under ubuntu Linux

1 set the key in the pop-up when using the following ”connect” program2 use NXT search and Linux Bluetooth-applet:

Linux: right click on the Bluetooth-applet > preferencesset a Bluetooth name for the PC dongleNXT: menu Bluetooth > searchselect your PC name and validatevalidate the default passkeyLinux: a pop-up ask for the passkey, give the same

3 it is a one time operation, NXT now appears in Bluetooth-applet >preferences > known devices




Programming of Direct Commands

Socket

a socket is an abstraction for an inter-process communication (overnetwork)

a socket can be bind to an IP or Bluetooth address

then we can read, write on the socket data between 2 process

Compilation with gcc

gcc -o executable_name -lm -lbluetooth source_name.c

to link with Bluetooth library: -lbluetooth (math: -lm)

Example: play_tone.c

opening of a socket bind to NXT Bluetooth address

write of the direct command PlayTone 0x03

read of the response (status)




play_tone.c

#include <stdio.h>#include <stdlib.h>#include <sys/socket.h> //socket lib#include <sys/types.h>#include <bluetooth/bluetooth.h> //bluettoh lib#include <bluetooth/rfcomm.h>#define max_message_size 59

int nxtsocket; //on declare la socket

void init_bluetooth(char * btAddress) {struct sockaddr_rc addr={0};//structure du type sockaddr _rcint status;

nxtsocket = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_R FCOMM);

addr.rc_family = AF_BLUETOOTH;addr.rc_channel = (uint8_t) 1;str2ba(btAddress, &addr.rc_bdaddr);// convertit un stri ng en adresse

status = connect(nxtsocket, (struct sockaddr * )&addr, sizeof(addr) );//connection }




play_tone.c

void nxt_tone(void) {/ ** 0x06 0x00 : length =6 bytes

0x00 : direct command with response0x03 : Playtone commande0x5B 0x1C : parameter 1 - 7259 (200-14000Hz)0x88 0x13 : parameter 2 - 0x1388=5000 (ms) ** /

char cmd[8]={0x06, 0x00, 0x00, 0x03,0x5B ,0x1C ,0x88,0x13 };char reply[max_message_size];

write(nxtsocket, cmd, 8); //send PlayTone command

read(nxtsocket, reply, 5); //read response

// check responseif (reply[2] != 0x02) fprintf(stderr, "not a response datag ramm");if (reply[3] != 0x03) fprintf(stderr, "not the reply to play tone");if (reply[4] != 0x00) fprintf(stderr, "status not ok");

}




play_tone.c

int main (void) {int blevel;char btaddress[18] = "00:16:53:02:f3:31"; //NXT adressinit_bluetooth(btaddress); //open connectionnxt_tone(); //direct commandclose(nxtsocket);return 0;

}




NXTlibC

A simple library to perform Direct Command

wrap the previous code in nice function

compilation:gcc -lm -lbluetooth -lnxtlibc test.c -o test

API of NXTlibC : samples

nxt_bluetooth_initialize() , nxt_bluetooth_done()

get_battery_level(),play_tone()

set_touch_sensor(), get_touch_value()

message_write(), message_read() (see next section)




play_tone.c with NXTlibC

#include <stdio.h>#include <nxtlibc.h>#define MAC "00:16:53:02:f3:31"

int main (void){int ret;

// * * * * * Connection Ã a la brique * * * * * //nxt_bluetooth_initialize(MAC);play_tone(500, 2000);

// * * * * * Verification de la batterie * * * * * //ret = get_battery_level();

// *** all motor at 10% of power in regulate mode ** //ret = _set_output_state(ALL_WHEEL,10,

MODE_MOTORON|MODE_BRAKE|MODE_REGULATED,REGULATION_MODE_MOTOR_SPEED|REGULATION_MODE_MOTOR_SYNC,0x00, MOTOR_RUN_STATE_RUNNING, 1000000);

nxt_bluetooth_done();

return 0; }



Bluetooth communication between NXT

Outline







Connection between NXT

Via the Bluetooth menu

search for others NXT

connect it on one of the free channel 1, 2 or 3 (up to 3 slave)

MasterChannel 0

Up to 3 slavesChannel 1−3

Nxt Piconet




NXC Bluetooth API

Via the Bluetooth menu

10 mailbox on each NXT to exchange messages

if there is no read program while sending = message lost

direct commands available to control the slave

Example of message exchange

master : check slave connection, send message to mailbox 5 on slave,read again message in mailbox 1 on slave,

slave : check master connection, read its mailbox 5,put an message in its mailbox 1




Message exchange

//MASTER#define INBOX 1#define OUTBOX 5

sub BTCheck(int conn){if (!BluetoothStatus(conn)==NO_ERR){

TextOut(5,LCD_LINE2,"Error");Wait(1000);Stop(true); }

}

task main(){string in,out="hello from master!";

BTCheck(1); //check slave connectionTextOut(10,LCD_LINE1,"Master Test");

SendRemoteString(BT_CONN,OUTBOX,out);

while (1){ ReceiveRemoteString(INBOX, true, in);TextOut(10,LCD_LINE3,in); }

}




Message exchange

//SLAVE#define INBOX 5#define OUTBOX 1


TextOut(5,LCD_LINE2,"Error");Wait(1000);Stop(true);}

}

task main(){string in,out="thanks, master!";

BTCheck(0); //check slave connectionTextOut(10,LCD_LINE1,"Slave Test");

while(1){ ReceiveRemoteString(INBOX, true, in);SendResponseString(OUTBOX,out); // Not SendRemote !!TextOut(10,LCD_LINE3,in); }

}




NXC Bluetooth API

Example of Direct Command

master : check slave connection,send play_tone command,wait for response status,send reset position on motor Await for response status,send full speed command on motor A with speed regulation




NXC Bluetooth API

//MASTER


TextOut(5,LCD_LINE2,"Error");Wait(1000);Stop(true); }

}

task main(){BTCheck(1);

RemotePlayTone(BT_CONN, 4000, 100);until(BluetoothStatus(BT_CONN)==NO_ERR);Wait(1100);

RemoteResetMotorPosition(BT_CONN,OUT_A,true);until(BluetoothStatus(BT_CONN)==NO_ERR);

RemoteSetOutputState(BT_CONN, OUT_A, 100,OUT_MODE_MOTORON+OUT_MODE_BRAKE+OUT_MODE_REGULATED,OUT_REGMODE_SPEED, 0, OUT_RUNSTATE_RUNNING, 0)

}


Mobile robots Wheeled Mobile Robots Technology Navigation

Part II

Introduction to Mobile Robotics



Outline

4 Mobile robotsClassification and applicationsIssues

5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors

6 NavigationLocalizationMappingPlanningControl



Outline






Classification and applications

Outline







Mobile robots classification

Mobile robots

Robots with a moving base, by opposition with robotic manipulators.

Classification based on the locomotion type.

Wheeled robots





Mobile robots



Legged robots





Mobile robots



Legged robots





Wheeled robots, legged robots+

flying robots, undersea robots.




Wheeled mobile robots applications

A few industrial applications

Automatic Guided Vehicles (AGV)FMC Technologies

http://www.fmcsgvs.com





More and more general public applications

ToysTribot, WowWee

http://www.wowwee.com

Automatic vacuum cleanersDirtyDog, iRobot

http://www.irobot.com





Education and research

Khepera II , K-teamhttp://www.k-team.com

1 h autonomy, 1 m/s max7x3 cm, 80g (payload<250g)

Multi-robot navigationSRI

http://www.ai.sri.com/centibots/index.html





A few High-Tech applications

Spatial exploration robotsSojourner mission to Mars

http://mars.jpl.nasa.gov/MPF/rover/sojourner.html





A few High-Tech applications

Inspectionhttp://crasar.csee.usf.edu



Issues

Outline






Issues

Issues

Technology

Issues:

mechanical architecture

embedded system

actuators

sensors

Navigation

Issues:

localization

mapping

planning

control



Outline






Mechanical architecture

Outline







Mobile robots mechanical architecture

A large amount of systems

several types of wheels

articulated platforms

caution: all the architectures arenot kinematically consistent

Lego Tribot: a simple differential drivemobile robot

simple to build: 2 fixed wheels,with 2 independent actuators

differential drive locomotion

nonholonomic, nonlinear. . . not sosimple




Mobile robots mechanical architecture

A large amount of systems

several types of wheels

articulated platforms

caution: all the architectures arenot kinematically consistent

Lego Tribot: a simple differential drivemobile robot

simple to build: 2 fixed wheels,with 2 independent actuators

differential drive locomotion

nonholonomic, nonlinear. . . not sosimple




Lego TriBot modelling

O

~y

~x

x

y

CIR

O′

L

ρ

ω

θ

v

vd

vg

ϕr

ϕl

Wheels velocities (!anglesconvention) :

vr = −r ϕr = (ρ + L)ω

vl = r ϕl = (ρ − L)ω

and then:

v =vr + vl

2

ω =−r(ϕr + ϕl)

2L

Differential kinematics:

x = v cos θ

y = v sin θ

θ = ω



Embedded system

Outline






Embedded system

Lego TriBot embedded system

NXT Block

The core of the robot, which enables to get measurements and to control theactuators.

Refer to the first part of the talk for more details. . .

NXT InputsNXT Outputs

Sound Sensor

Touch Sensor

Light Sensor

Servo Motors

Ultrasonic Sensor

with position encoders

(contact switch)

(pressure in dB)(distance in cm)

(light intensity in %)

LCD screen

Buttons

NXT processorwith Bluetooth



Actuators

Outline






Actuators

LEGO Mindstorms Servo-motor

Encoder wheel andwith optical sensor

Gear train (1:48)

Running_modes

ON: power controlopen loop controlreference: % of the max power

ON & REGULATED:speed regulationuse of position sensor signalsclosed loop with PID controller

ON & BRAKE: electronic braking

OFF: coasting (free motion)



Actuators

LEGO Mindstorms Servo-motor driving

O V

9 V

90% duty cycle

O V

9 V10% duty cycle

Power control

PWM (Pulse Width Modulation) ofa 8 kHz signal

90% duty cycle ≈ 8.1 V20% duty cycle ≈ 1.8 V

+Vcc

0V

In2

In2In1

M

In1

+−

In1=High, In2=Low In1=Low, In2=High

+Vcc

0V

In1 In2

In2In1

M+

−

Direction and braking

H-bridge with DC motorsIN1 IN2 ActionH L ForwardL H ReverseL L Brake



Actuators

Electronic braking

e

i

U

R

Lτ = Ki

ω = Ke

e : back e.m.f

τ : motor torque

ω : motor angular speed

Braking

1 motor motion: e 6= 02 short circuit current i ≃ − e

R

→ τ in opposition



Sensors

Outline






Sensors

Ultrasonic Telemeters

http://www.pages.drexel.edu/ kws23/tutorials/ultraso nic/ultrasonic.html

Properties:alternate emission/reception

sounds not heard by humanear (ultrasound=20kHz to200kHz)

minimal measurementdistance and variablemaximal frequency=f(maximaldistance)

different drawbacks(directivity, reflectivity,ambient conditions sensitivity)



Sensors

Lego TriBot Ultrasonic Telemeter



Outline






Localization

Outline






Localization

Dead reckoning

Odometry

Computation of the configuration by velocities integration:

x(t) =

Z t

0x(τ)dτ, y(t) =

Z t

0y(τ)dτ, θ(t) =

Z t

0θ(τ)dτ.

The TriBot example, with a Te sampling period:

x(k + 1) = x(k) + v(k)Te cos θ(k)

y(k + 1) = y(k) + v(k)Te sin θ(k)

θ(k + 1) = θ(k) + ω(k)Te

Remark

Odometry: current configuration relative to the initial configuration.



Mapping

Outline






Mapping

Environment mapping

Occupancy grid: discrete map of the environment

Cell decomposition, with probability of collision from sensors measurements.

The simplest case: binary grid, without filtering, trusting the odometrylocalization.



Planning

Outline






Planning

Configuration space

Definition

Configuration space = set of all the accessible configurations of the robot:

robot represented as a point: obstacles augmentation

robot with complex shapes and kinematics: more difficult problem. Overpessimistic obstacle augmentation can close doors !



Planning

Roadmaps

Visibility roadmap

Graph capturing the environment topology from the polygonal obstaclesvertices.

link initial point and goal to visible vertices



Planning

Roadmaps

Visibility roadmap


apply the same to the new vertices



Planning

Roadmaps

Visibility roadmap


search for a path in the graph



Control

Outline






Control

Path following problem

Problem

Find ω such that d(P, C) decreases to zero, with v imposed.

~x′

~y ′

O~x

O′

C

θr

Or

~xr

~yr

~y

P

d

a

θe

It can be showna that:

s =v cos θe − aω sin θe

1 − dc(s),

d = v sin θe + aω cos θe,

θe = ω − sc(s).

a|d c(s)|<1 under some adequateinitial conditions of the problem



Control

Path following control law

Distance equation:d = v sin θe + aω cos θe

It comes that:

ω = −v sin θe

a cos θe−

vcos θe

k(d , θe)d ,

with k(d , θe) > 0 such that k(d , ±π

2 ) = 0 gives:

d = −vak(d , θe)d .

Consequence

If a, v et k(d , θe) > 0: |d | decreases along any path.

Restrictions

No control of the orientation.

Can be solve at the planning step.


Documents

Slides Mecatro 1a