百元级树莓派pico四足机器狗设计(并联腿结构)持续更新

一.硬件选取
    1.1 主板
        主板用的是树莓官方的微控制器raspberry pi pico。这个板子的接口还是蛮丰富的，这个小项目完全够用。但是这个处理器有个缺点就是缺少WIFI和蓝牙，不过可以外接蓝牙模块解决。这个板子目前的货量充足，20几元便可以买到一个不带排针的板子。下面就是这个板子的正反面以及接口类型的示意图。板子上有一颗LED灯珠以及温度传感器，使用micro use接口供电。注意在编写程序的过程中要用数据线而非充电线，运行既定程序的时候使用充电线连接就好了。
        买这个板子最好带着排针，再买点杜邦线，直接插拔相对来说方便一些，不过同样的可能因布线太乱影响美观。不带排针直接焊接也可，但是相对麻烦一点。最后，PCB打板就不至于了，毕竟项目太小。

    1.2 舵机
        舵机是动力源，每条腿包含两个驱动舵机。舵机使用SG90与MG90均可。这两款都是数字舵机，MG90的扭矩更大，寿命更长，价格也同样是SG90的二倍。不过两款舵机均可胜任，SG90在5元左右，MG90在10元左右。这里需要八颗舵机，可以根据需求自行选择（左SG，右MG）。
        每个舵机有三根线，红棕色的两根线为电源线，红正(+)棕负(-)，黄色的是信号线。在将舵机连接到驱动板的时候，直接对应着颜色拔插即可。
        还有要注意的一点是，舵机为180°舵机，这点很重要。

    1.3 陀螺仪
        选取了最常见的MPU6050，用于读取系统的姿态数据，用于系统平衡稳定的保持等任务。市价6元左右。

    1.4 舵机驱动板
        这里不能用树莓派PICO直接驱动舵机，一来是电流电压的问题容易造成树莓派PICO的损坏，二来是这玩意儿也没那么多接口，不能同时控制八颗舵机。所以这里用了个16路的舵机控制板PCA9685，也就是可以用这个板子一次性控制16个舵机，所以对于这个项目就绰绰有余了。需要格外注意的是，不要用树莓派PICO给PCA9685供电，PCA9685需要外接电源来驱动。
        如下图，最左边的一排排针用于连接主板，用里面的4根线，GND，VCC，SCL，SDA即可。将这些接口连接到树莓派PICO上对应的位置即可，下面也会给出接线的示意图。最下面黄黑红（不是黄飞鸿）相间的一堆是舵机的连接接口，共有16个，对着舵机的接口的颜色连接即可。绿色的突起用于外接电源供电，在底部印有正负的字样，可不要连接错了。这个外接电源使用3.7—6.0V的锂电池均可。

        PCA9685驱动代码如下所示。使用类Servos下面的position()函数来驱动舵机运动，position()函数的输入包含两个参数，一个是舵机的编号(0-15)，另一个则是舵机转动的角度(0°-180°)。这段代码来自于哔哩哔哩UP。

import pca9685
import math

class Servos:
    def __init__(self, i2c, address=0x40, freq=50, min_us=600, max_us=2400,
                 degrees=180):
        self.period = 1000000 / freq
        self.min_duty = self._us2duty(min_us)
        self.max_duty = self._us2duty(max_us)
        self.degrees = degrees
        self.freq = freq
        self.pca9685 = pca9685.PCA9685(i2c, address)
        self.pca9685.freq(freq)

    def _us2duty(self, value):
        return int(4095 * value / self.period)

    def position(self, index, degrees=None, radians=None, us=None, duty=None):
        span = self.max_duty - self.min_duty
        if degrees is not None:
            duty = self.min_duty + span * degrees / self.degrees
        elif radians is not None:
            duty = self.min_duty + span * radians / math.radians(self.degrees)
        elif us is not None:
            duty = self._us2duty(us)
        elif duty is not None:
            pass
        else:
            return self.pca9685.duty(index)
        duty = min(self.max_duty, max(self.min_duty, int(duty)))
        self.pca9685.duty(index, duty)

    def release(self, index):
        self.pca9685.duty(index, 0)

    1.5 舵机驱动板外接电源
        板子上注明6V以内的电源均可，又考虑到重量什么的，最好还是用锂电池，3.7V的锂电池就可以驱动。这里也可以用5V的充电宝供电，只不过说相当于外接电源，不能直接扛着充电宝罢了。
    1.6 总结
        并联腿结构的四足较为稳定，这个项目开发所需要的配件基本如上所示。价格大约为20(主板) + 40(舵机) + 20(舵机驱动板) + 10(陀螺仪) + 10(电池) +20(3D打印外壳)= 120。当然还有一些其他的开销，比如说一些用于连接固定组件的胶水，螺丝。或者是连接传感器用到的杜邦线等。若要添加蓝牙模块等，还需另备。

二.结构设计与组装
    2.1 整体装配图
        这里使用Creo软件画了个简单的并联腿结构模型，下面是装配图的示意,打印出来将零件装上即可。
        这玩意比较露骨，只有单纯的架子和四条腿。身子类似于一个矩形结构，长180mm，宽100mm，还是比较小巧的。


    2.2 单腿结构示意与实体
        这玩意重点就是腿部的设计，一条腿由两个舵机来驱动。单腿上的三个关节都是可动的。为了保持稳定在腿部的末端都拓宽了表面积，以此来改善支撑。单腿的装配图如下所示。从最右边的子图中可以很清楚的看到装配的细节示意。关节的连接使用舵机螺丝包中同款的自攻螺丝来安装。

        实体的样子如下所示，黑色的为舵机摇臂，足端有毛边的感觉是因为涂了热熔胶，采取这种笨方法来增大摩擦力。

    2.3 组装示意
        拿到所有的零件之后，组装就变得很简单了，最起码比孩子玩的积木简单多了，现在小孩的积木直接能治低血压。
        将舵机并列装到装配孔里面，然后用螺丝拧上就好了，螺丝就是自攻螺丝，舵机里自带那种，不过每个舵机只带一颗，需要自行购买一些，很便宜，买那种长的螺丝，不要买错了。装好舵机之后，就要安装大腿了。安装的时候需要先将舵机复位，这里是将舵机调节到九十度。这里可以看到，一条腿包括四部分，即两个小腿，两个大腿。将小腿安装到舵机上时，要尽量保持水平，即将小腿按照平行于矩形的长边安装。要是实在没理解什么意思，可以参考2.1 整体装配图的第一个图的腿部的安装方式。
        这个安装的过程，由于舵机摇臂齿轮匹配的问题，很难将小腿按照很标准的水平安置，多少会有10°以内的偏差。不过也不用担心，后续用程序矫正即可。另外，很重要的一点，同一侧两条腿足端的安装朝向相反，安装的时候多注意一下。

    2.4 硬件连接示意图
        包含两个传感器的连接示意图，接口选择其实很灵活，对应连接即可。
        连接完了之后大概就是这么个样子，如下图。因为我没有好好布线，所以看起来乱糟糟的，规整一下就可以了。

三.MicroPython环境搭建
        这个项目使用MicroPython开发。这个语言非常容易上手，基本就是一了解就会的那种。买了那个板子之后，会给你一个教程，安装一个python的开发环境Thonny，然后使用micro usb数据线连接板子和电脑，就可以进行软件的开发工作了。卖家会给你提供全套的操作步骤，以及软件供你下载使用，在这里就不再过多赘述了。弄好之后就是这个样子，就可以进行编程工作了。

四.运动学正逆解
    运动学正解与运动学逆解是一组相反的概念。
    4.1 运动学正解
        运动学正解意味着已知关节的角度，然后去估算末端执行器的位姿。拿这个并联腿机器狗作为例子来说，运动学正解意味着我人为给关节驱动舵机一个角度，然后舵机响应这个命令，转动到指定的角度，然后腿部也会随着舵机的运动而运动，使得足端能移动到一定的位置。但是运动学正解在这里不作为重点。
    4.2 运动学逆解
        还是以这个项目为例子，运动学逆解意味着我已知足端的一个位置，然后根据这个位置的坐标(x,y)去计算出两个关节驱动舵机应该转动的角度。通过这种方式，可以任意的规划一条合法的轨迹然后驱动腿部沿着这个轨迹来运动。相应的，运动学逆解也是实现狗子行走转向的重点。

五.并联腿运动学逆解实现
    5.1 模型等价
        从2.2的单腿结构示意中可以看到，两个舵机并列放置。因此可以看到两个舵机的转轴之间有一个间距，在下图中体现为“L“，这个间距在模型等价的过程中不能被忽略。

        根据图中粉色线条表示，可以将该腿部等价为一个动态的五边形模型。当然这个五边形不是严格意义上的，因为有可能出现内凹的感觉，但是依然是五条边。接受了这个无奈的现实，就可以着手运动学逆解的规划了。
        将整个模型倒立过来，舵机在与AB平行的位置为90°，可在以B点为旋转轴向上向下各自移动90度的范围。从图可以看到，D点为单腿的足端，也是我们要规划运动的最终点。
        从D点向A、F、B点做虚轴，这些个轴是不存在的。使用L4、L5、L6将五边形分解为4个不同的三角形。

    5.2 运动学逆解求解
        已知： 足底D坐标(x,y)，L1、L2、L3。
        求解： Angle1、Angle2、Angle3、Angle4。
        方法： 等价出这个模型之后，求解就变得很容易了。先根据D的坐标(x,y)以及L3的长度由勾股定理可以轻松的得到L4、L5、L6的长度。

        注意点F是(0,0)点，向右为X轴正方向。 得到L4、L5、L6的长度之后，结合已知的条件，使用余弦定理，可以得到Angle1、Angle2、Angle3、Angle4的角度。Angle1的求解如下所示，其余三个角度方法均相同。

        因为结构与舵机转角的限制，舵机从水平的90°开始旋转，但不能转到0°，同样向下也不能转到180°。结合模型分析，可以得到两个舵机分别的转角数据A1、A2，如下所示。

    5.3 逆解代码实现

