实例10： 四足机器人运动学逆解单腿可视化

实验目的

1. 了解逆运动学的有无解、有无多解情况。
2. 了解运动学逆解的求解。
3. 熟悉逆运动学中求解的几何法和代数法。
4. 熟悉单腿舵机的简单校准。
5. 掌握可视化逆向运动学计算结果的方法。

实验要求

1. 拼装一条mini pupper的腿部。
2. 运行程序，可视化观察运动学逆解的多解情况和求解方法。
3. 对单腿舵机进行简单校准。
4. 观察运动学逆解的硬件运行情况。

实验知识

1.什么是逆向运动学 Inverse Kinematics

正向运动学探究的是已知关节角${\theta }_i$，求解工具坐标系 { H } \{H\} { H}或${}^{World}P$的问题。
而逆向运动学则是探究已知工具坐标系 { H } \{H\} { H}的位置和姿态或${}^{World}P$，求解满足要求的${\theta }_i$的问题。
运动学方程解的有无定义了工作空间，有解则表示机械臂能到达这个目标点，无解则表示机械臂无法到达这个点，这个目标点位于工作空间之外。
基本的逆运动学可以看做是给定操作臂末端执行器的位置和姿态，计算所有可达给定位置和姿态的关节角的问题，可以认为是机器人位姿从笛卡尔空间到关节空间的“定位”映射。

2.什么是封闭解和数值解

逆运动学不像正运动学那么容易，逆运动学是非线性的，难以找到封闭解，有时候无解，有时候有有多解的问题，这种非线性的超越方程组，没有规矩的、统一、通用的解法，解法分为封闭解法和数值解法。
封闭解是由数学公式的推导得出，对于任意自变量均能求出对应的因变量，计算量可能相对较多，精度高。
数值解则是可以由离散查表或者是插值一类的方法去模拟最终情况，计算量相对较小，精度相对较差。
因此，我们需要根据实际情况来考虑逆向运动学的解和解的情况。

2.解存在吗？

在逆向运动学（IK）中，我们可以通过给定点相对于世界坐标系（Frame World）的坐标来解算出机器人的手臂关节应该旋转的角度$\theta$。
假如给定的一个位置是在很远的地方，机器人的手臂完全够不着，那么求解是没有意义的，因此我们将会用到工作空间来描述机器人可触达的区域。

工作空间

工作空间是手臂末端所能到达的位置范围。指定的目标点必须在工作空间内，逆向运动学求解才有意义。
为了进一步描述工作空间，可以用以下常见的这两种工作空间的表示：
可达工作空间 Reachable workspace
是手臂能用一种或以上的姿态能够到达的位置范围。可达目标坐标系可以描述这个Frame相对于世界坐标系的位置，而一系列的可达目标坐标系的集合构成了可达工作空间。
灵巧工作空间 Dexterous workspace
是手臂末端用任何姿态都能够到达的位置，条件相当苛刻，比如平面2DOFs的RR机械臂模型中L1=L2的摆臂的圆心，在这个模型中，仅此一点是灵巧工作空间。灵巧工作空间是可达工作空间的子集。

3.是否有多个解？

在求解运动学方程时常常会遇到不只一个解的情况。比如平面中具有三个旋转关节的机器人手臂，对于同一个点$P$，这三个旋转关节可以有不同的位形，在不同的位型下，手臂末端的执行器的可达位置和姿态可以是相同的。
图片：三连杆操作臂多解图

解的选取

对于解的选取有一些基本原则：

1. 速度最快
2. 能耗最低
3. 避开障碍物
4. 在关节允许活动的范围限制内

4.如何求解？

