距离上一篇文章已经有一个半月了，越咕越得劲（其实是因为小学期整天上实验课啦），前天刚结束小学期的课程，觉得是时候给这个小项目结个尾了。

我花了四个小时设计机械结构，十二个小时接线，三个小时写代码，九个小时改错调试，最后的作品长这样：

（~~这TM也太丑了吧~~）

因为能力有限，机械结构的刚度也不够，前面文章所设想的部分功能未能实现，深表遗憾。

骚话不多说，直接进正题。

腿部运动

我使用的是飞特的SCS0009舵机，体积很小，扭矩足够，串行总线，简直不要太爽！

因为舵机个体以及舵机安装方式的问题，同一个位置信号发过去，不同的舵机反应不一样，所以最开始我们需要校准。

我记录的是四条腿都伸直的时候的舵机位置反馈值，存储在一个数组里面：

int init_position[12] = {550, 641, 796,
			 525, 491, 79,
			 600, 864, 766,
			 274, 342, 207};

当然小狗是不能保持伸直的，所以我在初始化的时候给了一个足尖初始坐标(0,0,86.6)，坐标系的建立见后面的图。

从前面的文章（四足机器人制作（一）腿部运动算法）可以看出，我的腿部控制逻辑为：先确定足尖的坐标，然后根据几何关系求解出腿部三个舵机的角度，最后转换为对应的舵机位置信号。先上代码：

void LegCalc(struct Leg_t* leg)
{
	leg->x =  leg->delta_x;
	leg->y =  leg->delta_y;
	leg->z =  86.6f + leg->delta_z;
	float L;
	L = sqrt(leg->x * leg->x + leg->y * leg->y + leg->z * leg->z);
	leg->angle_1 = atan(leg->y/leg->z);
	leg->angle_2 = acos(L/100) - asin(leg->x/L);
	leg->angle_3 = acos(1 - L*L/5000);
	leg->angle_1 *= RAD_TO_DEGREE;
	leg->angle_2 *= RAD_TO_DEGREE;
	leg->angle_3 *= RAD_TO_DEGREE;
	leg->angle_3 = 180 - leg->angle_3;
	leg->position[0] = leg->bias_position[0] + leg->coefficient_1 * leg->angle_1 * ANGLE_TO_ENCODER;
	leg->position[1] = leg->bias_position[1] + leg->coefficient_2 * leg->angle_2 * ANGLE_TO_ENCODER;
	leg->position[2] = leg->bias_position[2] + leg->coefficient_2 * leg->angle_3 * ANGLE_TO_ENCODER;
}

这里写的有点不规范，我为了图方便直接把我的狗腿长度带入计算公式化简了。我的小狗大腿和小腿长度都是50mm。

模型如下（注意坐标系的原点和坐标轴正方向）：

其中AF和FD长度都是50mm，可以通过解三角形得到，α（对应代码中angle_2），β（angle_3），γ（angle_1）的计算公式如下：

$$\gamma = arc\tan(\frac{BC}{CD})$$ $$\alpha = arc\cos(\frac{AD^2+AF^2-DF^2}{2AD·AF}) - arc\sin(\frac{DE}{AD})$$ $$\beta = 180 - arc\cos(\frac{AF^2+FD^2-AD^2}{2AF·FD})$$

把坐标和腿长带进去，就得到上面的代码。

上面的代码中，delta_x之类的变量记录的是机器人各种变化（比如走路）对单条腿产生的坐标变化值，将它叠加在腿的初始位置上就得到了这个时刻腿的目标位置。根据这个位置计算完角度后，将角度换算为舵机的位置信息（最后三行），这里有变量coefficient_1和coefficient_2，这是因为在机械结构上，不同的舵机安装方式不同，对于“角度增大”这一指令，它既可能逆时针旋转，也可能顺时针旋转，所以在初始化的时候为每条腿设置了对应的系数（1或-1），拿右前腿为例，它的初始化代码如下：

