运动学与动力学约束下的在线机器人时间最优轨迹规划算法

本篇目录

• • 一、简介
  • • 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::constraint::jointTorque::Pinocchio<>>(model);

  toppra::LinearConstraintPtrs constraints = toppra::LinearConstraintPtrs{
      std::make_shared<toppra::constraint::LinearJointVelocity>(velLimitLower, velLimitUpper),
      std::make_shared<toppra::constraint::LinearJointAcceleration>(accLimitLower, accLimitUpper),
      jntTorqueCnt_ptr};//velLimitLower，accLimitLower均为Eigen::VectorXd

  auto pts = CalToppZonePoints(p, waypoints, zone_ratio);

  toppra::Vectors positions;//此处省略了定义positions的具体实现，需要自己定义

  std::array<toppra::BoundaryCond, 2> boundary_cond;
  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::PiecewisePolyPath>(path);

  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例子，用于时间最优的考虑动力学约束的轨迹规划，后续会推出该库的原理解析。