CN118404583A - 一种移动机械臂柔顺开门运动规划方法及系统 - Google Patents

Abstract

本发明提供一种移动机械臂柔顺开门运动规划方法及系统，属于机器人运动规划领域。为解决现有机械臂完成开门任务时，由于移动平台与机械臂末端自身控制存在误差，导致旋拧把手及推拉门时会出现真实运动与设计轨迹不完全重合，造成开门失败或机械臂损坏的问题。包括计算出机械臂力域中的雅可比矩阵，将关节转矩与末端外力联系到一起，根据机械臂输出的各关节转矩计算得到末端所受环境外力；在笛卡尔空间控制机械臂末端执行器，在关节空间对关节控制；通过阻抗控制系统，可以显著降低运动干涉问题；将移动机械臂开门任务分解为四个次要任务，并针对每个次要任务提出了相对独立的控制器设计方法，实现车臂协同任务，显著提高开门效率和成功率。

Description

一种移动机械臂柔顺开门运动规划方法及系统

技术领域

本发明涉及机器人运动规划技术领域，具体而言，涉及一种移动机械臂柔顺开门运动规划方法及系统。

背景技术

随着机器人技术的不断发展和进步，如今诸多领域都广泛存在其应用，对于移动机器人而言，不可避免要解决的一个问题就是开门操作，开门操作的难点在于：一是定位把手位置后，在考虑到双臂空间约束，以及门不同开合角度的情况下，需要进行合理的车、臂路径规划；二是由于实操过程中不可避免的会出现标定、建模误差，这些误差会导致机械臂的末端执行器轨迹出现偏差，在自由空间中这些误差是允许的，但是在机器人与把手接触环境下，即使很小的误差也可能会出现较大的接触力导致零部件的损坏。

移动机械臂在完成开门任务时，由于移动平台与机械臂末端自身控制存在误差，因此旋拧门把手以及推拉门过程中会出现其真实运动与设计的轨迹不完全重合，会与门或门把手发生环境干涉，因此需要对机械臂进行柔顺控制。

发明内容

本发明要解决的技术问题是：

为了解决现有机械臂在完成开门任务时，由于移动平台与机械臂末端自身控制存在误差，导致旋拧把手及推拉门时出现真实运动与设计轨迹不完全重合，造成开门失败或机械臂损坏的问题。

本发明为解决上述技术问题所采用的技术方案：

本发明提供了一种移动机械臂柔顺开门运动规划方法，包括以下步骤：

S100、求解机械臂微分运动学，计算出机械臂力域中的雅可比矩阵；

S200、对机械臂在开门过程中进行抓取和旋钮门把手进行运动规划，包括在笛卡尔空间对机械臂末端执行器进行控制，获得对机械臂抓取门把手进行轨迹规划、对机械臂旋拧门把手进行圆弧轨迹规划以及对机械臂旋拧门把手过程中机械臂末端进行位姿规划，在关节空间对关节进行控制，获得在运动规划中的各关节转角；

S300、设计阻抗控制系统，将阻抗控制器应用于开门任务，用于对机械臂末端位置进行修正；

S400、对车臂系统进行运动规划，包括设计任务前馈控制器、末端执行器跟踪控制器、移动平台速度控制器、任务反馈控制器用以实现车臂协同开门任务。

进一步地，在步骤S100中，具体包括：

S110、设机械臂的雅可比矩阵为j_m(q_m)，x为笛卡尔空间下机械臂末端位置，为末端线速度，qm为机械臂各个关节转角，为机械臂各关节角速度，则末端线速度与关节角速度关系可表示为下式：

其中，x、y、z为机械臂末端位置，θ_x、θ_y、θ_z为机械臂末端位姿，θ₁、θ₂、θ₃、θ₄、θ₅、θ₆为机械臂六关节转角；

S120、采用微分变换法对机械臂的雅可比矩阵进行求解，根据末端位置与关节角度之间的变换矩阵，定义雅可比矩阵如下式：

其中，J_vi表示机械臂六关节线速度的传动比，J_wi表示机械臂六关节角速度的传动比；

定义机械臂正运动学变换矩阵如下式：

其中，n_xi、n_yi、n_zi、o_xi、o_yi、o_zi、a_xi、a_yi、a_zi为旋转矩阵的九个分量，p_xi、p_yi、p_zi表示沿x、y、z轴的平移分量；

S130、根据正运动学求解的变换矩阵求解雅可比矩阵各列如下式：