void BodyInit(void)
{
	FR_Leg.IDs[0] = 1;
	FR_Leg.IDs[1] = 2;
	FR_Leg.IDs[2] = 3;
	FR_Leg.bias_position[0] = init_position[0];
	FR_Leg.bias_position[1] = init_position[1];
	FR_Leg.bias_position[2] = init_position[2];
	FR_Leg.coefficient_1 = 1;
	FR_Leg.coefficient_2 = -1;
	FR_Leg.x = 0.0f;
	FR_Leg.y = 0.0f;
	FR_Leg.z = 86.6f;
}

其实从上面的信息中还可以看出，86.6f正好是50的根号3倍，所以我的狗腿初始位置如下：

姿态解析

这里涉及到的姿态解析和之前的文章里讲的有些不同，因为坐标系不一样，先看yaw轴吧。

yaw

yaw轴和前述的情况一样，具体公式及推导见四足机器人制作（二）姿态解析，实现代码如下：

	float yaw_angle = 42.249f - body.yaw;
	yaw_angle /= RAD_TO_DEGREE;
	float yaw_delta_x = 56.95f - 84.7f*sin(yaw_angle);
	float yaw_delta_y = 84.7f * cos(yaw_angle) - 62.7f;

顺便一提，我的小狗长为125.4mm，宽为113.9mm，上面出现的数值都是我带入公式之后化简后的。

pitch

pitch轴这里和之前的文章里写的不同，先看抽象一下：

这幅图有点小复杂，最关键的地方在于图中的那个小红色三角形，显然这是等腰三角形，顶角α就是机器狗的pitch轴角度，然后根据机械结构可以算出三角形的腰长L，以及图中的θ角，于是在红色三角形内通过解三角形就可以求出β角，底边长度，进而求出坐标的变化量。最后的公式如下： $$l = \sqrt{2L^2(1-\cos\alpha)}$$ $$\beta = \theta - \frac{\alpha}{2}$$ $$\delta x = -l\cos(\beta)$$ $$\delta z = l\sin(\beta)$$ 代码如下：

	float pitch_angle = 90.0f - Abs(body.pitch)/2;
	pitch_angle -= 53.489f;
	pitch_angle /= RAD_TO_DEGREE;
	float tmp_length = sqrt(22210.76f*(1-cos(body.pitch/57.596f)));
	float pitch_delta_x = tmp_length * cos(pitch_angle);
	float pitch_delta_z = tmp_length * sin(pitch_angle);

roll

roll轴和pitch轴的分析方法一模一样，就不再展开讲了，代码如下：

	tmp_length = sqrt(20834.785f*(1-cos(body.roll/57.596f)));
	pitch_angle = 90.0f - Abs(body.roll)/2;
	pitch_angle -= 56.084f;
	pitch_angle /= RAD_TO_DEGREE;
	float roll_delta_y = tmp_length * cos(pitch_angle);
	float roll_delta_z = tmp_length * sin(pitch_angle);

最后把三个轴整合起来：

