CSARA机械手正反解代码解读和左右手定则应用

网易足已

于 2024-09-16 18:23:49 发布

阅读量1.2k

点赞数 25

文章标签：机器人

本文链接：https://blog.csdn.net/carryoutw/article/details/142220043

版权

前言：前段时间在某鱼上买了一份CSARA的机械臂的程序，拿出来分享一下，并记录一下。说明一下并非是公司的核心代码，我也不搞这个....侵权就删了。

首先简单回顾一下CSARA的正逆解。

根据几何的方法能求出末端在平面坐标系中的xy坐标。也就是正解。知道了各个关节的角度，求末端的位置。一般的思路是这样的，但是有一个左右手定则的方式，也就是可以建立不同的坐标系。左右手定则，一般使用在还有摄像头的一个情况，摄像头进行识别的情况。

比如你建立的坐标系刚好是右手定则，而摄像头看物体就是符合左手定则，所以会有左右手坐标转换。如图

我们假设机器关节的theta2角度在右手定则是大于180度的，比如280度，而在左手定则中却是360-270 = 80的，进而对其转换成统一坐标系的。

左右手定则公式：

推理是很简单的，仅仅用到了三角函数和三角形求角度的通用公式。注意一点哈，左右手的主要目的是对其坐标系的转换。左->右或者右->左。

写成代码则是：

看的出来，实际上主要还是a的不同，通过a来进行转换的。

好了，左右手的用法已经知道了，相信利用几何法也能够进行出正逆解的公式。大家可以自行推导，这片文章的重点并不在这里。

主代码：


int main()
{

	ScaraLib scara;
	scara.createUserCrd(cv::Point2f(100,100),90.0/180.0*CV_PI);

	cv::Point2f rp1(100*sqrt(2), 0);
	cv::Point2f cp2;
	scara.cvtRobotUserCrd(rp1, cp2);
	double atn = atan2(1, 2);
	//反解
	scara.setXYZC(34, 22, 0, 0.56);
	double j1, j2, j3, j4;
	scara.getJn(j1, j2, j3, j4);
	double x, y, z, c;
	//正解
	scara.setJn(j1, j2, j3, j4);
	scara.getXYZC(x, y, z, c);
	//反解
	scara.setXYZC(x, y, z, c);
	scara.getJn(j1, j2, j3, j4);
    //验证正逆解
	std::vector<cv::Point2f> camera;
	std::vector<cv::Point2f> robot;
	camera.push_back(cv::Point2f(10, 10));
	camera.push_back(cv::Point2f(10, 100));
	camera.push_back(cv::Point2f(100, 100));
	camera.push_back(cv::Point2f(100, 10));
	robot.push_back(cv::Point2f(10, 10));
	robot.push_back(cv::Point2f(10, 100));
	robot.push_back(cv::Point2f(100, 100));
	robot.push_back(cv::Point2f(100, 10));
	scara.createUserCrd(camera, robot);
	cv::Point2f p1 = cv::Point2f(10, 10);   //相机坐标系
	cv::Point2f p2;                        //基坐标系
	scara.setUserXYZC(34, 22, 0, 0.56);
	scara.getUserXYZC(x, y, z, c);

    return 0;
}

一步步来，慢慢分析，第一句话scara.createUserCrd(cv::Point2f(100,100),90.0/180.0*CV_PI);的意思是用一个点+角度来创建用户坐标系。


//用一个点+一个角度创建用户坐标系统
bool ScaraLib::createUserCrd(const cv::Point2f& tnHome, double angle)
{
	double rd = 1000;

	cv::Point2f rp1(0, 0);
	cv::Point2f rp2(rd, 0);
	cv::Point2f rp3(0, rd);

	cv::Point2f cp1(tnHome.x, tnHome.y);
	cv::Point2f cp2(tnHome.x + rd*cos(angle), tnHome.y + rd*sin(angle));
	cv::Point2f cp3(tnHome.x + rd*cos(angle + CV_PI / 2.0), tnHome.y + rd*sin(angle + CV_PI / 2.0));

	std::vector<cv::Point2f> camera;
	std::vector<cv::Point2f> robot;

	camera.push_back(cp1); robot.push_back(rp1);
	camera.push_back(cp2); robot.push_back(rp2);
	camera.push_back(cp3); robot.push_back(rp3);

	return createUserCrd(camera, robot);
}

其实也很好理解，cp1是用户坐标系（摄像头坐标系），rp1是机器人本身的坐标系。先解释一波在看图，我们把rp1和cp1看做坐标系原点位置，rp2和cp2看做x方向的位置，rp3和cp3看做y方向的位置。好了是不是有点明白了。看图，输入是(cv::Point2f(100,100),90.0/180.0*CV_PI这个是openCV 读取出的数据，而最终返回的是camera和robot，但是接下来还内嵌了一个createUserCrd函数。

//创建用户坐标系统
bool ScaraLib::createUserCrd(const std::vector<cv::Point2f>& camera, const std::vector<cv::Point2f>& robot)
{
	mCamera = camera;
	mRobot = robot;

	//当数据只有两个点时，需要补充一个点
	cv::Point2f cpt_c;
	cv::Point2f rpt_c;
	rotate(camera[1], camera[0], CV_PI / 2.0, cpt_c);
	rotate(robot[1], robot[0], CV_PI / 2.0, rpt_c);

	mCamera.push_back(cpt_c);
	mRobot.push_back(rpt_c);

	return true;
}

这里的思路是把用户自己建立的坐标系转换成机器人坐标系，解释一下：camera和robot分别是（3个点都已知了）：

（数据为假设的，目的是明白camera的存储方式），这里有一个c++的函数重载用法，也就是同一个函数名字，不同输入参数，编译器会根据不同的输入参数进行识别函数体，进行运行，如下：

嗯。。最后的这个输入5个参数的rotate函数，是一个二维旋转变换的公式，即，可以理解绕z轴进行旋转。之后利用mCamera.push_back(cpt_c);这个函数放在mCamera后面，即现在就是0,0 1,0 0,1 1,1了。但是哈，查找了整个程序，没有发现其他地方使用mCamera函数，不太理解，有理解的可以一起说说。啊我猜测哈，这里是只有两个点的情况下，生成第3个点的情况，但是根据opencv来说给到了3个点，所以就是没有使用。

好了，这个先不重要，继续来看。这里才是创建用户坐标系统的重点，换一句话说就是设定的坐标系相当于基坐标系的旋转变换。

    mUserCrd_1 = createMarkN(camera, robot);
	mUserCrd_2 = createMarkN(robot, camera);


	cv::Point2f p1 = cv::Point2f(1000, 1000);
	cv::Point2f p2; //默认为0，0
	cvtUserRobotCrd(p1, p2);

	double angle1 = atan2(p1.y, p1.x);
	double angle2 = atan2(p2.y, p2.x);

	double angle3 = angle2 - angle1;

	mUserRobotAngle_Rz = angle3;


	double rx1 = atan2(0, p1.y);
	double rx2 = atan2(0, p2.y);
	
	mUserRobotAngle_Rx = rx2 - rx1;

最主要的还是上面的这个，这个说白了就是在求刚性（齐次）变换矩阵和旋转角，createMarkN是根据opencv里面的刚性变换矩阵函数estimateRigidTransform求出刚性变换矩阵，求出ABCDEF。

之后利用矩阵相乘的关系，求出用户坐标推换到机器人坐标。换一句话说就是把这个点在用户坐标系推到为机器人坐标系中。

其中点在用户坐标是1000,1000， mUserCrd_1是否销毁用户坐标系，p2是点在机器人坐标系中的坐标。

而知道了p1在用户坐标系下的坐标（摄像头），p2在机器人下的坐标（基坐标系，公式推到而来），那么计算这两个坐标系之间的旋转角度为：

	double angle1 = atan2(p1.y, p1.x);
	double angle2 = atan2(p2.y, p2.x);

	double angle3 = angle2 - angle1;

	mUserRobotAngle_Rz = angle3;

angle3 是点 p2 相对于点 p1 的旋转角度，注意可以看做是绕z轴进行旋转的，这一点很重要，不管是前面的左右手定则，还是坐标系的建立和他们之间的联系，都要时刻牢记这一点。应该很好理解。同理可证，后面的那个是对于x轴方向上的旋转角度。

好了，饶了上面这么一个大圈子，最终得到了用户坐标系上的点在机器坐标系下的点。那么可以做到的一件事是：摄像头捕捉到的点，可以转换到机器人坐标系下，那么这个转换为机器人坐标下的就可以说是机器人末端的位置。

理解了上面的内容，接下来再回到主函数。

将机器人坐标系中的点 rp1 转换到用户坐标系中的点 cp2，并计算一个角度 atn。

这一步是用来验证正逆解是否正确的，逆解得到的关节角度，赋值给正解，看最后末端末端位置姿态是否和给定的一致。此时后面的和前面分析是一样的。

好了。以上就是主函数的全部内容了，其实代码中还有一些直线和圆弧差补，以后再分析（主要看有没有人想看，想看就拿出来分享一下）

可以简单说一下：直线差补有很多种方式，此次程序就是最简单的方式，是deta的方式，计算出斜率和截距来，进行推算下一个位置点的坐标，这种方法不是说不好，有一个缺点就是不能够很丝滑的运行从初始点到终止点。还有一种就是利用一个定时周期的方法，利用速度和点的位置来计算，在使用其他的方法进行优化，这一方法不但大大减小了不够丝滑的运行，还解决可以进行高速运行。

每日一感悟：遗憾是什么?是初见少年拉满弓，不惧岁月不惧风。可终是东风吹醒英雄梦，生活磨平少年心。原以为山一程，水一程，人生何处不相逢，可后来才发现，一别再无归期，相见只在梦里，是这样吗?世事难两全，得失总相伴。

或许遗憾才是常态，不完美才是人生。