Realsense-Realman手眼标定

news2024/12/27 2:52:58

硬件设备

Realsense D405
Realman 65b

软件环境搭建

软件环境依赖：

librealsense
		https://github.com/IntelRealSense/librealsense.git
ROS1.0
	ros-noetic-aruco
		sudo apt-get install ros-noetic-aruco*
	realsense_ros
		https://github.com/IntelRealSense/realsense-ros.git
	rm_robot
		https://github.com/RealManRobot/rm_robot.git

下载标定软件

mkdir -p handeye_ws/src && cd handeye_ws/src

git clone https://github.com/HenryWJL/hand_eye_calibration

cd ../catkin_make

这里软件使用jaka虚拟机械比进行标定，因系需要进行适配。进入/home/kyle/ROS_Packages/handeye_ws/src/hand_eye_calibration/scripts修改hand_to_eye_calib.py代码以适配睿尔曼机械臂：

### 导入Pose包
from geometry_msgs.msg import TwistStamped, Pose

### 注释原有的位姿订阅callback

# def get_end2base_mat(pose):
#     # Calculating the gripper-to-base transformation matrix
#     tran = np.array((pose.twist.linear.x, pose.twist.linear.y, pose.twist.linear.z))
#     rot = tfs.euler.euler2mat(pose.twist.angular.x, pose.twist.angular.y, pose.twist.angular.z)
#     return rot, tran

### 订阅睿尔曼机械臂消息，并使用geometry_msgs.msg/Pose
### 有可能需要解算逆坐标
def get_end2base_mat(pose):
    # Calculating the gripper-to-base transformation matrix
    tran = np.array((pose.position.x, pose.position.y, pose.position.z))
    # rot = tfs.euler.euler2mat(pose.orientation.x, pose.orientation.y, pose.orientation.z)
    rot = tfs.quaternions.quat2mat([pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z])
    # invert
    # tran = -np.array(rot).T * tran
    # rot = np.array(rot).T
    return rot, tran

...
	### 机械臂订阅睿尔曼位姿话题
	rospy.Subscriber('/rm_driver/Pose_State', Pose, end_pose_callback, queue_size=10)

...
	### 打印不同形式位姿，方便检查
                    print("="*60)
                    print("R_cam2end")
                    print("Transform:")
                    print(cam2base)  # Transformation matrix
                    R_cam2base_quaternions = tfs.quaternions.mat2quat(R_cam2base)
                    cam2base_quaternions = np.concatenate((T_cam2base.reshape(3),
                                                           R_cam2base_quaternions.reshape(4)), axis=0)
                    print("Quaternions:")
                    print(cam2base_quaternions.tolist())  # Quaternions

                    R_cam2base_euler = tfs.euler.mat2euler(R_cam2base)
                    print("Euler angles:")
                    print(R_cam2base_euler[0]/np.pi*180, R_cam2base_euler[1]/np.pi*180, R_cam2base_euler[2]/np.pi*180)

修改标定板识别参数，这里使用的是棋盘格标定板。/home/kyle/ROS_Packages/handeye_ws/src/hand_eye_calibration/launch中checkerboard_start.launch文件：

    <arg name="image_topic"       default="/camera/color/image_raw"/>
    <arg name="camera_info"       default="/camera/color/camera_info"/> 
    <arg name="cornerX"          default="6"/>
    <arg name="cornerY"          default="3"/>    
    <arg name="gridLength"        default="0.05"/>

标定过程（眼在手上）

原理见下图，总的过程为采集数据，解一个方程 AX=XB。
原理图

#console1
roslaunch rm_control rm_65_control.launch &
roslaunch rm_bringup rm_65_robot.launch

#console2
roslaunch realsense2_camera rs_camera.launch

#console3
roslaunch hand_eye_calibration checkerboard_start.launch

#console4
rosrun hand_eye_calibration hand_to_eye_calib.py

标定程序启动后，不停摆动机械臂位姿，均使相机拍摄标定板。采集样本输入r命令，计算位姿输入c命令，退出输入q命令，采集样本>3个。

Record: r, Calculate: c, Save: s, Quit: q
r
[[-0.7599338   0.0060974   0.64997188]
 [ 0.04182341  0.99834257  0.03953361]
 [-0.64865354  0.05722697 -0.75892928]]
1 samples have been recorded

record1

Record: r, Calculate: c, Save: s, Quit: q
r
[[-0.74060468  0.49582793  0.45349683]
 [ 0.56793266  0.82259485  0.02811064]
 [-0.35910612  0.27837454 -0.89081447]]
2 samples have been recorded

record2

rRecord: r, Calculate: c, Save: s, Quit: q

[[-0.81163333 -0.14185728  0.56668144]
 [-0.26747336  0.95265154 -0.14461342]
 [-0.51933548 -0.26894526 -0.81114679]]
3 samples have been recorded
Record: r, Calculate: c, Save: s, Quit: q
r
[[-0.67410916  0.1870448   0.71455657]
 [ 0.22764802  0.97292509 -0.03991421]
 [-0.70267576  0.13576085 -0.69843809]]
4 samples have been recorded
c
Record: r, Calculate: c, Save: s, Quit: q
============================================================
R_cam2end
Transform:
[[ 0.02341403  0.96697815  0.25378148 -0.08822873]
 [-0.99964886  0.01949493  0.01794709  0.00566516]
 [ 0.01240699 -0.25411258  0.96709506  0.07756586]
 [ 0.          0.          0.          1.        ]]
Quaternions:
[-0.08822872609559883, 0.005665157492463942, 0.07756585852038214, 0.7088730522093655, -0.09594795174349201, 0.08512613400089308, -0.6935751781297171]
Euler angles:
-14.72216660609419 -0.7108864356472807 -88.65824916327023