MinimumSnap 代码细节

Hugin&Munin

已于 2023-03-22 16:52:19 修改

阅读量359

点赞数 1

分类专栏：无人机路径规划文章标签： matlab 矩阵线性代数

于 2023-03-22 16:51:33 首次发布

本文链接：https://blog.csdn.net/qq_29703971/article/details/129712175

版权

无人机路径规划专栏收录该内容

3 篇文章 1 订阅

订阅专栏

该文详细介绍了使用MATLAB进行移动机器人运动规划时，如何构建Q矩阵、M矩阵和Ct矩阵的过程。Q矩阵涉及多项式排序和积分计算，M矩阵用于状态变量转换，而Ct矩阵则是一个复杂的关系矩阵，涉及轨迹点的状态连接。文章还提到了数值解法在解决此类问题中的应用。

摘要由CSDN通过智能技术生成

(MATLAB)

基于高飞老师的移动机器人运动规划课程hw5作业写的代码。因为过程中在对Q,M,Ct函数的构造上花了很多功夫去理解，因此记下来。

注意：多项式的排列方式很重要，我们这里采用的是升幂排序，也就是

1. Q矩阵

下面的矩阵是不完全的，因为它只是Q矩阵中的一小部分，记为 $Q_k$ ，实际上是

然后还要积分，Q矩阵中对应的有值项还要乘上 $\frac{t}{i+j-7}$

最后写为：

代码写成这样，

function Q = getQ(n_seg, n_order,t_order,ts)
    Q = [];
    for k = 1:n_seg
        % 7阶，所以有8个自由度
        Q_k = zeros(n_order+1,n_order+1);
        %#####################################################
        % STEP 1.1: calculate Q_k of the k-th segment 
        for i=0:n_order
            for j=0:n_order
                if((i<t_order)||(j<t_order))
                    continue;
                else
                    Q_k(i+1,j+1) = factorial(i)/ factorial(i-t_order) * factorial(j) / factorial(j-t_order) /(i+j-n_order) * ts(k) ^ (i+j-n_order);
                end
            end
        end
        Q = blkdiag(Q, Q_k);
    end
end

最终结果（单段轨迹）是：