求解操作臂运动学方程是非线性的问题，非线性方程组没有通用的求解算法，算法需要针对机器人手臂的模型来制定。如果某一算法可以解出与已知位姿相关的全部关节变量，那么这个机器人手臂就是可解的。
从$Fram{e}_{object}$到$Fram{e}_{World}$的变换矩阵${}_o^{W}T$中的转动部分和平移部分可以提取出含未知数的16个数字。
其中的旋转矩阵被xyz相互垂直、xyz为单位向量这六个条件限制到只有三个自由度，其中的位置矢量分量的三个方程有三个自由度，共有6个限制条件，6个自由度，这些方程为非线性超越方程，求解不易。
对于六旋转关节的机械臂，存在解析解（封闭解）的充分条件是相邻的三关节的转轴交于一点。

$${}_6^{0}T={\left[\begin{array}{cc}{}_6^0{R}_{3x3}& {}^0{P}_{6ORG3x1}\\ 000& 1\end{array}\right]}_{4x4}=\left[\begin{array}{cccc}{\hat{X}}_6\cdot {\hat{X}}_0& {\hat{Y}}_6\cdot {\hat{X}}_0& {\hat{Z}}_6\cdot {\hat{X}}_0& {}_6^0{P}_{Xorg}\\ {\hat{X}}_6\cdot {\hat{Y}}_0& {\hat{Y}}_6\cdot {\hat{Y}}_0& {\hat{Z}}_6\cdot {\hat{Y}}_0& {}_6^0{P}_{Yorg}\\ {\hat{X}}_6\cdot {\hat{Z}}_0& {\hat{Y}}_6\cdot {\hat{Z}}_0& {\hat{Z}}_6\cdot {\hat{Z}}_0& {}_6^0{P}_{Zorg}\\ 0& 0& 0& 1\end{array}\right]$$
对于基于解析形式的解法，常见的求解方法有几何法和代数法。两种方法相似，求解过程不同。

几何法

几何法求解机械臂的逆运动学问题时，常常需要将空间几何参数转化为平面几何的问题。在${\alpha }_i=0{或}_{-}^{+}90°$时几何法会非常容易，应用平面几何常见的公式及角度转换即可求出${\theta }_i$的值。

$$x^2+y^2=l_1^2+l_2^2-2l_1{l}_2(\pi -{\theta }_2)\text{\hspace{1em}(余弦定理)}$$
$$Cos{\theta }_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1{l}_2}\text{\hspace{1em}(变形1)}$$
$$Cos\psi =\frac{（x^2+y^2）+l_1^2-l_2^2}{2l_1\sqrt{x^2+y^2}}\text{\hspace{1em}(变形2)}$$

$${\theta }_1=\{\begin{array}{ll}atan2(y,x)+\psi & {\theta }_2<0\\ atan2(y,x)-\psi & {\theta }_2>0\end{array}$$
在计算完${\theta }_2$和${\theta }_1$后，根据图的几何角度关系，又可算得${\theta }_3$，即成功反解运动学的各${\theta }_n$

math.atan2()方法

math.atan2()方法是双变量反正切公式，可以计算给定y,x值的反正切值，也就是以弧度形式表达的该段终点与起点连线斜率线的一个角度值。
atan2()优于atan()，因为可以计算x2-x1=0的情况。
参考链接：Python math.atan2(y,x)
计算空间中缺少的自由度

代数法

应用连杆参数（${\alpha }_{i-1}$,$a_{i-1}$,${\theta }_i$,$d_i$），通过运动学正解（FK）可以求得机械臂的运动学方程，表现形式为变换矩阵${}_3^{0}T$。因此，目标点的位置是由手臂末端坐标系相对基坐标系来定的，当研究对象为平面机械臂时，只需要知道x,y,$\varphi$即可确定目标点位置。
$\varphi$是 末端杆在平面内的姿态角
将已知的${}_3^{0}T$与新建立的${}_{object}^{world}T$取等，即可获得对应位置的值相等。
$$\left[\begin{array}{cccc}{c}_{123}& -s_{123}& 0.0& l_1{c}_1+l_2{c}_{12}\\ s_{123}& c_{123}& 0.0& l_1{s}_1+l_2{s}_{12}\\ 0.0& 0.0& 1.0& 0.0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}{c}_{\varphi }& -s_{\varphi }& 0.0& x\\ s_{\varphi }& c_{\varphi }& 0.0& y\\ 0.0& 0.0& 1.0& 0.0\\ 0& 0& 0& 1\end{array}\right]$$
利用三角函数和角公式
$$Si{n}_{1{}_{-}^{+}2}=Si{n}_{1}Co{s}_2{}_{-}^{+}Co{s}_{1}Si{n}_2$$
$$Co{s}_{1{}_{-}^{+}2}=Co{s}_{1}Co{s}_2{}_{+}^{-}Si{n}_{1}Si{n}_2$$
可得
$$Cos{\theta }_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1{l}_2}$$
此式在$1\ge Cos{\theta }_2\ge -1$时有解

