幾何和幀框(GEOMETRY AND FRAMES)
大綱
- Spot 機器人世界中的幀(Frames in the Spot robot world)
- Spot 幀之間的轉換 Transformations between Spot’s frames
- 轉換數學Transformation Math
- 在機器人操作期間添加幀Adding frames during robot operation
前言
Spot 使用 3D 變換來描述自身、周圍的物體以及它對世界的理解。Spot 有不同的框架來表示這些對象,例如“身體(body)”幀(frame),它描述了 Spot 的身體。然後使用 3D 變換使用平移向量和旋轉四元數來描述兩個幀之間的關係。這些變換可用於在空間中定位 Spot 並確定其位置。
這些變換使用 x、y 和 z 軸在 3D 空間中表示。平移是描述兩個幀原點之間差異的 3 元素向量 (x, y, z)。旋轉是一個四元數 (w, x, y, z),它描述了兩個框架坐標軸方向之間的變化。在 API 幾何中,我們使用 SE3Pose 來封裝用於變換的平移和旋轉。類似地,我們使用字符串來描述可以使用 SE3Pose 轉換相互關聯的不同幀(frame)。
例如,相機幀(frame)原點可能位於相機的鏡頭處,Z 軸被指出,X 軸和 Y 軸與圖片的水平/垂直尺寸對齊。相機幀中 [0, 0, 1m] 處的點將位於鏡頭的中心和相機指向的原點前方 1 米處。可以使用轉換來描述如何相對於 Spot 的主體幀來表達此相機幀,並且 API 使用 body_tform_camera 表示法來表示此轉換的 SE3Pose。
Spot 機器人世界中的幀(Frames in the Spot robot world)
通常,您只需要使用以下幀類型:inertial frames慣性幀、sensor frames傳感器幀、object frames物件幀和機器人的身體幀body frame。然而,機器人也可能報告它知道的其他幀。
Spot 的 "inertial frames慣性幀" 係是 "vision frame視覺幀" 和 " 路程計幀odom frame"。這些幀的原點和初始旋轉與機器人啟動時的身體相匹配。
"身體幀(body frame)" 描述了機器人的位置和方向,如圖所示。機器人上的每個關節都存在其他幀。要獲得機器人姿勢的完整再現,請為機器人上的每個關節添加幀值:每條腿兩個髖關節和一個膝蓋。
"sensor frames傳感器幀",描述給定傳感器的原點及其坐標軸。來自機器人的傳感器消息,例如相機圖像或點雲,將包含傳感器的字符串幀(string frame)名稱。sensor frames傳感器幀通常用於描述使用傳感器檢測到的對象。
Objects物件,也可以使用幀框來描述。例如,基準點( fiducial )的原點位於標籤中心,x、y 軸與標籤軸對齊,z 軸指向頁面外。
當機器人在世界各地移動時,兩個幀框之間的關係可以始終保持不變,也可以是非靜態和動態變化的。慣性坐標幀框和機器人身體坐標幀框之間的轉換會隨著機器人的移動而改變。相比之下,無論機器人是否移動,機器人的身體和相機幀框之間的變換都不會改變。
Spot 已知以下幀:
- " 路程計幀odom frame":一種慣性坐標幀,它使用機器人的運動學估計世界中的固定位置(相對於機器人啟動的位置)。
- "vision frame視覺幀":一種慣性幀,用於估計世界中的固定位置(相對於機器人啟動的位置),並使用世界的視覺分析和機器人的里程計(odometry.)進行計算。
- "身體幀(body frame)":描述機器人身體位置和方向的幀。幀的原點在臀部的幾何中心,x 軸從臀部中心指向前臀部的中間,如上圖所示。
- "身體平面幀(flat_body frame)":描述機器人身體位置和方向的重力對齊幀。該位置位於機器人的中心,x/y 軸位於" 路程計幀odom frame" x-y 平面中。
- "gpe frame":代表機器人地平面估計的幀。完整的 SE(3) 姿勢可以轉換為平面(點和法線/a point and normal)。
- “fiducial_XXX”幀:表示原始基準檢測的幀,其中“XXX”將填充基準的標籤 ID。
- “filtered_fiducial_XXX”幀:表示過濾的、更穩定的基準檢測的幀,其中“XXX”將填充基準的標籤 ID。此幀框僅適用於世界上靜態的基準點,不應用於可能移動的基準點。
- “frontleft_fisheye”框架:一個示例 傳感器幀;這個幀名稱代表前置左魚眼鏡頭。Spot的所有五個攝像頭都有自己的傳感器幀名稱,這些名稱將描述確切的攝像頭傳感器的位置和方向。
Spot 幀之間的轉換 Transformations between Spot’s frames
Spot 了解上述許多幀以及它們之間的關係。幀之間的這些關係可以存儲在類似圖(graph-like)的結構中,其中邊代表變換 A_tform_B,節點代表幀“A”或“B”。API 創建所有相關幀的樹形圖,其中每個幀都可以使用轉換相對於不同的幀進行唯一描述,並且所有幀都相對於單個根幀進行描述。
不同的服務在給定的時間戳(在字段獲取時間中描述)提供轉換樹圖的快照,其中包括“視覺”、“odom”和“身體”幀之間的轉換以及任何其他相關幀,例如 作為圖像的傳感器幀或真實世界物件的幀。快照中的所有轉換都保證為時間戳提供最準確的 SE3Pose。
轉換數學Transformation Math
由於幀通過轉換為彼此相互關聯,因此我們可以通過適當的轉換將一幀中表示的幾何數據更改為另一幀中表示的幾何數據。這是通過將變換相乘來完成的。例如,假設機器人在其左側攝像頭中檢測到一個足球,併計算了轉換 left_camera_tform_soccer_ball 以表達足球相對於攝像頭的位置。同樣,機器人知道變換 vision_tform_left_camera,它描述了視覺慣性坐標幀中的左相機坐標幀。使用這些變換,我們可以計算足球在視覺慣性幀中的位置:
vision_tform_soccer_ball = vision_tform_left_camera * left_camera_tform_soccer_ball
通過將這些變換相乘,我們可以"取消cancel out" 左相機幀以確定新的變換。
變換也可以反轉。對於在視覺慣性幀內描述 Spot 身體的變換 vision_tform_body。如果我們反轉這個變換,我們計算 body_tform_vision。此變換現在描述了相對於機器人身體的視覺慣性幀,因此視覺幀的原點是相對於 Spot 當前身體位置的。如果您想將兩個沒有匹配中間元素的轉換相乘,反轉轉換會很有用。例如,如果我們要計算變換vision_tform_body,但只知道變換vision_tform_odom 和 body_tform_odom,那麼我們可以使用逆運算:
vision_tform_body = vision_tform_odom * body_tform_odom.inverse()
可以使用 SE3Pose 變換計算其他數學運算,例如創建 4x4 矩陣、變換點或計算滾動roll、俯仰pitch或偏航yaw。API 在 math_helpers.py 中為這些操作提供了輔助函數,包括變換乘法和求逆。
在機器人操作期間添加幀Adding frames during robot operation
客戶端應用程序可以在運行時添加幀。例如,您可以為機器人在操作期間使用真實世界物件服務發現的 "AprilTag 基準(fiducial)" 添加一個幀。
... frame_tree_edges = {} # Dictionary mapping child frame name to the # ParentEdge, which stores the parent frame name # and the SE3Pose parent_tform_child # Example of a special transform that we want to add to the robot. vision_tform_special_frame = \ SE3Pose(position=Vec3(1,1,1),rotation=Quaternion(w=1,x=0,y=0,z=0)) # Update the frame tree edge dictionary with the new transform using the helper functions in frame_helpers.py. frame_tree_edges = add_edge_to_tree(frame_tree_edges, \ vision_tform_special_frame, VISION_FRAME_NAME, "special_frame") # Pack the dictionary into a FrameTreeSnapshot proto message. snapshot = FrameTreeSnapshot(child_to_parent_edge_map=frame_tree_edges) # Create the world object containing the special frame. world_obj_special_frame = WorldObject(id=21, name="SpecialFrame", \ transforms_snapshot=snapshot, acquisition_time=time.now()) # Add the world object containing the special frame to Spot using a mutation request. world_object_client.mutate_world_objects(mutation_req= \ make_add_world_object_request(world_obj_special_frame)) ...
留言
張貼留言
Aron阿龍,謝謝您的留言互動!