robotnonholonomic.cpp

基于vc 的环境下机器人自主避障的算法 图形处理 具有载物功能

//////////////////////////////////////////////////////////////////////
// MuRoS - Multi Robot Simulator
//
// Luiz Chaimowicz
// GRASP Lab. University of Pennsylvania
// VERLab - DCC - UFMG - Brasil
//
// RobotHolonomic.cpp: implementation of the CRobotHolonomic class.
//
// RobotNonHolonomic.cpp: implementation of the CRobotNonHolonomic class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "simulator.h"
#include "RobotNonHolonomic.h"
#include "RobotHolonomic.h"
#include "const.h"
#include <math.h>

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

#include "myglobals.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
IMPLEMENT_SERIAL(CRobotNonHolonomic, CRobot, 1 )

CRobotNonHolonomic::CRobotNonHolonomic(double x, double y, double theta, short status, short id, CString name) : CRobot(x, y, theta, status, id, name)
{
	m_v = 0;
	m_w = 0;
	m_radius = 10;
}

CRobotNonHolonomic::CRobotNonHolonomic()
{
}

CRobotNonHolonomic::~CRobotNonHolonomic()
{
}

void CRobotNonHolonomic::Update(CArray<CRobot*, CRobot*> *robots, double simTime, double dt, CBox* box, CMapPath *globalMap)
{
	CRobot::Update(robots, simTime, dt, box, globalMap);

	Potential(robots, dt, globalMap, box, simTime);
		
	m_x += m_v * cos(m_theta) * dt;
	m_y += m_v * sin(m_theta) * dt;
	m_theta += m_w * dt;
}

void CRobotNonHolonomic::Draw(CDC *pDC)
{
	m_rect = CRect( round(m_x - m_radius), round(m_y - m_radius), round(m_x + m_radius), round(m_y + m_radius) );
	
	pDC->SelectObject(&RED_PEN);

	if (m_status == TRANSPORT)
		pDC->SelectStockObject(BLACK_BRUSH);
	else if (m_status == DOCK)
		pDC->SelectStockObject(GRAY_BRUSH);
	else if (m_status == STOPPED)
		pDC->SelectStockObject(WHITE_BRUSH);
	else if (m_status == WAIT)
		pDC->SelectObject(&BLUE);

	pDC->Ellipse(m_rect);

	CPoint points[2];
	points[0] = CPoint( round(m_x), round(m_y));
	points[1] = CPoint( round(m_x + ( m_radius * cos(m_theta) )), round(m_y + ( m_radius * sin(m_theta) )));

	pDC->SelectObject(&RED_PEN);
	pDC->Polygon(points,2);  
}

void CRobotNonHolonomic::Serialize(CArchive& ar)
{
	CRobot::Serialize(ar);

	if (ar.IsStoring()) {
		ar << m_v;
		ar << m_w;
	}
	else {
		ar >> m_v;
		ar >> m_w;
	}
}

