PSINS源码阅读—test_SINS_trj

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

test_SINS_trj是航迹仿真程序，具体实现可以参考严恭敏老师的硕士论文《捷联惯导算法及车载组合导航系统研究》中第5章节中的第一小节。

关于航迹仿真

航迹仿真是根据设定的航迹生成惯性器件信息源（比力和角速度），并给出相应航迹点的航行参数（姿态、速度和位置）。比力和角速度用作捷联算法的数据输入，经过捷联算法解算后其结果可以和前述航行参数作比较。航迹仿真的方法是根据设定的航行运动状态解算一组航迹微分方程组，通常利用计算计求解该微分方程组的数值解，数值解法中常用到的是四阶龙格-库塔法。

代码解读

trjsegment函数中可以根据不同的动作以及动作参数生成相应的运动参数。
先说下结构体seg.wat 中8个参数的含义：

% wat(:,1) - period lasting time                 持续时间
% wat(:,2) - period initial velocity magnitude   速度大小
% wat(:,3:5) - angular rate in trajectory-frame  角速度 ,单位：rad/s
% wat(:,6:8) - acceleration in trajectory-frame  加速度

1. init：可以设定初始速度initvelocity；
2. uniform：可以设定按照上一时刻的速度，匀速直行，当速度为0时，就是设定可以静止多长时间；
3. accelerate：直线加速过程，可以设置加速度的大小和时间；
4. deaccelerate：直线减速过程，可以设置加速度的大小和时间；
5. headup：俯仰向上，此时速度不变，俯仰角变化到指定的值，可以看到seg.wat中第3个参数为俯仰角变化的角速度，改变过程中角速度是定值；
6. headdown：低垂向下，此时速度不变，俯仰角变化到指定的值，可以看到seg.wat中第3个参数为俯仰角变化的角速度，改变过程中角速度是定值；
7. turnleft：左转弯，只改变偏航角，偏航角改变过程中角速度是定值；
8. turnright：右转弯，只改变偏航角，偏航角改变过程中角速度是定值；
9. rollleft：这个动作就叫滚转左转弯吧，只改变滚转角，改变过程中角速度是定值；
10. rollright：这个动作就叫滚转右转弯吧，只改变滚转角，改变过程中角速度是定值；
11. coturnleft：论文中把这个动作叫做协调转弯，通俗点理解即是一个有转弯半径的转弯，该动作由3个动作组组成，具体叙述如下所示：
    

cf = (w*dps)*seg.vel; % centripetal force :向心力 
rollw = atan(cf/9.8)/dps/rolllasting; % 滚转角(角速度)

从滚转角角速度可以看出，其由线速度和偏航角速度决定，trjsegment函数代码注释如下所示：

function seg = trjsegment(seg, segtype, lasting, w, a, var1)
% Add trjsegment setting for trajectory simulator.
%
% Prototype: seg = trjsegment(seg, segtype, lasting, w, a, var1)
% Inputs: seg - trjsegment structure array
%         segtype - trjsegment type
%         lasting - segment lasting time 持续时间
%         w - trajectory angular rate (NOTE: in deg/sec!) 角速度
%         a - trajectory acceleration  加速度
%         var1 - augmented input, see code in detail
% Output: seg - new trjsegment structure array
%          
% See also  trjsimu, insupdate.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 07/01/2014

