python求解2D倒立摆模型（inverted pendulum）
机器人步态规划中，最简单的模型是倒立摆模型
根据倒立摆模型规划步态，是以轨道能量E Orbital Energy 为基础。
若倒立摆初始位置是在向左侧倾斜的状态，当E为负值时，意味着倒立摆模型重心向右运动一段距离后，速度减为零，无法通过竖直位置，倒回左侧。当E为正值，意味着倒立摆模型可以通过竖直位置，从左侧向右侧继续运动。E=0，刚好到达竖直位置静止。
求解轨道能量方程，给定重心初值和初始轨道能量，即可求解重心的运动轨迹和摆动腿轨迹。轨道能量方程为一阶非线形微分方程，使用scipy库的odeint进行求解。
表示行走摆腿过程中，抬起的一只脚在地面的投影位置。
from scipy.integrate import odeint
import numpy as np
import matplotlib.pyplot as plt
import math
# define parameter
E = 0.5 # initial obital energy
g = 9.8 # gravity acceleration
z = 1.0 # center of mass height
T = 1 # duration time from one foot lifted to the foot on the ground again when walking.
# that is also the duration time of one foot stand on the ground
# define initial condition
xt0 = -1 # initial position of center of mass(COM)
t = np.arange(0, T, 0.01) # (start,stop,step)
# define function (python lambda function function_name = lambda variable1, variable2,... : express format)
dx_dt = lambda x, t: np.sqrt(2 * E + g / z * x ** 2)
xt = odeint(dx_dt, xt0, t) # positon of COM
xt_dot = -np.sqrt(2 * E + g/z * np.power(xt, 2)) # speed of COM
xcp = [] # xcp the projected position on the ground of the lifted foot when moving
for i in range(xt.shape[0]):
if xt[i] < 0:
xcp.append(xt[i] - math.sqrt(z/g * xt_dot[i]**2 - 2 * z/g * E))
else:
xcp.append(xt[i] + math.sqrt(z/g * xt_dot[i]**2 - 2 * z/g * E))
xcp = np.array(xcp)
fig, ax = plt.subplots(3, 1, figsize=(4, 4))
ax = ax.ravel()
ax[0].plot(t, xt, 'r', lw=2)
ax[1].plot(t, xt_dot, 'r', lw=2)
ax[2].plot(t, xcp, 'r', lw=2)
plt.show()
进行步态规划时，通过改变单腿支撑时间T和初始轨道能量E，即可完成步幅的改变。