// Potential ontroller. The robot is attrated by the goal and the box depending on its state.
// There is a repulsion from other robots and obstacles, an the collisions are also treated
// Constants are defined in the file myglobals.h, initialized  in the file simulator.cpp and 
// can be modified dynamically
void CRobotNonHolonomic::Potential(CArray<CRobot*, CRobot*> *robots, double dt, CMapPath *map, CBox* box, double simTime)
{
	short i;

	double fx = 0;
	double fy = 0;

	double dist;
	double angle;

	// Repulsive forces from other robots
	for(i=0; i<robots->GetSize(); i++) 
		if (robots->GetAt(i)->m_id != m_id)
			ForceFromRobots(robots->GetAt(i), fx, fy);

	// Repulsive forces from obstacles
	for (i=0; i<m_localMap->m_obstacles.GetSize(); i++)
		ForceFromObstacles(m_localMap->m_obstacles[i], fx, fy);

	// Atractive force from box (Docking or waiting)
	// Repulsive if dist < 0 in all cases
	if (box->m_exist) {
		ForceFromBoxes(box, fx, fy);
	}
	else {
		m_status = TRANSPORT;
	}

	// Atractive force from goal
	if ( (m_localMap->m_goalReal.x != -1) && ( (m_status == TRANSPORT) || (m_status == FPPATH) ) ) {
		dist = sqrt( (m_localMap->m_goalReal.x - m_x) * (m_localMap->m_goalReal.x - m_x) + (m_localMap->m_goalReal.y - m_y) * (m_localMap->m_goalReal.y - m_y) );
		angle = atan2( (m_localMap->m_goalReal.y - m_y), (m_localMap->m_goalReal.x - m_x) );
		
		fx += Ka_rg * dist * cos(angle);
		fy += Ka_rg * dist * sin(angle);
	}

	// Computing velocities
	m_v += ( fx * cos(m_theta) + fy * sin(m_theta) - Cr * m_v) * dt;
	m_w += 5 * (atan2(fy,fx) - m_theta -0.5 * m_w) * dt;


	//Saturation 
	if (m_v > 200)
		m_v = 200;
	else if (m_v < -200)
		m_v = -200;

	if (m_w > PI)
		m_v = PI;
	else if (m_w < -PI)
		m_v = -PI;


	// Change to WAIT status if close to the box and the distance is small
	if ( (m_status == DOCK) && (fabs(m_v) < 30) && (m_distBox < 5) ) {
		m_status = WAIT;
		m_timer = simTime;

		CControlMsg *controlMsg;
		controlMsg = new CControlMsg;
		controlMsg->m_code = BOXLOCKED;
		controlMsg->m_from = m_id;
		controlMsg->m_to = 0;
		controlMsg->m_x = m_x;
		controlMsg->m_y = m_y;
		
		SendControlMsg(controlMsg);
	}
	else if ( (m_status == WAIT) && ( (simTime - m_timer) > 10) ) {
		CControlMsg *controlMsg;
		controlMsg = new CControlMsg;
		controlMsg->m_code = TIMEOUT;
		controlMsg->m_from = m_id;
		controlMsg->m_to = 0;

		SendControlMsg(controlMsg);
	}

	if ( (box->m_exist) && (m_status == TRANSPORT) && ( (m_distBox + m_radius) > m_sensorRange) ) {

		m_status = STOPPED;

		CControlMsg *controlMsg;
		controlMsg = new CControlMsg;
		controlMsg->m_code = LOSTBOX;
		controlMsg->m_from = m_id;
		controlMsg->m_to = 0;
		
		SendControlMsg(controlMsg);
	}
}


// Compute the fores due to the obstales
// Constants are defined in the file myglobals.h, initialized  in the file 
// simulator.cpp and can be modified dynamically
void CRobotNonHolonomic::ForceFromObstacles(CObstacle *obst, double &fx, double &fy)
{
	double angle;
	double deltaN;
	double deltaNDot;
	double lambdaN;

	double vx = m_v * cos(m_theta);
	double vy = m_v * sin(m_theta);

	if (obst->m_inRange) {
		angle = obst->m_angle + PI;

		if (obst->m_inContact) {
			deltaN = obst->m_dist;
			deltaNDot = vx * cos(angle) + vy * sin(angle); // velocity in the normal direction
			lambdaN = Kc_ro * deltaN - Cc_ro * deltaNDot;

			fx +=  lambdaN * cos(angle);
			fy +=  lambdaN * sin(angle);
		}
		else{
			fx += -Kr_ro * cos(angle-PI) * 1/fabs(obst->m_dist);
			fy += -Kr_ro * sin(angle-PI) * 1/fabs(obst->m_dist);
		}
	}
}

// Compute the forces due to the boxes
// Constants are defined in the file myglobals.h, initialized  in the file 
// simulator.cpp and can be modified dynamically
void CRobotNonHolonomic::ForceFromBoxes(CBox *box, double &fx, double &fy)
{
	double deltaN;
	double deltaNDot;
	double lambdaN;

	double vx = m_v * cos(m_theta);
	double vy = m_v * sin(m_theta);
	double angle = m_angleBox + PI;

	if ( (m_status == DOCK) || (m_status == WAIT) || ( (boxAttraction) && (m_status == TRANSPORT) ) )
		if (!m_inContactBox) {

			// Attractive force
			fx += Ka_rb * m_distBox * cos(angle-PI);
			fy += Ka_rb * m_distBox * sin(angle-PI);
		}
		
	if ( (m_inContactBox) ) {
		// Contact forces
		deltaN = m_distBox;
		deltaNDot = (vx - box->m_v[0]) * cos(angle) + (vy - box->m_v[1]) * sin(angle); // velocity in the normal direction
		lambdaN = Kc_rb * deltaN - Cc_rb * deltaNDot;

		fx += lambdaN * cos(angle);
		fy += lambdaN * sin(angle);
	}
}