假设目标点在工作空间内，又有
$$Sin{\theta }_2={}_{-}^{+}\sqrt{1-c^2}$$
应用几何法中提到的math.Atan2()求解${\theta }_2$，利用${\theta }_2$再去对其他${\theta }_n$求解，具体方法参考教材，本处仅作代数法引入。
通俗的来说，就是确认${\theta }_n$的$Sin{\theta }_n$和$Cos{\theta }_n$，再利用双变量反正切公式math.Atan2()求${\theta }_n$

实验步骤

1.逆运动学的多解与求解

运行程序，观察运动学逆解的多解情况，观察程序中运动学逆解的求解方法。

sudo python rr_IK.py
# 示例值： 3 7

#!/usr/bin/python
# coding:utf-8
# rr_IK.py
# 逆向运动学IK
# mini pupper的简化单腿，可视作同一平面的RR类机械臂，可视化该机械臂，由给定末端位置计算转轴角度
import matplotlib.pyplot as plt  # 引入matplotlib
import numpy as np  # 引入numpy
from math import degrees, radians, sin, cos


# 几何法：端点坐标转关节角
def position_2_theta(x, y, l1, l2):
    """
    运动学逆解 将输入的端点坐标转化为对应的关节角
    :param x: p点坐标x值
    :param y: p点坐标y值
    :param l1: 大臂长
    :param l2: 小臂长
    :return: 关节角1值1 关节角1值2 关节角2值1 关节角2值1
    """
    cos2 = (x ** 2 + y ** 2 - l1 ** 2 - l2 ** 2) / (2 * l1 * l2)
    # print(cos2)
    sin2_1 = np.sqrt(1 - cos2 ** 2)
    sin2_2 = -sin2_1
    # print(sin2_1)
    # print("sin2有两值，分别为sin2_1=%f, sin2_2=%f" % (sin2_1, sin2_2))  # 若考虑关节情况也可只取一个正值
    theta2_1 = np.arctan2(sin2_1, cos2)
    theta2_2 = np.arctan2(sin2_2, cos2)
    phi_1 = np.arctan2(l2 * sin2_1, l1 + l2 * cos2)
    phi_2 = np.arctan2(l2 * sin2_2, l1 + l2 * cos2)
    theta1_1 = np.arctan2(y, x) - phi_1
    theta1_2 = np.arctan2(y, x) - phi_2
    # print(degrees(theta1_1), degrees(theta1_2), degrees(theta2_1), degrees(theta2_2))
    return theta1_1, theta1_2, theta2_1, theta2_2


def preprocess_drawing_data(theta1, theta2, l1, l2):
    """
    处理角度数据,转化为matplotlib适应的绘图格式
    :param theta1: 角度数据1
    :param theta2: 角度数据2
    :param l1: 杆件长1
    :param l2: 杆件长2
    :return: 绘图数据x坐标list和对应的y坐标list
    """
    xs = [0]
    ys = [0]
    # 分别算出x1 y1和x2 y2
    x1 = l1 * cos(theta1)
    y1 = l1 * sin(theta1)
    x2 = x1 + l2 * cos(theta1 + theta2)
    y2 = y1 + l2 * sin(theta1 + theta2)
    xs.append(x1)
    xs.append(x2)
    ys.append(y1)
    ys.append(y2)
    return xs, ys


