【关于四足机器人那些事】姿态调节-俯仰角
本篇将会对四足机器人的俯仰角进行数学建模，求解器正逆运动学解，包含详细公式推导与计算
一、数学建模
1、几何模型
假设俯仰角为 λ \lambda λ,对于坐标系A足端位置 P A [ x , y ] P_A[x, y] PA[x,y]，我们求出其在B坐标系下的位置 P B = [ x ′ , y ′ ] P_B=[x',y'] PB=[x′,y′]，然后通过逆运动学求解关节角度 θ 1 , θ 2 \theta_1,\theta_2 θ1,θ2即可。假定机器人半身长为L，腿长分别为L1， L2
2、坐标变换
前腿
a、坐标系A-O变换矩阵：
T 1 = [ 1 0 L 0 1 0 0 0 1 ] T_1 = \begin{bmatrix} 1 & 0 & L\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix} T1=⎣⎡100010L01⎦⎤
b、坐标系O-B变换矩阵：
T 2 = [ cos λ − sin λ − L sin λ cos λ 0 0 0 1 ] T_2 = \begin{bmatrix} \cos\lambda & -\sin\lambda & -L\\ \sin\lambda & \cos\lambda & 0\\ 0 & 0 & 1 \end{bmatrix} T2=⎣⎡cosλsinλ0−sinλcosλ0−L01⎦⎤
根据变换矩阵，我们可以求出 P B P_B PB坐标：
P B = T 2 T 1 P A P_B = T_2T_1P_A PB=T2T1PA
即：
[ x ′ y ′ 1 ] = [ cos λ − sin λ − L sin λ cos λ 0 0 0 1 ] [ 1 0 L 0 1 0 0 0 1 ] [ x y 1 ] \begin{bmatrix}x'\\ y'\\1 \end{bmatrix} = \begin{bmatrix} \cos\lambda & -\sin\lambda & -L\\ \sin\lambda & \cos\lambda & 0\\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} 1 & 0 & L\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x\\ y \\1 \end{bmatrix} ⎣⎡x′y′1⎦⎤=⎣⎡cosλsinλ0−sinλcosλ0−L01⎦⎤⎣⎡100010L01⎦⎤⎣⎡xy1⎦⎤
化简之后：
[ x ′ y ′ 1 ] = [ cos λ − sin λ L cos λ − L sin λ cos λ L sin λ 0 0 1 ] [ x y 1 ] \begin{bmatrix}x'\\ y'\\1 \end{bmatrix} =\left[\begin{matrix}\cos\lambda & - \sin\lambda & L \cos\lambda - L\\\sin\lambda & \cos\lambda & L \sin\lambda \\0 & 0 & 1\end{matrix}\right]\begin{bmatrix} x\\ y \\1 \end{bmatrix} ⎣⎡x′y′1⎦⎤=⎣⎡cosλsinλ0−sinλcosλ0Lcosλ−LLsinλ1⎦⎤⎣⎡xy1⎦⎤
后腿
对于后腿实际上只是L取反即可：
[ x ′ y ′ 1 ] = [ cos λ − sin λ − L cos λ + L sin λ cos λ − L sin λ 0 0 1 ] [ x y 1 ] \begin{bmatrix}x'\\ y'\\1 \end{bmatrix} =\left[\begin{matrix}\cos\lambda & - \sin\lambda & -L \cos\lambda +L\\\sin\lambda & \cos\lambda & -L \sin\lambda \\0 & 0 & 1\end{matrix}\right]\begin{bmatrix} x\\ y \\1 \end{bmatrix} ⎣⎡x′y′1⎦⎤=⎣⎡cosλsinλ0−sinλcosλ0−Lcosλ+L−Lsinλ1⎦⎤⎣⎡xy1⎦⎤
3、逆运动学求解
根据离地高度确定初始位置 P 0 P0 P0，再根据变换矩阵将其转换致参考系B中得到 P B P_B PB，通过逆运动学求解两个关节角度，公式如下：
cos β = − l 1 2 − l 2 2 + x 2 + y 2 2 l 1 l 2 sin β = 1 − cos 2 β β = a t a n 2 ( s i n β , c o s β ) α = a t a n 2 ( y , x ) − a t a n 2 ( l 2 s 2 , l 1 + l 2 c 2 ) \begin{matrix} \cos\beta = \frac{- l_{1}^{2} - l_{2}^{2} + x^{2} + y^{2}}{2 l_{1} l_{2}} \\ \\ \sin\beta = \sqrt{1-\cos^2\beta} \\ \\ \beta = atan2(sin\beta, cos\beta) \\ \\ \alpha = atan2(y,x) - atan2( l_2s_2 ,l_1 + l2c_2)\end{matrix} cosβ=2l1l2−l12−l22+x2+y2sinβ=1−cos2ββ=atan2(sinβ,cosβ)α=atan2(y,x)−atan2(l2s2,l1+l2c2)
4、最大俯仰角
对于给定离地高度h，俯仰角存在一个最大值，在实际控制时，我们需要保证俯仰角在允许范围内。
如上图，当L1， L1共线时，俯仰角最大，此时有以下关系：
cos ( r + φ ) = L 2 + ( h 2 + L 2 ) − ( L 1 + L 2 ) 2 2 L ( L 2 + h 2 ) \cos(r+\varphi) = \frac{L^2+(h^2+L^2) - (L1+L2)^2}{2L(\sqrt{L^2+h^2})} cos(r+φ)=2L(L2+h2)L2+(h2+L2)−(L1+L2)2
其中 φ = a t a n 2 ( h , L ) \varphi = atan2(h, L) φ=atan2(h,L)，因此我们可以解出此时的 r r r
