六足机器人步态过程分析及控制

离诟

已于 2024-12-22 18:50:46 修改

阅读量3.3k

点赞数 35

文章标签：机器人

于 2024-06-01 15:58:06 首次发布

本文链接：https://blog.csdn.net/m0_74745278/article/details/139250252

版权

前言

经过了几个STM32的小项目，积攒了一些项目经验，决定做一个六足机器人，购买完舵机和支架组装好机器人，随后就开始了项目。六足机器人可以使用D-H建立模型构建运动学坐标系，再正逆运动学解算腿部末端的位置与各个关节角度的关系式，也可以使用几何法来分析出关系式。网上很容易找到，就不赘述了，这里用C语言直接表示出来。

#include "stdio.h"
#include "math.h"

//六足机器人腿上的三根连轴 
#define L1 45
#define L2 73
#define L3 115


float get_angle1(float x,float y,float z)
{
	double angle;
	angle=atan2(y,x);
	return angle;  
}
/*   注释部分时利用几何法解算的 
float get_angle2(float x,float y,float z)
{
	float angle;
	float R = sqrt(pow(x, 2) + pow(y, 2));
	float Lr=sqrt(pow(z, 2) + pow((R - L1), 2));
	float ar = atan(-z / (R - L1));                                                                                                                                                                                                              
	float a1=acos((pow(L2, 2) + pow(Lr, 2) - pow(L3, 2)) / (2 * Lr * L2));

	angle=a1-ar;

	return angle;
}

float get_angle3(float x,float y,float z)
{
	float angle;
	float R = sqrt(pow(x, 2) + pow(y, 2));
	float Lr=sqrt(pow(z, 2) + pow((R - L1), 2));
	float a1=acos((pow(L2, 2) + pow(Lr, 2) - pow(L3, 2)) / (2 * Lr * L2));
	float a2=acos((pow(Lr, 2) + pow(L3, 2) - pow(L2, 2)) / (2 * Lr * L3));

	angle=-(a1 + a2);
	
	return angle;
}
*/

//该部分时逆运动学解算的 
float get_angle2(float x,float y,float z)
{
	float data= L1-sqrt(pow(x,2)+pow(y,2)) ; 
	float a = (pow(L2,2)-pow(L3,2)+pow(data,2)+pow(z,2) )/(2*L2*sqrt( pow(data,2) +pow(z,2)));
	float b = z/data;
	float angle = acos(a)-atan(b);
	return angle;
}


float get_angle3(float x,float y,float z)
{
	float data= L1-sqrt(pow(x,2)+pow(y,2)) ; 
	float a = (pow(L2,2)-pow(L3,2)+pow(data,2)+pow(z,2) )/(2*L2*sqrt( pow(data,2) +pow(z,2)));
	float b = (pow(L3,2)-pow(L2,2)+pow(data,2)+pow(z,2) )/(2*L3*sqrt( pow(data,2) +pow(z,2)));
	
	float angle=-acos(a)-acos(b);
	
	return angle;
}


//正运动学结算 
float get_x(float angle1,float angle2,float angle3)
{
	return cos(angle1)*( L1+L3*cos(angle2+angle3)+L2*cos(angle2) );
}

float get_y(float angle1,float angle2,float angle3)
{
	return sin(angle1)*(L1+L3*cos(angle2+angle3)+L2*cos(angle2));
}

float get_z(float angle1,float angle2,float angle3)
{
	return L3*sin(angle2+angle3)+L2*sin(angle2);
}

int main(void)
{
    float angle1,angle2,angle3;
	float x,y,z;
	
	//逆运动学解算 
	angle1=get_angle1(100,100,-80);
	angle2=get_angle2(100,100,-80);
	angle3=get_angle3(100,100,-80);

	printf("angle1:%f\r\n",angle1* (180.0 / M_PI));
 	printf("angle2:%f\r\n",angle2* (180.0 / M_PI));
	printf("angle3:%f\r\n",angle3* (180.0 / M_PI));
	
	//正运动学结算 
	x=get_x(angle1,angle2,angle3);
	y=get_y(angle1,angle2,angle3);	
	z=get_z(angle1,angle2,angle3);
	
	printf("x:%f\r\n",x);
 	printf("y:%f\r\n",y);
	printf("z:%f\r\n",z);
	
    return 0；
}

查找资料的时候在嘉立创开源广场看到了一位大佬做的六足，我的六足机器人的姿态和位置控制就看大佬的PDF写的，如果大家想做六足机器人，建议先看一下这位大佬写的PDF，再来看这篇文章。链接：十二分感谢这位佬https://oshwhub.com/ryzee/Hexapod-robot

一、遇到的问题

看完这位佬的PDF里面的全向移动，我知道了全向移动时六条腿的落脚点位置如何解算出来的了，但还不知道这个运动过程是如何完成的，还有让这六条腿协调运动以及步态控制，经过一些时间的思考，想出了一个解决办法，先上图，下面是我的六足机器人步态时序图。（随手画的，有些潦草，见谅）

二、运动过程

黑色部分是抬腿在空中向前滑动至落脚点的时间（摆动相），白色部分是在地面上向后滑动的时间（站立相）

我认为控制这个步态应该从时间上入手，以黑色部分是抬腿在空中滑动至落脚点（这个运动过程我称为运动一）的时间，为一个时间节拍；白色部分是在地面上向后滑动（这个运动过程我称为运动二）的时间，跟据不同的步态分别为一个时间节拍，两个时间节拍和五个时间节拍。可以通过控制一个节拍的时间，来控制住机器人运动的快慢。

先解决从抬腿点移动至落腿点这个运动，如何使运动一的时间刚好为一个节拍呢，相信下面的公式大家都知道

$\left\{\begin{matrix}x=x_{0}+v_{x}*t & & \\ y=y_{0}+v_{y }*t & & \end{matrix}\right.$

从X-Y平面上看，因为已经知道了落脚点的位置（ $x_{1},y_{1 }$ ），那么就将落脚点的位置减去抬脚点（ $x_{0}$ ， $y_{0}$ ）的位置则就可以得到两点间的位移( $\triangle x,\triangle y$ )，再将位移除以一个时间节拍就可以得到速度 $v_{x}$ 和 $v_{y}$ 了,开始运动时就定时器中断计时得到时间，将时间和速度带入公式当中就可以得到每时每刻的位置坐标（ $x,y$ ）,一个时间节拍结束后就可以精准达到落脚点的位置了。

解决了起点到达落脚点，但还没有将抬腿运动这个问题解决，这里我取了个巧（感觉有点随意了），我感觉抬腿的动作有点像 $y=\sin x$ 函数在 $[0,\Pi ]$ 范围内波形，于是抬腿的函数就出来了。这里将抬腿的高度与需要移动的距离（ $\sqrt{\Delta x^{2}+\Delta y^{2}}$ ）成正比，还需根据自己请况调整比例系数K的大小。