机器⼈关节空间轨迹规划--S型速度规划关节空间 VS 操作空间
关节空间与操作空间轨迹规划流程图如下（上标$i$和$f$分别代表起始位置initial和⽬标位置final）：
在关节空间内进⾏轨迹规划有如下优点：
1. 在线运算量更⼩，即⽆需进⾏机器⼈的逆解或正解解算
2. 不受机器⼈奇异构型影响
3. 可以根据机器⼈或驱动器⼿册直接确定最⼤速度或⼒矩
韩国化装品其缺点是对应操作空间的轨迹⽆法预测，增加了机械臂与环境碰撞的可能。例如，考虑下⾯的⼆连杆机构，关节运动的限制为：$0^{\circ} \le
\theta_1 \le 180^{\circ}$，$0^{\circ} \le \theta_2 \le 150^{\circ}$
下图中，左侧为关节空间内规划的线性运动轨迹，⽽其对应在操作空间的轨迹却是弧线。机构末端
的可达空间在图中由灰⾊背景表⽰，其⼤⼩和形状受关节运动范围的影响。
下图在操作空间中规划了⼀条直线轨迹，其对应的关节空间轨迹为⼀弧线，且在运动过程中超出了关节值限制。操作空间内进⾏轨迹规划优点是直观，缺点是计算量⼤（需要计算逆解），会遇到奇异性问题以及关节运动超限等。
到底是选择在关节空间还是操作空间内进⾏轨迹规划，取决于任务需要。需要考虑避障或必须沿特定轨迹运动时选择操作空间轨迹规划，只需考虑速度、⼒矩、关节范围等运动约束时选择关节空间轨迹规划（The joint space scheme is appropriate to achieve fast motions in a free space）。
梯形速度曲线
运动控制系统中常⽤的梯形速度曲线如下图所⽰，会出现加速度不连续的情形（从$k_{aj}$到0的跳变），这样可能会导致机械系统出现冲击或不可预料的振动，不过由于机械系统存在⼀定的弹性并不是绝对刚体，这种加速度不连续造成的冲击会被机械机构滤除或减轻。⽽对于⾼速重载的机器⼈来说，这种加速度不连续造成的影响就不能忽略了。可以参考知乎上这个问题：
S型速度曲线
为了使加速度连续，可对梯形速度规划中的加速度曲线进⾏修改，使加速度曲线变为连续的⼆次曲线(a)或者梯形曲线(b)，如下图所⽰。其
中，$\tau'$为加速段时间，$\lambda_jk_{vj}$为第$j$个关节的最⼤运动速度
下⾯考虑a⽅法（Linear Trajectory with Polynomial Blends），关节$j$的运动边界条件如下，即关节$j$初始时刻位置为$q_j^i$，初始速度加速度为0，$\tau'$时刻加速到最⼤速度$\lambda_ik_{vj}sign(D_i)$，$k_{vj}$为理论上关节$j$允许的最⼤速度，$\lambda_j$为⼀⽐例系数（$0 \le \lambda_j \le 1$），$D_j$为从起始位置到⽬标位置的位移，它是⼀个有正负的数值。
根据边界条件加速度⼆次曲线表达式为：$k(t-\tau')t$，对其进⾏积分，可得$\dot{q_j}(t)=\frac{1}{6}k(2t-3\tau')t^2+C$，根据速度边界条件可知$C=0$，$k=\frac{-6}{\tau'^3}\lambda_jk_{vj}$。于是推算出加速度、速度、位置的表达式分别为：$$\begin{cas}& q_j(t)=q_j^i-\frac{1}{\tau'^3}\lambda_jk_{vj}sign(D_j)(\frac{1}{2}t-\tau')t^3\\&\dot{q_j}(t)=-\frac{1}{\tau'^3}\lambda_jk_{vj}sign(D_j)(2t-
3\tau')t^2\\&\ddot{q_j}(t)=-\frac{6}{\tau'^3}\lambda_jk_{vj}sign(D_j)(t-\tau')t \end{cas}$$
加速度在$t=\tau'/2$时最⼤，其幅值为$\left |\ddot{q}_{jmax} \right |=\frac{3}{2}\frac{\lambda_jk_{vj}}{
酒后头疼怎么办\tau'}=\upsilon_j k_{aj}$，则有：$$\tau'=\frac{3}{2}\frac{\lambda_jk_{vj}}{\upsilon_j k_{aj}}$$
根据上式和$q_j(t)$的表达式，可以计算出加速阶段的位移为：$$|q_j^i-q_j(\tau')|=\frac{3}{4}\frac{(\lambda_jk_{vj})^2}{\upsilon_j
k_{aj}}$$
属兔的贵人是什么属相 速度曲线与时间轴围成的⾯积为$|D_j|$，根据计算可以得到关系式：$$t'_f=\tau'+\frac{|D_j|}{\lambda_jk_{vj}}$$
在加速度为0的阶段（最⼤速度阶段，$\tau' \le t \le \tau'+h'$），关节速度表达式为：$$q_i(t)=q_j(\tau')+(t-
\tau')\lambda_jk_{vj}sign(D_j)$$
减速阶段与加速阶段对称($t'_f=2\tau'+h'$)，减速阶段在时间段$\tau'+h' \le t \le t'_f$上的轨迹为：
$$\begin{cas}&q_j(t)=q_j^f+\frac{1}{2}[\frac{1}{\tau'^3}(t-3\tau'-h')(t-\tau'-h')^3+(2t-3\tau'-2h')]\lambda_jk_{vj}sign(D_j) \\
&\dot{q_j}(t)=[\frac{1}{\tau'^3}(2t-5\tau'-2h')(t-\tau'-h')^2+1]\lambda_jk_{vj}sign(D_j) \\ &\ddot{q_j}(t)= \frac{6}{\tau'^3}(t-2\tau'-h') (t-\tau'-h')\lambda_jk_{vj}sign(D_j)\end{cas}$$
如果⽬标点距离初始位置过近，可能达不到最⼤速度和加速度就要开始减速，考虑以最⼤速度做匀速直线运动阶段的时间为0这种临界状态（The minimum time $t_f$ is obtained when the parameters $\lambda_j$ and $\upsilon_j$ are the largest），为了能以最⼤速度运动，位移$|D_j|$必须满⾜如下条件：$$|D_j| > \frac{3}{2}\frac{k_{vj}^2}{k_{aj}}$$ 如果该条件不能满⾜，则最⼤速度值应为：
$$k'_{vj}=\sqrt{\frac{2}{3}|D_j|k_{aj}}$$ 前⾯的计算都只考虑单轴运动的情况，当需要多轴同步时，要考虑运动时间最长的轴（与每个轴的最⼤速度、运动位移等因素有关），将该时间作为同步运动的时间。在确定了同步时间之后，需要重新计算速度曲线的最⼤速度（运动快的轴要降低最⼤速度等待慢的轴），使得各轴在同⼀时刻到达设定的⽬标位置。
参考《Modeling Identification and Control of Robots》的第 13.3.4节 Continuous acceleration profile with constant velocity pha 以及 ，修改关节空间轨迹规划代码，并在while循环中进⾏轨迹⽣成的模拟。
traj.h
怎么炒茄子
#pragma once
#include <array>
#include <Eigen/Core>
struct RobotState
{
std::array<double, 7> q_d{};
};
class TrajectoryGenerator
{
public:
/
/ Creates a new TrajectoryGenerator instance for a target q.
钟表英语怎么读// @param[in] speed_factor: General speed factor in range [0, 1].
安徽板面
// @param[in] q_goal: Target joint positions.
TrajectoryGenerator(double speed_factor, const std::array<double, 7> q_goal);
// Calculate joint positions for u inside a control loop.
bool operator()(const RobotState& robot_state, double time);
private:
using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
using Vector7i = Eigen::Matrix<int, 7, 1, Eigen::ColMajor>;
bool calculateDesiredValues(double t, Vector7d* delta_q_d); // generate joint trajectory void calculateSynchronizedValues();
便宴static constexpr double DeltaQMotionFinished = 1e-6;
const Vector7d q_goal_;
Vector7d q_start_; // initial joint position
Vector7d delta_q_; // the delta angle between start and goal
Vector7d dq_max_sync_;
Vector7d t_1_sync_;
Vector7d t_2_sync_;
Vector7d t_f_sync_;
Vector7d q_1_; // q_1_ = q(\tau) - q_start_
double time_ = 0.0;
Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished();
Vector7d ddq_max_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();
Vector7d dq;
};
View Code
夏朝历史
traj.cpp
#include "traj.h"
#include <algorithm>
#include <array>