// Forces and torque applied on the boxes by the robot
// Constants are defined in the file myglobals.h, initialized  in the file 
// simulator.cpp and can be modified dynamically
void CRobotNonHolonomic::ForceOnBoxes(CBox *box,double &fx, double &fy, double &ftheta)
{

	double deltaN;
	double deltaNDot;
	double lambdaN;
	double angle;
	double dist;

	double vx = m_v * cos(m_theta);
	double vy = m_v * sin(m_theta);
	double kw = 0.0001;  // constant for angular velocity of the box

	double fx_Box;
	double fy_Box;

	if (m_status == TRANSPORT) {
		if (m_inContactBox) {
			deltaN = m_distBox;
			angle = m_angleBox; // don't need to add PI here. m_angle box is computed in the robot frame!
				
			deltaNDot = (vx - box->m_v[0]) * cos(angle + PI) + (vy - box->m_v[1]) * sin(angle + PI); // velocity in the normal direction
			lambdaN = Kc_rb * deltaN - Cc_rb * deltaNDot;

			// The robot can never pull the box. So the force in the inverse normal direction 
			// of the box must be always positive
			if (lambdaN < 0)
				lambdaN = 0;

			fx_Box = lambdaN * cos(angle);
			fy_Box = lambdaN * sin(angle);

			fx += fx_Box;
			fy += fy_Box;

			// Computing torque
			DistanceCircleCircle(m_contactPoint.x, m_contactPoint.y, box->m_x, box->m_y, 0, 0, dist, angle);
			double ftheta_Box = fy_Box * cos(angle) -fx_Box * sin(angle);
			ftheta += kw * ftheta_Box * dist;
		}
	}
}

/*
void CRobotHolonomic::ForceOnBoxes(CBox *box, double &fx, double &fy, double &ftheta)
{
}

*/

// Forces due to the other robots
// Constants are defined in the file myglobals.h, initialized  in the file 
// simulator.cpp and can be modified dynamically
void CRobotNonHolonomic::ForceFromRobots(CRobot *robot, double &fx, double &fy)
{
	double deltaN;
	double deltaNDot;
	double lambdaN;

	double dist;
	double angle;

	DistanceCircleCircle(robot->m_x, robot->m_y, m_x, m_y, robot->m_radius, m_radius, dist, angle);
	angle += PI;

	double vx2;
	double vy2;

	double vx1 = m_v * cos(m_theta);
	double vy1 = m_v * sin(m_theta);

	if (dist < 0) {

		if (robot->IsKindOf( RUNTIME_CLASS(CRobotHolonomic) ) ) {
			vx2 = ( (CRobotHolonomic *) robot)->m_vx;
			vy2 = ( (CRobotHolonomic *) robot)->m_vy;
		}
		else {
			vx2 = ( (CRobotNonHolonomic *) robot)->m_v * cos(robot->m_theta);
			vy2 = ( (CRobotNonHolonomic *) robot)->m_v * sin(robot->m_theta);
		}
		deltaN = -dist;
		deltaNDot = (vx1 - vx2) * cos(angle) + (vy1 - vy2) * sin(angle); // velocity in the normal direction
		lambdaN = Kc_rr * deltaN - Cc_rr * deltaNDot;

		fx +=  lambdaN * cos(angle);
		fy +=  lambdaN * sin(angle);
	}
	else if (dist < m_sensorRange ){
		fx += -Kr_rr * cos(angle-PI) * 1/fabs(dist);
		fy += -Kr_rr * sin(angle-PI) * 1/fabs(dist);
	}
}


// -------------------------------------------
// Some controllers
// -------------------------------------------

// Move a non holonomic robot towards the 
// goal using feedback linearization
void CRobotNonHolonomic::ToGoal(double dt, CMapPath *globalMap)
{
	double acc;
	double kp = 2;
	double kd = 3;
	double dx = (globalMap->m_goalReal.x - m_x);
	double dy = (globalMap->m_goalReal.y - m_y);

	if ( (m_v < 0.01) && (m_v >= 0) ) m_v = 0.01;
	else if ( (m_v > -0.01) && (m_v <= 0) ) m_v = -0.01;

	m_w = -kp * (dx * sin(m_theta) - dy * cos(m_theta) ) / m_v;
	acc = -kd * m_v + kp * (dx * cos(m_theta) + dy * sin(m_theta) );

	if (m_w > PI/15) m_w = PI/15;
	else if (m_w < -PI/15) m_w = -PI/15;

	if (acc > 100) acc = 100;
	else if (acc < -100) acc = -100;

	m_v += acc * dt;
	if (m_v > 100) m_v = 100;
	else if (m_v < -100) m_v = -100;
}

