四足机器人单腿分析小记 平面上已知连杆末端轨迹，在约束条件下各杆的移动
程序员文章站
2022-07-14 22:15:54
...
已知条件1，也就是初始值
步长：S 最大抬腿高度：H 大腿长度： l1 小腿长度： l2 肩关节初始坐标(二维)：(250，450)
代码描述：
l1 = 220#大腿长度400mm
l2 = 200#小腿长度350mm
h = 980#机器人高度980mm
#固定的关节的坐标是(0,0)
# x1=0.0 #膝关节的坐标位：(x2,y2)
# y1=0.0
thyta1 = pi/4 #臀关节的角度45
thyta2 = 2*pi/3 #膝关节的角度60
S = 150 #步长
H = 50 #最大抬腿高度
Tm = 0.5 #一个周期
speed = 0.05 #运行速度，可自行改变
t = 0 #t时刻初始值
上面的参数中，今天的文章中不用：h 这个机器人高度变量
已知条件2，平面上已知连杆末端轨迹，即一条摆线方程，如图：
那么，根据我自己的实际状况也就是已知条件来分析，在二维空间内，适合我已知条件的摆线方程的起点应该落在，足端初始化位置的地方
因为两个杆，也就是大腿l1，小腿长度l2 已知，大腿起始一端固定[1]. 大腿，小腿分别的偏转角度已知，所以很容易用程序仿真出初始位置；
我用的是Python程序仿真编程，主要用的库是math和matplotlib;
程序开始，导入有用的库
import numpy as np
import matplotlib.pyplot as plt
import sys
from math import *然后通过公式计算膝关节，足端初始坐标位置：
由于我一开始用的是pygame模块，而且我的关节都用圆圈来代替，由于pygame比较抠脚的地方就是pygame.draw.circle函数里的点坐标必须是整数，这一点让我很无奈，重新写程序，才用起matplot库.....
#机器人初始状态
def robot_init():
#x2 = 250 - l1*cos(45)
#y2 = 100 + l1*sin(45)
x1 = 250-l1*cos(thyta1)+l2*cos(pi-thyta2)
y1 = 450-l1*sin(thyta1)-l2*sin(pi-thyta2)
print(x1,y1)
#pygame.draw.line(screen, BLUE, (100, 200), (540, 250), 1)
#pygame.draw.line(screen, BLUE, (250, 100), (x2, y2), 1)
# pygame.draw.circle(screen, GREEN, (x1, y1), 2,0) #足端
# pygame.draw.line(screen, BLUE, (x1, y1), ((x1-l2*cos(thyta2)),(y1-l2*sin(thyta2))), 1)
# pygame.draw.circle(screen, GREEN, ((x1-l2*cos(thyta2)),(y1-l2*sin(thyta2))), 2,0)
# pygame.draw.line(screen, BLUE, (250,100), ((x1-l2*cos(thyta2)),(y1-l2*sin(thyta2))), 1)如图初始位置：
然后，我的足端初始化坐标为(x1,y1)：
x1 = 250-l1*cos(thyta1)+l2*cos(pi-thyta2)
y1 = 450-l1*sin(thyta1)-l2*sin(pi-thyta2)
print(x1,y1)为：
所以足端轨迹起始点也是(x1,y1)，紧接着通过上述的轨迹方程我们用程序改写：
这个是原论文 的公式描述：
因为相当于坐标原点平移，将原来的摆线方程原点平移到点(x1,y1)
#固定坐标系我这里代码为(0,0).二维空间里竖直，水平两个方向
def trot(t):
#global Tm,l1,l2,thyta1,thyta2,S,H
x = S*(t/Tm-1/(2*pi)*sin(2*pi*t/Tm))+250-l1*cos(thyta1)+l2*cos(pi-thyta2)
y = H*(1/2-0.5*cos(2*pi*t/Tm))+450-l1*sin(thyta1)-l2*sin(pi-thyta2)
#ax.plot(x, y, 'ro') #足端
#print(t)
#print(x,y) #打印运动2个周期的足端坐标点
return x,y我实物四足狗狗的代码长这样：
//对角小跑步态
void trot()//小跑步态
{
d=atan(z/Hc);
r1=0.72;//大腿初始角度，约45度
r2=1.55;//小腿初始角度，约90度
H0=60;//抬腿最大高度，Y轴方向
a=60;//各腿Z轴方向坐标
if(S0==0&&z==0)
{
H0=0;//不抬腿
a=65;//各腿Z轴方向坐标
}
if(z>0)
{
a=62;//各腿Z轴方向坐标
H0=50;//抬腿最大高度，Y轴方向
z=18;//Z轴变化角度，即肩关节横向转角
}
if(z<0)
{
a=62;
H0=50;
z=-18;
}
if(t<Tm)//前半周期，1、4腿在后；2、3腿在前
{
c=cos(d*cos(PI*t/Tm));//c参数，接下来带入方程
b=(0.5-0.5*cos(2*PI*t/Tm));//b参数，接下来带入方程
x0=S0*(t/Tm-1/(2*PI)*sin(2*PI*t/Tm))-0.5*S0;//腿1 X轴坐标，前进方向，S0为迈腿长度
y0=(H0*b-Hc)/c;//腿1 Y轴坐标，抬腿方向
z0=a+z*cos(PI*t/Tm);//腿1 Z轴坐标，转弯方向
x3=-x0;//腿4 X轴坐标，前进方向（腿外侧为正向）
y3=y0;//腿4 Y轴坐标，抬腿方向
z3=z0;//腿4 Z轴坐标，转弯方向
x1=-x0;//腿2 X轴坐标，前进方向
y1=(-H0/40*b-Hc)/c;//腿2 Y轴坐标，抬腿方向
z1=z0;//腿2 Z轴坐标，转弯方向
x2=-x1;//腿3 X轴坐标，前进方向（腿外侧为正向）
y2=y1;//腿3 Y轴坐标，抬腿方向
z2=z0;//腿3 Z轴坐标，转弯方向
}
if(t>=Tm)//后半周期，2、3腿在后；1、4腿在前
{
c=cos(d*cos(PI*t/Tm));
b=(0.5-0.5*cos(2*PI*(t-Tm)/Tm));
x0=0.5*S0-S0*((t-Tm)/Tm-1/(2*PI)*sin(2*PI*(t-Tm)/Tm));
y0=(-H0/40*b-Hc)/c;
z0=a+z*cos(PI*t/Tm);
x3=-x0;
y3=y0;
z3=z0;
x1=-x0;
y1=(H0*b-Hc)/c;
z1=z0;
x2=-x1;
y2=y1;
z2=z0;
}
function2();//调用函数，将坐标转为各关节运动角度
sizhi();//调用函数，将各关节角对应为舵机角并驱动输出
if(t>=Tm*2)//一个完整的运动周期结束
{
t=0;
}
}我的实物是通过手柄操控的，但是仿真的时候最方便的用键盘指令，由于今天的文章只是总结性的，所以我大致只写了一小块儿，也就是给函数传值 ‘q’ 表示向前走，即代码描述为：
#控制函数
def key_control(key):
global t
print(t)
if key == 'q':
while True:
if t>Tm:#一个完整的运动周期结束
t=0
break
else:
t=t+speed
k = trot(t)
#print(k[0]-250,k[1]-450)
solution(k[0],k[1])而，我实际的控制狗狗由于有向前走，向后走，所以他们的摆线方程的方向也不一样，但是也有很直观的规律可循，也就是相对于足端坐标z轴来说是前后对称的，步长 S 有正负之分
if(ps2x.Button(PSB_L2)) //爬行步态
{ //print stick values if either is TRUE
k=0;
S0=127-ps2x.Analog(PSS_LY); //迈腿长度
z=(ps2x.Analog(PSS_RX)-128)/4.5; //拐弯，Z轴坐标
if(S0>0)
{
t=t+0.065; //时间不断变化和采集，改变参数可改变四足机器人速度
walk();
}
if(S0<0)
{
t=t+0.065; //时间不断变化和采集，改变参数可改变四足机器人速度
back();
}
}这个就是在每次增加speed时刻所展示的足端轨迹离散点
现在，得到了足端轨迹，那么，就可以通过运动学逆解计算各个关节点的旋转角度thyta1，thyta2了。
这篇文章教的是机械臂的运动学逆解，并且也是在二维平面内的两个连杆的运动描述，所以，大致思路看这里
http://www.docin.com/p-1402057960.html
这里，仿真的运动逆解我的代码描述是：
def solution(x,y):
#global thyta2,thyta1
xp = x #- 250
yp = y #- 450
#global thyta1,thyta2
a = pow(xp,2)+pow(yp,2)+pow(l1,2)-pow(l2,2)
b = cos(atan(yp/xp))
c = pow(xp,2)+pow(yp,2)+pow(l2,2)-pow(l1,2)
#print(c*b/(2*l2*xp))
#print(-acos(a*b/(2*l1*x))+atan2(x,y)) #出错原因是a*b/(2*l1*x)>1
#逆解求得的关节角度
thyta1 = -acos(a*b/(2*l1*xp))+atan2(xp,yp)
thyta2 = acos(c*b/(2*l2*xp))+atan2(xp,yp)
#print(thyta2)
xo = 250-l1*cos(thyta1)+l2*cos(pi-thyta2)
#print(x,xo+250)
#ax.plot(x, y, 'ro') #足端
# #加一个长度判断
# m = pow(l1*cos(thyta1),2)+pow(l1*sin(thyta1),2)
# d1 = sqrt(m)
# n = int(pow(l2*np.cos(thyta2),2)+pow(l2*np.cos(thyta2),2))
ax.plot(x, y, 'ro') #足端
ax.plot([x,250-l1*cos(thyta1)],[y,450-l1*sin(thyta1)] , label='sin')#画直线
ax.plot(250-l1*cos(thyta1), 450-l1*sin(thyta1), 'ro') #膝端
ax.plot([250,250-l1*cos(thyta1)],[450,450-l1*sin(thyta1)] , label='sin')#画直线
ax.plot(250 , 450, 'ro') #臀端我实物四足机器人的逆解代码长这样：
void function()//逆解函数
{
w01=atan(-z0/y0)-atan(L1/sqrt(y0*y0+z0*z0-L1*L1));//腿1转弯角度（肩关节角度）
w11=atan(-z1/y1)-atan(L1/sqrt(y1*y1+z1*z1-L1*L1));//腿2转弯角度（肩关节角度）
w21=atan(-z2/y2)-atan(L1/sqrt(y2*y2+z2*z2-L1*L1));//腿3转弯角度（肩关节角度）
w31=atan(-z3/y3)-atan(L1/sqrt(y3*y3+z3*z3-L1*L1));//腿4转弯角度（肩关节角度）
M0=(-y0+L1*sin(w01))/cos(w01);
M1=(-y1+L1*sin(w11))/cos(w11);
M2=(-y2+L1*sin(w21))/cos(w21);
M3=(-y3+L1*sin(w31))/cos(w31);
w03=acos(-(x0*x0+M0*M0-L2*L2-L3*L3)/(2*L2*L3))-r2;//腿1小腿摆动角度
w13=acos(-(x1*x1+M1*M1-L2*L2-L3*L3)/(2*L2*L3))-r2;//腿2小腿摆动角度
w23=acos(-(x2*x2+M2*M2-L2*L2-L3*L3)/(2*L2*L3))-r2;//腿3小腿摆动角度
w33=acos(-(x3*x3+M3*M3-L2*L2-L3*L3)/(2*L2*L3))-r2;//腿4小腿摆动角度
w02=-asin(x0/sqrt(x0*x0+M0*M0))+acos((x0*x0+M0*M0+L2*L2-L3*L3)/(2*L2*sqrt(x0*x0+M0*M0)))-r1;//腿1大腿摆动角度
w12=-asin(x1/sqrt(x1*x1+M1*M1))+acos((x1*x1+M1*M1+L2*L2-L3*L3)/(2*L2*sqrt(x1*x1+M1*M1)))-r1;//腿2大腿摆动角度
w22=-asin(x2/sqrt(x2*x2+M2*M2))+acos((x2*x2+M2*M2+L2*L2-L3*L3)/(2*L2*sqrt(x2*x2+M2*M2)))-r1;//腿3大腿摆动角度
w32=-asin(x3/sqrt(x3*x3+M3*M3))+acos((x3*x3+M3*M3+L2*L2-L3*L3)/(2*L2*sqrt(x3*x3+M3*M3)))-r1;//腿4大腿摆动角度
}然后通过解算各关节点的坐标，然后仿真出每一 t 时刻的位置，即代码描述：
def solution(x,y):
#global thyta2,thyta1
xp = x #- 250
yp = y #- 450
#global thyta1,thyta2
a = pow(xp,2)+pow(yp,2)+pow(l1,2)-pow(l2,2)
b = cos(atan(yp/xp))
c = pow(xp,2)+pow(yp,2)+pow(l2,2)-pow(l1,2)
#print(c*b/(2*l2*xp))
#print(-acos(a*b/(2*l1*x))+atan2(x,y)) #出错原因是a*b/(2*l1*x)>1
#逆解求得的关节角度
thyta1 = -acos(a*b/(2*l1*xp))+atan2(xp,yp)
thyta2 = acos(c*b/(2*l2*xp))+atan2(xp,yp)
#print(thyta2)
xo = 250-l1*cos(thyta1)+l2*cos(pi-thyta2)
#print(x,xo+250)
#ax.plot(x, y, 'ro') #足端
# #加一个长度判断
# m = pow(l1*cos(thyta1),2)+pow(l1*sin(thyta1),2)
# d1 = sqrt(m)
# n = int(pow(l2*np.cos(thyta2),2)+pow(l2*np.cos(thyta2),2))
ax.plot(x, y, 'ro') #足端
ax.plot([x,250-l1*cos(thyta1)],[y,450-l1*sin(thyta1)] , label='sin')#画直线
ax.plot(250-l1*cos(thyta1), 450-l1*sin(thyta1), 'ro') #膝端
ax.plot([250,250-l1*cos(thyta1)],[450,450-l1*sin(thyta1)] , label='sin')#画直线
ax.plot(250 , 450, 'ro') #臀端运行程序，它长这样：
原论文是，固定点也随 t 移动
当实际做机器人的时候，上述结算出来的角度就可以通过转换传给控制器，我用的控制器是 舵机，所以，转换就得转换成舵机所需的PWM，实际的转换代码长这样：
//12*度，所以12个舵机
void change()//关节计算角度对应到舵机转角
{
pwm.setPWM(9, 0,z9-w11*100);//第9路舵机转角，腿2肩关节横向转角
pwm.setPWM(1, 0,Z1-w12*100);//第1路舵机转角，腿2大腿转角
pwm.setPWM(5, 0,z5+w13*100);//第5路舵机转角，腿2小腿转角
pwm.setPWM(8, 0,z8+w01*100);//第8路舵机转角，腿1肩关节横向转角
pwm.setPWM(0, 0,Z0+w02*100);//第0路舵机转角，腿1大腿转角
pwm.setPWM(4, 0,z4-w03*100);//第4路舵机转角，腿1小腿转角
pwm.setPWM(10, 0,z10-w21*100);//第10路舵机转角，腿3肩关节横向转角
pwm.setPWM(2, 0, Z2-w22*100);//第2路舵机转角，腿3大腿转角
pwm.setPWM(6, 0, z6+w23*100);//第6路舵机转角，腿3小腿转角
pwm.setPWM(11,0,z11+w31*100);//第11路舵机转角，腿4肩关节横向转角
pwm.setPWM(3, 0, Z3+w32*100);//第3路舵机转角，腿4大腿转角
pwm.setPWM(7, 0, z7-w33*100);//第7路舵机转角，腿4小腿转角
}所以，静态步态通过运动学逆解得来，即已知末端轨迹方程，逆解得出各个关节的转角。
由于实际的是四条腿，今天原理展示的是一条腿，那么，实际的机器人如果以上述对角小跑步态移动的话每条腿都有时序规律的：
其中就有这么些步态时序，而我通过实际操作，得出上述图中矩形对角线的足端向前运动的时序是一致的时候效果巨佳。
我自己的机器人代码描述：
if t<Tm:#前半周期，1、4腿在后；2、3腿在前
#print("前半周期")
c=cos(d*cos(pi*t/Tm))#c参数，接下来带入方程
b=(0.5-0.5*cos(2*pi*t/Tm))#b参数，接下来带入方程
x0=S0*(t/Tm-1/(2*pi)*sin(2*pi*t/Tm))-0.5*S0#腿1 X轴坐标，前进方向，S0为迈腿长度
y0=(H0*b-Hc)/c#腿1 Y轴坐标，抬腿方向
z0=a+z*cos(pi*t/Tm)#腿1 Z轴坐标，转弯方向
x3=-x0#腿4 X轴坐标，前进方向（腿外侧为正向）
y3=y0#腿4 Y轴坐标，抬腿方向
z3=z0#腿4 Z轴坐标，转弯方向
x1=-x0#腿2 X轴坐标，前进方向
y1=(-H0/40*b-Hc)/c#腿2 Y轴坐标，抬腿方向
z1=z0#腿2 Z轴坐标，转弯方向
x2=-x1#腿3 X轴坐标，前进方向（腿外侧为正向）
y2=y1#腿3 Y轴坐标，抬腿方向
z2=z0#腿3 Z轴坐标，转弯方向
#print(x0,y0,z0,x1,y1,z1,x2,y2,z2,x3,y3,z3)这些参数都表示足端轨迹坐标，例如：x0,x1,x2,x3
if t>=Tm:#后半周期，2、3腿在后；1、4腿在前
#print("后半周期")
c=cos(d*cos(pi*t/Tm))
b=(0.5-0.5*cos(2*pi*(t-Tm)/Tm))
x0=0.5*S0-S0*((t-Tm)/Tm-1/(2*pi)*sin(2*pi*(t-Tm)/Tm))
y0=(-H0/40*b-Hc)/c
z0=a+z*cos(pi*t/Tm)
x3=-x0
y3=y0
z3=z0
x1=-x0
y1=(H0*b-Hc)/c
z1=z0
x2=-x1
y2=y1
z2=z0
#print(x0,y0,z0,x1,y1,z1,x2,y2,z2,x3,y3,z3)在自己实际做狗的时候，实际与理论结合非常重要。
即使仿真的步态有多么多么好，但是机械结构，舵机如果粗略，随便选择，那么都会影响最后的实际效果的
如果有朋友发现我今天这篇哪里有点小毛病就请多多指教。