Acrob navigation in maze using neural network (Kundlová, Jurenka)
From RoboWiki
Konštrukcia
Backpropagation v Matlabe
function [J, grad] = nnCostFunction(nn_params, ...
input_layer_size, ...
hidden_layer_size, ...
num_labels, ...
X, y, lambda)
Theta1 = reshape(nn_params(1:hidden_layer_size * (input_layer_size + 1)), ...
hidden_layer_size, (input_layer_size + 1));
Theta2 = reshape(nn_params((1 + (hidden_layer_size * (input_layer_size + 1))):end), ...
num_labels, (hidden_layer_size + 1));
m = size(X, 1);
J = 0;
Theta1_grad = zeros(size(Theta1));
Theta2_grad = zeros(size(Theta2));
X = [ones(m, 1) X];
a1 = X;
z2 = a1*Theta1';
a2 = sigmoid(z2);
a2 = [ones(size(a2,1), 1) a2];
z3 = a2*Theta2';
a3 = sigmoid(z3);
h = a3;
y1 = zeros(size(y,1), num_labels);
for i=1:size(y)
y1(i, y(i)) = 1;
end
J = 1/m * sum(sum(((-y1).* log(h) - (1-y1).* log(1-h))));
temp1 = Theta1;
temp1(:,1) = zeros(size(temp1,1),1);
temp2 = Theta2;
temp2(:,1) = zeros(size(temp2,1),1);
J = J + lambda / (2*m) * ( ...
sum(sum(temp1 .*temp1)) + sum(sum(temp2.*temp2)) ...
);
for t = 1:m
a1 = X(t,:);
z2 = a1*Theta1';
a2 = sigmoid(z2);
a2 = [ones(size(a2,1), 1) a2];
z3 = a2*Theta2';
a3 = sigmoid(z3);
yk = zeros(num_labels,1);
yk(y(t)) = 1;
d3 = a3' - yk;
d2 = (Theta2'*d3).*[ones(1,1); sigmoidGradient(z2')];
d2 = d2(2:end);
Theta1_grad = Theta1_grad + d2*a1;
Theta2_grad = Theta2_grad + d3*a2;
end
Theta1_grad = Theta1_grad / m;
Theta2_grad = Theta2_grad / m;
Theta1_grad = Theta1_grad + ( (lambda/m) .* [ zeros( hidden_layer_size, 1 ), Theta1( :, 2:end ) ] );
Theta2_grad = Theta2_grad + ( (lambda/m) .* [ zeros( num_labels, 1 ), Theta2( :, 2:end ) ] );
grad = [Theta1_grad(:) ; Theta2_grad(:)];
end
Simulácia v Jave
Arduino
#include <Servo.h>
Servo LeftServo;
Servo RightServo;
double THETA_1[5][5] = {
{ -3.3015, -3.2262, 8.4318, -4.8450, 15.3742},
{ -4.7470, -0.3573, -9.7632, -9.3562, 18.5839},
{ -13.2009, 8.7361, 8.5763, 7.5276, 10.6005},
{ 7.0469, -10.8166, -9.0304, -4.0693, 13.2435},
{ 2.6464, -6.6205, -2.8116, 11.4895, 8.3321}
};
double THETA_2[5][6] = {
{ -12.1840, 25.3169, 27.4735, -23.5952, -0.5421, -2.3738},
{ -14.6229, -2.5615, -26.4061, -0.6488, 27.8756, 1.7607},
{ 12.8366, -16.8083, -3.1984, -14.8165, -10.5194, -16.8826},
{ -17.2910, -4.2473, -10.6967, 8.6676, -24.9129, 25.1679},
{ -12.0564, 3.1205, -3.9859, 21.9216, -4.5046, -25.5430}
};
char* ACTION_NAMES[]={
"ERROR",
"FORWARD",
"RIGHT",
"LEFT",
"FORWARD RIGHT",
"FORWARD LEFT"
};
double LINE_TRESHOLD = 0.5;
#define FW_SPEED 1.0
#define BIG_ROT_AMOUNT 1.0
#define SLOWDOWN 1
#define SMALL_ROT_AMOUNT 1
#define LEFT_SENSOR 3
#define RIGHT_SENSOR 2
#define CENTER_SENSOR 0
#define FRONT_SENSOR 1
#define TRESHOLD 500
double ACTIONS[6][2] = {
{0,0},
{FW_SPEED, 0},
{0, -BIG_ROT_AMOUNT },
{0, BIG_ROT_AMOUNT },
{/*FW_SPEED / SLOWDOWN*/0, -SMALL_ROT_AMOUNT},
{/*FW_SPEED / SLOWDOWN*/0, SMALL_ROT_AMOUNT}
} ;
#define INPUT_SIZE 4
#define Euler 2.71828182845904523536
#define THETA_1_ROWS 5
#define THETA_1_COLUMNS 5
#define THETA_2_ROWS 5
#define THETA_2_COLUMNS 6
int REPEAT_1[INPUT_SIZE] = {0,0,0,0};
int REPEAT_2[INPUT_SIZE] = {1,1,1,0};
int last = 1;
boolean compareArrays(int a[], int b[]){
for(int i =0 ; i < INPUT_SIZE; i++){
if(a[i] != b[i]){
return false;
}
}
return true;
}
void transpose(double * matrix, double * result , int rr, int cc){
for(int r =0 ; r < rr; r++){
for(int c =0 ; c < cc; c++){
result[c * rr + r] = matrix[r * cc + c];
}
}
}
int feed(int input[]) {
if (last != 1 && ( compareArrays(input, REPEAT_1) || compareArrays (input, REPEAT_2) )) {
return last;
}
double x[1][INPUT_SIZE];
for (int i = 0; i < INPUT_SIZE; i++) {
x[0][i] = input[i];
}
int m = 1;
double ones[m][1];
for (int i = 0; i < m; i++) {
ones[i][0] = 1;
}
double t1t[THETA_1_COLUMNS][THETA_1_ROWS];
transpose((double*)THETA_1, (double*)t1t , THETA_1_ROWS, THETA_1_COLUMNS);
double t2t[THETA_2_COLUMNS][THETA_2_ROWS];
transpose((double*)THETA_2, (double*)t2t, THETA_2_ROWS, THETA_2_COLUMNS);
double a0[m][INPUT_SIZE+1];
int params[6];
params[0] = m;
params[1] = INPUT_SIZE+1;
params[2] = m;
params[3] = 1;
params[4] = 0;
params[5] = 0;
combineMatrix((double*)ones, (double*)a0, params);
params[2] = m;
params[3] = INPUT_SIZE;
params[4] = 0;
params[5] = 1;
combineMatrix((double*)x, (double*)a0, params);
double h1[m][THETA_1_ROWS];
zeroMatrix((double*)h1, m, THETA_1_ROWS);
int mparams[4];
mparams[0] = m;
mparams[1] = INPUT_SIZE + 1;
mparams[2] = THETA_1_COLUMNS;
mparams[3] = THETA_1_ROWS;
multiply((double*)a0, (double*)t1t, (double*)h1, mparams);
sigmoid((double*)h1,(double*)h1 , m , THETA_1_ROWS);
double a1[m][THETA_1_ROWS+1];
params[0] = m;
params[1] = THETA_1_ROWS+1;
params[2] = m;
params[3] = 1;
params[4] = 0;
params[5] = 0;
combineMatrix((double*)ones, (double*)a1, params);
params[2] = m;
params[3] = THETA_1_ROWS;
params[4] = 0;
params[5] = 1;
combineMatrix((double*)h1, (double*)a1, params);
double h2[m][THETA_2_COLUMNS];
zeroMatrix((double*)h2, m, THETA_2_ROWS);
mparams[0] = m;
mparams[1] = THETA_1_ROWS + 1;
mparams[2] = THETA_2_COLUMNS;
mparams[3] = THETA_2_ROWS;
multiply((double*)a1, (double*)t2t, (double*)h2, mparams);
sigmoid((double*)h2,(double*)h2 , m , THETA_2_ROWS);
int actionIndex = maxValue((double*)h2, THETA_2_ROWS * m) + 1;
last = actionIndex;
return actionIndex;
}
double sigmoid(double * matrix, double * result, int rr, int cc){
for(int r =0 ; r < rr; r++){
for(int c =0 ; c < cc; c++){
result[r*cc + c] = 1.0/(1.0+exp(-matrix[r*cc + c]));
}
}
}
void copyMatrix(double * matrix, double * result, int rr, int cc){
int param[6];
param[0] = rr;
param[1] = cc;
param[2] = rr;
param[3] = cc;
param[4] = 0;
param[5] = 0;
combineMatrix(matrix, result, param);
}
void combineMatrix(double * matrix, double * result, int param[6]){
int lr = param[0];
int lc = param[1];
int sr = param[2];
int sc = param[3];
int ro = param[4];
int co = param[5];
for(int r =0 ; r < sr; r++){
for(int c =0 ; c < sc; c++){
result[(r + ro ) * lc + co + c] = matrix[r * sc + c];
}
}
}
int maxValue(double* A, int length){
int mValue = 0;
for(int i=0; i<length; i++){
if(A[mValue] <= A[i]){
mValue = i;
}
}
return mValue;
}
void multiply(double * A, double * B, double * C, int dim[4]) {
/*int mA = A.length;
int nA = A[0].length;
int mB = B.length;
int nB = A[0].length;*/
int mA = dim[0];
int nA = dim[1];
int mB = dim[2];
int nB = dim[3];
if (nA != mB){
Serial.print("Multiplication error ");
Serial.print(nA);
Serial.print(" ");
Serial.println(mB);
}
for (int i = 0; i < mA; i++){
for (int j = 0; j < nB; j++){
for (int k = 0; k < nA; k++){
C[i*nB+j] += (A[i*nA+k] * B[k*nB+j]);
}
}
}
}
void printMatrix(double * matrix, int rr, int cc){
for(int r =0 ; r < rr; r++){
for(int c =0 ; c < cc; c++){
Serial.print(matrix[r*cc+c]);
Serial.print(" ");
}
Serial.println();
}
Serial.println();
}
void zeroMatrix(double * matrix, int rr, int cc){
for(int r =0 ; r < rr; r++){
for(int c =0 ; c < cc; c++){
matrix[r*cc+c] = 0;
}
}
}
void setup() {
Serial.begin(9600);
int input[4];
input[0] = 0;
input[1] = 0;
input[2] = 0;
input[3] = 0;
int actionIndex = feed(input);
Serial.println(actionIndex);
LeftServo.attach(9);
RightServo.attach(10);
}
int convert(long val){
if(val >= TRESHOLD){
return 1;
}
return 0;
}
void fw(double left, double right){
double scale = 1;
int lAmmount = 90 - round(left*scale);
int rAmmount = 90 + round(right*scale);
Serial.print("move ");
Serial.print(lAmmount);
Serial.print(" ");
Serial.println(rAmmount);
LeftServo.write(lAmmount);
RightServo.write(rAmmount);
}
void loop() {
long leftVal = analogRead(LEFT_SENSOR);
long rightVal = analogRead(RIGHT_SENSOR);
long centerVal = analogRead(CENTER_SENSOR);
long frontVal = analogRead(FRONT_SENSOR);
Serial.print("left: ");
Serial.print(leftVal, DEC);
Serial.print(" right: ");
Serial.print(rightVal, DEC);
Serial.print(" center: ");
Serial.print(centerVal, DEC);
Serial.print(" front: ");
Serial.println(frontVal, DEC);
int input[4];
input[0] = convert(leftVal);
input[1] = convert(centerVal);
input[2] = convert(rightVal);
input[3] = convert(frontVal);
int action = feed(input);
Serial.println(ACTION_NAMES[action]);
double * move = ACTIONS[action];
double left = move[0] + move[1];
double right = move[0] - move[1];
fw(left, right);
delay(1000);
}