import math
import time

class Leg():
    
    def __init__(self,l1,l2):
        
        self.name = "leg"
        self.version = 1.0
        self.x1 = l1
        self.x2 = l2
        self.len1 = 4.5
        self.len2 = 8.0
        
    def Angle(self,x,y):
        
        l5 = (x**2+y**2)**0.5
        l4 = ((x-self.x1)**2 + y**2)**0.5
        l6 = ((x-self.x2)**2 + y**2)**0.5
        
        cosa1 = (l4**2 + self.len1**2 - self.len2**2)/(2*l4*self.len1+0.01)
        cosa2 = (self.x1**2 + l4**2 - l5**2)/(2*l4*self.x1+0.01)
        cosa4 = (l6**2 + self.x1**2 - l5**2)/(2*self.x1*l6+0.01)
        cosa3 = (self.len1**2 + l6**2 - self.len2**2)/(2*self.len1*l6+0.01)
        
        degree1 = math.degrees(math.acos(cosa1))
        degree2 = math.degrees(math.acos(cosa2))
        degree3 = math.degrees(math.acos(cosa3))
        degree4 = math.degrees(math.acos(cosa4))
        
        angle1 = 270 - degree1 - degree2
        angle2 = 270 - degree3 - degree4
        
        return int(angle1),int(angle2)
        

l = Leg(0.7,-0.7)
one,two = l.Angle(0,9.0)

六.行走转向步态规划
    没病走两步。
    6.1 行走步态设计
        下图为一种四足直行的步态设计。首先图中的每一个蓝色方块代表一条腿的安放位置。以最左边的图形为例，右上角为1号腿，右下角为2号腿。同样的，左上角为3号腿，左下角为4号腿。1、2号两条腿为前腿，其余的两条为后腿。

        足端的轨迹为摆线加直线的设计。即足端运行的过程分为两部分，包含两个相位，一部分是轨迹为摆线的运动相，另一部分是轨迹为直线的支撑相。
        在单腿的每一个运动周期中，都包含了两个状态。一是处于运动相的运动状态，另一则为支撑相的支撑状态。处于对角线上的两条腿的运动状态相同。在这里假设1、4号腿先运动。则当1、4号腿处于运动相的时候，2、3腿则处于支撑相。随着时间推移，1、4号腿结束运动相进而进入支撑相的运动状态中，此时2、3腿也结束支撑相进入运动相的状态。重复这个过程，如图所示，即可以实现行走步态。
        行走步态的实现流程如下。
            1. 生成一条摆线轨迹，并生成一条直线，与所形成的摆线相连，组成一条闭合的弧线。取一个特定的周期，自首至尾在圆弧上取一定数量的点，并拿到所有点的（x,y）坐标。将这个坐标与对应的偏置量合并，作为行走轨迹上的坐标点。
            2．将所得到的坐标放入运动学逆解的函数之中，会返回两个逆解的角度值，这两个角度为对应的伺服器应当运动的值。
            3. 根据上图所给出的步态驱动对应的腿，使得同组腿与异组腿交替运动。
            4. 重复1-3操作，实现行走步态。
    6.2 转向步态设计
        转向步态与前进后退步态基本上无异。在这里提及一个词，即差速。与车的转向原理不同，这个智能体的运动方式更类似于履带式的机械，如坦克，挖掘机等。他们采取差速的方式，可进行各种角度甚至是原地的转向。如下图所述，以向右转向为例。

        1、4腿先迈出，但是1号腿迈出的步幅更大，4号腿移动的距离很小。但重点是仍要保持腿部的运动的相位相同，方能保持稳定。之后2、3腿迈出，但是3号腿迈出的步幅大于2号腿。同样的，重复这个过程，利用差速的方式，可以实现右转。同理，左转亦是如此。
    6.3 行走转向代码设计
        第一段代码用于生成摆线的坐标点。
        代码出现问题，近期更新