【四足大创】1.2单腿足尖的曲线控制
思路
已知足尖的运动轨迹,每隔一小段时间取一个点,通过该点足尖的坐标,倒算出舵机此刻的角度,再让舵机转动到解算出的角度。
Tip:设置几个参数(如足尖运动方程,周期长度,取点间隔等),程序开始运行时,批量计算特定时刻足尖位置,以及位置坐标对应的舵机角度,然后存入数组,之后调用数组数据即可。
足尖曲线
如图所示,曲线分为两部分,上部是一摆线,下部是一条直线,足尖在曲线上周期循环。假设一个周期为2秒,以胯关节为原点,向右为x轴正方向,向下为y轴正方向,则曲线方程可以为(具体参数乱编的):
unsigned long long time;
double t,x,y;
time=millis();
time=time/2000;
t=12.56*time/1000;
if (t<6.28){
x=t-sin(t)-3.14;
y=-1+cos(t)+10;
} else {
x=-t+9.42
y=10;
}
运动学逆解算
令足尖C点的坐标为(x0,y0),顺时针为x轴正方向,上肢长L1,下肢长L2,C点距原点L0,胯关节舵机应转动至angle_1,膝关节应转动至angle_2,则如图所示:
L0=sqrt(x02+y02);
θ=-arctan(x0/y0);
α=arccos((L02+L12-L22)/(2 * L0 * L1));
β=arccos((L12+L22-L02)/(2 * L1 * L2));
angle_1=init_1+α+θ;
angle_2=init_2+β-90;