void AttitudeParse(void)
{
	float yaw_angle = 42.249f - body.yaw;
	yaw_angle /= RAD_TO_DEGREE;
	float yaw_delta_x = 56.95f - 84.7f*sin(yaw_angle);
	float yaw_delta_y = 84.7f * cos(yaw_angle) - 62.7f;
	
	float pitch_angle = 90.0f - Abs(body.pitch)/2;
	pitch_angle -= 53.489f;
	pitch_angle /= RAD_TO_DEGREE;
	float tmp_length = sqrt(22210.76f*(1-cos(body.pitch/57.596f)));
	float pitch_delta_x = tmp_length * cos(pitch_angle);
	float pitch_delta_z = tmp_length * sin(pitch_angle);
	if(body.pitch < 0)
	{
		pitch_delta_x = -pitch_delta_x;
		pitch_delta_z = -pitch_delta_z;
	}
	
	tmp_length = sqrt(20834.785f*(1-cos(body.roll/57.596f)));
	pitch_angle = 90.0f - Abs(body.roll)/2;
	pitch_angle -= 56.084f;
	pitch_angle /= RAD_TO_DEGREE;
	float roll_delta_y = tmp_length * cos(pitch_angle);
	float roll_delta_z = tmp_length * sin(pitch_angle);
	if(body.roll < 0)
	{
		roll_delta_y = -roll_delta_y;
		roll_delta_z = -roll_delta_z;
	}
	
	FR_Leg.delta_x = yaw_delta_x - pitch_delta_x;
	FR_Leg.delta_y = -yaw_delta_y + roll_delta_y;
	FR_Leg.delta_z = pitch_delta_z - roll_delta_z;
	
	FL_Leg.delta_x = -yaw_delta_x - pitch_delta_x;
	FL_Leg.delta_y = -yaw_delta_y + roll_delta_y;
	FL_Leg.delta_z = pitch_delta_z + roll_delta_z;
	
	BL_Leg.delta_x = -yaw_delta_x - pitch_delta_x;
	BL_Leg.delta_y = yaw_delta_y + roll_delta_y;
	BL_Leg.delta_z = -pitch_delta_z + roll_delta_z;
	
	BR_Leg.delta_x = yaw_delta_x - pitch_delta_x;
	BR_Leg.delta_y = yaw_delta_y + roll_delta_y;
	BR_Leg.delta_z = -pitch_delta_z - roll_delta_z;
}

有了姿态，那IMU平衡简直轻而易举，代码如下：

if(body.workstate == Body_Stable)
{
	body.pitch = -mpu6500.angle.pitch;
	body.roll = mpu6500.angle.roll;
	if(body.pitch > 5) body.pitch = 5;
	if(body.pitch < -5) body.pitch = -5;
	if(body.roll > 5) body.roll = 5;
	if(body.roll < -5) body.roll = -5;
	AttitudeParse();
}

效果如下：（~~好像效果不是很明显~~）

全向移动

在之前的理论分析中，我还以为我能实现walk（走）、pace（踱步）和trot（小跑），没想到只能实现速度最慢的walk。先回顾一下walk是什么样子：

仔细分析可以发现，四条腿是等周期间隔依次落地和抬起的，顺序可以是：右前→左后→左前→右后。假设间隔周期为T，那么对于一条腿来说，抬腿往前迈的动作需要在T内完成，然后保持放在地上的状态3T时间，然后循环。

考虑到前进、横移、转弯三种命令可能同时执行，也方便代码的书写，我把抬腿这个动作单独用一个函数计算，代码如下：

