目標的方位測量的製作方法

2023-12-02 13:00:36

專利名稱：目標的方位測量的製作方法
技術領域：
本發明涉及目標的方位測量，具體地涉及用加速度計測量目標方位的方法和系 統。
背景技術：
可將三維加速度計裝附於各種目標上，並可按三維方式測量目標的加速度。作為 這些測量的一部分，加速度計測量由重力引起的作用在目標上的力。通過利用測量由重力引起的作用在目標上的力，加速度計能夠用作傾斜傳感器來 測量目標相對於地球的角方位。然而，由於加速度計不能區分由重力引起的力和由於非重力的「慣性力」引起的加 速度，所以，在大多數情況下，能夠確定所測得的加速度與重力的不符，但是僅從這些測量 結果不能確定目標的方位。此外，通常不可能通過對加速度計的測量結果求積分來估算(estimate)目標的 位置。這有如下三條理由首先，由於重力和「慣性力」所引起的總加速度的各個部分都是 未知的；第二，「慣性力」作用的方向相對於重力作用的方向是未知的；以及第三，必須兩次 積分加速度測量結果才能得到位置測量結果，而由於測量結果的漂移(drift)，這實際上是 不可能精確地實現的。眾所周知，利用三維加速度計測量目標傾斜與兩維或三維磁強計測量地球磁場的 組合，能測量或確定目標的方位。圖1示出這種系統的方框圖。系統2包括加速度計4和磁強計6，它們提供加速度 測量，並得出裝有加速度計4和磁強計6的目標的相應方向(heading)。該系統2還包括用於存儲目標方位的先前估算值(estimate) Q的寄存器或存儲器 8。該方位Q可按數學方式表示為四元數、歐拉角或者任何其它合適的方位表達。第一單元10對於在基於地球的參考系中由重力(矢量G)對目標引起的加速度提 供預期測量結果。該預期測量結果提供給第一變換單元11，第一變換單元11根據目標方位 的先前估算值Q計算來自加速度計4的預期測量結果。換言之，第一變換單元11將在基於 地球的參考系中與重力相關的矢量G轉換到相對於目標固定的參考系中。同樣，第二單元12對於在基於地球的參考系中的磁場(矢量M)提供預期測量結 果。該預期測量結果提供給第二變換單元13，第二變換單元13根據目標方位的先前估算 值Q計算來自磁強計6的預期測量結果。第二變換單元13也是將矢量M轉換到相對於目 標固定的參考系中。加法器14確定分別來自加速度計4和磁強計6的實際測量結果與它們的來自第 一變換單元11和第二變換單元13的預期值之間的差值。所得到的誤差信號提供給乘法器 15。在單元18中，通過對估算信號關於方位估算值Q求微分(differentiating)，計算 估算的加速度計4和磁強計6的信號對方位估算值的靈敏度矩陣。在逆變塊20中對靈敏
4度矩陣求逆(例如，通過偽逆變)。將逆變塊20的輸出提供給乘法器15。乘法器15將逆變塊20的輸出和來自加法 器14的誤差信號組合，產生校正值AQ。在更新器22中，將校正值A Q與方位的先前估算值Q組合，產生方位的新估算值 Q，存儲在寄存器或存儲器8中。當把方位估算值Q表示為四元數時，更新器22是稱為四元數乘法器的乘法器。當 使用旋轉矩陣表示方位估算值Q時，更新器是稱為矩陣乘法器的乘法器。然後該過程以迭代循環方式重複。如上所述，加速度計4給出的各測量結果隨目標方位的變化(即從目標和加速度 計的觀點沿不同方向的重力引力)而變化，同時也隨作用於目標的其它非重力的結果而變 化。加速度計4不能區分這些重力和「慣性力」。因此，為了在解決因目標運動引起的高頻 加速度力的同時跟蹤目標的快速旋轉，將三維陀螺儀加入到系統2中。圖2示出帶陀螺儀的系統。這裡的方位重構算法基本上與圖1的相同，其差別是 現在還用來自陀螺儀24的測量結果更新方位估算值Q。特別是為了角速度測量結果與角方 位Q的正確積分，在第二更新器28中與更新器22的輸出組合之前，在第二乘法器26中，用 採樣周期dt乘陀螺儀24的測量結果。在將校正值A Q提供給第一更新器22之前，還通過第三乘法器30用因子K (其中 K << 1)加權校正值AQ。然而，使用陀螺儀的缺點是，它們相當昂貴、體積大，而且功耗大。因此，在使用加速度傳感器和磁強計來測量目標方位的系統中，當加速度是由除 作用在目標上的重力以外的力所引起時，這種估算的目標方位將是不準確的。比如，考慮一桶水圍繞水平軸迴轉。假定桶以足夠快的速度迴轉，當桶在迴轉的頂 部被倒置時水不會落下，因為作用在水上的重力和慣性力的合力將向上，而當桶在迴轉的 底部時，作用在水上的重力和慣性力的合力將向下。如果把加速度傳感器放置在桶中，由於方位重構算法期望所測加速度對應於重力 (重力總是作用向下)而不考慮因桶迴轉所引起的加速度力(在所有現有技術的系統，如圖 1和2所示系統中都是如此)，所以當桶在迴轉的頂部時，估算的方位Q是相當不準確的。如上所述，為了避免這種問題，在該系統中可以包含陀螺儀。然而，陀螺儀具有漂 移，這意味著陀螺儀測量結果的低頻分量(DC)可能是不可靠的。因此，在包含陀螺儀的系 統中，在利用圖2所示加速度計和磁強計的組合的同時，要增加低頻反饋迴路，以便補償陀 螺儀測量結果的漂移。如果作用在目標上的加速度力是瞬時的(例如，桶僅迴轉幾秒鐘)，這些現有技術 系統適用。然而，陀螺儀的漂移和加速度計的失敗補償意味著如果目標加速度持續超過幾 秒鐘，則方位估算可能是完全錯誤的。因此，需要改善用加速度計測量或估算目標方位的方法和系統。

