机器人定位坐标变换相关

2013年5月28日 由 Creater 留言 »

机器人定位坐标变换相关
前一状态(XBaseIn, YBaseIn, angle1)相对于大地坐标。
后一状态(Xin,Yin,angle2),在前一状态的坐标系下(机器人坐标系)
根据前后两个状态可以计算出Xin,Yin,然后进行坐标变换后得到大地坐标系下的坐标,测试正常。

int main()
{
	double angle1, angle2,deltAngle;
	double Xin, Yin;
	double XBaseIn, YBaseIn;

	double deltRad;
	double dist;
	angle1 = 45;
	angle2 = 90;
	deltAngle = angle2 -angle1;
	deltRad = degToRad(deltAngle);

	dist = 6;
	Xin = dist * cos(deltRad);
	Yin = dist * sin(deltRad);
	XBaseIn = 2;
	YBaseIn = 2;
	double RadBaseIn = degToRad(angle1);
	trans(Xin,Yin,XBaseIn, YBaseIn, RadBaseIn);
	return 0;

}
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <algorithm>
#include <iostream>
using namespace std;

#define PI 3.14

static float fixAngle(float angle) //把角度转换到-180度到180
{
	if (angle >= 360)
		angle = static_cast<float>(angle - 360.0 * (float)((int)angle / 360));
	if (angle < -360)
		angle = static_cast<float>(angle + 360.0 * (float)((int)angle / -360));
	if (angle <= -180)
		angle = static_cast<float>(+ 180.0 + (angle + 180.0));
	if (angle > 180)
		angle = static_cast<float>(- 180.0 + (angle - 180.0));
  return angle;
} 


static float degToRad(float deg) { return static_cast<float>(deg * PI / 180.0); }	//角度转弧度
static float radToDeg(float rad) { return static_cast<float>(rad * 180.0 / PI); }	//弧度转角度
static float cos(float angle) { return ::cos(degToRad(angle)); }//cos(角度)
static float sin(float angle) { return ::sin(degToRad(angle)); }//sin(角度)
static float tan(float angle) { return ::tan(degToRad(angle)); }//tan(角度)
static float matan2(float y, float x) { return radToDeg(::atan2(y, x)); }//arctan(x,y)


void trans(double Xin, double Yin, double XBaseIn, double YBaseIn, double RadBase)
{
	double x, y, angle;
	x = Xin * cos (RadBase) - Yin * sin(RadBase) + XBaseIn;
	y = Xin * sin (RadBase) + Yin * cos(RadBase) + YBaseIn;
	angle = matan2(y,x);//角度值
	cout<<"X= "<<x<<"  Y= "<<y<<"  Angle= "<<angle<<endl;
}




int main()
{
	double angle1, angle2,deltAngle;
	double Xin, Yin;
	double XBaseIn, YBaseIn;

	double deltRad;
	double dist;
	angle1 = -135;
	angle2 = -90;
	deltAngle = angle2 -angle1;
	deltRad = degToRad(deltAngle);

	dist = 6;
	Xin = dist * cos(deltRad);
	Yin = dist * sin(deltRad);
	XBaseIn = -2;
	YBaseIn = -2;
	double RadBaseIn = degToRad(angle1);
	trans(Xin,Yin,XBaseIn, YBaseIn, RadBaseIn);
	return 0;

}
广告位

发表评论

你必须 登陆 方可发表评论.