wellton/ardrone-autonomy
1
0
Fork 0
Código Issues Pull Requests Ações Pacotes Projetos Versões Wiki Atividade

Improving the PID Controller

Ver Código-Fonte 
This is a WIP, the controller needs more work,
but it is slowly getting there :-)

- Filter background noise in EKF
- Decouple goals (set goal per dof leaving others floating)
- Exeute control on navdata (to reduce delay)
- Improved PID coefficient (need more work)
Esse commit está contido em:
Laurent Eschenauer
2013-06-18 12:09:33 +02:00
pai 0b9d892f79
commit 7699fd9710
5 arquivos alterados com 96 adições e 87 exclusões
Baixar Arquivo de Patch Baixar Arquivo de Diff Expandir todos os arquivos Colapsar todos os arquivos
examples/repl.js
+23 -2
Ver Arquivo
@@ -1,9 +1,30 @@
var fs = require('fs');
var path = require('path');
var df = require('dateformat');
var arDrone = require('ar-drone');
var autonomy = require('..');
var client = arDrone.createClient();
var ctrl = new autonomy.Controller(client);
var repl = client.createRepl();
var ctrl = new autonomy.Controller(client, {debug: false});
var repl = client.createRepl();
// Add a ctrl object to the repl. You can use the controller
// from there. E.g.
// ctrl.go({x:1, y:1});
//
repl._repl.context['ctrl'] = ctrl;
// Log navdata and estimated state - for debugging
//
// var folder = df(new Date(), "yyyy-mm-dd_hh-MM-ss");
// fs.mkdir(path.join('/tmp', folder), function() {
// navStream = fs.createWriteStream(path.join('/tmp', folder, 'navdata.txt'));
// stateStream = fs.createWriteStream(path.join('/tmp', folder, 'state.txt'));
// });
//client.on('navdata', function(data) {
// navStream.write(JSON.stringify(data) + "\n");
// stateStream.write(JSON.stringify(ctrl.state()) + "\n");
//});
lib/Controller.js
+44 -67
Ver Arquivo
@@ -3,19 +3,8 @@ var PID = require('./PID');
var EKF = require('./EKF');
var Camera = require('./Camera');
CONTROL_FREQ = 100; // Interval in ms.
// Settings for controling translation motion
EPS_LIN = 0.1; // We are ok with 10 cm precision
KP_LIN = 0.15;
KI_LIN = 0.05;
KD_LIN = 0.05;
// Settings for controlling angular motion
EPS_ANG = 0.05; // We are ok with 0.05 rad precision (2.5 deg)
KP_ANG = 1;
KI_ANG = 0.1;
KD_ANG = 0.1;
module.exports = Controller;
function Controller(client, options) {
@@ -24,12 +13,11 @@ function Controller(client, options) {
this._state = options.state || {x: 0, y:0, z:1, yaw: 0};
this._tag = options.tag || {x: 0, y: 0, yaw: 0};
this._freq = options.control_freq || CONTROL_FREQ;
this._debug = options.debug || false;
this._pid_x = new PID({kp: KP_LIN, ki: KI_LIN, kd: KD_LIN});
this._pid_y = new PID({kp: KP_LIN, ki: KI_LIN, kd: KD_LIN});
this._pid_z = new PID({kp: KP_LIN, ki: KI_LIN, kd: KD_LIN});
this._pid_yaw = new PID({kp: KP_ANG, ki: KI_ANG, kd: KD_ANG});
this._pid_x = new PID(0.5, 0, 0.35);
this._pid_y = new PID(0.5, 0, 0.35);
this._pid_z = new PID(0.6, 0.001, 0.1);
this._pid_yaw = new PID(5.0, 0, 1.0);
this._ekf = new EKF(options);
this._camera = new Camera();
this._enabled = false;
@@ -39,11 +27,8 @@ function Controller(client, options) {
var self = this;
client.on('navdata', function(d) {
self._processNavdata(d);
});
Timers.setInterval(function() {
self._control();
}, this._freq);
});
}
/*
@@ -80,20 +65,11 @@ Controller.prototype.go = function(goal, callback) {
// disable the controller, just in case.
this.disable();
// Goals can be incomplete. In this case, we attempt to maintain
// the current state.
var state = this._state;
if (goal.x == undefined) {goal.x = state.x};
if (goal.y == undefined) {goal.y = state.y};
if (goal.z == undefined) {goal.z = state.z};
// Normalize the yaw, to make sure we don't spin 360deg for
// nothing :-)
if (goal.yaw != undefined) {
var yaw = goal.yaw.toRad();
goal.yaw = Math.atan2(Math.sin(yaw),Math.cos(yaw));
} else {
goal.yaw = state.yaw;
}
// Update our goal
@@ -154,10 +130,10 @@ Controller.prototype._control = function() {
var ux = uy = uz = uyaw = 0;
// Compute error between current state and goal
var ex = this._goal.x - this._state.x
, ey = this._goal.y - this._state.y
, ez = this._goal.z - this._state.z
, eyaw = this._goal.yaw - this._state.yaw
var ex = (this._goal.x != undefined) ? this._goal.x - this._state.x : 0
, ey = (this._goal.y != undefined) ? this._goal.y - this._state.y : 0
, ez = (this._goal.z != undefined) ? this._goal.z - this._state.z : 0
, eyaw = (this._goal.yaw != undefined) ? this._goal.yaw - this._state.yaw : 0
;
// Compute control commands
@@ -176,52 +152,43 @@ Controller.prototype._control = function() {
// If all commands are zero, we have reached our destination;
// we go into hover mode and trigger the callback if defined.
//
// TODO This does not work, since we may speed past the target
// we need a better way to estimate when we have reached the destination,
// probably should have error and speed < EPS for some time.
//
if (ux == 0 && uy == 0 && uz == 0 && uyaw == 0) {
this._client.stop();
if (this._callback != null) {
var cb = this._callback;
this._callback = null;
this.disable();
cb(this.state());
}
// this.disable();
// if (this._callback != null) {
// var cb = this._callback;
// this._callback = null;
// cb(this.state());
// }
}
// Else map controller commands to drone commands
else {
var yaw = this._state.yaw;
var cx = Math.cos(yaw) * ux - Math.sin(yaw) * uy;
var cy = -Math.sin(yaw) * ux + Math.cos(yaw) * uy;
var cz = uz;
var cyaw = uyaw;
var cx = within(Math.cos(yaw) * ux - Math.sin(yaw) * uy, -1, 1);
var cy = within(-Math.sin(yaw) * ux + Math.cos(yaw) * uy, -1, 1);
var cz = within(uz, -1, 1);
var cyaw = within(uyaw, -1, 1);
// Send commands to drone
if (Math.abs(cx) > EPS_LIN) {
if (cx >= 0) {
this._client.front(Math.min(cx, 1));
} else {
this._client.back(Math.min(Math.abs(cx), 1));
}
if (Math.abs(cx) > 0) {
this._client.front(cx);
}
if (Math.abs(cy) > EPS_LIN) {
if (cy >= 0) {
this._client.right(Math.min(cy, 1));
} else {
this._client.left(Math.min(Math.abs(cy), 1));
}
if (Math.abs(cy) > 0) {
this._client.right(cy);
}
if (cz >= 0) {
this._client.up(Math.min(cz, 1));
} else {
this._client.down(Math.min(Math.abs(cz), 1));
if (Math.abs(cz) > 0) {
this._client.up(cz);
}
if (cyaw >= 0) {
this._client.clockwise(Math.min(cyaw, 1));
} else {
this._client.counterClockwise(Math.min(Math.abs(cyaw), 1));
if (Math.abs(cyaw) > 0) {
this._client.clockwise(cyaw);
}
}
@@ -230,7 +197,17 @@ Controller.prototype._control = function() {
console.log("Goal: \t %d,%d,%d,%d", this._goal.x, this._goal.y, this._goal.z, this._goal.yaw);
console.log("State:\t %d,%d,%d,%d", this._state.x, this._state.y, this._state.z, this._state.yaw);
console.log("Error:\t %d,%d,%d,%d", ex, ey, ez, eyaw);
console.log("Control: \t%d,%d,%d,%d", ux, uy, uz, uyaw);
console.log("Commands:\t %d,%d,%d,%d", cx, cy, cz, cyaw);
console.log("Ctrl: \t %d,%d,%d,%d", ux, uy, uz, uyaw);
console.log("Cmds: \t %d,%d,%d,%d", cx, cy, cz, cyaw);
}
}
function within(x, min, max) {
if (x < min) {
return min;
} else if (x > max) {
return max;
} else {
return x;
}
}
lib/EKF.js
+18 -6
Ver Arquivo
@@ -13,13 +13,9 @@ function EKF(options) {
this._options = options;
this._delta_t = options.delta_t || EKF.DELTA_T;
this._state = options.state || {x: 0, y: 0, z: 1, yaw: 0};
this._tag = options.tag || {x: 0, y: 0, yaw: 0};
this._last_yaw = null;
this._sigma = Matrix.I(3);
this._q = Matrix.Diagonal([0.0003, 0.0003, 0.0001]);
this._r = Matrix.Diagonal([0.3, 0.3, 0.1]);
this.reset();
}
EKF.prototype.state = function() {
@@ -30,6 +26,14 @@ EKF.prototype.confidence = function() {
return this._sigma;
}
EKF.prototype.reset = function() {
this._state = this._options.state || {x: 0, y: 0, z: 1, yaw: 0};
this._sigma = Matrix.I(3);
this._q = Matrix.Diagonal([0.0003, 0.0003, 0.0001]);
this._r = Matrix.Diagonal([0.3, 0.3, 0.1]);
this._last_yaw = null;
}
EKF.prototype.predict = function(data) {
var pitch = data.demo.rotation.pitch.toRad()
, roll = data.demo.rotation.roll.toRad()
@@ -37,6 +41,7 @@ EKF.prototype.predict = function(data) {
, vx = data.demo.velocity.x / 1000 //We want m/s instead of mm/s
, vy = data.demo.velocity.y / 1000
, vz = data.demo.velocity.z / 1000
, dphi = yaw - this._last_yaw
, alt = data.demo.altitude
, dt = this._delta_t
;
@@ -48,8 +53,15 @@ EKF.prototype.predict = function(data) {
return;
}
// A simplistic way to filter backround noise on the odometry
// TODO - We may want to calibrate by averaging noise over time
// when steady and then substract it.
vx = (Math.abs(vx) < 0.15) ? 0 : vx;
vy = (Math.abs(vy) < 0.15) ? 0 : vy;
dphi = (Math.abs(dphi) < 0.01) ? 0 : dphi;
// Compute the odometry by integrating the motion over delta_t
var o = {dx: vx * dt, dy: vy * dt, dphi: yaw - this._last_yaw};
var o = {dx: vx * dt, dy: vy * dt, dphi: dphi};
this._last_yaw = yaw;
// Update the state estimate
lib/PID.js
+6 -10
Ver Arquivo
@@ -1,17 +1,13 @@
DEFAULT_KP = 0.15;
DEFAULT_KI = 0.1;
DEFAULT_KD = 0.1;
module.exports = PID;
function PID(options) {
this.configure(options || {});
function PID(kp, ki, kd) {
this.configure(kp, ki, kd);
this.reset();
}
PID.prototype.configure = function(options) {
this._kp = options.kp || DEFAULT_KP;
this._ki = options.ki || DEFAULT_KI;
this._kd = options.kd || DEFAULT_KD;
PID.prototype.configure = function(kp,ki,kd) {
this._kp = kp;
this._ki = ki;
this._kd = kd;
}
PID.prototype.reset = function() {
tests/simple.js
+5 -2
Ver Arquivo
@@ -1,10 +1,13 @@
// TODO This is broken. Need to write real tests.
console.log("The test is broken. Will have to write proper tests :-)");
var autonomy = require('..');
var client = require('./mock/client');
var controller = new autonomy.Controller(new client(), {state: {x: 0, y:0, z:1, yaw: Math.PI/4}, debug: true});
var controller = new autonomy.Controller(new client(), {state: {x: 0, y:0, z:1, yaw: 0}, debug: false});
console.log("State: %j", controller.state());
controller.go({x: 0, y: 0, z:1, yaw: -45}, function(state) {
controller.go({x: 1, y: 1}, function(state) {
console.log("Reached state %j", state);
});