本篇目录
一、简介1.1 问题描述1.2 时间最优轨迹规划现状简介
二、动力学约束下的时间最优轨迹规划2.1 引用的库2.2 相关代码2.3 效果展示2.4 小结
关键词： 轨迹规划、时间最优、动力学约束、机器人、在线
KeyWord:Trjectory plan、Time-Optimal、Dynamics constraint、Robot、Online
一、简介
1.1 问题描述
对于机器人来说，轨迹规划通常可以分为两个层次，第一个层次，根据碰撞等约束找到空间中的几何路径点，不带时间信息，通常称之为路径规划(path planning)，该问题的求解思路可以参考这里，第二个层次是根据已经获取的路径以及机器人系统的约束，找到一条带时间参数的轨迹(trajectory planning)，本文章的就是尝试去解决第二个问题。 对于机器人来说，时间最优的轨迹规划在一些的场景下是非常有意义的事情，比如抓取，通常希望节拍越快越好，大多数的轨迹规划约束通常只考虑机器人的运动学约束，但在很多时候，机器人在达到运动学约束时，驱动器的能力并没有拉满，因此时间最优的轨迹规划考虑动力学约束是非常有意义的，这篇文章就是以时间最优为目标，考虑动力学约束进行轨迹规划。
1.2 时间最优轨迹规划现状简介
当前动力学约束下的时间最优的轨迹规划思路主要有两类，第一类是基于数值积分的方法【Numerical Integration (NI)】，类似bang-bang控制的思路，该方法求解时间快，有利于进行在线求解，但是很难求解，鲁棒性不足；第二类方法是基于凸优化的方法【Convex Optimization(CO)】，该方法问题很好形成，只要形成凸优化问题，即可找到现有的凸优化求解器进行求解，鲁棒性也很好，但是求解效率很低，很难进行在线的轨迹规划。 当前基于CO的方法比较好的有这个库，对应文章：Time-Optimal Path Tracking for Robots a Convex Optimization Approach，这个库展示了如何形成一个凸锥问题然后进行求解的过程，原理清晰，但是求解效率很低，无法进行在线规划；基于NI的方法比较好的是Toppra库，该链接里面有对应的文章，该方法最大的缺点是不是jerk bound，因此需要考虑动力学约束，但是该库并没有展示完整的例子，这也是本文主要做的事情。
二、动力学约束下的时间最优轨迹规划
2.1 引用的库
1、Pinocchio库，动力学计算的库 几种安装方式都不是很好用，还是推荐使用conda安装。
2、Toppra库
pip install toppra
3、机器人模型库（获取urdf文件） 这个库里面包含了多种机器人模型，比如panda、UR、人形机器人等，获取机器人模型文件可以从这两获取。
2.2 相关代码
相关代码的解释在代码注释里面
import toppra as ta
import pinocchio
import numpy as np
import time
import matplotlib.pyplot as plt
def generate_random_trajectory(robot, npts, maxd, randskip=0):
for i in range(randskip):
pinocchio.randomConfiguration(robot)
ts = [
0.0,
]
qs = [
pinocchio.randomConfiguration(robot),
]
while len(qs) < npts:
qlast = qs[-1]
q = pinocchio.randomConfiguration(robot)
d = pinocchio.distance(robot, qlast, q)
if d > maxd:
q = pinocchio.interpolate(robot, qlast, q, maxd / d)
qs.append(q)
ts.append(ts[-1] + pinocchio.distance(robot, qlast, q))
# Compute velocities and accelerations
vs = [
np.zeros(robot.nv),
]
accs = [
np.zeros(robot.nv),
]
eps = 0.01
for q0, q1, q2 in zip(qs, qs[1:], qs[2:]):
qprev = pinocchio.interpolate(
robot, q0, q1, 1 - eps / pinocchio.distance(robot, q0, q1)
)
qnext = pinocchio.interpolate(
robot, q1, q2, eps / pinocchio.distance(robot, q1, q2)
)
# (qnext - qprev) / eps
vs.append(pinocchio.difference(robot, qprev, qnext) / eps)
# ((qnext - q) - (q - qprev)) / eps^2
accs.append(
(
pinocchio.difference(robot, q, qnext)
- pinocchio.difference(robot, qprev, q)
)
/ (eps ** 2)
)
vs.append(np.zeros(robot.nv))
accs.append(np.zeros(robot.nv))
path = ta.SplineInterpolator(np.linspace(0, 1, npts), qs) #采用Spline进行path的插值
return path
def joint_velocity_constraint(robot, scale=1.0):
from toppra.constraint import JointVelocityConstraint
return JointVelocityConstraint(
np.vstack(
[-scale * robot.velocityLimit, scale * robot.velocityLimit]
).T
)
def joint_acceleration_constraint(robot, limit):
from toppra.constraint import JointAccelerationConstraint, DiscretizationType
l = np.array([limit,] * robot.nv)
ja = JointAccelerationConstraint(np.vstack([-l, l]).T)
ja.discretization_type = DiscretizationType.Interpolation
return ja
def inv_dyn1(robot,q, v, a):
data = robot.createData()
return pinocchio.rnea(robot,data, q, v, a)
def torque_constraint(robot, scale=1.0):
from toppra.constraint import JointTorqueConstraint, DiscretizationType
def inv_dyn(q, v, a):
data = robot.createData()
return pinocchio.rnea(robot,data, q, v, a)
jt = JointTorqueConstraint(
inv_dyn,
np.vstack(
[-scale * robot.effortLimit, scale * robot.effortLimit]
).T,
np.zeros(robot.nv),
)
jt.discretization_type = DiscretizationType.Interpolation
return jt
def main():
robot = pinocchio.buildModelFromUrdf('robot.urdf') #导入机器人模型
print('robot name: ' + robot.name)
path = generate_random_trajectory(robot, 5, 0.3)#生成路径规划点
constraints = [
joint_velocity_constraint(robot),#速度约束
joint_acceleration_constraint(robot, 2.0),#加速度约束
torque_constraint(robot),#动力学约束
]
Ngripoints = 1000
instance = ta.algorithm.TOPPRA(
constraints,
path,
solver_wrapper="seidel",
gridpoints=np.linspace(0, path.duration, Ngripoints + 1)
)#构建TOPPRA问题
jnt_traj = instance.compute_trajectory(0, 0)
ts_sample = np.linspace(0, jnt_traj.get_duration(), 100)
print("traj duration:",jnt_traj.get_duration())#输出轨迹的耗时
#轨迹的位置、速度、加速度信息
qs_sample = jnt_traj.eval(ts_sample)
qds_sample = jnt_traj.evald(ts_sample)
qdds_sample = jnt_traj.evaldd(ts_sample)
#计算输出轨迹的力矩信息
torque = []
for q_, qd_, qdd_ in zip(qs_sample, qds_sample, qdds_sample):
torque.append(inv_dyn1(robot,q_, qd_, qdd_))
torque = np.array(torque)
#绘制力矩
fig, axs = plt.subplots(6, 1)
for i in range(0, 6):
axs[i].plot(ts_sample, torque[:, i])
plt.xlabel("Time (s)")
plt.ylabel("Torque $(Nm)$")
plt.legend(loc='upper right')
plt.show()
#绘制位置
fig, axs = plt.subplots(6, 1)
for i in range(0, 6):
axs[i].plot(ts_sample, qs_sample[:,i],label = "pos")
axs[i].legend()
plt.show()
#绘制速度
fig, axs = plt.subplots(6, 1)
for i in range(0, 6):
axs[i].plot(ts_sample, qds_sample[:,i],label = "vel")
axs[i].legend()
plt.show()
#绘制加速度
fig, axs = plt.subplots(6, 1)
for i in range(0, 6):
axs[i].plot(ts_sample, qdds_sample[:,i],label = "acc")
axs[i].legend()
plt.show()
# Compute the feasible sets and the controllable sets for viewing.
# Note that these steps are not necessary.
_, sd_vec, _ = instance.compute_parameterization(0, 0)
X = instance.compute_feasible_sets()
K = instance.compute_controllable_sets(0, 0)
X = np.sqrt(X)
K = np.sqrt(K)
plt.plot(X[:, 0], c='green', label="Feasible sets")
plt.plot(X[:, 1], c='green')
plt.plot(K[:, 0], '--', c='red', label="Controllable sets")
plt.plot(K[:, 1], '--', c='red')
plt.plot(sd_vec, label="Velocity profile")
plt.title("Path-position path-velocity plot")
plt.xlabel("Path position")
plt.ylabel("Path velocity square")
plt.legend()
plt.tight_layout()
plt.show()
if __name__ == "__main__":
main()
C++版，这里仅给出部分核心代码
const std::string urdf_filename = "/robot.urdf";
pinocchio::Model model;
pinocchio::urdf::buildModel(urdf_filename, model);
pinocchio::Data data(model);
auto jntTorqueCnt_ptr = std::make_shared
toppra::LinearConstraintPtrs constraints = toppra::LinearConstraintPtrs{
std::make_shared
std::make_shared
jntTorqueCnt_ptr};//velLimitLower，accLimitLower均为Eigen::VectorXd
auto pts = CalToppZonePoints(p, waypoints, zone_ratio);
toppra::Vectors positions;//此处省略了定义positions的具体实现，需要自己定义
std::array
toppra::BoundaryCond bc{"clamped"};
boundary_cond[0] = bc;
boundary_cond[1] = bc;
toppra::Vector times(positions.size());
times.setLinSpaced(0, 1);
toppra::PiecewisePolyPath path =
toppra::PiecewisePolyPath::CubicSpline(positions, times, boundary_cond);
toppra::GeometricPathPtr path_ptr = std::make_shared
toppra::algorithm::TOPPRA problem{constraints, path_ptr};
toppra::ReturnCode ret_code = problem.computePathParametrization();
toppra::Vector gridpoints = problem.getParameterizationData().gridpoints;
toppra::Vector vsquared = problem.getParameterizationData().parametrization;
toppra::parametrizer::ConstAccel output_traj{path_ptr,
problem.getParameterizationData().gridpoints,
problem.getParameterizationData().parametrization};
auto path_interval = output_traj.pathInterval();
double duration = path_interval[1] - path_interval[0];
std::cout << "Traj duration:" << duration << std::endl;
int rows = std::ceil(duration / 0.002);
toppra::Vector time(rows);
time.setLinSpaced(0, duration);
//插值后的位置、速度、加速度
toppra::Vectors topp_pos_vec_ = output_traj.eval(time, 0);
toppra::Vectors topp_vel_vec_ = output_traj.eval(time, 1);
toppra::Vectors topp_acc_vec_ = output_traj.eval(time, 2);
2.3 效果展示
这里使用的是UR5机械臂，力矩、位置、速度、加速度曲线如下所示： Toppra库的前向与反向传播图如下：
2.4 小结
以上代码展示了一个简单的python例子，用于时间最优的考虑动力学约束的轨迹规划，后续会推出该库的原理解析。
精彩链接
您阅读本篇文章共花了：
发表评论