[md]# 项目简介
这个项目是基于T站上的一个模仿波士顿动力的小黄狗的模型。原作者把他叫做spotmini （https://www.thingiverse.com/thing:3445283） 用的是996R的舵机。
刚看到这个模型就被这个好看的外观吸引了，就把它打印出来了。虽然结构很好看，但是缺陷比较多，加上树莓派电池之后重量差不多在2.5kg左右，996舵机只能勉强能撑起这只狗，如果要做一些需要高响应速度的运动估计是不行的。如果不差钱的话，还是尽量选择串行舵机或数字舵机。如果要自己设计结构的话，推荐把结构做成前肘后膝式或者单腿并联臂的形式，并且要尽可能减轻重量和腿的转动惯量。希望之后原作者能给出模型源文件 ，以便适配其它更好的舵机。
由于接下来自己的事情比较多，所以这个机器狗也只能算是半成品，我只能分享一下我做的制作过程中的一些思路，并不能提供一个完整的机器狗控制程序。这也是我第一次做足式机器人，文献看得不多，有什么不对的地方欢迎指正，一起交流。
gitbub地址 https://github.com/YANGGTT/spotmicro-controller
# 结构和硬件
按照原作者的模型需要：
- 12个MG996R舵机（应该是最廉价的舵机了）
- 8个F625法兰轴承（内径有点小，设计成mr128轴承会更好）
- 8个M5*15螺栓
- 40个M4*20螺栓
- 8个M4*15螺栓
- 48个M4螺母
- 4个M3*20螺栓
- 28个M3*10螺栓
- 16个M3螺母
然后我把控制部分全换了：
- 树莓派
- stm32f103c8t6
- 自带卡尔曼滤波的9轴陀螺仪（省事）
- pca9685pwm扩展板
- 5v10A-20A稳压
- usb摄像头
- 3s或2s电池
下面是图片 )
 )
 )
