相机到Apriltag的对齐

上面是一个简单的手画的关于机器人、摄像头以及Apriltag码之间的位置关系图。机器人的基坐标系{s}由$({\mathbf{x}}_{\mathbf{s}},{\mathbf{y}}_{\mathbf{s}},{\mathbf{z}}_{\mathbf{s}},{\mathbf{t}}_{\mathbf{s}})$表示，摄像头的初始位置的坐标系{c}由$({\mathbf{x}}_{\mathbf{c}},{\mathbf{y}}_{\mathbf{c}},{\mathbf{z}}_{\mathbf{c}},{\mathbf{t}}_{\mathbf{c}})$表示，apriltag码的坐标系{a}由$({\mathbf{x}}_{\mathbf{a}},{\mathbf{y}}_{\mathbf{a}},{\mathbf{z}}_{\mathbf{a}},{\mathbf{t}}_{\mathbf{a}})$表示。其中$\mathbf{t}$是各坐标系原点相对于全局参考坐标系中的位置，$\mathbf{x},\mathbf{y},\mathbf{z}$则是各坐标系的坐标轴单位向量在全局参考坐标系的表示，这也形成各坐标系相对于全局参考坐标系的旋转矩阵$\mathbf{R}=[\mathbf{x},\mathbf{y},\mathbf{z}]$。同时我们使用坐上标表示父坐标系，右下标表示子坐标系，例如相机坐标系的旋转矩阵为${}^s{\mathbf{R}}_c={[}^s{\mathbf{x}}_c{,}^s{\mathbf{y}}_c{,}^s{\mathbf{z}}_c]$。

通常我们可以将全局参考坐标系就设在机器人的基坐标系，因此有
t s = ( 0 , 0 , 0 ) T x s = ( 1 , 0 , 0 ) T y s = ( 0 , 1 , 0 ) T z s = ( 0 , 0 , 1 ) T

$$\begin{array}{r}{\mathbf{t}}_s=(0,0,0{)}^T\\ {\mathbf{x}}_s=(1,0,0{)}^T\\ {\mathbf{y}}_s=(0,1,0{)}^T\\ {\mathbf{z}}_s=(0,0,1{)}^T\end{array}$$

通过Apriltag检测，我们知道了Apriltag在摄像头坐标系{c}下的方位为${[}^c{\mathbf{R}}_a{|}^c{\mathbf{t}}_a]$，目标是：获得相机坐标系相对与全局参考坐标系的方位${[}^s{\mathbf{R}}_{c^{\prime }}{|}^s{\mathbf{t}}_{c^{\prime }}]$，使得相机在保持相同距离的情况下视角正对于Apriltag码。