%==============================================================%
% wat(:,1) - period lasting time                 持续时间
% wat(:,2) - period initial velocity magnitude   速度大小
% wat(:,3:5) - angular rate in trajectory-frame  角速度 ,单位：rad/s
% wat(:,6:8) - acceleration in trajectory-frame  加速度
%==============================================================%
        
    dps = pi/180/1;  % deg/second 转换为rad
    if exist('w', 'var')
        cf = (w*dps)*seg.vel; % centripetal force :向心力 
        % 向心力公式如下：
        % https://baike.baidu.com/item/%E5%90%91%E5%BF%83%E5%8A%9B/975374
    end
    switch(segtype)
        case 'init' % trjsegment(***, 'init', initvelocity)
            initvelocity = lasting; % 0
            seg = [];
            seg.vel = initvelocity;  seg.wat = [];
        case 'uniform'
            seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0, 0, 0]];
        case 'accelerate'
            seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0, a, 0]];
            seg.vel = seg.vel + lasting*a; % 加速过程
        case 'deaccelerate' % 减速
            seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0,-a, 0]];
            seg.vel = seg.vel - lasting*a;
        % 俯仰向上    
        case 'headup'
            seg.wat = [seg.wat; [lasting, seg.vel, w*dps, 0, 0, 0, 0, cf]];
        % 低垂向下
        case 'headdown'
            seg.wat = [seg.wat; [lasting, seg.vel,-w*dps, 0, 0, 0, 0,-cf]];
        % w*dps:rad
        % 偏航角速率保持定值
        case 'turnleft'
            seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, w*dps,-cf, 0, 0]];
        case 'turnright'
            seg.wat = [seg.wat; [lasting, seg.vel, 0, 0,-w*dps, cf, 0, 0]];
        % 向左滚转
        % 滚转角速度由0变为某个值(y轴)
        % lasting: 转弯角速度
        % w:横滚角改变量,单位：deg/s
        case 'rollleft'
            seg.wat = [seg.wat; [lasting, seg.vel, 0,-w*dps, 0, 0, 0, 0]];
        % 向右滚转
        case 'rollright'
            seg.wat = [seg.wat; [lasting, seg.vel, 0, w*dps, 0, 0, 0, 0]];
        % compound ------
        case 'coturnleft'% coordinate turn left
            % cf = (w*dps)*seg.vel; 
            % centripetal force :向心力 
            % dps = pi/180/1;  % deg/second 转换为rad
            % rolllasting：转弯角速度
            % rollw:横滚角角速度,单位：deg/s
            % 改变滚转角进入转弯阶段持续时间:roll lasting
            % 参考：硕士论文70页
            % w：偏航角角速度
            rolllasting = var1; rollw = atan(cf/9.8)/dps/rolllasting; % 滚转角(角速度)
            seg = trjsegment(seg, 'rollleft',  rolllasting, rollw);
            seg = trjsegment(seg, 'turnleft',  lasting, w);
            seg = trjsegment(seg, 'rollright', rolllasting, rollw);
        case 'coturnright' % coordinate turn right
            rolllasting = var1; rollw = atan(cf/9.8)/dps/rolllasting;
            seg = trjsegment(seg, 'rollright', rolllasting, rollw);
            seg = trjsegment(seg, 'turnright', lasting, w);
            seg = trjsegment(seg, 'rollleft',  rolllasting, rollw);
        case '8turn'
            % w：deg/s
            % lasting：持续时间
            lasting = 360/w;
            rolllasting = var1;
            seg = trjsegment(seg, 'coturnleft',  lasting, w, 0, rolllasting);
            seg = trjsegment(seg, 'coturnright', lasting, w, 0, rolllasting);
        case 'sturn'
            lasting1 = 90/w; lasting2 = 180/w;
            rolllasting = var1;
            seg = trjsegment(seg, 'coturnright', lasting1, w, 0, rolllasting);
            seg = trjsegment(seg, 'coturnleft',  lasting2, w, 0, rolllasting);
            seg = trjsegment(seg, 'coturnright', lasting1, w, 0, rolllasting);
            seg = trjsegment(seg, 'coturnleft',  lasting1, w, 0, rolllasting);
            seg = trjsegment(seg, 'coturnright', lasting2, w, 0, rolllasting);
            seg = trjsegment(seg, 'coturnleft',  lasting1, w, 0, rolllasting);
        case 'climb'
            uniformlasting = var1;
            seg = trjsegment(seg, 'headup',   lasting, w);
            seg = trjsegment(seg, 'uniform',  uniformlasting);
            seg = trjsegment(seg, 'headdown', lasting, w);
        case 'descent'
            uniformlasting = var1;
            seg = trjsegment(seg, 'headdown', lasting, w);
            seg = trjsegment(seg, 'uniform',  uniformlasting);
            seg = trjsegment(seg, 'headup',   lasting, w);
        otherwise
            error('trjsegment type mismatch.');
    end

参考论文

1. 捷联惯导算法及车载组合导航系统研究 西北工业大学 严恭敏