[发明专利]一种基于凸松弛全局优化算法的机器人手眼标定方法

摘要

本发明提供了一种基于四元数理论的凸松弛全局最优化手眼标定方法，该方法是针对机器人运动学正解及相机的外参数标定存在偏差时，基于非线性最优化的手眼标定算法无法确保目标函数收敛到全局极小值的问题而提出；考虑到机械手末端相对运动旋转轴之间的夹角对标定方程求解精度的影响，首先利用随机抽样一致性(Random Sample And Consensus，RANSAC)算法对标定数据中旋转轴之间的夹角进行预筛选，再利用四元数参数化旋转矩阵，建立多项式几何误差目标函数和约束，采用基于线性矩阵不等式(linear matrix inequality，LMI)凸松弛全局优化算法求解全局最优手眼变换矩阵。

主权项

1.一种基于凸松弛全局优化算法的机器人手眼标定方法，其特征在于，包括以下步骤：S1，设定角度阈值初值，利用自适应RANSAC算法对标定数据集Cⁿ中旋转轴之间的夹角进行预筛选，其中，所述标定数据集Cⁿ的筛选方法，具体包括：S101：建立标定数据筛选模型，标定方程中旋转矩阵的误差模型表示为：其中，其中，∠(r_B12,r_B23)表示两次相对运动的单位旋转轴的夹角，θ_B12，θ_B23分别表示机械手末端执行器从位姿1到位姿2以及从位姿2到位姿3的旋转角，表示靶标世界坐标系到摄像机坐标系的变换矩阵A在位姿1的旋转误差，表示靶标世界坐标系到摄像机坐标系的变换矩阵A在位姿2的旋转误差；表示机械手末端执行器坐标系到机械手基坐标系的变换矩阵B在位姿1的旋转误差，表示机械手末端执行器坐标系到机械手基坐标系的变换矩阵B在位姿2的旋转误差；表示手眼标定方程中旋转矩阵的误差；定义r_ij和r_kl分别表示机械手末端执行器从位姿i到位姿j及从位姿k到位姿l的单位旋转轴，θ_ij,kl表示两次相对运动的单位旋转轴的夹角，当θ_ij,kl接近90°或者θ_t接近0°时，旋转矩阵的误差最小，表示为：θ_ij,kl＝∠(r_ij,r_kl)，θ_t＝||90‑θ_ij,kl||；S102：将摄像机固定在机械手末端执行器上，采集标定数据；利用机械手带动摄像机每次选取N个不同位姿对平面靶标拍照成像，两两进行组合可以得到M＝N(N‑1)/2组手眼标定数据集Cⁿ，利用罗德里格旋转公式将标定数据集Cⁿ中的变换矩阵A，B表示成轴角形式；S103：利用自适应RANSAC算法对标定数据集Cⁿ中机械手末端执行器两次相对运动旋转轴之间的夹角进行预筛选；S2，利用四元数法参数化手眼变换矩阵X，将S1中筛选出的满足角度阈值要求的靶标世界坐标系到摄像机坐标系的变换矩阵A和机械手末端执行器坐标系到机械手基坐标系的变换矩阵B代入手眼标定方程AX＝XB，建立多项式几何误差目标函数和约束条件，其中，所述四元数法参数化手眼标定方程具体包括：S201：定义A₁，A₂表示靶标世界坐标系到两个不同姿态的摄像机坐标系的变换矩阵，B₁，B₂表示为两次不同姿态的机械手末端执行器坐标系到机械手基坐标系的变换矩阵，X表示摄像机坐标系到机械手末端执行器坐标系的变换矩阵，手眼关系表示为：AX＝XB其中，A、B、X都为4×4的矩阵，展开为旋转矩阵和平移向量的形式为：用四元数法参数化变换矩阵X中的旋转矩阵R_x表示为：其中，旋转矩阵R(q_x)表示为：S202：以最小化标定方程AX＝XB为几何误差目标函数，以单位四元数的性质为约束条件，建立关于变量X(q_x,t_x)多元多项式优化问题，其中，R_x表示变换矩阵X中的旋转矩阵，R_A表示表示靶向世界坐标系到摄像机变换矩阵A中的旋转矩阵，R_B表示机械手末端执行器坐标系到机械手基坐标系的变换矩阵中的旋转矩阵，t_x、t_A、t_B分别表示平移向量，q_x、q₀、q₁、q₂、q₃分别表示旋转矩阵R(q_x)的元素，n＝2；S203：利用LMI凸松弛全局优化算法把上述多项式函数优化问题松弛为半正定规划问题求解；S3，采用基于线性矩阵不等式凸松弛全局优化算法对S2参数化的手眼标定方程进行求解，得到满足标定方程的全局最优参数值，然后利用该参数值得到全局最优的手眼变换矩阵，其中，基于线性矩阵不等式(LMI)凸松弛全局优化算法具体步骤包括：S301：使用提升变量线性化目标函数和约束项W(X)，使用替代目标函数W₀(X)和约束项W_i(X)中的单项式S302：添加半正定矩阵约束，即按照δ阶多项式的基v_δ(X)的排列顺序，添加半正定矩阵约束M_δ(y)≥0，M_δ(Wy)≥0；其中，表示目标函数W₀(X)和约束项W_i(X)中的单项式，M_δ(y)≥0，M_δ(Wy)≥0表示按照基v_δ(X)排列顺序组成的半正定矩阵，表示半正定矩阵M_δ(y)≥0中的单项式；S303：将凸松弛多项式优化问题转化为半正定规划问题求解。