机器人运动估计——IMU运动方程与ESKF原理介绍（下）
Hello!欢迎来到我的博客今天的内容关于机器人中常用的传感器IMU，我们用它来实现机器人姿态、速度、位置的估计。今天将会介绍使用低成本IMU进行机器人运动估计的一个常用方法——ESKF。
3ErrorStateKalmanFilter（ESKF介绍）3.1动机与流程使用IMU的初衷是为了求解载体的位置、速度、姿态等系统状态，然而由于IMU测量数据（包括加速度计和陀螺仪的测量数据）都带有大量噪声，直接利用IMU运动方程进行积分会随着时间发生显著的漂移。因此，为了避免漂移我们需要融合其他传感器的信息（如GPS、视觉），对IMU运动积分的结果进行修正。
Error-stateKalmanFilter（ESKF）就是一种传感器融合的算法，它的基础仍然是卡尔曼滤波。它的核心思想是把系统的状态分为三类：
truestate：实际状态，系统实际的运行状态nominalstate：名义状态，描述了运动状态的主要趋势，主导成分。（large-signal，非线性）errorstate：误差状态，实际状态与名义状态之间的差值（small-signal，近似线性，适合线性高斯滤波）基于以上状态分类，我们可以将关心的true-state，分为两部分，分别进行估计，即nominalstate和errorstate，然后再进行二者叠加。
ESKF的全过程可以描述为如下步骤：
对高频率的IMU数据ummathbf{u}_mum进行积分，获得nomimalstatexmathbf{x}x。（注意：nominalstate里面不考虑噪声，因此势必会累积误差。这部分误差可依据errorstate进行修正）利用KalmanFilter估计errorstate，这里同样也包括时间更新和量测更新两部分。（注意：这个过程考虑了噪声，并且由于误差状态方程是近似线性的，所以可以直接用卡尔曼滤波）利用errorstate修正nominalstate，获取truestate重置errorstate使用ESKF的优势：
errorstate中的参数数量与运动自由度是相等的，避免了过参数化（over-parameterizationorredundancy）引起的协方差矩阵奇异的风险。errorstate总是接近于0，KalmanFilter工作在原点附近。因此，远离奇异值、万向节锁，并且保证了线性化的合理性和有效性。errorstate总是很小，因此二阶项都可以忽略，因此雅可比矩阵的计算会很简单，很迅速。errorstate的变化平缓，因此KF修正的频率不需要太高。3.2状态定义所有在ESKF中用到的变量定义如下：图片中定义了所以状态，包括truestate，nominalstate以及errorstate。以及errorstate和nominalstate组合获得truestate的方式。另外，图中的各个运算符在上一篇文章中均已有了定义。除了一个叉乘矩阵的符号，其定义如下：
[a]×≜[0−azayaz0−ax−ayax0][mathbf{a}]_{ imes} riangleqleft[egin{array}{ccc}0&-a_{z}&a_{y}\a_{z}&0&-a_{x}\-a_{y}&a_{x}&0end{array} ight][a]×≜⎣⎡0az−ay−az0axay−ax0⎦⎤
图片来自于参考文献[1]的P31。
3.3NominalState积分更新（每个时间步都要执行）3.3.1Nominalstate方程（连续时间）：p˙=vv˙=R(am−ab)+gq˙=12q⊗(ωm−ωb)a˙b=0ω˙b=0g˙=0egin{aligned}dot{mathbf{p}}&=mathbf{v}\dot{mathbf{v}}&=mathbf{R}left(mathbf{a}_{m}-mathbf{a}_{b} ight)+mathbf{g}\dot{mathbf{q}}&=frac{1}{2}mathbf{q}otimesleft(momega_{m}-momega_{b} ight)\dot{mathbf{a}}_{b}&=0\dot{momega}_{b}&=0\dot{mathbf{g}}&=0end{aligned}p˙v˙q˙a˙bω˙bg˙=v=R(am−ab)+g=21q⊗(ωm−ωb)=0=0=0
其中，R=R{q}mathbf{R}=mathbf{R}{mathbf{q}}R=R{q}是基于qmathbf{q}q生成的旋转矩阵，表示从IMU系到惯性系的旋转。注意到，这组方程完全没有考虑噪声。这组方程的推导是显而意见的，只需要在IMU运动方程的基础上，假设噪声项全部为0即可。
3.3.2Nominalstate方程（离散时间，用于写程序）：p←p+vΔt+12(R(am−ab)+g)Δt2v←v+(R(am−ab)+g)Δtq←q⊗q{(ωm−ωb)Δt}ab←abωb←ωbg←gegin{array}{l}mathbf{p}leftarrowmathbf{p}+mathbf{v}Deltat+frac{1}{2}left(mathbf{R}left(mathbf{a}_{m}-mathbf{a}_{b} ight)+mathbf{g} ight)Deltat^{2}\mathbf{v}leftarrowmathbf{v}+left(mathbf{R}left(mathbf{a}_{m}-mathbf{a}_{b} ight)+mathbf{g} ight)Deltat\mathbf{q}leftarrowmathbf{q}otimesmathbf{q}left{left(momega_{m}-momega_{b} ight)Deltat ight}\mathbf{a}_{b}leftarrowmathbf{a}_{b}\momega_{b}leftarrowmomega_{b}\mathbf{g}leftarrowmathbf{g}end{array}p←p+vΔt+21(R(am−ab)+g)Δt2v←v+(R(am−ab)+g)Δtq←q⊗q{(ωm−ωb)Δt}ab←abωb←ωbg←g其中，定义q{v}=ev/2mathbf{q}left{mathbf{v} ight}=e^{mathbf{v}/2}q{v}=ev/2，所以：q{(ωm−ωb)Δt}=e(ωm−ωb)Δt/2=euθ/2=[cos(θ/2)usin(θ/2)]mathbf{q}left{left(momega_{m}-momega_{b} ight)Deltat ight}=e^{mathbf{left(momega_{m}-momega_{b} ight)Deltat}/2}=e^{mathbf{u} heta/2}=left[egin{array}{c}cos( heta/2)\mathbf{u}sin( heta/2)end{array} ight]q{(ωm−ωb)Δt}=e(ωm−ωb)Δt/2=euθ/2=[cos(θ/2)usin(θ/2)]其中，θ=∣∣(ωm−ωb)Δt∣∣,u=(ωm−ωb)Δtθ heta=||left(momega_{m}-momega_{b} ight)Deltat||,mathbf{u}=frac{left(momega_{m}-momega_{b} ight)Deltat}{ heta}θ=∣∣(ωm−ωb)Δt∣∣,u=θ(ωm−ωb)Δt
实际上，上面这个操作就是把角度增量改造成单位四元数，两个单位四元数相乘仍然是单位四元数，这样才能保证qmathbf{q}q始终为单位四元数，保证迭代是收敛的。
3.4ErrorState时间更新（每个时间步都要执行）3.4.1Errorstate方程（连续时间）：δp˙=δvδv˙=−R[am−ab]×δθ−Rδab+δg−Ranδθ˙=−[ωm−ωb]×δθ−δωb−ωnδa˙b=awδω˙b=ωwδg˙=0egin{aligned}deltadotmathbf{p}&=deltamathbf{v}\dot{deltamathbf{v}}&=-mathbf{R}left[mathbf{a}_{m}-mathbf{a}_{b} ight]_{ imes}deltaoldsymbol{ heta}-mathbf{R}deltamathbf{a}_{b}+deltamathbf{g}-mathbf{R}mathbf{a}_{n}\{delta}dotoldsymbol{ heta}&=-left[oldsymbol{omega}_{m}-oldsymbol{omega}_{b} ight]_{ imes}deltaoldsymbol{ heta}-deltaoldsymbol{omega}_{b}-oldsymbol{omega}_{n}\deltadotmathbf{a}_{b}&=mathbf{a}_{w}\deltadot{oldsymbol{omega}}_{b}&=oldsymbol{omega}_{w}\dot{deltamathbf{g}}&=0end{aligned}δp˙δv˙δθ˙δa˙bδω˙bδg˙=δv=−R[am−ab]×δθ−Rδab+δg−Ran=−[ωm−ωb]×δθ−δωb−ωn=aw=ωw=0这个误差方程，考虑了噪声等不确定性。其形式看起来似乎不太直观，但实际上推导也很简单。比如，以位置为例：δp=pt−pdeltamathbf{p}=mathbf{p}_t-mathbf{p}δp=pt−p所以其导数为：δp˙=p˙t−p˙=vt−v=δvdeltadotmathbf{p}=dotmathbf{p}_t-dotmathbf{p}=mathbf{v}_t-mathbf{v}=deltamathbf{v}δp˙=p˙t−p˙=vt−v=δv再比如以速度为例：δv=vt−vdeltamathbf{v}=mathbf{v}_t-mathbf{v}δv=vt−v所以其导数为：δv˙=v˙t−v˙=Rt(am−abt−an)+gt−(R(am−ab)+g)=(RδR−R)(am−ab)−RδR(δab+am)+(gt−g)≈[R(I+[δθ]×)−R](am−ab)−R(I+[δθ]×)(δab+am)+δg≈R[δθ]×(am−ab)−R(δab+am)+δg=−R[am−ab]×δθ−Rδab+δg−Ranegin{aligned}deltadotmathbf{v}&=dotmathbf{v}_t-dotmathbf{v}\&=mathbf{R}_{t}left(mathbf{a}_{m}-mathbf{a}_{bt}-mathbf{a}_{n} ight)+mathbf{g}_{t}-(mathbf{R}left(mathbf{a}_{m}-mathbf{a}_b ight)+mathbf{g})\&=(mathbf{R}deltamathbf{R}-mathbf{R})(mathbf{a}_{m}-mathbf{a}_{b})-mathbf{R}deltamathbf{R}(deltamathbf{a}_b+mathbf{a}_m)+(mathbf{g}_t-mathbf{g})\&approx[mathbf{R}(mathbf{I}+[deltam{ heta}]_ imes)-mathbf{R}](mathbf{a}_{m}-mathbf{a}_{b})-mathbf{R}(mathbf{I}+[deltam{ heta}]_ imes)(deltamathbf{a}_b+mathbf{a}_m)+deltamathbf{g}\&approxmathbf{R}[deltam{ heta}]_ imes(mathbf{a}_{m}-mathbf{a}_{b})-mathbf{R}(deltamathbf{a}_b+mathbf{a}_m)+deltamathbf{g}\&=-mathbf{R}left[mathbf{a}_{m}-mathbf{a}_{b} ight]_{ imes}deltaoldsymbol{ heta}-mathbf{R}deltamathbf{a}_{b}+deltamathbf{g}-mathbf{R}mathbf{a}_{n}\end{aligned}δv˙=v˙t−v˙=Rt(am−abt−an)+gt−(R(am−ab)+g)=(RδR−R)(am−ab)−RδR(δab+am)+(gt−g)≈[R(I+[δθ]×)−R](am−ab)−R(I+[δθ]×)(δab+am)+δg≈R[δθ]×(am−ab)−R(δab+am)+δg=−R[am−ab]×δθ−Rδab+δg−Ran其中，用到了δR=e[δθ]×≈I+[δθ]×deltamathbf{R}=e^{[deltam{ heta}]_ imes}approxmathbf{I}+[deltam{ heta}]_ imesδR=e[δθ]×≈I+[δθ]×。此外，还用到了两个小量相乘可以忽略的假设。
其它方程的推导，就不在此说明了，可以参考文献[1]的P33页。
3.4.2Errorstate方程（离散时间，用于写程序）：定义状态向量：x=[pvqabωbg]∈R19×1,δx=[δpδvδθδabδωbδg]∈R18×1,um=[amωm]∈R6×1,i=[viθiaiωi]∈R12×1mathbf{x}=left[egin{array}{l}mathbf{p}\mathbf{v}\mathbf{q}\mathbf{a}_{b}\oldsymbol{omega}_{b}\mathbf{g}end{array} ight]inmathbb{R}_{19 imes1},quaddeltamathbf{x}=left[egin{array}{c}deltamathbf{p}\deltamathbf{v}\deltaoldsymbol{ heta}\deltamathbf{a}_{b}\deltaoldsymbol{omega}_{b}\deltamathbf{g}end{array} ight]inmathbb{R}_{18 imes1}quad,quadmathbf{u}_{m}=left[egin{array}{c}mathbf{a}_{m}\oldsymbol{omega}_{m}end{array} ight]inmathbb{R}_{6 imes1},quadmathbf{i}=left[egin{array}{c}mathbf{v}_{mathbf{i}}\oldsymbol{ heta}_{mathbf{i}}\mathbf{a}_{mathbf{i}}\oldsymbol{omega}_{mathbf{i}}end{array} ight]inmathbb{R}_{12 imes1}x=⎣⎢⎢⎢⎢⎢⎢⎡pvqabωbg⎦⎥⎥⎥⎥⎥⎥⎤∈R19×1,δx=⎣⎢⎢⎢⎢⎢⎢⎡δpδvδθδabδωbδg⎦⎥⎥⎥⎥⎥⎥⎤∈R18×1,um=[amωm]∈R6×1,i=⎣⎢⎢⎡viθiaiωi⎦⎥⎥⎤∈R12×1于是误差状态方程可以写作：δx←f(x,δx,um,i)=Fx(x,um)⋅δx+Fi⋅ideltamathbf{x}leftarrowfleft(mathbf{x},deltamathbf{x},mathbf{u}_{m},mathbf{i} ight)=mathbf{F}_{mathbf{x}}left(mathbf{x},mathbf{u}_{m} ight)cdotdeltamathbf{x}+mathbf{F}_{mathbf{i}}cdotmathbf{i}δx←f(x,δx,um,i)=Fx(x,um)⋅δx+Fi⋅i其中:Fx=∂f∂δx∣x,um=[IIΔt00000I−R[am−ab]×Δt−RΔt0IΔt00R⊤{(ωm−ωb)Δt}0−IΔt0000I000000I000000I]∈R18×18mathbf{F}_{mathbf{x}}=left.frac{partialf}{partialdeltamathbf{x}} ight|_{mathbf{x},mathbf{u}_{m}}=left[egin{array}{cccccc}mathbf{I}&mathbf{I}Deltat&0&0&0&0\0&mathbf{I}&-mathbf{R}left[mathbf{a}_{m}-mathbf{a}_{b} ight]_{ imes}Deltat&-mathbf{R}Deltat&0&mathbf{I}Deltat\0&0&mathbf{R}^{ op}left{left(omega_{m}-omega_{b} ight)Deltat ight}&0&-mathbf{I}Deltat&0\0&0&0&mathbf{I}&0&0\0&0&0&0&mathbf{I}&0\0&0&0&0&0&mathbf{I}end{array} ight]inmathbb{R}_{18 imes18}Fx=∂δx∂f∣∣∣∣x,um=⎣⎢⎢⎢⎢⎢⎢⎡I00000IΔtI00000−R[am−ab]×ΔtR⊤{(ωm−ωb)Δt}0000−RΔt0I0000−IΔt0I00IΔt000I⎦⎥⎥⎥⎥⎥⎥⎤∈R18×18
Fi=∂f∂i∣x,um=[0000I0000I0000I0000I0000]∈R18×12,Qi=[Vi0000Θi0000Ai0000Ωi]∈R12×12mathbf{F}_{mathbf{i}}=left.frac{partialf}{partialmathbf{i}} ight|_{mathbf{x},mathbf{u}_{m}}=left[egin{array}{cccc}0&0&0&0\mathbf{I}&0&0&0\0&mathbf{I}&0&0\0&0&mathbf{I}&0\0&0&0&mathbf{I}\0&0&0&0end{array} ight]quadinmathbb{R}_{18 imes12},quadmathbf{Q}_{mathbf{i}}=left[egin{array}{cccc}mathbf{V}_{mathbf{i}}&0&0&0\0&mathbf{Theta}_{mathbf{i}}&0&0\0&0&mathbf{A}_{mathbf{i}}&0\0&0&0&mathbf{Omega}_{mathbf{i}}end{array} ight]inmathbb{R}_{12 imes12}Fi=∂i∂f∣∣∣∣x,um=⎣⎢⎢⎢⎢⎢⎢⎡0I000000I000000I000000I0⎦⎥⎥⎥⎥⎥⎥⎤∈R18×12,Qi=⎣⎢⎢⎡Vi0000Θi0000Ai0000Ωi⎦⎥⎥⎤∈R12×12
其中，vi,θi,ai,ωimathbf{v}_{mathbf{i}},oldsymbol{ heta}_{mathbf{i}},mathbf{a}_{mathbf{i}},oldsymbol{omega}_{mathbf{i}}vi,θi,ai,ωi代表随机脉冲，他们的均值为0，协方差矩阵为对角阵。另外，定义：R⊤{(ωm−ωb)Δt}=e−[ωm−ωb]×Δt=I−[ωm−ωb]×Δt+12[ωm−ωb]×2Δt2−...mathbf{R}^{ op}left{left(oldsymbol{omega}_{m}-oldsymbol{omega}_{b} ight)Deltat ight}=e^{-[oldsymbol{omega}_{m}-oldsymbol{omega}_{b}]_ imesDeltat}=mathbf{I}-[oldsymbol{omega}_{m}-oldsymbol{omega}_{b}]_ imesDeltat+frac{1}{2}[oldsymbol{omega}_{m}-oldsymbol{omega}_{b}]_ imes^2Deltat^2-...R⊤{(ωm−ωb)Δt}=e−[ωm−ωb]×Δt=I−[ωm−ωb]×Δt+21[ωm−ωb]×2Δt2−...这是一个微分方程的闭合解，利用这个解进行递推会更精确。当然，也可以只取前面两项。
3.4.3对于Errorstate的卡尔曼滤波器的时间更新（用于写程序）误差状态更新δx^←Fx(x,um)⋅δx^hat{deltamathbf{x}}leftarrowmathbf{F}_{mathbf{x}}left(mathbf{x},mathbf{u}_{m} ight)cdothat{deltamathbf{x}}δx^←Fx(x,um)⋅δx^协方差矩阵更新P←FxPFx⊤+FiQiFi⊤mathbf{P}leftarrowmathbf{F}_{mathbf{x}}mathbf{P}mathbf{F}_{mathbf{x}}^{ op}+mathbf{F}_{mathbf{i}}mathbf{Q}_{mathbf{i}}mathbf{F}_{mathbf{i}}^{ op}P←FxPFx⊤+FiQiFi⊤这个就是很基础的卡尔曼滤波方程了，不了解的可以移步我的博客：机器人运动估计系列（番外篇）——从贝叶斯滤波到卡尔曼（上）
3.5ErrorState量测更新（只有测量值到来的时间步需要执行）由于IMU的数据中夹带了大量噪声，我们需要利用更多的传感器的信息，来对状态进行修正。
而这些传感器中常用的组合包括：GPS+IMU，单目视觉+IMU，双目视觉+IMU等。
3.5.1量测方程通常，量测方程的基本形式如下：y=h(xt)+vmathbf{y}=hleft(mathbf{x}_{t} ight)+vy=h(xt)+v其中h()h()h()是量测函数，与传感器有关。vvv是高斯白噪声，为v∼N{0,V}vsimmathcal{N}{0,mathbf{V}}v∼N{0,V}。
为了实现量测更新，我们需要求hhh对δxdeltamathbf{x}δx的导数，所以有：
H≡∂h∂δx∣x=∂h∂xt∣x∂xt∂δx∣x=HxXδxleft.mathbf{H}equivfrac{partialh}{partialdeltamathbf{x}} ight|_{mathbf{x}}=left.left.frac{partialh}{partialmathbf{x}_{t}} ight|_{mathbf{x}}frac{partialmathbf{x}_{t}}{partialdeltamathbf{x}} ight|_{mathbf{x}}=mathbf{H}_{mathbf{x}}mathbf{X}_{deltamathbf{x}}H≡∂δx∂h∣∣∣∣x=∂xt∂h∣∣∣∣x∂δx∂xt∣∣∣∣x=HxXδx其中Hx≜∂h∂xt∣xleft.mathbf{H}_{mathbf{x}} riangleqfrac{partialh}{partialmathbf{x}_{t}} ight|_{mathbf{x}}Hx≜∂xt∂h∣∣∣x是量测函数的雅可比矩阵，根据传感器的不同而具有不同形式。
而Xδx≜∂xt∂δx∣xleft.mathbf{X}_{deltamathbf{x}} riangleqfrac{partialmathbf{x}_{t}}{partialdeltamathbf{x}} ight|_{mathbf{x}}Xδx≜∂δx∂xt∣∣∣x则可以表示为：Xδx≜∂xt∂δx∣x=[I6000Qδθ000I9],Qδθ=12[−qx−qy−qzqw−qzqyqzqw−qx−qyqxqw]left.mathbf{X}_{deltamathbf{x}} riangleqfrac{partialmathbf{x}_{t}}{partialdeltamathbf{x}} ight|_{mathbf{x}}=left[egin{array}{ccc}mathbf{I}_{6}&0&0\0&mathbf{Q}_{deltaoldsymbol{ heta}}&0\0&0&mathbf{I}_{9}end{array} ight],mathbf{Q}_{deltaoldsymbol{ heta}}=frac{1}{2}left[egin{array}{ccc}-q_{x}&-q_{y}&-q_{z}\q_{w}&-q_{z}&q_{y}\q_{z}&q_{w}&-q_{x}\-q_{y}&q_{x}&q_{w}end{array} ight]Xδx≜∂δx∂xt∣∣∣∣x=⎣⎡I6000Qδθ000I9⎦⎤,Qδθ=21⎣⎢⎢⎡−qxqwqz−qy−qy−qzqwqx−qzqy−qxqw⎦⎥⎥⎤
关于上式的详细推导，可以参考文献[1]的P39页。
3.5.2对于Errorstate的卡尔曼滤波器的量测更新（用于写程序）卡尔曼增益的计算K=PH⊤(HPH⊤+V)−1mathbf{K}=mathbf{P}mathbf{H}^{ op}left(mathbf{H}mathbf{P}mathbf{H}^{ op}+mathbf{V} ight)^{-1}K=PH⊤(HPH⊤+V)−1误差状态量测更新δ^x←K(y−h(x^t))hat{delta}mathbf{x}leftarrowmathbf{K}left(mathbf{y}-hleft(hat{mathbf{x}}_{t} ight) ight)δ^x←K(y−h(x^t))协方差矩阵更新P←(I−KH)Pmathbf{P}leftarrow(mathbf{I}-mathbf{K}mathbf{H})mathbf{P}P←(I−KH)P这是卡尔曼滤波的基础方程，就不做介绍了。
3.6状态合并以及ErrorState重置（在量测更新过后执行）3.6.1状态合并这一步是要把errorstate与nominalstate合并，即修正nominalstate随时间的漂移。
x←x⊕δx^mathbf{x}leftarrowmathbf{x}oplushat{deltamathbf{x}}x←x⊕δx^即：p←p+δp^v←v+δv^q←q⊗q{δθ^}ab←ab+δa^bωb←ωb+δω^bg←g+δg^egin{aligned}&egin{array}{l}mathbf{p}leftarrowmathbf{p}+hat{deltamathbf{p}}\mathbf{v}leftarrowmathbf{v}+hat{deltamathbf{v}}end{array}\&mathbf{q}leftarrowmathbf{q}otimesmathbf{q}{hat{deltaoldsymbol{ heta}}}\&mathbf{a}_{b}leftarrowmathbf{a}_{b}+deltahat{mathbf{a}}_{b}\&momega_{b}leftarrowmomega_{b}+deltahat{momega}_{b}\&mathbf{g}leftarrowmathbf{g}+hat{deltamathbf{g}}end{aligned}p←p+δp^v←v+δv^q←q⊗q{δθ^}ab←ab+δa^bωb←ωb+δω^bg←g+δg^
3.6.2Errorstate重置由于nominalstate的误差已经修正了，所以errorstate要置零，同时协方差矩阵也要相应的修改。
通常来说，重置的方法如下：δx^←0P←GPG⊤egin{aligned}&hat{deltamathbf{x}}leftarrow0\&mathbf{P}leftarrowmathbf{G}mathbf{P}mathbf{G}^{ op}end{aligned}δx^←0P←GPG⊤其中：G=I18mathbf{G}=mathbf{I}_{18}G=I18。
4Matlab代码https://github.com/CASIA-RoboticFish/FBUS-EKF
参考文献[1]JoanSol`a.Quaternionkinematicsfortheerror-stateKF.Sep12,2016.
关于的上下两篇文章主要内容出自上面这篇参考文献，我只是进行了翻译和重新梳理逻辑顺序的工作。因此，大家如果使用到本博客内容，也烦请引用上述参考文献。谢谢~
机器人运动规划技术介绍
文章目录运动规划、路径规划和轨迹规划路径规划算法随机采样的算法梯度下降法轨迹规划算法基本思路开源算法介绍总结参考资料：运动规划、路径规划和轨迹规划机器人的运动规划可以看做是包括了路径规划和轨迹规划两个步骤，目的是根据给定的任务，计算出一条可以下发给机器人控制器去执行的轨迹。
路径规划，顾名思义就是在有障碍的空间里找出一条没有碰撞的路径来，一般情况下，这些路径都由一系列的离散点来表示，相邻的点进行线性插值，确保每个离散点和中间的插值点都不会有碰撞。
轨迹规划是根据路径规划生成的离散点，计算一条连续的满足机器人运动学、动力学约束的包含时间信息的轨迹。在轨迹上按照实际的伺服周期采样位置点，下发给伺服控制器，从而使机器人运动到目标位置。