• 欢迎浏览“String me = Creater\忠实的资深Linux玩家;”,请文明浏览,理性发言,有侵犯你的权益请邮件我(creater@vip.qq.com).
  • 把任何的失败都当作一次尝试,不要自卑;把所有的成功都想成是一种幸运,不要自傲。
  •    5年前 (2013-05-28)  开源项目 |   64 条评论  163 
    文章评分 0 次,平均分 0.0

    机器人定位坐标变换相关
    前一状态(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;
    
    }
     

    除特别注明外,本站所有文章均为String me = "Creater\忠实的资深Linux玩家";原创,转载请注明出处来自http://unix8.net/home.php/1247.html

    关于

    发表评论

    暂无评论

    切换注册

    登录

    忘记密码 ?

    切换登录

    注册

    扫一扫二维码分享