# 程序
总体的控制思路比较简单，就是用stm32做基本的运动控制，树莓派负责获取环境信息和调度运动控制。不过完成运动控制部分可能就需要花上一段时间，所以树莓派上的部分暂时就不在这里写了，以后有空再慢慢完成。
运动控制部分基于arduino_stm32,为了方便开发使用里面的FreeRTOS。环境安装的话可以搜索arduino+stm32关键词，就有很多教程的。这里给几个参考链接：
> https://www.cnblogs.com/chdfelix/p/9637648.html
https://my.oschina.net/microxdd/blog/919787
http://docs.leaflabs.com/static.leaflabs.com/pub/leaflabs/maple-docs/latest/maple-quickstart.html
## 主要思路
我把四足的控制分成了三个部分：运动控制，平衡控制和一些保护机制。运动控制就是实现小跑、慢走等移动，平衡控制就是维持机身姿态的相对稳定，保护机制是当机器人已经无力回天的时候做的一些保护动作。这里先实现前两个，保护机制以后再慢慢做。用RTOS建两个任务，一个负责接收指令执行运动，一个负责平衡控制。后面可能会考虑用三个任务，把接收指令和执行也运动分开。
#### 指令
指令采用“起始符+数字 字符+数字 字符+数字+...+结束符”的形式，例如“M1 X10 Y10 \n”。"M1"指定了运动控制的类型，前进或者是转弯；中间部分的"X10 Y10"指定了一些必要的参数，向前多少距离或者是转弯多少度；最后“\n”结束符表示这条指令结束。这个类似于数控系统的G代码。也可以用16进制的协议，不过字符的会更直观一点。
#### 平衡
平衡包扩动态的平衡和静态的平衡（姿态控制）。前者是机器人收到一些冲击（比如侧踹）时，还能够稳定机身不倒；后者是机器人在不同的地面上始终能保持给定的姿态。这里也是先做简单一点的后面那个，主要思路是：先构建各个腿的运动学逆解，再结合陀螺仪输出的roll和Pitch角，用带死区的PD控制（对精度要求不高，加个小角度死区）。
#### 步态
步态分为动态步态和静态步态。动态步态是两节拍的，每一次都抬起对角线的两只腿，这一过程中，机器狗处于不稳定状态，因此需要使用一些方法机器狗维持相对稳定，如zmp控制。动态步态是足式机器人控制的难点而且也比较考验电机性能，所以我没有实现，一方面是能力不够，一方面是舵机不给力。动态步态的优点是动作敏捷，可以实现小跑，但因为不能长时间保持平衡，动态步态没有越障能力。静态步态是四节拍的，每次抬起一条腿，同时让重心落在三只支撑腿构成的三角形内，靠三条支撑腿保持平衡。优点是可以跨越障碍，缺点是动作迟钝。
## 单腿运动学
腿部运动学是其他所有运动控制的基础，确定好腿部的运动学可以方便实现后续的运动。因为运动学解算在一些运动控制中调用地比较频繁，而有时机器狗根本达不到计算精度，所以要权衡好计算精度和资源占用，该近似的地方就近似，该简化的地方就简化。还有就是很多控制都靠pid去完成，腿部运动学的计算精度就可以适当放宽了，毕竟传感器还是要来的更准一些，控制上只要有一个正确的“趋势”就行了。
单只腿只有三个自由度，所以这里用几何法来解运动学正逆解，就不搞什么机器人学了。以左边的腿为例。如图所示，简化关节后构建右手坐标系（右边的腿的话也可以用左边的正逆解，使用的时候把y取反就行了）。
#### 定义关节角 )
如图分别是左腿的前视图、左腿的大腿和小腿所平面视图。定义髋关节的关节角为`$\theta_0$`,大腿关节为`$\theta_1$`,小腿关节为`$\theta_2$`。大腿长l1，小腿长l2，腿部平面和髋关节的偏置为d。
#### 运动学正解
运动学正解在控制中暂时还没用到过，但还是先实现一下
先看大腿和小腿所在的平面( ),定义另外一个坐标轴垂直于x轴为Rz：
```math
x=l_1cos\theta_1+l_2cos(\theta_1-\theta_2)
R_z=l_1sin\theta_1+l_2sin(\theta_1-\theta_2)
```
转到前视图：
```math
y=scos\varphi
z=ssin\varphi
s=\sqrt{d^2+R^2_z}
\varphi=\theta_0-atan2(R_z,d)
```
#### 运动学逆解
先看前视图：
```math
R_z=\sqrt{y^2+z^2-d^2}
\theta_0=atan2(z,y)+atan2(R_z,d)
```
转到大腿和小腿所在的平面：
由余弦定理得：
```math
x^2+R^2_z=l^2_1+l^2_2-2l_1l_2cos(\pi-\theta)
cos\theta_2=(x^2+y^2+z^2-l^2_1-l^2_2-d^2)/(2l_1l_2)
sin\theta_2=\sqrt{1-cos^2\theta_2}
\theta_2=atan2(sin\theta_2,cos\theta_2)
```
作过小腿末端点的垂线交于大腿（或其延长线）：
```math
\theta_1=antan2(l_2sin\theta_2,l_1+l2cos\theta_2)+atan2(R_z,x)
```
#### 代码
在使用运动学的时候，使用增量形式的控制。即每一次控制都让小腿末端运动一个位移增量（矢量）。
```
void KINE::mov(float vx,float vy,float vz)
{
pose[X]+=vx;
pose[Y]+=vy;
pose[Z]+=vz;
IK();
move_servo();
}
```
## 平衡控制
机器狗平台实际上就是一个并联平台，可以完成6个自由度的运动，而且还存在冗余。不过在实际使用过程中可以不用对这个并联平台做精确的解算，只实现必要的几个特殊运动，如三个坐标的平动和身体姿态的改变就可以了。
#### 三个坐标的平动
在建立好单腿运动学模型的条件下，这个很好实现。四条腿的落脚点相对平台同时后移，平台就向前移动；同时抬起，平台就向下移动。
```
//右边的腿y是取反的
void move_platform(float x,float y,float z)//移动平台
{
leg_lf.mov(-x,-y,-z);
leg_rf.mov(-x,y,-z);
leg_lb.mov(-x,-y,-z);
leg_rb.mov(-x,y,-z);
}
```
#### 姿态调整
当左边的腿下放（抬起），右边的退抬起（下放）时，改变平台的roll角；当前面的腿下放（抬起），后面的退抬起（下放）时，改变平台的pitch角；四只脚按同一方向绕平台中心移动时，改变yall角。对于roll和pitch，这么做其实是不对的，因为在完整约束条件下，用这种方法除了会改变roll和pitch角，还会使整个平台向一侧偏移。如图，假设落脚点相对地面的位置不会改变，如果当一边的腿抬高时，平台在这一侧相对下降，平台的体态改变，同时为了保证约完整，平台要向一侧偏移。如果要使平台不偏移则还需要相应地使两边的腿内收或者外伸。不过呢，实际情况嘛，小角度就当它近似了。
```
//把roll和pitch叠加在一起
//yall暂时没写
void rotation_platform(float rx,float ry)
{
leg_lf.mov(0,0,rx-ry);
leg_rf.mov(0,0,-rx-ry);
leg_lb.mov(0,0,rx+ry);
leg_rb.mov(0,0,-rx+ry);
}
```
#### 平衡
有了前面的基础，平衡控制就很简单了，用两个pid（pd控制就够了）分别控制roll和pitch角就可以了。
```
//循环
current=xTaskGetTickCount();
if((current-lastime)>=SAMEPLE_TIME)
{
get_rpy();
ROLL.compute();
PITCH.compute();
rotation_platform(-add_roll,-add_pitch);
lastime=current;
}
```
## 步态
主流的步态调节使用cpg算法，一种用数学模型模拟中枢神经生成节律的方法。然而我不会。所以就先死调了一个步态先试试。
#### 动态步态
这里我没有使用算法让机器人维持动态平衡，只是调了一个两节拍的步态，相当于机器狗的小跑，但是效果不是很理想，感觉舵机的力矩和响应速度不够。
在行走过程中腿部的运动有三个状态，抬腿，迈腿和收腿。遵循抬多少就落多少，迈多少就收多少的原则就可以了。需要注意的是，行走之前有一个起势。具体的可以看看真的狗是怎么小跑的。
```
//给定walk_x,-walk_y,walk_ro就可以实现全向移动了。
```
#### 静态步态
静态步态我还没有尝试，但是思路不复杂。只要调步态的时候保证中心始终落在支撑三角形内就可以了。比如抬起右后脚的时候，让身体左前移。
# 视频
[/md]
|
发表于 2019-4-27 16:49
楼主