def annotate_angle(x0, y0, rad1, rad2, name, inverse=False):
    """
    为两条直线绘制角度
    :param x0: 圆心x坐标
    :param y0: 圆心x坐标
    :param rad1: 起始角
    :param rad2: 终止角
    :param name: 角名
    :param inverse: 用于解决点1的重叠问题
    :return: 无
    """
    theta = np.linspace(rad1, rad2, 100)  # 0~rad
    r = 0.2  # circle radius
    x1 = r * np.cos(theta) + x0
    y1 = r * np.sin(theta) + y0
    plt.plot(x1, y1, color='red')
    plt.scatter(x0, y0, color='blue')
    degree = degrees((rad2 - rad1))
    if inverse:
        plt.annotate("%s=%.1f°" % (name, degree), [x0, y0], [x0 - r / 1.5, y0 - r / 1.5])
    else:
        plt.annotate("%s=%.1f°" % (name, degree), [x0, y0], [x0 + r / 1.5, y0 + r / 1.5])


# 关节信息
# 大臂长度：5 cm 小臂长度：7.5 cm
link_length = [5, 7.5]  # in cm
# 输入末端位置
position_pre = input("请输入末端的x坐标和y坐标，以空格隔开：")
position = [float(n) for n in position_pre.split()]
print(position)

# 计算并预处理绘图数据
joints_angles = position_2_theta(position[0], position[1], link_length[0], link_length[1])
# print(joints_angles)
figure1 = preprocess_drawing_data(joints_angles[0], joints_angles[2], link_length[0], link_length[1])
figure2 = preprocess_drawing_data(joints_angles[1], joints_angles[3], link_length[0], link_length[1])
# print(figure1)
# print(figure2)

# 绘图
fig, ax = plt.subplots()  # 建立图像
plt.axis("equal")
ax.grid()
plt.plot(figure1[0], figure1[1], color='black', label='method 1')
plt.scatter(figure1[0], figure1[1], color='black')
plt.plot(figure2[0], figure2[1], color='red', label='method 2')
plt.scatter(figure2[0], figure2[1], color='blue')
ax.set(xlabel='X', ylabel='Y', title='mini pupper IK RR model')
plt.legend()
# 标注
annotate_angle(figure1[0][0], figure1[1][0], 0, joints_angles[0], "theta1_1")
annotate_angle(figure1[0][1], figure1[1][1], joints_angles[0], joints_angles[2]+joints_angles[0], "theta2_1")
annotate_angle(figure2[0][0], figure2[1][0], 0, joints_angles[1], "theta1_2", inverse=True)
annotate_angle(figure2[0][1], figure2[1][1], joints_angles[1], joints_angles[3]+joints_angles[1], "theta2_2")
plt.annotate("P(%d, %d)" % (position[0], position[1]), [figure1[0][2], figure1[1][2]],
             [figure1[0][2] + 0.1, figure1[1][2] + 0.1])
plt.tight_layout()
plt.show()

2.逆运动学可视化

观察程序，通过圆轨迹的运动学逆解来观察mini pupper腿部的运动

#!/usr/bin/python
# coding:utf-8
# rr_IK_circle.py
# 逆向运动学IK
# mini pupper的简化单腿，可视作同一平面的RR类机械臂，可视化四足机器人逆运动学画圈
import matplotlib.pyplot as plt  # 引入matplotlib
import numpy as np  # 引入numpy
from math import degrees, radians, sin, cos
import matplotlib.animation as animation