J_vi＝[(p_i×n_i) (p_i×o_i) (p_i×a_i)]^T (4)

J_ωi＝[n_z o_z a_z]^T (5)

其中，n_z、o_z、a_z分别为机械臂末端姿态欧拉角对i关节的偏导数；

利用正运动学所求得的旋转矩阵，将末端空间坐标系转换到基座坐标系下：

其中，为机械臂从基座坐标系到六轴坐标系的旋转矩阵，J_m0(q_m)表示基座坐标系下的雅可比矩阵，J_m6(q_m)表示末端坐标系下的雅可比矩阵；

定义f为机械臂末端x，y，z三个方向上的外力，m为机械臂末端相对于基座在三个方向上的力矩，τ为各关节电机转矩，利用力雅可比矩阵将关节力矩转换为末端力F，末端力及各关节电机转矩如下式：

F＝[f m]^T (7)

τ＝[τ₁ τ₂ τ₃ τ₄ τ₅ τ₆]^T (8)

由虚功原理将末端力与关节电机转矩联系在一起，再根据速度雅可比矩阵定义机械臂的力雅可比矩阵得到力域中的雅可比矩阵如下式：

τ＝J^TF (9)。

进一步地，在步骤S200中，具体包括：

S210、对机械臂抓取门把手进行轨迹规划，根据机械臂末端初始位置点a和目标点b在机械臂基座坐标系下的坐标分别为a(a₀,b₀,c₀)、b(a₁,b₁,c₁),在直线轨迹之间插入n个点，确定每个点的坐标如下式：

得到的n个点利用逆运动学解得n个对应的关节转角，利用关节空间对机械臂进行轨迹规划；

S220、对机械臂旋拧门把手进行圆弧轨迹规划，设初始位置点p₁、p₃在机械臂基座坐标系下坐标分别为p₁(x₁,y₁,z₁)、p₃(x₃,y₃,z₃)，中间点为p₂(x₂,y₂,z₂)，则可以确定其外接圆方程如下式：

平面T可确定平面方程如下：

平面S可确定平面方程：

联立平面M、T、S三个平面方程，可得圆心p₀(x₀,y₀,z₀)，且圆弧半径为：

再转换到基座直角坐标系下即可得到机械臂末端圆弧轨迹；

S230、对机械臂旋拧门把手过程中机械臂末端进行位姿规划，机械臂末端位姿由时变的末端空间坐标系对基坐标系的旋转矩阵指定，将基于世界坐标系的欧拉角转为旋转矩阵如下式：

其中，R_x(γ),R_y(β),R_z(α)分别为机械臂末端绕X,Y,Z的旋转矩阵，cγ＝cosγ,sγ＝sinγ,cβ＝cosβ,sγ＝sinγ,cα＝cosα,sα＝sinα,α,β,γ分别表示机械臂末端绕Z,Y,X三个轴的旋转角度；

将得到的旋转矩阵与公式中的正运动学矩阵中9个参数n_xi、n_yi、n_zi、o_xi、o_yi、o_zi、a_xi、a_yi、a_zi联立方程得到机械臂的6个关节角度，利用关节空间控制机械臂平稳运动，实现对末端姿态的规划；

S240、关节空间轨迹规划，在步骤S230中利用位置规划，规划出机械臂末端的轨迹路径，再利用逆运动学解得各关节转角。

进一步地，在步骤S300中，具体包括：设位置控制为内环，外环为机械臂末端与环境之间的接触力，通过期望阻抗模型解算出机械臂末端的位置信息反馈给机械臂各关节，对机械臂末端位置进行修正。

进一步地，在步骤S300中，具体包括：

S310、将机械臂末端环境受力与位置关系等效为由质量M_d、阻尼B_d和弹簧K_d组成的弹簧质量阻尼二阶控制系统，其二阶微分方程数学模型如下式：

其中，x_d、表示期望位置、速度、加速度，x、表示实际位置、速度、加速度，F_d表示机械臂末端期望力，F表示机械臂末端力传感器实际输出力；

S320、由步骤S310中构建的二阶微分方程数学模型，通过环境接触力的力反馈解算出机械臂末端的位置误差，通过微分运动学解算出各关节转速与转矩，设计基于位置的阻抗控制模拟如下式：

其中，Mx、My、Mz表示x、y、z轴方向上的质量参数，Bx、By、Bz表示x、y、z轴方向上的阻尼参数，Kx、Ky、Kz表示x、y、z轴方向上的刚度参数。

