机动车辆姿态的测量方法及测量装置的制造方法
【专利摘要】本发明涉及机动车辆姿态的测量方法及测量装置，该方法包括如下步骤：通过卫星定位模块获取车辆的航向角和瞬时速度以得到地理坐标系上的加速度；根据地理坐标上的加速度换算得到载体水平坐标系上的加速度；根据该载体水平坐标系上的加速度获取得到载体水平坐标系上的比力，并求取载体坐标系与载体水平坐标系之间的变换，之后求得理论姿态；根据公式Q＝Pi?1·D求取输出姿态，其中，Q为陀螺仪积分出的姿态，D为角速度乘以时间间隔构造的微旋转，Pi为当前输出姿态；将理论姿态和输出姿态进行插值处理，并将输出姿态以四元数的形式输出。本发明将卫星定位、加速度计和陀螺仪结合在一起作为倾角传感器，使得输出准确的倾角。
【专利说明】
机动车辆姿态的测量方法及测量装置
技术领域
[0001] 本发明设及机动车机动自动检测技术，尤其设及机动车辆姿态测量的方法及测量 装置。
【背景技术】
[0002] 驾驶人在取得机动车驾驶证前，需要通过场地驾驶技能和道路驾驶技能的考试， 即俗称的"科目二考试"和"科目Ξ考试"；前者用于考核驾驶人在常见道路、常见交通环境、 常见气候条件和突发交通状况下对机动车的操控能力，W及对交通事件的处理能力；后者 用于考核驾驶人在实际道路驾驶机动车时对机动车的操控能力、对交通法规的识别和遵守 能力、对道路交通情况的处理能力、W及文明驾驶、安全驾驶的意识。
[0003] 在科目二和科目Ξ考试中已经使用基于卫星定位系统的解决方案。为了判断车辆 行驶是否符合考试规则，需要获得车辆的位置和姿态。双天线卫星定位可W提供位置、航向 角和俯仰角，而横滚角需要额外的倾角传感器。倾角传感器的物理原理是用加速度计测量 比力，当设备静止时，倾角与比力相关。但是考试过程中车辆是变速运动的，比力包含运动 加速度，运时单独使用倾角传感器测出来的倾角并不准确。
【发明内容】
[0004] 针对上述技术问题，本发明的目的在于提供一种机动车辆姿态的测量方法及测量 装置，其能够准确的得到倾角。
[0005] 为实现上述目的，本发明采用如下技术方案：
[0006] 方案一：
[0007] -种机动车辆姿态的测量方法，应用于机动车辆，该机动车辆上设有巧螺仪、卫星 定位模块和加速度计;包括如下步骤：
[000引步骤A:通过卫星定位模块获取车辆的航向角和瞬时速度W得到地理坐标系上的 加速度;定义地理坐标系上的向量右上角用G标记，载体坐标系上的向量右上角用C标记，载 体水平坐标系上的向量右上角用Η标记；
[0009] 步骤Β:根据地理坐标上的加速度换算得到载体水平坐标系上的加速度；
[0010] 步骤C:根据该载体水平坐标系上的加速度获取得到载体水平坐标系上的比力，并 求取载体坐标系与载体水平坐标系之间的变换，之后求得理论姿态；
[OOW 步骤D:根据公式Q = Pi-i · D求取输出姿态，其中，Q为巧螺仪积分出的姿态，D为角 速度乘W时间间隔构造的微旋转，Pi为当前输出姿态，Pi-l为上一次输出姿态；
[0012] 步骤E:将理论姿态和输出姿态进行插值处理，并将输出姿态W四元数的形式输 出。
[0013] 优选的，步骤A中，根据公式
化理坐标系上的加速度，其中，互6为地理 坐标系上的加速度，为瞬时速度。
[0014] 优选的，步骤B中，根据公式= rH 'SG换算得到载体水平坐标系上的加速度， 其中，过Η为载体水平坐标系上的加速度，Rg人地理坐标系至载体水平坐标系的变换。
[0015] 优选的，步骤C具体包括如下子步骤：
[0016] 步骤C1:根据公式;+ gH得到载体水平坐标系上的比力，其中，复Η为载 体水平坐标系上的比力，gH为重力等效加速度.
[0017] 步骤C2:通过加速度计获取载体坐标系下的比力，记为gC;
[001引步骤C3:获取载体坐标系至载体水平坐标系的变换，记为Rg，并根据公式
求得理论姿态，其中，民若为理论姿态。
[0019] 优选的，步骤E具体包括如下子步骤：
[0020] 步骤E1:根据公式巧=.α+ Q.(l _ α)将理论姿态和输出姿态进行插值处 理，其中，Ρ功输出姿态，α为插值权重，且α的取值范围为(0,1);
[0021] 步骤Ε2:将PiW四元数的形式输出，即Pi=[w X y Ζ]。
[0022] 优选的，还包括步骤F:将输出姿态通过公式一、公式二和公式Ξ转换成姿态角输 出；
[00%]其中，yaw为航向角，Ρ i tch为俯仰角，ro 11为横滚角。
[0027] 方案二：
[0028] -种机动车辆姿态的测量装置，应用于机动车辆，该机动车辆上设有巧螺仪、卫星 定位模块和加速度计;包括如下模块：
[0029] 定位模块:用于通过卫星定位模块获取车辆的航向角和瞬时速度W得到地理坐标 系上的加速度;定义地理坐标系上的向量右上角用G标记，载体坐标系上的向量右上角用C 标记，载体水平坐标系上的向量右上角用Η标记；
[0030] 换算模块:用于根据地理坐标上的加速度换算得到载体水平坐标系上的加速度； [0031 ]变换模块:用于根据该载体水平坐标系上的加速度获取得到载体水平坐标系上的 比力，并求取载体坐标系与载体水平坐标系之间的变换，之后求得理论姿态；
[0032] 求取模块：用于根据公式Q = Pi-i . D求取输出姿态，其中，Q为巧螺仪积分出的姿 态，D为角速度乘W时间间隔构造的微旋转，Pi为当前输出姿态，Pi-i为上一次输出姿态；
[0033] 输出模块：用于将理论姿态和输出姿态进行插值处理，并将输出姿态W四元数的 形式输出。
[0034] 相比现有技术，本发明的有益效果在于：
[0035] 本发明将卫星定位、加速度计和巧螺仪结合在一起作为倾角传感器，使得输出准 确的倾角。
【附图说明】
[0036] 图1为本发明的机动车辆姿态的测量方法的流程图。
【具体实施方式】
[0037] 下面，结合附图W及【具体实施方式】，对本发明做进一步描述：
[0038] 参见图1，本发明提供一种机动车辆姿态的测量方法，其应用于机动车辆，该机动 车辆上设有巧螺仪、卫星定位模块和加速度计，本发明的改进在于将运Ξ者联合起来，求得 更精确的倾角。
[0039] 包括如下步骤：
[0040] 步骤S1:通过卫星定位模块获取车辆的航向角和瞬时速度W得到地理坐标系上的 加速度;定义地理坐标系上的向量右上角用G标记，载体坐标系上的向量右上角用C标记，载 体水平坐标系上的向量右上角用Η标记；
[0041] 地理坐标系固定在地面上，可W看做惯性系，ΧΥΖ轴方向由东北向定义;载体坐标 系固定在车上，ΧΥΖ轴方向由车的右向、前向和上向定义;载体水平坐标系，其Υ轴方向由载 体坐标系的Υ轴在水平面的投影定义，Ζ轴方向与地理坐标系相同，X轴为Ζ、Υ轴正交右手方 向。因为只设及姿态，忽略各坐标系的原点位置，姿态W矩阵和四元数的形式运算。车辆姿 态可W用载体坐标系到地理坐标系的变化表示，包含航向角、俯仰角和横滚角，而载体水平 坐标系与载体坐标系的区别是：W上变化不含俯仰角和横滚角，仅有航向角。
[0042] 具体的，在步骤S1中，根据公
也理坐标系上的加速度，其中，gG为 地理坐标系上的加速度，为瞬时速度。
[0043] 步骤S2:根据地理坐标上的加速度换算得到载体水平坐标系上的加速度；
[0044] 具体的，在步骤S2中，根据公式
换算得到载体水平坐标系上的加速 度，其中，gH为载体水平坐标系上的加速度，Rg为地理坐标系至载体水平坐标系的变换。
[0045] 地理坐标系至载体水平坐标系的变换仅包含航向角，可W利用航向角反算出来， Rg的作用之一是把加速度从地理坐标系变换到载体水平坐标系，因为只包含航向角，因而 Rg就是绕Z轴转航向角的旋转。
[0046] 步骤S3:根据该载体水平坐标系上的加速度获取得到载体水平坐标系上的比力， 并求取载体坐标系与载体水平坐标系之间的变换，之后求得理论姿态。
[0047] 具体的，步骤S3包括如下子步骤：
[004引步骤S31:根据公式
爵到载体水平坐标系上的比力，其中，为载 体水平坐标系上的比力，gH为重力等效加速度;重力等效加速度的取值为9.8m/s2，为物理 上的已知值；
[0049] 步骤S32:通过加速度计获取载体坐标系下的比力，记为客C;
[0化0]步骤S33:获取载体坐标系至载体水平坐标系的变换，记为R?并根据公式 Rf二(RS)-i.RS求得理论姿态，其中，R若为理论姿态。
[0化1]步骤S33中，先把gc旋转得到:§H，得到第一个旋转，用第一个旋转变换Y轴单位向 量，把得到的向量投影到XY平面，再旋转到Y轴，此为第二个旋转，两个旋转合起来就是Rg 其中，第一个旋转是主要的，把两个比力弄成重合，第二个旋转保证最终不破坏两坐标系的 Y轴在水平面的投影重合的要求;变换可W用矩阵或者四元数表示，取决于具体需求，数学 上是等价的。
[0052] 由于车辆实际行驶中，会有各种振动和冲击，直接用比力计算出的理论姿态有很 大的干扰，不利于考试评判，因而还需将巧螺仪的信息融合进来，W削弱振动和冲击的影 响。
[0053] 步骤S4:根据公式Q = Pi-i · D求取输出姿态，其中，Q为巧螺仪积分出的姿态，D为角 速度乘W时间间隔构造的微旋转，Pi为当前输出姿态，Pi-l为上一次输出姿态；
[0054] 步骤S5:将理论姿态和输出姿态进行插值处理，并将输出姿态W四元数的形式输 出。
[0055] 步骤S5具体是包括如下子步骤：
[0化6]步骤E1:根据公式
隐理论姿态和输出姿态进行插值处 理，其中，Pi为输出姿态，α为插值权重，且α的取值范围为（〇，1)，在实际应用中取接近0的 数；
[0化7]步骤Ε2:将PiW四元数的形式输出，即Pi=[w X y zLR^和Q对应同一个量，只是 通过不同的方式计算，长期来说是准确的，但是波动大，Q短期很稳定，但会慢慢累积误 差，因此用插值的方法把两者的优点结合起来。插值的时候用四元数表示姿态更加方便和 平滑。
[0058] 姿态角包括航向角、俯仰角和横滚角，Ξ个角度有旋转顺序，与由线-面角定义的 倾角不同。
[0059] 还可W包括步骤S6:将输出姿态通过公式一、公式二和公式Ξ转换成姿态角输出；
[0063] 其中，yaw为航向角，pitch为俯仰角，roll为横滚角。
[0064] 另一方面，本发明还提供一种机动车辆姿态的测量装置，应用于机动车辆，该机动 车辆上设有巧螺仪、卫星定位模块和加速度计;包括如下模块：
[0065] 定位模块:用于通过卫星定位模块获取车辆的航向角和瞬时速度W得到地理坐标 系上的加速度;定义地理坐标系上的向量右上角用G标记，载体坐标系上的向量右上角用C 标记，载体水平坐标系上的向量右上角用Η标记;本模块中，是根据公iS
I理 坐标系上的加速度，其中，这G为地理坐标系上的加速度，巧i6为瞬时速度。
[0066] 换算模块：用于根据地理坐标上的加速度换算得到载体水平坐标系上的加速度。 具体是根据公式
矣算得到载体水平坐标系上的加速度，其中，忌Η为载体水平 坐标系上的加速度，民g为地理坐标系至载体水平坐标系的变换。
[0067] 变换模块:用于根据该载体水平坐标系上的加速度获取得到载体水平坐标系上的 比力，并求取载体坐标系与载体水平坐标系之间的变换，之后求得理论姿态；
[0068] 变化模块可W包括如下子模块：
[0069] 相加模块:用于根据公式
尋到载体水平坐标系上的比力，其中，§H 为载体水平坐标系上的比力，gH为重力等效加速度；
[0070] 第一获取模块:用于通过加速度计获取载体坐标系下的比力，记为s'E;
[0071] 第二获取模块：用于获取载体坐标系至载体水平坐标系的变换，记为并根据 公
得理论姿态，其中，Rf为理论姿态。
[0072] 求取模块：用于根据公式Q = Pi-i . D求取输出姿态，其中，Q为巧螺仪积分出的姿 态，D为角速度乘W时间间隔构造的微旋转，Pi为当前输出姿态，Pi-1为上一次输出姿态；
[0073] 输出模块：用于将理论姿态和输出姿态进行插值处理，并将输出姿态W四元数的 形式输出。
[0074] 输出模块具体可包括如下子模块：
[00巧]处理子模块：用于根据公式
将理论姿态和输出姿态进 行插值处理，其中，Pi为输出姿态，α为插值权重；
[0076] 输出子模块:用于将PiW四元数的形式输出，即Pi=[w X y Ζ]。
[0077] 另外，还可W包括转换模块:用于将输出姿态通过公式一、公式二和公式Ξ转换成 姿态角输出；
[0081] 其中，yaw为航向角，pitch为俯仰角，roll为横滚角。
[0082] 对本领域的技术人员来说，可根据W上描述的技术方案W及构思，做出其它各种 相应的改变W及形变，而所有的运些改变W及形变都应该属于本发明权利要求的保护范围 之内。
【主权项】
1. 一种机动车辆姿态的测量方法，应用于机动车辆，该机动车辆上设有陀螺仪、卫星定 位模块和加速度计;其特征在于，包括如下步骤： 步骤A:通过卫星定位模块获取车辆的航向角和瞬时速度以得到地理坐标系上的加速 度;定义地理坐标系上的向量右上角用G标记，载体坐标系上的向量右上角用C标记，载体水 平坐标系上的向量右上角用Η标记； 步骤Β:根据地理坐标上的加速度换算得到载体水平坐标系上的加速度； 步骤C:根据该载体水平坐标系上的加速度获取得到载体水平坐标系上的比力，并求取 载体坐标系与载体水平坐标系之间的变换，之后求得理论姿态； 步骤D:根据公式〇 = · D求取输出姿态，其中，Q为陀螺仪积分出的姿态，D为角速度 乘以时间间隔构造的微旋转，Pi为当前输出姿态，Ρκ为上一次输出姿态； 步骤Ε:将理论姿态和输出姿态进行插值处理，并将输出姿态以四元数的形式输出。2. 如权利要求1所述的机动车辆姿态的测量方法，其特征在于，步骤Α中，根据公式地理坐标系上的加速度，其中，gG为地理坐标系上的加速度，f为瞬时速度。3. 如权利要求2所述的机动车辆姿态的测量方法，其特征在于，步骤B中，根据公式算得到载体水平坐标系上的加速度，其中，gH为载体水平坐标系上的加速 度，Rg为地理坐标系至载体水平坐标系的变换。4. 如权利要求3所述的机动车辆姿态的测量方法，其特征在于，步骤C具体包括如下子 步骤： 步骤C1:根据公式= 5H + ^?得到载体水平坐标系上的比力，其中，为载体水平 坐标系上的比力，gH为重力等效加速度； 步骤C2:通过加速度计获取载体坐标系下的比力，记为# , 步骤C3:获取载体坐标系至载体水平坐标系的变换，记为Rg.片根据公式求得理论姿态，其中，R|为理论姿态。5. 如权利要求4所述的机动车辆姿态的测量方法，其特征在于，步骤E具体包括如下子 步骤： 步骤E1:根据公式巧二. Ct + - α)将理论姿态和输出姿态进行插值处理，其 中，Pi为输出姿态，α为插值权重，且α的取值范围为(〇，1); 步骤Ε2:将Pi以四元数的形式输出，即Pi=[w X y ζ]。6. 如权利要求4所述的机动车辆姿态的测量方法，其特征在于，还包括步骤F:将输出姿 态通过公式一、公式二和公式三转换成姿态角输出；其中，yaw为航向角，pitch为俯仰角，roll为横滚角。7. -种机动车辆姿态的测量装置，应用于机动车辆，该机动车辆上设有陀螺仪、卫星定 位模块和加速度计;其特征在于，包括如下模块： 定位模块:用于通过卫星定位模块获取车辆的航向角和瞬时速度以得到地理坐标系上 的加速度;定义地理坐标系上的向量右上角用G标记，载体坐标系上的向量右上角用C标记， 载体水平坐标系上的向量右上角用Η标记； 换算模块:用于根据地理坐标上的加速度换算得到载体水平坐标系上的加速度； 变换模块：用于根据该载体水平坐标系上的加速度获取得到载体水平坐标系上的比 力，并求取载体坐标系与载体水平坐标系之间的变换，之后求得理论姿态； 求取模块:用于根据公式Q = Ph · D求取输出姿态，其中，Q为陀螺仪积分出的姿态，D为 角速度乘以时间间隔构造的微旋转，Pi为当前输出姿态，Pi-i为上一次输出姿态； 输出模块：用于将理论姿态和输出姿态进行插值处理，并将输出姿态以四元数的形式 输出。
【文档编号】G01C9/00GK105823463SQ201610158753
【公开日】2016年8月3日
【申请日】2016年3月17日
【发明人】鄢尤帅, 禤永俊
【申请人】广州展讯信息科技有限公司