基于双线的移动物体姿态测量方法与流程

1.本发明公开了基于双线的移动物体姿态测量方法，属于姿态测量技术领域。


背景技术：

2.姿态角的测量方法有多种，通常是使用磁敏传感器、旋转电位器、角度传感器、机器视觉、惯性传感器等设备对姿态角进行测量，但每种设备的测量方式都有自己的长处与约束。磁敏传感器对环境要求苛刻，温度适应范围窄，可靠性差，通常仅仅作为辅助测量手段；旋转电位器受到的滑动触点的接触效果和机械运动阻力的影响和ad转换模块的转换精度限制，在高分辨率的情况下数据波动严重，难以实现稳定测量；角度传感器中，增量式编码器的角度检测精度受到单圈脉冲数量限制，长期工作会产生累积误差，且断电重启会出现脉冲丢失，需要人工复位，而绝对值编码器的每一个位置对应一个确定的数字码，但受到量程范围的制约，且结构复杂，价格昂贵；机器视觉容易受到粉尘等污染物、摄像仪分辨率以及摄像距离的影响，受到环境的约束较大。惯性传感器存在安装误差、初始对准误差和运动干扰误差等影响，且随着使用时间延长，惯性传感器会因加速度和角速度的积分产生累积漂移，影响正常使用。


技术实现要素：

