给定机器人坐标和朝向，判断某点在机器人的哪个方位

最新推荐文章于 2022-10-07 19:34:21 发布

原创最新推荐文章于 2022-10-07 19:34:21 发布 · 2.4k 阅读

3 ·

CC 4.0 BY-SA版权

本文介绍了一个简单的程序，该程序能够根据已知的机器人位置和角度，判断任意目标点相对于机器人的方位，并计算出两者之间的夹角。

我的数学基础和代码功底实在需要加强啊~下面简单实现了已知机器人的位姿（r_x,r_y,r_th），判断任一点(obj_x,obj_y)的方位。

/声明部分/

double   get0to360(double dx, double dy);
int           checkLorR(double obj, double r);
double   getTheta(double obj, double r);
void        getRelation(double r_th, double r_x, double r_y, double obj_x, double obj_y, int &flag, double &theta);
typedef struct {
	double x;
	double y;
	double th;
}odm;

/主函数/
int main() {

double obj_x = 383, obj_y = 397; //点的坐标示例
int flag;
double theta;
odm robot;
robot.x = 386, robot.y = 373, robot.th = 31.6908; //机器人位姿示例
getRelation(robot.th, robot.x, robot.y, obj_x, obj_y, flag, theta);

//根据flag和夹角判断在哪个方位
if (flag == 1)
	std::cout << "家具在机器人左、";
else if (flag == 2)
	std::cout << "家具在机器人右、";
else
	std::cout << "家具在机器人正" ;
if (theta < 90)
	std::cout << "前" << std::endl;
else
	std::cout << "后" << std::endl;
std::cout << "夹角为" << theta << std::endl;
system("pause");
return 0;

}
/*函数定义部分/
/范围0~360/
double get0to360(double dy, double dx) {

double radian,degree;
radian = atan2(dy, dx);   //atan2范围-pi~pi,atan2(y,x)
degree = radian * 180 / PI;
if (degree < 0)degree = degree + 360;
return degree;

}

/* 判断家具在机器人左边还是右边 */
// 1—左边；2—右边；0–方向重合
int checkLorR(double obj, double r) {

if (r > obj) {
	if (r - obj < 180) {
		return 2;
	}
	else if (r - obj > 180) {
		return 1;
	}
	else
		return 0;
}
else if (obj > r ) {
	if (obj - r < 180) {
		return 1;
	}
	else if (obj - r > 180) {
		return 2;
	}
	else
		return 0;
}
else
	return 0;

}

/点和机器人的夹角/
double getTheta(double obj, double r) {

if (r > obj) {
	if (r - obj < 180) {
		return r - obj;
	}
	else if (r - obj >= 180) {
		return 360 - (r - obj);
	}
}
else if (obj > r) {
	if (obj - r < 180) {
		return obj - r;
	}
	else if (obj - r >= 180) {
		return 360 - (obj - r);
	}
}
else
	return 0;

}

void getRelation(double r_th, double r_x, double r_y, double obj_x, double obj_y, int &flag, double &theta) {

double dx,dy,obj_th;
if (r_th < 0)  r_th = r_th + 360;
dx = obj_x - r_x, dy = obj_y - r_y;
obj_th = get0to360(dy, dx);

flag = checkLorR(obj_th, r_th);
theta = getTheta(obj_th, r_th);

}