进一步地，在步骤S400中，具体包括：

S410、设计任务前馈控制器，门平面需要以恒定的角速度绕门轴进行旋转，将移动机械臂开门在空间中分为四大坐标系，包括门把手与门的坐标系∑O_H,机械臂末端执行器坐标系∑O_E，机械臂基座坐标系∑O_B，移动平台坐标系∑O_C；

设定开门速度，该速度方向为门坐标系下x方向的线速度为：

其中，表示机械臂末端垂直于门方向上的速度；

移动平台的速度沿移动平台坐标系x轴直线运动，将其转换到门把手与门坐标系下，门的运动方向上，并将门的恒定速度与移动平台的速度进行合成分解，利用移动平台与门之间、末端执行器与门之间的速度变换矩阵，得到机械臂末端的速度在基座坐标系下的笛卡尔速度为：

其中，^ET_H为机械臂末端与门把手之间的变换矩阵，^HT_C为机械臂基座与移动平台之间的变换矩阵，为机械臂末端相对于机械臂基座在前进方向上的速度，为机械臂末端相对于机械臂基座在水平方向上的速度；

S420、设计末端执行器跟踪控制器，所述末端执行器绕末端执行器坐标系∑O_E的z轴方向做旋转运动，且该旋转角度时刻与门开度夹角相等，因此末端执行器绕z轴的旋转角速度为：

其中，为机械臂末端绕机械臂末端坐标系Z方向的角速度，d为门的旋转半径；

在机械臂末端的速度控制中将位置速度运动与姿态变换运动结合，末端执行器的合速度为：

利用速度雅可比矩阵，将末端速度转换到关节空间中，利用关节角速度对机械臂进行速度控制；

S430、设计移动平台速度控制器，设定移动平台的速度随门开度夹角的增大，所述移动平台的速度为：

_(C)v_C＝v_C,ref sin(θ) (22)

其中，θ为门打开角度，v_C,ref为门打开时的线速度；

S440、设计位置反馈控制器，设移动平台任意时刻的坐标为(x_c,y_c)，基座在移动平台坐标系下的坐标也为(x_c,y_c),门把手在世界坐标系下为(x_h0,y_h0),门轴在世界坐标系下为(x_h0,y_h0-d),已知门的旋转半径为d，则门绕门轴旋转得到的方程为：

(x-x_h0)²+(y-y_h0+d)²＝d² (23)

引入速度修正系数ρ用于根据机械臂末端位置对末端速度进行修正，当机械臂末端在圆弧内时，增大x、y方向的速度，在圆弧外时，减小x、y方向的速度，用于保证机械臂末端执行器能够在圆弧附近与门把手维持稳定接触：

其中，x_E、y_E为在世界坐标系下机械臂末端位置，为世界坐标系下机械臂末端速度。

本发明提供了一种移动机械臂柔顺开门运动规划系统，该系统具有与上述步骤对应的程序模块，运行时执行上述的移动机械臂柔顺开门运动规划方法中的步骤。

本发明提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现移动机械臂柔顺开门运动规划方法的步骤。

相较于现有技术，本发明的有益效果是：

本发明一种移动机械臂柔顺开门运动规划方法及系统，计算出机械臂力域中的雅可比矩阵，将关节转矩与末端外力联系到一起，可以根据机械臂输出的各关节转矩计算得到末端所受环境外力；在笛卡尔空间控制机械臂末端执行器，在关节空间对关节进行控制；通过设计基于位置的阻抗控制系统，可以显著降低运动干涉问题；将移动机械臂开门任务分解为四个次要任务，并针对每个次要任务提出了相对独立的控制器设计方法，实现车臂协同任务，显著提高开门效率和成功率。

附图说明

图1为本发明实施例中一种移动机械臂柔顺开门运动规划方法的流程图；

图2为本发明实施例中机械臂旋拧门把手的圆弧轨迹规划图；

图3为本发明实施例中基于位置的阻抗系统的结构示意图；

图4为本发明实施例中开门过程中速度的规划示意图；

图5为本发明实施例中机械臂末端执行器的姿态示意图；

图6为本发明实施例中推拉门过程中移动平台的位置变化示意图；

图7为本发明实施例中移动平台在开门过程中的位置与门开度夹角之间的对应关系示意图；

图8为本发明实施例中车臂协同推门试验的实际效果图；

图9为本发明实施例中开门过程中机械臂末端轨迹图；

图10为本发明实施例中门开度夹角变化图；

图11为本发明实施例中移动机械臂速度曲线图。