# 几何法：端点坐标转关节角
def position_2_theta(x, y, l1, l2):
    """
    运动学逆解 将输入的端点坐标转化为对应的关节角
    :param x: p点坐标x值
    :param y: p点坐标y值
    :param l1: 大臂长
    :param l2: 小臂长
    :return: 关节角1值1 关节角1值2 关节角2值1 关节角2值1
    """
    cos2 = (x ** 2 + y ** 2 - l1 ** 2 - l2 ** 2) / (2 * l1 * l2)
    sin2_1 = np.sqrt(1 - cos2 ** 2)
    sin2_2 = -sin2_1
    theta2_1 = np.arctan2(sin2_1, cos2)
    theta2_2 = np.arctan2(sin2_2, cos2)
    phi_1 = np.arctan2(l2 * sin2_1, l1 + l2 * cos2)
    phi_2 = np.arctan2(l2 * sin2_2, l1 + l2 * cos2)
    theta1_1 = np.arctan2(y, x) - phi_1
    theta1_2 = np.arctan2(y, x) - phi_2
    return theta1_1, theta1_2, theta2_1, theta2_2


def preprocess_drawing_data(theta1, theta2, l1, l2):
    """
    处理角度数据,转化为matplotlib适应的绘图格式
    :param theta1: 角度数据1
    :param theta2: 角度数据2
    :param l1: 杆件长1
    :param l2: 杆件长2
    :return: 绘图数据x坐标list和对应的y坐标list
    """
    xs = [0]
    ys = [0]
    # 分别算出x1 y1和x2 y2
    x1 = l1 * cos(theta1)
    y1 = l1 * sin(theta1)
    x2 = x1 + l2 * cos(theta1 + theta2)
    y2 = y1 + l2 * sin(theta1 + theta2)
    xs.append(x1)
    xs.append(x2)
    ys.append(y1)
    ys.append(y2)
    return xs, ys


def animate_plot(n):
    # 生成圆轨迹
    circle_point = [2.696152422706633, -7.330127018922193]  # 圆周运动的圆心
    position = [0, 0]
    history_position_x = [0]
    history_position_y = [0]
    circle_r = 2
    theta = n * np.pi / 100
    position[0] = circle_point[0] + circle_r * np.cos(theta)
    position[1] = circle_point[1] + circle_r * np.sin(theta)

    # 计算并预处理绘图数据
    joints_angles = position_2_theta(position[0], position[1], link_length[0], link_length[1])
    figure1 = preprocess_drawing_data(joints_angles[0], joints_angles[2], link_length[0], link_length[1])

    # 轨迹追踪
    for i in range(0, (n % 200)+1):
        history_theta = ((n % 200) + 1 - i) * np.pi / 100
        history_position_x.append(circle_point[0] + circle_r * np.cos(history_theta))
        history_position_y.append(circle_point[1] + circle_r * np.sin(history_theta))

    # 画图
    p = plt.plot(figure1[0], figure1[1], 'o-', lw=2, color='black')
    p += plt.plot(history_position_x, history_position_y, '--', color='blue', lw=1)
    return p


# 关节信息
# 大臂长度：5 cm 小臂长度：7.5 cm
link_length = [5, 7.5]  # in cm

# matplotlib可视化部分
fig, ax = plt.subplots()  # 建立图像
plt.axis("equal")
plt.grid()
ax.set(xlabel='X', ylabel='Y', title='mini pupper IK RR model Circle Plot')
ani = animation.FuncAnimation(fig, animate_plot, interval=10, blit=True)
plt.show()

3.校准舵机

将组装好的单腿各舵机线材，按照程序中提示的接线接入，对舵机2与舵机3进行回零校准。

#!/usr/bin/python
# coding:utf-8
# servo_calibrate.py
# 默认舵机全部回零，随后等待输入角度

import RPi.GPIO as GPIO
import time


