Open imuncle opened 3 years ago
碎碎念：俩月没写文章了，五月初终于得以返校，然后两周狂肝毕业设计，还好最后顺利毕业。6.10答辩，6.17毕业典礼，然后就回家了。回家后这段时间一直在参加今年优必选依托WAIC大会举办的WALKER仿真挑战赛，控制WALKER机器人完成指定任务。其中一项任务如下图所示。
最后拿了第四名（前三有奖金），哎，还是差一点点。
这篇文章就记录一下我在比赛期间研究的机械臂角度解算，摘录自技术报告的第二章。
Walker机器人为仿人型机器人，普通的工业机械臂只需6个自由度即可到达活动空间内的任何位置，而人类的手臂因各个关节有角度限制，需要新增一个大臂扭转的自由度，所以Walker机器人的仿人机械臂为7自由冗余机械臂，使用传统的D-H模型求逆解的话会得到无数个解，一般选择先给定多出来的大臂扭转关节的角度，再求解其余6个自由度。
本队采用几何法求解机械臂的关节角度，遍历了大臂扭转关节的所有角度值，分别求解出对应的其余关节的角度值，然后设计代价函数，选取与上一个状态的角度相差最小的解作为当前状态的角度解。
此处以机器人左臂为例，对机械臂的逆运动学解算过程作简要介绍。左臂的自由度分布关系如图2-1所示。
为简化分析，可以将肩部的关节①②③合并为一个球关节，将腕部的关节⑤⑥⑦合并为一个球关节，最后简化图如图2-2所示。为方便解算，我们采用位姿分离的方法分析，即任务腕关节只影响末端手掌的姿态，肩部关节和肘部关节只影响腕部的空间坐标。首先根据腕部的空间坐标求解前四个关节角度。
机械臂大臂和小臂的任一姿态可由图2-3表示。因为肘部不能外拐，所以关节④的角度一定是一个负值，如图所示，在△OAB中可由余弦定理求得∠OAB，进而可求得关节4的角度。
根据B点的坐标，可以求得角α和角β。如果关节③的角度为0，那么显然机械臂OAB只会在面OCB内运动，而随着关节③角度的变化，A点可以绕直线OB作圆周运动，设面OAB与面OCB的夹角为θ。将基坐标系Oxyz依次绕x轴、y轴旋转α和β角度，得到新的坐标系Ox’y’z’，如图2-4所示。在新的坐标系中，从面x’Oy’看A点，如图2-5所示，因此可以求得A点在坐标系Ox’y’z’中的坐标，然后再根据基坐标系和新的坐标系的转换关系即可求得A点在基坐标系中的坐标，进而求得关节①和关节②的角度。
A点在坐标系Ox’y’z’中的坐标(xA’, yA’, zA’)与在基坐标系Oxyz中的坐标(xA, yA, zA)的转换关系如下所示。
针对关节③的角度求解，将基坐标系Oxyz依次绕x轴、y轴旋转关节①和关节②的角度，得到坐标系Ox’’y’’z’’，如图2-6所示。将AB投影至Ox’’y’’面，关节③的角度即为图中γ角。
在求解到前4个关节的角度之后，即可求得AB杆在基坐标系中的方位（用旋转矩阵表示），而手掌的方位是已知量，即可求得手掌相对于AB的旋转矩阵。该旋转矩阵使用腕部的三个关节的角度w1、w2、w3表示如下。
从上式可以看出，在求得旋转矩阵后，可以很方便地从中提取出三个关节的角度。至此，机械臂的七个自由度的角度均确定了。
我们以一定的步长遍历了面OAB与面OCB的夹角，求得了多组逆解，将这些解与上一个状态的角度相减，使用如下的代价函数评价角度的变化程度，选取所有逆解中代价函数最小的解作为当前状态的角度值，使得手臂在移动的过程中保持平滑。
上式中|joints(i)|表示第i个关节的角度差值。
碎碎念：俩月没写文章了，五月初终于得以返校，然后两周狂肝毕业设计，还好最后顺利毕业。6.10答辩，6.17毕业典礼，然后就回家了。回家后这段时间一直在参加今年优必选依托WAIC大会举办的WALKER仿真挑战赛，控制WALKER机器人完成指定任务。其中一项任务如下图所示。
最后拿了第四名（前三有奖金），哎，还是差一点点。
这篇文章就记录一下我在比赛期间研究的机械臂角度解算，摘录自技术报告的第二章。
Walker机器人为仿人型机器人，普通的工业机械臂只需6个自由度即可到达活动空间内的任何位置，而人类的手臂因各个关节有角度限制，需要新增一个大臂扭转的自由度，所以Walker机器人的仿人机械臂为7自由冗余机械臂，使用传统的D-H模型求逆解的话会得到无数个解，一般选择先给定多出来的大臂扭转关节的角度，再求解其余6个自由度。
本队采用几何法求解机械臂的关节角度，遍历了大臂扭转关节的所有角度值，分别求解出对应的其余关节的角度值，然后设计代价函数，选取与上一个状态的角度相差最小的解作为当前状态的角度解。
此处以机器人左臂为例，对机械臂的逆运动学解算过程作简要介绍。左臂的自由度分布关系如图2-1所示。
为简化分析，可以将肩部的关节①②③合并为一个球关节，将腕部的关节⑤⑥⑦合并为一个球关节，最后简化图如图2-2所示。为方便解算，我们采用位姿分离的方法分析，即任务腕关节只影响末端手掌的姿态，肩部关节和肘部关节只影响腕部的空间坐标。首先根据腕部的空间坐标求解前四个关节角度。
机械臂大臂和小臂的任一姿态可由图2-3表示。因为肘部不能外拐，所以关节④的角度一定是一个负值，如图所示，在△OAB中可由余弦定理求得∠OAB，进而可求得关节4的角度。
根据B点的坐标，可以求得角α和角β。如果关节③的角度为0，那么显然机械臂OAB只会在面OCB内运动，而随着关节③角度的变化，A点可以绕直线OB作圆周运动，设面OAB与面OCB的夹角为θ。将基坐标系Oxyz依次绕x轴、y轴旋转α和β角度，得到新的坐标系Ox’y’z’，如图2-4所示。在新的坐标系中，从面x’Oy’看A点，如图2-5所示，因此可以求得A点在坐标系Ox’y’z’中的坐标，然后再根据基坐标系和新的坐标系的转换关系即可求得A点在基坐标系中的坐标，进而求得关节①和关节②的角度。
A点在坐标系Ox’y’z’中的坐标(xA’, yA’, zA’)与在基坐标系Oxyz中的坐标(xA, yA, zA)的转换关系如下所示。
针对关节③的角度求解，将基坐标系Oxyz依次绕x轴、y轴旋转关节①和关节②的角度，得到坐标系Ox’’y’’z’’，如图2-6所示。将AB投影至Ox’’y’’面，关节③的角度即为图中γ角。
在求解到前4个关节的角度之后，即可求得AB杆在基坐标系中的方位（用旋转矩阵表示），而手掌的方位是已知量，即可求得手掌相对于AB的旋转矩阵。该旋转矩阵使用腕部的三个关节的角度w1、w2、w3表示如下。
从上式可以看出，在求得旋转矩阵后，可以很方便地从中提取出三个关节的角度。至此，机械臂的七个自由度的角度均确定了。
我们以一定的步长遍历了面OAB与面OCB的夹角，求得了多组逆解，将这些解与上一个状态的角度相减，使用如下的代价函数评价角度的变化程度，选取所有逆解中代价函数最小的解作为当前状态的角度值，使得手臂在移动的过程中保持平滑。
上式中|joints(i)|表示第i个关节的角度差值。