目录
写在前面
本篇文章介绍的是,运动控制线程padog.py的逻辑顺序这篇文章中如红框所示的部分
运动学逆解原理
推荐预习视频:运动学逆解,观看2:00~9:13即可。
运动学正解,运动学逆解这些名词想必大家都不会太陌生,
运动学正解:已经知道关节角度计算出足端坐标
运动学逆解:已经知道足端坐标计算出关节角度
看完视频想必大家对这个的推导已经非常清晰,如果去网上搜这部分的知识,会有很多教程,你可能会觉得这和我们看的不太一样,但我想说的是,由于我们使用的机器人只是一个 8 自由度的机器人,因此还不需要用到网上 12 自由度的推导方法(旋转矩阵)。
对于推到过程,我只是觉得有几点需要提醒的,计算时,需要注意象限问题。
将单腿模型简化成坐标形式,则任何时刻都存在下式
即可得,当足端坐标处于右半平面(即小于90°)时,由三角形的关系可得;当足端坐标处于左半平面(即大于90°)时,由三角形的关系可得。
运动学逆解PA_IK代码实现
明白了原理之后,我们就可以明确输入的物理量需要什么?首先是 4 条腿的足端坐标,和大小腿的长度,这份程序是可以兼容串联腿和并联腿的,因此有个选择case的变量。
按照我们的物理公式转换成代码,先计算关节,接着计算关节,计算时,需要注意我上面说的,判断象限的问题。算出一条腿后,以此类推算出其余四条即可。
#函数功能:运动学逆解运算库,运算完成后返回每个舵机的转动角度
#变量作用:
# case:0代表串联腿,1代表并连腿。
# l1,l2:机器人大小腿长度,用于运动学计算,
# x1-x4:逆解的足端x坐标
# y1-y4:逆解的足端y坐标
#返回值:8个舵机所需要到达的角度
def ik(case,l1,l2,x1,x2,x3,x4,y1,y2,y3,y4):
if case==0:
#腿2
x2=-x2
shank2=pi-acos((x2*x2+y2*y2-l1*l1-l2*l2)/(-2*l1*l2))
fai2=acos((l1*l1+x2*x2+y2*y2-l2*l2)/(2*l1*sqrt(x2*x2+y2*y2)))
if x2>0:
ham2=abs(atan(y2/x2))-fai2
elif x2<0:
ham2=pi-abs(atan(y2/x2))-fai2
else:
ham2=pi-1.5707-fai2
shank2=180*shank2/pi
ham2=180*ham2/pi
#并联模式和其余腿省略
VMC算法原理
VMC代码实现
(鸽了太久,实在是对不住灯哥了)
在主循环文件中,可以通过对模式的选择进入到VMC代码模块,并将由步态计算出的四个腿的x,y坐标传入到此代码模块中,即P_[0]+ges_x_1等。
VMC代码模块中主要可以分为三部分,即变量的声明,传参进入VMC计算器计算和对VMC计算器计算出的数值进行处理,这里重点要介绍的是传参进入VMC计算器计算这一个部分,重点需要搞明白在这一个部分中,VMC计算器如何将期望足端坐标转化为舵机的输入角度。
VMC计算器计算是对单腿起作用,即输入单腿的坐标得到该腿的角度,故我们对其中一个腿进行讲解即可。VMC计算器的输入量主要有:
def cal_vmc_single_leg(l1,l2,ham1_origin,shank1_origin,Xe,Ze,X,Z,Xl,Zl):
分别代表的意义为,大小腿的长度,长小腿的关节角度,Xe,Ze是期望角度,X,Z是当前角度,Xl,Zl是上次角度。值得注意的是,这与逆运动学相比多了上次角度,这个参数的引入是为了达到计算PID中的D项而做的铺垫。对VMC计算器模块进行分解,其又可以分为6大部分,分别为
1.计算x,y上的力,此项在上文VMC原理中有详细阐述。
2.对变量进行弧度的转换,储存当前位置值为下次循环做准备。
3.利用雅可比矩阵求力矩,此项在上文VMC原理中有详细阐述。
4.建立伪反馈,通过力矩求出角度,此项在上文VMC原理中有详细阐述。此步中的参数整定,由于本人才疏学浅没整明白,懂得大神可以在评论区讲一下,我对于此参数的整定倾向于是通过实际调试所获得的。
5.由正解求足端坐标,属于常规步骤。
6.输出。
至此,VMC计算器模块已经完成任务,返回大小腿关节角度,需要输出的足端坐标和上次的足端坐标。这些值返回到VMC代码模块中,代码模块对所有值进行累加或者更新,得到最新状态下的数据值,并为下次循环做准备!
最后,主循环对VMC计算的值进行输入,就完成了一次完美的VMC循环。
引用文献
李田法. 电机驱动四足机器人能耗优化与高速运动步态规划研究 [D]; 山东大学, 2020.
谢惠祥. 四足机器人对角小跑步态虚拟模型直觉控制方法研究 [D]; 国防科学技术大学, 2015.