發明內容
因此，本發明提供一種目標方位測量系統，用於改善裝有該系統的目標的方位的 第一估算值準確度，該系統包括用於測量目標的第一加速度的加速度計；提供目標的位置或速度的估算單元；以及處理裝置，用於將目標的第一加速度和目標的位置或速度進行 比較，以形成校正信號，並用於將校正信號應用到目標的方位的第一估算值，以產生目標的 方位的第二估算值。根據本發明的第二方面，提供一種用於改善目標方位的第一估算值準確度的方 法，該方法包括測量目標的第一加速度；提供目標的位置或速度；將目標的第一加速度和 目標的位置或速度進行比較，以形成校正信號；以及將校正信號應用於目標方位的第一估 算值，以產生目標方位的第二估算值。


現在僅僅通過實例參考下面的附圖描述本發明，其中圖1示出使用加速度計和磁強計的測量結果來估算目標方位的現有技術系統；圖2示出使用加速度計、磁強計和陀螺儀的測量結果來估算目標方位的現有技術 系統；圖3示出本發明第一實施例用於測量目標方位的系統；圖4示出本發明第二實施例用於測量目標方位的系統；以及圖5示出本發明第一實施例用於測量目標方位的另一系統。
具體實施例方式本發明通過對於目標的運動補償由加速度計所做的測量來改善如圖1所示的系 統。按照這樣的方式，就能夠識別因重力和非重力所引起的由加速度計所做測量結果的各 個部分。現在參照附圖3描述本發明的第一實施例。目標方位測量系統32包括加速度計 34和磁強計36，加速度計34和磁強計36對於裝有它們的目標的各處地球磁場，提供加速 度的測量結果和方向(heading)。下文中將加速度計34對目標的加速度測量結果稱作「第 一」加速度。系統32還包括用於存儲目標方位的先前估算值Q的寄存器或存儲器38。該方位 Q可按數學方式表示為四元數、歐拉角或其它合適的方位表達。在該實施例中，第一計算單元40在基於地球(或至少基於非目標)的參考系中為 由於重力和非重力引起的目標的加速度提供預期測量結果，因此，與圖1所示的系統不同， 通過考慮重力和其它的目標的加速度來計算預期測量結果。第一計算單元40包括估算單元41，用於估算因作用在目標上的非重力引起的目 標加速度。估算單元41包括用於測量目標位置P的位置傳感器42。位置傳感器可以是任 何合適的傳感器，例如像全球定位系統接收器、雷射跟蹤系統、視覺跟蹤系統、進行電容性 測量或電感性測量的一個或多個傳感器的這樣一類的傳感器。除全球定位系統接收器之 外，這些位置測量系統測量目標相對於具有已知位置的一個參考點或多個參考點的位置。來自位置傳感器42的測量結果被相繼設置的微分塊44和46關於時間兩次微分， 並提供給第一加法器48。因而，微分塊46的輸出將表示因作用在目標上的非重力所引起的 目標加速度。該部分目標加速度在下文中稱為「第二」加速度。應能理解，可以由一個以單一操作來計算位置測量結果P的二階微分的模塊代替
6順序設置的微分塊44和46。此外，還應能夠理解，可以測量目標的速度(或許是從位置測量結果得到速度，或 者相反地是直接測量速度)，並且通過對速度測量結果一次微分來計算目標的加速度。第一計算單元40還包括第一存儲單元50，第一存儲單元50存儲矢量G，矢量G表 示因地球坐標(或其它基於非目標的)參考系中的重力所引起的加速度，並將矢量G提供 給第一加法器48。這部分目標加速度在下文中稱為「第三」加速度。第一加法器48組合模塊46以及第一存儲器50的輸出(第二和第三加速度)，以 產生目標加速度的淨值(net value) 0將第一加法器48的輸出與目標方位的先前估算值Q—道提供給第一變換塊52，第 一變換塊52將第一加法器48的輸出轉換到目標參考系，從而為特定的目標方位Q確定來 自加速度計34的預期測量結果。設置第二存儲單元54和第二變換單元56，根據目標方位的先前估算值Q和表示地 球磁場的矢量M計算來自磁強計36的預期測量結果。第二加法器58分別接收來自第一和第二計算單元40、54的加速度計34和磁強計 36的預期測量結果，並接收來自加速度計34和磁強計36的實際測量結果，確定實際測量結 果和它們的期望值之間的差值。該差值是個誤差信號，並將該差值提供給第一乘法器60。在單元64中，通過對估算信號關於方位估算值Q求微分來計算估算的加速度計34 和磁強計36的信號對方位估算值的靈敏度矩陣。在逆變塊66中，對所述靈敏度矩陣求逆 (例如通過偽逆變)。逆變塊66的輸出提供給第一乘法器60。第一乘法器60將逆變塊66的輸出與來 自第二加法器58的誤差信號組合，產生校正值AQ。在更新器68中，將校正值A Q與方位的先前估算值Q組合，產生存儲在寄存器或 存儲器38中的方位的新估算值Q。如前面所述的那樣，當把方位估算值Q表示為四元數時，更新器68是稱為四元數 乘法器的乘法器。當使用旋轉矩陣表示方位估算值Q時，更新器是稱為矩陣乘法器的乘法
o然後，系統32以迭代循環方式重複該過程，以便更新方位估算值Q，這是因為目標 響應在目標上的力而運動。通過用目標位置的二階導數或目標速度的一階導數補償預期的加速度測量結果， 使所預期的目標加速度測量值更加準確，以此結果，改善了目標方位的估算值。此外，這樣 的改善足夠顯著地減少或者甚至是免除了在該系統中要包括陀螺儀的需求。本發明的這一實施例特別適合用於各種運輸方式，例如，小汽車，公共汽車，火車， 船，飛機和直升機，以致系統34能確定交通工具行駛的方向，或者本發明的這一實施例特 別適合在飛機或直升機中提供人工地平儀。本發明還適合於室內應用，例如測量人體一部 分的方位。然而，如果在系統32中包括了陀螺儀，例如通過包括如圖2所示的附加部件24-30 而在系統32中包括了陀螺儀，那麼與現有技術相比進一步改善了方位估算的準確度。在有些目標方位測量系統的應用中，眾所周知的是，目標的所有運動都起因於旋 轉，例如在康復應用中人體的上肢和下肢。這意味著由目標方位的變化而引發所有的「慣性」力。在這種情況下，可用加速度計使這些加速度因所述測量結果的預期值而得到補償。下面將參考附圖4描述適合這類應用的本發明的第二實施例。目標方位測量系統72的許多部分與附圖3所示系統32的許多部分對應。對於系 統72與系統32相同的部件已經給出相同的附圖標記，並且下面不再描述這些部件。在系統72中，第一計算單元80在基於地球(或至少基於非目標)的參考系中為 因重力和非重力所引起的目標加速度提供預期測量結果。與第一實施例一樣，通過考慮重 力和其它的目標的加速度來計算加速度計34的預期測量結果。然而，在該實施例中，不是 使用位置傳感器測量目標的位置，而是使用目標的體模型(body model)和目標方位的估算 值Q來計算目標的位置P。因而，第一計算單元80包括估算單元81，估算單元81本身包括體模型82，體模型 82包含多個目標運動方程，作為目標方位的函數。於是，體模型使目標的特定方位與特定位 置相連結。將體模型82與目標方位的先前估算值Q —道提供給第三變換單元84。第三變換單元84使用目標方位的先前估算值Q計算目標的位置P(如方框86所 示)°然後，算得的位置P被相繼設置的微分塊88和90關於時間微分兩次，並提供給第 一加法器92。因而，微分塊90的輸出將表示因作用在目標上的非重力所引起的目標加速 度。如前所述，這部分目標加速度在下文中稱為「第二」加速度。此外，還應該理解，可以由一個模塊代替順序設置的微分塊88和90，所述這一個 模塊以單一操作計算所算得的位置P的二階微分。第一計算塊80還包括第一存儲單元94，第一存儲單元94存儲表示因地球坐標 (或其它基於非目標的)參考系的重力所引起的目標加速度的矢量G，並將矢量G提供給第 一加法器92。第一加法器92組合模塊90和第一存儲單元94的輸出(第二和第三加速度)，得 出目標加速度的淨值。將第一加法器92的輸出與目標方位的先前估算值Q —道提供給第一變換塊52，第 一變換塊52將第一加法器92的輸出轉換到目標參考系，從而為特定的目標方位Q確定來 自加速度計34的預期測量結果。這之後，系統72計算誤差信號和校正值，與上面對附圖3的描述一樣。此外，通過用目標位置的二階導數或目標速度的一階導數來補償所述預期的加速 度測量結果，該預期的測量結果值更加準確，以此使目標方位的估算值得到改善。另外，在系統72中可以包括陀螺儀，與附圖2所示的一樣，相對於現有技術進一步 改善了方位估算的準確度。所描述的系統72還可以改善目標的方位測量，甚至在使用各種複雜的體模型時 也是如此。這樣的體模型會包括鉸接的目標的模型(如胸腔、上臂、下臂和手)。在一個實 施例中，在體模型中被識別的目標的不同部分可以各自具有附著於其上的加速度計和磁強 計，用於測量目標的該部分(例如，可以對上臂和下臂分開測量)。在一個實施例中，在同一迭代過程期間，系統72能估算目標的所有部分的方位， 這意味著該系統中的每個部件能處理目標所有部分的估算方位和測量結果。然而，該種方 案的缺點是，對於具有許多鉸接的目標的較大體模型，計算靈敏度矩陣在計算強度方面將是密集的。可供選擇的是，可以採用分級體模型，其中假定「母肢(parentlimbs)，，的方位估 算獨立於「子肢(child limbs)」的測量(例如，臂的上部的方位估算與臂的下部的測量無 關)。在該種情況下，靈敏度矩陣應該僅包括在該迭代期間所考慮的那部分目標的運動所引 起的那些加速度。例如，假設在人體臂的上部和下部有傳感器裝置，而且要確定的是整個臂的姿勢。 在該種情況下，下臂的體模型應該包括如下的加速度力因肩關節的角加速度引起的力，因 肩關節的角速度引起的離心力，因肘關節的角加速度引起的力，以及因肘關節的角速度引 起的離心力。然而，靈敏度矩陣應該僅包括Q的靈敏度，Q的靈敏度是因肘關節角加速度引 起的力和因肘關節角速度引起的離心力的函數，而不是與肩有關的力的函數。應該理解，在附圖3和4的系統中，把對非重力的補償加給重力，以產生加速度計 34的測量結果的預期值。然而，可以理解的是，可以代之以從加速度計34所做的實際測量 結果中減去所述補償。附圖3和4示出的每個系統所採用的方案都是利用估算的方位Q將預期的加速度 計和磁強計的測量結果轉換到目標參考系中。然而，也可以採用這樣的方案使用估算的方位Q將加速度計和磁強計的實際測 量結果轉換到地球坐標中(而不是在目標參考系中)，隨後用於利用地球坐標中期望的力 和場來計算所述校正信號。附圖5示出如何修改本發明的第一實施例來實現這樣的方案。此外，目標方位測量系統102的許多部分與附圖3所示系統3的許多部分對應。 對系統102與系統32相同的部件已經給出了相同的附圖標記，並且下面就不再描述這些部 件。因此，在系統102中，使用位置傳感器42測量的位置的二階微分和存儲在存儲器 50中的矢量G計算加速度計34的預期測量結果。該預期測量結果(在地球坐標參考系中) 直接提供給靈敏度矩陣62和第二加法器58。同樣地，由存儲在存儲器104中的矢量M(在地球坐標參考系中)表示的磁強計的 預期測量結果，也直接提供給靈敏度矩陣62和第二加法器58。在該實施例中，分別用使用方位的先前估算Q的第一和第二變換單元106和108， 將加速度計34和磁強計36的測量結果(在目標參考系中測得)轉換到地球坐標參考系中。如果希望，可以按照類似方式修改本發明的第二實施例。對本發明的進一步修改可以包括由(從位置傳感器、體模型或另外的得到的)位 置測量結果計算由於非重力所引起的估算的加速度，使用該估算的加速度形成校正信號。 代替對位置測量結果的雙重微分，現在需要對來自第一存儲單元的G的值和來自加速度計 的測量結果進行兩次積分。儘管主要是關於硬體描述了本發明，但是，應該理解，能用軟體容易地實現該系統 的一個或多個部件。因此，描述了一種使用加速度計確定目標方位的系統和方法。儘管在附圖和前面的說明中詳細地描述和說明了本發明，但是認為這樣的描述和 說明是示意性或示範性的，並且是非限制性的；本發明不限於這裡公開的實施例。
9
在實施要求保護的本發明時，本領域技術人員通過研究附圖、公開的內容和所附 的權利要求，能理解和實現對所公開的實施例的變化。在權利要求書中，術語「包括」並不 排除其它元件或步驟，以及不定冠詞不排除多個。信號處理器或其它單元，可以實現在權利 要求書中限定的幾項功能。在相互不同的從屬權利要求中限定的某些措施的事實並不表明 這些措施的組合不能有利被使用。不應該認為在權利要求書中的任何附圖標記限制本發明 的範圍。電腦程式可以存儲/分配在合適介質上，例如以其它硬體一起或作為其它硬體 的一部分而提供的光學存儲介質或固態介質，還可以以其它形式分配電腦程式，例如，通 過網際網路或其它有線或無線的電信系統。
權利要求
一種目標方位測量系統(32；72；102)，用於改善裝有該系統的目標的方位的第一估算值準確度，該系統包括用於測量目標的第一加速度的加速度計(34)；提供目標的位置或速度的估算單元(41；81)；以及處理裝置(58，60，64，68)，用於將第一加速度和估算單元的輸出進行比較，以形成校正信號，以及用於將校正信號應用到目標的方位的第一估算值，以產生目標的方位的第二估算值。
2.如權利要求1所述的目標方位測量系統，其中，估算單元(41)包括用於測量目標位 置的位置或速度傳感器(42)。
3.如權利要求2所述的目標方位測量系統，其中，位置或速度傳感器(42)包括如下之 一全球定位系統接收器，雷射跟蹤系統，視覺跟蹤系統，進行電容性測量或電感性測量的 傳感器。
4.如權利要求1所述的目標方位測量系統，其中，估算單元(81)包括用於目標的體模 型(82)。
5.如權利要求4所述的目標方位測量系統，其中，體模型(82)包含用於目標的一個或 多個運動方程。
6.如權利要求4或5所述的目標方位測量系統，其中，估算單元(81)適於用體模型 (82)使用目標的方位的第一估算值來估算目標的位置或速度。
7.如前面權利要求中任一項所述的目標方位測量系統，其中，估算單元(41；81)提供 目標的位置，並且所述估算單元進一步還包括微分塊(44，46 ；88，90)，微分塊(44，46 ；88， 90)關於時間對位置兩次微分，以產生目標的第二加速度。
8.如權利要求1至6的任一項所述的目標方位測量系統，其中，估算單元(41；81)提 供目標的速度，並且所述估算單元進一步還包括微分塊，微分塊關於時間對位置微分一次， 以產生目標的第二加速度。
9.如權利要求1至6的任一項所述的目標方位測量系統，其中，估算單元(41；81)提 供目標的位置，並且所述系統進一步還包括積分塊，積分塊關於時間對目標的第一加速度 兩次積分，以產生目標位置的估算值。
10.如權利要求1至6的任一項所述的目標方位測量系統，其中，估算單元(41；81)提 供目標的速度，並且所述系統進一步還包括積分塊，積分塊關於時間對目標的第一加速度 積分一次，以產生目標的速度的估算值。
11.如前面權利要求中任一項所述的目標方位測量系統，該系統進一步地包括一個或 多個變換塊(52，56 ；106，108)，用於將相應的測量結果或估算值從相對於目標為不固定的 參考系轉換到相對於目標為固定的參考系，或者反之亦然。
12.如權利要求11所述的目標方位測量系統，其中，一個或多個變換塊(52，56;106， 108)使用目標的方位的第一估算值轉換相應的測量結果或估算值。
13.如權利要求11或12所述的目標方位測量系統，其中，一個或多個變換塊(52，56; 106，108)起作用，將測量結果和/或估算值轉換到公共參考系。
14.如前面權利要求中任一項所述的目標方位測量系統，其中，處理裝置適於將第一加 速度、估算單元的輸出以及因重力引起的目標加速度的值進行比較，形成校正信號。
15.如權利要求14所述的目標方位測量系統，其中，從存儲單元(50;94)將因重力引 起的目標加速度值提供給處理裝置。
16.如權利要求14或15所述的目標方位測量系統，進一步包括括變換塊(52)，用於將 因重力引起的目標加速度值從相對於目標為不固定的參考系轉換到相對於目標為固定的 參考系。
17.如前面權利要求中任一項所述的目標方位測量系統，進一步包括磁強計(36)，磁 強計(36)在相對於目標為固定的參考系中測量磁場的強度和/或方向。
18.如權利要求17所述的目標方位測量系統，進一步包括存儲單元(54;104)，存儲單 元(54;104)存儲在相對於目標為不固定的參考系中的磁場的預定強度和/或方向。
19.如權利要求18所述的目標方位測量系統，進一步包括變換塊(56)，變換塊(56)將 測量的或預定的強度和/或方向轉換到公共參考系。
20.如權利要求18或19所述的目標方位測量系統，其中，處理裝置適於在形成校正信 號中將測量的和預定的強度和/或方向進行比較。
21.如前面權利要求中任一項所述的目標方位測量系統，其中，用四元數或歐拉角表示 目標方位的估算值。
22.一種用於改善目標方位的第一估算值準確度的方法，該方法包括測量目標的第一加速度；提供目標的位置或速度；將第一加速度和目標的位置或速度進行比較，形成校正信號；以及將校正信號應用於目標方位的第一估算值，產生目標方位的第二估算值。
全文摘要
本發明提供一種目標方位測量系統，用於改善裝有該系統的目標的方位的第一估算值準確度，該系統包括用於測量目標的第一加速度的加速度計；提供目標的位置或速度的估算單元；以及處理裝置，用於將第一加速度和估算單元的輸出進行比較，以形成校正信號，並且還用於將校正信號應用到目標的方位的第一估算值，以產生目標的方位的第二估算值。
文檔編號G01C25/00GK101855517SQ200880115659
公開日2010年10月6日 申請日期2008年11月11日 優先權日2007年11月13日
發明者N·拉姆伯特, V·M·G·范阿克特 申請人:皇家飛利浦電子股份有限公司