本文仅供学习使用
本文结合个人理解体会作者的思路，也从个人科研角度出发，部分翻译参考由：QQ群（2群legged robot that balance）提供，感谢他们为本文英文版翻译所做的贡献（但有些部分阅读起来仍有困难），若有改进之处欢迎共同讨论！
三维空间中的跳行Ch03
腿部的运动会推动动物向前和向上，因此脚可以沿着行进路线进一步恢复到新的立足点，并且腿部运动还能使动物自我平衡，从而不会翻倒。尽管出现了运动的平面性，但是动物的运动还是在三个维度空间中进行，使得运动具有六个自由度——平面中用于运动的技术是否可以扩展到三维运动中去？ 直线奔跑中的动力学很大程度上取决于矢状面中发生的运动，而垂直与矢状面的运动带来的影响几乎可以忽略（真的可以忽略吗？）。
仍然将运动分解为垂直跳动，前向运动以及姿态控制三个部分
照片序列展示了3D 单腿跳跃机器的一个完整周期。从左至右为运动方向。运动速度约为1.75m/s，其中运动步长为0.63m，单步周期为380ms。
1. 三维空间中的平衡
在每个运动周期中，每次脚与地面的碰撞都会有机会保持或改变前进速度——支撑相（the stance phase），用于控制平面跳跃机的前向速度。将支撑相的时变行为归结为一个单一输出，即净前向加速度（ the net forward acceleration）。整个运动步态的一次循环被视为速度调整的基本单位。在每个周期中，控制系统通过结合中性点和加速度来找到一个合适的位置来放置腿。
- 理想情况下，预测支撑相的身体位置和移动方式将通过求解系统模型的运动方程来进行，然后将脚的位置表示为当前状态和期望加速度的函数。然而，就算是针对非常简单的模型，这些运动方程的解析解也是很难获得的，甚至根本就不存在。
本章中性点的位置是通过水平面中的向量来指定的。当机器人的足端着陆到中性点时，机体历经一个对称轨迹，整个过程净加速度为零。速度和前进方向保持不变。当腿着陆到距离中性点有一定距离的点上的时候，机体依据腿的位移的大小和方向做加速或减速。腿的位置和产生的加速度由水平面内的向量（即跨越量）分别给定。
对于在平面内的前向速度为零的跳跃情况，中性点位于机体的下方。它具有倒立摆模型预测的圆对称性。对于每个足端位移，加速度矢量指向中性点（也是原点）
脚在触地时的位置会影响站立姿势期间的净加速度。每个线段表示在着陆之前将机脚放在相应圆上的净加速度的方向和相对大小。等势线是加速度恒定的线
飞行过程中发生的运动由前向速度矢量，重力矢量和质心位置来定义三维系统在摆动相过程中，质心的运动平面（the plane of motion）。
运动平面通过前向速度矢量、重力矢量以及机器的质心来定义。
当机腿从运动平面横向位移时，净加速度将包含侧向分量，就会给机器一个基于原来前进方向的侧向速度。下一个摆动相的运动平面也会有新的方向，该方向由先前触地时的速度矢量和姿态阶段的净加速度共同决定：
x ˙ i + 1 = x ˙ i + Δ x ˙ {{\dot{x}}_{i+1}}={{\dot{x}}_{i}}+\Delta \dot{x} x˙i+1=x˙i+Δx˙
其中:
x ˙ i {{\dot{x}}_{i}} x˙i：前向速度
Δ x ˙ \Delta \dot{x} Δx˙：净前向加速度矢量
识别一个足端位置的加速，要么通过速度，要么通过方向，但不能同时判断。It is possible to identify the foot positions that accelerate either the speed or the direction, but not both.（没理解）
中性速度位置集（neutral speed locus）是一些足端位置的集合，在集合中的足端位置在提供前进方向变化的同时保留速度的大小（范数大小）。
中性航向位置集（neutral heading locus）能够保留前进方向而调整速度大小。这两个位置集在中性点相交。
对于每个前向速度，对于每个前进速度，都有一个足端的位置集，能够使运动方向改变，而运动速度保持不变——中性速度位置集
2. 三维单腿跳跃机器人
三维单腿跳跃机器人与平面单腿跳跃机器人具有两个基本区别：
- 由于三维机器没有系链与连接杆，所以机体可以在六个自由度上运动
- 髋关节是带有额外自由度的万向节，它允许腿在相对于机体侧向摆动和前后摆动
一对陀螺仪（Gyroscope and Vertical Gyroscope）测量机体在空间中的roll，pitch和yaw三个方向角。在机器的髋部，一对液压执行器可以在机体和机腿之间施加力矩。
压力调节器和止回阀控制机腿执行器上端的压力。腿部机构拥有弹性，当腿长缩短时，滞留在腿部执行器中的空气会被压缩。腿部通过一个万向节型的髋部连接到机体，髋部在一个方向有±30°的摆动而另一个垂直方向具有±20°的摆动。
上气室用作缓冲弹跳，下气室用作执行器。开关电磁阀将机脚气缸的下气室室连接到80 psi的气源或大气。下气室中的高压使得活塞向上运动，缩短腿长。低压使活塞向下运动，增长腿长。在弹跳时，在飞行过程中控制系统向下气室加压，在支撑相排气。推力幅度定义为进气阀在摆动相中打开的时间长度。
在机器人的髋部，一对液压执行器可以输出机体和腿部之间的力矩。控制计算机用线性伺服系统来伺服这些执行器：
τ i = − k p ( w i − w i , d ) − k v ( w ˙ i ) {{\tau }_{i}}=-{{k}_{p}}({{w}_{i}}-{{w}_{i,d}})-{{k}_{v}}({{{\dot{w}}}_{i}}) τi=−kp(wi−wi,d)−kv(w˙i)
其中：
τ i {{\tau }_{i}} τi：第i个执行器的阀门信号
w i {{w}_{i}} wi、 w ˙ i {{{\dot{w}}}_{i}} w˙i：分别是第i个执行器的长度和速度
k p {{k}_{p}} kp、 k v {{k}_{v}} kv：是位置反馈和速度反馈的增益
前进(heading)控制的时候不必将机器的面朝方向(facing direction)考虑进去——转向不需要转身（整个机体没有前后之分）。用前进(heading)表示运动的方向，而面向(facing)则表示关于竖直轴的机体方向。前进(heading)方向和面朝(facing)方向的差值是偏航角(crab angle)。前进（heading）角为：
ψ
=
arctan
(
y
˙
/
x
˙
)
\psi =\arctan (\dot{y}/\dot{x})
ψ=arctan(y˙/x˙)，速度为：
x
˙
=
[
x
˙
,
y
˙
]
T
\dot{\text{x}}={{[\dot{x},\dot{y}]}^{T}}
x˙=[x˙,y˙]T，面向（朝向，facing)角为：
ϕ
Y
{{\phi }_{Y}}
ϕY。
| 参数 | 公制单位 |
|---|---|
| 总高 | 1.10m |
| 总宽 | 0.76m |
| 髋关节高度 | 0.58m |
| 总质量 | 17kg |
| 无弹簧腿质量 | 0.91kg |
| 身体质量/无弹簧腿质量 | 18:1 |
| 身体惯性矩 | 0.709kg·m² |
| 腿部惯性矩 | 0.111kg·m² |
| 身体惯性矩/腿部惯性矩 | 6.4:1 |
| 腿部轴向运动 | |
| 行程 | 0.25m |
| 静态力 | 630N@620kPa |
| 腿部摆动运动 | |
| 摆动角度 | ±0.5rad/±0.33rad |
| x向静转矩 | 90N·m@14MPa |
| y向静转矩 | 136N·m@14MPa |
3. 三维单腿跳跃机器人的控制系统
将控制任务分解成三个独立的部分——前向速度、机体姿态以及跳跃高度；跳跃高度的控制解决方式与平面机器的情况类似。前向速度和机体姿态的控制也基于平面机器的解决方式。
3.1 前向速度控制
前向速度控制系统在每个摆动相期间为腿部选择一个前向位置，该位置可以为下一个支撑相提供期望的净加速度。前向速度的误差决定足端的位置差，该位置差可以提供一个合适的加速度。足端的中性点位置及其位置差共同指定了控制系统放置足端的位置。前向速度控制器在摆动相控制退与髋部的角度。一旦足端接触到地面，姿态控制接管髋部的操作，前向运动成为被动状态。
通过 x ˙ \dot{\text{x}} x˙给定前向速度，足端相对于机体的前向位置由另一个向量 x f {{x}_{f}} xf给定。坐标系的原点与机体的质心一起运动，但是其坐标轴的方向在空间中是固定的。则中性点的的位置可以从CG轨迹中得到：
x f 0 = x ˙ T s 2 {{x}_{f0}}=\frac{\dot{\text{x}}{{T}_{s}}}{2} xf0=2x˙Ts
其中：
x f 0 {{x}_{f0}} xf0：中性点的位置
T s {{T}_{s}} Ts：支撑相的时长
控制系统系统利用线性函数
Δ
x
˙
=
−
x
f
Δ
/
k
x
˙
\Delta \dot{x}=-{{x}_{f\Delta }}/{{k}_{{\dot{x}}}}
Δx˙=−xfΔ/kx˙来近似机腿到中性点
x
f
Δ
{{x}_{f\Delta }}
xfΔ的位置差与净加速度
Δ
x
˙
\Delta \dot{x}
Δx˙（此时对净加速度的理解可视为位移导数度的差分），
k
x
˙
{{k}_{{\dot{x}}}}
kx˙为常数。这种近似忽略了速度与方向的变化。控制系统使用这种近似将前向速度误差转换成足端位置差：
x
f
Δ
=
k
x
˙
(
x
˙
−
x
˙
d
)
{{x}_{f\Delta }}={{k}_{{\dot{x}}}}(\dot{x}-{{\dot{x}}_{d}})
xfΔ=kx˙(x˙−x˙d)。
通过将中性机腿位置及其位差结合在一起，得到了摆动相的前向足端位置公式： x f = x ˙ T s 2 + k x ˙ ( x ˙ − x ˙ d ) {{x}_{f}}=\frac{\dot{x}{{T}_{s}}}{2}+{{k}_{{\dot{x}}}}(\dot{x}-{{\dot{x}}_{d}}) xf=2x˙Ts+kx˙(x˙−x˙d)
- 支撑相的时长 T s {{T}_{s}} Ts受机腿的弹性影响，所以它很大程度上独立于跳跃高度的，对于给定的腿部刚度，近似为常数。控制系统使用上一个支撑相的测量时长作为下一个支撑相的期望时长。期望的前向速度经常由两个操纵杆来控制。
一旦计算出一个期望的足端位置，就会用运动学转换来决定执行器的长度，以正确放置足端位置。 F F F是将髋部执行器和机腿长度转换成足端位置的函数——空间中机体方位 Φ = [ ϕ P , ϕ R , ϕ Y ] T \Phi ={{[{{\phi }_{P}},{{\phi }_{R}},{{\phi }_{Y}}]}^{T}} Φ=[ϕP,ϕR,ϕY]T的隐式函数，其中下标分表表示绕pitch、roll和yaw三个轴的旋转。函数 F F F及其逆函数 F − 1 {{F}^{-1}} F−1由文末给出。一旦执行器长度已知，则髋部伺服器的位置由 τ i = − k p ( w i − w i , d ) − k v ( w ˙ i ) {{\tau }_{i}}=-{{k}_{p}}({{w}_{i}}-{{w}_{i,d}})-{{k}_{v}}({{{\dot{w}}}_{i}}) τi=−kp(wi−wi,d)−kv(w˙i)给出。
对于 x = [ x , y ] T \text{x}={{[x,y]}^{T}} x=[x,y]T可以表示成两个标量方程： { x f = x ˙ T s 2 + k x ˙ ( x ˙ − x ˙ d ) y f = y ˙ T s 2 + k x ˙ ( y ˙ − y ˙ d ) \left\{ \begin{matrix} {{x}_{f}}=\frac{\dot{x}{{T}_{s}}}{2}+{{k}_{{\dot{x}}}}(\dot{x}-{{{\dot{x}}}_{d}}) \\ {{y}_{f}}=\frac{\dot{y}{{T}_{s}}}{2}+{{k}_{{\dot{x}}}}(\dot{y}-{{{\dot{y}}}_{d}}) \\ \end{matrix} \right. {xf=2x˙Ts+kx˙(x˙−x˙d)yf=2y˙Ts+kx˙(y˙−y˙d)，等价于使用两个平面算法操作固定两个角度合适的坐标系。
3.2 从腿部运动中估计前向速度
因为足端在支撑相相对于地面不会移动，可以从测得的腿部相对于髋部的运动中，推导髋部相对于地面的运动：
x
˙
=
−
x
˙
f
\dot{x}=-{{\dot{x}}_{f}}
x˙=−x˙f
通过使用运动学转换，腿部相对于髋部的位置，可以从测得的髋部执行器的长度、腿长以及陀螺仪角度中得到：
x
f
=
F
−
1
(
w
)
{{x}_{f}}={{F}^{-1}}(w)
xf=F−1(w)
3.3 控制机体姿态
控制系统通过给腿部与机体之间髋部关节施加力矩来维持机体在支撑期支撑相的直立姿势，测量机体的pitch和roll方向角的陀螺仪是对齐的，所以pitch轴平行于髋部的一个轴，而roll轴平行于髋部的另一个轴。这样就姿态控制伺服器使用不需转换角度结果得到pitch和roll值，直接伺服髋部执行器：
{ τ 1 = k p ( ϕ P − ϕ P , d ) − k v ( ϕ ˙ P ) τ 2 = k p ( ϕ R − ϕ R , d ) − k v ( ϕ ˙ R ) \left\{ \begin{matrix} {{\tau }_{1}}={{k}_{p}}({{\phi }_{P}}-{{\phi }_{P,d}})-{{k}_{v}}({{{\dot{\phi }}}_{P}}) \\ {{\tau }_{2}}={{k}_{p}}({{\phi }_{R}}-{{\phi }_{R,d}})-{{k}_{v}}({{{\dot{\phi }}}_{R}}) \\ \end{matrix} \right. {τ1=kp(ϕP−ϕP,d)−kv(ϕ˙P)τ2=kp(ϕR−ϕR,d)−kv(ϕ˙R)
其中：
τ 1 {{\tau }_{1}} τ1、 τ 2 {{\tau }_{2}} τ2：关于髋部执行器的控制信号
ϕ P {{\phi }_{P}} ϕP、 ϕ R {{\phi }_{R}} ϕR：机体的pitch和roll角
k p {{k}_{p}} kp、 k v {{k}_{v}} kv：增益系数
关于yaw轴的方向角是机体姿态的第三个自由度，即面向（facing）。尽管pitch和roll角是相对容易控制的，而yaw却是要更难一些——没有提供施加扭矩牵引力的执行器（提供矫正航向运动的力矩）。在航行方向上没有首选方向，允许机器人在其yaw轴方向上自由旋转。
为了补偿诱发yaw轴回转力矩时所需的pitch运动以及机腿的侧向位移，控制系统可以在交替步骤中使用位移补偿对。（To compensate for the induced pitching motion and lateral displacement, the control system could use compensating pairs of displacements on alternate steps.可以理解为两次有偏的跳跃），例如，它可以在前倾移同时向左迈腿，而在下一步时则向后倾的同时并向右迈腿。这可以在交替步骤中产生扰动补偿对，但是每一步都会产生逆时针的yaw扭矩。
不幸的是，该方法中能够产生的最大的yaw扭矩，实际上也会比由跳跃机器的系链电缆所带来的的扰动小得多。因此，控制系统不能产生一个合适的yaw力矩来控制机体朝向。
4. 三维空间中的跳跃实验
4.1 速率控制
在运动过程中，腿部与机体反向震荡。pitch和roll震荡的相对大小会变动，因为机器会沿着yaw轴进行旋转，腿部的前后扫动并未明确编程， 而是由在飞行过程中将足端定位的前向速度控制与在姿态过程中使机架直立的姿势控制之间的相互作用而产生的。
在另一个实验中，期望的速度是一个常量，而期望的方向会突然改变90°。跳跃机器人花了1秒钟，即两步，来改变方向，在此以后它继续运动。机器人在转向时失去速度，在那以后又重新获得速度。
所需方向上的阶跃变化会在保持所需速度恒定的同时生成直角转弯。
4.2 位置控制
位置控制可以用来使跳行机器人在一个位置持续跳动，以及使其从一个位置移动至另一个位置。将位置误差转化为期望速度的位置控制算法：
x ˙ d = min { − k p ( x − x d ) − k v x ˙ , x ˙ m a x } {{\dot{x}}_{d}}=\min \{-{{k}_{p}}(x-{{x}_{d}})-{{k}_{v}}\dot{x},\quad {{\dot{x}}_{max}}\} x˙d=min{−kp(x−xd)−kvx˙,x˙max}
其中：
k p {{k}_{p}} kp、 k v {{k}_{v}} kv：位置和速度的反馈增益。
x ˙ m a x {{\dot{x}}_{max}} x˙max：允许速度的上限。
机器人在房间中的位置由两种方法确定：一种是对估计出来的前向速度 x ˙ \dot{x} x˙作积分，另一种是使用光电传感器来追踪机体上的光源（类似于同步卫星）。
积分得到的数据和光电传感器的测量值之间的差异——积分器固定的5mm/step的积分飘移。这意味着，如果命令机器跳到一个点并且使用积分器进行控制，则可能会在一分钟内漂移一米。对于人蒙着双眼时的单腿跳跃的非正式实验表明，他们经常以相似的量漂移。对于跳跃机器人的情况，漂移的主要来源是陀螺仪的校准误差以及系链电缆在机器人上施加的未被建模的力——当希望前向速度恒定时，系链电缆却会在摆动相过程中进一步加速机器人。
当机器人跳动到指定位置时，向机器的身体施加了一个水平冲击（图的垂直虚线）。机器人仍保持平衡并在几秒钟后返回到位置设定点。当在roll轴和pitch轴上受到较大的扭转扰动时，系统也能保持其平衡。
左图为三维跳行机器人遍历方形路径时的数据记录。控制系统对前向速度积分，以估计和控制机器的位置；右图为一个固定在机体顶端的小灯源，光电系统提供的位置信息被用于控制。
4.3 圆形奔跑
机腿从运动平面的偏离会引入一个侧向加速度并改变前进方向。控制系统可以操作机腿的侧向位移使机器沿着特定曲率的圆轮廓运动。在单腿跳跃前进方向的改变量是前向速度和净加速度向量的函数：
Δ
ψ
=
arccos
(
x
˙
⋅
Δ
x
˙
∣
x
˙
∣
∣
Δ
x
˙
∣
)
\Delta \psi =\arccos (\frac{\dot{x}\cdot \Delta \dot{x}}{|\dot{x}||\Delta \dot{x}|})
Δψ=arccos(∣x˙∣∣Δx˙∣x˙⋅Δx˙)
控制系统可以通过调整机腿侧向位移和奔跑速度来控制转弯量。
（粗线：
∣
x
˙
∣
=
0.6
m
/
s
|\dot{x}|=0.6m/s
∣x˙∣=0.6m/s ，
x
f
Δ
=
0.04
m
/
s
{{\text{x}}_{f\Delta }}=0.04m/s
xfΔ=0.04m/s；实线：
∣
x
˙
∣
=
0.4
m
/
s
|\dot{x}|=0.4m/s
∣x˙∣=0.4m/s ，
x
f
Δ
=
0.04
m
/
s
{{\text{x}}_{f\Delta }}=0.04m/s
xfΔ=0.04m/s ；点划线：
∣
x
˙
∣
=
0.6
m
/
s
|\dot{x}|=0.6m/s
∣x˙∣=0.6m/s，
x
f
Δ
=
0.02
m
/
s
{{\text{x}}_{f\Delta }}=0.02m/s
xfΔ=0.02m/s ）
通过改变侧向机腿偏移量和速度都可以改变曲率半径。机器改变运动路径曲率半径的能力反映了在任意平面轮廓路径的通行能力。
跳行机器人的方形仿真路径：上面的轨迹标记出髋部的路径，下面的轨迹标记出机腿的路径。
5. 总结
-
前向速度的控制需要矢量计算以确定足端的合适位置，以保存平衡并提供加速度。通过检视运动平面的中的速度和加速的情况，可以使足端放置更加简单。为了控制身体姿势，控制系统在允许机器人自由旋转，但对机架的pitch和roll轴进行伺服控制。
-
在原地跳动和运动时，三维跳行机器人能够在没有外部支撑的情况下就保持平衡。以±0.25 m/s的精度跟踪期望速度（斜率）以及方向的突然变化。在较高速度下，由于估算CG投影的长度和中性点的位置都不够精确，系统始终比指定的运行速度慢。最高记录的运行速度为2.2m / s（4.8mph）。
-
控制系统可以通过对估计出的前向速度积分，确定出机器人的位置。在设置固定位置点时，机器能够以大约±0.2 m的精度跳动到位。机器也遍历了方形路径，但是由于系链在方形的一侧上施加了较大的干扰力，因此精度不是很高。手动施加扰动时，机器能够保持平衡。
6. 附录：运动学
定义了三个坐标系{W}，{H}和{B}：
- 坐标系{W}是固定在地面的世界坐标系。
- 坐标系{H}的原点随髋部一起运动，但是它的方向保持与{W}平行。 可以将坐标系{H}视为固定在陀螺仪最里面的万向节上。对于{W}和{H}，其z轴与重力矢量对齐并指向上。
- 坐标系{B}固定在机体上，它的原点同样随髋部一起运动，但是{B}相对于{W}和{H}会改变方向。欧拉角指定{B}坐标系的方向，分别是 ( ϕ Y , ϕ R , ϕ P ) ({{\phi }_{Y}},{{\phi }_{R}},{{\phi }_{P}}) (ϕY,ϕR,ϕP) 。髋部和腿部执行器决定了足端在坐标系{B}中位置。
假定
P
x
^{P}x
Px表示坐标系{P}中的向量。则坐标系{B}到坐标系{H}的变换关系为：
H
x
=
B
H
T
B
x
^{H}x={}_{B}^{H}{{T}^{B}}x
Hx=BHTBx，其中：
B
H
T
=
[
cos
ϕ
P
cos
ϕ
Y
−
sin
ϕ
P
sin
ϕ
R
sin
ϕ
Y
−
cos
ϕ
R
sin
ϕ
Y
−
cos
ϕ
P
sin
ϕ
R
sin
ϕ
Y
−
cos
ϕ
Y
sin
ϕ
P
0
cos
ϕ
P
sin
ϕ
Y
+
sin
ϕ
P
sin
ϕ
R
sin
ϕ
Y
cos
ϕ
R
cos
ϕ
Y
cos
ϕ
P
sin
ϕ
R
cos
ϕ
Y
−
sin
ϕ
Y
sin
ϕ
P
0
cos
ϕ
R
sin
ϕ
P
−
sin
ϕ
R
cos
ϕ
P
cos
ϕ
R
0
0
0
0
1
]
{}_{B}^{H}T=\left[ \begin{matrix} \cos {{\phi }_{P}}\cos {{\phi }_{Y}}-\sin {{\phi }_{P}}\sin {{\phi }_{R}}\sin {{\phi }_{Y}} & -\cos {{\phi }_{R}}\sin {{\phi }_{Y}} & -\cos {{\phi }_{P}}\sin {{\phi }_{R}}\sin {{\phi }_{Y}}-\cos {{\phi }_{Y}}\sin {{\phi }_{P}} & 0 \\ \cos {{\phi }_{P}}\sin {{\phi }_{Y}}+\sin {{\phi }_{P}}\sin {{\phi }_{R}}\sin {{\phi }_{Y}} & \cos {{\phi }_{R}}\cos {{\phi }_{Y}} & \cos {{\phi }_{P}}\sin {{\phi }_{R}}\cos {{\phi }_{Y}}-\sin {{\phi }_{Y}}\sin {{\phi }_{P}} & 0 \\ \cos {{\phi }_{R}}\sin {{\phi }_{P}} & -\sin {{\phi }_{R}} & \cos {{\phi }_{P}}\cos {{\phi }_{R}} & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]
BHT=⎣
⎡cosϕPcosϕY−sinϕPsinϕRsinϕYcosϕPsinϕY+sinϕPsinϕRsinϕYcosϕRsinϕP0−cosϕRsinϕYcosϕRcosϕY−sinϕR0−cosϕPsinϕRsinϕY−cosϕYsinϕPcosϕPsinϕRcosϕY−sinϕYsinϕPcosϕPcosϕR00001⎦
⎤
从坐标系{H}到{B}的变换为：
B
x
=
H
B
T
H
x
^{B}x={}_{H}^{B}{{T}^{H}}x
Bx=HBTHx，其中：
H
B
T
=
[
cos
ϕ
P
cos
ϕ
Y
−
sin
ϕ
P
sin
ϕ
R
sin
ϕ
Y
cos
ϕ
P
sin
ϕ
Y
+
cos
ϕ
Y
sin
ϕ
P
sin
ϕ
R
sin
ϕ
P
cos
ϕ
R
0
−
cos
ϕ
R
sin
ϕ
Y
cos
ϕ
R
cos
ϕ
Y
−
sin
ϕ
R
0
−
cos
ϕ
P
sin
ϕ
R
sin
ϕ
Y
−
sin
ϕ
P
cos
ϕ
Y
cos
ϕ
P
sin
ϕ
R
cos
ϕ
Y
−
sin
ϕ
Y
sin
ϕ
P
cos
ϕ
P
cos
ϕ
R
0
0
0
0
1
]
{}_{H}^{B}T=\left[ \begin{matrix} \cos {{\phi }_{P}}\cos {{\phi }_{Y}}-\sin {{\phi }_{P}}\sin {{\phi }_{R}}\sin {{\phi }_{Y}} & \cos {{\phi }_{P}}\sin {{\phi }_{Y}}+\cos {{\phi }_{Y}}\sin {{\phi }_{P}}\sin {{\phi }_{R}} & \sin {{\phi }_{P}}\cos {{\phi }_{R}} & 0 \\ -\cos {{\phi }_{R}}\sin {{\phi }_{Y}} & \cos {{\phi }_{R}}\cos {{\phi }_{Y}} & -\sin {{\phi }_{R}} & 0 \\ -\cos {{\phi }_{P}}\sin {{\phi }_{R}}\sin {{\phi }_{Y}}-\sin {{\phi }_{P}}\cos {{\phi }_{Y}} & \cos {{\phi }_{P}}\sin {{\phi }_{R}}\cos {{\phi }_{Y}}-\sin {{\phi }_{Y}}\sin {{\phi }_{P}} & \cos {{\phi }_{P}}\cos {{\phi }_{R}} & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]
HBT=⎣
⎡cosϕPcosϕY−sinϕPsinϕRsinϕY−cosϕRsinϕY−cosϕPsinϕRsinϕY−sinϕPcosϕY0cosϕPsinϕY+cosϕYsinϕPsinϕRcosϕRcosϕYcosϕPsinϕRcosϕY−sinϕYsinϕP0sinϕPcosϕR−sinϕRcosϕPcosϕR00001⎦
⎤
很容易能够看出上述两个矩阵具有相同的对角元——单位正交阵
髋部执行器长度用
w
1
{{w}_{1}}
w1、
w
2
{{w}_{2}}
w2表示，机腿长度用r表示。从执行器长度到脚相对于髋部的机腿位置做正向变换，足端点在机架坐标系{B}中表示为：
B
x
f
=
A
B
T
(
w
)
=
{
x
f
=
r
cos
(
arccos
(
w
1
2
−
l
1
2
−
l
2
2
2
l
1
l
2
)
+
α
)
y
f
=
r
cos
(
arccos
(
w
2
2
−
l
1
2
−
l
3
2
2
l
1
l
3
)
+
β
)
z
f
=
−
r
2
−
x
f
2
−
y
f
2
^{B}{{x}_{f}}={}_{A}^{B}T(w)=\left\{ \begin{matrix} {{x}_{f}}=r\cos (\arccos (\frac{{{w}_{1}}^{2}-{{l}_{1}}^{2}-{{l}_{2}}^{2}}{2{{l}_{1}}{{l}_{2}}})+\alpha ) \\ {{y}_{f}}=r\cos (\arccos (\frac{{{w}_{2}}^{2}-{{l}_{1}}^{2}-{{l}_{3}}^{2}}{2{{l}_{1}}{{l}_{3}}})+\beta ) \\ {{z}_{f}}=-\sqrt{{{r}^{2}}-{{x}_{f}}^{2}-{{y}_{f}}^{2}} \\ \end{matrix} \right.
Bxf=ABT(w)=⎩
⎨
⎧xf=rcos(arccos(2l1l2w12−l12−l22)+α)yf=rcos(arccos(2l1l3w22−l12−l32)+β)zf=−r2−xf2−yf2
此处x与y的坐标求解中l1的取值需要注意
从足端位置到执行器长度逆向变换为：
w
=
A
B
T
(
B
x
f
)
=
{
w
1
=
l
1
2
+
l
2
2
−
2
l
1
l
2
cos
(
arccos
(
x
f
r
)
)
−
α
w
2
=
l
1
2
+
l
3
2
−
2
l
1
l
3
cos
(
arccos
(
y
f
r
)
)
−
β
r
=
x
f
2
+
y
f
2
+
z
f
2
w={}_{A}^{B}T{{(}^{B}}{{x}_{f}})=\left\{ \begin{matrix} {{w}_{1}}=\sqrt{{{l}_{1}}^{2}+{{l}_{2}}^{2}-2{{l}_{1}}{{l}_{2}}\cos (\arccos (\frac{{{x}_{f}}}{r}))-\alpha } \\ {{w}_{2}}=\sqrt{{{l}_{1}}^{2}+{{l}_{3}}^{2}-2{{l}_{1}}{{l}_{3}}\cos (\arccos (\frac{{{y}_{f}}}{r}))-\beta } \\ r=\sqrt{{{x}_{f}}^{2}+{{y}_{f}}^{2}+{{z}_{f}}^{2}} \\ \end{matrix} \right.
w=ABT(Bxf)=⎩
⎨
⎧w1=l12+l22−2l1l2cos(arccos(rxf))−αw2=l12+l32−2l1l3cos(arccos(ryf))−βr=xf2+yf2+zf2
显然可以看出上述两式之间的变换关系，此处作者求解r处存在笔误（或以实际情况为准取负号）
执行器变量和足端位置之间的整体正向和反向转换为：
{
H
x
f
=
B
H
T
A
B
T
(
w
)
=
F
−
1
(
w
)
w
=
B
A
T
(
H
B
T
H
x
f
)
=
F
(
H
x
f
)
\left\{ \begin{matrix} ^{H}{{x}_{f}}={}_{B}^{H}T{}_{A}^{B}T(w)={{F}^{-1}}(w) \\ w={}_{B}^{A}T({}_{H}^{B}{{T}^{H}}{{x}_{f}})=F{{(}^{H}}{{x}_{f}}) \\ \end{matrix} \right.
{Hxf=BHTABT(w)=F−1(w)w=BAT(HBTHxf)=F(Hxf)
1886
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
