TWI811733B - Attitude measurement method, navigation method and system of transportation vehicle - Google Patents

Attitude measurement method, navigation method and system of transportation vehicle Download PDF

Info

Publication number
TWI811733B
TWI811733B TW110125489A TW110125489A TWI811733B TW I811733 B TWI811733 B TW I811733B TW 110125489 A TW110125489 A TW 110125489A TW 110125489 A TW110125489 A TW 110125489A TW I811733 B TWI811733 B TW I811733B
Authority
TW
Taiwan
Prior art keywords
vehicle
attitude
attitude data
information
satellite positioning
Prior art date
Application number
TW110125489A
Other languages
Chinese (zh)
Other versions
TW202303097A (en
Inventor
宇廷 譚
Original Assignee
台灣智慧駕駛股份有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 台灣智慧駕駛股份有限公司 filed Critical 台灣智慧駕駛股份有限公司
Priority to TW110125489A priority Critical patent/TWI811733B/en
Publication of TW202303097A publication Critical patent/TW202303097A/en
Application granted granted Critical
Publication of TWI811733B publication Critical patent/TWI811733B/en

Links

Images

Abstract

一種交通載具的姿態量測方法、導航方法及其系統，先利用一衛星定位裝置產生的一衛星定位資訊取得該交通載具的一航跡角，接著，對一慣性測量單元量測到該交通載具的一加速度值進行計算而取得該交通載具的一重力加速度。之後，利用一旋轉矩陣且根據該重力加速度以及該航跡角計算該交通載具的一姿態數據。利用對來自於不同感測器的數據進行數據融合，先預測移動中的該交通載具在下一時間點的姿態，並使用其他資訊更新該交通載具的姿態，再根據更新後的姿態進行更下一時間點的姿態預測，以做準確的姿態預測。An attitude measurement method, navigation method and system for a traffic vehicle. First, a satellite positioning information generated by a satellite positioning device is used to obtain a track angle of the traffic vehicle. Then, an inertial measurement unit is used to measure the traffic angle. An acceleration value of the vehicle is calculated to obtain a gravity acceleration of the vehicle. Then, a rotation matrix is used to calculate an attitude data of the vehicle according to the gravity acceleration and the track angle. Using data fusion from different sensors, we first predict the attitude of the moving vehicle at the next time point, use other information to update the attitude of the vehicle, and then update the vehicle based on the updated attitude. Attitude prediction at the next time point to make accurate attitude prediction.

Description

交通載具的姿態量測方法、導航方法及其系統Attitude measurement method, navigation method and system for transportation vehicles

本發明是有關於一種交通載具的方法及其系統，且特別關於一種交通載具的姿態量測方法、導航方法及其系統。 The present invention relates to a transportation vehicle method and a system thereof, and in particular to a transportation vehicle attitude measurement method, a navigation method and a system thereof.

車輛導航的技術中，通常利用全球定位系統(GPS)取得車輛在地圖上的位置，並配合二維濾波方法，根據車速與轉向角速度來推估車輛的行動軌跡，將位置資訊融合行動軌跡後，得到車輛的精確位置，從而提供導航或行駛資訊。然而，在某些坡度起伏的場地(如高爾夫球場)，由於將三維投影至二維時會產生角度的變形，導致推估位置與實際位置產生不可忽略的偏差，造成定位結果不準確。 In vehicle navigation technology, the global positioning system (GPS) is usually used to obtain the vehicle's position on the map, and is combined with a two-dimensional filtering method to estimate the vehicle's movement trajectory based on the vehicle speed and steering angular velocity. After integrating the location information with the movement trajectory, Obtain the precise location of the vehicle to provide navigation or driving information. However, in some venues with undulating slopes (such as golf courses), due to angular deformation when projecting three-dimensional to two-dimensional, there will be a non-negligible deviation between the estimated position and the actual position, resulting in inaccurate positioning results.

為解決此問題，可以在車輛中引入姿態航向參考系統(attitude and heading reference system，AHRS)，直接利用AHRS取得車輛的三維姿態，例如美國發明專利第US6697736B2號、第US7409290B2號以及第US9924314B2號等。由於AHRS通常包括三軸加速度計、陀螺儀以及磁力計，又AHRS大多裝設於車輛的輪軸附近的區域，而設置在該區域的大量鋼鐵材質的零件、馬達以及電路元件會產生難以消除的磁場干擾，導致AHRS無法正常運作。 To solve this problem, an attitude and heading reference system (AHRS) can be introduced into the vehicle, and the AHRS can be directly used to obtain the three-dimensional attitude of the vehicle, such as U.S. Invention Patent Nos. US6697736B2, US7409290B2, and US9924314B2. Since AHRS usually includes a three-axis accelerometer, gyroscope and magnetometer, and AHRS is mostly installed in the area near the wheel axle of the vehicle, a large number of steel parts, motors and circuit components installed in this area will generate a magnetic field that is difficult to eliminate. Interference causes AHRS to fail to function properly.

本發明提供一種交通載具的姿態量測方法、導航方法及其系統，能在不採用磁力計的情況下，量測移動中的交通載具姿態，以更精準地定位交通載具，並進一步提供導航資訊。 The present invention provides an attitude measurement method, a navigation method and a system for a transportation vehicle, which can measure the attitude of a moving transportation vehicle without using a magnetometer to more accurately position the transportation vehicle, and further Provide navigation information.

本發明一實施例提供一種導航方法，應用於一交通載具，該方法包括以下步驟：利用一衛星定位裝置產生的一衛星定位資訊取得該交通載具的一航跡角φ；對一慣性測量單元量測到該交通載具的一加速度值進行計算而取得該交通載具在體座標系下的一重力加速度向量，該重力加速度向量以下式表示：

Figure 110125489-A0305-02-0003-4
，其中g x 、g y 、g z 為重力加速度在體座標系下之分量； 根據下式計算該交通載具的一第一姿態數據：
Figure 110125489-A0305-02-0003-7
 An embodiment of the present invention provides a navigation method, which is applied to a transportation vehicle. The method includes the following steps: using a satellite positioning information generated by a satellite positioning device to obtain a track angle φ of the transportation vehicle; and measuring an inertial measurement unit. An acceleration value of the vehicle is measured and calculated to obtain a gravity acceleration vector of the vehicle in the body coordinate system. The gravity acceleration vector is expressed by the following formula:
Figure 110125489-A0305-02-0003-4
, where g x , g y , g z are the components of gravity acceleration in the body coordinate system; calculate a first attitude data of the transportation vehicle according to the following formula:
Figure 110125489-A0305-02-0003-7

其中，

Figure 110125489-A0305-02-0003-8
，
Figure 110125489-A0305-02-0003-10
，
Figure 110125489-A0305-02-0003-12
，
Figure 110125489-A0305-02-0003-13
Figure 110125489-A0305-02-0003-9
，A=l x cos φ，B=l x sin φ，C=g x ，D=g y ，E=g z ，
Figure 110125489-A0305-02-0003-14
； 以該慣性測量單元量測到該交通載具的一角速度為時變量，並用該角速度對時間進行積分而取得一第二姿態數據；利用一遞歸濾波器，並至少根據該交通載具的該第一姿態數據以及該第二姿態數據計算該交通載具的一姿態資訊；以及根據該姿態資訊提供一導航資訊。 in,
Figure 110125489-A0305-02-0003-8
,
Figure 110125489-A0305-02-0003-10
,
Figure 110125489-A0305-02-0003-12
,
Figure 110125489-A0305-02-0003-13
Figure 110125489-A0305-02-0003-9
, A = l x cos φ, B = l x sin φ, C = g x , D = g y , E = g z ,
Figure 110125489-A0305-02-0003-14
; Using the inertial measurement unit to measure an angular velocity of the vehicle as a time variable, and integrating the angular velocity with time to obtain a second attitude data; using a recursive filter, and at least based on the angular velocity of the vehicle; The first attitude data and the second attitude data calculate an attitude information of the vehicle; and provide navigation information based on the attitude information.

本發明又一實施例提供一種導航系統，應用於一交通載具，該系統包括：一衛星定位裝置，裝設於該交通載具上，該衛星定位裝置用於從複數個衛星接收訊號後產生一衛星定位資訊；一慣性測量單元，裝設於該交通載具上，該慣性測量單元用於量測該交通載具的一加速度值以取得該交通載具在體座標系下的一重力加速度向量，該重力加速度向量以下式表示：

Figure 110125489-A0305-02-0004-15
，其中g x 、g y 、g z 為重力加速度在體座標系下之分量； 至少一處理器，該處理器被配置為：根據該衛星定位資訊計算該交通載具的一航跡角φ；根據下式計算該交通載具的一第一姿態數據：
Figure 110125489-A0305-02-0004-16
 Yet another embodiment of the present invention provides a navigation system applied to a transportation vehicle. The system includes: a satellite positioning device installed on the transportation vehicle. The satellite positioning device is used to generate signals after receiving signals from a plurality of satellites. A satellite positioning information; an inertial measurement unit installed on the transportation vehicle. The inertial measurement unit is used to measure an acceleration value of the transportation vehicle to obtain a gravity acceleration of the transportation vehicle in the body coordinate system. Vector, the gravity acceleration vector is expressed by the following formula:
Figure 110125489-A0305-02-0004-15
, where g x , g y , g z are the components of gravity acceleration in the body coordinate system; at least one processor, the processor is configured to: calculate a track angle φ of the traffic vehicle according to the satellite positioning information; according to The following formula calculates a first attitude data of the transportation vehicle:
Figure 110125489-A0305-02-0004-16

其中，

Figure 110125489-A0305-02-0004-17
，
Figure 110125489-A0305-02-0004-22
，
Figure 110125489-A0305-02-0004-21
，
Figure 110125489-A0305-02-0004-19
Figure 110125489-A0305-02-0004-18
，A=l x cos φ，B=l x sin φ，C=g x ，D=g y ，E=g z ，
Figure 110125489-A0305-02-0004-20
； 以該慣性測量單元量測到該交通載具的一角速度值為時變量，並用該角速度值對時間進行積分而取得一第二姿態數據；利用一遞歸濾波器，並至少根據該交通載具的該第一姿態數據以及該第二姿態數據計算該交通載具的一姿態資訊；以及根據該姿態資訊提供一導航資訊。 in,
Figure 110125489-A0305-02-0004-17
,
Figure 110125489-A0305-02-0004-22
,
Figure 110125489-A0305-02-0004-21
,
Figure 110125489-A0305-02-0004-19
Figure 110125489-A0305-02-0004-18
, A = l x cos φ, B = l x sin φ, C = g x , D = g y , E = g z ,
Figure 110125489-A0305-02-0004-20
; Use an angular velocity value of the traffic vehicle measured by the inertial measurement unit as a time variable, and use the angular velocity value to integrate time to obtain a second attitude data; use a recursive filter, and at least according to the traffic vehicle Calculate a posture information of the vehicle based on the first posture data and the second posture data; and provide navigation information based on the posture information.

本發明另一實施例提供一種交通載具的姿態量測方法，包括以下步驟：利用一衛星定位裝置產生的一衛星定位資訊取得該交通載具的一航跡角φ；利用一慣性測量單元量測到該交通載具的一加速度值取得該交通載具在體座標系下的一重力加速度向量，該重力加速度向量以下式表示：

Figure 110125489-A0305-02-0004-24
，其中g x 、g y 、g z 為重力加速度在體座標系下之分量；以及 根據下式得計算該交通載具的一姿態數據：
Figure 110125489-A0305-02-0004-23
 其中，
Figure 110125489-A0305-02-0005-25
，
Figure 110125489-A0305-02-0005-28
，
Figure 110125489-A0305-02-0005-29
，d=
Figure 110125489-A0305-02-0005-30
，A=l x cos φ，B=l x sin φ，C=g x ，D=g y ， E=g z ，
Figure 110125489-A0305-02-0005-32
。 Another embodiment of the present invention provides an attitude measurement method of a transportation vehicle, which includes the following steps: using a satellite positioning information generated by a satellite positioning device to obtain a track angle φ of the transportation vehicle; using an inertial measurement unit to measure Obtain an acceleration value of the transportation vehicle to obtain a gravity acceleration vector of the transportation vehicle in the body coordinate system. The gravity acceleration vector is expressed by the following formula:
Figure 110125489-A0305-02-0004-24
, where g x , g y , g z are the components of gravity acceleration in the body coordinate system; and an attitude data of the transportation vehicle can be calculated according to the following formula:
Figure 110125489-A0305-02-0004-23
 in,
Figure 110125489-A0305-02-0005-25
,
Figure 110125489-A0305-02-0005-28
,
Figure 110125489-A0305-02-0005-29
, d =
Figure 110125489-A0305-02-0005-30
, A = l x cos φ, B = l x sin φ, C = g x , D = g y , E = g z ,
Figure 110125489-A0305-02-0005-32
.

10:姿態量測系統 10: Attitude measurement system

11:處理器 11: Processor

12:衛星定位裝置 12: Satellite positioning device

13:慣性測量單元 13:Inertial measurement unit

14:光檢測和測距裝置 14:Light detection and ranging device

15:影像擷取裝置 15:Image capture device

20:交通載具 20:Transportation vehicles

21:後輪軸 21:Rear axle

S1-1、S1-2、S1-3、S2-1、S2-2、S2-3、S2-4、S2-5、S2-6、S2-7:步驟 S1-1, S1-2, S1-3, S2-1, S2-2, S2-3, S2-4, S2-5, S2-6, S2-7: steps

『圖1』是根據本發明一實施例的姿態量測系統示意圖。 [Fig. 1] is a schematic diagram of an attitude measurement system according to an embodiment of the present invention.

『圖2』是根據本發明一實施例的姿態量測系統示意圖。 [Fig. 2] is a schematic diagram of an attitude measurement system according to an embodiment of the present invention.

『圖3』是根據本發明一實施例的姿態量測方法流程示意圖。 [Fig. 3] is a schematic flowchart of an attitude measurement method according to an embodiment of the present invention.

『圖4』是根據本發明一實施例的交通載具航跡角的示意圖。 [Fig. 4] is a schematic diagram of the track angle of a traffic vehicle according to an embodiment of the present invention.

『圖5』是根據本發明一實施例的交通載具姿態的示意圖。 [Fig. 5] is a schematic diagram of the posture of a transportation vehicle according to an embodiment of the present invention.

『圖6』是根據本發明一實施例的導航方法流程示意圖。 [Fig. 6] is a schematic flowchart of a navigation method according to an embodiment of the present invention.

『圖7』是根據本發明另一實施例的姿態量測系統示意圖。 [Fig. 7] is a schematic diagram of an attitude measurement system according to another embodiment of the present invention.

『圖1』、『圖2』是根據本發明一實施例的姿態量測系統示意圖，該姿態量測系統10包括一處理器11、一衛星定位裝置12以及一慣性測量單元(Inertial Measurement Unit，IMU)13，該衛星定位裝置12以及該慣性測量單元13分別電性連接至該處理器11，其中，該慣性測量單元13至少包括一加速度計以及一角速度計，但不需要包括有磁力計或磁場感應元件。本實施例中，該衛星定位裝置12用於從一衛星定位系統接收一衛星定位訊號並產生一衛星定位資訊，該衛星定位系統可以是全球定位系統(Global Positioning System，GPS)、全球導航衛星系統(Global Navigation Satellite System，或稱格洛納斯(GLONASS)系統)、伽利略(Galileo)定位系統或北斗衛星導航系統等。該姿態量測系統10可以內建於交通工具，或是一可攜式導航裝置(Portable Navigation Device，PND)，亦可以整合於一電子裝置(例如手機或平板電腦)中。本實施例中，該姿態量測系統10內建於一交通載具20中進一步來說，該姿態量測系統10是 裝設於該交通載具20的一後輪軸21。本發明中，該交通載具20可以是任何前輪轉向的陸上載具。 "Figure 1" and "Figure 2" are schematic diagrams of an attitude measurement system according to an embodiment of the present invention. The attitude measurement system 10 includes a processor 11, a satellite positioning device 12 and an inertial measurement unit (Inertial Measurement Unit, IMU) 13, the satellite positioning device 12 and the inertial measurement unit 13 are electrically connected to the processor 11 respectively, wherein the inertial measurement unit 13 at least includes an accelerometer and an angular velocity meter, but does not need to include a magnetometer or Magnetic field sensing element. In this embodiment, the satellite positioning device 12 is used to receive a satellite positioning signal from a satellite positioning system and generate satellite positioning information. The satellite positioning system may be a Global Positioning System (GPS) or a Global Navigation Satellite System. (Global Navigation Satellite System, or GLONASS system), Galileo positioning system or Beidou satellite navigation system, etc. The attitude measurement system 10 can be built into a vehicle, a portable navigation device (PND), or integrated into an electronic device (such as a mobile phone or tablet computer). In this embodiment, the attitude measurement system 10 is built into a traffic vehicle 20. Further, the attitude measurement system 10 is Installed on a rear axle 21 of the transportation vehicle 20 . In the present invention, the transportation vehicle 20 may be any front-wheel steering land vehicle.

參閱『圖3』，『圖3』是根據本發明一實施例的姿態量測方法流程示意圖。根據本發明一實施例，先利用該衛星定位裝置12產生的該衛星定位資訊取得該交通載具20的一航跡角(Track angle或Course angle，亦可稱航向角)φ(步驟S1-1)，如『圖4』所示，該航跡角φ為該交通載具20的一行進方向和北方N之間的夾角，本例中，該航跡角φ以『圖4』中的順時針方向為正。接著，根據該慣性測量單元13的該加速度計量測到該交通載具20移動中的一加速度值進行計算(步驟S1-2)，取得該交通載具20移動中在體座標系下的一重力加速度向量，該重力加速度向量以下式(1)表示：

Figure 110125489-A0305-02-0006-33
Referring to "Fig. 3", "Fig. 3" is a schematic flow chart of an attitude measurement method according to an embodiment of the present invention. According to an embodiment of the present invention, the satellite positioning information generated by the satellite positioning device 12 is first used to obtain a track angle (Track angle or Course angle, also called a heading angle) φ of the transportation vehicle 20 (step S1-1) , as shown in "Figure 4", the track angle φ is the angle between a traveling direction of the traffic vehicle 20 and the north N. In this example, the track angle φ is the clockwise direction in "Figure 4". just. Then, based on the acceleration value measured by the accelerometer of the inertial measurement unit 13 while the transportation vehicle 20 is moving, calculation is performed (step S1-2), and an acceleration value of the transportation vehicle 20 in the body coordinate system during movement is obtained. Gravity acceleration vector, the gravity acceleration vector is expressed by the following formula (1):
Figure 110125489-A0305-02-0006-33

其中g x 、g y 、g z 為重力加速度在體座標系下之分量； 之後，根據下式(2)計算該交通載具20的一姿態資訊(步驟S1-3)：

Figure 110125489-A0305-02-0006-73
Among them, g x , g y , and g z are the components of gravity acceleration in the body coordinate system; then, calculate an attitude information of the transportation vehicle 20 according to the following formula (2) (step S1-3):
Figure 110125489-A0305-02-0006-73

其中，R為一旋轉矩陣，

Figure 110125489-A0305-02-0006-40
，
Figure 110125489-A0305-02-0006-74
，
Figure 110125489-A0305-02-0006-75
Figure 110125489-A0305-02-0006-35
，
Figure 110125489-A0305-02-0006-36
，A=l x cos φ，B=l x sin φ，C=g x ，D=g y ， E=g z ，
Figure 110125489-A0305-02-0006-37
。 Among them, R is a rotation matrix,
Figure 110125489-A0305-02-0006-40
,
Figure 110125489-A0305-02-0006-74
,
Figure 110125489-A0305-02-0006-75
Figure 110125489-A0305-02-0006-35
,
Figure 110125489-A0305-02-0006-36
, A = l x cos φ, B = l x sin φ, C = g x , D = g y , E = g z ,
Figure 110125489-A0305-02-0006-37
.

據此，經由R的結果，可以得知該交通載具20的體坐標系相對於一導航坐標系的旋轉，即為該交通載具20當前的三維姿態。如『圖5』所示，其中該體座標系為以該交通載具20的中心為原點，X軸為該交通載具20的一行進方向，Z軸為一垂直該交通載具20的向車頂平面的方向。本例中，而該導航坐標系為一東北天(ENU)坐標系，N軸與地球 表面相切且指向北極，E軸與地球表面相切且指向東，U軸垂直地球表面且指向天空。兩個座標系之間的三維旋轉關係，即為該交通載具20之三維姿態。 Accordingly, through the result of R , it can be known that the rotation of the body coordinate system of the transportation vehicle 20 relative to a navigation coordinate system is the current three-dimensional posture of the transportation vehicle 20 . As shown in "Figure 5", the body coordinate system takes the center of the transportation vehicle 20 as the origin, the X-axis is a traveling direction of the transportation vehicle 20, and the Z-axis is a direction perpendicular to the transportation vehicle 20. towards the roof plane. In this example, the navigation coordinate system is an ENU coordinate system. The N axis is tangent to the earth's surface and points to the North Pole. The E axis is tangent to the earth's surface and points to the east. The U axis is perpendicular to the earth's surface and points to the sky. The three-dimensional rotation relationship between the two coordinate systems is the three-dimensional posture of the transportation vehicle 20 .

上述的姿態量測方法可進一步應用於導航方法，在取得該交通載具20在三維空間的該姿態資訊後，除了可判斷該交通載具在二維平面上的方位之外，還可以得知該交通載具20在高度方向的方位，即該交通載具20是否行駛於具備坡度的平面或道路上以及斜度，從而更精準地推估該交通載具20所在的位置。 The above attitude measurement method can be further applied to the navigation method. After obtaining the attitude information of the transportation vehicle 20 in the three-dimensional space, in addition to judging the orientation of the transportation vehicle 20 on the two-dimensional plane, it can also be known The orientation of the transportation vehicle 20 in the height direction, that is, whether the transportation vehicle 20 is traveling on a plane or road with a slope and the slope, thereby more accurately estimating the location of the transportation vehicle 20 .

參閱『圖6』，『圖6』是根據本發明一實施例的導航方法流程示意圖，步驟S2-1至步驟S2-3與上述的步驟S1-1至步驟S1-3相同。本實施例中，除了利用式(1)以及式(2)求得該姿態數據(本實施例稱第一姿態數據)之外，還進一步地根據一第二姿態數據(步驟S2-4)計算該姿態資訊，該第二姿態數據是由該交通載具20移動中的一角速度值對時間積分而得到，而該角速度值是該慣性測量單元13的該角速度計量測得到。 Referring to "Fig. 6", "Fig. 6" is a schematic flow chart of a navigation method according to an embodiment of the present invention. Steps S2-1 to S2-3 are the same as the above-mentioned steps S1-1 to S1-3. In this embodiment, in addition to using equation (1) and equation (2) to obtain the posture data (referred to as the first posture data in this embodiment), it is further calculated based on a second posture data (step S2-4). The attitude information and the second attitude data are obtained by integrating an angular velocity value of the moving vehicle 20 over time, and the angular velocity value is measured by the angular velocity meter of the inertial measurement unit 13 .

此外，該姿態資訊還可以根據一第三姿態數據計算得到，該第三姿態數據可利用一光檢測和測距(LiDAR)裝置或一影像擷取裝置所得到的資訊而取得(步驟S2-5)。在步驟S2-6，該處理器11根據該第一姿態數據、該第二姿態數據以及/或該第三姿態數據計算得到。本實施例中，該姿態數據是利用一遞歸濾波器計算得到，例如採用一卡爾曼濾波器(Kalman filter)。接著，根據該姿態資訊提供一導航資訊。進一步來說，該第一姿態數據以及該第二姿態數據(或該第三姿態數據)輸入至該遞歸濾波器進行一數據融合(Data fusion)，係將當前時間的該姿態數據輸入該遞歸濾波器，得到下一時間的該姿態數據(即該姿態資訊)，並根據該姿態資訊提供該導航資訊(步驟S2-7)。當該交通載具20正在移動中， 利用該遞歸濾波器對來自於不同感測器的數據進行數據融合，從而預測移動中的該交通載具20在下一時間點的姿態進行，並更新該交通載具20的姿態。 In addition, the posture information can also be calculated based on a third posture data. The third posture data can be obtained using information obtained by a light detection and ranging (LiDAR) device or an image capture device (step S2-5 ). In step S2-6, the processor 11 calculates based on the first posture data, the second posture data and/or the third posture data. In this embodiment, the attitude data is calculated using a recursive filter, such as a Kalman filter. Then, provide navigation information based on the posture information. Furthermore, the first posture data and the second posture data (or the third posture data) are input to the recursive filter to perform a data fusion, which is to input the posture data at the current time into the recursive filter. The device obtains the attitude data (that is, the attitude information) at the next time, and provides the navigation information based on the attitude information (step S2-7). When the transportation vehicle 20 is moving, The recursive filter is used to fuse data from different sensors to predict the attitude of the moving vehicle 20 at the next time point and update the attitude of the vehicle 20 .

在一例子中，該第一姿態數據是根據該衛星定位裝置以及該慣性測量單元(例如該加速度計)的量測結果取得，該第二姿態數據是根據該慣性測量單元(例如該角速度計)的量測結果取得，若該衛星定位裝置、該加速度計及該角速度計的更新頻率為相異，舉例來說，該角速度計的更新頻率大於該加速度計以及該衛星定位裝置。 In one example, the first attitude data is obtained based on the measurement results of the satellite positioning device and the inertial measurement unit (such as the accelerometer), and the second attitude data is obtained based on the inertial measurement unit (such as the angular velocity meter) The measurement results are obtained if the update frequencies of the satellite positioning device, the accelerometer and the angular velocity meter are different. For example, the update frequency of the angular velocity meter is greater than that of the accelerometer and the satellite positioning device.

用從不同感測器取得不同更新頻率的數據，再進一步根據該些數據進行數據融合，以得到最為精準的一姿態預測值。再進一步地根據該姿態預測值預測下一個時間點的姿態，從而循環更新和預測的流程。 Data with different update frequencies is obtained from different sensors, and then data fusion is performed based on the data to obtain the most accurate posture prediction value. Furthermore, the attitude at the next time point is predicted based on the attitude prediction value, thereby cyclically updating and predicting the process.

本發明中，步驟S2-1至步驟S2-3、步驟S2-4以及步驟S2-5的順序可能有別於以上的描述或圖示的順序。舉例來說，可以先取得該第二姿態數據，再取得該第一姿態數據以及/或該第三姿態數據；亦可以先取得該第三姿態數據，再取得該第一姿態數據以及/或該第二姿態數據；或者可能同時取得該第一姿態數據、該第二姿態數據以及/或該第三姿態數據。 In the present invention, the order of step S2-1 to step S2-3, step S2-4 and step S2-5 may be different from the order described or illustrated above. For example, the second posture data can be obtained first, and then the first posture data and/or the third posture data can be obtained; the third posture data can also be obtained first, and then the first posture data and/or the third posture data can be obtained. second posture data; or the first posture data, the second posture data and/or the third posture data may be obtained simultaneously.

參閱『圖7』，『圖7』是根據本發明另一實施例的姿態量測系統示意圖，配合『圖6』所描述的方法，該姿態量測系統10還包括一光檢測和測距(LiDAR)裝置14以及一影像擷取裝置15，分別電性連接至該處理器11。 Referring to "Fig. 7", "Fig. 7" is a schematic diagram of an attitude measurement system according to another embodiment of the present invention. In conjunction with the method described in "Fig. 6", the attitude measurement system 10 also includes a light detection and ranging ( LiDAR device 14 and an image capture device 15 are electrically connected to the processor 11 respectively.

本發明描述的方法以及系統，可以在不採用磁力計的情況下，量測移動中交通載具在三維空間的姿態，以精準地定位交通載具在三維空間中的位置。此外，該慣性測量單元取得角速度的速度更快(約30Hz)，卻有資料漂移的問題，然而本發明將衛星定位取得航跡角結合該慣性測 量單元取得角速度，再輸入到遞歸濾波器，可以收斂出兼顧速度以及改善漂移問題的姿態結果。 The method and system described in the present invention can measure the attitude of a moving vehicle in three-dimensional space without using a magnetometer, so as to accurately locate the position of the vehicle in three-dimensional space. In addition, the inertial measurement unit obtains the angular velocity faster (about 30Hz), but has the problem of data drift. However, the present invention combines the satellite positioning to obtain the track angle with the inertial measurement unit. The angular velocity is obtained from the measurement unit and then input to the recursive filter, which can converge to an attitude result that takes into account the speed and improves the drift problem.

10:姿態量測系統 10: Attitude measurement system

11:處理器 11: Processor

12:衛星定位裝置 12: Satellite positioning device

13:慣性測量單元 13:Inertial measurement unit

20:交通載具 20:Transportation vehicles

21:後輪軸 21:Rear axle

Claims (7)

一種導航方法，應用於一交通載具，該方法包括以下步驟：利用一衛星定位裝置產生的一衛星定位資訊取得該交通載具的一航跡角φ；對一慣性測量單元量測到該交通載具的一加速度值進行計算而取得該交通載具在體座標系下的一重力加速度向量，該重力加速度向量以下式表示：
Figure 110125489-A0305-02-0010-44
，其中g x 、g y 、g z 為重力加速度在體座標系下之分量； 根據下式計算該交通載具的一第一姿態數據：
Figure 110125489-A0305-02-0010-47
 其中，
Figure 110125489-A0305-02-0010-48
，
Figure 110125489-A0305-02-0010-56
，
Figure 110125489-A0305-02-0010-51
，
Figure 110125489-A0305-02-0010-53
， A=l x cos φ，B=l x sin φ，C=g x ，D=g y ，E=g z ，
Figure 110125489-A0305-02-0010-54
； 以該慣性測量單元量測該交通載具的一角速度為時變量，並用該角速度對時間進行積分而取得一第二姿態數據；利用一遞歸濾波器，並至少根據該交通載具的該第一姿態數據以及該第二姿態數據計算該交通載具的一姿態資訊；以及根據該姿態資訊提供一導航資訊。 A navigation method, applied to a traffic vehicle, the method includes the following steps: using a satellite positioning information generated by a satellite positioning device to obtain a track angle φ of the traffic vehicle; measuring the traffic vehicle with an inertial measurement unit Calculate an acceleration value of the vehicle to obtain a gravity acceleration vector of the vehicle in the body coordinate system. The gravity acceleration vector is expressed by the following formula:
Figure 110125489-A0305-02-0010-44
, where g x , g y , g z are the components of gravity acceleration in the body coordinate system; calculate a first attitude data of the transportation vehicle according to the following formula:
Figure 110125489-A0305-02-0010-47
 in,
Figure 110125489-A0305-02-0010-48
,
Figure 110125489-A0305-02-0010-56
,
Figure 110125489-A0305-02-0010-51
,
Figure 110125489-A0305-02-0010-53
, A = l x cos φ, B = l x sin φ, C = g x , D = g y , E = g z ,
Figure 110125489-A0305-02-0010-54
; Using the inertial measurement unit to measure an angular velocity of the vehicle as a time variable, and integrating the angular velocity with time to obtain a second attitude data; using a recursive filter, and at least based on the third attitude of the vehicle; An attitude data and the second attitude data calculate an attitude information of the vehicle; and provide navigation information based on the attitude information. 如請求項1的導航方法，其中，該姿態資訊是由該第一姿態數據、該第二姿態數據以及一第三姿態數據計算得到，該第三姿態數據是利用一光檢測和測距(LiDAR)裝置得到。 The navigation method of claim 1, wherein the attitude information is calculated from the first attitude data, the second attitude data and a third attitude data, and the third attitude data is calculated using a light detection and ranging (LiDAR) ) device is obtained. 如請求項1的導航方法，其中，該姿態資訊是由該第一姿態數據、該第二姿態數據以及一第三姿態數據計算得到，該第三姿態數據是利用一影像擷取裝置得到。 The navigation method of claim 1, wherein the attitude information is calculated from the first attitude data, the second attitude data and a third attitude data, and the third attitude data is obtained by using an image capturing device. 一種導航系統，應用於一交通載具，該系統包括：一衛星定位裝置，裝設於該交通載具上，該衛星定位裝置用於從複數個衛星接收訊號後產生一衛星定位資訊；一慣性測量單元，裝設於該交通載具上，該慣性測量單元用於量測該交通載具的一加速度值以取得該交通載具在體座標系下的一重力加速度向量，該重力加速度向量以下式表示：
Figure 110125489-A0305-02-0011-57
，其中g x 、g y 、g z 為重力加速度在體座標系下之分量； 至少一處理器，該處理器被配置為：根據該衛星定位資訊計算該交通載具的一航跡角φ；根據下式計算該交通載具的一第一姿態數據：
Figure 110125489-A0305-02-0011-58
 其中，
Figure 110125489-A0305-02-0011-59
，
Figure 110125489-A0305-02-0011-60
，
Figure 110125489-A0305-02-0011-61
，
Figure 110125489-A0305-02-0011-62
， A=l x cos φ，B=l x sin φ，C=g x ，D=g y ，E=g z ，
Figure 110125489-A0305-02-0011-63
； 以該慣性測量單元量測該交通載具的一角速度為時變量，並用該角速度對時間進行積分而取得一第二姿態數據；利用一遞歸濾波器，並至少根據該交通載具的該第一姿態數據以及該第二姿態數據計算該交通載具的一姿態資訊；以及根據該姿態資訊提供一導航資訊。 A navigation system applied to a transportation vehicle. The system includes: a satellite positioning device installed on the transportation vehicle. The satellite positioning device is used to generate satellite positioning information after receiving signals from a plurality of satellites; an inertia A measuring unit is installed on the vehicle. The inertial measurement unit is used to measure an acceleration value of the vehicle to obtain a gravity acceleration vector of the vehicle in the body coordinate system. The gravity acceleration vector is as follows: Expression:
Figure 110125489-A0305-02-0011-57
, where g x , g y , g z are the components of gravity acceleration in the body coordinate system; at least one processor, the processor is configured to: calculate a track angle φ of the traffic vehicle according to the satellite positioning information; according to The following formula calculates a first attitude data of the transportation vehicle:
Figure 110125489-A0305-02-0011-58
 in,
Figure 110125489-A0305-02-0011-59
,
Figure 110125489-A0305-02-0011-60
,
Figure 110125489-A0305-02-0011-61
,
Figure 110125489-A0305-02-0011-62
, A = l x cos φ, B = l x sin φ, C = g x , D = g y , E = g z ,
Figure 110125489-A0305-02-0011-63
; Using the inertial measurement unit to measure an angular velocity of the vehicle as a time variable, and integrating the angular velocity with time to obtain a second attitude data; using a recursive filter, and at least based on the third attitude of the vehicle; An attitude data and the second attitude data calculate an attitude information of the vehicle; and provide navigation information based on the attitude information. 如請求項4所述的導航系統，其中還包括一光檢測和測距裝置，且該姿態資訊是由該第一姿態數據、該第二姿態數據以及一利用該光檢測和測距裝置得到的第三姿態數據計算得到。 The navigation system as claimed in claim 4, further comprising a light detection and ranging device, and the attitude information is obtained from the first attitude data, the second attitude data and a light detection and ranging device. The third attitude data is calculated. 如請求項4所述的導航系統，其中還包括一影像擷取裝置，且該姿態資訊是由該第一姿態數據、該第二姿態數據以及一利用該影像擷取裝置得到的第三姿態數據計算得到。 The navigation system according to claim 4, further comprising an image capture device, and the attitude information is composed of the first attitude data, the second attitude data and a third attitude data obtained by using the image capture device. calculated. 一種交通載具的姿態量測方法，包括以下步驟：利用一衛星定位裝置產生的一衛星定位資訊取得該交通載具的一航跡角φ；利用一慣性測量單元量測到該交通載具的一加速度值取得該交通載具在體座標系下的一重力加速度向量，該重力加速度向量以下式表示：
Figure 110125489-A0305-02-0012-64
，其中g x 、g y 、g z 為重力加速度在體座標系下之分量； 以及根據下式得計算該交通載具的一姿態數據：
Figure 110125489-A0305-02-0012-65
 其中，
Figure 110125489-A0305-02-0012-66
，
Figure 110125489-A0305-02-0012-67
，
Figure 110125489-A0305-02-0012-68
，
Figure 110125489-A0305-02-0012-69
， A=l x cos φ，B=l x sin φ，C=g x ，D=g y ，E=g z ，
Figure 110125489-A0305-02-0012-70
。 An attitude measurement method of a transportation vehicle includes the following steps: using a satellite positioning information generated by a satellite positioning device to obtain a track angle φ of the transportation vehicle; using an inertial measurement unit to measure an angle of the transportation vehicle The acceleration value obtains a gravity acceleration vector of the vehicle in the body coordinate system. The gravity acceleration vector is expressed by the following formula:
Figure 110125489-A0305-02-0012-64
, where g x , g y , g z are the components of gravity acceleration in the body coordinate system; and an attitude data of the transportation vehicle can be calculated according to the following formula:
Figure 110125489-A0305-02-0012-65
 in,
Figure 110125489-A0305-02-0012-66
,
Figure 110125489-A0305-02-0012-67
,
Figure 110125489-A0305-02-0012-68
,
Figure 110125489-A0305-02-0012-69
, A = l x cos φ, B = l x sin φ, C = g x , D = g y , E = g z ,
Figure 110125489-A0305-02-0012-70
.
TW110125489A 2021-07-12 2021-07-12 Attitude measurement method, navigation method and system of transportation vehicle TWI811733B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
TW110125489A TWI811733B (en) 2021-07-12 2021-07-12 Attitude measurement method, navigation method and system of transportation vehicle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
TW110125489A TWI811733B (en) 2021-07-12 2021-07-12 Attitude measurement method, navigation method and system of transportation vehicle

Publications (2)

Publication Number Publication Date
TW202303097A TW202303097A (en) 2023-01-16
TWI811733B true TWI811733B (en) 2023-08-11

Family

ID=86657906

Family Applications (1)

Application Number Title Priority Date Filing Date
TW110125489A TWI811733B (en) 2021-07-12 2021-07-12 Attitude measurement method, navigation method and system of transportation vehicle

Country Status (1)

Country Link
TW (1) TWI811733B (en)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN201697636U (en) * 2010-06-07 2011-01-05 长安大学 Automobile inertial navigation system
CN103759730B (en) * 2014-01-16 2016-06-29 南京师范大学 The collaborative navigation system of a kind of pedestrian based on navigation information two-way fusion and intelligent mobile carrier and air navigation aid thereof
US20180176741A1 (en) * 2016-12-15 2018-06-21 Gracenote, Inc. Dynamic content delivery based on vehicle navigational attributes
US20190236399A1 (en) * 2014-11-04 2019-08-01 The Regents Of The University Of California Visual-inertial sensor fusion for navigation, localization, mapping, and 3d reconstruction
CN107289951B (en) * 2017-07-31 2020-05-12 电子科技大学 Indoor mobile robot positioning method based on inertial navigation
US20200201359A1 (en) * 2018-12-19 2020-06-25 Joby Aero, Inc. Vehicle navigation system
CN106705968B (en) * 2016-12-09 2020-11-27 北京工业大学 Indoor inertial navigation algorithm based on attitude identification and step size model
CN109579853B (en) * 2019-01-24 2021-02-26 燕山大学 Inertial navigation indoor positioning method based on BP neural network
CN108731676B (en) * 2018-05-04 2021-06-15 北京摩高科技有限公司 Attitude fusion enhanced measurement method and system based on inertial navigation technology
US20210207961A1 (en) * 2020-01-06 2021-07-08 Qualcomm Incorporated Correction of motion sensor and global navigation satellite system data of a mobile device in a vehicle

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN201697636U (en) * 2010-06-07 2011-01-05 长安大学 Automobile inertial navigation system
CN103759730B (en) * 2014-01-16 2016-06-29 南京师范大学 The collaborative navigation system of a kind of pedestrian based on navigation information two-way fusion and intelligent mobile carrier and air navigation aid thereof
US20190236399A1 (en) * 2014-11-04 2019-08-01 The Regents Of The University Of California Visual-inertial sensor fusion for navigation, localization, mapping, and 3d reconstruction
CN106705968B (en) * 2016-12-09 2020-11-27 北京工业大学 Indoor inertial navigation algorithm based on attitude identification and step size model
US20180176741A1 (en) * 2016-12-15 2018-06-21 Gracenote, Inc. Dynamic content delivery based on vehicle navigational attributes
CN107289951B (en) * 2017-07-31 2020-05-12 电子科技大学 Indoor mobile robot positioning method based on inertial navigation
CN108731676B (en) * 2018-05-04 2021-06-15 北京摩高科技有限公司 Attitude fusion enhanced measurement method and system based on inertial navigation technology
US20200201359A1 (en) * 2018-12-19 2020-06-25 Joby Aero, Inc. Vehicle navigation system
CN109579853B (en) * 2019-01-24 2021-02-26 燕山大学 Inertial navigation indoor positioning method based on BP neural network
US20210207961A1 (en) * 2020-01-06 2021-07-08 Qualcomm Incorporated Correction of motion sensor and global navigation satellite system data of a mobile device in a vehicle

Also Published As

Publication number Publication date
TW202303097A (en) 2023-01-16

Similar Documents

Publication Publication Date Title
CN106289275B (en) Unit and method for improving positioning accuracy
EP2095148B1 (en) Arrangement for and method of two dimensional and three dimensional precision location and orientation determination
JP3375268B2 (en) Navigation device
US9026263B2 (en) Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis
CN109186597B (en) Positioning method of indoor wheeled robot based on double MEMS-IMU
JP5602070B2 (en) POSITIONING DEVICE, POSITIONING METHOD OF POSITIONING DEVICE, AND POSITIONING PROGRAM
CN106842271B (en) Navigation positioning method and device
JP2019074532A (en) Method for giving real dimensions to slam data and position measurement using the same
CN111562603B (en) Navigation positioning method, equipment and storage medium based on dead reckoning
JP2005283586A (en) Error correction of inertia navigation system
CN113203418B (en) GNSSINS visual fusion positioning method and system based on sequential Kalman filtering
CN110057356B (en) Method and device for positioning vehicles in tunnel
US20170074678A1 (en) Positioning and orientation data analysis system and method thereof
JP4941199B2 (en) Navigation device
CN108051839A (en) A kind of method of vehicle-mounted 3 D locating device and three-dimensional localization
Park et al. MEMS 3D DR/GPS integrated system for land vehicle application robust to GPS outages
JPH10267650A (en) Instrument for automatically surveying linearity of road
JP2004125689A (en) Position calculation system for self-contained navigation
JP2009204385A (en) Targeting device, method, and program
JP2006119144A (en) Road linearity automatic survey device
CN112985420A (en) Small celestial body attachment optical navigation feature recursion optimization method
TWI811733B (en) Attitude measurement method, navigation method and system of transportation vehicle
Kim et al. High accurate affordable car navigation using built-in sensory data and images acquired from a front view camera
CN111566443A (en) Method for estimating navigation data of a land vehicle using road geometry and direction parameters
US20230349699A1 (en) Absolute heading estimation with constrained motion