前言：前段时间在某鱼上买了一份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，0cvtUserRobotCrd(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的方式，计算出斜率和截距来，进行推算下一个位置点的坐标，这种方法不是说不好，有一个缺点就是不能够很丝滑的运行从初始点到终止点。还有一种就是利用一个定时周期的方法，利用速度和点的位置来计算，在使用其他的方法进行优化，这一方法不但大大减小了不够丝滑的运行，还解决可以进行高速运行。
每日一感悟：遗憾是什么?是初见少年拉满弓，不惧岁月不惧风。可终是东风吹醒英雄梦，生活磨平少年心。原以为山一程，水一程，人生何处不相逢，可后来才发现，一别再无归期，相见只在梦里，是这样吗?世事难两全，得失总相伴。
或许遗憾才是常态，不完美才是人生。