具体实施方式

在本发明的描述中，应当说明的是，各实施例中的术语名词例如“上”、“下”、“前”、“后”、“左”、“右”等指示方位的词语，只是为了简化描述基于说明书附图的位置关系，并不代表所指的元件和装置等必须按照说明书中特定的方位和限定的操作及方法、构造进行操作，该类方位名词不构成对本发明的限制。

为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。

具体实施方案一：结合图1至图8所示，本发明提供一种移动机械臂柔顺开门运动规划方法，包括以下步骤：

S100、求解机械臂微分运动学，计算出机械臂力域中的雅可比矩阵，具体包括：

S110、设机械臂的雅可比矩阵为j_m(q_m)，x为笛卡尔空间下机械臂末端位置(m)，为末端线速度(m/s)，qm为机械臂各个关节转角(rad)，为机械臂各关节角速度(rad/s)，则末端线速度与关节角速度关系可表示为下式：

其中，x、y、z为机械臂末端位置，θ_x、θ_y、θ_z为机械臂末端位姿，θ₁、θ₂、θ₃、θ₄、θ₅、θ₆为机械臂六关节转角；

S120、采用微分变换法对机械臂的雅可比矩阵进行求解，根据末端位置与关节角度之间的变换矩阵，定义雅可比矩阵如下式：

其中，J_vi表示机械臂六关节线速度的传动比，J_wi表示机械臂六关节角速度的传动比；

定义机械臂正运动学变换矩阵如下式：

其中，n_xi、n_yi、n_zi、o_xi、o_yi、o_zi、a_xi、a_yi、a_zi为旋转矩阵的九个分量，p_xi、p_yi、p_zi表示沿x、y、z轴的平移分量；

S130、机械臂各关节均为转动关节，关节在绕旋转轴进行微分转动时，根据正运动学求解的变换矩阵可以求解出雅可比矩阵各列如下式：

J_vi＝[(p_i×n_i) (p_i×o_i) (p_i×a_i)]^T (4)

J_ωi＝[n_z o_z a_z]^T (5)

其中，n_z、o_z、a_z分别为机械臂末端姿态欧拉角对i关节的偏导数；

利用正运动学所求得的旋转矩阵，可以将机械臂末端空间坐标系即机械臂六轴坐标系转换到基座坐标系即机械臂一轴坐标系下：

其中，为机械臂从基座坐标系到六轴坐标系的旋转矩阵，J_m0(q_m)表示基座坐标系下的雅可比矩阵，J_m6(q_m)表示末端坐标系下的雅可比矩阵；

定义f为机械臂末端x，y，z三个方向上的外力(N)，m为机械臂末端相对于基座在三个方向上的力矩(Nm)，τ为各关节电机转矩(Nm)，利用力雅可比矩阵将关节力矩转换为末端力F，末端力及各关节电机转矩如下式：

F＝[f m]^T (7)

τ＝[τ₁ τ₂ τ₃ τ₄ τ₅ τ₆]^T (8)

由虚功原理将末端力与关节电机转矩联系在一起，再根据速度雅可比矩阵定义机械臂的力雅可比矩阵得到力域中的雅可比矩阵如下式：

τ＝J^TF (9)

S200、对机械臂在开门过程中进行抓取和旋钮门把手进行运动规划，包括在笛卡尔空间对机械臂末端执行器进行控制，在关节空间对关节进行控制，具体包括：

S210、对机械臂抓取门把手进行轨迹规划，根据机械臂末端初始位置点a和目标点b在机械臂基座坐标系下的坐标分别为a(a₀,b₀,c₀)、b(a₁,b₁,c₁),在直线轨迹之间插入n个点，确定每个点的坐标如下式：

得到的n个点利用逆运动学解得n个对应的关节转角，利用关节空间对机械臂进行轨迹规划；

S220、对机械臂旋拧门把手进行圆弧轨迹规划，结合图2所示，设初始位置点p₁、p₃在机械臂基座坐标系下坐标分别为p₁(x₁,y₁,z₁)、p₃(x₃,y₃,z₃)，中间点p₂(x₂,y₂,z₂)则可以确定其外接圆方程如下式：

平面T可确定平面方程如下：

平面S可确定平面方程：

联立平面M、T、S三个平面方程，可得圆心p₀(x₀,y₀,z₀)，且圆弧半径为：

再转换到基座直角坐标系下即可得到机械臂末端圆弧轨迹；

