Mobile object controller and floor surface estimator

A floor surface estimator which estimates a position and posture of an actual floor surface on which a mobile object moves, in a mobile object controller performing motion control of the mobile object according to a desired motion of the mobile object and a desired total floor reaction force which is a desired value of a total floor reaction force to be applied to the mobile object to realize the desired motion, the mobile object including a body, a plurality of leg links connected to the body, and a joint actuator which drives a joint of each leg link, and moving on the floor surface by movements of the plurality of leg links, the floor surface estimator comprising:

• a total floor reaction force required correction amount determination processor configured to determine, as a total floor reaction force required correction amount, a result of integrating an error between an observed value of the total floor reaction force actually acting on the mobile object and the desired total floor reaction force, the total floor reaction force required correction amount being a correction amount of the total floor reaction force to be additionally applied to the mobile object so that the error approaches zero;
• a representative contact surface position/posture displacement amount calculation processor configured to calculate, from the determined total floor reaction force required correction amount and a predetermined spring constant of a representative contact surface, a required displacement amount of a position and posture of the representative contact surface corresponding to the total floor reaction force required correction amount based on an assumption that the total floor reaction force required correction amount is generated by a spring displacement of the position and posture of the representative contact surface, the representative contact surface being a single virtual contact surface representative of all contact surfaces between the mobile object and the floor surface;
• a representative contact surface Jacobian matrix calculation processor configured to calculate a representative contact surface Jacobian matrix Jc according to the following equation (27) from a leg link Jacobian matrix J_i for each leg link, the spring constant, a relative position of an actual floor reaction force central point of a distal end of each leg link relative to a total floor reaction force central point, and a floor reaction force actually acting on each leg link, the representative contact surface Jacobian matrix Jc being a Jacobian matrix representing a relation between a temporal change rate of the position and posture of the representative contact surface and a temporal change rate of a generalized variable vector whose components are a position and posture of the body and a displacement amount of each joint of the mobile object, the leg link Jacobian matrix J_i being a Jacobian matrix representing a relation between a temporal change rate of a position of a distal end of the leg link or a temporal change rate of a position and posture of the distal end of the leg link and the temporal change rate of the generalized variable vector, and the total floor reaction force central point being an acting point of the total floor reaction force actually acting on the mobile object;
• a joint displacement correction amount determination processor configured to determine a joint displacement correction amount by multiplying the calculated required displacement amount of the position and posture of the representative contact surface by a pseudo inverse matrix Jc−1 of the calculated representative contact surface Jacobian matrix Jc, the joint displacement correction amount being a correction amount of the displacement amount of each joint of the mobile object for realizing the required displacement amount of the position and posture of the representative contact surface;
• and
• a joint displacement control processor configured to control the joint actuator according to a corrected desired joint displacement amount obtained by correcting a desired joint displacement amount by the determined joint displacement correction amount, the desired joint displacement amount being the displacement amount of each joint of the mobile object defined by the desired motion of the mobile object,
• wherein the position and posture of the actual floor surface are estimated by correcting a position and posture of a supposed floor surface according to the required displacement amount calculated by the representative contact surface position/posture displacement amount calculation processor, the supposed floor surface being a floor surface supposed in the desired motion,
• Jc
• =
• ∑
• i
• =
• 1
• N
• ⁢
• (
• r_i
• ·
• A_i
• ·
• J_i
• )
• (
• 27
• )
• where
• Jc is the representative contact surface Jacobian matrix,
• i is an identification number of a leg link,
• N is a total number of leg links,
• r_i is a weight coefficient of an i-th leg link determined by the following equation (27-1),
• r_i
• =
• Fn_i
• /
• (
• ∑
• j
• =
• 1
• N
• ⁢
• Fn_j
• )
• (
• 27
• ⁢
• -
• ⁢
• 1
• )
• Fn_i is a normal force component of a floor reaction force acting on the i-th leg link,
• A_i is a matrix defined by the following equation (27-2),
• A_i
• ≡
• [
• I
• 0
• Rk
• ·
• VV_i
• I
• ]
• (
• 27
• ⁢
• -
• ⁢
• 2
• )
• Rk is a coefficient matrix defined by the following equation (27-3),
• Rk≡Kc—rot−1·Kc—org (27-3)
• Kc_org is a spring constant matrix relating to a translational displacement of the position of the representative contact surface,
• Kc_rot is a spring constant matrix relating to a rotational displacement of the posture of the representative contact surface,
• VV_i is a matrix such that VV_i·↑F_i=↑V_i×↑F_i,
• ↑F_i is a floor reaction force vector acting on the i-th leg link,
• ↑V_i is a position vector of a floor reaction force central point of a distal end of the i-th leg link relative to the total floor reaction force central point, and
• J_i is a leg link Jacobian matrix of the i-th leg link.

