五连杆正运动学解算

如图所示五连杆模型，其中、为关节电机；、为电机输出角度，可由编码器读出；为连杆长度。需要求解和。

以中点为原点，为轴，垂直的向上为轴，建立平面直角坐标系。设的坐标分别为。

对于点，有：

对公式移项并平方相加，得：

令，则有：

移项，令，则有：

由二倍角公式，有：

则式可化为：

整理得：

式为关于的一元二次方程，其判别式：

其解为：

则有：

考虑机械结构及限位，，当时，判别式恒成立：

易知式的解其实就是和。根据图中关系有：

因此取正根：

由此可求出点坐标：

化为极坐标系，解出和：

至此，五连杆正运动学解算完毕。