S230、对机械臂旋拧门把手过程中机械臂末端进行位姿规划，机械臂末端位姿由时变的末端空间坐标系对基坐标系的旋转矩阵指定，因此将基于世界坐标系的欧拉角转为旋转矩阵如下式：

其中，R_x(γ),R_y(β),R_z(α)分别为机械臂末端绕X,Y,Z的旋转矩阵，cγ＝cosγ,sγ＝sinγ,cβ＝cosβ,sγ＝sinγ,cα＝cosα,sα＝sinα,α,β,γ分别表示机械臂末端绕Z,Y,X三个轴的旋转角度；

所述世界坐标系即原点位于移动平台初始位置，由原点为起始点，水平向右为x轴方向，水平向前为y轴方向，竖直向上为z轴方向；

将得到的旋转矩阵与公式(3)中的正运动学矩阵中前9个参数n_xi、n_yi、n_zi、o_xi、o_yi、o_zi、a_xi、a_yi、a_zi联立方程得到机械臂的6个关节角度，利用关节空间控制机械臂平稳运动，实现对末端姿态的规划；

S240、关节空间轨迹规划，在步骤S230中利用位置规划，规划出机械臂末端的轨迹路径，再利用逆运动学解得各关节转角；

S300、设计阻抗控制系统，将阻抗控制器应用于开门任务，具体包括：

S310、将机械臂末端环境受力与位置关系等效为弹簧质量阻尼二阶控制系统，弹簧质量阻尼二阶控制系统由质量M_d、阻尼B_d和弹簧K_d组成，保证机械臂能够计算出期望位置与实际位置之间以及期望力与实际力之间的差值，其二阶微分方程数学模型如下式：

其中，x_d、表示期望位置、速度、加速度，x、表示实际位置、速度、加速度；F_d表示机械臂末端期望力；F表示机械臂末端力传感器实际输出力；

S320、由步骤S310中构建的二阶微分方程数学模型，通过环境接触力的力反馈解算出机械臂末端的位置误差，通过微分运动学解算出各关节转速与转矩，设计基于位置的阻抗控制模拟如下式：

其中，Mx、My、Mz表示x、y、z轴方向上的质量参数，Bx、By、Bz表示x、y、z轴方向上的阻尼参数，Kx、Ky、Kz表示x、y、z轴方向上的刚度参数；

设计如附图4所示的位置阻抗控制器，采用位置控制作为内环，外环为机械臂末端与环境之间的接触力，通过期望阻抗模型解算出机械臂末端的位置信息反馈给机械臂各关节，对机械臂末端位置进行修正；

S400、车臂系统运动规划，设计任务前馈控制器、末端执行器跟踪控制器、移动平台速度控制器、任务反馈控制器用以实现车臂协同开门任务，具体包括：

S410、设计任务前馈控制器，为保证开门过程的柔顺平滑，门平面应以恒定的角速度绕门轴进行旋转，将移动机械臂开门在空间中分为四大坐标系，分别为门把手与门的坐标系∑O_H,机械臂末端执行器坐标系∑O_E，机械臂基座坐标系∑O_B，移动平台坐标系∑O_C，如附图4所示，

设定开门的速度，该速度方向为门坐标系下x方向的线速度为：

其中，表示机械臂末端垂直于门方向上的速度；

移动平台的速度沿自身坐标系∑O_C的x轴直线运动，将其转换到门把手与门坐标系下，门的运动方向上，并将门的恒定速度与移动平台的速度进行合成分解，利用移动平台与门之间、末端执行器与门之间的速度变换矩阵，得到机械臂末端的速度在基座坐标系下的笛卡尔速度为：

其中，^ET_H为机械臂末端与门把手之间的变换矩阵，^HT_C为机械臂基座与移动平台之间的变换矩阵，为机械臂末端相对于机械臂基座在前进方向上的速度，为机械臂末端相对于机械臂基座在水平方向上的速度；

S420、设计末端执行器跟踪控制器，机械臂末端执行器姿态示意图如附图6所示，通过图中的几何关系可以发现，末端执行器需要绕末端执行器坐标系∑O_E的z轴方向做旋转运动，且该旋转角度时刻与门开度夹角相等，因此末端执行器绕z轴的旋转角速度为：

其中，为机械臂末端绕机械臂末端坐标系Z方向的角速度，d为门的旋转半径；

由于机械臂末端的速度控制需要将位置速度运动与姿态变换运动相结合，因此末端执行器的合速度为：