从上图中我们可知，新的相机是与Apriltag码的方向相同的，既有${}^s{\mathbf{R}}_{c^{\prime }}{=}^s{\mathbf{R}}_a$。而我们肯定是知道运动中的机器人末端的相机在全局坐标系下的方位的，即${[}^s{\mathbf{R}}_c{|}^s{\mathbf{t}}_c]$。从相机坐标到全局参考坐标的变换为：
s T c = [ s R c s t c 0 1 ] ^s\mathbf{T}_c=\left[$${}^s{\mathbf{T}}_c=\left[\begin{array}{cc}{}^s{\mathbf{R}}_c& {}^s{\mathbf{t}}_c\\ 0& 1\end{array}\right]$$
Apriltag码在全局坐标系中的位置为${}^s{\mathbf{T}}_c^c{\mathbf{t}}_a$。所以Apriltag码在全局参考坐标系下的方位就为${[}^s{\mathbf{R}}_a{|}^s{\mathbf{t}}_a]={[}^s{\mathbf{R}}_c{\cdot }^c{\mathbf{R}}_a{|}^s{\mathbf{T}}_c{\cdot }^c{\mathbf{t}}_a]$。另外可以得出Apriltag码坐标系到全局坐标系的${}^s{\mathbf{T}}_a$为：
s T a = [ s R a s t a 0 1 ] = [ s R c ⋅ c R a s T c ⋅ c t a 0 1 ] ^s\mathbf{T}_a=\left[ $$\begin{array}{ccc} ^s\mathbf{R}_a & ^s\mathbf{t}_a \\ 0 & 1 \end{array}$$ \right] = \left[

$${}^s{\mathbf{T}}_a=\left[\begin{array}{cc}{}^s{\mathbf{R}}_a& {}^s{\mathbf{t}}_a\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{}^s{\mathbf{R}}_c{\cdot }^c{\mathbf{R}}_a& {}^s{\mathbf{T}}_c{\cdot }^c{\mathbf{t}}_a\\ 0& 1\end{array}\right]$$

然后需要确定的就是新的相机位置${}^s{\mathbf{t}}_c$，该位置其实在Apriltag码的坐标系下是${}^a{\mathbf{t}}_c=(0,0,-|{|}^c{\mathbf{t}}_a||)$，将其通过${}^s{\mathbf{T}}_a$变换到全局坐标系下即可，即$^s\mathbf{t}_c = ^{s\mathbf{T}_a\cdot}a\mathbf{t}_c $。

关于摄像头在机械臂上的位置

一般情况下，摄像头都是安装在机械臂的末端执行器的一侧的，所以就存在一个${}^t{\mathbf{T}}_c$表示相机到机械臂末端的位置变换，并且有${}^s{\mathbf{T}}_c{=}^s{\mathbf{T}}_t{\cdot }^t{\mathbf{T}}_c$。因此当新的${}^s{\mathbf{T}}_c$计算出来后，还要计算${}^s{\mathbf{T}}_t{=}^s{\mathbf{T}}_c{\cdot }^t{\mathbf{T}}_c^{-1}$来得到机械臂末端的方位，以便使用Moveit来控制机械臂运动。

代码

首先，我们假设以及执行了另外一个ROS节点在不断地检测摄像头图像中的Apriltag码，并将检测结果存放到名为"/apriltag/pose"的Topic中。

下面给出主要的执行代码。注意其中aligning函数执行了两次。因为第一次摄像头距离Apriltag码比较远，对方位的估计会比较粗糙，所以当第一次使得摄像头比较近时，再一次执行该操作可以使对齐更加精确。

import cv2
import numpy as np
import rospy
import moveit_commander
from geometry_msgs.msg import *
import tf_conversions

class FindApriltag(object):
    def __init__(self):
        super(FindApriltag, self).__init__()

        group_name = "arm"  # 这是对机器人进行moveit配置定义的关节组名称
        self.group = moveit_commander.MoveGroupCommander(group_name)
        print("effector_link=", self.group.get_end_effector_link())

        self.pose_pub = rospy.Subscriber("/apriltag/pose", PoseArray, self.poseCB)
        self.poseArr =PoseArray()
        # 等待Topic订阅
        rospy.sleep(1)

    def poseCB(self, poseArr):
        self.poseArr = poseArr

    def aligning(self):
        pose_st = self.group.get_current_pose()
        # 获得Apriltag相对于摄像头的方位
        for i in (0,1,2,3):
            if len(self.poseArr.poses)<1:
                print "There is no Apriltag"
            else:
                print "Apriltag found"
                break
            rospy.sleep(0.1)
        pose_ca= self.poseArr.poses[0]
        # 计算机械臂末端到全局坐标系中的方位
        new_pose_st = self.compute_new_pose_st(pose_st.pose, pose_ca)
        # 运动到新位置
        pose_st.pose = new_pose_st
        self.move_to_pose(pose_st)

    def run(self):
        #1. 运动到指定位置，以便查找Apriltag
        pose_st = self.group.get_current_pose()
        print("cur pose_st: ", pose_st.pose)
        pose_st.pose.orientation.x = 1; pose_st.pose.orientation.y=0
        pose_st.pose.orientation.z = 0; pose_st.pose.orientation.w=0
        pose_st.pose.position.x = 0;  pose_st.pose.position.z =0.5
        pose_st.pose.position.y = 0.5
        self.move_to_pose(pose_st)
        self.aligning()
        self.aligning() # 再一次

def main():
    rospy.init_node('find_apriltag', anonymous=True)
    app = FindApriltag()
    app.run()
    rospy.spin()
    return

if __name__ == '__main__':
        main()

实验

下面的动画显示了一个对齐的过程。其中将相机到Apriltag码的距离设置为0.15米。