def servo_map(before_value, before_range_min, before_range_max, after_range_min, after_range_max):
    """
    功能:将某个范围的值映射为另一个范围的值
    参数：原范围某值，原范围最小值，原范围最大值，变换后范围最小值，变换后范围最大值
    返回：变换后范围对应某值
    """
    percent = (before_value - before_range_min) / (before_range_max - before_range_min)
    after_value = after_range_min + percent * (after_range_max - after_range_min)
    return after_value


signal_ports = input("键入各舵机的信号端口号，以空格隔开,无输入按回车默认信号口为：32 33 35\n请输入：") or "32 33 35"
signal_ports = [int(n) for n in signal_ports.split()]
for i in range(0, len(signal_ports)):
    print("舵机%d对应的口为%d" % (i+1, signal_ports[i]))

GPIO.setmode(GPIO.BOARD)  # 初始化GPIO引脚编码方式
servo = [0, 0, 0]
servo_SIG = signal_ports  # PWM信号端
servo_VCC = [2, 4, 1]  # VCC端
servo_GND = [30, 34, 39]  # GND端
servo_freq = 50  # PWM频率
servo_width_min = 2.5  # 工作脉宽最小值
servo_width_max = 12.5  # 工作脉宽最大值
GPIO.setmode(GPIO.BOARD)  # 初始化GPIO引脚编码方式
for i in range(0, len(servo_SIG)):
    GPIO.setup(servo_SIG[i], GPIO.OUT)
    servo[i] = GPIO.PWM(servo_SIG[i], servo_freq)
    servo[i].start(0)
    servo[i].ChangeDutyCycle((servo_width_min + servo_width_max) / 2)  # 回归舵机中位
print("初始化回零完成，两秒后等待输入")
time.sleep(2)

# 为舵机指定位置
try:  # try和except为固定搭配，用于捕捉执行过程中，用户是否按下ctrl+C终止程序
    while 1:
        angles = input("如果你需要改变舵机角度，请依次为不同舵机输入0°-180°的角度值：\n")
        angles = [int(n) for n in angles.split()]

        for i in range(0, len(angles)):
            dc_trans = servo_map(angles[i], 0, 180, servo_width_min, servo_width_max)
            servo[i].ChangeDutyCycle(dc_trans)
            print("舵机%d已转动到%d°" % (i+1, angles[i]))
except KeyboardInterrupt:
    pass
for i in range(0, len(servo_SIG)):
    servo[i].stop()  # 停止pwm
GPIO.cleanup()  # 清理GPIO引脚

4.观察运动学逆解的实际运行

运动学逆解的实际运行会受到非常多因素的干扰，例如校准情况、杆间的测量误差、信号线材的传输波动、树莓派本身的计算能力。
值得一提的是，千机千面，舵机的校准每个人遇到的情况不同，对于单腿的舵机，在3中提到的校准程序只能帮助你发现简单的安装错误，如果需要实际校准，需要运行整机的可视化校准代码。

如果你希望可视化硬件的运行情况，虽然mini pupper并没有设置足端反馈，但你也可以将代码中的绘图部分注释恢复，这会使得舵机运动和电脑端绘图同步进行，这对算力要求较高，可能会出现卡顿。

#!/usr/bin/python
# coding:utf-8
# rr_IK_circle_synchronous.py
# 运动学逆解画圆，同步控制端图像显示和硬件运动
import matplotlib.pyplot as plt  # 引入matplotlib
import numpy as np  # 引入numpy
from math import degrees, radians, sin, cos
import matplotlib.animation as animation
import time
import RPi.GPIO as GPIO


