基于惯导/多普勒雷达组合的车载定位定向方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN105509738A(43)申请公布日2016.04.20(21)申请号201510891103.6(22)申请日2015.12.07(71)申请人西北工业大学地址710072陕西省西安市友谊西路127号(72)发明人付强文秦永元严恭敏梅春波翁浚崔潇李四海刘洋(74)专利代理机构西北工业大学专利中心61204代理人王鲜凯(51)Int.Cl.G01C21/16(2006.01)G01S13/86(2006.01)权利要求书4页说明书10页附图3页(54)发明名称基于惯导/多普勒雷达组合的车载定位定向方法(57)摘要本发明公开了一种基于惯导/多普勒雷达组合的车载定位定向方法，用于解决现有车载定位定向方法定位精度低的技术问题。技术方案是将捷联惯导(SINS)和多普勒雷达(DOP)自身误差、惯导和多普勒雷达在车辆上的安装偏角和安装杆臂作为状态变量，采用单位时间内车体坐标系下的位置增量误差作为量测方式，对非完整性约束和多普勒雷达测速分别进行序贯滤波和故障检测隔离，增强了组合导航系统的环境适应性，保证了系统长时间和远距离工作的能力，提高了导航定位精度。经测试，在工作时间和行驶距离大于背景技术方法6倍的情况下，定位精度仍然显著提高，最大水平定位误差由120m减小至30m。CN105509738ACN105509738A权利要求书1/4页1.一种基于惯导/多普勒雷达组合的车载定位定向方法，其特征在于包括以下步骤：步骤一、建立坐标系；定义IMU载体坐标系为b，原点位于IMU内部敏感中心OI，坐标轴指向按右-前-上方向确定；定义惯导解算的导航坐标系为n，原点位于IMU内部敏感中心OI点所处地理位置，坐标轴指向按当地地理位置的东-北-天方向定义；定义多普勒雷达测量坐标系为d，原点位于多普勒雷达内部敏感中心OD，坐标轴指向按右-前-上方向确定；定义车体坐标系为m，原点与多普勒雷达敏感中心OD重合，ym轴沿车体纵轴指向正前方，xm轴沿车体横轴指向右侧，zm轴垂直于xm、ym轴并构成右手直角坐标系；定义IMU载体坐标系b与车体坐标系m之间的俯仰、横滚、方位安装角为αx、αy、αz，且安装偏差角满足小角度要求；建立IMU载体坐标系b与车体坐标系m之间的坐标转换关系式中符号c表示cos，s表示sin；定义多普勒测量坐标系d与车体坐标系m之间的俯仰、横滚、方位安装角为βx、βy、βz，且安装偏差角满足小角度要求；建立多普勒测量坐标系d与车体坐标系m之间的坐标转换关系根据车辆在道路上正常行驶时存在的非完整性约束，将车体坐标系m下OD点的理论行驶速度表示为式中标量为理论纵向行驶速度；步骤二、建立惯导和多普勒雷达速度误差模型；根据IMU载体坐标系b、导航坐标系n和车体坐标系m之间的角度和位置关系，忽略高阶误差的影响，建立惯导解算速度在车体坐标系m下OD点的输出模型为式中为车体坐标系m下OD点的理论速度；为导航坐标系n至IMU载体坐标系b的转nT换矩阵；v＝[vEvNvU]为导航坐标系n下的东-北-天向速度；为惯导解算的东-北-天nT向速度误差；φ为惯导系统姿态解算的平台失准角；δα＝[αx0αz]为IMU载体坐标系b与车体坐标系m之间的安装偏差角；为相对运动角速率，由陀螺输出获得；Lb为IMU内部敏感中心OI点和多普勒雷达内部敏感中心OD点之间的3维安装杆臂；安装偏差角δα和安装杆臂2CN105509738A权利要求书2/4页Lb为随机常数，即根据多普勒雷达测速原理，建立单通道多普勒速度输出模型式中表示多普勒雷达输出纵向速度，为理论纵向速度，δkd为多普勒雷达测速标度因数误差，为零位偏差，为输出噪声；根据公式(2)～(3)建立关系式式中ei表示第i个元素为1其余元素为0的3维矢量；根据公式(6)～(7)建立关系式忽略高阶误差，建立多普勒雷达在车体坐标系m下的等效输出速度式中δKD、δv0、为车体坐标系m下等效的标度因数误差、零位误差和量测噪声；参照公式(3)和(9)，根据车辆非完整性约束和多普勒测速模型构造OD点的测量速度矢量为式中为非完整性约束噪声；多普勒雷达测速误差δKD、δv0为随机常数，即步骤三、建立组合导航状态方程；选择捷联惯导解算相关的误差状态包含速度误差平台失准角φn、位置误差δP、陀螺零偏εb、加速度计零位共15维；同时考虑多普勒雷达测速误差模型和安装关系，将多b普勒雷达速度标度因数误差δKD、速度零位误差δv0、安装偏差角αx和αz、安装杆臂误差L也扩充为状态变量；建立22维的惯导/多普勒雷达组合导航模型的状态变量：在此基础上建立卡尔曼滤波器状态方程：3CN105509738A权利要求书3/4页式中为导航坐标系n下等效的陀螺和加速度计零位噪声；FI为根据经典捷联惯导误差