3.本发明提出了基于双线的移动物体姿态测量方法，解决现有技术中通过仪器进行姿态测量不准确的问题。
4.基于双线的移动物体姿态测量方法，包括：
5.s1.测量移动物体绕坐标系的y轴旋转得到的航向角；
6.s2.测量移动物体绕坐标系的x轴旋转得到的俯仰角；
7.s3.测量移动物体绕坐标系的z轴旋转得到的横滚角；
8.s4.计算其中一条激光照射点在参考激光接收标靶坐标系(未发生横滚)下的坐标值。
9.优选地，采用基准激光接收标靶、参考激光接收标靶和两个激光器，所述两个激光器的高度一致，发射出的激光相互平行，激光穿透基准激光接收标靶并射在参考激光接收标靶上，其中一条激光固定照射基准激光接收标靶的一点，两个标靶均竖直放置、中心点的连线平行于激光且随移动物体同步移动。
10.优选地，所述基准激光接收标靶和参考激光接收标靶在各自左侧面中心位置建立坐标系，读取激光照射点在各标靶坐标系下的坐标值，通过空间几何关系计算出移动物体在空间中的姿态角。
11.优选地，设a、c分别是第一激光线、第二激光线在基准激光接收标靶上的照射点，b、d分别是第一激光线、第二激光线在参考激光接收标靶上的照射点，则横滚角roll的计算公式为：
12.式中，o1为基准激光接收标靶的左侧面坐标系原点，表示o1和a的连线，x和y分别是c在坐标系{o1}的横纵坐标值。
13.优选地，俯仰角沿水平面向上转动时值为正，取值范围为(-90
°‑
90
°
)，设b在坐标系{基准o2}坐标为(x，y)，bhm平面垂直于两个标靶，交坐标系{o1}与坐标系{基准o2}于h和m。即式中，l
hm
为两个标靶之间的距离，pitch为俯仰角。坐标系{基准o2}：未发生横滚时的{o2}坐标系；坐标系{o2}：基准激光接收标靶左侧面中心位置建立的坐标系。
14.优选地，航向角yaw绕点a处y轴逆时针旋转为正，且取值范围为(-90
°‑
90
°
)，)，
15.优选地，b的坐标求解过程为：根据b点在参考激光接收标靶的位置情况，将参考激光接收标靶分割为8个区域，求解相邻区域之间的分割线方程，根据横滚角的8个范围，分别求解b在坐标系{基准o2}下的坐标。
16.与现有技术对比，本发明的有益效果是：本发明结构简单，不需要依靠外部数据，姿态角计算方法简单，可以长期工作，不会出现累计误差，可以实现稳定测量；红外激光照射间隔远，产品寿命长，且红外线具有很强的穿透能力，受到粉尘等颗粒物的影响较小，使用温度范围较高；激光接收标靶相对间隔较小，直接附连在物体上，质量较轻，且断电后重新上电不用调零。
附图说明
17.图1为本发明的测量装置；
18.图2为基于图1的三角求解示意图；
19.图3为q方向的视图；
20.图4为n方向的视图；
21.图5为横滚角：rall∈(-180
°‑‑
90
°
)时的区域标识图；
22.图6为横滚角：rall∈(-90
°‑0°
)时的区域标识图；
23.图7为横滚角：rall∈(0
°‑
90
°
)时的区域标识图；
24.图8为横滚角：rall∈(90
°‑
180
°
)时的区域标识图；
25.附图标记包括：1-第一激光器，2-第二激光器，3-第一激光线，4-第二激光线，5-基准激光接收标靶，6-参考激光接收标靶。
具体实施方式
26.下面结合具体实施例对本发明的具体实施方式做进一步说明：
27.基于双线的移动物体姿态测量方法，包括：
28.s1.测量移动物体绕坐标系的y轴旋转得到的航向角；
29.s2.测量移动物体绕坐标系的x轴旋转得到的俯仰角；
30.s3.测量移动物体绕坐标系的z轴旋转得到的横滚角；
31.s4.计算其中一条激光照射点在参考激光接收标靶坐标系(未发生横滚)下的坐标值。
32.采用基准激光接收标靶5、参考激光接收标靶6和两个激光器，所述两个激光器的高度一致，发射出的激光相互平行，激光穿透基准激光接收标靶5并射在参考激光接收标靶6上，其中一条激光固定照射基准激光接收标靶5的一点，两个标靶均竖直放置、中心点的连线平行于激光且随移动物体同步移动。
33.所述基准激光接收标靶5和参考激光接收标靶6在各自左侧面中心位置建立坐标系，读取激光照射点在各标靶坐标系下的坐标值，通过空间几何关系计算出移动物体在空间中的姿态角。
34.设a、c分别是第一激光线3、第二激光线4在基准激光接收标靶5上的照射点，b、d分别是第一激光线3、第二激光线4在参考激光接收标靶6上的照射点。横滚角求解中，以基准激光接收标靶5为研究对象，在基准激光接收标靶5存在偏转与俯仰的情况下，如图2，正视该标靶(图中l视向)，在不同角度的横滚角情况下进行分析。图2中坐标系具体为：坐标系{o0}：旋转坐标系：即移动物体绕其三轴依次旋转；坐标系{o1}：基准激光接收标靶5左侧面中心位置建立的坐标系；旋转角度即基准激光接收标靶5的在l视向下的旋转角度:roll；旋转角roll为∠o1ac；点e为过点c往坐标系{o1}的x1轴做垂线，垂足为点e；bhm平面垂直于两个标靶，交坐标系{o1}与坐标系{基准o2}于h和m。根据不同方向的视图，说明书附图中包括了l、n、p、q四个方向的箭头，来表示由该方向看去的对应视觉效果，其中q方向的视图如图3，n方向的视图如图4；
35.横滚角roll的计算公式为：
36.式中，o1为基准激光接收标靶5左侧面坐标系原点，表示o1和a的连线，x和y分别是c在坐标系{o1}的横纵坐标值。
37.c点坐标为：c：(x,0)，符号为：(-,0)，此时横滚角为平角：∠o1ac＝
±
180
°
。
38.c点坐标为：c(x,y)，符号为：(-,-)，此时横滚角为钝角且为负值：
[0039][0040]
即：
[0041]
c点坐标为：c(x,y)，符号为：此时横滚角为直角：∠o1ac＝-90
°
；
[0042]
c点坐标为：c(x,y)，符号为：(-,-)，此时横滚角为锐角且为负值：
[0043][0044]
即：
[0045]
c点坐标为c：(0,y)，符号为：(0,-)，此时横滚角为锐角且为负值：
[0046]
[0047]
即：
[0048]
c点坐标为：c(x,y)，符号为：(+,-)，此时横滚角为锐角且为负值：
[0049][0050]
即：
[0051]
c点坐标为：c(x,0)，符号为：(+,0)，此时横滚角：roll＝0
°
[0052]
c点坐标为：c(x,y)，符号为：(+,+)，此时横滚角为锐角且为正值：
[0053][0054]
即：
[0055]
c点坐标为：c(0,y)，符号为：(0,+)，此时横滚角为锐角且为正值：
[0056][0057]
即：
[0058]
c点坐标为：c(x,y)，符号为：(-,+)，此时横滚角为锐角且为正值：
[0059][0060]
即：
[0061]
c点坐标为：c(x,y)，符号为：此时横滚角为直角：roll＝90
°
[0062]
c点坐标为：c(x,y)，符号为：(-,+)，此时横滚角为钝角：
[0063][0064]
即：
[0065]
俯仰角沿水平面向上转动时值为正，取值范围为(-90
°‑
90
°
)，设b在坐标系{基准o2}坐标为(x，y)，bhm平面垂直于两个标靶，交坐标系{o1}与坐标系{基准o2}于h和m。即式中，l
hm
为两个标靶之间的距离，pitch为俯仰角。坐标系{基准o2}：未发生横滚时的{o2}坐标系；坐标系{o2}：基准激光接收标靶5左侧面中心位置建立的坐标系。
[0066]
航向角yaw绕点a处y轴逆时针旋转为正，且取值范围为(-90
°‑
90
°
)，)，
[0067]
b的坐标求解过程为：根据b点在参考激光接收标靶的位置情况，将参考激光接收标靶6分割为8个区域，求解相邻区域之间的分割线方程，根据横滚角的8个范围，分别求解b在坐标系{基准o2}下的坐标，划分的8个区域如图5至图8所示。
[0068]
b点坐标求解中，以参考激光接收标靶6为研究对象，以下各个图形是在参考激光接收标靶6存在偏转与俯仰的情况下，如图2，正视该标靶(图中p视向)，不同角度的横滚角情况下进行的分析作图。图中坐标系具体为：坐标系{基准o2}：未发生横滚时的{o2}坐标系；坐标系{o2}：基准激光接收标靶5左侧面中心位置建立的坐标系；区域分割方程是在坐标系{o2}下的方程；定义以下计算均采用，即b0(-25，0)；点b0是第一激光线3在参考激光接收标靶6(装置未发生任何转动时)上的照射点；各个区域角度标识为总结求得；点f为过点b往坐标系{o2}的x轴做垂线，垂足为点f。
[0069]
b1.横滚角：rall＝
±
180
°
时，点b在坐标为：b(-(50+x)，-y)，式中50为o2到基准o2的距离。
[0070]
如图5，b2.横滚角：rall∈(-180
°‑‑
90
°
)时，
[0071]
1区域与2区域的分割线方程为：y＝tan(roll)x+25tan(roll)；
[0072]
2区域与3区域的分割线方程为：x＝-25；
[0073]
3区域与4区域的分割线方程为：
[0074]
4区域与5区域的分割线方程为：y＝0；
[0075]
当x＝-25，y＝0时，x＝-25，y＝0；
[0076]
当x》-25且即在1区域时：
[0077][0078]
当x《-25且即在5区域时：
[0079][0080]
当x》-25且y》tan(roll)x+25tan(roll)，即在2区域时：
[0081][0082]
当x《-25且y《tan(roll)x+25tan(roll)时，即在6区域时：
[0083][0084]
当x《-25且即在3区域时：
[0085][0086]
当x》-25且即在7区域时：
[0087][0088]
当x《-25且即在4区域时：
[0089][0090]
当x》-25且即在8区域时：
[0091][0092]
在边界条件时分割线两侧公式均可运用。
[0093]
b3.横滚角：rall＝-90
°
时：点b为：b(-(25+y)，25+x)；
[0094]
如图6，b4.横滚角：rall∈(-90
°‑0°
)时：
[0095]
1区域与2区域的分割线方程为：y＝-tan(roll)x-25tan(roll)；
[0096]
2区域与3区域的分割线方程为：x＝-25；
[0097]
3区域与4区域的分割线方程为：
[0098]
4区域与5区域的分割线方程为：y＝0；
[0099]
当x＝-25，y＝0时，x＝-25，y＝0；
[0100]
当x》-25且即在1区域时：
[0101][0102]
当x《-25且即在5区域时：
[0103][0104]
当x》-25且y》-tan(roll)x-25tan(roll)，即在2区域时：
[0105][0106]
当x《-25且y《-tan(roll)x-25tan(roll)，即在6区域时：
[0107][0108]
当x《-25且即在3区域时：
[0109][0110]
当x》-25且即在7区域时：
[0111][0112]
当x《-25且即在4区域时：
[0113]
[0114]
当x》-25且即在8区域时
[0115][0116]
b5.横滚角：rall＝0
°
时：点b的坐标为：b(x，y)；
[0117]
如图7，b6.横滚角：rall∈(0
°‑
90
°
)时：
[0118]
1区域与2区域的分割线方程为：y＝tan(roll)x+25tan(roll)；
[0119]
2区域与3区域的分割线方程为：x＝-25；
[0120]
3区域与4区域的分割线方程为：
[0121]
4区域与5区域的分割线方程为：y＝0；
[0122]
当x＝-25，y＝0时，x＝-25，y＝0；
[0123]
当x》-25且即在1区域时：
[0124][0125]
当x《-25且即在5区域时：
[0126][0127]
当x》-25且y》tan(roll)x+25tan(roll)，即在2区域时：
[0128]
当x《-25且y《tan(roll)x+25tan(roll)，即在6区域时：
[0129][0130]
当x《-25且即在3区域时：
[0131][0132]
当x》-25且即在7区域时：
[0133][0134]
当x《-25且即在4区域时：
[0135][0136]
当x》-25且即在8区域时：
[0137][0138]
b7.横滚角：rall＝90
°
时：点b的坐标为：b(-25+y，-(25+x))；
[0139]
如图8，b8.横滚角：rall∈(90
°‑
180
°
)时：
[0140]
1区域与2区域的分割线方程为：y＝-tan(roll)x-25tan(roll)；
[0141]
2区域与3区域的分割线方程为：x＝-25；
[0142]
3区域与4区域的分割线方程为：
[0143]
4区域与5区域的分割线方程为：y＝0；
[0144]
当x＝-25，y＝0时，x＝-25，y＝0；
[0145]
当x》-25且即在1区域时：
[0146][0147]
当x《-25且即在5区域时：
[0148][0149]
当x》-25且y》-tan(roll)x-25tan(roll)，即在2区域时：
[0150][0151]
当x《-25且y《tan(roll)x+25tan(roll)时，即在6区域时：
[0152][0153]
当x《-25且即在3区域时：
[0154]
当x》-25且即在7区域时：
[0155]
当x《-25且即在4区域时：
[0156]