# 几何法：端点坐标转关节角
def position_2_theta(x, y, l1, l2):
    """
    运动学逆解 将输入的端点坐标转化为对应的关节角
    :param x: p点坐标x值
    :param y: p点坐标y值
    :param l1: 大臂长
    :param l2: 小臂长
    :return: 关节角1值1 关节角1值2 关节角2值1 关节角2值1
    """
    cos2 = (x ** 2 + y ** 2 - l1 ** 2 - l2 ** 2) / (2 * l1 * l2)
    sin2_1 = np.sqrt(1 - cos2 ** 2)
    sin2_2 = -sin2_1
    theta2_1 = np.arctan2(sin2_1, cos2)
    theta2_2 = np.arctan2(sin2_2, cos2)
    phi_1 = np.arctan2(l2 * sin2_1, l1 + l2 * cos2)
    phi_2 = np.arctan2(l2 * sin2_2, l1 + l2 * cos2)
    theta1_1 = np.arctan2(y, x) - phi_1
    theta1_2 = np.arctan2(y, x) - phi_2
    return theta1_1, theta1_2, theta2_1, theta2_2


def servo_map(before_value, before_range_min, before_range_max, after_range_min, after_range_max):
    """
    功能:将某个范围的值映射为另一个范围的值
    参数：原范围某值，原范围最小值，原范围最大值，变换后范围最小值，变换后范围最大值
    返回：变换后范围对应某值
    """
    percent = (before_value - before_range_min) / (before_range_max - before_range_min)
    after_value = after_range_min + percent * (after_range_max - after_range_min)
    return after_value


def theta_to_servo_degree(theta, servo_number, relation_list, config_calibration_value=None):
    """
    将杆件的角度转化为舵机角度
    :param theta: 弧度制 杆件角度
    :param servo_number: 舵机号
    :param relation_list: 舵机关系映射表
    :param config_calibration_value:
    :return:舵机角 in 角度制
    """
    if config_calibration_value is None:
        config_calibration_value = [0, 0, 0]
    theta = degrees(theta)
    servo_degree = 0
    if servo_number == 1:
        #print("servo1")
        servo_degree = 0  # 此处需要根据舵机1修改
    elif servo_number == 2:
        #print("servo2")
        servo_degree = theta + relation_list[1] + config_calibration_value[1]
    elif servo_number == 3:
        #print("servo3")
        servo_degree = theta + relation_list[2] + config_calibration_value[2]
    else:
        #print("ERROR:theta_to_servo_degree")
        servo_degree = 0
    return servo_degree


def servo_control(servo_number, degree):
    """
    通过角度值控制电机输出对应的角度
    :return:
    """
    dc_trans = servo_map(degree, 0, 180, servo_width_min, servo_width_max)
    servo[servo_number - 1].ChangeDutyCycle(dc_trans)
    print("舵机%d已转动到%f°处" % (servo_number, degree))


def circle_point_generate(center_point, radius, frame):
    """
     输入圆心[x0,y0]，半径r，及计数c，返还圆周上单个点的坐标[x,y]
    :param center_point: 圆心[x0,y0]
    :param radius: 半径
    :param frame: 切分的样本点个数
    :return: 圆周上单个点的坐标[x,y]组成的两个array的list
    """
    theta = np.linspace(0, 2 * np.pi, frame)
    xs = center_point[0] + radius * np.cos(theta)
    ys = center_point[1] + radius * np.sin(theta)
    return xs, ys


def preprocess_drawing_data(theta1, theta2, l1, l2):
    """
    处理角度数据,转化为matplotlib适应的绘图格式
    :param theta1: 角度数据1
    :param theta2: 角度数据2
    :param l1: 杆件长1
    :param l2: 杆件长2
    :return: 大小腿点位x坐标数组和y坐标数组
    """
    xs = [0]
    ys = [0]
    # 分别算出x1 y1和x2 y2
    x1 = l1 * cos(theta1)
    y1 = l1 * sin(theta1)
    x2 = x1 + l2 * cos(theta1 + theta2)
    y2 = y1 + l2 * sin(theta1 + theta2)
    xs.append(x1)
    xs.append(x2)
    ys.append(y1)
    ys.append(y2)
    return xs, ys


