做惯导解算和组合导航的人应该都听过PSINS这套MATLAB工具箱。它把捷联惯导算法封装得很完整从初始对准、纯惯性解算到组合滤波基本拿到就能跑通。不过很多新手拿到工具箱后的第一件事不是跑demo而是被各种初始化函数搞懵avpset到底在干嘛参数是什么顺序为什么姿态角里写个45就代表航向这篇文章就专门把avpset这个函数拆开讲透结合我实际跑过的静基座对准、轨迹仿真和滤波初始化三个场景把输入输出、量纲约定、常见坑一次说清楚适合刚开始接触惯导、准备用工具箱做仿真的同学。1. 先把avp这个状态量搞明白1.1 avp三个字母分别代表什么avp是attitude、velocity、position的缩写对应惯导系统里最核心的三组状态姿态、速度、位置。搞惯导的人都清楚捷联惯导解算本质上就是在递推这三个量。姿态告诉载体转了多少度速度告诉载体往哪边飞了多快位置告诉载体现在在哪个经纬高上。三者合在一起就是一条完整的导航状态。很多教材里会把姿态写成一个3x3的方向余弦矩阵把速度写成一个三维向量把位置写成纬度、经度、高度三个标量。这样一来状态量就分散开了。工具箱在实际工程里为了方便存储和传递经常把这三组数拼成一个1x9的行向量也就是avp的约定格式avp [att(1:3), vn(1:3), pos(1:3)]其中att是姿态角、vn是导航系下的速度、pos是位置。avpset这个函数的作用说白了就是帮你把这9个数按正确顺序拼成工具箱能直接吃的格式。看起来很简单但恰恰是这种简单工具最容易因为顺序和单位出错。1.2 姿态、速度、位置各自的坐标约定参数顺序之外坐标系的约定才是真正的坑。PSINS这套工具箱默认用的是东北天导航坐标系也就是我们常说的ENU坐标系。速度vn的顺序必须是东向速度、北向速度、天向速度单位是米每秒。位置pos的顺序是纬度、经度、高度纬度和经度单位是度高度单位是米。然后是姿态角att。三个分量依次是俯仰角、横滚角、航向角单位是度。航向角以地理北为0度基准顺时针为正。这一点跟很多航空惯例不一样有的系统航向以正东为0度有的用弧度有的还是逆时针为正一旦搞错姿态初值就是错的后面整个仿真都会歪掉。我习惯拿到avpset之前先确认一下坐标定义再决定要不要对原始数据做转换。还有一点容易忽略姿态角的取值范围和旋转顺序。通常俯仰角范围是正负90度横滚角范围是正负180度航向角范围是0到360度或者正负180度。工具箱内部处理时会有约定但我们在avpset这一层只需要保证角度值符合常识比如维度在合理范围、姿态角没有出现几千的数值基本就说明单位没搞错。1.3 为什么工具箱喜欢把状态拼成一个超长向量单看avp只是9个数为什么要专门用一个函数去拼因为后续很多环节都要用这个格式。做初始对准要给对准算法一个attitude初值做纯惯性递推要给它速度和位置初值做组合导航卡尔曼滤波状态转移矩阵的初值也要从avp里面拆。既然处处都要用工具箱干脆统一约定成这种扁平的行向量调用起来省事。另外这种拼接格式也方便后面做批量处理。一次仿真可能有很多组初始状态比如蒙特卡洛打靶跑50条轨迹每条轨迹的起点或初始姿态不同。把50组avp按行堆叠成50x9的矩阵工具箱内部的循环处理会非常顺手。avpset表面上看只是个拼串工具实际上承担了数据规范化的作用它保证你传进去的姿态是度、速度是米每秒、位置是纬度经度高度不会把弧度、英尺、经纬度顺序混进去。2. avpset函数到底怎么用2.1 输入输出与调用规则avpset最常见的调用方式就是三组输入拼一组输出avp0 avpset(att0, vn0, pos0);att0是3x1的姿态列向量vn0是3x1的速度列向量pos0是3x1的位置列向量。返回值avp0是1x9的行向量。这里要特别注意输入的行列方向。我自己一开始就吃过亏att0用了行向量vn0用了列向量结果拼出来的avp顺序混乱仿真出来的位置完全不合理。看一下最简单的用法% 载体初始姿态俯仰0度、横滚0度、航向45度 att0 [0; 0; 45]; % 初始速度东向0、北向0、天向0静止状态 vn0 [0; 0; 0]; % 初始位置纬度30度、经度104度、高度500米 pos0 [30; 104; 500]; avp0 avpset(att0, vn0, pos0); disp(avp0);运行出来应该是0 0 45 0 0 0 30 104 500前三个是姿态中间三个是速度最后三个是位置。这样看就很直观了。如果你要模拟一个正在匀速东飞的平台那就把vn0改成[100; 0; 0]其他不变。需要注意的是avpset在部分文档里可能还支持其他调用形式比如传入结构体参数。我的建议是不要盲目依赖那些没在demo里出现的用法工具箱更新迭代比较频繁不同版本的签名可能有细微差别。最稳妥的办法是直接打开工具箱里的例程文件照着里面avpset的使用方式来写能跑通例程说明你对函数行为的理解大概率没错。2.2 与工具箱其他函数的衔接avpset的输出往往不会单独使用它只是初值准备的一个环节。后面紧跟着的就是初始对准或者解算初始化。以最常见的静基座对准场景为例avp0 avpset(att0, vn0, pos0); ins insinit(avp0);这边insinit拿到了avp0之后会根据姿态初值构建初始的方向余弦矩阵根据位置初值计算重力、地球自转角速度等参数。也就是说avpset这边的9个数写错了后面insinit里生成的所有东西都会跟着错。在轨迹仿真场景里avpset的结果还会被传给轨迹发生器。轨迹发生器需要一个初始状态作为起点再从起点出发按照给定的加减速、转弯角速度生成IMU数据。这时avp0就是整个轨迹的“起跑线”。起点姿态差一度整条轨迹的朝向就偏一度起点位置差一个量级后面的经度纬度计算就全部偏移。所以我的个人习惯是写完avpset这一行立刻加一句disp(avp0)看输出先人工判断一下姿态量级、速度量级、位置量级对不对再往下一步传。等到解算结果出了问题再去查初始值成本就高多了。2.3 自己写一个最简单的替代实现理解一个函数最好的方式就是尝试自己实现一遍。如果只看拼接逻辑avpset的核心代码其实很短function avp my_avpset(att, vn, pos) % 把姿态、速度、位置统一reshape成1x3行向量 att reshape(att, 1, 3); vn reshape(vn, 1, 3); pos reshape(pos, 1, 3); avp [att, vn, pos]; end这段代码能跑而且大部分情况下结果跟工具箱函数一致。但它没有做异常检查比如att长度不是3、vn不是3、pos不是3时会直接报错或静默出错。实际工具箱里的avpset大概率还做了维度校验、单位约定说明、多组数据格式兼容等额外处理只是这些处理对普通使用者是透明的。自己写一遍有什么好处一是能把9个数到底按什么顺序排这个信息彻底记住下次看到一行avp数据扫一眼就能判断哪个是航向、哪个是纬度。二是在调试的时候如果怀疑avpset本身有问题可以直接用自己写的简化版替换排查到底是不是工具箱函数的行为跟预期不一致。我实际调试中就遇到过因为版本差异导致对不上的情况当时就是用简化版一点点对比定位的。3. 三种典型实操场景3.1 场景一静基座初始对准初值生成静基座对准是惯导新手第一个跑的仿真也是最容易踩坑的地方。所谓静基座就是载体放在地面上不动用IMU测量的重力和地球自转角速度来确定姿态初值。这种场景下速度初值就是零位置是某个固定的经纬高姿态初值一般是人为给定的粗略值。我通常这样设置% 姿态初值俯仰0度、横滚0度、航向90度 att0 [0; 0; 90]; % 静止速度为0 vn0 [0; 0; 0]; % 纬度北纬30度东经104度高度300米 pos0 [30; 104; 300]; avp0 avpset(att0, vn0, pos0); % 构造初始惯导解算结构体 ins insinit(avp0);这里航向设成90度意味着载体头部指向正东。如果后续仿真里IMU数据也是按这个航向生成的那么解算出来的航向应该在90度附近小范围波动不会出现突然跳到负值或者其他诡异角度的现象。建完初值后我会先跑一小段零速数据看结果姿态应该保持稳定速度和位置漂移缓慢。如果位置几秒钟就飞出去几公里多半不是算法问题而是初值里姿态或位置写错了。静基座场景下有一个很容易被忽视的点位置初值里的高度对重力计算有影响。高度差个几百米当然不会让仿真崩溃但如果做高精度比对初始高度的准确度会直接影响重力模型的值进而影响姿态解算的收敛精度。所以能用真实高度就不要随手填0。3.2 场景二轨迹发生器起点设置做带运动轨迹的仿真时avpset的价值更明显。很多初学者以为轨迹仿真只需要给定速度和转弯半径就行其实第一步永远是设轨迹起点。这个起点就是一个完整的avp状态包含初始姿态、初始速度和初始位置。我做过一个模拟车载导航的Demo起点在某个测点车辆先沿着直线加速再匀速再转弯。整个轨迹的初始状态我是这样写的% 初始航向朝北 att0 [0; 0; 0]; % 初始车速为0 vn0 [0; 0; 0]; % 初始位置纬度、经度、高度 pos0 [30.5; 104.2; 520]; avp0 avpset(att0, vn0, pos0); % 传给轨迹发生器作为起点 trj trjsimu(avp0, ...);这里有一个值得多说的细节航向角为0是正北方向不是正东。很多从别的软件转过来的朋友习惯性把0当成正东结果整条轨迹的朝向整体转了90度。我自己也犯过这个错当时生成的轨迹和实测路线始终对不上最后对比放大才发现航向基准差了一个象限。轨迹仿真还有一个好处是可以直观验证avp单位的正确性。如果位置纬度和经度填反了生成的轨迹落点会跑到奇怪的地方去如果高度填的是千米而不是米轨迹整体会飘在平流层。跑一次轨迹仿真把经纬度画出来基本一眼就能看出初值设置有沒有問題。3.3 场景三组合导航状态初始化组合导航比纯惯导解算多了一步融合滤波但初值准备同样是必需的。无论是惯导加卫星、惯导加里程计还是其他组合模式滤波器的状态初值、协方差初值、系统噪声初值都要在解算开始前准备好。我常用的做法是先把滤波需要的初始状态用avpset搭出来再拆开去填滤波器avp0 avpset(att0, vn0, pos0); % 从avp中拆出初始状态给滤波器使用 x0 [zerror(3); avp0(4:6)]; % 示意具体写法以工具箱定义为准这里要特别提醒不要直接把avp0当作滤波器状态。组合导航滤波状态里通常包含姿态误差、速度误差、位置误差以及陀螺仪和加速度计的零偏等跟完整导航状态量还不完全是一回事。avpset提供的是惯导解算用的“真实状态初值”滤波器里的误差状态初值往往是在这个基础上人为加一组小扰动得来的。比如做对准精度分析会给初始姿态加一个小角度误差比如横滚误差0.1度、航向误差0.5度然后让滤波算法去估计并修正这个误差。avpset这边放的是基准真值扰动则在滤波初值里体现。很多初学者把两者混在一起结果滤波器估计出来的误差一直在原地打转就是因为初值本身和滤波状态定义对不上。4. 常见问题与排查技巧实录4.1 量纲和单位翻车现场avpset用起来虽然简单但单位问题一旦出错仿真结果会非常离谱。下面这张表是我在实际项目里遇到过的典型症状按出现频率排了个序方便大家对号入座。症状可能原因排查思路姿态发散或者震荡剧烈姿态角用了弧度而不是度打印att0看数值是否在-180到360的合理范围内位置纬度纬度异常不在正负90区间把经度填到了纬度位置直接看pos0三个值第一个值必须是纬度高度漂移得离谱高度单位用了千米确认pos0第三项的量级普通场景应该在几百米左右速度大小明显不对速度单位不是米每秒对比实际运动场景车载速度通常在0到50之间avp0出现NaN输入数据里带了NaN或者非法角度检查原始输入是否有脏数据单位错误在MATLAB里通常不会报错因为数字还是数字姿态90度和姿态1.57在语法上都可以接受但物理含义完全变了。所以我自己写脚本时都会在avpset调用前加一段注释标清楚每个变量的单位。这不是可有可无的规范而是防呆。我还见过一种很隐蔽的情况数据文件里存的是经纬度但用了分和秒的格式比如30度30分直接写成了30.30导致位置偏出几十公里。这种问题靠肉眼盯很难发现最好的办法是把位置信息的量级跟我已知的测试点对比。如果测试点明明在四川解算位置却跑到了海里那大概率就是位置初值或者单位换算出了问题。4.2 维度与顺序错误MATLAB对向量维度比较宽容行向量和列向量在很多运算里可以混用但这恰恰是隐患。avpset内部可能做了reshape但如果工具箱版本做了严格的维度检查行列不一致会直接报错。更多时候是没报错但后面的矩阵运算把姿态和速度的位置搞混了。我踩过的一个典型坑是从外部文件读取初始经纬度时数据是1x3行向量而姿态是3x1列向量直接拼到avp0之后后面的insinit虽然能跑但解算出来的位置始终不对。排查了很久才发现是行、列不一致导致avp的顺序悄悄变了。建议统一的写法是全部用列向量att0 [0; 0; 45]; vn0 [0; 0; 0]; pos0 [30; 104; 500];不要图省事写[0, 0, 45]这种行向量。虽然avpset可能也能处理但统一成列向量可以让整个工具箱链路上的数据处理风格一致减少出问题的概率。我后续所有脚本都遵循这个约定基本没再遇到过维度相关的问题。顺序方面还有一个容易被带偏的点卫星定位输出通常是经度、纬度、高度也就是先经度后纬度。而PSINS工具箱的位置状态是纬度、经度、高度。如果直接把卫星数据填给avpset经度会被当成纬度纬度会被当成经度位置直接错乱。正确做法是做一次交换pos0 [lon; lat; h]; % 手滑写反是常事 pos0 [lat; lon; h]; % 这才是工具箱需要的顺序这段看起来像废话但我在实际代码评审里见过至少三次类似的错误。位置顺序错乱在短时间仿真里不一定立刻暴露因为纬度经度都是几十的数量级初看起来都像是“合理的经纬度”但细看纬度超过90或者经度超过180就能发现猫腻。4.3 快速定位问题的小习惯最后分享几个我平时调试avpset相关问题时保持的习惯不复杂但很管用。第一avpset之后立刻打印结果。每次构造完avp0我都会手动看一眼九个数。姿态是几十左右速度是零或者几十纬度是零到九十经度是零到一百八高度是几百。任何一项偏离这个常识区间都说明初值有问题。这个习惯帮我拦下了很多本会在后续仿真里爆炸的小错误。第二单独把小函数验证一遍。如果从外部文件读初始值我会先把读取到的原始数据和avpset的输入打印出来两个文件一对比就能看出是不是在读取阶段就发生了行列转置或者单位换算错误。很多时候问题根本不在avpset而在上游数据处理。第三用最小复现案例测试。怀疑avpset的某个调用方式不对时我会写一个最简脚本只设置姿态、速度、位置拼接完打出来然后用工具箱自带的例程对比。如果简例结果正常而完整脚本结果异常说明问题出在复杂环节如果简例都异常那就要检查工具箱版本或者函数签名了。第四保留一份“黄金初值”。我会把一组已经确认正确的avp0存成mat文件每次做新项目就直接加载再在它的基础上修改。这样可以避免同样的初值错误反复出现。尤其是做蒙特卡洛仿真时一组稳定的初值能省掉大量调参时间。个人感受是avpset这个函数本身小到不能再小但它处于所有惯导仿真的最上游。函数用错了后面无论算法多么精确输出结果都不会正确。我见过不少同学花了大量时间调滤波器参数却始终没发现问题出在第二行的avpset上这种经历真的很不值。做惯导仿真初值不是“随便设一个就行”的事情而是整个仿真可信度的地基。