利用速度雅可比矩阵，将末端速度转换到关节空间中，利用关节角速度对机械臂进行速度控制；

S430、设计移动平台速度控制器，结合附图7所示为移动平台在开门过程中的位置与门开度夹角之间的对应示意图，为了保证在机械臂的可达工作空间内实现稳定连续开门，且机械臂末端执行器时刻与门把手接触，设定移动平台的速度随门开度夹角的增大，逐渐增大但增长速度逐渐减小：

_(C)v_C＝v_C,ref sin(θ) (22)

其中，θ为门打开角度，v_C,ref为门打开时的线速度；

S440、设计位置反馈控制器，设移动平台任意时刻的坐标为(x_c,y_c)，基座在移动平台坐标系下的坐标也为(x_c,y_c),门把手在世界坐标系下为(x_h0,y_h0),门轴在世界坐标系下为(x_h0,y_h0-d),已知门的旋转半径为d，则门绕门轴旋转得到的方程为：

(x-x_h0)²+(y-y_h0+d)²＝d² (23)

引入速度修正系数ρ用于根据机械臂末端位置对末端速度进行修正，当机械臂末端在圆弧内时，增大x、y方向的速度，在圆弧外时，适当减小x、y方向的速度，保证机械臂末端执行器能够在圆弧附近与门把手维持稳定接触：

其中，x_E、y_E为在世界坐标系下机械臂末端位置，为世界坐标系下机械臂末端速度；

车臂协同推门试验如附图8所示，将移动平台对准门把手与门中心之间位置，对门把手抓取并推门，拉门过程中，机械臂对准门中心，防止拉门过程中机械臂末端出现不可达位姿，记录移动平台以及机械臂末端的轨迹及速度变化，用以验证移动机械臂开门过程中，机械臂末端夹爪与移动平台的合运动为圆弧运动，并且验证移动平台的速度与规划的速度变化趋势相同。

附图9所示为移动机械臂开门过程中机械臂末端在笛卡尔空间下的轨迹，移动平台为直线运动，将移动平台x方向速度与机械臂末端在基座坐标系下的x方向合成为移动机械臂末端的合运动轨迹，其轨迹近似圆弧状，其半径约为0.75m，与门半径相近，验证了车臂协同开门在运动学上的可行性。

附图10为开门过程中门的开度，本文设定门绕门轴旋转速度为0.2rad/s，因此其门夹角呈线性变化，最终开度为1.5rad左右，与末端轨迹最终x、y两个方向上的夹角相吻合。

附图11所示分别为机械臂末端在x、y方向速度与移动平台移动速度，为保证移动平台不与门发生碰撞，对移动平台的速度规划为由慢到快，符合正弦变化曲线，观察图像，由于移动平台速度逐渐增加，机械臂末端x方向速度随之减小，同时增大y方向速度，以维持开门过程中的圆弧运动。

具体实施方式二：本发明提供了一种移动机械臂柔顺开门运动规划系统，该系统具有与上述步骤对应的程序模块，运行时执行上述的移动机械臂柔顺开门运动规划方法中的步骤。

本实施方案的其他组合和连接关系与具体实施方案一相同。

具体实施方式三：本发明提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现移动机械臂柔顺开门运动规划方法的步骤。

本实施方案的其他组合和连接关系与具体实施方案一相同。

虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本发明领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

Claims (8)

1.一种移动机械臂柔顺开门运动规划方法，其特征在于，包括以下步骤：

S100、求解机械臂微分运动学，计算出机械臂力域中的雅可比矩阵；

S200、对机械臂在开门过程中进行抓取和旋钮门把手进行运动规划，包括在笛卡尔空间对机械臂末端执行器进行控制，获得对机械臂抓取门把手进行轨迹规划、对机械臂旋拧门把手进行圆弧轨迹规划以及对机械臂旋拧门把手过程中机械臂末端进行位姿规划，在关节空间对关节进行控制，获得在运动规划中的各关节转角；

S300、设计阻抗控制系统，将阻抗控制器应用于开门任务，用于对机械臂末端位置进行修正；

S400、对车臂系统进行运动规划，包括设计任务前馈控制器、末端执行器跟踪控制器、移动平台速度控制器、任务反馈控制器用以实现车臂协同开门任务。

2.根据权利要求1所述的一种移动机械臂柔顺开门运动规划方法，其特征在于，在步骤S100中，具体包括：

