本发明提供了一种四自由度串联机器人逆运动学求解方法和系统，包括建立各连杆坐标系、求机器人的运动学正解、根据机器人末端位姿求解机器人各关节转角/位移，对关节超程错误进行报警，对机器人各关节进行行程约束。本发明能够根据机器人的位形唯一确定机器人的逆解，并判断目标位姿的可达性，能够根据期望的机器人位形求解对应的逆解，在多解的情况下实现逆解可控，有利于进行路径规划及避障。
1.一种四自由度串联机器人逆运动学求解方法，其特征在于，包括如下步骤：步骤一：建立各连杆坐标系并确定各关节的D‑H运动学参数，所述四自由度串联机器人由四个连杆串联构成四个关节，依次为：回转臂‑关节1、大臂‑关节2、外节臂‑关节3和伸缩臂‑关节4，其中关节1、关节2、关节3为旋转关节，关节4为平移关节；
步骤二：求机器人的运动学正解，得到由各关节运动学参数表示的机器人末端坐标系相对于基座坐标系的齐次变换矩阵表达式；
步骤三：根据输入的机器人末端位姿向量，求解对应的齐次变换矩阵；
步骤四：根据末端位置与姿态的约束条件，判断输入的末端位姿是否可达，若满足约束条件，则进行后续步骤；否则输入的末端位姿不可达，逆解无解；
步骤五：联立步骤二所述齐次变换矩阵表达式和步骤三所述齐次变换矩阵，求解关节1的转角θ1；
步骤六：代入步骤五所述求得的θ1，对步骤二所述齐次变换矩阵表达式进行分离变量处理，获得关于关节4的位移d4的一元二次方程，若方程无实数解，则输入的末端位姿不可达，逆解无解；若方程有实数解，则根据期望的机器人位形，选择对应解作为关节4的位移d4；
步骤七：代入步骤五所述求得的θ1和步骤六所述求得的d4，由步骤二所述齐次变换矩阵表达式中求得关节2的转角θ2的正弦值与余弦值，进而求解关节2的转角θ2；
步骤八：联立步骤二所述齐次变换矩阵表达式和步骤三所述齐次变换矩阵，求解关节2和关节3的转角和θ2+θ3；
步骤九：将步骤七所述求得的θ2代入步骤八所述求得的θ2+θ3，求解关节3的转角θ3；
步骤十：判断求解得到的θ1、θ2、θ3和d4是否超程，对关节超程错误进行报警，对机器人各关节进行行程约束；
末端坐标系相对于基座坐标系的齐次变换矩阵表达式为：
其中，ci表示cosθi，si表示sinθi，s23表示sin(θ2+θ3)，c23表示cos(θ2+θ3)，a1、a2、d1、d3、d4、d5为常数；
所述步骤三包括：输入的机器人末端位姿向量为：[px py pz γ β α]，则该位姿向量对应的齐次变换矩阵按下式计算：px、py、pz分别表示机器人末端在基座坐标系下的x、y、z位置坐标；γ、β、α分别表示机器人末端绕基座坐标系x、y、z轴的姿态角；nx、ny、nz分别表示机器人末端坐标系的x轴在基座坐标系下的方向余弦；ox、oy、oz分别表示机器人末端坐标系的y轴在基座坐标系下的方向余弦；ax、ay、az分别表示机器人末端坐标系的z轴在基座坐标系下的方向余弦；c表示余弦函数cos()；s表示正弦函数sin()；
若方程无实数解，则说明输入的末端位姿不可达；若有实数解，则可根据一元二次方程的求根公式求得d4的解；
根据给定的机器人位形，选取对应的解作为d4的唯一确定解，设方程的解分别为d′4和d″4，若为短臂位形，则：若为长臂位形，则：
在d4有实数解的情况下，继续计算θ2和θ3的值，如果d4无解，则退出求解并报错。
2.根据权利要求1所述的四自由度串联机器人逆运动学求解方法，其特征在于，将求得的d4代入式(1)，得：因此计算得到θ2的值：
3.根据权利要求2所述的四自由度串联机器人逆运动学求解方法，其特征在于，联立式(1)和式(2)，则有：因此，θ2+θ3的值由下式计算：
θ2+θ3＝Atan2(s23,c23)＝Atan2(az,nz)……(14)。
4.根据权利要求3所述的四自由度串联机器人逆运动学求解方法，其特征在于，θ3的值由下式计算：θ3＝Atan2(az,nz)‑Atan2(s2,c2)……(15)。
5.一种四自由度串联机器人逆运动学求解系统，其特征在于，采用权利要求1‑4中任一项所述的四自由度串联机器人逆运动学求解方法，包括如下模块：模块M1：建立各连杆坐标系并确定各关节的D‑H运动学参数，所述四自由度串联机器人由四个连杆串联构成四个关节，依次为：回转臂‑关节1、大臂‑关节2、外节臂‑关节3和伸缩臂‑关节4，其中关节1、关节2、关节3为旋转关节，关节4为平移关节；
模块M2：求机器人的运动学正解，得到由各关节运动学参数表示的机器人末端坐标系相对于基座坐标系的齐次变换矩阵表达式；
模块M3：根据输入的机器人末端位姿向量，求解对应的齐次变换矩阵；
模块M4：根据末端位置与姿态的约束条件，判断输入的末端位姿是否可达，若满足约束条件，则进行后续模块；否则输入的末端位姿不可达，逆解无解；
模块M5：联立模块M2所述齐次变换矩阵表达式和模块M3所述齐次变换矩阵，求解关节1的转角θ1；
模块M6：代入模块M5所述求得的θ1，对模块M2所述齐次变换矩阵表达式进行分离变量处理，获得关于关节4的位移d4的一元二次方程，若方程无实数解，则输入的末端位姿不可达，逆解无解；若方程有实数解，则根据期望的机器人位形，选择对应解作为关节4的位移d4；
模块M7：代入模块M5所述求得的θ1和模块M6所述求得的d4，由模块M2所述齐次变换矩阵表达式中求得关节2的转角θ2的正弦值与余弦值，进而求解关节2的转角θ2；
模块M8：联立模块M2所述齐次变换矩阵表达式和模块M3所述齐次变换矩阵，求解关节2和关节3的转角和θ2+θ3；
模块M9：将模块M7所述求得的θ2代入模块M8所述求得的θ2+θ3，求解关节3的转角θ3；
模块M10：判断求解得到的θ1、θ2、θ3和d4是否超程，对关节超程错误进行报警，对机器人各关节进行行程约束。
四自由度串联机器人逆运动学求解方法和系统
技术领域
[0001] 本发明涉及机器人运动学及机器人运动控制技术领域，具体地，涉及一种四自由度串联机器人逆运动学求解方法和系统。
背景技术
[0002] 机器人的逆运动学求解问题是机器人学领域的重要问题，是研究机器人轨迹规划、运动控制、动力学分析等问题的前提与基础。机器人逆运动学的求解方法一般分为数值解法和封闭解法。数值解法是求解机器人运动学逆解的通用解法，但由于数值解法的迭代性质，导致其计算量大、求解速度慢，且不能保证求解的精度，更不能保证求出所有可能的运动学逆解。封闭解法即求出机器人运动学逆解的解析解，计算速度快、精度高，且能够求出所有可能的逆解，但封闭解法仅适用于特定构型的机器人，通用性较差。
[0003] 专利文献CN111113425A(申请号：CN201911407238.5)公开了一种有寄生运动的五自由度串并联机器人运动学逆解求解方法，所述方法建立一种三自由度并联机构等效机构，将原串并联机器人转化为由等效机构和两串联关节组成的五自由度串联机构，设末端刀具沿基坐标系Z轴旋转方向自由度为寄生运动，令寄生运动不参与计算，消减方程数量，通过解非线性方程组获得等效关节变量及串联关节驱动变量与末端刀具位置坐标及2个欧拉角的方程，最后利用向量解析法求取等效关节变量与三自由度并联机构驱动变量的关系。
[0004] 目前，针对满足Pieper准则的六自由度串联机器人的逆运动学封闭解法已较为成熟，但该方法无法直接应用于四自由度串联机器人。根据四自由度机器人的构型特点，其运动学逆解也存在多解的情况，且其在操作空间中存在若干特定的运动约束条件。因此，为实现四自由度机器人逆运动学求解的快速、精确、可靠，需要研究一种考虑机器人位形及运动约束的逆运动学封闭解法。
发明内容
[0005] 针对现有技术中的缺陷，本发明的目的是提供一种四自由度串联机器人逆运动学求解方法和系统。
[0006] 根据本发明提供的四自由度串联机器人逆运动学求解方法，包括如下步骤：
[0007] 步骤一：建立各连杆坐标系并确定各关节的D‑H运动学参数，所述四自由度串联机器人由四个连杆串联构成四个关节，依次为：回转臂‑关节1、大臂‑关节2、外节臂‑关节3和伸缩臂‑关节4，其中关节1、关节2、关节3为旋转关节，关节4为平移关节；
[0008] 步骤二：求机器人的运动学正解，得到由各关节运动学参数表示的机器人末端坐标系相对于基座坐标系的齐次变换矩阵表达式；
[0009] 步骤三：根据输入的机器人末端位姿向量，求解对应的齐次变换矩阵；
[0010] 步骤四：根据末端位置与姿态的约束条件，判断输入的末端位姿是否可达，若满足约束条件，则进行后续步骤；否则输入的末端位姿不可达，逆解无解；
[0011] 步骤五：联立步骤二所述齐次变换矩阵表达式和步骤三所述齐次变换矩阵，求解关节1的转角θ1；
[0012] 步骤六：代入步骤五所述求得的θ1，对步骤二所述齐次变换矩阵表达式进行分离变量处理，获得关于关节4的位移d4的一元二次方程，若方程无实数解，则输入的末端位姿不可达，逆解无解；若方程有实数解，则根据期望的机器人位形，选择对应解作为关节4的位移d4；
[0013] 步骤七：代入步骤五所述求得的θ1和步骤六所述求得的d4，由步骤二所述齐次变换矩阵表达式中求得关节2的转角θ2的正弦值与余弦值，进而求解关节2的转角θ2；
[0014] 步骤八：联立步骤二所述齐次变换矩阵表达式和步骤三所述齐次变换矩阵，求解关节2和关节3的转角和θ2+θ3；
[0015] 步骤九：将步骤七所述求得的θ2代入步骤八所述求得的θ2+θ3，求解关节3的转角θ3；
[0016] 步骤十：判断求解得到的θ1、θ2、θ3和d4是否超程，对关节超程错误进行报警，对机器人各关节进行行程约束。
[0017] 优选的，末端坐标系相对于基座坐标系的齐次变换矩阵表达式为：
[0018]
[0019] 其中，ci表示cosθi，si表示sinθi，s23表示sin(θ2+θ3)，c23表示cos(θ2+θ3)，a1、a2、d1、d3、d4、d5为常数。
[0020] 优选的，所述步骤三包括：输入的机器人末端位姿向量为：[px py pz γ β α]，则该位姿向量对应的齐次变换矩阵按下式计算：
[0021]
[0022] px、py、pz分别表示机器人末端在基座坐标系下的x、y、z位置坐标；γ、β、α分别表示机器人末端绕基座坐标系x、y、z轴的姿态角；nx、ny、nz分别表示机器人末端坐标系的x轴在基座坐标系下的方向余弦；ox、oy、oz分别表示机器人末端坐标系的y轴在基座坐标系下的方向余弦；ax、ay、az分别表示机器人末端坐标系的z轴在基座坐标系下的方向余弦；c表示余弦函数cos()；s表示正弦函数sin()。
[0023] 优选的，四自由度串联机器人的运动约束为：
[0024]
[0025] 输入的位置和姿态满足式(3)的约束条件。
[0026] 优选的，由式(1)和式(2)得到：
[0027]
[0028] 因此θ1直接由下式求解：
[0029] θ1＝Atan2(‑ox,oy)……(5)
[0030] 其中，Atan2(x,y)为双参数反正切函数。
[0031] 优选的，关于d4的一元二次方程如下：
[0032]
[0033] 其中：
[0034]
[0035] K2＝d1+d5az‑pz……(8)
[0036] 若方程无实数解，则说明输入的末端位姿不可达；若有实数解，则可根据一元二次方程的求根公式求得d4的解；
[0037] 根据给定的机器人位形，选取对应的解作为d4的唯一确定解，设方程的解分别为d′4和d″4，若为短臂位形，则：
[0038]
[0039] 若为长臂位形，则：
[0040]
[0041] 在d4有实数解的情况下，继续计算θ2和θ3的值，如果d4无解，则退出求解并报错。
[0042] 优选的，将求得的d4代入式(1)，得：
[0043]
[0044] 因此计算得到θ2的值：
[0045] θ2＝Atan2(s2,c2)……(12)。
[0046] 优选的，联立式(1)和式(2)，则有：
[0047]
[0048] 因此，θ2+θ3的值由下式计算：
[0049] θ2+θ3＝Atan2(s23,c23)＝Atan2(az,nz)……(14)。
[0050] 优选的，θ3的值由下式计算：
[0051] θ3＝Atan2(az,nz)‑Atan2(s2,c2)……(15)。
[0052] 根据本发明提供的四自由度串联机器人逆运动学求解系统，包括如下模块：
[0053] 模块M1：建立各连杆坐标系并确定各关节的D‑H运动学参数，所述四自由度串联机器人由四个连杆串联构成四个关节，依次为：回转臂‑关节1、大臂‑关节2、外节臂‑关节3和伸缩臂‑关节4，其中关节1、关节2、关节3为旋转关节，关节4为平移关节；
[0054] 模块M2：求机器人的运动学正解，得到由各关节运动学参数表示的机器人末端坐标系相对于基座坐标系的齐次变换矩阵表达式；
[0055] 模块M3：根据输入的机器人末端位姿向量，求解对应的齐次变换矩阵；
[0056] 模块M4：根据末端位置与姿态的约束条件，判断输入的末端位姿是否可达，若满足约束条件，则进行后续模块；否则输入的末端位姿不可达，逆解无解；
[0057] 模块M5：联立模块M2所述齐次变换矩阵表达式和模块M3所述齐次变换矩阵，求解关节1的转角θ1；
[0058] 模块M6：代入模块M5所述求得的θ1，对模块M2所述齐次变换矩阵表达式进行分离变量处理，获得关于关节4的位移d4的一元二次方程，若方程无实数解，则输入的末端位姿不可达，逆解无解；若方程有实数解，则根据期望的机器人位形，选择对应解作为关节4的位移d4；
[0059] 模块M7：代入模块M5所述求得的θ1和模块M6所述求得的d4，由模块M2所述齐次变换矩阵表达式中求得关节2的转角θ2的正弦值与余弦值，进而求解关节2的转角θ2；
[0060] 模块M8：联立模块M2所述齐次变换矩阵表达式和模块M3所述齐次变换矩阵，求解关节2和关节3的转角和θ2+θ3；
[0061] 模块M9：将模块M7所述求得的θ2代入模块M8所述求得的θ2+θ3，求解关节3的转角θ3；
[0062] 模块M10：判断求解得到的θ1、θ2、θ3和d4是否超程，对关节超程错误进行报警，对机器人各关节进行行程约束。
[0063] 与现有技术相比，本发明具有如下的有益效果：
[0064] (1)本发明能够求出四自由度串联机器人的全部可能逆解的封闭解，求解速度快、精度高；
[0065] (2)本发明能够根据机器人的位形唯一确定机器人的逆解，并判断目标位姿的可达性，能够根据期望的机器人位形求解对应的逆解，在多解的情况下实现逆解可控，有利于进行路径规划及避障；
[0066] (3)本发明考虑了四自由度机器人在操作空间中的运动约束，能够排除不合理的目标位姿对算法的影响；
[0067] (4)本发明考虑了机器人各关节的行程约束，能够对关节超程错误进行报警。
附图说明
[0068] 通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0069] 图1为本发明适用求解的一类四自由度串联机器人的结构示意图；
[0070] 图2为本发明所采用的连杆坐标系建立方法与D‑H运动学参数定义示意图；
[0071] 图3为本发明所建立的四自由度串联机器人的连杆坐标系示意图。
具体实施方式
[0072] 下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
[0073] 实施例1：
[0074] 本发明是通过以下技术方案实现的：一种四自由度串联机器人逆运动学求解方法，该方法是一种考虑机器人位形及运动约束，根据机器人末端位姿求解机器人各关节转角/位移的方法，是一种能够求出一类四自由度串联机器人的全部逆解的封闭解法。
[0075] 所述四自由度串联机器人由四个连杆串联构成四个关节，依次为回转臂(关节1)、大臂(关节2)、外节臂(关节3)和伸缩臂(关节4)，其中关节1、关节2、关节3为旋转关节，关节4为平移关节。
[0076] 所述方法的输入为机器人末端坐标系相对于基座坐标系的期望位姿，以及机器人的期望位形(短臂位形或长臂位形)。
[0077] 所述方法包括以下步骤：
[0078] 步骤一：建立各连杆坐标系并确定各关节的D‑H运动学参数，如图2；
[0079] 步骤二：求机器人的运动学正解，得到由各关节运动学参数表示的机器人末端坐标系相对于基座坐标系的齐次变换矩阵表达式；
[0080] 步骤三：根据输入的机器人末端位姿向量，求解对应的齐次变换矩阵；
[0081] 步骤四：根据末端位置与姿态的约束条件，判断输入的末端位姿是否可达，若满足约束条件，则进行后续步骤；否则输入的末端位姿不可达，逆解无解；
[0082] 步骤五：联立步骤二所述齐次变换矩阵表达式和步骤三所述齐次变换矩阵，求解关节1的转角θ1；
[0083] 步骤六：代入步骤五所述求得的θ1，对步骤二所述齐次变换矩阵表达式进行分离变量处理，获得关于关节4的位移d4的一元二次方程，若方程无实数解，则输入的末端位姿不可达，逆解无解；若方程有实数解，则根据期望的机器人位形，选择合适的解作为关节4的位移d4；
[0084] 步骤七：代入步骤五所述求得的θ1和步骤六所述求得的d4，由步骤二所述齐次变换矩阵表达式中求得关节2的转角θ2的正弦值与余弦值，进而求解关节2的转角θ2；
[0085] 步骤八：联立步骤二所述齐次变换矩阵表达式和步骤三所述齐次变换矩阵，求解关节2和关节3的转角和θ2+θ3；
[0086] 步骤九：将步骤七所述求得的θ2代入步骤八所述求得的θ2+θ3，求解关节3的转角θ3；
[0087] 步骤十：判断求解得到的θ1、θ2、θ3和d4是否超程。
[0088] 实施例2：
[0089] 实施例2是实施例1的优选例。
[0090] 本发明所述一种四自由度串联机器人逆运动学求解方法，适用于如图1所示的一类四自由度串联机器人。其中，关节1为绕竖直轴回转的旋转关节；关节2与关节3平行，为绕水平轴俯仰的旋转关节；关节4为平移关节，平移方向与关节3的旋转轴线垂直。
[0091] 本发明所述一种四自由度串联机器人逆运动学求解方法，包括以下步骤：
[0092] 步骤一：建立各连杆坐标系并确定各关节的D‑H运动学参数。
[0093] 建立四自由度串联机器人的连杆坐标系，如图3所示，其中坐标系1为机器人的基座坐标系，坐标系f为机器人末端坐标系，坐标系5为过渡坐标系。
[0094] 各连杆坐标系对应的D‑H参数如表1所示，其中参数θ1、θ2、θ3、d4为变量，其他参数为常量。
[0095] 表1
[0096]
[0097]
[0098] 步骤二：求机器人的运动学正解，得到由各关节运动学参数表示的机器人末端坐标系相对于基座坐标系的齐次变换矩阵表达式。
[0099] 根据表1所示的D‑H运动学参数，计算得到末端坐标系相对于基座坐标系的齐次变换矩阵：
[0100]
[0101] 其中，ci表示cosθi，si表示sinθi，s23表示sin(θ2+θ3)，c23表示cos(θ2+θ3)。
[0102] 步骤三：根据输入的机器人末端位姿向量，求解对应的齐次变换矩阵。
[0103] 设输入的机器人末端位姿向量为[px py pz γ β α]，则该位姿向量对应的齐次变换矩阵按下式计算：
[0104]
[0105] 步骤四：根据末端位置与姿态的约束条件，判断输入的末端位姿是否可达，若满足约束条件，则进行后续步骤；否则输入的末端位姿不可达，逆解无解。
[0106] 图1所示的四自由度串联机器人的运动约束为：
[0107]
[0108] 即输入的位置和姿态应满足式(3)的约束条件。
[0109] 步骤五：联立步骤二所述齐次变换矩阵表达式和步骤三所述齐次变换矩阵，求解关节1的转角θ1。
[0110] 由式(1)和式(2)可知：
[0111]
[0112] 因此θ1可直接由下式求解：
[0113] θ1＝Atan2(‑ox,oy)……(5)
[0114] 其中，Atan2(x,y)为双参数反正切函数。
[0115] 步骤六：代入步骤五所述求得的θ1，对步骤二所述齐次变换矩阵表达式进行分离变量处理，获得关于关节4的位移d4的一元二次方程，若方程无实数解，则输入的末端位姿不可达，逆解无解；若方程有实数解，则根据期望的机器人位形，选择合适的解作为关节4的位移d4。
[0116] 关于d4的一元二次方程如下：
[0117]
[0118] 其中：
[0119]
[0120] K2＝d1+d5az‑pz……(8)
[0121] 若方程无实数解，则说明输入的末端位姿不可达；若有实数解，则可根据一元二次方程的求根公式求得d4的解。
[0122] 根据给定的机器人位形，选取对应的解作为d4的唯一确定解。设方程的解分别为d′4和d″4，若为短臂位形(即机器人的伸缩臂较短)，则：
[0123]
[0124] 若为长臂位形(即机器人的伸缩臂较长)，则：
[0125]
[0126] 在d4有实数解的情况下，可继续计算θ2和θ3的值。如果d4无解，则退出求解并报错。
[0127] 步骤七：代入步骤五所述求得的θ1和步骤六所述求得的d4，由步骤二所述齐次变换矩阵表达式中求得关节2的转角θ2的正弦值与余弦值，进而求解关节2的转角θ2。
[0128] 将求得的d4代入式(1)，得：
[0129]
[0130] 因此可以计算得到θ2的值：
[0131] θ2＝Atan2(s2,c2)……(12)
[0132] 步骤八：联立步骤二所述齐次变换矩阵表达式和步骤三所述齐次变换矩阵，求解关节2和关节3的转角和θ2+θ3。
[0133] 联立式(1)和式(2)，有：
[0134]
[0135] 因此，θ2+θ3的值可由下式计算：
[0136] θ2+θ3＝Atan2(s23,c23)＝Atan2(az,nz)……(14)
[0137] 步骤九：将步骤七所述求得的θ2代入步骤八所述求得的θ2+θ3，求解关节3的转角θ3。
[0138] θ3的值由下式计算：
[0139] θ3＝Atan2(az,nz)‑Atan2(s2,c2)……(15)
[0140] 步骤十：判断求解得到的θ1、θ2、θ3和d4是否超程。
[0141] 在本申请的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。
[0142] 本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0143] 以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。
