运动控制理论（5）——轨迹规划
重点：现实中更加在意的是手臂末端与物体（G）之间的相对关系，例如，要做一个喷漆的动作，就可以规划一条末端的路线。同时也要控制过程速度，时间。
理想轨迹：Smooth path，就是位置，速度的曲线，都是连续“丝滑”的。
Initial：手臂末端最初的位置，姿态。
via Point：中间必须经过的位置，姿态。
final：最后要停留的位置，姿态。
I.K：反算出机器臂各个关节，再各个点下的角度。（上图，假设机械臂没有伸缩）
（看看有没有角度，是机器做不了的，也看看两点之间的角度斜率，有没有很大，很大有可能手臂的硬件加速度也很大）
Trajectory planning：相当于拟合一条线，得到连续时间下各个关节的角度。
F.K：正算，检查一下和最开始规划的图，是不是一样的。
另一种规划方式
先将位置连起来。（应该是按照曲线，加密一下via point，多I.K几组姿态）
使用两个via points来拟合3次曲线
θ(t) = a1 * t + a2 * t2 + a3 * t3 + a0
因为只有2个点，但是要有4个未知数，因此加入2个速度，才刚好够4个方程
将θ(t) = a1 * t + a2 * t2 + a3 * t3 + a0 变为：
θ(Δt) = a1 * Δt + a2 * Δt2 + a3 * Δt3 + a
使t归一化，只看[ti, ti+1] 那一段，有：
如何设置速度条件 θi'
8 个未知数
当t~ = 0 时，可以得到 a10 = θ0
注意到，两段使用不同的两个模型的，因此，单独看[t1,tf]段，
目标：获得位置，速度，加速度的，平滑的表达式，用于仿真。
（蓝色轨迹，改为绿色轨迹）
假设：
开始速度：θ'0 = 0
最終速度：θ'final = 0
那么，要做的事情，就是找出：
θb , θf-b , Δt = tb - t0
θb , θf-b 就是中间恒定速度的起始、结束的位置。
绿色线中间，速度是恒定的，加速度为0
[t0, tb]段，轨迹为二次曲线。
[tb, tf - tb]段，轨迹为直线。
[ tf - tb，tf ]段，轨迹也为二次曲线
问：tb如何确定？ （总不能按照直觉，随便点一个θb，tb出來）
思考：
1. θh, th，直接是已知的，直接一半。
2. 为保证平滑， θb 必须大于θ0 ，θf-b 必须小于θ f 。
3. 是不是应该根据硬件的参数，比如说最大扭力的50%之类的来确定？
4. 最大扭力决定的是加速度，是不是应该将时间t ， 位置，加速度的关系列出来，再根据设定的速度或者加速度，得到tb的值？？
由2式可得：
tb 处的速度 = 初速 （0） + 加速度 * tb
根据： h处的速度 = b处的速度
θh = (θf + θ0) / 2
θb = θ0 + （θ0)' * tb + 1 / 2 *（θ0)'' * ( tb)2 ( θ0处的速度为0（θ0)’ = 0)
th = tf / 2
最终得到：tb，是加速度θ''的函数
结论：
1. tb，是加速度θ''的函数
2. 加速度θ''， 有最小值
（如果机器无法达到最小加速度，那么初速度（θ0)' 可能不能为0，才能使得曲线平滑）
加速度θ'' 对轨迹的几何影响
如果使用最小的
代入，得到：
这表明：如果机器出最小的力气，那么中间的轨迹，就是2条开口相反的二次曲线。
加速θ'' 达不到θ''min，造成轨迹的影响：
结论：加速θ''不够时，不能在某个时间点，到达某个位置，除非改变初速度
补充：
如果出力最小，那么中间速度，就是匀速2倍了。
也就是，可以选择大于θ''min 的加速度，控制速度。
首先可以得到直线部分的速度：
已知速度与加速度有如下关系：
(θkl)' = (θjk)' + tk * (θk)''
速度对时间微分，就是加速度。
也就是：
可以：
直接根据硬件的承受能力，得到(θk)'' , 从而得到tk, 注意正负号
或者根据tk，得到(θk)'' ， 再判断有无超出硬件承受能力。
目标：
要得到一段段方程，形如：
θ = f1(t) t∈ [tj, tj + Δt1 ]
θ = f2(t) t∈ [ tj + Δt1 , tk - Δt1]
θ = f3(t) t∈ [ tk - Δt1 , tk + Δt1]
θ = f4(t) t∈ [ tk + Δt1 , tl - Δt2]
.....
起始位置
对于整个过程，在t0 和 tf 要特别处理，因为那里的速度为0
在实际的操作中，位置相同：θ0 = θ1 ，但速度不相同， 因为机器需要一个启动的过程，而t = 0时的速度为0， t = tθ1时的速度就不是为0了
1. 直线段的速度 = θ1 处的加速度（θ‘’1 ） * t1时间段 ， 因为t = 0时的初始速度为0
（相当于在t1时间段，加速到平均速度，然后保持匀速）
2. 直线段的速度 = t12 时间内的平均速度
根据机器能提供的加速度，去反推时间t1
或者规划时间， 去求加速度，但是要考虑机子的承受能力：
最终算出的（θ‘’1 ）≤ 硬件最大能提供的加速度
结束位置
位置相同：θn = θn+1 ，但速度不相同t = tn 时，还有一个微小的速度，直到 t = t n +1 时完全静止
那么，同理可得：
值得注意的是，这只是机械要提供运动所分配的加速度，具体但离电机提供的加速度还有一段距离。
电机提供的力 = 加速度需要的力 + 克服重力提供的力 （和姿态有关）+ 克服惯性有关的力（和运动速度有关）
所以，加速度θ‘’ 的最大值， 最起码要从机器能承受的最大值，扣掉克服重力的值才可以。
遗留的问题：
如果via point是必须要通过，没得商量的，那就在via point前后插入一些额外的 via Point，做一段直线段
插入后，中间的必须要通过的via point，是不是可以省掉了？？不纳入编程中？
编程部分
前面已经计算好：
1. 加速区；（tj）
2. 减速区；(tk)
3. 加速度；（θ‘’k ）
接下来就是定义好位置方程，就可以在matlab上画线了。
直线段：
初始姿态对轨迹的影响
可以看出，如果A点是初始位置，那么手臂的姿态有两个解。
如果以第1张图的解，那么第1个关节，在3、4之间的旋转角度，就发生剧烈的变化，不符合平滑的要求
例子：
已知手臂DH表。
设定via Point
矩阵最后一列，就是via Point的位置。
矩阵的旋转部分，就是C坐标系的轴，在以World 坐标系为参考下的向量。
0到1处，只是让杯子提高一点
1到2处，就是让到差不多的位置，并且旋转杯子令杯耳对准。
注意到：2处，C坐标系可以想象先与word对齐，然后绕Y轴旋转，使得杯子横放。
W0T 、 6CT是固定的， C与6是固连的（定义上C与6必须是固连）
60T = W0T-1 WCT 6CT-1
4、5、6 的原點，在0系下的位置：
0P4org = 01T 12T 23T3P4org
而3P4org ， 又是 34T 的第4列。
令：
[f1(θ3), f2(θ3) ,f3(θ3), 1 ]T = 23T 3P4org 得到第1层
0P4org = 01T 12T * [f1(θ3), f2(θ3) ,f3(θ3), 1 ]T
[g1(θ2,θ3), g2(θ2,θ3) ,g3(θ2,θ3), 1 ]T = 12T * [f1(θ3), f2(θ3) ,f3(θ3), 1 ]T 得到第2层
然後，發現：
0P4org = [ x , y , z , 1] 中，与g有如下关系：
r = x2 + y2 + z2 = g12 + g22 + g32
并且：z = g3
令：
k1 = f1
k2 = -f2
k3 = f12 + f22 + f32 + a12 + d22 + 2d2f3
k4 = f3cos(α1) + d2cos(α1)
然后，代入 r = x2 + y2 + z2 = g12 + g22 + g32
得到：
r = (k1c2 + k2s2) 2a1 + k3
z = g3 = (k1s2 - k2c2) sin(α1) + k4
里面k都只是θ3的函数，将：
u = tan(θ3 / 2)
cosθ3 = (1 - u2 ) / (1 + u2)
sinθ3 = 2u / (1 + u2)
代入, 解u，再解θ3
然后解θ1 θ2
然后解36R
最后，根据Z-Y-Z构成的欧拉角矩阵，解出θ4 θ5 θ6
因为初始时，几个坐标系原点重合在一起。
在此处建立一个统一的坐标系，根据欧拉角，先绕Z轴（Z4）旋转，再绕旋转后的Y轴（Z5）旋转，再绕（Z6）旋转时，Z4其实和Z6是重合的，也就是依然绕Z轴旋转，因此是Z-Y-Z顺序的欧拉角。
注意：
因为DH定义的关系，使用ZYZ求出的角度，要经过加工才能变成θ4，θ5，θ6
从正算的角度来看：
1. 绕Z4旋转θ4
2. 如果要使用欧拉角，那么相当于还要绕Z4 多转180
那么，用ZYZ求得的欧拉角α，与θ4 有此关系（同理θ6）：
而α，β，γ是有多解的，最終选θ变化最小的解
轨迹规划，其实就是对0P6org 进行的规划
因此，先得到60T
方法1：先规划XYZ，再绘制θ1θ2θ3θ4θ5θ6 图
首先，需要将其转换成坐标，与6系相对于0系的姿态，用来做轨迹规划。
根据坐标规划， 绘制图θ1θ2θ3θ4θ5θ6 ，
方法2：先规划θ1θ2θ3θ4θ5θ6 ， 再绘制XYZ图