void ListLeg(void)
{
	static int count = 0;
	if(Abs(body.vx) < 2 && Abs(body.vy) < 2 && Abs(body.rotate) < 2)
	{
		count = 0;
		FR_Leg.state = Down;
		BL_Leg.state = Down;
		FL_Leg.state = Down;
		BR_Leg.state = Down;
		FR_Leg.delta_z = 0;
		BL_Leg.delta_z = 0;
		FL_Leg.delta_z = 0;
		BR_Leg.delta_z = 0;
		return;
	}
	count ++;
	uint8_t cycle = 36;
	FR_Leg.last_state = FR_Leg.state;
	BL_Leg.last_state = BL_Leg.state;
	FL_Leg.last_state = FL_Leg.state;
	BR_Leg.last_state = BR_Leg.state;
	if(count<cycle)
	{
		FR_Leg.state = Up;
		BL_Leg.state = Down;
		FL_Leg.state = Down;
		BR_Leg.state = Down;
		if(count < cycle/2)
		{
			FR_Leg.delta_z = -count*40/cycle;
		}
		else
		{
			FR_Leg.delta_z = -(cycle-count)*40/cycle;
		}
	}
	else if(count < 2 * cycle)
	{
		FR_Leg.state = Down;
		BL_Leg.state = Up;
		FL_Leg.state = Down;
		BR_Leg.state = Down;
		if(count < cycle*3/2)
		{
			BL_Leg.delta_z = -(count-cycle)*40/cycle;
		}
		else
		{
			BL_Leg.delta_z = -(2*cycle-count)*40/cycle;
		}
	}
	else if(count < 3 * cycle)
	{
		FR_Leg.state = Down;
		BL_Leg.state = Down;
		FL_Leg.state = Up;
		BR_Leg.state = Down;
		if(count < cycle*5/2)
		{
			FL_Leg.delta_z = -(count-2*cycle)*40/cycle;
		}
		else
		{
			FL_Leg.delta_z = -(3*cycle-count)*40/cycle;
		}
	}
	else if(count < 4 * cycle)
	{
		FR_Leg.state = Down;
		BL_Leg.state = Down;
		FL_Leg.state = Down;
		BR_Leg.state = Up;
		if(count < cycle*7/2)
		{
			BR_Leg.delta_z = -(count-3*cycle)*40/cycle;
		}
		else
		{
			BR_Leg.delta_z = -(4*cycle-count)*40/cycle;
		}
	}
	else
	{
		count = 0;
	}
}

函数写的很~~啰嗦~~（算了就这样吧），上面的函数每5ms执行一次，所以周期间隔T为180ms。在函数中四条腿按照设定的顺序依次抬起放下。

剩下的三种运动用另一个函数表示，也就是说delta_z由一个函数控制，delta_x和delta_y由另一个函数控制。

运动函数代码如下：

void GoStraight(struct Leg_t * leg)
{
	uint8_t cycle = 36;
	int StepLength = 2*body.vx;
	int LR_StepLength = 2*body.vy;
	int R_StepLength = 2*body.rotate;
	if(Abs(body.vx) < 2 && Abs(body.vy) < 2 && Abs(body.rotate) < 2)
	{
		leg->delta_x = 0;
		leg->count = 0;
		return;
	}
	if(leg->state != leg->last_state) leg->count = 0;
	leg->count++;
	if(leg->state == Up)
	{
		leg->delta_x = -StepLength+leg->count*2*StepLength/cycle;
		leg->delta_y = -LR_StepLength+leg->count*2*LR_StepLength/cycle;
		if(leg == &FR_Leg || leg == &FL_Leg)
		{
			leg->delta_y += -R_StepLength+leg->count*2*R_StepLength/cycle;
		}
		else
		{
			leg->delta_y -= -R_StepLength+leg->count*2*R_StepLength/cycle;
		}
	}
	else if(leg->state == Down)
	{
		leg->delta_x = StepLength-leg->count*2*StepLength/(3*cycle);
		leg->delta_y = LR_StepLength-leg->count*2*LR_StepLength/(3*cycle);
		if(leg == &FR_Leg || leg == &FL_Leg)
		{
			leg->delta_y += R_StepLength-leg->count*2*R_StepLength/(3*cycle);
		}
		else
		{
			leg->delta_y -= R_StepLength-leg->count*2*R_StepLength/(3*cycle);
		}
	}
}

为展示得更直观，只看前进的部分吧：

void Walk(struct Leg_t * leg)
{
	uint8_t cycle = 36;
	int StepLength = 2*body.vx;
	if(Abs(body.vx) < 2)
	{
		leg->delta_x = 0;
		leg->count = 0;
		return;
	}
	if(leg->state != leg->last_state) leg->count = 0;
	leg->count++;
	if(leg->state == Up)
	{
		leg->delta_x = -StepLength+leg->count*2*StepLength/cycle;
	}
	else if(leg->state == Down)
	{
		leg->delta_x = StepLength-leg->count*2*StepLength/(3*cycle);
	}
}

