四足机器人（二）---运动学逆解和步态规划

逃离我推掉我的手 2022-11-28 00:21 672阅读 0赞

### 四足机器人（二）---运动学逆解和步态规划 ###

*  运动学逆解
 *  步态规划
 *  MATLAB仿真

# 运动学逆解 #

其实运动学分为运动学正解和运动学逆解，二者有什么区别呢？因为在四足机器人中用的是12个舵机，所以运动学正解是已经知道运动关节的各个电机运动参数，也就是此时对于初始位置转动的角度，去求末端执行器的相对参考坐标系的位姿。而运动学逆解恰恰相反，是根据相对参考坐标系的位姿去求出运动关节的各个电机此时对于初始位置转动的角度。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center]  
    由上图可以看出，机体对于地的高度为z，髋关节的长度为h，腿的上半段长度为hu，下半段长度为hl，此时这个状态下，足端和机体的距离为y。如果竖直战力的图像是如下图所示：

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 1]  
    那么对于正视图的运动学力学分析如下图所示：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 2]

在图中dyz’和hu是平行的，我们需要的是T的角度值,那么T = γ-a； 随后我们可以通过勾股定理得出dyz = 根号(z*z+ y*y)。因为我们是正视图，所以对于hu和hl并不是整个腿的长度，lyz = 根号（dyz*dyz - h*h），γ可以算出：γ = atan(y/z)，a = atan(h/lyz)，最终T = γ-a，我们就算出了髋关节电机转动的角度。  
对于侧视图如下：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 3]

由图可以看到lzp = 根号（lyz*lyz + x*x）(lyz是正视图中求得的)，n = (lzp*lzp - hl*hl - hu\*hu)/2hu。此时我们的n已经得到了，那么就可以求出膝关节的角度β = acos(n/hl)。此时P = acos((hu+n)/lzp)。  
& = atan(x/lyz)，最终得到α = P-&。  
    我们此时就可以得到α，β，γ三个角度了，我们可以通过设定X,Y,Z的三个值来求出三个角度值，再利用角度值去对应舵机应受到占空比为多少的pwm，从而达到我们所需的控制目的，这就是运动学逆解。

以下为MATLAB仿真函数，我们建立一个function块，将其中的函数替换为以下的程序：

function [alfa,beta,gamma] = xyztoang(x,y,z)
    h = 45.0;
    hu = 125.5899;
    hl = 136.0;
    
    dyz = sqrt(y^2+z^2);
    lyz = sqrt(dyz^2-h^2);
    gamma_yz = -atan(y/z);
    gamma_h_offset = -atan(h/lyz);
    gamma = gamma_yz-gamma_h_offset;//gamma理解为上图的T
    
    lzp = sqrt(lyz^2+x^2);
    n = (lxzp^2-hl^2-hu^2)/(2*hu);
    beta = -acos(n/hl); //β
    
    alfa_xzp = -atan(x/lyz);
    alfa_off = acos((hu+n)/lxzp);
    alfa = (alfa_xzp+alfa_off); //α
    end

其中alfa，beta，gamma是我们要输出的三个角度，最后那个α不要觉得那个+写错了，看好alfa\_xzp求得的前面带有负号。

# 步态规划 #

步态是步行机器人的一种迈步的方式，是步行机器人各腿之间运动的规律，即各条腿的抬腿和放腿的顺序，是研究步行机构的一个重要的参数，是确保步行机构稳定运行的重要因素。我们观察马最为直观，当然也可以观察狗的，通过四条腿按一定的顺序抬起、移动和落地支撑来实现移动。整个身体的重心沿着运动方向不停移动，并始终落在不断更新的三角形中来保证静态步行。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 4]

图中所示为一个运动循环，设稳定裕量就是中文数字所占的范围，小箭头表示腿的摆动方向，足端带有小圆圈的腿为正在于地面接触的腿，足端不带圆圈的腿为抬起了的腿，虚线围起了的三角形为稳定三角形。模型一为机器人原始状态，第一步为，腿1抬起向运动方向伸出，这是机器人重心不变，仍在原先三条支撑腿构成的三角形，如模型二所示。接着，图三中腿4抬起并向运动方向移动，模型还是保持着静态步行。图四为重心调整环节，腿2、3,支撑着地并向后移动，使重心向运动方向移动一个步长（λ），图五、图六分别将腿2、3抬起并还原到机器人初始时的状态。  
![在这里插入图片描述][20200817184420697.png_pic_center]  
    填充了颜色的为腿部抬起运动时间段，而非填充了的为腿部支撑地面的时间段。  
    对于小跑，快跑的步态规划又是不同的，这里就不给出了。