S110、设机械臂的雅可比矩阵为j_m(q_m)，x为笛卡尔空间下机械臂末端位置，为末端线速度，qm为机械臂各个关节转角，为机械臂各关节角速度，则末端线速度与关节角速度关系可表示为下式：

其中，x、y、z为机械臂末端位置，θ_x、θ_y、θ_z为机械臂末端位姿，θ₁、θ₂、θ₃、θ₄、θ₅、θ₆为机械臂六关节转角；

S120、采用微分变换法对机械臂的雅可比矩阵进行求解，根据末端位置与关节角度之间的变换矩阵，定义雅可比矩阵如下式：

其中，J_vi表示机械臂六关节线速度的传动比，J_wi表示机械臂六关节角速度的传动比；

定义机械臂正运动学变换矩阵如下式：

其中，n_xi、n_yi、n_zi、o_xi、o_yi、o_zi、a_xi、a_yi、a_zi为旋转矩阵的九个分量，p_xi、p_yi、p_zi表示沿x、y、z轴的平移分量；

S130、根据正运动学求解的变换矩阵求解雅可比矩阵各列如下式：

J_vi＝[(p_i×n_i) (p_i×o_i) (p_i×a_i)]^T (4)

J_ωi＝[n_z o_z a_z]^T (5)

其中，n_z、o_z、a_z分别为机械臂末端姿态欧拉角对i关节的偏导数；

利用正运动学所求得的旋转矩阵，将末端空间坐标系转换到基座坐标系下：

其中，为机械臂从基座坐标系到六轴坐标系的旋转矩阵，J_m0(q_m)表示基座坐标系下的雅可比矩阵，J_m6(q_m)表示末端坐标系下的雅可比矩阵；

定义f为机械臂末端x，y，z三个方向上的外力，m为机械臂末端相对于基座在三个方向上的力矩，τ为各关节电机转矩，利用力雅可比矩阵将关节力矩转换为末端力F，末端力及各关节电机转矩如下式：

F＝[f m]^T (7)

τ＝[τ₁ τ₂ τ₃ τ₄ τ₅ τ₆]^T (8)

由虚功原理将末端力与关节电机转矩联系在一起，再根据速度雅可比矩阵定义机械臂的力雅可比矩阵得到力域中的雅可比矩阵如下式：

τ＝J^TF (9)。

3.根据权利要求2所述的一种移动机械臂柔顺开门运动规划方法，其特征在于，在步骤S200中，具体包括：

S210、对机械臂抓取门把手进行轨迹规划，根据机械臂末端初始位置点a和目标点b在机械臂基座坐标系下的坐标分别为a(a₀,b₀,c₀)、b(a₁,b₁,c₁),在直线轨迹之间插入n个点，确定每个点的坐标如下式：

得到的n个点利用逆运动学解得n个对应的关节转角，利用关节空间对机械臂进行轨迹规划；

S220、对机械臂旋拧门把手进行圆弧轨迹规划，设初始位置点p₁、p₃在机械臂基座坐标系下坐标分别为p₁(x₁,y₁,z₁)、p₃(x₃,y₃,z₃)，中间点为p₂(x₂,y₂,z₂)，则可以确定其外接圆方程如下式：

平面T可确定平面方程如下：

平面S可确定平面方程：

联立平面M、T、S三个平面方程，可得圆心p₀(x₀,y₀,z₀)，且圆弧半径为：

再转换到基座直角坐标系下即可得到机械臂末端圆弧轨迹；

S230、对机械臂旋拧门把手过程中机械臂末端进行位姿规划，机械臂末端位姿由时变的末端空间坐标系对基坐标系的旋转矩阵指定，将基于世界坐标系的欧拉角转为旋转矩阵如下式：

其中，R_x(γ),R_y(β),R_z(α)分别为机械臂末端绕X,Y,Z的旋转矩阵，cγ＝cosγ,sγ＝sinγ,cβ＝cosβ,sγ＝sinγ,cα＝cosα,sα＝sinα,α,β,γ分别表示机械臂末端绕Z,Y,X三个轴的旋转角度；

将得到的旋转矩阵与公式中的正运动学矩阵中9个参数n_xi、n_yi、n_zi、o_xi、o_yi、o_zi、a_xi、a_yi、a_zi联立方程得到机械臂的6个关节角度，利用关节空间控制机械臂平稳运动，实现对末端姿态的规划；

S240、关节空间轨迹规划，在步骤S230中利用位置规划，规划出机械臂末端的轨迹路径，再利用逆运动学解得各关节转角。

