连续体机器人常曲率运动学建模

连续体机器人常曲率运动学建模

本文系统推导了基于常曲率假设的连续体机器人运动学模型，包括完整的正逆运动学求解及末端位姿计算。

---

1. 符号定义

符号	含义
$\theta$	弯曲角度（圆心角），$\theta \in [0, \pi]$
$\varphi$	方位角（弯曲方向），$\varphi \in [0, 2\pi]$
$L$	连续体长度（中性线弧长）
$\rho$	曲率半径，$\rho = L/\theta$
$r$	驱动绳分度圆半径
$\Delta l_i$	第$i$根驱动绳位移量，$i = 1,2,3,4$
$(p_x, p_y, p_z)$	末端位置
$(\alpha, \beta, \gamma)$	末端姿态欧拉角

三个空间的映射关系：

• 任务空间：$(p_x, p_y, p_z, \alpha, \beta, \gamma)$ — 末端6自由度位姿
• 配置空间：$(\theta, \varphi)$ — 常曲率参数
• 驱动空间：$(\Delta l_1, \Delta l_2, \Delta l_3, \Delta l_4)$ — 驱动绳位移

---

2. 常曲率模型几何关系

2.1 末端位置

根据常曲率几何模型，末端位置与配置参数的关系为：

$$
\boxed{
\begin{cases}
p_x = \rho\cos\varphi(1-\cos\theta) \[8pt]
p_y = \rho\sin\varphi(1-\cos\theta) \[8pt]
p_z = \rho\sin\theta
\end{cases}
}
$$

MathType版本：