# MATLAB仿真 #

调用功能块还需要数模转换的一个元件。  
![在这里插入图片描述][2020081719153435.png_pic_center]  
最终将各个模块封装成以下的模样，这样看着好看一些：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 5]

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 6]  
那么我们对各个舵机的控制就可以了，我们开始加入步态控制，在走路的时候应该是对角的腿步伐是一致的，而且对于步态是一个圆弧形，或者说是一个半椭圆状的，这个形状更像是去掉了负半轴的正弦波，我们就可以用matlab里面的正弦波发生器。我们给X一个正弦波，并且让两个对角的腿拥有一定的时间差去运动，这样就可以走路了，Y轴方向因为我们的髋关节有45mm长，我们就给Y一个固定的值45就可以了，z轴我们可以暂时不管。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 7]  
其中最上面的Function的函数是，最上面的和下面的一个函数块内的函数是一样的，因为考虑到步态运动时腿是交替运行的，所以我们加了延时函数，等会我们说怎么去调节延时函数让步态更好，上面两个Function块的作用是移动X的距离。

function y = fcn(u)
    
    y = 30*sin(u);

最下面的两个function块的函数是，这两个函数块的作用是控制抬腿高度，使用Dead zoom功能块进行一个限幅控制。

function y = fcn(u)
    
    y = 17*sin(u);

先将参数输入进去看看效果。  
可以看到机器狗运动起来了，那么就是那个延时函数给多少合适呢？我们可以用一个波形显示功能块去观察，名字叫scope。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 8]

我们要比较两对腿的X移动状况和Z轴移动状况，所以我们将scope功能块设置一下配置为4路输入，四路显示，设置如下：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 9]  
然后将四根线分别接到x1,x2,z1,z2上就可以看到运动的波形了。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 10]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 11]

我们分析一下波形，这个是已经调好的，在从上往下看，第一个波形是x1和x4的波形，在0的下方部分为脚往前的移动，而此时x2和x3应该处于后蹬的状态，所以在x1波形刚开始小于0的时候，x2的波形应该是刚开始大于0的，所以我们就可以改变控制X的延时让波形达到这个效果就可以了，并且让x的移动距离不要太大。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 12]  
后面我加了一个延时是因为模型在掉落在平台上的时候会有些晃动，所以加了延时之后等稍微稳定一些了再走。所以这些参数我给的是：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 13]

那么对于腿抬的高度，我们将幅值限制在180，当然190也可以，由原理，当x1和x4要抬起来的时候x2和x3应该是刚落下，也就是当x2和x3刚达到180的时候，x1和x4处于抬起状态，也就是小于180的状态，我们可以将六自由度功能块去掉之后仔细看步态的运动和波形，来改变延时时间。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 14]  
当在没有六自由度模块的时候将步态调整好，调整好后再加入六自由度模块就好了。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 15]  
需要仿真文件可通过网盘下载;链接：https://pan.baidu.com/s/1THIom-JuPCWNJkrCym1rRQ  
提取码：8r7i

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center]: /images/20221124/5c43697793d84acfb1a3e25860346740.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20221124/c6aca72ab2a7486eb4e9d5b3c1f89aad.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 2]: /images/20221124/449feeded2494d4e9bbc12c611321216.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 3]: /images/20221124/55330e1194f343f5940dbace9afb5fe4.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 4]: /images/20221124/3d071fb96e8140449adee2c34bf18ff7.png
[20200817184420697.png_pic_center]: /images/20221124/db1bacf2b2884b3a8a975289a6040b27.png
[2020081719153435.png_pic_center]: /images/20221124/1aaf74584d454d2e85c30d4ec9178148.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 5]: /images/20221124/780f4be894fd4738aad77f0c055ac95d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 6]: /images/20221124/87ab18deae004717badf1ef0e5a9af65.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 7]: /images/20221124/469d8cc654314f53850488e3765cc0ad.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 8]: /images/20221124/80b542829b4c4f95be85372e3cdf8255.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 9]: /images/20221124/b70a859c84334ff5aad2c90ac8fbc873.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 10]: /images/20221124/0e5071f53524437f9a823e1a0aa59413.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 11]: /images/20221124/8535e0f3fc9846568ada89a361054fb1.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 12]: /images/20221124/e9fdd78eccc44a67b925508140cac430.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 13]: /images/20221124/e7386c28777b4876a7c1072d7b9573d8.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 14]: /images/20221124/724b730585734ee889ef669b3275ee1d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 15]: /images/20221124/d71992b52acd4067aadb3ac511d3cb47.png