从上面的代码中可以看出，在腿抬起阶段（Up），腿从-StepLength匀速移动到StepLength，在放下阶段（Down），又从StepLength匀速移动到-StepLength，实现了前进移动。

横移和转弯也是类似，不做赘述。

没病走两步

云台稳定

在我之前的设想中，这一部分是整个机器人最复杂的部分，也是整个机器人最亮眼的地方，然而经过一番缜密的推算后发现，我之前提出的想法（惯导实现室内定位部分）无法实现，原因是传感器数据不够，本来想着在狗头上加一个光流传感器算了，但是千不该万不该我当初买舵机少买了一个，导致光流没地方装（现在就是后悔，非常后悔.jpg）

但最基本的机器狗原地稳定狗头还是能做的嘛。

我们需要解决两个轴：yaw和pitch，roll轴由于机械方面的限制而无法实现。我们一个一个来。

yaw

yaw轴的模型如下：

这里的计算和前面的pitch轴可以说是非常相似了，我们根据机械结构可以知道L的长度，可以算出β的值，α是机器狗的yaw轴偏转角度，很容易得出以下公式： $$l=\sqrt{2L^2(1-\cos\alpha)}$$ $$\beta = 90 - \frac{\alpha}{2}$$ $$\delta x = -l\cos\beta$$ $$\delta y = -l\sin\beta$$

实现代码如下：

	float Length = sqrt(27852.72f*(1-cos(body.yaw/57.596f)));
	float theta = 90 - body.yaw/2;
	theta /= RAD_TO_DEGREE;
	head.delta_y = -Length * sin(theta);
	head.delta_x = -Length * cos(theta);

pitch

不多说，建模：

和前面简直一模一样，直接上公式： $$l=\sqrt{2L^2(1-\cos\alpha)}$$ $$\beta = 90 - \frac{\alpha}{2}$$ $$\delta x = -l\cos\beta$$ $$\delta z = l\sin\beta$$

实现代码如下：

	Length = sqrt(29529.36f*(1-cos(body.pitch/57.596f)));
	theta = 90 - body.pitch/2;
	theta /= RAD_TO_DEGREE;
	head.delta_x -= Length * cos(theta);
	head.delta_z = -Length * sin(theta);

写在最后

回看发现，四足的底层控制其实不难，耐心分析就行。我的四足之旅到此就暂时停下了，后面或许还有机会再拾起它，代码整理之后也会放在GitHub上供大家参考，希望对四足机器人爱好者有所帮助。

最后简单列一下我的开发环境：

机械设计：SolidWorks 2016
结构材料：PLA 3D打印
电机：飞特SCS0009舵机
电池：12V锂电池
控制芯片：遥控器：STM32F103C8T6；机器狗：STM32F405RGT6
无线通信模块：NRF24L01
陀螺仪：MPU6050
开发环境：STM32CubeMX+Keil 5
代码库：遥控器：HAL库；机器狗：HAL库+FreeRTOS

Jul 21 '19 12:07 imuncle

源代码地址：https://github.com/imuncle/RobotDog

Jul 21 '19 14:07 imuncle

看了机器人学后颇有感触，有空把矩阵用在四足上

Jul 26 '19 15:07 imuncle

很棒！很期待！

Jul 27 '19 04:07 zhuangfengzi

太强了

Jul 30 '19 05:07 FENYUN323

imuncle.github.io
imuncle.github.io copied to clipboard

四足机器人制作（五）代码实现

腿部运动

姿态解析

yaw

pitch

roll

全向移动

云台稳定

yaw

pitch

写在最后

imuncle.github.io imuncle.github.io copied to clipboard

四足机器人制作 （五） 代码实现

腿部运动

姿态解析

yaw

pitch

roll

全向移动

云台稳定

yaw

pitch

写在最后

imuncle.github.io
imuncle.github.io copied to clipboard

四足机器人制作（五）代码实现