\left\{ {\begin{array}{*{20}{l}}
{{p_x} = \rho \cos \varphi (1 - \cos \theta )}\\
{{p_y} = \rho \sin \varphi (1 - \cos \theta )}\\
{{p_z} = \rho \sin \theta }
\end{array}} \right.

其中 $\rho = L/\theta$ 为曲率半径。

2.2 末端姿态

末端姿态的欧拉角直接由配置参数给出：

$$
\boxed{
\begin{cases}
\beta = \theta \quad \text{（俯仰角）} \[6pt]
\gamma = \varphi \quad \text{（偏航角）} \[6pt]
\alpha = 0 \quad \text{（滚转角，无扭转）}
\end{cases}
}
$$

MathType版本：

\left\{ {\begin{array}{*{20}{l}}
{\beta  = \theta }\\
{\gamma  = \varphi }\\
{\alpha  = 0}
\end{array}} \right.

2.3 驱动绳位移

四根驱动绳（90°均布）的位移量为：

$$
\boxed{
\begin{cases}
\Delta l_1 = -r\theta \cos\varphi \[6pt]
\Delta l_2 = r\theta \sin\varphi \[6pt]
\Delta l_3 = r\theta \cos\varphi \[6pt]
\Delta l_4 = -r\theta \sin\varphi
\end{cases}
}
$$

MathType版本：

\left\{ {\begin{array}{*{20}{l}}
{\Delta {l_1} = -r\theta \cos \varphi }\\
{\Delta {l_2} = r\theta \sin \varphi }\\
{\Delta {l_3} = r\theta \cos \varphi }\\
{\Delta {l_4} = -r\theta \sin \varphi }
\end{array}} \right.

对称性：$\Delta l_1 + \Delta l_3 = 0$，$\Delta l_2 + \Delta l_4 = 0$

---

3. 逆运动学求解

3.1 任务空间 → 配置空间

输入：末端位置 $(p_x, p_y, p_z)$
输出：配置参数 $(\theta, \varphi)$

$$
\boxed{
\begin{cases}
\theta = 2\arctan\dfrac{\sqrt{p_x^2 + p_y^2}}{p_z} \[12pt]
\varphi = \arctan2(p_y, p_x)
\end{cases}
}
$$

MathType版本：

\left\{ {\begin{array}{*{20}{l}}
{\theta  = 2\arctan \frac{{\sqrt {p_x^2 + p_y^2} }}{{{p_z}}}}\\
{\varphi  = \arctan 2({p_y},{p_x})}
\end{array}} \right.

3.2 配置空间 → 驱动空间

将配置参数代入2.3节公式即可得到驱动绳位移。

3.3 任务空间 → 驱动空间（联立公式）

直接从末端位置求解驱动绳位移：

$$
\boxed{
\begin{cases}
\Delta l_1 = -2rL \cdot \dfrac{p_x}{p_x^2 + p_y^2} \cdot \left(1 - \dfrac{p_z}{\sqrt{p_x^2 + p_y^2 + p_z^2}}\right) \[12pt]
\Delta l_2 = -2rL \cdot \dfrac{p_y}{p_x^2 + p_y^2} \cdot \left(1 - \dfrac{p_z}{\sqrt{p_x^2 + p_y^2 + p_z^2}}\right) \[12pt]
\Delta l_3 = -\Delta l_1 \[8pt]
\Delta l_4 = -\Delta l_2
\end{cases}
}
$$

---

4. 正运动学求解

4.1 驱动空间 → 配置空间

输入：驱动绳位移 $(\Delta l_1, \Delta l_2, \Delta l_3, \Delta l_4)$
输出：配置参数 $(\theta, \varphi)$

利用对称性，计算差值：

• $\Delta l_1 - \Delta l_3 = -2r\theta\cos\varphi$
• $\Delta l_4 - \Delta l_2 = -2r\theta\sin\varphi$

求解得到：

$$
\boxed{
\begin{cases}
\theta = \dfrac{1}{2r}\sqrt{(\Delta l_1 - \Delta l_3)^2 + (\Delta l_4 - \Delta l_2)^2} \[12pt]
\varphi = \arctan2(\Delta l_4 - \Delta l_2, \Delta l_1 - \Delta l_3) + \pi
\end{cases}
}
$$

MathType版本：

\left\{ {\begin{array}{*{20}{l}}
{\theta  = \frac{1}{{2r}}\sqrt {{{(\Delta {l_1} - \Delta {l_3})}^2} + {{(\Delta {l_4} - \Delta {l_2})}^2}} }\\
{\varphi  = \arctan 2(\Delta {l_4} - \Delta {l_2},\Delta {l_1} - \Delta {l_3}) + \pi}
\end{array}} \right.

注意：

1. $\varphi$ 公式中的 $+\pi$ 修正项是必须的，因为差值公式中包含负号
2. 计算后需将 $\varphi$ 归一化到 $[0, 2\pi]$ 范围

4.2 配置空间 → 任务空间

将配置参数代入2.1节和2.2节公式即可得到末端位姿。

4.3 驱动空间 → 任务空间（联立公式）

4.3.1 末端位置（相对位移版本）

$$
\boxed{
\begin{cases}
p_x = -2rL \cdot \dfrac{\Delta l_1 - \Delta l_3}{(\Delta l_1 - \Delta l_3)^2 + (\Delta l_4 - \Delta l_2)^2} \cdot \left(1-\cos\left[\dfrac{1}{2r}\sqrt{(\Delta l_1 - \Delta l_3)^2 + (\Delta l_4 - \Delta l_2)^2}\right]\right) \[16pt]
p_y = -2rL \cdot \dfrac{\Delta l_4 - \Delta l_2}{(\Delta l_1 - \Delta l_3)^2 + (\Delta l_4 - \Delta l_2)^2} \cdot \left(1-\cos\left[\dfrac{1}{2r}\sqrt{(\Delta l_1 - \Delta l_3)^2 + (\Delta l_4 - \Delta l_2)^2}\right]\right) \[16pt]
p_z = 2rL \cdot \dfrac{\sin\left[\dfrac{1}{2r}\sqrt{(\Delta l_1 - \Delta l_3)^2 + (\Delta l_4 - \Delta l_2)^2}\right]}{\sqrt{(\Delta l_1 - \Delta l_3)^2 + (\Delta l_4 - \Delta l_2)^2}}
\end{cases}
}
$$

MathType版本：

\left\{ {\begin{array}{*{20}{l}}
{{p_x} =  - 2rL \cdot \frac{{\Delta {l_1} - \Delta {l_3}}}{{{{(\Delta {l_1} - \Delta {l_3})}^2} + {{(\Delta {l_4} - \Delta {l_2})}^2}}} \cdot \left( {1 - \cos \left[ {\frac{1}{{2r}}\sqrt {{{(\Delta {l_1} - \Delta {l_3})}^2} + {{(\Delta {l_4} - \Delta {l_2})}^2}} } \right]} \right)}\\
{{p_y} =  - 2rL \cdot \frac{{\Delta {l_4} - \Delta {l_2}}}{{{{(\Delta {l_1} - \Delta {l_3})}^2} + {{(\Delta {l_4} - \Delta {l_2})}^2}}} \cdot \left( {1 - \cos \left[ {\frac{1}{{2r}}\sqrt {{{(\Delta {l_1} - \Delta {l_3})}^2} + {{(\Delta {l_4} - \Delta {l_2})}^2}} } \right]} \right)}\\
{{p_z} = 2rL \cdot \frac{{\sin \left[ {\frac{1}{{2r}}\sqrt {{{(\Delta {l_1} - \Delta {l_3})}^2} + {{(\Delta {l_4} - \Delta {l_2})}^2}} } \right]}}{{\sqrt {{{(\Delta {l_1} - \Delta {l_3})}^2} + {{(\Delta {l_4} - \Delta {l_2})}^2}} }}}
\end{array}} \right.

4.3.2 末端姿态（相对位移版本）

$$
\boxed{
\begin{cases}
\alpha = 0 \[8pt]
\beta = \dfrac{1}{2r}\sqrt{(\Delta l_1 - \Delta l_3)^2 + (\Delta l_4 - \Delta l_2)^2} \[12pt]
\gamma = \arctan2(\Delta l_4 - \Delta l_2, \Delta l_1 - \Delta l_3)
\end{cases}
}
$$

MathType版本：

\left\{ {\begin{array}{*{20}{l}}
{\alpha  = 0}\\
{\beta  = \frac{1}{{2r}}\sqrt {{{(\Delta {l_1} - \Delta {l_3})}^2} + {{(\Delta {l_4} - \Delta {l_2})}^2}} }\\
{\gamma  = \arctan 2(\Delta {l_4} - \Delta {l_2},\Delta {l_1} - \Delta {l_3})}
\end{array}} \right.

4.3.3 绝对位移版本

若已知绳长 $(l_1, l_2, l_3, l_4)$，只需将公式中的差值替换：

• $\Delta l_1 - \Delta l_3 \rightarrow l_3 - l_1$
• $\Delta l_4 - \Delta l_2 \rightarrow l_2 - l_4$

---

5. 公式总结

5.1 逆运动学核心公式

映射	输入	输出	公式
任务→配置	$(p_x, p_y, p_z)$	$(\theta, \varphi)$	$\theta = 2\arctan\frac{\sqrt{p_x2+p_y2}}{p_z}$ $\varphi = \arctan2(p_y, p_x)$
配置→驱动	$(\theta, \varphi)$	$(\Delta l_1, \Delta l_2, \Delta l_3, \Delta l_4)$	$\Delta l_i = -r\theta\cos(\varphi+(i-1)\frac{\pi}{2})$

5.2 正运动学核心公式

映射	输入	输出	公式
驱动→配置	$(\Delta l_1, \Delta l_2, \Delta l_3, \Delta l_4)$	$(\theta, \varphi)$	$\theta = \frac{1}{2r}\sqrt{(\Delta l_1-\Delta l_3)^2+(\Delta l_4-\Delta l_2)^2}$ $\varphi = \arctan2(\Delta l_4-\Delta l_2, \Delta l_1-\Delta l_3)+\pi$
配置→任务	$(\theta, \varphi)$	$(p_x, p_y, p_z, \alpha, \beta, \gamma)$	$p_x = \rho\cos\varphi(1-\cos\theta)$ $p_y = \rho\sin\varphi(1-\cos\theta)$ $p_z = \rho\sin\theta$ $\alpha=0, \beta=\theta, \gamma=\varphi$

---

6. 关键要点

1. 常曲率假设：连续体弯曲形成圆弧，中性线长度不变
2. 配置空间维度：两个参数 $(\theta, \varphi)$ 完全描述形态
3. 驱动绳约束：$\Delta l_1 + \Delta l_3 = 0$，$\Delta l_2 + \Delta l_4 = 0$
4. 角度修正：正运动学中 $\varphi$ 需要 $+\pi$ 修正（arctan2负号补偿）
5. 姿态简洁性：欧拉角直接等于配置参数，无需复杂计算

---

7. 应用示例

7.1 逆运动学示例

给定目标位置：$(p_x, p_y, p_z) = (15, 15, 42.4)$ mm

求解步骤：

1. 计算 $\theta = 2\arctan\frac{\sqrt{15²⁺¹⁵2}}{42.4} \approx 60°$
2. 计算 $\varphi = \arctan2(15, 15) = 45°$
3. 代入驱动绳公式（设 $r=10$ mm）：
   • $\Delta l_1 = -10 \times 60° \times \cos(45°) = -0.8145$ mm
   • $\Delta l_2 = 10 \times 60° \times \sin(45°) = 0.8145$ mm
   • $\Delta l_3 = 0.8145$ mm
   • $\Delta l_4 = -0.8145$ mm

7.2 正运动学示例

给定驱动绳位移：$(\Delta l_1, \Delta l_2, \Delta l_3, \Delta l_4) = (-0.8145, 0.8145, 0.8145, -0.8145)$ mm

求解步骤：

1. 计算 $\theta = \frac{1}{2 \times 10}\sqrt{(-0.8145-0.8145)^{2+((-0.8145)-0.8145)}2} \approx 60°$
2. 计算 $\varphi = \arctan2(-1.629, -1.629) + \pi = 45°$
3. 代入位置公式得到末端位置

---

8. 参考文献

1. Webster, R. J., & Jones, B. A. (2010). Design and kinematic modeling of constant curvature continuum robots. International Journal of Robotics Research, 29(13), 1661-1683.

2. Renda, F., et al. (2014). Dynamic model of a multibending soft robot arm driven by cables. IEEE Transactions on Robotics, 30(5), 1109-1122.

---

版权声明：本文档基于常曲率假设推导，公式已通过数值仿真验证。如需引用，请注明出处。

最后更新：2025年12月30日