A floor surface estimator which estimates a position and posture of an actual floor surface on which a mobile object moves, in a mobile object controller performing motion control of the mobile object according to a desired motion of the mobile object and a desired total floor reaction force which is a desired value of a total floor reaction force to be applied to the mobile object to realize the desired motion, the mobile object including a body, a plurality of leg links connected to the body, and a joint actuator which drives a joint of each leg link, and moving on the floor surface by movements of the plurality of leg links, the floor surface estimator comprising:

• a total floor reaction force required correction amount determination processor configured to determine a total floor reaction force required correction amount which is a correction amount of a total floor reaction force to be additionally applied to the mobile object so that an error between an observed value of the total floor reaction force actually acting on the mobile object and the desired total floor reaction force approaches zero, by combining at least a proportional term proportional to the error and an integral term obtained by integrating the error;
• a representative contact surface position/posture displacement amount calculation processor configured to calculate, from the determined total floor reaction force required correction amount and a predetermined spring constant of a representative contact surface, a required displacement amount of a position and posture of the representative contact surface corresponding to the total floor reaction force required correction amount based on an assumption that the total floor reaction force required correction amount is generated by a spring displacement of the position and posture of the representative contact surface, the representative contact surface being a single virtual contact surface representative of all contact surfaces between the mobile object and the floor surface;
• a representative contact surface Jacobian matrix calculation processor configured to calculate a representative contact surface Jacobian matrix Jc according to the following equation (27) from a leg link Jacobian matrix J_i for each leg link, the spring constant, a relative position of an actual floor reaction force central point of a distal end of each leg link relative to a total floor reaction force central point, and a floor reaction force actually acting on each leg link, the representative contact surface Jacobian matrix Jc being a Jacobian matrix representing a relation between a temporal change rate of the position and posture of the representative contact surface and a temporal change rate of a generalized variable vector whose components are a position and posture of the body and a displacement amount of each joint of the mobile object, the leg link Jacobian matrix J_i being a Jacobian matrix representing a relation between a temporal change rate of a position of a distal end of the leg link or a temporal change rate of a position and posture of the distal end of the leg link and the temporal change rate of the generalized variable vector, and the total floor reaction force central point being an acting point of the total floor reaction force actually acting on the mobile object;
• a joint displacement correction amount determination processor configured to determine a joint displacement correction amount by multiplying the calculated required displacement amount of the position and posture of the representative contact surface by a pseudo inverse matrix Jc−1 of the calculated representative contact surface Jacobian matrix Jc, the joint displacement correction amount being a correction amount of the displacement amount of each joint of the mobile object for realizing the required displacement amount of the position and posture of the representative contact surface;
• a joint displacement control processor configured to control the joint actuator according to a corrected desired joint displacement amount obtained by correcting a desired joint displacement amount by the determined joint displacement correction amount, the desired joint displacement amount being the displacement amount of each joint of the mobile object defined by the desired motion of the mobile object;
• and
• a representative contact surface steady-state displacement amount calculation processor configured to calculate a representative contact surface steady-state displacement amount from the integral term in the total floor reaction force required correction amount and the spring constant of the representative contact surface, the representative contact surface steady-state displacement amount being a displacement amount of the position and posture of the representative contact surface corresponding to the integral term,
• wherein the position and posture of the actual floor surface are estimated by correcting a position and posture of a supposed floor surface according to the representative contact surface steady-state displacement amount calculated by the representative contact surface steady-state displacement amount calculation processor, the supposed floor surface being a floor surface supposed in the desired motion,
• Jc
• =
• ∑
• i
• =
• 1
• N
• ⁢
• (
• r_i
• ·
• A_i
• ·
• J_i
• )
• (
• 27
• )
• where
• Jc is the representative contact surface Jacobian matrix,
• i is an identification number of a leg link,
• N is a total number of leg links,
• r_i is a weight coefficient of an i-th leg link determined by the following equation (27-1),
• r_i
• =
• Fn_i
• /
• (
• ∑
• j
• =
• 1
• N
• ⁢
• Fn_j
• )
• (
• 27
• ⁢
• -
• ⁢
• 1
• )
• Fn_i is a normal force component of a floor reaction force acting on the i-th leg link,
• A_i is a matrix defined by the following equation (27-2),
• A_i
• ≡
• [
• I
• 0
• Rk
• ·
• VV_i
• I
• ]
• (
• 27
• ⁢
• -
• ⁢
• 2
• )
• Rk is a coefficient matrix defined by the following equation (27-3),
• Rk≡Kc—rot−1·Kc—org (27-3)
• Kc_org is a spring constant matrix relating to a translational displacement of the position of the representative contact surface,
• Kc_rot is a spring constant matrix relating to a rotational displacement of the posture of the representative contact surface,
• VV_i is a matrix such that VV_i·↑F_i=↑V_i×↑F_i,
• ↑F_i is a floor reaction force vector acting on the i-th leg link,
• ↑V_i is a position vector of a floor reaction force central point of a distal end of the i-th leg link relative to the total floor reaction force central point, and
• J_i is a leg link Jacobian matrix of the i-th leg link.