4.根据权利要求3所述的一种移动机械臂柔顺开门运动规划方法，其特征在于，在步骤S300中，具体包括：设位置控制为内环，外环为机械臂末端与环境之间的接触力，通过期望阻抗模型解算出机械臂末端的位置信息反馈给机械臂各关节，对机械臂末端位置进行修正。

5.根据权利要求4所述的一种移动机械臂柔顺开门运动规划方法，其特征在于，在步骤S300中，具体包括：

S310、将机械臂末端环境受力与位置关系等效为由质量M_d、阻尼B_d和弹簧K_d组成的弹簧质量阻尼二阶控制系统，其二阶微分方程数学模型如下式：

其中，x_d、表示期望位置、速度、加速度，x、表示实际位置、速度、加速度，F_d表示机械臂末端期望力，F表示机械臂末端力传感器实际输出力；

S320、由步骤S310中构建的二阶微分方程数学模型，通过环境接触力的力反馈解算出机械臂末端的位置误差，通过微分运动学解算出各关节转速与转矩，设计基于位置的阻抗控制模拟如下式：

其中，Mx、My、Mz表示x、y、z轴方向上的质量参数，Bx、By、Bz表示x、y、z轴方向上的阻尼参数，Kx、Ky、Kz表示x、y、z轴方向上的刚度参数。

6.根据权利要求5所述的一种移动机械臂柔顺开门运动规划方法，其特征在于，在步骤S400中，具体包括：

S410、设计任务前馈控制器，门平面需要以恒定的角速度绕门轴进行旋转，将移动机械臂开门在空间中分为四大坐标系，包括门把手与门的坐标系∑O_H,机械臂末端执行器坐标系∑O_E，机械臂基座坐标系∑O_B，移动平台坐标系∑O_C；

设定开门速度，该速度方向为门坐标系下x方向的线速度为：

其中，表示机械臂末端垂直于门方向上的速度；

移动平台的速度沿移动平台坐标系x轴直线运动，将其转换到门把手与门坐标系下，门的运动方向上，并将门的恒定速度与移动平台的速度进行合成分解，利用移动平台与门之间、末端执行器与门之间的速度变换矩阵，得到机械臂末端的速度在基座坐标系下的笛卡尔速度为：

其中，^ET_H为机械臂末端与门把手之间的变换矩阵，^HT_C为机械臂基座与移动平台之间的变换矩阵，为机械臂末端相对于机械臂基座在前进方向上的速度，为机械臂末端相对于机械臂基座在水平方向上的速度；

S420、设计末端执行器跟踪控制器，所述末端执行器绕末端执行器坐标系∑O_E的z轴方向做旋转运动，且该旋转角度时刻与门开度夹角相等，因此末端执行器绕z轴的旋转角速度为：

其中，为机械臂末端绕机械臂末端坐标系Z方向的角速度，d为门的旋转半径；

在机械臂末端的速度控制中将位置速度运动与姿态变换运动结合，末端执行器的合速度为：

利用速度雅可比矩阵，将末端速度转换到关节空间中，利用关节角速度对机械臂进行速度控制；

S430、设计移动平台速度控制器，设定移动平台的速度随门开度夹角的增大，所述移动平台的速度为：

_(C)v_C＝v_C,refsin(θ) (22)

其中，θ为门打开角度，v_C,ref为门打开时的线速度；

S440、设计位置反馈控制器，设移动平台任意时刻的坐标为(x_c,y_c)，基座在移动平台坐标系下的坐标也为(x_c,y_c),门把手在世界坐标系下为(x_h0,y_h0),门轴在世界坐标系下为(x_h0,y_h0-d),已知门的旋转半径为d，则门绕门轴旋转得到的方程为：

(x-x_h0)²+(y-y_h0+d)²＝d² (23)

引入速度修正系数ρ用于根据机械臂末端位置对末端速度进行修正，当机械臂末端在圆弧内时，增大x、y方向的速度，在圆弧外时，减小x、y方向的速度，用于保证机械臂末端执行器能够在圆弧附近与门把手维持稳定接触：

其中，x_E、y_E为在世界坐标系下机械臂末端位置，为世界坐标系下机械臂末端速度。

7.一种移动机械臂柔顺开门运动规划系统，其特征在于：该系统具有与上述权利要求1-6任一项权利要求的步骤对应的程序模块，运行时执行上述的移动机械臂柔顺开门运动规划方法中的步骤。

8.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现权利要求1-6中任一项所述的移动机械臂柔顺开门运动规划方法的步骤。