// Follow lines that were computed by the path planner
// uses a controller similar to the wall follower of MARS
void CRobotNonHolonomic::FollowPath(double simTime, CMapPath *map)
{

	double a ,b, c;
	double a_next, b_next, c_next;
	double intersectX, intersectY;
	double alpha, theta_t;
	double dist, distNext;
	double kp, kd;

	kp = 2;
	kd = 4;

	// selecting what line segment to follow
	if (map->m_currentLine + 1 < map->m_lineSize) {
		a_next = map->m_line[map->m_currentLine+1][0];
		b_next = map->m_line[map->m_currentLine+1][1];
		c_next = map->m_line[map->m_currentLine+1][2];
		distNext = (a_next * m_x + b_next * m_y + c_next) / sqrt(a_next * a_next + b_next * b_next);

		
		// getting the next line to follow
		if(fabs(distNext) < 10){
			distNext = 10000;
			map->m_oldDist = 10000;
			map->m_currentLine++;
			map->m_setVelocity = TRUE;

			if (map->m_currentLine + 1 < map->m_lineSize) {
				a_next = map->m_line[map->m_currentLine+1][0];
				b_next = map->m_line[map->m_currentLine+1][1];
				c_next = map->m_line[map->m_currentLine+1][2];
			}
		}
	}
	else{
		distNext = sqrt( (m_x - map->m_goalReal.x) * (m_x - map->m_goalReal.x)  + (m_y - map->m_goalReal.y) * (m_y - map->m_goalReal.y) );
	}

	// getting the line equation ax + by + c = 0 
	a = map->m_line[map->m_currentLine][0];
	b = map->m_line[map->m_currentLine][1];
	c = map->m_line[map->m_currentLine][2];


	if (map->m_setVelocity){
		// computing linear velocity direction( + or - ) according to the next intersection
		if (map->m_currentLine + 1 < map->m_lineSize) {
			intersectX = ( (b * c_next) - (b_next * c) ) / ( (a * b_next) - (a_next * b) );
			intersectY = ( (a_next * c) - (a * c_next) ) / ( (a * b_next) - (a_next * b) );
		}
		else{
			intersectX = map->m_goalReal.x;
			intersectY = map->m_goalReal.y;
		}
		alpha = atan2( (m_y - intersectY) , (m_x - intersectX) );

		
		// setting linear velocity
		if ( ( (m_theta - alpha) > -PI/2 ) && ( (m_theta - alpha) < PI/2 ) )
			m_v = (m_numObstacles == 0) ? -80 : 80;
		else
			m_v = (m_numObstacles == 0) ? 80 : 80;

		map->m_setVelocity = FALSE;
	}
	// to certify that the robot is moving towards the next point
	if ( fabs(distNext) > map->m_oldDist)
		m_v = -m_v;
	map->m_oldDist = fabs(distNext);

	// computing the distance to the line and the line slope
	dist = (a * m_x + b * m_y + c) / sqrt(a * a + b * b);
	theta_t = (b != 0) ?  atan(-a/b) : PI/2;

	// controller for angular velocity 
	m_w = ( kp * (0 - dist) - kd * m_v * sin(m_theta - theta_t) ) / ( m_v * cos(m_theta - theta_t) );

	// saturation
	if (m_w > 1) 
		m_w = 1;
	else if (m_w < -1) 
		m_w = -1;

	// test if the goal was reached.
	if ( ( fabs(map->m_goalReal.x - m_x) < 30) && ( fabs(map->m_goalReal.y - m_y) < 30) ) {
		m_v = 0;
		m_w = 0;
	}

	return;

/* Leadership pass. Need be modified


	// if I am not able to follow the path (angle > 55 degress)
	// or velocity is positive, then request backup
	if ( ( ( fabs( tan(theta_t - m_theta) ) > 1.5) || (m_v > 0) ) && (m_numObstacles > 0) ){
			CControlMsg *controlMsg = new CControlMsg;
			controlMsg->m_code = BACKUPMSG;
			controlMsg->m_tstamp = simTime;
			controlMsg->m_extra = theta_t - m_theta;
			controlMsg->m_from = m_id;
			//controlMsg->m_to = (m_id < 3) ? m_id + 2 : m_id - 2; // passing to the opposite robot
			controlMsg->m_to = 1;
			SendControlMsg(controlMsg);
			m_status = FOLLOW;
			m_v = 0;
			m_w = 0;
			return;
	}
*/
}