基于卫星定位系统的车辆姿态计算方法
技术领域
本发明涉及位姿计算领域，尤其涉及一种基于卫星定位系统的车辆姿态计算方法。
背景技术
车辆姿态主要指车辆俯仰角、侧倾角、转向角速度。车辆姿态监控是整车中一个必不可少的组成部分。车辆姿态测量计算的准确性对驾驶安全、底盘操控、自动驾驶控制等各方面性能都有重要影响。一般情况下，车辆是通过三轴向加速度传感器、三轴向陀螺仪、磁传感器等传感器测量车辆在绕X、Y、Z三个轴向的线加速度和角加速度，再进行积分计算得出速度，然后进行多次计算得到车辆的俯仰角、侧倾角、转向角等信息，其过程较为复杂，存在累积的计算误差。三轴向加速度传感器对温度、振动等要求较高，因此对布置位置的约束较大。虽然近年来也通过引入卫星定位系统进行修正，但是仍需要借助加速度传感器、陀螺仪等进行修正。同时，随着自动驾驶对车辆功能安全要求的提高，车辆上需要有一套能提供车辆姿态信息的独立系统作为安全备份。
现有技术中一般通过三轴向加速度传感器获得给定方向上的加速度和矢量，通过三轴向陀螺仪获得各个方向上的旋转角速度，通过速度和加速度积分获得车辆俯仰角、侧倾角、航向角；三轴向传感器通常集成在车辆的电子稳定控制系统中。
但是三轴向陀螺仪对温度比较敏感，存在零点偏移的现象，即使进行校正也可能存在偏差，因此布置上需要尽量远离热源；三轴向加速度传感器对振动也比较敏感，因此要求布置位置尽量远离振动源，同时传感器与车身之间要做减震处理，会导致成本增加；此外三轴向传感器、陀螺仪等通常与电子稳定控制系统集成在一起后，为了满足电子稳定控制系统和制动管路的连接，只能布置在机舱内，存在碰撞后受损的风险。
此外现有技术中也有通过至少两个布置在车上不同位置的定位模块，构建一个位置矢量，车辆行驶过程中，该矢量会发生变化，将其与存储的初始位置矢量进行比对，即可获得车辆当前的姿态。但是该解决方案无法计算车辆转向角速度；因此为了保证计算准确性，需要至少3个定位模块，会产生3组位置向量，计算时需要先比对3组位置向量的变化，且需要通过2次加速度的坐标系间转换，然后进行差值处理，才能得到车辆姿态变化值，计算过程复杂，计算量大，对计算模块的运算能力、线束的数据传输能力要求较高。
发明内容
本发明主要目的在于提供一种新的车辆姿态计算方法，能够不受温度、湿度的影响，计算方法简单可靠的基于卫星定位系统的车辆姿态计算方法。
本发明所采用的技术方案是：
提供一种基于卫星定位系统的车辆姿态计算方法，包括以下步骤：
在车辆出厂时进行标定，获得三个卫星定位模块在整车姿态坐标系中的固定坐标，并与卫星定位系统中的坐标进行对比，建立整车姿态坐标系和地心坐标系之间的初始转换关系，作为车辆的初始姿态；其中，三个卫星定位模块布设在车辆不同位置上；
车辆使用过程中，获取三个卫星定位模块在不同时刻的卫星定位系统坐标，并与整车姿态坐标系中的固定坐标进行对比，得到整车姿态坐标系和地心坐标系的实时转换关系，其与初始转换关系的差值，即为车辆实时姿态相比初始姿态的变化。
接上述技术方案，在车辆上布设多个分析单元，并赋以不同的权重，每个分析单元包括至少三个卫星定位模块和一个计算分析模块。
接上述技术方案，若其中一个分析单元中的某个卫星定位模块损坏，则屏蔽该分析单元。
接上述技术方案，具体通过四元数法或者旋转矩阵法进行坐标系的转换。
接上述技术方案，通过旋转矩阵法进行坐标系的转换时，将三个卫星定位模块在整车姿态坐标系内的坐标和地心坐标系内的坐标通过旋转矩阵的方法建立两个坐标系之间的转换关系，获得初始状态和实时状态下的由整车姿态坐标系转换到地心坐标系的旋转角度，将绕X轴、Y轴两个方向的旋转角度相减，获得俯仰角和侧倾角，对绕Z轴的旋转角度进行时间求导，得到角速度。
本发明还提供了一种基于卫星定位系统的车辆姿态计算系统，包括至少一个分析单元，该分析单元包括：
至少三个卫星定位模块，布设在车辆不同位置上；
一个计算分析模块，与卫星定位模块连接，用于计算车辆姿态，具体用于：在车辆出厂时通过标定，获得三个卫星定位模块在整车姿态坐标系中的固定坐标，并与卫星定位系统中的坐标进行对比，建立整车姿态坐标系和地心坐标系之间的初始转换关系，作为车辆的初始姿态；
车辆使用过程中，获取三个卫星定位模块在不同时刻的卫星定位系统坐标，并与整车姿态坐标系中的固定坐标进行对比，得到整车姿态坐标系和地心坐标系的实时转换关系，其与初始转换关系的差值，即为车辆实时姿态相比初始姿态的变化。
接上述技术方案，在车辆上布设多个分析单元，并赋以不同的权重。
接上述技术方案，若其中一个分析单元中的某个卫星定位模块损坏，则屏蔽该分析单元。
接上述技术方案，计算分析模块具体通过四元数法或者旋转矩阵法进行坐标系的转换。
接上述技术方案，计算分析模块通过旋转矩阵法进行坐标系的转换时，具体将三个卫星定位模块在整车姿态坐标系内的坐标和地心坐标系内的坐标通过旋转矩阵的方法建立两个坐标系之间的转换关系，获得初始状态和实时状态下的由整车姿态坐标系转换到地心坐标系的旋转角度，将绕X轴、Y轴两个方向的旋转角度相减，获得俯仰角和侧倾角，对绕Z轴的旋转角度进行时间求导，得到角速度。
接上述技术方案，卫星定位模块布置在车辆的乘客舱内。
本发明产生的有益效果是：本发明通过在车辆上安装卫星定位系统，通过卫星定位模块获取坐标，再与整车姿态坐标系中的固定坐标进行对比，得到整车姿态坐标系和地心坐标系的实时转换关系，其与初始转换关系的差值，即为车辆实时姿态相比初始姿态的变化，从而计算出车辆姿态。
进一步地，布置在乘客舱内的卫星定位模块，不但更安全，而且受温度、振动的影响也很小，同时减少了和电子稳定控制系统集成在一起后所需要采取的减震措施带来的成本。
附图说明
下面将结合附图及实施例对本发明作进一步说明，附图中：
图1是本发明实施例整车布置的结构示意图；
图2是本发明实施例整车姿态坐标系与地心坐标系示意图；
图3是本发明实施例整车姿态坐标系与地心坐标系转换示意图；
图4是本发明实施例基于卫星定位系统的车辆姿态计算方法流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
本发明的基于卫星定位系统的车辆姿态计算系统，如图所示，该系统包括：布置在车辆不同位置上的至少三个卫星定位模块、一个计算分析模块。可以三个卫星定位模块作为一个分析单元，由计算分析模块进行运算得到车辆姿态角度。
进一步地，为了获得更准确的计算结果，可以增加卫星定位模块的数量以构建更多的分析单元，在标定阶段确定各分析单元的准确度，赋以不同的权重，得到最终的输出结果，同时在个别定位模块损坏后，屏蔽掉其相关单元，仍能提供计算结果。
卫星定位模块可布置在乘客舱内，不但更安全，而且受温度、振动的影响也很小，同时减少了和电子稳定控制系统集成在一起后所需要采取的减震措施带来的成本。
以整车前向为X轴正向，横向左侧为Y轴正向，竖直向上为Z轴构建整车姿态坐标系(如图2所示，为整车姿态坐标系与地心坐标系示意图)。以三个定位模块组成的一个分析单元为例，需将三个定位模块安装在车上不同的位置。车辆出厂时通过标定，获得三个定位模块在整车姿态坐标系中的固定坐标，并通过和卫星定位系统坐标的对比，建立整车姿态坐标系和地心坐标系之间的初始转换关系(图三整车姿态坐标系与地心坐标系转换示意图)，作为车辆的初始姿态。车辆使用过程中，通过对比三个定位模块在不同时刻的卫星定位系统坐标，与整车姿态坐标系中的固定坐标，得到整车姿态坐标系和地心坐标系的实时转换关系，其与初始转换关系的差值，即为车辆实时姿态相比初始姿态的变化。
坐标系的转换有多种方法，可以使四元数法，也可以是旋转矩阵法或者其他方法。
本发明实施例以旋转矩阵法为例，一个分析单元中三个卫星定位模块在整车姿态坐标系下的坐标为vehicleA(XV1,YV1,ZV1),vehicleB(XV2,YV2,ZV2),vehicle C(XV3,YV3,ZV3),在地心坐标系下的坐标为wordA(XW1,YW1,ZW1),wordB(XW2,YW2,ZW2),word C(XW3,YW3,ZW3)。根据旋转矩阵方法，整车姿态坐标系中的所有点通过绕整车姿态坐标系X轴、Y轴、Z轴分别旋转α、β、γ的角度，然后在整车姿态坐标系中平移向量TV-W(XV-W,YV-W,ZV-W)，即可得到地心坐标系下的坐标，见公式(1)：
其中，
在初始状态，可以将三个卫星定位模块分别安装在整车姿态坐标系vehicleA(XV10,YV10,ZV10),vehicleB(XV20,YV20,ZV20),vehicle C(XV30,YV30,ZV30)位置。车辆出厂前进行标定，获得三个卫星定位模块可以通过卫星定位系统，获得在地心坐标系下的安装位置坐标分别为wordA(XW10,YW10,ZW10),wordB(XW20,YW20,ZW20),word C(XW30,YW30,ZW30)。将这两组数据代入公式1，获得初始状态下的坐标系转换角度α0、β0、γ0。
车辆使用过程中，随着车辆姿态的不同，三个定位模块在卫星定位系统中的坐标系也会发生变化，假设在t′时刻，三个定位模块的坐标分别为wordA′(XW10,YW10,ZW10),wordB′(XW20,YW20,ZW20),word C′(XW30,YW30,ZW30)。而在整车姿态坐标系下，卫星定位模块的位置是相对固定的，都是vehicleA(XV10,YV10,ZV10),vehicleB(XV20,YV20,ZV20),vehicle C(XV30,YV30,ZV30)。将这两组数据代入公式1，获得t′时刻的坐标系转换角度α′、β′、γ′。根据公式(2)，获得车身姿态角度Δα、Δβ、Δγ。
其中，Δα为车辆绕整车姿态坐标系X轴旋转的角度，即俯仰角。Δβ为车辆绕整车姿态坐标系Y轴旋转的角度，即侧倾角。Δγ为车辆绕整车姿态坐标系Z轴旋转的角度，即车辆实时状态下相比最初标定时车辆姿态时，绕整车姿态坐标系Z轴的旋转角度，对其进行时间求导，即可得到车辆绕整车姿态坐标系Z轴旋转的角速度w,即公式3.
若设置超过3个定位模块，则可以组合多个计算分析模块。计算分析模块可以和其中一个定位模块或其他控制器集成在一起，也可单独作为一个零件。
如图4所示，本发明实施例基于卫星定位系统的车辆姿态计算方法，主要包括以下步骤：
在车辆出厂时标定初始位置(即原始位置)，获得三个卫星定位模块在整车姿态坐标系中的固定坐标，并与卫星定位系统中的坐标进行对比，建立整车姿态坐标系和地心坐标系之间的初始转换关系，作为车辆的初始姿态；其中，三个卫星定位模块布设在车辆不同位置上；
车辆使用过程中，获取三个卫星定位模块在不同时刻的卫星定位系统坐标，并与整车姿态坐标系中的固定坐标进行对比，得到整车姿态坐标系和地心坐标系的实时转换关系，其与初始转换关系的差值，即为车辆实时姿态相比初始姿态的变化。
其中，坐标系的转换可以采用多种方法，不局限于上述实施例的旋转矩阵方法。
本发明基于精度越来越高的卫星定位系统，通过定位模块获取坐标，经过坐标系转化后可直接计算出车辆姿态角度，为车辆姿态的计算提供了一种全新的解决思路。
应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。
- 上一篇：石墨接头机器人自动装卡簧、装栓机
- 下一篇：一种自动驾驶的定位传感方法、系统及装置
