JP7250940B2 - Method for state estimation of the position and orientation of multiple movable modules of a common system - Google Patents
Method for state estimation of the position and orientation of multiple movable modules of a common system Download PDFInfo
- Publication number
- JP7250940B2 JP7250940B2 JP2021544239A JP2021544239A JP7250940B2 JP 7250940 B2 JP7250940 B2 JP 7250940B2 JP 2021544239 A JP2021544239 A JP 2021544239A JP 2021544239 A JP2021544239 A JP 2021544239A JP 7250940 B2 JP7250940 B2 JP 7250940B2
- Authority
- JP
- Japan
- Prior art keywords
- modules
- vector
- pair
- orientation
- wnref
- Prior art date
- Legal status (The legal status 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 status listed.)
- Active
Links
Images
Classifications
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
- E02F9/26—Indicating devices
- E02F9/264—Sensors and their calibration for indicating the position of the work tool
- E02F9/265—Sensors and their calibration for indicating the position of the work tool with follow-up actions (e.g. control signals sent to actuate the work tool)
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/28—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets
- E02F3/36—Component parts
- E02F3/42—Drives for dippers, buckets, dipper-arms or bucket-arms
- E02F3/43—Control of dipper or bucket position; Control of sequence of drive operations
- E02F3/435—Control of dipper or bucket position; Control of sequence of drive operations for dipper-arms, backhoes or the like
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
- E02F9/20—Drives; Control devices
Landscapes
- Engineering & Computer Science (AREA)
- Mining & Mineral Resources (AREA)
- Civil Engineering (AREA)
- General Engineering & Computer Science (AREA)
- Structural Engineering (AREA)
- Mechanical Engineering (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
- Operation Control Of Excavators (AREA)
- Navigation (AREA)
Description
本発明は、共通システムの関節を介して相互に可動な複数のモジュールの位置及び配向を状態推定するための方法に関し、ここでは、可動モジュール及び関節の運動学的関係が状態推定に導入される。さらに、本発明は、計算機上において実行されるときに方法の各ステップを実施するためのコンピュータプログラム、及び、当該コンピュータプログラムが記憶されている機械可読記憶媒体に関する。最後に、本発明は、本発明に係る方法を実施するように構成された電子制御装置に関する。 The present invention relates to a method for state estimation of the position and orientation of a plurality of modules mutually movable via joints of a common system, wherein the kinematic relationships of the movable modules and the joints are introduced into the state estimation. . Furthermore, the invention relates to a computer program for performing the steps of the method when running on a computer, and a machine-readable storage medium on which the computer program is stored. Finally, the invention relates to an electronic control device arranged to implement the method according to the invention.
従来技術
今日、作業機械の分野における自動化は、急速に進展している。作業機械及びその工具を自動化するためには、状態推定によって作業機械及びその工具の位置及び配向を認識する必要がある。関節を介して相互に接続され相互に可動な複数のモジュールの位置及び配向は、モジュール上に配置された複数の慣性センサを用いて求められる。
PRIOR ART Today, automation in the field of working machines is progressing rapidly. In order to automate a work machine and its tools, state estimation needs to know the position and orientation of the work machine and its tools. The position and orientation of multiple mutually movable modules interconnected via joints are determined using multiple inertial sensors located on the modules.
図1には、作業機械の例として、下部構造体Uと上部車体L1とを備えた掘削機1が示されており、ここで、上部車体L1は、第1の関節J1を介して下部車体Uに対して水平方向において回転可能である。上部構造体L1には、第1のセンサユニットS1が配置されており、当該第1のセンサユニットS1は、位置固定された基準座標系に対する上部構造体の線形加速度及び/又はヨーレートを測定する慣性センサ、並びに、当該箇所における地磁気を測定する磁力計を有している。掘削機1は、さらなるリンクブームL2、調整ブームL3及びステムL4並びにバケット3又はL5を有する掘削機アーム2を備える。ブームL2は、第2の関節J2を介して掘削機1の上部車体L1に接続され、調整ブームL3は、第3の関節J3を介してブームL2に接続され、ステムL4は、第4の関節を介して調整ブームL3に接続され、バケットL5は、第5の関節J5を介してステムL4に接続されている。各リンクL1,L2,L3,L4,L5には、それぞれ、慣性センサ及び磁力計を有するセンサユニットS1,S2,S3,S4,S5が配置されている。慣性センサは、位置固定された基準座標系に対する各センサの線形加速度及びヨーレートを測定する加速度センサ及びヨーレートセンサである。さらに、センサユニットS1,S2,S3,S4,S5は、それぞれ各関節J1,J2,J3,J4,J5の関節角度を測定する関節角度センサを有し得る。掘削機1が存在しているグローバル座標系は、符号Wによって表されている。
FIG. 1 shows, as an example of a working machine, an
上部車体L1及びブームL2について状態推定するためのそれ自体公知の方法は、図2に示され、この図に基づき、以下において簡単に説明される。 A method known per se for estimating the state of the upper body L1 and the boom L2 is shown in FIG. 2 and will be briefly explained below on the basis of this figure.
下部車体Uについて、変換行列Twuとそこから導出可能な単位四元数qwuとが提供され(10)、これらは、グローバル座標系Wにおける下部車体Uの位置及び配向を示す。同時に又は相前後して、センサユニットS1,S2(図2においては示されていないが、さらなるセンサS3,S4,S5も)、より正確には慣性センサ及び磁力計は、測定信号を受け取る(20,30)。測定信号として、符号ωにより示される角速度、符号aにより示される加速度、及び、符号mにより示される磁場が受け取られる。図2において、左側のインデックスは、それぞれの測定値がどの基準座標系において受け取られたかを示す。上部車体L1について、測定された角速度S1ωmess、測定された加速度S1amess、及び、測定された磁場S1mmessは、フィルタ21を通過し、これにより、特に、グローバル座標系Wに対する上部車体L1の旋回を表す四元数qW,L1及び上部車体L1の推定角速度L1ωestが求められる。第1の関節J1について、ここでは、上部車体L1の四元数qW,L1と下部車体Uの単位四元数とを用いて、第1の関節角度θ1が求められ(40)、次いで、そこから引き続き、下部車体Uと上部車体L1との間の遷移についての変換行列TU,L1が求められる(41)。最後に、グローバル座標系Wと上部車体L1との間の遷移についての変換行列TW,L1を得るために、下部車体Uについての変換行列TW,Uと、下部車体Uと上部車体L1との間の遷移についての変換行列TU,L1との行列乗算42が行われる。
For the underbody U, a transformation matrix T wu and a derivable unit quaternion q wu are provided (10), which describe the position and orientation of the underbody U in the global coordinate system W. Simultaneously or one after the other, sensor units S1, S2 (also further sensors S3, S4, S5, but not shown in FIG. 2), more precisely inertial sensors and magnetometers, receive measurement signals (20 , 30). As measurement signals, an angular velocity, denoted by ω, an acceleration, denoted by a, and a magnetic field, denoted by m, are received. In FIG. 2, the index on the left indicates in which reference coordinate system each measurement was received. For the upper body L1, the measured angular velocity S1 ω mess , the measured acceleration S1 a mess and the measured magnetic field S1 m mess are passed through a
同様の方法により、掘削機アーム2のブームL2について、測定された角速度S2ωmess、測定された加速度S2amess、及び、測定された磁場S2mmessが、第2のセンサS2から受け取られ(30)、フィルタ31を通過し、これによって、特に、グローバル座標系Wに対するブームL2の旋回を表す四元数qW,L2及びブームL2の推定角速度L2ωestが求められる。第2の関節J2について、ここでは、ブームL2の四元数qW,L2と上部車体L1の四元数qW,L1とを用いて、第2の関節角度θ2が求められる(50)。そこから引き続き、ロボットアームのいわゆる順運動学と同様に、関節アームの、例えばDenavit-Hartenbergパラメータのように既知であると想定される運動パラメータを使用して、上部車体L1とブームL2との間の遷移についての変換行列TL1,L2が求められる(51)。最後に、グローバル座標系WとブームL2との間の遷移についての変換行列TW,L2を得るために、グローバル座標系Wと上部車体L1との間の遷移についての変換行列TW,L1と、上部車体L1とブームL2との間の遷移についての変換行列TL1,L2との行列乗算52が行われる。
In a similar manner, for the boom L2 of the
さらなるリンクについても、この方法は、同様にさらに続けることができる。 For further links, the method can continue further as well.
静止した機械についての関節角度から順運動学を計算するための方法の詳細な説明については、例えば、Spong、Mark W.、Seth Hutchinson及びMathukumalli Vidyasagarによる論文「Robot modeling and control, Vol.3. New York:Wiley,2006」が挙げられ、その限りにおいては、これが参照される。 For a detailed description of the method for computing forward kinematics from joint angles for stationary machines, see, for example, the paper by Spong, Mark W., Seth Hutchinson and Mathukumalli Vidyasagar, Robot modeling and control, Vol. York: Wiley, 2006", to which reference is made to this extent.
本発明の開示
典型的には、質量の大きい金属を有する作業機械の場合、磁力計の測定値は、質量の大きい金属と、それにより発生する磁場とによって、これらの測定値が往々にして使いものにならなくなり又は少なくとも信頼性が低くなる程度に変化する。なお、個々の磁力計の測定値が異なる影響を受ける可能性もあり、そのため、それぞれのモジュール又はリンクの状態推定値が相互にばらばらになり、これによって、状態推定において運動学的には不可能なモジュール又はリンクの配向及び/又は位置の構成が発生する。例として、1つの関節を介して接続された2つのモジュールについての状態推定は、運動学的には除外されるべきであるにもかかわらず、それぞれ異なるヨー角を示す。本発明の枠内においては、モジュールとは、リンクを意味するものと理解することができる。
DISCLOSURE OF THE INVENTION Typically, for work machines having heavy metals, the magnetometer measurements are often unusable due to the heavy metals and the magnetic fields generated thereby. or at least change to such an extent that it becomes unreliable. It should also be noted that the individual magnetometer measurements may be affected differently, causing the state estimates of each module or link to disjoint from each other, making state estimation kinematically impossible. Various module or link orientations and/or positional configurations occur. As an example, the state estimates for two modules connected via one joint show different yaw angles, even though kinematically they should be excluded. Within the framework of the present invention, a module can be understood to mean a link.
基準ベクトルと「測定された」ベクトルとからなる少なくとも1つの対を求めることが提案され、この少なくとも1つの対は、関節の少なくとも1つと、関節に接続された2つのモジュールとの運動学的関係を表す。ここではせいぜい間接的にしか測定を実施することができず、それどころか前述のベクトルを求めることは、むしろいわゆる仮想測定とみなすことができるので、少なくとも1つのベクトルは、仮想ベクトルとも称される。従って、このベクトル対は、モジュールの状態推定に導入される。運動学的関係は、特に、構成要素の限界を表す運動学的制約条件(英語;kinematic constraints)であるものとしてもよい。そのため、関節は、典型的には最大角度までしか回転することができず、固定モジュールは、重ねることができない。 It is proposed to determine at least one pair consisting of a reference vector and a "measured" vector, the at least one pair representing the kinematic relationship between at least one of the joints and the two modules connected to the joint. represents At least one vector is also referred to as a virtual vector, since here the measurements can be carried out at most indirectly, and the determination of the aforementioned vectors can even be regarded as a so-called virtual measurement. This vector pair is therefore introduced into the module's state estimation. The kinematic relationships may in particular be kinematic constraints that describe the limits of the components. As such, joints can typically only be rotated to a maximum angle and fixation modules cannot be stacked.
特に好適には、運動学的関係を表すベクトル対が求められる。第1のベクトル対は、ここでは、関節が当該関節に接続された2つのモジュールの各観点から同一の関節軸を有しているという運動学的関係を表す。「~の観点から」という表現は、どの座標系を観察に用いるかを示す。換言すれば、関節又は関節軸の位置及び配向は、それがどちらのモジュールから求められるかには依存せず、そのため、両モジュールにとって同一である。従って、第1のベクトルによっても、関節が2つのモジュールの各観点から同一の関節軸を有しているという運動学的関係が表される。 Particularly preferably, vector pairs representing kinematic relationships are determined. The first vector pair here represents the kinematic relationship that the joint has the same joint axis from each point of view of the two modules connected to it. The phrase "from the point of view of" indicates which coordinate system is used for observation. In other words, the position and orientation of the joint or joint axis does not depend on which module it is derived from, and is therefore identical for both modules. Thus, the first vector also describes the kinematic relationship that the joint has the same joint axis from each point of view of the two modules.
第2のベクトル対は、測定された関節角度が、関節に接続された一方のモジュールの観点から当該関節に接続された他方のモジュールの少なくとも1つの軸を予め設定するという運動学的関係を表す。換言すれば、関節角度が測定され、一方のモジュールの配向が既知であるならば、他方のモジュールの軸の配向も既知となる。関節角度は、例えば関節角度センサによって測定することができる。 A second vector pair represents a kinematic relationship in which the measured joint angle presets at least one axis of one module connected to the joint from the perspective of the other module connected to the joint. . In other words, if the joint angle is measured and the orientation of one module is known, the orientation of the axis of the other module is also known. A joint angle can be measured by, for example, a joint angle sensor.
好適には、少なくとも1つのベクトル対は、各モジュールに割り当てられた慣性センサのセンサデータの融合において導入される。好適には、上記の2つのベクトル対は、慣性センサごとに求められ、当該センサのセンサデータの関連するフィルタリングにおいて導入される。特に好適には、融合は、フィルタリングによって行われる。ただし、他のセンサ融合方法、例えば、グラフの評価に基づく方法も使用することができる。 Preferably, at least one vector pair is introduced in the fusion of the sensor data of the inertial sensors assigned to each module. Preferably, the above two vector pairs are determined for each inertial sensor and introduced in the associated filtering of the sensor data for that sensor. Particularly preferably, the fusion is done by filtering. However, other sensor fusion methods, such as those based on graph evaluation, can also be used.
これらのモジュールは、典型的には、運動学的連鎖機構に沿って配置されており、即ち、モジュールの運動は、その前に配置されたモジュールの運動に依存する。好適には、1つのモジュールについてベクトルを求めることは、特にグローバル座標系において、固定された基準に接続されている第1のモジュールから開始する順番により順次実施される。 These modules are typically arranged along a kinematic chain, ie the motion of a module depends on the motion of the module placed before it. Preferably, the determination of the vectors for the modules is carried out sequentially, especially in the global coordinate system, starting with the first module connected to a fixed reference.
コンピュータプログラムは、特に、それが計算機上又は制御装置上において実行されるときに、本方法の各ステップを実施するために構成されている。なお、ここでは、構造的な変更の実施を要することなく、従来の電子制御装置への本方法の実装が可能になる。この目的のために、本コンピュータプログラムは、機械可読記憶媒体上に記憶されている。 The computer program is especially designed for implementing the steps of the method when it is run on a computer or controller. It should be noted that implementation of the method in conventional electronic control units is now possible without having to implement structural changes. To this end, the computer program is stored on a machine-readable storage medium.
本コンピュータプログラムを従来の電子制御装置に読み込ませることによって、運動学的な関係を状態推定において導入するように構成された電子制御装置が得られる。 By loading the computer program into a conventional electronic controller, an electronic controller configured to introduce kinematic relationships in state estimation is obtained.
本方法は、多肢関節アームを有する作業機械に使用される。そのような作業機械の例として、バケットアームを有する掘削機がある。この場合、モジュールは、アームのリンクに対応しているが、掘削機のさらなる部分に、例えば上部構造体などに対応することもできる。 The method is used on work machines having articulated arms. An example of such a work machine is an excavator with a bucket arm. In this case the modules correspond to the links of the arm, but they can also correspond to further parts of the excavator, for example the superstructure.
本発明の実施例は、図面に示され、以下の明細書において、より詳細に説明される。 Embodiments of the invention are illustrated in the drawings and are explained in more detail in the following description.
発明の実施例
以下においては、図1の掘削機1の可動モジュールの位置及び配向を状態推定するための本発明に係る方法の一実施形態を説明する。下部車体Uに対して回転可能な上部車体L1、ブームL2、調整ブームL3、ステムL4及びバケットL5は、可動なモジュールとみなされる。これらの可動なモジュールは、運動学的連鎖機構に沿って関節J1,J2,J3,J4,J5を介して相互に接続されており、それぞれ、慣性センサと磁気計とを含むセンサユニットS1,S2,S3,S4,S5を有している。詳細な説明は、上記の「従来技術」の欄において行われている。図3は、本発明に係る方法の実施形態のフローチャートを示している。図2に示されている従来技術による方法と同一のステップは、同一の参照符号により表され、それらの新たな説明は省略される。
Embodiment of the Invention In the following, an embodiment of the method according to the invention for state estimation of the position and orientation of the mobile module of the
表記においては、右側のインデックスは、どのモジュール又は座標系間において運動が行われたかを示し、左側のインデックスは、どの座標系から運動が観察されるかを示す点に留意されたい(「~の観点から」という表現は、どの座標系を観察に用いるかを示す。)。 Note that in the notation, the index on the right indicates between which modules or coordinate systems the motion was made, and the index on the left indicates from which coordinate system the motion is observed ("of The expression "from the point of view" indicates which coordinate system is used for observation).
センサS1,S2,S3,S4,S5ごとに、それぞれ、ベクトル対Sinmess,i,Wnref,iを求めるステップ100,110、及び、第2のベクトル対Siomess,i,Woref,iを求めるステップ101,111が行われ(インデックスiは、ここでは、関連するセンサを有する任意のモジュールを表す)、これらは、以下において詳細に説明する。これらの2つのベクトル対o及びnは、従来技術により測定された磁場ベクトルSmmessを置き換え又は補足するものである(図2参照)。式1によれば、ベクトルSinmess,i,Wnref,i及びSiomess,i,Woref,iは、仮想測定値と期待される基準変数Wnref,i及びWoref,iとの間の偏差又は誤差を最小化するためにフィルタリング21,31において導入される。
Sinmess,i-(RW,Si)T・Wnref,i
(式1) Siomess,i-(RW,Si)T・Woref,i
ここで、Sinmess,i及びSiomess,iは、センサ座標系において測定された(又は仮想的に測定された)ベクトルであり、RW,Siは、グローバル座標系Wに対するセンサSiの配向を示し、Wnref,i及びWoref,iは、グローバル座標系Wの観点からの基準としてのベクトルである。
For each sensor S1, S2, S3, S4, S5, respectively, determine 100, 110 the vector pair Si mess,i , Wnref ,i and the second vector pair Si mess,i , Wo Steps 101, 111 of determining ref,i are performed (index i here representing any module with an associated sensor), which are described in detail below. These two vector pairs o and n replace or supplement the magnetic field vector S m mess measured by the prior art (see FIG. 2). According to Equation 1 , the vectors Sin mess,i , Wnref ,i and Siomess ,i , Woref ,i are the virtual measurements and the expected reference variables Wnref ,i and Woref , i are introduced in filtering 21, 31 to minimize deviations or errors between i .
Si n mess,i −(R W,Si ) T · W n ref,i
(Formula 1) Si o mess,i −(R W, Si ) T · W o ref, i
where Si n mess,i and Si o mess,i are the vectors measured (or virtually measured) in the sensor coordinate system, and R W,Si are the vectors of the sensor Si relative to the global coordinate system W. Denoting the orientation, W n ref,i and W o ref,i are vectors as references from the point of view of the global coordinate system W;
以下においては、第1のセンサユニットS1についてのベクトル対を求めるステップ100,101を説明する。この第1のセンサユニットS1は、上部車体L1上に配置され、慣性センサ及び関節角度センサを有する。上部車体L1は、運動学的連鎖機構に沿って第1の関節J1を介して下部車体Uに接続されている。なお、第1のベクトル対S1nmess,1,Wnref,1を求めるステップ100は、以下のとおりである。
In the following, the
順次連続するモジュールの上部構造体L1と下部構造体Uとを接続する第1の関節J1の関節軸は、モジュールの両座標系に対して直接指定することができる。上部車体L1上に配置された第1のセンサS1についての第1のベクトル対のベクトルS1nmess,1を、一方では、センサ1と上部車体L1との間の既知と想定される配向を使用して指定し、他方では、先行リンクL1の配向推定値を使用してベクトルWnref,iを指定すると、そのように求められた2つのベクトルは、それらが共通の座標系に変換されると直ちに運動学的関係に基づいて同一又は平行になるはずである。
The joint axis of the first joint J1 connecting the upper structure L1 and the lower structure U of successive modules can be specified directly with respect to both coordinate systems of the module. Using the vector S1 n mess,1 of the first vector pair for the first sensor S1 located on the upper body L1, on the one hand the known and assumed orientation between the
Spongらの論文「Robot modeling and control」(上述参照)からのDenavit-Hartenberg表記に応じて、関節軸は、先行するモジュールのz軸に対応している。先行するモジュールとは、ここでは、動作のないモジュールを起点とした運動学的連鎖機構に沿って現在のモジュールの前に配置され、これと直接接続されているモジュールとみなすことができる。この場合、先行するモジュールとは、即ち、下部車体Uである。
(式2) Un=ez
According to the Denavit-Hartenberg notation from Spong et al.'s paper "Robot modeling and control" (see above), the joint axis corresponds to the z-axis of the preceding module. A preceding module can here be regarded as a module that is positioned before and directly connected to the current module along the kinematic chain starting from the inactive module. In this case, the preceding module is the underbody U.
(Formula 2) U n = e z
なお、上部車体L1上に配置された第1のセンサS1の座標系における第1のベクトルの仮想測定は、式3に従って実施することができる。
(式3) S1nmess,1=(RL1,S1)T(RU,L1)Tez
ここで、RL1,S1は、上部車体L1に対する第1のセンサS1の配向を表し、既知であると想定することができる一定の適用パラメータを示す。RU,L1は、上部車体L1と下部車体Uとの間の変換行列TU,L1の回転成分(図2参照)、即ち、Denavit-Hartenberg表記のいわゆるA行列を表す。冒頭に述べたSpongらの論文「Robot modeling and control」(式3,10)においては、これは以下のように定義される。
(Equation 3) S1 nmess ,1 = (R L1, S1 ) T (R U, L1 ) T e z
Here, R L1,S1 represents the orientation of the first sensor S1 with respect to the upper body L1 and indicates a constant application parameter that can be assumed to be known. RU ,L1 represents the rotation component of the transformation matrix TU,L1 between the upper vehicle body L1 and the lower vehicle body U (see FIG. 2), that is, the so-called A matrix in Denavit-Hartenberg notation. In the paper "Robot modeling and control" by Spong et al. (equations 3 and 10) mentioned at the beginning, this is defined as follows.
それゆえ、回転成分については、以下のとおりである。
z軸に平行なベクトルは、z軸を中心とする回転に対して不変であるため、式3をさらに簡略化することができ、そのため、上部構造体L1に関する第1のセンサS1の配向RL1,S1又はパラメータaiにより表される関節の運動学に係る一定のパラメータにのみ依存する。
同時に、第1のベクトルWnrefは、先行するリンク、即ち、下部車体Uの配向の状態推定値を介して、式7に従ってグローバル座標系Wの観点からの基準として指定することができる。
(式7) Wnref,1=RW,Uez
At the same time, the first vector W n ref can be specified as a reference from the point of view of the global coordinate system W according to Eq.
(Equation 7) W n ref, 1 = R W, U e z
なお、第1のセンサS1についての仮想測定値の第1のベクトルS1nmessの記述についても、グローバル座標系Wの観点からの基準としての第1のベクトルWnref,1の記述についても、観察されるリンク、即ち、上部車体L1についての状態推定値又は第1のセンサS1についての状態推定値は使用されていないことに留意すべきである。それゆえ、式8により表される関係(第1のセンサS1についての上記の式1に対応)は、仮想測定値と期待される基準値Wnref,1との間の差分ベクトルを最小化するために、第1のセンサS1の配向RW,S1の状態推定のためのフィルタリング21において導入することができる。
(式8) S1nmess-(RW,S1)T・Wnref,1
It should be noted that both the description of the first vector S1 n mess of virtual measurements for the first sensor S1 and the description of the first vector W n ref,1 as a reference from the point of view of the global coordinate system W, It should be noted that the observed link, ie the state estimate for the upper body L1 or the state estimate for the first sensor S1, is not used. Therefore, the relationship expressed by Equation 8 (corresponding to
(Formula 8) S1 n mess - (R W, S1 ) TW n ref, 1
第2のベクトル対S1omess,1、Woref,1を求めるステップ101は(さらに第1のセンサS1についても)、以下のとおりである。
The
なお、上部車体L1上に配置された第1のセンサS1の座標系における第2のベクトルの仮想測定は、式9に従って実施することができる。
(式9) S1omess,1=(RS1,L1)ex
ここで、RS1,L1は、RL1,S1の逆回転行列又は転置回転行列を示し、従って、同様に、上部車体L1に対する第1のセンサS1の配向を表し、既知であると想定することができる一定の適用パラメータである。
It should be noted that the virtual measurement of the second vector in the coordinate system of the first sensor S1 arranged on the upper vehicle body L1 can be performed according to Equation 9.
(Equation 9) S1 o mess,1 = (R S1, L1 ) e x
Here, R S1,L1 denotes the inverse or transposed rotation matrix of R L1,S1 , thus similarly representing the orientation of the first sensor S1 with respect to the upper body L1 and assumed to be known. is a constant application parameter that allows
同時に、第2のベクトルWoref,1は、先行するリンク、即ち、下部車体Uの配向の状態推定値を介して、式10に従ってグローバル座標系Wの観点からの基準として指定することができ、さらに、上部車体L1と下部車体Uとの間の変換行列TU,L1の回転成分RU,L1を指定することができる。
このベクトルは、基準座標系W及び測定された関節角度θ1に関する下部車体Uの配向のみに依存する。より一般的には、このベクトルは、先行するモジュールの配向推定値及び2つのモジュール間の関節角度のみに依存する。 This vector depends only on the orientation of the underbody U with respect to the frame of reference W and the measured joint angle θ 1 . More generally, this vector depends only on the orientation estimate of the preceding module and the joint angle between the two modules.
なお、ここでも、第1のセンサS1についての第2のベクトルS1omess,1の記述についても、グローバル座標系Wの観点からの基準としての第2のベクトルWoref,1の記述についても、観察されるモジュール、即ち、上部車体L1についての状態推定値又は第1のセンサS1についての状態推定値は使用されていないことに留意すべきである。それゆえ、式11により表される関係(第1のセンサS1についての下記の式11に対応)は、仮想測定値と期待される基準値Woref,1との間の差分ベクトルを最小化するために、第1のセンサのためのフィルタリング21において導入することができる。
(式11) S1omess-(RW,S1)T・Woref,1
It should be noted that here also the description of the second vector S1 o mess,1 for the first sensor S1 and the description of the second vector W o ref,1 as a reference from the point of view of the global coordinate system W , the observed module, ie the state estimate for the upper body L1 or the state estimate for the first sensor S1, is not used. Therefore, the relationship expressed by Equation 11 (corresponding to Equation 11 below for the first sensor S1) minimizes the difference vector between the virtual measured value and the expected reference value W o ref,1 To do so, it can be introduced in the
(Formula 11) S1 o mess - (R W, S1 ) T · W o ref, 1
図3に示す第1のベクトル対S2nmess,2,Wnref,2を求めるステップ110、及び、第2のセンサS2についての第2のベクトル対S2omess,2,Woref,2を求めるステップ111は、同様の方法により実施することができる。このことは、他のセンサS3,S4,S5についてのベクトルを求めるステップにも当てはまるが、これらは明確化の理由から図3には示されていない。 Step 110 of determining the first vector pair S2 n mess,2 , W n ref,2 shown in FIG. 3 and the second vector pair S2 o mess,2 , W o ref,2 for the second sensor S2. The step of determining 111 can be performed in a similar manner. This also applies to the vector determination steps for the other sensors S3, S4, S5, but these are not shown in FIG. 3 for reasons of clarity.
Claims (9)
前記関節(J1,J2,J3,J4,J5)の少なくとも1つと、当該関節(J1,J2,J3,J4,J5)に接続された2つのモジュール(L1,L2,L3,L4,L5)との運動学的関係を表す少なくとも1つのベクトル対(Sinmess,i,Wnref,i又はSiomess,i,Woref,i)であって、測定されたベクトルと基準ベクトルとからなる少なくとも1つのベクトル対( Si n mess,i , W n ref,i 又は Si o mess,i , W o ref,i )が求められ(100,101,110,111)、
前記少なくとも1つのベクトル対(Sinmess,i,Wnref,i又はSiomess,i,Woref,i)が前記状態推定に導入され、
前記複数のモジュール(L1,L2,L3,L4,L5)の位置及び配向を、前記少なくとも1つのベクトル対( Si n mess,i , W n ref,i 又は Si o mess,i,W o ref,i )に基づいて状態推定する、
ことを特徴とする方法。 The positions and orientations of a plurality of modules (L1, L2, L3, L4, L5) movable relative to one another via joints (J1, J2, J3, J4, J5) of a common system are determined by said modules (L1, L2, L3, A method for state estimation using a plurality of inertial sensors (S1, S2, S3, S4, S5) arranged on L4, L5),
at least one of said joints (J1, J2, J3, J4, J5) and two modules (L1, L2, L3, L4, L5) connected to said joints (J1, J2, J3, J4, J5); at least one pair of vectors ( Sinmess ,i , Wnref ,i or Siomess ,i , Woref ,i ) representing the kinematic relationship of from the measured vector and the reference vector At least one pair of vectors ( Sinmess ,i , Wnref ,i or Siomess ,i , Woref , i ) is determined ( 100 , 101 , 110, 111) such that
said at least one vector pair ( Sinmess ,i , Wnref ,i or Siomess ,i, Woref,i ) is introduced into said state estimation ;
The positions and orientations of the plurality of modules (L1, L2, L3, L4 , L5 ) are defined by the at least one pair of vectors ( Sin mess,i , Wnref ,i or Siomess ,i, Woref , i ) to estimate the state based on
A method characterized by:
前記各観点は、それぞれ所定の座標系を観察に用いることを示す、
請求項1に記載の方法。 The first vector pair ( Sinmess ,i , Wnref ,i ) represents the two modules (L1, L2, L3, L4, L5) represent the kinematic relationship that they have the same joint axis from each point of view,
Each of the viewpoints indicates that a predetermined coordinate system is used for observation ,
The method of claim 1.
前記関節角度(θ)が測定され、前記一方のリンク(L1,L2,L3,L4,L5)の配向が既知であるとすると、前記第2のベクトル対( Si o mess,i , W o ref,i )は、前記他方のリンク(L1,L2,L3,L4,L5)の前記少なくとも1つの軸(e x )の配向を示す、
請求項1又は2に記載の方法。 The second vector pair ( Siomess ,i , Woref ,i ) is the measured joint angle (θ) along one link ( L1, L2, L3, L4, L5) at least one axis ( e x ) , representing the kinematic relationship that presets
Assuming that the joint angles (θ) are measured and the orientation of one of the links (L1, L2, L3, L4, L5 ) is known, the second pair of vectors (Si o mess , i , Wo ref , i ) indicates the orientation of said at least one axis (e x ) of said other link (L1, L2, L3, L4, L5);
3. A method according to claim 1 or 2.
請求項1乃至3のいずれか一項に記載の方法。 said at least one vector pair ( Sinmess ,i , Wnref ,i or Siomess ,i , Woref ,i ) assigned to said module (L1, L2, L3, L4, L5) introduced in fusion of sensor data of said inertial sensors (S1, S2, S3, S4, S5),
4. A method according to any one of claims 1-3.
請求項4に記載の方法。 said fusion of sensor data is performed by filtering (21, 31);
5. The method of claim 4.
Applications Claiming Priority (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
DE102019201091.1 | 2019-01-29 | ||
DE102019201091.1A DE102019201091A1 (en) | 2019-01-29 | 2019-01-29 | Method for estimating the position and orientation of several moving modules in a common system |
PCT/EP2020/051473 WO2020156897A1 (en) | 2019-01-29 | 2020-01-22 | Method for state estimation of position and orientation of a plurality of movable modules of a common system |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2022523713A JP2022523713A (en) | 2022-04-26 |
JP7250940B2 true JP7250940B2 (en) | 2023-04-03 |
Family
ID=69187795
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2021544239A Active JP7250940B2 (en) | 2019-01-29 | 2020-01-22 | Method for state estimation of the position and orientation of multiple movable modules of a common system |
Country Status (6)
Country | Link |
---|---|
US (1) | US20220162834A1 (en) |
EP (1) | EP3918138A1 (en) |
JP (1) | JP7250940B2 (en) |
CN (1) | CN113330167B (en) |
DE (1) | DE102019201091A1 (en) |
WO (1) | WO2020156897A1 (en) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE202021103332U1 (en) | 2021-06-22 | 2021-07-05 | Robert Schreiber | Mini excavator with adjustable boom |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2015190293A (en) | 2014-03-31 | 2015-11-02 | 日立建機株式会社 | Control device for area limiting excavation of construction machine |
US20160160472A1 (en) | 2014-12-08 | 2016-06-09 | Caterpillar Global Mining Llc | System for Determining a Position of a Component |
JP2018155027A (en) | 2017-03-17 | 2018-10-04 | 日立建機株式会社 | Construction machine |
Family Cites Families (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE19918140A1 (en) * | 1999-04-01 | 2000-10-12 | Deutsch Zentr Luft & Raumfahrt | Measuring arrangement for controlling robots, machine tools and the like as well as a measuring method carried out with this measuring arrangement |
DE102005025536A1 (en) * | 2005-06-03 | 2007-02-01 | Technische Universität Ilmenau | Mobile machine used as a hydraulic driven excavator comprises a unit for generating traveling and working movement, devices for measuring the position and/or the speed of working hinges and pressure sensors |
DE102009018070A1 (en) * | 2009-04-20 | 2010-10-21 | Robert Bosch Gmbh | Mobile work machine with a position control device of a working arm and method for position control of a working arm of a mobile machine |
US8463569B2 (en) * | 2011-03-21 | 2013-06-11 | Caterpillar Trimble Control Technologies Llc | Method of operating a magnetic compass on a machine |
EP3126785B1 (en) * | 2014-03-31 | 2019-09-04 | Topcon Positioning Systems, Inc. | Automatic identification of sensors |
WO2015199570A1 (en) * | 2014-06-23 | 2015-12-30 | Llc "Topcon Positioning Systems" | Estimation with gyros of the relative attitude between a vehicle body and an implement operably coupled to the vehicle body |
US9428885B2 (en) * | 2014-09-15 | 2016-08-30 | Trimble Navigation Limited | Guidance system for earthmoving machinery |
AR104370A1 (en) * | 2015-04-13 | 2017-07-19 | Leica Geosystems Pty Ltd | MAGNETOMETRIC COMPENSATION |
AR104232A1 (en) * | 2015-04-13 | 2017-07-05 | Leica Geosystems Pty Ltd | DYNAMIC MOVEMENT COMPENSATION IN MACHINERY |
US9617708B2 (en) * | 2015-08-06 | 2017-04-11 | Honeywell International, Inc. | Methods and apparatus for correcting a position of an excavation vehicle using tilt compensation |
JP6684682B2 (en) * | 2016-08-18 | 2020-04-22 | 株式会社神戸製鋼所 | Construction machinery |
US10662613B2 (en) * | 2017-01-23 | 2020-05-26 | Built Robotics Inc. | Checking volume in an excavation tool |
US10030354B1 (en) * | 2017-02-28 | 2018-07-24 | CNH Industrial America, LLC | Anti-spill for loaders |
DE102017203653A1 (en) * | 2017-03-07 | 2018-09-13 | Robert Bosch Gmbh | Method and device for determining the position of kinematics, in particular mobile machines |
US20180313061A1 (en) * | 2017-04-26 | 2018-11-01 | Caterpillar Inc. | Control system using fuzzy logic to display machine productivity data |
-
2019
- 2019-01-29 DE DE102019201091.1A patent/DE102019201091A1/en active Pending
-
2020
- 2020-01-22 WO PCT/EP2020/051473 patent/WO2020156897A1/en unknown
- 2020-01-22 CN CN202080011335.7A patent/CN113330167B/en active Active
- 2020-01-22 EP EP20701593.4A patent/EP3918138A1/en active Pending
- 2020-01-22 US US17/425,768 patent/US20220162834A1/en active Pending
- 2020-01-22 JP JP2021544239A patent/JP7250940B2/en active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2015190293A (en) | 2014-03-31 | 2015-11-02 | 日立建機株式会社 | Control device for area limiting excavation of construction machine |
US20160160472A1 (en) | 2014-12-08 | 2016-06-09 | Caterpillar Global Mining Llc | System for Determining a Position of a Component |
JP2018155027A (en) | 2017-03-17 | 2018-10-04 | 日立建機株式会社 | Construction machine |
Also Published As
Publication number | Publication date |
---|---|
DE102019201091A1 (en) | 2020-07-30 |
WO2020156897A1 (en) | 2020-08-06 |
US20220162834A1 (en) | 2022-05-26 |
JP2022523713A (en) | 2022-04-26 |
EP3918138A1 (en) | 2021-12-08 |
CN113330167A (en) | 2021-08-31 |
CN113330167B (en) | 2022-11-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
DK2760642T3 (en) | Calibration and programming of robots | |
JP5618066B2 (en) | Force control robot calibration apparatus and method | |
US8509951B2 (en) | Controlling the trajectory of an effector | |
KR100986669B1 (en) | A device and method for calibrating a robot | |
US9517563B2 (en) | Robot system using visual feedback | |
Du et al. | Online serial manipulator calibration based on multisensory process via extended Kalman and particle filters | |
US20120303318A1 (en) | Kinematic Predictor for Articulated Mechanisms | |
CN114045893B (en) | Excavator bucket tooth tip positioning method and device and excavator | |
JP6248544B2 (en) | Robot, control device, robot system | |
CN110450143B (en) | Workpiece fatigue testing method based on cooperative robot | |
WO2021031346A1 (en) | Online robot kinematic calibration method based on plurality of sensors combined with filters | |
JP2019177436A (en) | Robot control device, method for determining angle of joint of robot, and program | |
CN115008476B (en) | Robot calibration method and device based on equivalent kinematics model | |
JP7250940B2 (en) | Method for state estimation of the position and orientation of multiple movable modules of a common system | |
Xiao et al. | A new fixed axis-invariant based calibration approach to improve absolute positioning accuracy of manipulators | |
CN110053049A (en) | Robot track precision determination method and device and industrial robot | |
CN111002304A (en) | Device for determining the position and position of an end effector of a robot | |
JPH1139019A (en) | Off-line teaching method | |
US20090228144A1 (en) | Method For Calculating Rotation Center Point And Axis Of Rotation, Method For Generating Program, Method For Moving Manipulator And Positioning Device, And Robotic System | |
JP6485620B2 (en) | Robot control system and robot control method | |
CN117306614A (en) | Excavator bucket operation track determination method, device, equipment and storage medium | |
Zhang et al. | Angle estimation for robotic arms on floating base using low-cost imus | |
JP7384653B2 (en) | Control device for robot equipment that controls the position of the robot | |
CN115062482A (en) | Serial robot kinematics modeling method considering rotary joint positioning error | |
JP5667437B2 (en) | Robot external axis measurement method, robot teaching data creation method, and robot controller |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20210927 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20220826 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20220906 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20221205 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20230314 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20230322 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 7250940 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |