apriltag使用之三：机械臂与Apriltag码对齐

本文主要是介绍apriltag使用之三：机械臂与Apriltag码对齐，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

相机到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]$。

通常我们可以将全局参考坐标系就设在机器人的基坐标系，因此有
$$\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{\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{\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_conversionsclass 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 = poseArrdef 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"breakrospy.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_stself.move_to_pose(pose_st)def run(self):#1. 运动到指定位置，以便查找Apriltagpose_st = self.group.get_current_pose()print("cur pose_st: ", pose_st.pose)pose_st.pose.orientation.x = 1; pose_st.pose.orientation.y=0pose_st.pose.orientation.z = 0; pose_st.pose.orientation.w=0pose_st.pose.position.x = 0;  pose_st.pose.position.z =0.5pose_st.pose.position.y = 0.5self.move_to_pose(pose_st)self.aligning()self.aligning() # 再一次def main():rospy.init_node('find_apriltag', anonymous=True)app = FindApriltag()app.run()rospy.spin()returnif __name__ == '__main__':main()

实验

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

这篇关于apriltag使用之三：机械臂与Apriltag码对齐的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---