零力矩点（Zero Moment Point，ZMP）概述
- ZMP步态控制是人形机器人步态控制中的一个关键概念，旨在确保机器人在行走或站立过程中保持平衡。ZMP是指机器人接触面上力矩为零的点，确保在该点上机器人不会倾倒。
- 这个示例展示了ZMP步态控制的基本原理和实现方法。实际应用中，机器人动力学模型会更加复杂，通常需要更高级的控制算法和优化技术来实现稳定和高效的步态控制
以下是ZMP步态控制的原理和基本步骤。
ZMP 步态控制原理
1、ZMP 定义：
ZMP 是机器人在支撑面上的一个点，在这个点上，支撑面的所有水平力矩为零。换句话说，ZMP 是重力中心投影点在支撑面上的位置。
2、质心轨迹规划：
在步态规划过程中，首先需要确定机器人质心（Center of Mass，CoM）的轨迹。CoM 轨迹通常是平滑的曲线，用来确保机器人行走的平稳性。
3、ZMP 轨迹规划：
确定目标的 ZMP 轨迹，使其在机器人支撑面内移动，以确保在行走过程中机器人保持平衡。
4、逆运动学：
基于 CoM 和 ZMP 轨迹，使用逆运动学计算每个关节的期望角度和角速度，确保机器人身体的各部分按照规划轨迹移动。
5、前馈控制和反馈控制：
使用前馈控制基于规划好的轨迹产生关节力矩，并结合反馈控制来纠正由于模型误差和外界扰动导致的轨迹偏差。
数学原理
ZMP 的计算基于质心的运动和重力的作用。假设机器人只有两个关节，一个在髋部，一个在膝部，分别用角度 q1和 q2表示。其质心位置 x𝑐和 y𝑐
以及质心加速度可以表示为：
其中 x1,y1和 x2,y2分别是各个链节的质心位置。
- ZMP 位置可以表示为：
其中z𝑐是质心的高度，¨x𝑐和 ¨y𝑐分别是质心在水平和垂直方向上的加速度，g 是重力加速度。
ZMP 步态控制的基本步骤
1、规划质心轨迹：
设定机器人的质心轨迹，确保其在行走过程中保持稳定。质心轨迹通常在支撑面上方。
2、规划 ZMP 轨迹：
确保 ZMP 轨迹始终在支撑面内，以避免机器人倾倒。ZMP 轨迹可以是质心轨迹的投影。
3、逆运动学计算：
基于质心和 ZMP 轨迹，计算机器人各关节的期望角度和角速度。
4、生成关节力矩：
使用前馈控制和反馈控制生成关节力矩，以确保各关节按照规划的轨迹移动。
5、实时控制：
实时监控机器人的实际运动，使用传感器数据进行反馈控制，调整关节力矩，纠正轨迹偏差。
示例代码
import numpy as np
import matplotlib.pyplot as plt
# 定义机器人参数
mass = 50 # 机器人的总质量
g = 9.81 # 重力加速度
z_c = 0.9 # 质心高度
# 定义步态参数
step_length = 0.3 # 步长
step_time = 1.0 # 每步时间
num_steps = 10 # 步数
# 规划质心轨迹
time = np.linspace(0, step_time * num_steps, num_steps * 100)
x_c = np.linspace(0, step_length * num_steps, num_steps * 100)
y_c = np.zeros_like(x_c) # 假设质心在y方向没有移动
# 计算质心加速度
x_c_dot = np.gradient(x_c, time)
x_c_ddot = np.gradient(x_c_dot, time)
# 计算 ZMP 轨迹
zmp_x = x_c - (z_c * x_c_ddot / g)
zmp_y = y_c
# 绘制质心轨迹和 ZMP 轨迹
plt.figure(figsize=(10, 6))
plt.plot(time, x_c, label='CoM Trajectory')
plt.plot(time, zmp_x, label='ZMP Trajectory')
plt.xlabel('Time [s]')
plt.ylabel('Position [m]')
plt.title('CoM and ZMP Trajectories')
plt.legend()
plt.grid(True)
plt.show()
代码解释
- 规划质心轨迹：
使用线性函数规划质心在行走过程中的轨迹。 - 计算质心加速度：
使用 np.gradient 计算质心位置的速度和加速度。 - 计算 ZMP 轨迹：
使用 ZMP 公式计算 ZMP 位置。 - 绘制轨迹：
使用 Matplotlib 绘制质心和 ZMP 的轨迹。
