Minimum Snap軌迹規劃詳解（3）閉式求解

如果QP問題隻有等式限制沒有不等式限制，那麼是可以閉式求解（close form）的。閉式求解效率要快很多，而且隻需要用到矩陣運算，不需要QPsolver。

這裡介紹Nicholas Roy文章中閉式求解的方法。

1. QP等式限制建構

閉式法中的Q

矩陣計算和之前一樣（參照​​文章一​​），但限制的形式與之前略為不同，在之前的方法中，等式限制隻要構造成[...]p=b的形式就可以了，而閉式法中，每段poly都構造成

 其中d0,dT為第i段poly的起點和終點的各階導數組成的向量，比如隻考慮PVA：

，當然也可以把jerk，snap等加入到向量。注意：這裡是不管每段端點的PVA是否已知，都寫進來。

塊合并各段軌迹的限制方程得到

k為軌迹段數,n為軌迹的階數，設隻考慮pva，Atotal的size為

。這裡為了簡化，沒有把每段poly的timestamp都改成從0開始，一般，為了避免timestamp太大引起數值問題，每段poly的timestamp都成0開始。

由上式可以看到，Atotal是已知的（怎麼構造可參見文章一中的等式限制構造方法），而d中隻有少部分（起點、終點的pva等）是已知的，其他大部分是未知的。如果能夠求出d，那麼軌迹參數可以通過p=A-1d很容易求得。

2. 如何求d？

閉式法的思路是：将向量中的變量分成兩部分：”d中所有已知量組成的Fix部分dF”和”所有未知量組成的Free部分dP”。然後通過推導，根據dF求得dP，進而得到d，最後求得p。

下面介紹整個推導過程，

2.1. 消除重複變量（連續性限制）

可以會發現，上面構造等式限制時，并沒有加入連續性限制，連續性限制并不是直接加到等式限制中。考慮到連續性（這裡假設PVA連續），d

向量中很多變量其實重複了，即

pi(ti)=pi+1(ti),  vi(ti)=vi+1(ti),  ai(ti)=ai+1(ti)

 是以需要一個映射矩陣将一個變量映射到兩個重複的變量上，怎麼映射？

• 如

[aa]=[11]a

，将變量a映射到左邊向量中的兩個變量。

是以構造映射矩陣

 即d=Md'd=Md′。

2.2 向量元素置換

消除掉重複變量之後，需要調整d'中的變量，把fix部分和free部分分開排列，可以左乘一個置換矩陣C，使得

C矩陣怎麼構造？

• 舉個例子，設

  

  ，其中a,b,c是已知(dF)，b未知(dp)，構造一個4×4的機關陣，取dF所在的(1,3,4)列放到左邊，再取dp所在的(2)列放到右邊，就構造出置換矩陣C：