引入 

机器人的运动学正逆解

四足机器人的姿态控制

欧拉角与旋转矩阵之间的转换

  近期在做一个八自由度的四足机器人（以下统称狗），完成了模型搭建、仿真搭建、和步态的仿真开环测试，正打算使用一些传感器进入闭环阶段，对机器狗的姿态自稳进行了一些研究，总结了一些自己的东西。以下参考两篇文章。

【四足机器人】学习笔记 单腿逆运动学和站立姿态控制https://blog.csdn.net/weixin_45417246/article/details/115862456

我的狗子-业余四足机器人笔记（3）-自稳https://blog.csdn.net/qq_43537243/article/details/107107756

正逆解运算

机器人运动学中的正解和逆解应该算是四足机器人入门的基础，在此就不过多的赘述。

在姿态的控制中，关节的角度和足端坐标解算应该都属于静态解算，对应IMU的角度，逆解出关节角度，即可实现四足机器人的自稳效果。

运动学正解：已知关节角度计算出足端坐标

运动学逆解：已知足端坐标计算出关节角度

typedef struct {
    double ex;
    double ey;
    double ez;
} End_point;

//正解计算足端坐标 ，原点为关节中心点 输出为（x,y,z）
void forward_kinematics(int Leg_num, double l1, double l2, double theta1, double theta2) {
    End_point foot_position;

    double hip_x = l1 * cos(theta1);
    double hip_z = l1 * sin(theta1);

    double end_x = hip_x + l2 * cos(theta1 + theta2);
    double end_z = hip_z + l2 * sin(theta1 + theta2);

    foot_position.ex = end_x;
    foot_position.ey = 0; 
    foot_position.ez = end_z;

    return foot_position;

}

#define ARM1_LENGTH 130  // 大臂长度  
#define ARM2_LENGTH 171  // 小臂长度 

// 逆解计算机械臂角度的函数 输入足端位置x,y计算关节角度
//逆解存在多解取最近值，若是解不符合要求在double beta = atan2(-sqrt(1 - d * d), d);  中的sqrt前变号即可
void calculateAngles(double x, double y, double* theta1, double* theta2) {
	double theta = atan2(y, x);
	double d = (x * x + y * y - ARM1_LENGTH * ARM1_LENGTH - ARM2_LENGTH * ARM2_LENGTH) / (2.0 * ARM1_LENGTH * ARM2_LENGTH);
	
		double beta = atan2(-sqrt(1 - d * d), d);   
	*theta2 =  beta;
	
	double alpha = atan2(ARM2_LENGTH * sin(beta), ARM1_LENGTH + ARM2_LENGTH * cos(beta));
	*theta1 = theta - alpha;

	}

IMU如何与四足狗形成闭环

首先我们需要知道开环控制和闭环控制的区别是什么：

机器人系统开环控制是一种控制方法，它不考虑系统的反馈信息，只根据输入信号直接控制输出信号。这种控制方法适用于一些简单的系统，但对于复杂的系统来说，闭环控制更为常用。

机器人系统闭环控制是指通过传感器获取机器人当前状态，然后根据预设的控制算法进行控制，最终达到期望的目标。这个过程中需要不断地进行反馈和调整，以保证机器人的稳定性和精度。

在该四足狗中，IMU就是获取RPY的传感器，机器人通过IMU获取机器人的roll（滚转角）、pitch（俯仰角）、yaw（偏航角），输入狗的控制系统中，得到角度参数输出关节最终达到自稳的效果。在自稳功能中，我们不需要偏航角以及其他的输入，只需要roll和pitch即可，由于狗是八自由度的机器人，其实可以从几何意义上实现角度的转换，但是市面上的大多是十二自由度机器人且为了之后更好的实现功能，我仍然更希望直接用欧拉角转换为旋转矩阵的方式来实现自稳的功能。

// 定义欧拉角结构体
typedef struct {
    double roll;
    double pitch;
    double yaw;
} IMU_Euler;


// 定义旋转矩阵结构体
typedef struct {
    double matrix[3][3];
} RotationMatrix;


// 将欧拉角转换为旋转矩阵
  RotationMatrix eulerToRotationMatrix(IMU_Euler euler) {
    RotationMatrix rotMat;
    euler.yaw = 0;//忽略偏航角的输出
   //X Y Z
    rotMat.matrix[0][0] = cos(euler.pitch) * cos(euler.yaw);
    rotMat.matrix[0][1] = -cos(euler.pitch) * sin(euler.yaw);
    rotMat.matrix[0][2] = sin(euler.pitch);
    rotMat.matrix[1][0] = sin(euler.roll) * sin(euler.pitch) * cos(euler.yaw) + cos(euler.roll) * sin(euler.yaw);
    rotMat.matrix[1][1] = -sin(euler.roll) * sin(euler.pitch) * sin(euler.yaw) + cos(euler.roll) * cos(euler.yaw);
    rotMat.matrix[1][2] = -sin(euler.roll) * cos(euler.pitch);
    rotMat.matrix[2][0] = -cos(euler.roll) * sin(euler.pitch) * cos(euler.yaw) + sin(euler.roll) * sin(euler.yaw);
    rotMat.matrix[2][1] = cos(euler.roll) * sin(euler.pitch) * sin(euler.yaw) + sin(euler.roll) * cos(euler.yaw);
    rotMat.matrix[2][2] = cos(euler.roll) * cos(euler.pitch);
    
    return rotMat;
}

为什么需要讲欧拉角转换为旋转矩阵？

在机器人运动学中，机器人的运动方式都是通过矩阵的形式表示出来，我们需要以矩阵的形式与后面的关系做运算，才能得到机器人的输出。

通过向量运算实现姿态平衡

下面讲关于使用向量计算姿态并输出的方式，可能只有看到这里，你才会知道前面的逆解和欧拉角转换为旋转矩阵的作用。

因为我们研究的是狗的站立自稳，以狗的单腿作为研究对象（其他三条腿亦然），可以设狗质心（最简单的就是对角线交点）在地面的投影为基坐标O，质心点为O'，在狗的自稳状态下，足端B1的位置相对坐标O点的位置是相对不变的，即向量OO'和向量OB为常量。

这个时候我们刚才在前面学习的欧拉角转换为旋转矩阵R就发挥作用了，通过旋转矩阵R，将向量O'A转换姿态发生变化的向量O'A'的形式，就可以通过上述公式求得向量A'B，向量A'B的表现形式为（x,y,0）即相对于A点B的坐标，即相对于关节中心足端坐标。

得到了足端坐标就可以通过逆解计算关节电机的输出。

由结果和IMU的输出可以发现，最终的姿态输出Pitch值仍然存在一定的稳态误差（调到其他角度也仍然如此），说明通过把RPY的输出直接反馈到控制器输入中是存在一定问题的（自动控制原理中提到的稳态误差），仅仅直接输入显然是不太能得到正确结果的，还需要将输出的数据进行一定的处理后再输出回去，如可以采用PID的反馈控制或LQR等。