[发明专利]基于机身姿态角度校正的六足机器人关节角度标定方法

摘要

本发明提供一种基于机身姿态角度校正的六足机器人关节角度标定方法，通过微型惯性传感器获取六足机器人机身姿态角度，结合足式机器人系统运动学与逆运动学模型完成多足运动机器人支撑足各关节角度校正；驱动六足机器人以任意三足支撑地面，另外三足抬起并始终保持不接触地面；选取其中一条支撑足驱动该足上某一关节发生角度转动，则该足足底将滑动，另外两足足底与地面不滑动；通过建立关节角度与姿态间的函数关系，获得关于关节零点的线性方程组，求解得支撑足足底相对机身坐标系中的坐标，再利用已知足末端位置即可反解出支撑足各关节角度。本方法能快速准确完成对多足移动机器人关节角度的校正，保证机器人多足协调与运动轨迹精度。

主权项

1.一种基于机身姿态角度校正的六足机器人关节角度标定方法，其特征在于：驱动六足移动机器人以任意三条支撑足支撑地面，选取其中第一条支撑足并驱动该第一条支撑足上某一关节发生角度转动，且该支撑足的足底滑动；第二条和第三条支撑足的足底与地面不滑动，保持第二条和第三条支撑足固定不动；建立关节角度与姿态间的函数关系，获得关于关节零点的线性方程组，求解即解出第一条支撑足的各关节角度，从而实现对六足机器人的全部关节角度标定；所述方法包括如下步骤：步骤1、初始化六足机器人机身的IMU传感器的三自由度陀螺角度；步骤2、使六足机器人某一支撑足任意关节旋转给定角度；步骤3、读取IMU传感器中陀螺三自由度角度；步骤4、使步骤2中所述关节再次旋转，再重复步骤3，直至得到五组姿态IMU传感器中陀螺三自由度角度数据；步骤5、若步骤4得到的五组数据不全相同，则转到步骤6进行求解；若步骤4得到的五组姿态数据有两组以上是相同的，则转动步骤2中该支撑足的任意一个关节一定角度，但该关节不得是步骤2中已旋转过的关节，重复步骤2～4，直至得到五组不全相同的姿态IMU传感器中陀螺三自由度角度姿态数据；步骤6、根据支撑足足底相对机身坐标系的坐标，求解六足机器人步骤2中进行旋转运动的支撑足的关节角度；步骤7、重复以上操作，即对六足机器人六个支撑足进行关节角度标定；步骤6中，求解关节角度需要建立关节零点、关节变化量同机器人位姿之间的映射关系：六足机器人支撑至少需要三条支撑足，设支撑足足底到机身重心位置矢量分别为p_1c、p_2c、p_3c，简化为：p_ic＝(x_ic,y_ic,z_ic)^T，i＝1,2,3，令：(x_iα,y_iα,z_iα)为髋关节在机器人机身坐标系中的坐标；根据足底相对机身坐标系的坐标，求解六足机器人关节角：其中：A＝(x_ic‑x_iα‑l₁sinα_i)²+(y_ic‑y_iα‑l₁cosα_i)²                 (4)B＝(z_ic‑z_i0)²                           (5)α_i表示第i个髋关节相对零点的转动角度，β_i表示第i个臀关节相对零点的转动角度，γ_i表示第i个膝关节相对零点的转动角度，髋关节相对于机身重心距离为l₁，大腿机构长度为l₂，小腿结构长度为l₃；步骤4中，为了保证求解的有效性，任意一条支撑足的某一关节转动5次并记录相对转角，且5次获得的机身姿态角度不全相同；令x_i＝(l₁+l₂cosβ_i+l₃cosθ_i)sinα_i                        (6)y_i＝(l₁+l₂cosβ_i+l₃cosθ_i)cosα_i                        (7)z_i＝l₂sinβ_i+l₃sinθ_i                              (8)则：x_ic＝x_iα+x_i                                    (9)y_ic＝y_iα+y_i                                    (10)z_ic＝z_iα+z_i                                    (11)设机器人第N次转动时机身坐标系相对世界坐标系的方向余弦矩阵为：机身坐标系原点C在世界坐标系O中位置矢量为^op_cn＝(^ox_cn,^oy_cn,^oz_cn)^T，三支撑足末端在世界坐标系中的位置矢量分别为p_1o、p_2o、p_3o，其中：p_io＝(x_io,y_io,z_io)^T，支撑足末端在机身坐标系C和世界坐标系O中的位置矢量有关系：p_io＝^oR_cnp_ic+^op_cn                               (13)取第三分量上的方程，得：其中，r_ni＝r_31nx_i0+r_32ny_i0+r_33nz_i0为已知常数项；当第i足的髋关节每次转动角度Δα，转动n次后，n＝0,1,2...4：z_1o‑^oz_cn＝r_n1+r′_31nx_1c+r′_32ny_1c+r_33nz_1c                    (15)其中：r′_31n＝r_31ncosnΔα‑r_32nsinnΔα                        (16)r′_32n＝r_31nsinnΔα+r_32ncosnΔα                     (17)在平地上，足末端位置在世界坐标系中z_io相同，即z_1o＝z_2o＝z_3o：r′_31nx₁+r′_32ny₁+r_33nz₁‑r_31nx₂‑r_32ny₂‑r_33nz₂＝r_n2‑r_n1                (18)r′_31nx₁+r′_32ny₁+r_33nz₁‑r_31nx₃‑r_32ny₃‑r_33nz₃＝r_n3‑r_n1              (19)转动5次，取n＝0,1,2...4得10个方程，只须取其中9个，得：根据克拉默法则，求解支撑足足底相对机身坐标系中的坐标，再利用已知足末端位置即解出支撑足各关节角度，以此进行角度标定。