引入
本篇与上篇单腿逆解内容相互联系,单腿逆解和自稳都属于静态解算
理论
![在这里插入图片描述](https://img-blog.csdnimg.cn/20200703160837510.png)
以左前腿作为讨论对象,其他腿只需要类似步骤并更换符号即可
- 向量关系
![在这里插入图片描述](https://img-blog.csdnimg.cn/20200703161428931.png)
- 向量描述
![在这里插入图片描述](https://img-blog.csdnimg.cn/20200703162411689.png)
3.代入计算
![在这里插入图片描述](https://img-blog.csdnimg.cn/20200703163122542.png)
这里,x,y,z就是前篇单腿的x,y,z代入坐标
结语
其余三条腿只需要更换90,82前面的符号即可(90,82分别为站立姿态支撑四边形的长和宽的一半)
整体思路就是把整体的解算向每条腿的解算靠拢。这样已知整体的RPY角和体心的坐标(px,py,pz)就可以得到足端的坐标,进而通过单腿逆解得到整体12个角度值。用IMU的数据作为RPY角的值,即可实现自稳