正北坐标系由组合导航给出，以正北为0，顺时针为正。本车的heading与期望heading 之间的夹角，delta_heading 计算方法如下：
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62function [delta_heading] = CalHeading(heading_desire, heading_current) index_store = 0; index_current = 0; index_store = floor(heading_desire/90); index_current = floor(heading_current /90); delta_heading = 0; index_store = floor(heading_desire/90); index_current = floor(heading_current /90); delta_heading = 0; if abs(index_store - index_current) == 0 delta_heading = heading_desire - heading_current; elseif abs(index_store - index_current) == 1 delta_heading = heading_desire - heading_current; elseif index_store ==0 && index_current ==3 delta_heading = heading_desire - heading_current + 360; elseif index_store ==3 && index_current ==0 delta_heading = heading_desire - heading_current - 360; return; end if abs(index_store - index_current) == 2 if index_store ==0 && index_current ==2 if abs(heading_desire - heading_current)> 180 delta_heading = - (heading_desire + 360 - heading_current); else delta_heading = heading_current - heading_desire; end return; end if index_store ==2 && index_current ==0 if abs(heading_desire- heading_current) > 180 delta_heading = 360 + heading_current - heading_desire; else delta_heading = heading_current - heading_desire ; end return; end if index_store ==1 && index_current ==3 if abs(heading_desire- heading_current) > 180 delta_heading = - (heading_desire + 360 - heading_current); else delta_heading = heading_current - heading_desire; end return; end if index_store ==3 && index_current ==1 if abs(heading_desire- heading_current) > 180 delta_heading = heading_current + 360 - heading_desire; else delta_heading = -(heading_desire - heading_current); end return; end end end
最后
以上就是务实百合最近收集整理的关于正北坐标系和车辆坐标系下的heading转换的全部内容，更多相关正北坐标系和车辆坐标系下内容请搜索靠谱客的其他文章。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复