-
關節空間 VS 操作空間
關節空間與操作空間軌跡規划流程圖如下（上標$i$和$f$分別代表起始位置initial和目標位置final）：
在關節空間內進行軌跡規划有如下優點：
- 在線運算量更小，即無需進行機器人的逆解或正解解算
- 不受機器人奇異構型影響
- 可以根據機器人或驅動器手冊直接確定最大速度或力矩
其缺點是對應操作空間的軌跡無法預測，增加了機械臂與環境碰撞的可能。例如，考慮下面的二連桿機構，關節運動的限制為：$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{cases}& 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{cases}$$
加速度在$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{cases}&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{cases}$$
如果目標點距離初始位置過近，可能達不到最大速度和加速度就要開始減速，考慮以最大速度做勻速直線運動階段的時間為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 phase 以及 libfranka MotionGenerator，修改關節空間軌跡規划代碼，並在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 use 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; };
traj.cpp
#include "traj.h" #include <algorithm> #include <array> #include <cmath> TrajectoryGenerator::TrajectoryGenerator(double speed_factor, const std::array<double, 7> q_goal) : q_goal_(q_goal.data()) { dq_max_ *= speed_factor; ddq_max_ *= speed_factor; dq_max_sync_.setZero(); q_start_.setZero(); delta_q_.setZero(); t_1_sync_.setZero(); t_2_sync_.setZero(); t_f_sync_.setZero(); q_1_.setZero(); } bool TrajectoryGenerator::calculateDesiredValues(double t, Vector7d* delta_q_d) { Vector7i sign_delta_q; sign_delta_q << delta_q_.cwiseSign().cast<int>(); // sign(D_j) Vector7d t_d = t_2_sync_ - t_1_sync_; // h' std::array<bool, 7> joint_motion_finished{}; // motion falgs for (size_t i = 0; i < 7; i++) // calculate joint positions { if (std::abs(delta_q_[i]) < DeltaQMotionFinished){ // target approaches the goal (*delta_q_d)[i] = 0; joint_motion_finished[i] = true;} else { if (t < t_1_sync_[i]) { (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] * (0.5 * t - t_1_sync_[i]) * std::pow(t, 3.0); dq[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] * (2.0 * t - 3 * t_1_sync_[i]) * std::pow(t, 2.0); } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) { (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i]; dq[i] = dq_max_sync_[i]; } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) { (*delta_q_d)[i] = delta_q_[i] + 0.5 *(1.0 / std::pow(t_1_sync_[i], 3.0) *(t - 3.0 * t_1_sync_[i] - t_d[i]) *std::pow((t - t_1_sync_[i] - t_d[i]), 3.0) + (2.0 * t - 3.0 * t_1_sync_[i] - 2.0 * t_d[i])) *dq_max_sync_[i] * sign_delta_q[i]; dq[i] = (1.0 / std::pow(t_1_sync_[i], 3.0) *(2 * t - 5.0 * t_1_sync_[i] - 2 * t_d[i])*std::pow((t - t_1_sync_[i] - t_d[i]), 2.0) + 1) * dq_max_sync_[i] * sign_delta_q[i]; } else { (*delta_q_d)[i] = delta_q_[i]; // reach the goal joint_motion_finished[i] = true;} } } return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(),[](bool x) { return x; }); } void TrajectoryGenerator::calculateSynchronizedValues() { Vector7d dq_max_reach(dq_max_); Vector7d t_f = Vector7d::Zero(); Vector7d t_1 = Vector7d::Zero(); Vector7i sign_delta_q; sign_delta_q << delta_q_.cwiseSign().cast<int>(); // only consider single axis for (size_t i = 0; i < 7; i++) { if (std::abs(delta_q_[i]) > DeltaQMotionFinished) { if ( std::abs(delta_q_[i]) < 3.0 / 2.0 * std::pow(dq_max_[i], 2.0) / ddq_max_[i] ) { // the goal not far enough from start position dq_max_reach[i] = std::sqrt( 2.0 / 3.0 * delta_q_[i] * sign_delta_q[i] * ddq_max_[i] ); // recalculate the maximum velocity } t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_[i]; t_f[i] = t_1[i] + std::abs(delta_q_[i]) / dq_max_reach[i]; } } // take account of the slowest axis double max_t_f = t_f.maxCoeff(); // consider the synchronization of multiple axises for (size_t i = 0; i < 7; i++) { if (std::abs(delta_q_[i]) > DeltaQMotionFinished) { double a = 3.0 / 2.0 * ddq_max_[i]; double b = -1.0 * max_t_f * std::pow(ddq_max_[i] , 2.0); double c = std::abs(delta_q_[i]) * std::pow(ddq_max_[i], 2.0); double delta = b * b - 4.0 * a * c; if (delta < 0.0) { delta = 0.0; } // according to the area under velocity profile, solve equation "a * Kv^2 + b * Kv + c = 0" for Kv dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a); // Kv: maximum synchronization velocity t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_[i]; t_f_sync_[i] =(t_1_sync_)[i] + std::abs(delta_q_[i] / dq_max_sync_[i]); t_2_sync_[i] = (t_f_sync_)[i] - t_1_sync_[i]; q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]); } } } bool TrajectoryGenerator::operator()(const RobotState& robot_state, double time) { time_ = time; if (time_ == 0.0) { q_start_ = Vector7d(robot_state.q_d.data()); delta_q_ = q_goal_ - q_start_; calculateSynchronizedValues(); } Vector7d delta_q_d; bool motion_finished = calculateDesiredValues(time_, &delta_q_d); std::array<double, 7> joint_positions; Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ + delta_q_d); return motion_finished; }
main.cpp
#define _USE_MATH_DEFINES #include <math.h> #include "traj.h" #include <thread> #include <iostream> int main(int argc, char *argv[]) { RobotState current_state; current_state.q_d = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; double speed_factor = 0.5; std::array<double, 7> q_goal = { M_PI_4, M_PI_2, 0.0 , 0.0 , 0.0 , 0.0, 0.0 }; TrajectoryGenerator traj_generator(speed_factor, q_goal); quint64 count = 0; bool isfinished = false; while (!isfinished) { isfinished = traj_generator(current_state, count / 1000.0); std::this_thread::sleep_for(std::chrono::milliseconds(1)); count++; } std::cout << "Motion finished" << std::endl; return 0; }
注意以下幾點：
1. 原examples_common.h代碼中的ddq_max_start_為加速度，ddq_max_goal_為減速度（接近目標點，開始減速），大多數情況下兩者相等
2. 在根據速度曲線與時間軸圍成的面積計算最大同步速度的時候，會遇到一元二次方程$a\cdot v_{sync}^2+b\cdot v_{sync}+c=0$求解的問題，對於大於零的兩個解要選其中數值小的那個，否則會超過最大速度限制，即取值為$\frac{-b-\sqrt{b^2-4ac}}{2a}$。可以簡要證明如下：
這兩個解分布在$v=\frac{-b}{2a}$的兩側，而$\frac{-b}{2a}=\frac{t_{f}k_a}{3}=\frac{1}{3}(\frac{3}{2}\frac{k_v}{k_a}+\frac{|D_j|}{k_v})k_a=\frac{1}{2}k_v+\frac{1}{2}(\frac{2|D_j|k_a}{3k_v})$，根據$|D_j|$的條件 $\frac{2|D_j|k_a}{3k_v}-k_v=\frac{1}{3k_v}(2|D_j|k_a-3k_v^2)> 0$，因此$\frac{-b}{2a}> k_v$，即值較大的解會超出速度限制。
將時間、軸1軸2的關節角度和速度保存在CSV文件中，用Excel畫出散點圖。關節角度隨時間變化曲線如下（軸1從0→45°，軸2從0→90°）：
關節速度隨時間變化曲線如下：
參考：
機器人中的軌跡規划（Trajectory Planning ）
周期同步位置模式(CSP)，輪廓位置模式(PPM)，位置模式(PM)
Robot and interface specifications
Trajectory Planning for Automatic Machines and Robots
Modeling Identification and Control of Robots. 13.3.4 Continuous acceleration profile with constant velocity phase