四轴scara机器人正解逆解算法，功能有：设定臂长，末端姿态，用户坐标系统及其转换。 不含标定、死区管理，要自己加。 c++源码，vs2015工程，用户坐标系是用opencv库。 支持左手右手两种模式

class ScaraRobot {
public:
    struct Config {
        double L1 = 300.0; // 大臂长度
        double L2 = 250.0; // 小臂长度
        bool isLeftHand = false; // 左手系标志
    };

    cv::Mat userCoordMatrix; // 用户坐标系变换矩阵

    // 正解计算：关节角度转末端坐标
    cv::Point3d forwardKinematics(double theta1, double theta2, double z) const {
        double x = config.L1 * cos(theta1) + config.L2 * cos(theta1 + theta2);
        double y = config.L1 * sin(theta1) + config.L2 * sin(theta1 + theta2);
        return applyCoordTransform(cv::Point3d(x, y, z));
    }
};

这里的applyCoordTransform实现了用户坐标系的转换。我们用OpenCV的矩阵运算处理坐标系变换：

cv::Point3d applyCoordTransform(const cv::Point3d& pt) const {
    cv::Mat src = (cv::Mat_<double>(4,1) << pt.x, pt.y, pt.z, 1);
    cv::Mat dst = userCoordMatrix * src;
    return cv::Point3d(dst.at<double>(0), dst.at<double>(1), dst.at<double>(2));
}

逆解部分稍微复杂，需要处理两种可能的解：

bool inverseKinematics(const cv::Point3d& target, double& theta1, double& theta2) const {
    // 转换到基坐标系
    cv::Point3d basePt = reverseCoordTransform(target);
    
    double x = basePt.x;
    double y = basePt.y;
    
    // 计算第二关节角度
    double cosTheta2 = (x*x + y*y - config.L1*config.L1 - config.L2*config.L2) 
                      / (2 * config.L1 * config.L2);
    if (abs(cosTheta2) > 1.0) return false; // 不可达
    
    theta2 = acos(cosTheta2);
    
    // 两种解的情况处理
    double alpha = atan2(y, x);
    double beta = atan2(config.L2 * sin(theta2), 
                       config.L1 + config.L2 * cos(theta2));
    
    theta1 = alpha - beta;       // 肘部向上解
    double theta1_alt = alpha + beta; // 肘部向下解
    
    // 选择符合关节限制的解
    return checkAngleLimit(theta1, theta2);
}

这里的坐标系镜像处理藏在reverseCoordTransform中，根据左右手模式自动翻转X轴：

cv::Point3d reverseCoordTransform(const cv::Point3d& pt) const {
    cv::Mat invMatrix;
    cv::invert(userCoordMatrix, invMatrix);
    
    cv::Mat src = (cv::Mat_<double>(4,1) << pt.x, pt.y, pt.z, 1);
    cv::Mat dst = invMatrix * src;
    
    // 左手系X轴镜像
    if(config.isLeftHand) dst.at<double>(0) *= -1;
    
    return cv::Point3d(dst.at<double>(0), dst.at<double>(1), dst.at<double>(2));
}

实际使用时可以这样设置用户坐标系：

// 创建偏移+旋转的坐标系
cv::Mat createUserCoord(cv::Point2d offset, double rotateDeg) {
    cv::Mat matrix = cv::Mat::eye(4,4,CV_64F);
    cv::Mat rot = cv::getRotationMatrix2D(cv::Point2f(0,0), rotateDeg, 1.0);
    rot.copyTo(matrix(cv::Rect(0,0,3,2)));
    matrix.at<double>(0,3) = offset.x;
    matrix.at<double>(1,3) = offset.y;
    return matrix;
}

调试时遇到过坑：OpenCV的矩阵乘法默认不考虑齐次坐标，必须显式补1。有个取巧的写法是用4x4变换矩阵，把三维点扩展到(x,y,z,1)再做乘法。

四轴scara机器人正解逆解算法，功能有：设定臂长，末端姿态，用户坐标系统及其转换。 不含标定、死区管理，要自己加。 c++源码，vs2015工程，用户坐标系是用opencv库。 支持左手右手两种模式

完整工程里还包含关节限位检测、速度规划等模块，但核心算法就这些。重点在于坐标变换的矩阵运算和三角函数解算，建议用Eigen替代OpenCV的矩阵计算会更高效，但考虑到用户环境这里保持兼容。