A mobile object controller which performs motion control of a mobile object according to a desired motion and a desired total contact force, the mobile object including a body, a plurality of movable links connected to the body, and a joint actuator which drives a joint of each movable link, the desired motion being for moving the mobile object while at least one movable link contacts each of first to N-th contact target surfaces which are a plurality of mutually different contact target surfaces existing in a mobile environment of the mobile object where N is an integer equal to or more than 2, and the desired total contact force being a desired value of a total contact force to be applied to the mobile object from each of the first to N-th contact target surfaces to realize the desired motion, the mobile object controller comprising:

• a total contact force required correction amount determination processor configured to determine an i-th total contact force required correction amount according to an error between an observed value of a total contact force actually acting on the mobile object from an i-th contact target surface and a desired total contact force corresponding to the i-th contact target surface where i=1, 2, . . . , N, the i-th total contact force required correction amount being a required correction amount of the total contact force to be additionally applied to the mobile object from the i-th contact target surface so that the error approaches zero, the i-th contact target surface being each of the first to N-th contact target surfaces;
• a representative contact surface position/posture displacement amount calculation processor configured to calculate, from the determined i-th total contact force required correction amount and a predetermined spring constant of an i-th representative contact surface corresponding to the i-th contact target surface where i=1, 2, . . . , N, a required displacement amount of a position and posture of the i-th representative contact surface based on an assumption that the i-th total contact force required correction amount is generated by a spring displacement of the position and posture of the i-th representative contact surface, the i-th representative contact surface being a single virtual contact surface representative of all contact surfaces between the mobile object and the i-th contact target surface;
• a representative contact surface Jacobian matrix calculation processor configured to calculate an i-th representative contact surface Jacobian matrix Jc(i) according to the following equation (77) from a movable link Jacobian matrix J(i)_j for each movable link in an i-th contact movable link group where i=1, 2, . . . , N, the spring constant of the i-th representative contact surface, a relative position of an actual contact force central point of a contact portion of each movable link in the i-th contact movable link group relative to a total contact force central point, and a contact force actually acting on each movable link in the i-th contact movable link group, the i-th representative contact surface Jacobian matrix Jc(i) being a Jacobian matrix representing a relation between a temporal change rate of the position and posture of the i-th representative contact surface and a temporal change rate of a generalized variable vector whose components are a position and posture of the body and a displacement amount of each joint of the mobile object, the movable link Jacobian matrix J(i)_j being a Jacobian matrix representing a relation between a temporal change rate of a position of a contact portion of the movable link in the i-th contact movable link group or a temporal change rate of a position and posture of the contact portion of the movable link in the i-th contact movable link group and the temporal change rate of the generalized variable vector, the total contact force central point being an acting point of the total contact force actually acting on the mobile object from the i-th contact target surface, and the i-th contact movable link group being a group of movable links contacting the i-th contact target surface;
• a joint displacement correction amount determination processor configured to determine a joint displacement correction amount by multiplying an overall required displacement amount by a pseudo inverse matrix Jc−1 of an overall Jacobian matrix Jc, the joint displacement correction amount being a correction amount of the displacement amount of each joint of the mobile object for realizing the required displacement amount of the position and posture of each of the first to N-th representative contact surfaces, the overall required displacement amount being formed by arranging calculated required displacement amounts of positions and postures of first to N-th representative contact surfaces, and the overall Jacobian matrix Jc being formed by arranging calculated first to N-th representative contact surface Jacobian matrices Jc(i) where i=1, 2, . . . , N;
• and
• a joint displacement control processor configured to control the joint actuator according to a corrected desired joint displacement amount obtained by correcting a desired joint displacement amount by the determined joint displacement correction amount, the desired joint displacement amount being the displacement amount of each joint of the mobile object defined by the desired motion of the mobile object,
• Jc
• ⁡
• (
• i
• )
• =
• ∑
• j
• =
• 1
• m
• ⁡
• (
• i
• )
• ⁢
• (
• r
• ⁡
• (
• i
• )
• ⁢
• _j
• ·
• A
• ⁡
• (
• i
• )
• ⁢
• _j
• ·
• J
• ⁡
• (
• i
• )
• ⁢
• _j
• )
• (
• 77
• )
• where
• Jc(i) is an i-th representative contact surface Jacobian matrix,
• j is an identification number of each movable link belonging to the i-th contact movable link group,
• m(i) is a total number of movable links belonging to the i-th contact movable link group,
• r(i)_j is a weight coefficient of a j-th movable link in the i-th contact movable link group determined by the following equation (77-1),
• r(i)—j=Fn(i)—j/(j) (77-1)
• Fn(i)_j is a normal force component of a contact force acting on the j-th movable link in the i-th contact movable link group,
• A(i)_j is a matrix defined by the following equation (77-2),
• A
• ⁡
• (
• i
• )
• ⁢
• _j
• ≡
• [
• I
• 0
• Rk
• ⁡
• (
• i
• )
• ·
• VV
• ⁡
• (
• i
• )
• ⁢
• _j
• I
• ]
• (
• 77
• ⁢
• -
• ⁢
• 2
• )
• Rk(i) is a coefficient matrix defined by the following equation (77-3),
• Rk(i)=Kc(i)—rot−1·Kc(i)—org (77-3)
• Kc(i)_org is a spring constant matrix relating to a translational displacement of the position of the i-th representative contact surface,
• Kc(i)_rot is a spring constant matrix relating to a rotational displacement of the posture of the i-th representative contact surface,
• VV(i)_j is a matrix such that VV(i)_j·↑F(i)_j=↑V(i)_j×T F(i)_j,
• ↑F(i)_j is a contact force vector acting on the j-th movable link in the i-th contact movable link group,
• ↑V(i)_j is a position vector of a contact force central point of a contact portion of the j-th movable link in the i-th contact movable link group relative to the total contact force central point of the i-th contact target surface, and
• J(i)_j is a movable link Jacobian matrix of the j-th movable link in the i-th contact movable link group.