def animation_update(frame):
    """
    更新动画并同步到硬件电机
    注意：matplotlib在线动画锁帧在30fps，frame不得高于30
    :param frame:
    :return:
    """
    # 硬件舵机运动同步
    servo_control(1, servo_degree[0][frame])
    servo_control(2, servo_degree[1][frame])

    # 更新circle绘画
    circle_artist.set_xdata(xs[0:frame])  # 直接设置x
    circle_artist.set_ydata(ys[0:frame])  # 直接设置y
    # 更新leg绘画
    leg_artist.set_xdata(leg_data_xs[frame])  # 直接设置x
    leg_artist.set_ydata(leg_data_ys[frame])  # 直接设置y
    return circle_artist, leg_artist


# 配置及初始化
center_point = [1.767767, -8.838835]  # 圆周运动的圆心
radius = 2  # 圆半径
frame = 60  # 切分样本数
leg_data_xs=[]#腿部各个点位数据x
leg_data_ys=[]#腿部各个点位数据y
position = [0,0]  # 传给舵机的位置
link_length = [5, 7.5]  # 杆件长度 in cm
config_degree_relation_list = [+0, +225, +0]
servo = [0, 0, 0]
servo_degree = [[],[]] #舵机数据表
servo_SIG = [32, 33]  # PWM信号端
servo_VCC = [2, 4, 1]  # VCC端
servo_GND = [30, 34, 39]  # GND端
servo_freq = 50  # PWM频率
servo_width_min = 2.5  # 工作脉宽最小值
servo_width_max = 12.5  # 工作脉宽最大值
GPIO.setmode(GPIO.BOARD)  # 初始化GPIO引脚编码方式
for i in range(0, len(servo_SIG)):
    GPIO.setup(servo_SIG[i], GPIO.OUT)
    servo[i] = GPIO.PWM(servo_SIG[i], servo_freq)
    servo[i].start(0)
    servo[i].ChangeDutyCycle((servo_width_min + servo_width_max) / 2)  # 回归舵机中位


# 圆轨迹生成
xs, ys = circle_point_generate(center_point, radius, frame)
# 腿部轨迹生成
for i in range(0,frame):
    position[0] = xs[i]
    position[1] = ys[i]
    # 获取运动学逆解值
    joints_angles = position_2_theta(position[0], position[1], link_length[0], link_length[1])
    # 逆解值转绘图数据
    leg_data_pre = preprocess_drawing_data(joints_angles[0], joints_angles[2], link_length[0], link_length[1])
    leg_data_xs.append(leg_data_pre[0])
    leg_data_ys.append(leg_data_pre[1])
    # 杆件角度转舵机角度
    servo_degree[0].append(theta_to_servo_degree(joints_angles[0], 2, config_degree_relation_list))
    servo_degree[1].append(theta_to_servo_degree(joints_angles[2], 3, config_degree_relation_list))
    
print("初始化完成，1秒后等待操作")
time.sleep(1)
# matplotlib可视化部分
fig, ax = plt.subplots(figsize=(6,6))  # 建立图像
plt.axis("equal")
plt.grid()
circle_artist = ax.plot(xs[0], ys[0], '--', lw=2, color='blue')[0]
leg_artist = ax.plot(leg_data_xs[0], leg_data_ys[0], 'o-', lw=2, color='black')[0]
ax.set(xlim=[-6,7],ylim=[-12,1],xlabel='X', ylabel='Y', title='mini pupper IK RR model Circle Plot')
#plt.tick_params(axis="both")
ani = animation.FuncAnimation(fig=fig, func=animation_update, frames=frame, interval=1,blit=True)  # 设置动画，interval单位为ms

plt.show()
plt.clf("all")
for i in range(0, len(servo_SIG)):
    servo[i].stop()  # 停止pwm
GPIO.cleanup()  # 清理GPIO引脚

实验总结

经过本知识点的学习和实验操作，你应该能达到以下水平：

版权信息：教材尚未完善，此处预留版权信息处理方式
mini pupper相关内容可访问：https://github.com/mangdangroboticsclub