# Whole-Body Motion Planning for a Six-Legged Robot Walking on Rugged Terrain
## 2 Methods
### Support Planning
#### 2.2.1 Torso Horizontal Position and Height  
For a walking step of the six-legged robot, one key point of motion planning is to find a proper target state that maximizes the stability margin of the robot, then the robot can move from current state to the target state. According to geometric knowledge and the definition of stability [19], when the projection of the robot CoG is exactly at the center of the maximum inner circle of the support polygon formed by target footholds, the robot can obtain the maximum stability margin. In order to derive the incenter \(O_{2}\) (\(X_{r},\ Y_{r}\)) of the support polygon, as illustrated in Figure 3, the following steps are adopted:  
Step 1: Arbitrarily select a point \(S\) (\(X_{s}\), \(Y_{s}\)) in the polygon as the incenter to start calculation. In this paper, we choose the midpoint of two vertices of a polygon that are not adjacent to each other.  
Figure 3: Flow diagram to find the incenter of the support polygon.  
Figure 2: Schematic of the support planning process.  
Step 2: Calculate the shortest distance from the incenter \(S\) to each side of the supporting polygon and the position of the corresponding points. The two points closest to each other were determined and recorded as \(A\) (\(X_{A}\), \(Y_{A}\)), distance \(L_{A}\), \(B\) (\(X_{B}\), \(Y_{B}\)) and distance \(L_{B}\), respectively.  
Step 3: Select a new point \(C\) (\(X_{C}\), \(Y_{C}\)) according to the following rules: if \(A\), \(B\) and \(S\) are collinear, then \(C=A\); if \(A\), \(B\) and \(S\) are not collinear, then choose  
\[\left\{\begin{array}{l}X_{C}=X_{A}+\frac{L_{A}}{L_{A}+L_{B}}(X_{B}-X_{A})\\ Y_{C}=Y_{A}+\frac{L_{A}}{L_{A}+L_{B}}(Y_{B}-Y_{A})\end{array}\right. \tag{1}\]  
Step 4: Set the step size factor \(a\), and take a point \(S^{\prime}\) (\(X_{s}\), \(Y_{s}\)) on the extension line of \(CS\) as the new center of the calculation circle according to the following rules, namely:  
\[\left\{\begin{array}{l}X_{S^{\prime}}=X_{S}+a\frac{(X_{S}-X_{C})}{\sqrt{(X_{ S}-X_{C})^{2}+(Y_{S}-Y_{C})^{2}}}\\ Y_{S^{\prime}}=Y_{S}+a\frac{(Y_{S}-Y_{C})}{\sqrt{(X_{S}-X_{C})^{2}+(Y_{S}-Y_{C} )^{2}}}\end{array}\right. \tag{2}\]  
Step 5: Repeat Step 2 with the newly calculated center \(S^{\prime}\), and compare the calculated minimum distance \(L^{\prime}\) with \(L_{A}\). If the value of \(L^{\prime}\) increases, continue Steps 3 and 4; otherwise, the step factor is decreased to 0.5a and recalculate the incenter with Equation (2). With cyclic calculation of the above steps, the incenter \(O_{2}\) (\(X_{r}\), \(Y_{r}\)) of the maximum inner circle of the supporting polygon can be obtained. In this regard, the planning task of support legs is to propel the horizontal position of the torso from the current \(O_{1}\) to the incenter \(O_{2}\) (\(X_{r}\), \(Y_{r}\)) of the support polygon.  
In terms of the torso height, the purpose is to keep a certain distance from the ground. In this paper, it is prescribed that the robot CoG and the incennter of the supporting polygon are always maintained at a certain height \(h\). This can prevent the collision between the robot and the ground simply and effectively.